MAVLink and Arduino: step by step

Sorry for being late.
Dear @jplopezll
I want that if the ultrasound sensor detects an obstacle the Arduino sends to pixhawk a commend for the drone to stop at the same position and land for example.
so i need a mavlink function wich can do that.
I looked on the Internet but I did not find a file that explains the functions of mavlink and how to use it.
I am currently in Internship in Madrid and I am working on this project. the ultimate goal is for the drone to fulfill a mission and avoid obstacles with ultrasound senser and Arduino.
if you can help me, I will be very happy.
Thank you in advance for your answer.

Hi all, might be a strange question but has anyone had any luck taking MAVLINK data directly from a telemetry modem into the arduino? Would be used in a ground station to pick out key information (Battery, RSSI, distance from home, altitude etc) and display it on an LCD.

Any thoughts?

I’ve tee’ed off a RFD900x telemetry modem output on the ground (received from the plane side) to capture the GPS position (lat, long, and altitude) from the plane with an antenna tracker). The tracker runs Arduino code on an STM32 Maple Mini.

Since the tracker can’t talk back to the telemetry radio (I did not connect a wire from the STM TX side to the telemetry radio input), the link telemetry link works as normal with a Raspberry PI for a MavLink ground station.

Any telemetry data coming down the link should be available to use. I’m not sure if distance from home is sent over MavLink, but I calculate distance between my tracker GPS and the plane GPS.

I display the plane and tracker altitudes and positions as well as the distance/azimuth/and elevation angles between them. However the display is typically only used for troubleshooting the tracker since it’s not daylight readable…and it is mounted on the pan axis so it slowly rotates most of the time.

If I lose sight of the aircraft, the antenna point direction is helpful in showing where to aim my eyes.

RR

Greetings,

So after a brief scan of this thread, can I infer that this library can be used to get commands from the PixHawk 2.1 and have an Arduino activate small motors through a shield?

Cheers,

Coach

This just shows a very basic way to send and receive Mavlink messages. The Mavlink library has all of the methods to package data.

Done it. I have a rfd900x receieving data from the plane side mounted on a pole and am creating a tcp serial server using an ESP32, the recieved serial stream is cloned by esp32 to its other serial port which is connected with an arduino nano which uses this mavlink library to display status of vehicle on an OLED. If interested, I can share the schematics and code.

1 Like

This is very nice! Plese, share this, I thinks that would be very nice! I would like to use esp32 in a similar way.

Please Ignore the shorted lines. They are not meant to be shorted. New to PCB software hence cant draw them properly.

Top Left side is RFD900x. TOP Right is an ESP32 Dev Module. Please take care of pinout if using any other board. Different versions are available which have different pinouts.

Left Middle is input for ground and PPM from any PPM capable device. Below that is an OLED panel.

Bottom Right is an arduino nano. the CL and DA connections on the arduino are meant to move one pin to the left from the reader’s perspective.

Code for Arduino Nano is as below: (I am not so good at writing code, hence I copied from somewhere and tinkered with it)

#include <Wire.h>
#include “SSD1306Ascii.h”
#include “SSD1306AsciiWire.h”
#include <mavlink.h>
#include <EEPROM.h>

#define I2C_ADDRESS 0x3C
#define RST_PIN -1
SSD1306AsciiWire oled;

