There is severe Mavlink packet loss on simple example code and I have no idea why it happens

Hello.

[1]
Terminal Output of MAVLink Message Reception(Mavlink Example code) :

file descriptor opened as 3
Received message ID  : 125
Received message Sequence : 40
Received message ID  : 36
Received message Sequence : 43
Received message ID  : 65
Received message Sequence : 44
Received message ID  : 74
Received message Sequence : 56
Received message ID  : 125
Received message Sequence : 65
Received message ID  : 42
Received message Sequence : 67
Received message ID  : 36
Received message Sequence : 68
Received message ID  : 116
Received message Sequence : 71
Received message ID  : 29
Received message Sequence : 72
Received message ID  : 241
Received message Sequence : 102
Received message ID  : 74
Received message Sequence : 106
Received message ID  : 125
Received message Sequence : 115
Received message ID  : 42
Received message Sequence : 117
Received message ID  : 36
Received message Sequence : 118
Received message ID  : 29
Received message Sequence : 122
Received message ID  : 74
Received message Sequence : 156
Received message ID  : 42
Received message Sequence : 167
Received message ID  : 36
Received message Sequence : 168
Received message ID  : 125
Received message Sequence : 190
Received message ID  : 42
Received message Sequence : 192
Received message ID  : 36
Received message Sequence : 193
Received message ID  : 65
Received message Sequence : 194
Received message ID  : 74
Received message Sequence : 206
Received message ID  : 111
Received message Sequence : 213
Received message ID  : 125
Received message Sequence : 216
Received message ID  : 42
Received message Sequence : 218
Received message ID  : 36
Received message Sequence : 219
Received message ID  : 116
Received message Sequence : 222
Received message ID  : 125
Received message Sequence : 241
Received message ID  : 42
Received message Sequence : 243
Received message ID  : 36
Received message Sequence : 244
Received message ID  : 65
Received message Sequence : 245
Received message ID  : 241
Received message Sequence : 253
Received message ID  : 74
Received message Sequence : 1
Received message ID  : 42
Received message Sequence : 12
Received message ID  : 241
Received message Sequence : 49
Received message ID  : 42
Received message Sequence : 64
Received message ID  : 36
Received message Sequence : 65
Received message ID  : 125
Received message Sequence : 112
Received message ID  : 42
Received message Sequence : 114
Received message ID  : 36
Received message Sequence : 115
Received message ID  : 125
Received message Sequence : 137
Received message ID  : 125
Received message Sequence : 162
Received message ID  : 27
Received message Sequence : 192
Received message ID  : 116
Received message Sequence : 193
Received message ID  : 29
Received message Sequence : 194
Received message ID  : 241
Received message Sequence : 199
Received message ID  : 74
Received message Sequence : 203
Received message ID  : 125
Received message Sequence : 212
Received message ID  : 42
Received message Sequence : 214
Received message ID  : 36
Received message Sequence : 215
Received message ID  : 125
Received message Sequence : 237
Received message ID  : 42
Received message Sequence : 239
Received message ID  : 36
Received message Sequence : 240
Received message ID  : 65
Received message Sequence : 241
Received message ID  : 74
Received message Sequence : 253
Received message ID  : 125
Received message Sequence : 6
Received message ID  : 42
Received message Sequence : 8
Received message ID  : 36
Received message Sequence : 9
Received message ID  : 125
Received message Sequence : 31
Received message ID  : 42
Received message Sequence : 33
Received message ID  : 36
Received message Sequence : 34
Received message ID  : 65
Received message Sequence : 35
Received message ID  : 241
Received message Sequence : 43
Received message ID  : 74
Received message Sequence : 47

As you can see, the packet loss is severe(The message sequence does not increase by 1 and suddenly jumps, not linear)

[2] My code :

/* ~included for receiving serial char~ */
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdint.h>
#include <fcntl.h> //open()
#include <termios.h>
#include <errno.h>
#include <sys/ioctl.h>
/* ~inluded for receiving serial char~ */

#include "../include/mavlink/common/mavlink.h"
#include <iostream> //include for std::cout

