Read two Mavlink streams in same loop

example.cpp

#include <iostream>
#include <cstring>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <thread>

#include "ardupilotmega/mavlink.h"

int open_socket(uint16_t port)
{
  int sock = socket(AF_INET, SOCK_DGRAM, 0);
  struct sockaddr_in bind_addr;
  memset(&bind_addr, 0, sizeof(bind_addr));
  bind_addr.sin_family = AF_INET;
  bind_addr.sin_addr.s_addr = htonl(INADDR_ANY);
  bind_addr.sin_port = htons(port);
  bind(sock, (struct sockaddr *) &bind_addr, sizeof(bind_addr));
  return sock;
}

void process_packet(mavlink_message_t& msg, mavlink_status_t &status)
{
  printf("Received mavlink packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n",
         msg.sysid, msg.compid, msg.len, msg.msgid);
}

void read_from_sock(mavlink_channel_t mavlink_channel, int sock) {
  uint8_t buf[2048];
  struct sockaddr_in client_addr;
  socklen_t client_len;
  ssize_t readlen = recvfrom(sock, buf, 2048, 0, (struct sockaddr *) &client_addr, &client_len);
  mavlink_message_t msg;
  mavlink_status_t status;
  for(int i = 0; i < readlen; ++i) {
    if (mavlink_parse_char(mavlink_channel, buf[i], &msg, &status)) {
      process_packet(msg, status);
    }
  }
}

int main()
{
  int telem_sock = open_socket(14550);
  int sensor_sock = open_socket(14551);
  int max_fd = std::max(telem_sock, sensor_sock);
  fd_set sock_set;

  while(true) {

    FD_ZERO(&sock_set);
    FD_SET(telem_sock, &sock_set);
    FD_SET(sensor_sock, &sock_set);

    int n = select(max_fd + 1, &sock_set, NULL, NULL, NULL);
    if (n > 0) {
      if (FD_ISSET(telem_sock, &sock_set)) {
        read_from_sock(MAVLINK_COMM_0, telem_sock);
      }
      if (FD_ISSET(sensor_sock, &sock_set)) {
        read_from_sock(MAVLINK_COMM_1, sensor_sock);
      }
    }
  }

  return 0;
}