MAVLink and Arduino: step by step

Hi @fezile_mageba,
That sounds like a cool project.
So from what I can gather you need a way to use the pix hawk with ardupilot to control the Stepper motor, is that correct?
If so then I have a few ideas and will get back to you.
Cheers,
PJ

Hi Paul

Yes that’s correct, thank you very much.

Hi @fezile_mageba,
I think what you need to do is a two step thing.
First you need to use your Arduino to read the PWM signal coming out of the AutoPilot for the steering servo. That should give you a value that is proportional to the Autopilots desired steering angle. There are loads of examples how to read PWM values via an Arduino like How to read RC receiver signal with Arduino | by Ricardo Paiva | Medium
Second you now need to take that steering value and use that to control your stepper motor, again there are many examples of how to do that via Arduino on the net.

You do not need to use MAVLink on your Arduino in this case.

So, just break your testing into the two parts, get each of them working and then combine them.

Hope this helps, please let us know how you go.

Hi Paul

My concern with what you are explaining is that I don’t understand how the pixhawk will be able to control the stepper motor autonomously. Or that will come after doing these two steps?

Fezile

Your essentially converting the stepper motor into a servo for the pixhawk to control.

Hey Fezile,

The Pixhawk usually controls a servo to steer a boat/plane/rover. It does that by putting out a PWM signal on one of the servo connectors on the pixhawk.
But as you are using a stepper motor to control steering on your surfboard, you need to convert the PWM signal coming out of the pixhawk, to something the stepper motor will understand.
So to do that conversion, I’ve suggested you use an Arduino to listen to the PWM signal, convert it to a number which is relative to the steering angle that the pixhawk wants and then take that number as input to how you position your stepper motor.
How does that sound?

Actually I wouldn’t be surprised if someone else has already done something like this. Have a good search around the forum to check.
You are looking for how to convert PWM to Stepper motor or something like that. Or maybe how to use a stepper motor with ardupilot/pixhawk.

I think the problem with using a stepper motor is it has no feed back, so you can tell it to turn 180 degrees but it doesnt know its starting position. In 3d printers they have an end point switch then essentually count the turns to know where it is relative to the switch. So you would have to set your rudder position every time you switched it on or add a potentiometer for feedback, at that point you might as well get a servo.

I tried to use these codes but i just got
16:17:21.046 → Streams requested!

Hi guys
I was looking forward to buying a stepper motor control from pololu, which make it easier to drive a stepper motor using the PWM signal from the pixhawk. But I’ll have to go with what you explained, and I hope everything goes well. I will inform you of every step.

Thank you guys

Can you please explain what you did and which code you are talking about

You can use this code to turn any geared brushed motor into a servo.

#include “mavlink.h”
#include <SoftwareSerial.h>
SoftwareSerial _MavLinkSerial(21, 20);
// PIN 21=Telemetry TX->Pixhawk RX, PIN 20=Telemetry RX->Pixhawk TX

// Mavlink variables
unsigned long previousMillisMAVLink = 0; // will store last time MAVLink was transmitted and listened
unsigned long next_interval_MAVLink = 1000; // next interval to count
const int num_hbs = 2; // # of heartbeats to wait before activating STREAMS from Pixhawk. 60 = one minute.
int num_hbs_pasados = num_hbs;

