Hello, I want to receive any information from PX4 via MAVLink. Therefore, I wrote simple code that prints MAVLink data to the screen. The purpose of the code is to open the serial port, listen for incoming data, and print it to the screen. When I run this code, it does not produce any output. Then, I close the C++ file I wrote, run the QGC application or the mavlink_shell.py file under PX4-Autopilot Tools, and close the file. When I try the code again, it works this time. Why does the code start working correctly after running QGC?
Do I need to do anything extra for the code to work correctly? Do I need to send some data first to get PX4 to start broadcasting MAVLink data when it is initially started?
Do I need to send some data first to initiate MAVLink in the code?
#include <iostream>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include "mavlink/common/mavlink.h"
#define SERIAL_PORT "/dev/ttyACM0"
int main() {
int fd = open(SERIAL_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (fd < 0) {
std::cerr << "****" << std::endl;
return -1;
}
struct termios tty;
if (tcgetattr(fd, &tty) != 0) {
std::cerr << ",,," << std::endl;
close(fd);
return -1;
}
cfsetospeed(&tty, B115200);
cfsetispeed(&tty, B115200);
tty.c_cflag |= (CLOCAL | CREAD);
tty.c_cflag &= ~PARENB;
tty.c_cflag &= ~CSTOPB;
tty.c_cflag &= ~CSIZE;
tty.c_cflag |= CS8;
tcsetattr(fd, TCSANOW, &tty);
while (true) {
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
ssize_t recsize = read(fd, buffer, sizeof(buffer));
if (recsize > 0) {
mavlink_message_t msg;
mavlink_status_t status;
for (ssize_t i = 0; i < recsize; i++) {
if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, &status)) {
if (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
mavlink_heartbeat_t heartbeat;
mavlink_msg_heartbeat_decode(&msg, &heartbeat);
std::cout << "rx: " << heartbeat.custom_mode << std::endl;
if (heartbeat.custom_mode == 99) {
close(fd);
return 0;
}
}
}
}
} else if (recsize < 0) {
std::cerr << "-*-----" << std::endl;
close(fd);
return -1;
}
}
close(fd);
return 0;
}