I’ve written a tcp communication example code using the udp communication example currently posted on the mavlink github. When I run this code and connect tcp from QGroundControl, I’m getting the heartbeat message from QGC fine. But when I press mode change in QGC, I’m not getting the mavlink message for it. The output value always shows the heartbeat message with message id 0. Can you tell me where I’m going wrong?
My environment right now is using QGC on one computer and running the code on the same computer for testing, and the fixhawk is connected to the computer via usb-c to change modes.
and this is my code
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h> // for close
#include <time.h>
#include "mavlink.h"
void receive_some(int client_fd);
void handle_heartbeat(const mavlink_message_t* message);
void send_some(int client_fd);
void send_heartbeat(int client_fd);
int main(int argc, char* argv[])
{
int listen_fd = socket(PF_INET, SOCK_STREAM, 0);
if (listen_fd < 0) {
printf("socket error: %s\n", strerror(errno));
return -1;
}
struct sockaddr_in addr = {};
memset(&addr, 0, sizeof(addr));
addr.sin_family = AF_INET;
addr.sin_port = htons(42224);
addr.sin_addr.s_addr = htonl(INADDR_ANY);
if (bind(listen_fd, (struct sockaddr *)&addr, sizeof(addr)) != 0) {
printf("bind error: %s\n", strerror(errno));
return -2;
}
if (listen(listen_fd, 5) != 0) {
printf("listen error: %s\n", strerror(errno));
return -3;
}
struct sockaddr_in client_addr = {};
socklen_t client_addr_len = sizeof(client_addr);
int client_fd = accept(listen_fd, (struct sockaddr *)&client_addr, &client_addr_len);
if (client_fd < 0) {
printf("accept error: %s\n", strerror(errno));
return -4;
}
while (true) {
receive_some(client_fd);
send_some(client_fd);
}
close(client_fd);
close(listen_fd);
return 0;
}
void receive_some(int client_fd) {
mavlink_message_t message;
mavlink_status_t status;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
ssize_t len = recv(client_fd, buffer, sizeof(buffer), 0);
if (len > 0) {
for (int i = 0; i < len; ++i) {
if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &message, &status)) {
printf("Received packet: SYS = %d, COMP = %d, LEN = %d, MSG ID = %d\n",
message.sysid, message.compid, message.len, message.msgid);
switch (message.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
handle_heartbeat(&message);
break;
}
}
}
}
}
void handle_heartbeat(const mavlink_message_t* message) {
mavlink_heartbeat_t heartbeat;
mavlink_msg_heartbeat_decode(message, &heartbeat);
printf("Got heartbeat from ");
switch (heartbeat.autopilot) {
case MAV_AUTOPILOT_GENERIC:
printf("generic");
break;
case MAV_AUTOPILOT_ARDUPILOTMEGA:
printf("ArduPilot");
break;
case MAV_AUTOPILOT_PX4:
printf("PX4");
break;
default:
printf("other");
break;
}
printf(" autopilot\n");
}
void send_some(int client_fd) {
static time_t last_time = 0;
time_t current_time = time(NULL);
if (current_time - last_time >= 1) {
send_heartbeat(client_fd);
last_time = current_time;
}
}
void send_heartbeat(int client_fd) {
mavlink_message_t message;
const uint8_t system_id = 42;
const uint8_t component_id = MAV_COMP_ID_PERIPHERAL;
mavlink_msg_heartbeat_pack(system_id, component_id, &message, MAV_TYPE_GENERIC, MAV_AUTOPILOT_GENERIC, 0, 0, MAV_STATE_STANDBY);
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
int len = mavlink_msg_to_send_buffer(buffer, &message);
send(client_fd, buffer, len, 0);
}