void setup() {
// MAVLink
/* The default UART header for your MCU */
int sysid = 1; ///< ID 20 for this airplane. 1 PX, 255 ground station
int compid = 158; ///< The component sending the message
int type = MAV_TYPE_QUADROTOR; ///< This system is an airplane / fixed wing

// Define the system type, in this case an airplane → on-board controller
// uint8_t system_type = MAV_TYPE_FIXED_WING;
uint8_t system_type = MAV_TYPE_GENERIC;
uint8_t autopilot_type = MAV_AUTOPILOT_INVALID;

uint8_t system_mode = MAV_MODE_PREFLIGHT; ///< Booting up
uint32_t custom_mode = 0; ///< Custom mode, can be defined by user/adopter
uint8_t system_state = MAV_STATE_STANDBY; ///< System ready for flight

// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message
//mavlink_msg_heartbeat_pack(sysid,compid, &msg, type, autopilot_type, system_mode, custom_mode, system_state);
mavlink_msg_heartbeat_pack(1, 0, &msg, type, autopilot_type, system_mode, custom_mode, system_state);

// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

// Send the message with the standard UART send function
// uart0_send might be named differently depending on
// the individual microcontroller / library in use.
unsigned long currentMillisMAVLink = millis();
if (currentMillisMAVLink - previousMillisMAVLink >= next_interval_MAVLink)
{
// Record last HB update
previousMillisMAVLink = currentMillisMAVLink;

//Mav_Request_Data();
num_hbs_pasados++;
if (num_hbs_pasados >= num_hbs) {
  // Request streams from Pixhawk
  Serial.println("Streams requested!");
  Mav_Request_Data();
  num_hbs_pasados = 0;
}

}

// Check reception buffer
comm_receive();
}
void loop() {

// MAVLink
/* The default UART header for your MCU */
int sysid = 1; ///< ID 20 for this airplane. 1 PX, 255 ground station
int compid = 158; ///< The component sending the message
int type = MAV_TYPE_QUADROTOR; ///< This system is an airplane / fixed wing

// Define the system type, in this case an airplane → on-board controller
// uint8_t system_type = MAV_TYPE_FIXED_WING;
uint8_t system_type = MAV_TYPE_GENERIC;
uint8_t autopilot_type = MAV_AUTOPILOT_INVALID;

uint8_t system_mode = MAV_MODE_PREFLIGHT; ///< Booting up
uint32_t custom_mode = 0; ///< Custom mode, can be defined by user/adopter
uint8_t system_state = MAV_STATE_STANDBY; ///< System ready for flight

// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message
//mavlink_msg_heartbeat_pack(sysid,compid, &msg, type, autopilot_type, system_mode, custom_mode, system_state);
mavlink_msg_heartbeat_pack(1, 0, &msg, type, autopilot_type, system_mode, custom_mode, system_state);

// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

// Send the message with the standard UART send function
// uart0_send might be named differently depending on
// the individual microcontroller / library in use.
unsigned long currentMillisMAVLink = millis();
if (currentMillisMAVLink - previousMillisMAVLink >= next_interval_MAVLink)
{
// Record last HB update
previousMillisMAVLink = currentMillisMAVLink;

//Mav_Request_Data();
num_hbs_pasados++;
if (num_hbs_pasados >= num_hbs) {
  // Request streams from Pixhawk
  Serial.println("Streams requested!");
  Mav_Request_Data();
  num_hbs_pasados = 0;
}

}

// Check reception buffer
comm_receive();
}

void Mav_Request_Data()
{
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// To be setup according to the needed information to be requested from the Pixhawk
const int maxStreams = 1;
const uint8_t MAVStreams[maxStreams] = {MAV_DATA_STREAM_POSITION};
const uint16_t MAVRates[maxStreams] = {0x02};

for (int i = 0; i < maxStreams; i++) {
mavlink_msg_request_data_stream_pack(6, 200, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
_MavLinkSerial.write(buf, len);
}
}

void comm_receive() {
mavlink_message_t msg;
mavlink_status_t status;

while (_MavLinkSerial.available() > 0) {
uint8_t c = _MavLinkSerial.read();

// Try to get a new message
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
{
  Serial.print("DEBUG msgid:"); Serial.println(msg.msgid);
  // Handle message
  switch (msg.msgid)
  {
    case MAVLINK_MSG_ID_HEARTBEAT:  // #0: Heartbeat
      {
        // E.g. read GCS heartbeat and go into
        // comm lost mode if timer times out
      }
      break;
  }
}

}
}

16:07:21.758 → Streams requested!
16:07:23.750 → Streams requested!
16:07:25.776 → Streams requested!
16:07:27.769 → Streams requested!
16:07:29.761 → Streams requested!
16:07:31.754 → Streams requested!
16:07:33.780 → Streams requested!
16:07:35.773 → Streams requested!
16:07:37.766 → Streams requested!
16:07:39.760 → Streams requested!
16:07:41.752 → Streams requested!
16:07:43.777 → Streams requested!

Hello, I have a problem. Need HELP!
I connect arduino to pixhawk.

Could someone explaid me why I have a heartbeat, I can write an rewrite the RC.
But I can’t request data from my pixhawk.
My pixhawk sys is Vehicle 1,Comp 1

#include "mavlink.h"

// Mavlink variables
int sysid = 1;                   ///< ID 20 for this airplane. 1 PX, 255 ground station
int compid = 158;                ///< The component sending the message
uint8_t target_system = 1;
uint8_t target_component = 1;
int type = MAV_TYPE_GROUND_ROVER;   ///< This system is an airplane / fixed wing
uint8_t autopilot_type = MAV_AUTOPILOT_PIXHAWK;
uint8_t system_mode = MAV_MODE_PREFLIGHT; ///< Booting up
uint32_t custom_mode = 0;                 ///< Custom mode, can be defined by user/adopter
uint8_t system_state = MAV_STATE_STANDBY; ///< System ready for flight
unsigned long request_timer =millis();
unsigned long heartbeat_timer = millis();

void setup() {
  // MAVLink interface start
  Serial1.begin(57600);
  Serial.begin(115200);
}

void loop() {     
doSendHeartbeat();
dorequest();
comm_receive() ;
rc_override(1200,1200);
}

void doSendHeartbeat(){
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  if(heartbeat_timer < millis()){
        mavlink_msg_heartbeat_pack(sysid, compid, &msg, type, autopilot_type, system_mode, custom_mode, system_state);
        uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
        Serial1.write(buf, len);
      heartbeat_timer = millis() + 100;
  }
}
void dorequest(){
  if(request_timer < millis()){
    Mav_Request_Data();
    request_timer = millis() + 5000;
  }
}

void Mav_Request_Data()
{
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
  mavlink_msg_request_data_stream_pack(target_system,target_component, &msg, sysid,compid, MAV_DATA_STREAM_ALL, 0x01, 1);
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  Serial1.write(buf, len);
}


void comm_receive() {
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  mavlink_status_t status;
  while(Serial1.available()>0) {

    uint8_t c = Serial1.read();
    // Try to get a new message
    if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
      // Handle message
      switch(msg.msgid) {
        case MAVLINK_MSG_ID_HEARTBEAT:  // #0: Heartbeat
          {
            // E.g. read GCS heartbeat and go into
            // comm lost mode if timer times out
          }
          break;

        case MAVLINK_MSG_ID_SYS_STATUS:  // #1: SYS_STATUS
          {
            /* Message decoding: PRIMITIVE
             *    mavlink_msg_sys_status_decode(const mavlink_message_t* msg, mavlink_sys_status_t* sys_status)
             */
            //mavlink_message_t* msg;
            mavlink_sys_status_t sys_status;
            mavlink_msg_sys_status_decode(&msg, &sys_status);
            Serial.println("2-");
          }
          break;

        case MAVLINK_MSG_ID_PARAM_VALUE:  // #22: PARAM_VALUE
          {
            /* Message decoding: PRIMITIVE
             *    mavlink_msg_param_value_decode(const mavlink_message_t* msg, mavlink_param_value_t* param_value)
             */
            //mavlink_message_t* msg;
            mavlink_param_value_t param_value;
            mavlink_msg_param_value_decode(&msg, &param_value);
            Serial.println("3-");
          }
          break;

        case MAVLINK_MSG_ID_RAW_IMU:  // #27: RAW_IMU
          {
            /* Message decoding: PRIMITIVE
             *    static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavlink_raw_imu_t* raw_imu)
             */
            mavlink_raw_imu_t raw_imu;
            mavlink_msg_raw_imu_decode(&msg, &raw_imu);
            Serial.println(raw_imu.zgyro);
          }
          break;
 
        case MAVLINK_MSG_ID_REQUEST_DATA_STREAM :  // #66
          {


          }
          break;
          
        case MAVLINK_MSG_ID_ATTITUDE:  // #30
          {
            /* Message decoding: PRIMITIVE
             *    mavlink_msg_attitude_decode(const mavlink_message_t* msg, mavlink_attitude_t* attitude)
             */
            mavlink_attitude_t attitude;
            mavlink_msg_attitude_decode(&msg, &attitude);
            Serial.println("5-");
          }
          break;

        
       default:
          break;
      }
    }
  }
}