#define MAV_TIMEOUT 5000 // mavlink timeout
#define SERIAL_SPEED 115200 // 9600 for sbus, 57600 for cppm
#define DEBUG
mavlink_message_t msg;
mavlink_status_t status;
// mavlink_global_position
int32_t alt, relative_alt;
int16_t vx, vy, vz;
uint16_t hdg;
// __ mavlink_sys_status_t
int8_t battery_remaining;
uint16_t current_battery, voltage_battery, cpu_load, drop_rate_comm;
// mavlink_gps_raw_int_t
int32_t lat, lon, gps_alt;
uint8_t satellites_visible, fix_type;
uint16_t cog, vel;
// oth
uint8_t flag, eeprom_flag;
uint32_t time_flag;
// ------------------------------------------------------------------------------
void setup () {
Wire.begin ();
Wire.setClock (400000L);
oled.begin (& Adafruit128x32, I2C_ADDRESS);
display_wait ();
//
Serial.begin (SERIAL_SPEED);
//
time_flag = millis ();
}
// ------------------------------------------------------------------------------
void loop () {
while (Serial.available ()) {
uint8_t c = Serial.read ();
if (mavlink_parse_char (MAVLINK_COMM_0, c, & msg, & status)) {
flag = 0;
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode (& msg, & packet);
if (packet.hdg == 65535) packet.hdg = 0;
// if (lat!= packet.lat) {lat = packet.lat; set_flag (); }
// if (lon!= packet.lon) {lon = packet.lon; set_flag (); }
// if (alt!= packet.alt) {alt = packet.alt; set_flag (); }
if (relative_alt!= packet.relative_alt) {relative_alt = packet.relative_alt; set_flag (); }
// if (vx!= packet.vx) {vx = packet.vx; set_flag (); }
// if (vy!= packet.vy) {vy = packet.vy; set_flag (); }
// if (vz!= packet.vz) {vz = packet.vz; set_flag (); }
if (hdg!= packet.hdg) {hdg = packet.hdg; set_flag (); }
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
__mavlink_sys_status_t packet;
mavlink_msg_sys_status_decode (& msg, & packet);
if (battery_remaining!= packet.battery_remaining && packet.battery_remaining>= 0) {battery_remaining = packet.battery_remaining; set_flag (); }
if (voltage_battery!= packet.voltage_battery && packet.voltage_battery!= 65535) {voltage_battery = packet.voltage_battery; set_flag (); }
if (current_battery!= packet.current_battery) {current_battery = packet.current_battery; set_flag (); }
if (cpu_load!= packet.load) {cpu_load = packet.load; set_flag (); }
if (drop_rate_comm!= packet.drop_rate_comm) {drop_rate_comm = packet.drop_rate_comm; set_flag (); }
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
break;
}
case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: {
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
break;
}
case MAVLINK_MSG_ID_VFR_HUD: {
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT: {
__mavlink_gps_raw_int_t packet;
mavlink_msg_gps_raw_int_decode (& msg, & packet);
if (packet.cog == 65535) packet.cog = 0;
if (packet.vel == 65535) packet.vel = 0;
if (packet.alt == 65535) packet.alt = 0;
if (lat!= packet.lat) {lat = packet.lat; set_flag (); }
if (lon!= packet.lon) {lon = packet.lon; set_flag (); }
// if (gps_alt!= packet.alt) {gps_alt = packet.alt; set_flag (); }
if (vel!= packet.vel) {vel = packet.vel; set_flag (); }
if (cog!= packet.cog) {cog = packet.cog; set_flag (); }
if (fix_type!= packet.fix_type) {fix_type = packet.fix_type; set_flag (); }
if (satellites_visible!= packet.satellites_visible) {satellites_visible = packet.satellites_visible; set_flag (); }
break;
}
default: {
#ifdef DEBUG
Serial.println (msg.msgid); // see unused packet types
#endif
break;
}
} // switch
if (flag == 1) {
display_data ();
} // print flag
else {
no_data ();
}
} // if mavlink_parse_char
} // while serial available
no_data (); // check no serial input data fuction
}

void set_flag () {
flag = 1;
eeprom_flag = 0;
time_flag = millis ();
}
void display_wait () {
oled.setFont (font8x8);
oled.set2X ();
oled.clear ();
oled.println (“WAIT FOR”);
oled.println (“MAVLINK”);
oled.set1X ();
oled.setFont (font5x7);
}