int main(int argc, char *argv[])
{
int file_descriptor;
char c;
char buf[64] = "temp text";
struct termios toptions;

  /* open serial port */
file_descriptor = open("/dev/ttyUSB0", O_RDWR);
printf("file descriptor opened as %i\n", file_descriptor);

/* get current serial port settings to termios struct. */
tcgetattr(file_descriptor, &toptions);
/* set baud rate*/
cfsetispeed(&toptions, B57600);
cfsetospeed(&toptions, B57600);

/* 8 bits, no parity, 1 stop bits */
toptions.c_cflag &= ~PARENB;
toptions.c_cflag &= ~CSTOPB; /* only need 1 stop bit */
toptions.c_cflag &= ~CSIZE;
toptions.c_cflag |= CS8;

/* Enable Hardware flow control */
toptions.c_cflag |= CRTSCTS; /* enable hardware flow control */
toptions.c_iflag &= ~(IXON | IXOFF | IXANY);/* Disable software flow control */

/* Enable Software flow control */
//toptions.c_iflag |= (IXON | IXOFF | IXANY); /* Enable software flow control (XON/XOFF) */
//toptions.c_cflag &= ~CRTSCTS; /* no hardware flow control */

/* Disable both Hardware and Software Flow Control */
//toptions.c_cflag &= ~CRTSCTS; /* no hardware flow control */
//toptions.c_iflag &= ~(IXON | IXOFF | IXANY);/* Disable software flow control */

/* Canonical mode(for line-based input) */
// toptions.c_lflag |= ICANON; //canonical mode on
toptions.c_lflag &= ~ICANON; //canonical mode off

/* commit the serial port settings */
/* TCSANOW : the change shall occur immediately.
TCSADRAIN : the change shall occur after all output written to folders is transmitted.
this function should be used when changing parameters that affect output.
TCSAFLUSH : the change shall occur after all output written to folders is transmitted,
and all input so far received but not read shall be dsicarded before the change is made. */
tcsetattr(file_descriptor, TCSANOW, &toptions);

mavlink_status_t status;
mavlink_message_t msg;
int chan = MAVLINK_COMM_0;
uint8_t mavlink_byte;

while(true) {
int n = read(file_descriptor, &c, 1);

if(n > 0) {
mavlink_byte = static_cast<uint8_t>(c);
if(mavlink_parse_char(chan, mavlink_byte, &msg, &status)){
std::cout << "Received message ID  : " << static_cast<int>(msg.msgid) << std::endl;
std::cout << "Received message Sequence : " << static_cast<int>(msg.seq) << std::endl;
}
}
if(n < 1) {
perror("read");
break;
}
//add a small delay to reduce CPU usage <- don't use this. it will ruin the serial input result.
//usleep(10000); //10ms delay
}
close(file_descriptor);

return 0;
}//end of main()

[3] My Environment

  • Software: Latest ArduRover (2024.08)
  • Hardware: Jetson TK1 board (OS: Linux 4 Tegra 21.8, Jetpack 3.1)
  • Connection: Pixhawk 1 connected to Jetson TK1 via FT232

[4] What I’ve tried

I can successfully receive strings (e.g., “This is string”) from an Arduino Nano using the same code, suggesting no fundamental issue with the serial reception code.

I’ve also experimented with various serial port settings, but the result was same.

Additionally, I’ve tested the FT232 and Pixhawk connection with Windows Mission Planner, and it worked perfectly, so I believe the hardware setup is fine.

[5] Additional Question

When analyzing the raw serial data, I noticed that the “payload_length” field seems to be missing when the message ID type is VFR_HUD.

Is this normal, or could there be an issue with my serial communication?

The strange thing is, if I add the code below, MAVLink parsing suddenly stops working.

usleep(10000); //10ms delay


In Windows Mission Planner, the packet rate shows 100%, but on my Jetson TK1 board, I am experiencing close to 70% packet loss. It seems like MAVLink is very sensitive, and there might be an issue with the serial communication code on the Linux board.

I checked that the library is MAVLink v2, not MAVLink v1. The Pixhawk’s “SERIAL1_BAUD” is set to 57600, and the protocol is set to MAVLink v2.

hi,

I think that is a 10 seconds delay !! Are you sure .

[1]
Thank you for pointing out the issue with the delay() functions. However, please note that the line in question is commented out in the actual code. It has been a long time since I used delay() in my code, so it is not related to the problem.

[2]
I have uploaded a partial, but not perfect, solution here:

Hello,

sleep(10000) is 10000ms = 10 seconds, so if I use the sleep function, you are correct.

However, the function name is usleep, which measures time in microseconds, not milliseconds. So, a usleep(10000) call results in a 10-millisecond delay.

1 Like

Problem solved.

It was due to the misuse of the dialect.