Servers by jDrones

MAVLink and Arduino: step by step

Is there ONE source that has all the code? Putting together all the snippets is a pain. I am looking for code that doesn’t run LEDs - it simply outputs requested parameters and allows the user to send Mavlink messages to change parameters inside a PixHawk.

I would greatly appreciate if someone posted a .ZIP file or some other source that was directly compilable. I need to start from this code and add my own functionality.

I figured out how to get things working to a basic degree. I’m using a TEENSY 3.2, and the PixHawk is connected to Serial1. My computer is connected to the USB port (which is Serial0). This thread is so long maybe I missed it but I would like to be able to read the current MODE (STABILIZE, AUTO etc) of the controller, and I would like to be able to change the MODE of the controller, as well as change the direction of movement.

Can anyone give me a few pointers on how to accomplish this?

Hi @jplopezll,

I having some doubts and need help from everyone. I am building an obstacle avoidance system for F450 quadcopter by using hc-sr04 ultrasonic sensor. The arduino UNO TX is connect to RX telem2 pixhawk 2.4.8. It seem like there is no function to the pitch and roll. I was wondering there are some problem in my coding. Below are the code to connect arduino with MAVlink. Thanks.

void FHeartBeat() {
mavlink_message_t msg;
uint16_t len;
// System ID = 255 = GCS
mavlink_msg_heartbeat_pack(1, 0, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, 0, 1, 0);

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

// Send the message (.write sends as bytes)
Serial.write(buf, len);


void RCOverride(mavlink_message_t *msg, uint16_t len, uint8_t *buf, uint16_t PitchOut, uint16_t RollOut) {
//Pack and send the calculated Pitch and Roll data. Only send if the data is new
mavlink_msg_rc_channels_override_pack(1, 0 , msg, 1, 0, RollOut, PitchOut, 0, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, msg);
Serial.write(buf, len);
Serial.print("\n\rPitch: “);
Serial.print(" Roll: ");

Dear @jplopezll:
Your way of encode MAVlink is so readable and clear .Thank you so much! I got the msgids from PIXhawk,but the msgids I got are imperfect.
In short, the msgid I want to get is 141.
I have been tried to change the MAVStreams,but it no useful.

>     MAV_DATA_STREAM_ALL=0, // Enable all data streams | 
>     MAV_DATA_STREAM_EXTRA1=10, // Dependent on the autopilot | 
>     MAV_DATA_STREAM_EXTRA2=11, // Dependent on the autopilot |
>     MAV_DATA_STREAM_EXTRA3=12, //Dependent on the autopilot | 

I have tried each MAVStreams.But the msgids don’t seem to have changed.
It will be grateful if you could answer my dump questions.I will try my best to answer asap.By the way,there may be jet lag,because I am in China.
best wishes for you!:grinning:

Hi, @Agent_of_Fooyou:

Sorry for the belated reply, I have not much spare time nowadays…

I canno’t find any stream that include 141 as a reply message, I am almost sure it is not implemented. I don’t know what is your final need, at first sight you have two choices:

  1. Implement the function: write the code, compile, upload to the Pixhawk. Complex task and error prone.

  2. Calculate the altitude yourself based on the pressure reported by the barometer, the GPS and the initial offset. Sounds complex but it is more or less trivial if you do not need extreme precission in the calculations. You need to request for MAV_DATA_STREAM_RAW_SENSORS.

If you need further help, better send me a PM.


Hi, @charles_linquist:

Sorry for the belated answer.

As far as I know, you cannot request for the mode via streams. But you can read the value of the RC channel where you send the mode and calculate in which mode you are based on your configuration.

You can also override the value of that channel to manually force a mode on the flight controller. Be careful when doing this, as you may lose control of the aircraft.

For more focused recommendations I would need to know more about your setup and objectives. Send me a PM if you need further assistance.

Kind regards.

Hi, Juan. Your post is the most helpful to understand how to receive data from Mavlink.

I’m having one problem right now, probably you can help.
My code is working but somehow the code -> mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status) always return the value as zero. Do you have any idea why?

Solved, I did twice Mav_Request_Data() that’s why it doens’t work.

1 Like

Dear @Fazlur_Rahman:

Thank you very much for your kind words. Good to see that your problem is already solved!

Do not hesitate to ask if you need further help.

Kind regards.

Dear @jplopezll
I’m working on a connection between Pixhawk and Arduino and I want to control the movement of the drone from the Arduino according to the ultra sound sensor. I can reicive pixhawk packets on the Arduino and see them on the Arduino’s serial monitor, but my objectif is to write an Arduino code that allows me to send commands to the Pixhawk to control it (Land if there is an obstacle). and also see it on QGroundControl.
My problem now is that I do not know what functions I should use and how to use it.
Please I need HELP.
And Thank you soo much.


Yours is not an easy question and has many potential answers depending on your setup and your objectives.

Have a look at the definitions and descriptions of the common part of the MAVLink library and revert with what could be your proposal. Either me or anybody else will be willing to help!


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.



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?



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 = ();
if (mavlink_parse_char (MAVLINK_COMM_0, c, & msg, & status)) {
flag = 0;
switch (msg.msgid) {
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode (& msg, & packet);
if (packet.hdg == 65535) packet.hdg = 0;
// if (lat!= {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 (); }
__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 (); }
__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!= {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 (); }
default: {
#ifdef DEBUG
Serial.println (msg.msgid); // see unused packet types
} // 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”);
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 …”);
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 …”);
lat = EEPROM_int32_read (5);
lon = EEPROM_int32_read (16);
satellites_visible = (30);
fix_type = (32);
cpu_load = (34);
drop_rate_comm = (40);
hdg = (46);
vel = (52);
relative_alt = EEPROM_int32_read (56);
current_battery = (66);
voltage_battery = (72);
battery_remaining = (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] = (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};

#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};

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() {
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.softAPConfig(ip, ip, netmask); // configure ip address for softAP
WiFi.softAP(ssid, pw); // configure ssid and password for softAP
#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.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) {
if(debug) COM[DEBUG_COM]->print(”.”);
if(debug) COM[DEBUG_COM]->println("\nWiFi connected");
COM[1]->println(“Starting TCP Server 2”);
if(debug) COM[DEBUG_COM]->println(“Starting TCP Server 2”);
server[1]->begin(); // start TCP server

esp_err_t esp_wifi_set_max_tx_power(100); //lower WiFi Power


void loop()
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);
//no free/disconnected spot so reject
WiFiClient TmpserverClient = server[1]->available();
if(COM[1] != NULL)
for(byte cln = 0; cln < MAX_NMEA_CLIENTS; cln++)
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;
      //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++)
        TCPClient[1][cln].write(buf2[1], i2[1]);

COM[2]->write(buf2[1], i2[1]);
i2[1] = 0;

The config.h file for ESP32 is as follows:

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


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) -> for UART0
// -> for UART1
// -> 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.


Servers by jDrones