void display_data () {
oled.clear ();
printL (lat); // gps
oled.print (" “);
printL (lon);
oled.println ();
oled.println ((String) “SA:” + satellites_visible + (String) " F:” + fix_type + (String) “D L:” + cpu_load / 10 + (String) “% E:” + drop_rate_comm / 100 + (String) “%”);
oled.println ((String) “H:” + hdg + (String) " S:" + (uint8_t) (vel / 100 * 3.6) + (String) “kmh A:” + relative_alt / 1000 + (String) "m ");
oled.println ((String) “BAT:” + current_battery / 100.0 + (String) "A " + voltage_battery / 1000.0 + (String) “V R:” + battery_remaining + (String) “%”);
}

void printL (int32_t degE7) {
// Extract and print negative sign
if (degE7 <0) {
degE7 = -degE7;
oled.print (’-’);
}
// whole degrees
int32_t deg = degE7 / 10000000L;
oled.print (deg);
oled.print (’.’);
// Get fractional degrees
degE7 -= deg * 10000000L;
// Print leading zeroes, if needed
int32_t factor = 1000000L;
while ((degE7 <factor) && (factor> 1L)) {
oled.print (‘0’);
factor /= 10L;
}
// Print fractional degrees
oled.print (degE7);
}

void no_data () {
if ((millis () - time_flag)> MAV_TIMEOUT) {// no mavlink data at 2sec
#ifdef DEBUG
Serial.println ((String) “LOST MAVLINK DATA”);
#endif
display_wait ();
delay (300);
if (eeprom_flag == 0 && lat!= 0 && lon!= 0 && fix_type> 1)
{// if gps coordinates present, save it
#ifdef DEBUG
Serial.println (“save …”);
#endif
EEPROM_int32_write (5, lat);
EEPROM_int32_write (16, lon);
EEPROM.write (30, satellites_visible);
EEPROM.write (32, fix_type);
EEPROM.write (34, cpu_load);
EEPROM.write (40, drop_rate_comm);
EEPROM.write (46, hdg);
EEPROM.write (52, vel);
EEPROM_int32_write (56, relative_alt);
EEPROM.write (66, current_battery);
EEPROM.write (72, voltage_battery);
EEPROM.write (74, battery_remaining);
eeprom_flag = 1;
} else
{// no fresh data on mavlink, read from memory
#ifdef DEBUG
Serial.println (“read …”);
#endif
lat = EEPROM_int32_read (5);
lon = EEPROM_int32_read (16);
satellites_visible = EEPROM.read (30);
fix_type = EEPROM.read (32);
cpu_load = EEPROM.read (34);
drop_rate_comm = EEPROM.read (40);
hdg = EEPROM.read (46);
vel = EEPROM.read (52);
relative_alt = EEPROM_int32_read (56);
current_battery = EEPROM.read (66);
voltage_battery = EEPROM.read (72);
battery_remaining = EEPROM.read (74);
eeprom_flag = 1;
}
display_data ();
time_flag = millis ();
}
}

int32_t EEPROM_int32_read (int addr) // read from EEPROM 4 bytes unsigned long
{
byte raw [4];
for (byte i = 0; i <4; i ++) raw [i] = EEPROM.read (addr + i);
int32_t & data = (int32_t &) raw;
return data;
}
// *** *****************
void EEPROM_int32_write (int addr, int32_t data) // write to EEPROM 4 bytes unsigned long
{
byte raw [4];
(int32_t &) raw = data;
for (byte i = 0; i <4; i ++) EEPROM.write (addr + i, raw [i]);
}

The Code for ESP 32 is as below (it is a modified version of ESP32 Serial Bridge) I use only port 8881:

#include <mavlink.h>

// Disclaimer: Don’t use for life support systems
// or any other situations where system failure may affect
// user or environmental safety.

#include “config.h”
#include <esp_wifi.h>
#include <WiFi.h>

HardwareSerial Serial_one(1);
HardwareSerial Serial_two(2);
HardwareSerial* COM[NUM_COM] = {&Serial, &Serial_one , &Serial_two};

#define MAX_NMEA_CLIENTS 1
#ifdef PROTOCOL_TCP
#include <WiFiClient.h>
WiFiServer server_0(SERIAL0_TCP_PORT);
WiFiServer server_1(SERIAL1_TCP_PORT);
WiFiServer server_2(SERIAL2_TCP_PORT);
WiFiServer *server[NUM_COM]={&server_0,&server_1,&server_2};
WiFiClient TCPClient[NUM_COM][MAX_NMEA_CLIENTS];
#endif

uint8_t buf1[NUM_COM][bufferSize];
uint16_t i1[NUM_COM]={0,0,0};

uint8_t buf2[NUM_COM][bufferSize];
uint16_t i2[NUM_COM]={0,0,0};

uint8_t BTbuf[bufferSize];
uint16_t iBT =0;

void setup() {
//delay(500);
COM[0]->begin(UART_BAUD0, SERIAL_PARAM0, SERIAL0_RXPIN, SERIAL0_TXPIN);
COM[1]->begin(UART_BAUD1, SERIAL_PARAM1, SERIAL1_RXPIN, SERIAL1_TXPIN);
COM[2]->begin(UART_BAUD2, SERIAL_PARAM2, SERIAL2_RXPIN, SERIAL2_TXPIN);
if(debug) COM[DEBUG_COM]->println("\n\nHA WiFi serial bridge with MAVLINK V2.0");
#ifdef MODE_AP
if(debug) COM[DEBUG_COM]->println(“Open ESP Access Point mode”);
//AP mode (phone connects directly to ESP) (no router)
WiFi.mode(WIFI_AP);
WiFi.softAPConfig(ip, ip, netmask); // configure ip address for softAP
WiFi.softAP(ssid, pw); // configure ssid and password for softAP
#endif
#ifdef MODE_STA
if(debug) COM[DEBUG_COM]->println(“Open ESP Station mode”);
// STATION mode (ESP connects to router and gets an IP)
// Assuming phone is also connected to that router
// from RoboRemo you must connect to the IP of the ESP
WiFi.mode(WIFI_STA);
WiFi.begin(ssid, pw);
if(debug) COM[DEBUG_COM]->print(“try to Connect to Wireless network: “);
if(debug) COM[DEBUG_COM]->println(ssid);
while (WiFi.status() != WL_CONNECTED) {
//delay(500);
if(debug) COM[DEBUG_COM]->print(”.”);
}
if(debug) COM[DEBUG_COM]->println("\nWiFi connected");
#endif
#ifdef PROTOCOL_TCP
COM[1]->println(“Starting TCP Server 2”);
if(debug) COM[DEBUG_COM]->println(“Starting TCP Server 2”);
server[1]->begin(); // start TCP server
server[1]->setNoDelay(true);
#endif

esp_err_t esp_wifi_set_max_tx_power(100); //lower WiFi Power

}

void loop()
{
#ifdef PROTOCOL_TCP
if (server[1]->hasClient())
{
for(byte i = 0; i < MAX_NMEA_CLIENTS; i++){
//find free/disconnected spot
if (!TCPClient[1][i] || !TCPClient[1][i].connected()){
if(TCPClient[1][i]) TCPClient[1][i].stop();
TCPClient[1][i] = server[1]->available();
if(debug) COM[DEBUG_COM]->print(“New client for COM”);
if(debug) COM[DEBUG_COM]->print(1);
if(debug) COM[DEBUG_COM]->println(i);
continue;
}
}
//no free/disconnected spot so reject
WiFiClient TmpserverClient = server[1]->available();
TmpserverClient.stop();
}
#endif
if(COM[1] != NULL)
{
for(byte cln = 0; cln < MAX_NMEA_CLIENTS; cln++)
{
if(TCPClient[1][cln])
{
while(TCPClient[1][cln].available())
{
buf1[1][i1[1]] = TCPClient[1][cln].read(); // read char from client (LK8000 app)
if(i1[1]<bufferSize-1) i1[1]++;
}

      COM[1]->write(buf1[1], i1[1]); // now send to UART(num):
      i1[1] = 0;
    }
  }
  if(COM[1]->available())
  {
    while(COM[1]->available())
    {     
      //buf2[num][i2[num]] = COM[num]->read(); // read char from UART(num)
      uint8_t c = COM[1]->read();
      buf2[1][i2[1]] = c;
      if(i2[1]<bufferSize-1) i2[1]++;
    }
    // now send to WiFi:
    for(byte cln = 0; cln < MAX_NMEA_CLIENTS; cln++)
    {   
      if(TCPClient[1][cln])                     
        TCPClient[1][cln].write(buf2[1], i2[1]);

//-------------TEST READ FROM TWO COMS AND WRITE TO COM2 FOR ARDUINO NANO----------
COM[2]->write(buf2[1], i2[1]);
//-------------TEST READ FROM TWO COMS AND WRITE TO COM2 FOR ARDUINO NANO----------
}
i2[1] = 0;
}
}
}

The config.h file for ESP32 is as follows:

// config: ////////////////////////////////////////////////////////////
//
//#define BLUETOOTH // NO BLUETOOTH REQUIRED =>> 20191114
//#define OTA_HANDLER // NO OTA FUNCTIONALITY REQUIRED =>> 20191114
//#define MODE_AP // phone connects directly to ESP
#define MODE_STA

#define PROTOCOL_TCP

bool debug = true;

#define VERSION “1.10”

// For AP mode:
const char *ssid = “HA”; // You will connect your phone to this Access Point
const char *pw = “”; // and this is the password
//IPAddress ip(192, 168, 4, 1); // From RoboRemo app, connect to this IP
//IPAddress netmask(255, 255, 255, 0);

// You must connect the phone to this AP, then:
// menu -> connect -> Internet(TCP) -> 192.168.4.1:8880 for UART0
// -> 192.168.4.1:8881 for UART1
// -> 192.168.4.1:8882 for UART2

#define NUM_COM 3 // total number of COM Ports
#define DEBUG_COM 0 // debug output to COM0
/************************* COM Port 0 ******/
#define UART_BAUD0 115200 // Baudrate UART0
#define SERIAL_PARAM0 SERIAL_8N1 // Data/Parity/Stop UART0
#define SERIAL0_RXPIN 21 // receive Pin UART0
#define SERIAL0_TXPIN 1 // transmit Pin UART0
#define SERIAL0_TCP_PORT 8880 // Wifi Port UART0
/
COM Port 1 ******/
#define UART_BAUD1 115200 // Baudrate UART1
#define SERIAL_PARAM1 SERIAL_8N1 // Data/Parity/Stop UART1
#define SERIAL1_RXPIN 16 // receive Pin UART1
#define SERIAL1_TXPIN 17 // transmit Pin UART1
#define SERIAL1_TCP_PORT 8881 // Wifi Port UART1
/
COM Port 2 *******************************/
#define UART_BAUD2 115200 // Baudrate UART2
#define SERIAL_PARAM2 SERIAL_8N1 // Data/Parity/Stop UART2
#define SERIAL2_RXPIN 15 // receive Pin UART2
#define SERIAL2_TXPIN 4 // transmit Pin UART2
#define SERIAL2_TCP_PORT 8882 // Wifi Port UART2

#define bufferSize 4096 // Changed 2048 from 1024

//////////////////////////////////////////////////////////////////////////

1 Like

Thank you very much!!! I definitally want try that.

cheers

Dear Sir,

I am working on a project that is building an autonomous full-scale sailboat using Pixhawk. In my project, I suppose to use 2 stepper motor to control the sail and the rudder, but I don’t know how to create the communication between the Pixhawk and the Arduino as well as from Arduino and the stepper motor. So, I wonder if the code you provided will help me to solve the problem of communicating between the Pixhawk and Arduino for the boat?
Thank you so much, Sir.
Duy.

Dear @duynp89:

I do not fully understand your question. “Communicate” is a broad term. MAVLink is a simple way to send a receive certain data (streams). Depending on your needs, the answer to your question can be yes or no. Please explain a bit more, give details on your cabling and layout and maybe we could help.

Regarding the control of the stepper motors, here the answer is yes, you can use Arduino to control them, but you need a driver to be able to provide enough power to the motors. Again, tell us about the model of the motors, the nominal power to provide. For small steppers you can use the well known L298N (h-brigde), but you mention a “full-scale” boat and probably these will be too small.

Again, provide us with more detail and we will very happy to try to help!

Kind regards,
JP

Dear @jplopezll ,

In my project, I will use 2 stepper motors STP-MTRH-34127, NEMA 34 to control the movement of the sail and the rudder. And I read somewhere that said the Pixhawk could not control the stepper motor directly, it has to be connected via some sources of microcontroller like Arduino. I don’t know if I understand it correctly. Also, I don’t know if I need to use anything else besides 2 stepper motors, such as stepper drive or gearbox.

Here is a short description of my project’s goal. The boat can only travel within the wind, and the Pixhawk has to determine the angle for the sail and the rudder so that the boat can obtain the maximum speed. The Pixhawk will get the information about the wind direction via an anemometer to figure out the possible angles for rudder and sail.
So, I have a difficult time to figure out how to connect the Pixhawk and Arduino, so that after calculating the angles, the Pixhawk will send that information to the Arduino and Arduino will control the stepper motor to rotate to the determine angles.

I am new to the Pixhawk, so I am not too sure if I use the right word to describe it. Please excuse me for that.

Thank you so much, Sir.

Duy

You’ve got quite a lot of reading and learning to do: I strongly suggest building a scale version before tackling a full size sailboat.
You don’t need mavlink to control the stepper motors. I’d suggest googling “pwm control of bipolar stepper motors” as a start point.

Dear @james_pattisone
Thank you so much for your advice. I wish I can build a small scale boat too. But unfortunately, this is the senior design project for mechanical engineering, and the previous year’s team already did the small scale version of the boat, and now our team’s mission is to upscale it. None of our team members have experience with the Pixhawk or coding, so this is a whole new world to us.
We are trying our best to understand how the pixhawk work as well as how to send the signal to the arduino so that it can control the stepper motor.
Once again, thank you so much, Sir.
Duy.

Hi, @duynp89:

As James suggests, you have a lot of reading to do…

Firstly, have a look at the specsheets of the mottors of your choice. I have found some docs in the download tab of this page. Pay attention to (1) the driver you will need to power and control the motors and (2) the braking resistors to avoid damage of the driver when there will be a force back.

What James suggests could be possible: using the pwm signal from the Pixhawk to control the steppers. For this, you will need some circuit to transform the pwm to pulses. It depends a lot on the driver you chose for the motors.

By the way, you can also go with Arduino to create the pulses needed by the driver of the motors. It is all a matter of design choices. As you mention there was a previous scale boat, what were the choices taken there?

If your questions are not precise, we cannot give you precise answers…

KR,
JP

Hi, @NiltonMadede:

Short answer: yes, there is.

Request the appropriate stream from the Arduino and then use the adequate library to display the received data on the display.

Hi @jplopezll, thank you very much for your information on MAVLink and Arduino. From my understanding, there is only for Pixhawk sending data to Arduino? Is it possible Arduino send sensor data to Pixhawk and display in Mission Planner?
Please help me. Thank you very much.

Hi, @Jamieyee16:

I have never tried it, but YES, you should be able to send messages from Arduino to your Ground Station. If you read again the post (in particular the last paragraphs about “Routing”) you will see that each message has a destination node address. Just use 255 as destination address for the Ground Station (Mission Planner in your setup).

In the code you can find that info were it says:

int sysid = 1;                   ///< ID 20 for this airplane. 1 PX, 255 ground station
int compid = 158;                ///< The component sending the message

If you have any futher doubts, just revert.
KR

Don’t forget that you have to send heartbeat as well to activate mavlink routing… otherwise packets will not passed to GCS.

ok, I will try on it.

Have u try to send a customise message? I understand MAVLink has its own unique packet format. Is it the same for the external sensor ( temperature sensor DHT22 ) while sending data to Pixhawk? If YES, what should I define for BYTE 3 ( System ID ), BYTE 4 ( Component ID ) and BYTE 5 ( Message ID )? Thank you for ur help! =)