void rc_override(int mav_steer, int mav_speed){
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
  
  mavlink_msg_rc_channels_override_pack(sysid, compid, &msg, 0, 0,
    mav_steer, 
    mav_speed, 
    65535,65535,65535,65535,65535,65535);    
  
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  Serial1.write(buf, len);
}

Hi @Slava_Golitsin,
I think you may have the parameters in your mavlink_msg_request_data_stream_pack() function mixed up. I think you need to put your sysid, compid first and then the target_system and target_component later. See the following;

Cheers,
PJ

/**

  • @brief Pack a request_data_stream message
  • @param system_id ID of this system
  • @param component_id ID of this component (e.g. 200 for IMU)
  • @param msg The MAVLink message to compress the data into
  • @param target_system The target requested to send the message stream.
  • @param target_component The target requested to send the message stream.
  • @param req_stream_id The ID of the requested data stream
  • @param req_message_rate [Hz] The requested message rate
  • @param start_stop 1 to start sending, 0 to stop sending.
  • @return length of the message in bytes (excluding serial stream start sign)
    /
    static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t
    msg,
    uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop)

Should be

mavlink_msg_request_data_stream_pack(sysid,compid, &msg, target_system, target_component, MAV_DATA_STREAM_ALL, 0x01, 1);

Hi, I read your posts, it really help me alot.
Now, I am trying to read the Rc channel data, that how much throttle is given by user in remote controller, so that I will use that data to increase or decreasee the rpm of secondary engine which I fixed.
Kind Regards

Hi @Vinoth_Kumar,
Let us know if you get stuck or need any help.
Whats your project by the way?
Good Luck,
PJ

Dear @pauljeff , I am making series drone generator, onboard I need to control the rpm of this generator engine with arduino, so I need the throttle control information from Pixhawk, whenever user in ground make changes in controller stick.
I was not able to program this task, i,e, I want information/live feedback about throttle input from pixhawk to arduino.
MAV_DATA_STREAM_RC_CHANNELS, I am trying with this function, but I am not familiar with it.
It would be great if you tell me how can i complete this task.
Thanks,

Thanks for your source with some updates to use PixhawkArduinoMAVLink and hardware serial, started getting data from the ardupilot using ESP32 :slight_smile: