Publish incoming serial Mavlink to ZeroMQ

mavlinktozmq.cc

#include <iostream>
#include <csignal>
#include <cstring>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <zmq.h>
#include "mavlink/ardupilotmega/mavlink.h"

inline bool set_baud(termios *cfg, speed_t baud)
{
  return (cfsetispeed(cfg, baud)) >= 0 && (cfsetospeed(cfg, baud)) >= 0;
}

bool select_baud(termios *cfg, uint32_t baud)
{
  bool result;
  switch (baud) {
    case 1200:
      result = set_baud(cfg, B1200);
      break;
    case 1800:
      result = set_baud(cfg, B1800);
      break;
    case 9600:
      result = set_baud(cfg, B9600);
      break;
    case 19200:
      result = set_baud(cfg, B19200);
      break;
    case 38400:
      result = set_baud(cfg, B38400);
      break;
    case 115200:
      result = set_baud(cfg, B115200);
      break;
    case 921600:
      result = set_baud(cfg, B921600);
      break;
    default:
    case 57600:
      result = set_baud(cfg, B57600);
      break;
  }
  if (not result) {
    std::cerr << "Could not set baud rate " << baud << std::endl;
  }
  return result;
}

int open_serial_port(const char *port, uint32_t baud)
{
  int fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
  if (fd == -1) {
    std::cerr << "Couldn't open serial port." << std::endl;
    return -1;
  }
  fcntl(fd, F_SETFL, 0);
  if (!isatty(fd)) {
    std::cerr << "File descriptor is NOT a serial port" << std::endl;
    return -1;
  }
  struct termios config;
  if (tcgetattr(fd, &config) < 0) {
    std::cerr << "Could not read configuration of serial port" << std::endl;
    return -1;
  }
  config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
  config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);
  config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
  config.c_cflag &= ~(CSIZE | PARENB);
  config.c_cflag |= CS8;
  config.c_cc[VMIN] = 1;
  config.c_cc[VTIME] = 10; // was 0

  select_baud(&config, baud);

  if (tcsetattr(fd, TCSAFLUSH, &config) < 0) {
    std::cerr << "Could not read configuration of serial port" << std::endl;
    return -1;
  }
  return fd;
}

int main()
{
  // open serial port
  const char *port = "/dev/ttyUSB0";
  const uint32_t baud = 57600;
  int fd = open_serial_port("/dev/ttyUSB0", 57600);
  if (fd < 0) {
    std::cerr << "Error opening serial port" << std::endl;
    return -1;
  }
  std::cout << "Opened serial port " << port << ", baud " << baud << std::endl;

  // open zeromq socket
  const char *zmq_bind_addr = "tcp://*:14440";
  void *zmq_ctx = zmq_ctx_new();
  void *zmq_sock = zmq_socket(zmq_ctx, ZMQ_PUB);
  int zmq_rc = zmq_bind(zmq_sock, zmq_bind_addr);
  if (zmq_rc != 0) {
    std::cerr << "Error binding ZeroMQ socket" << std::endl;
    return -1;
  }
  std::cout << "Opened ZeroMQ socket: " << zmq_bind_addr << std::endl;

  // start reading
  fd_set read_fds;
  mavlink_message_t msg;
  mavlink_status_t status;
  while (true) {
    FD_ZERO(&read_fds);
    FD_SET(fd, &read_fds);
    uint8_t data[2048];
    int activity = select(fd + 1, &read_fds, NULL, NULL, NULL);
    if (activity > 0 && FD_ISSET(fd, &read_fds)) {
      ssize_t n = read(fd, data, 2048);
      for (int i = 0; i < n; ++i) {
        if (mavlink_parse_char(MAVLINK_COMM_0, data[i], &msg, &status)) {
          uint8_t out[MAVLINK_MAX_PACKET_LEN];
          uint16_t bytes_msg = mavlink_msg_to_send_buffer(out, &msg);
          zmq_send(zmq_sock, out, bytes_msg, 0);
        }
      }
    }
  }

  // clean up
  close(fd);
  zmq_close(zmq_sock);
  zmq_ctx_destroy(zmq_ctx);

  return 0;
}