Created
August 26, 2018 22:20
-
-
Save vo/3c01dc18bf26cef97f4d8a495cab3438 to your computer and use it in GitHub Desktop.
Publish incoming serial Mavlink to ZeroMQ
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment