mavfwd.c

// gcc -o mavfwd mavfwd.c -levent -levent_core -s
#include <arpa/inet.h>
#include <assert.h>
#include <ctype.h>
#include <errno.h>
#include <fcntl.h>
#include <getopt.h>
#include <netinet/in.h>
#include <signal.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>

#include <event2/buffer.h>
#include <event2/bufferevent.h>
#include <event2/event.h>
#include <event2/util.h>

#include "mavlink/common/mavlink.h"

#define MAX_MTU 9000

bool verbose = false;

const char *default_master = "/dev/ttyAMA0";
const int default_baudrate = 115200;
const char *default_out_addr = "127.0.0.1:14600";
const char *default_in_addr = "127.0.0.1:0";
// RC_CHANNELS (#65) for regular MAVLINK RC Channels read (https://mavlink.io/en/messages/common.html#RC_CHANNELS)
const int RC_CHANNELS = 65;
// RC_CHANNELS_RAW (#35) for ExpressLRS,Crossfire and other RC procotols (https://mavlink.io/en/messages/common.html#RC_CHANNELS_RAW)
const int RC_CHANNELS_RAW = 35;

static uint8_t ch_count = 0;
static uint16_t ch[14];
static uint8_t system_id = 1;
static unsigned long long LastWfbSent = 0;

struct bufferevent *serial_bev;
struct sockaddr_in sin_out = {
	.sin_family = AF_INET,
};

int out_sock;
long wait_after_bash = 2000; // Time to wait between bash script starts.
int ChannelPersistPeriodmMS = 2000; // time needed for a RC channel value to persist to execute a commands
long aggregate = 1;

static bool monitor_wfb = false;
static int temp = false;

static void print_usage() {
	printf(
		"Usage: mavfwd [OPTIONS]\n"
		"Where:\n"
		"  -m --master      Local MAVLink master port (%s by default)\n"
		"  -b --baudrate    Serial port baudrate (%d by default)\n"
		"  -o --out         Remote output port (%s by default)\n"
		"  -i --in          Remote input port (%s by default)\n"
		"  -c --channels    RC Channel to listen for commands (0 by default) and call channels.sh\n"
		"  -w --wait        Delay after each command received(2000ms default)\n"
		"  -p --persist     How long a channel value must persist to generate a command (0ms default)\n"
		"  -a --aggregate   Aggregate packets in frames (1 no aggregation, 0 raw data forward, %ld by default)\n"
		"  -f --folder      Folder for file mavlink.msg (default is current folder)\n"
		"  -t --temp        Inject SoC temperature into telemetry\n"
		"  -d --wfb         Monitors wfb.log file and reports errors via mavlink HUD messages\n"
		"  -v --verbose     Display each packet, default not\n"
		"  --help           Display this help\n",
		default_master, default_baudrate, default_out_addr, default_in_addr, aggregate);
}

static speed_t speed_by_value(int baudrate) {
	switch (baudrate) {
	case 9600:
		return B9600;
	case 19200:
		return B19200;
	case 38400:
		return B38400;
	case 57600:
		return B57600;
	case 115200:
		return B115200;
	case 230400:
		return B230400;
	case 460800:
		return B460800;
	case 500000:
		return B500000;
	case 921600:
		return B921600;
	case 1500000:
		return B1500000;
	default:
		printf("Not implemented baudrate %d\n", baudrate);
		exit(EXIT_FAILURE);
	}
}

// in milliseconds
uint64_t get_current_time_ms() {
	struct timespec ts;
	clock_gettime(CLOCK_MONOTONIC, &ts);
	return ts.tv_sec * 1000LL + ts.tv_nsec / 1000000;
}

static bool parse_host_port(const char *s, struct in_addr *out_addr, in_port_t *out_port) {
	char host_and_port[32] = {0};
	strncpy(host_and_port, s, sizeof(host_and_port) - 1);

	char *colon = strchr(host_and_port, ':');
	if (NULL == colon) {
		return -1;
	}

	*colon = '\0';
	const char *host = host_and_port, *port_ptr = colon + 1;

	const bool is_valid_addr = inet_aton(host, out_addr) != 0;
	if (!is_valid_addr) {
		printf("Cannot parse host `%s'.\n", host);
		return false;
	}

	int port;
	if (sscanf(port_ptr, "%d", &port) != 1) {
		printf("Cannot parse port `%s'.\n", port_ptr);
		return false;
	}
	*out_port = htons(port);

	return true;
}

static void signal_cb(evutil_socket_t fd, short event, void *arg) {
	struct event_base *base = arg;
	(void)event;

	printf("%s signal received\n", strsignal(fd));
	event_base_loopbreak(base);
}

static void send_msg_to_groundstation(const char *msg_buf) {
	mavlink_message_t message;
	mavlink_msg_statustext_pack_chan(system_id,
		MAV_COMP_ID_SYSTEM_CONTROL, MAVLINK_COMM_1, &message, 4, msg_buf, 0, 0);

	uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
	int len = mavlink_msg_to_send_buffer(buffer, &message);
	sendto(out_sock, buffer, len, 0, (struct sockaddr *)&sin_out, sizeof(sin_out));
}

static void dump_mavlink_packet(unsigned char *data, const char *direction) {
	uint8_t seq;
	uint8_t sys_id;
	uint8_t comp_id;
	uint32_t msg_id;

	if (data[0] == 0xFE) { // mavlink 1
		seq = data[2];
		sys_id = data[3];
		comp_id = data[4];
		msg_id = data[5];
	} else if (data[0] == 0xFD) { // mavlink 2
		seq = data[4];
		sys_id = data[5];
		comp_id = data[6];
		msg_id = data[7];
	}

	if (verbose)
		printf("%s %#02x sender %d/%d\t%d\t%d\n", direction, data[0], sys_id, comp_id, seq, msg_id);

	uint16_t val;
	if ((msg_id == RC_CHANNELS || msg_id == RC_CHANNELS_RAW) && ch_count > 0) {
		uint8_t offset = 18; // 15 = 1ch;
		for (uint8_t i = 0; i < ch_count; i++) {
			val = data[offset] | (data[offset + 1] << 8);
			if (ch[i] != val) {
				ch[i] = val;
				char buffer[44];
				sprintf(buffer, "channels.sh %d %d &", i + 5, val);
				system(buffer);
				if (verbose)
					printf("called channels.sh %d %d\n", i + 5, val);
			}
			offset = offset + 2;
		} // for
	}	  // msg_id
}

/* https://discuss.ardupilot.org/uploads/short-url/vS0JJd3BQfN9uF4DkY7bAeb6Svd.pdf
 * 0. Message header, always 0xFE
 * 1. Message length
 * 2. Sequence number -- rolls around from 255 to 0 (0x4e, previous was 0x4d)
 * 3. System ID - what system is sending this message
 * 4. Component ID- what component of the system is sending the message
 * 5. Message ID (e.g. 0 = heartbeat and many more! Don’t be shy, you can add too..)
 */
static bool get_mavlink_packet(unsigned char *in_buffer, int buf_len, int *packet_len) {
	if (buf_len < 6 /* header */) {
		return false;
	}
	assert(in_buffer[0] == 0xFE || in_buffer[0] == 0xFD); // mavlink 1 or 2

	uint8_t msg_len = in_buffer[1];
	if (in_buffer[0] == 0xFE)
		*packet_len = 6 /* header */ + msg_len + 2 /* crc */; // mavlink 1
	else
		*packet_len = 10 /* header */ + msg_len + 2 /* crc */; // mavlink 2
	if (buf_len < *packet_len)
		return false;

	dump_mavlink_packet(in_buffer, ">>");

	return true;
}

// Returns num bytes before first occurrence of 0xFE or full data length
static size_t until_first_fe(unsigned char *data, size_t len) {
	for (size_t i = 1; i < len; i++) {
		if (data[i] == 0xFE || data[i] == 0xFD) {
			return i;
		}
	}
	return len;
}

#define MAX_BUFFER_SIZE 50 // Limited by mavlink

static char MavLinkMsgFile[128] = "mavlink.msg";
static char WfbLogFile[128] = "wfb.log";

bool Check4MavlinkMsg(char *buffer) {
	// const char *filename = MavLinkMsgFile;//"mavlink.msg";
	FILE *file = fopen(MavLinkMsgFile, "rb");
	if (file == NULL) // No file, no problem
		return false;

	size_t bytesRead = fread(buffer, 1, MAX_BUFFER_SIZE, file);
	fclose(file);

	if (bytesRead > 0) {
		if (verbose)
			printf("Mavlink msg from file:%s\n", buffer);
		if (remove(MavLinkMsgFile) != 0)
			printf("Error deleting file");
		return true;
	} else
		printf("Mavlink empty file ?!\n");

	return false;
}

/// @brief wfb_tx output should be redirected to wfb.log. Parse it and extracted dropped packets!
/// @return
static bool SendWfbLogToGround() {
	if (!monitor_wfb)
		return false;
	if (abs(get_current_time_ms() - LastWfbSent) < 1000) // Once a second max
		return false;

	LastWfbSent = abs(get_current_time_ms());

	char msg_buf[200];

	FILE *file = fopen(WfbLogFile, "r");
	if (file == NULL) {
		if (verbose)
			printf("No file %s\n", WfbLogFile);
		return false;
	}

	char buff[200];
	int total_dropped_packets = 0;

	/*
	UDP rxq overflow: 2 packets dropped
	UDP rxq overflow: 45 packets dropped
	*/
	if (verbose)
		printf("Parsing file: %s\n", WfbLogFile);

	int maxlinestoparse = 0;
	// Read lines from the file and parse for dropped packets
	while (fgets(buff, sizeof(buff), file) != NULL) {
		// If the file is very long, no need to struggle
		if ((maxlinestoparse++) > 30) {
			total_dropped_packets = 9999;
			break;
		}
		// Check if the line contains "packets dropped"
		if (strstr(buff, "packets dropped") != NULL) {
			// split by space, Parse the line to extract the number of dropped packets
			char *token = strtok(buff, " ");
			while (token != NULL) {
				if (isdigit(token[0])) {
					int dropped_packets = atoi(token);
					total_dropped_packets += dropped_packets;
					break;
				}

				token = strtok(NULL, " ");
			}
		}
	}

	fclose(file);

	// file was empty
	if (maxlinestoparse == 0 && total_dropped_packets == 0)
		return false;

	// remove(WfbLogFile) // This will break console output in the file!
	// Open the file in write mode, truncating it to zero size
	file = fopen(WfbLogFile, "w");
	if (file != NULL)
		fclose(file);

	sprintf(msg_buf, "%d video pckts dropped!\n", total_dropped_packets);
	printf("%s", msg_buf);
	send_msg_to_groundstation(msg_buf);

	return true;
}

static bool SendInfoToGround() {
	char msg_buf[MAX_BUFFER_SIZE];
	if (!Check4MavlinkMsg(msg_buf))
		return false;

	send_msg_to_groundstation(msg_buf);

	return true;
}

uint64_t get_current_time_ms_Old() {
	time_t current_time = time(NULL);
	return (uint64_t)(current_time * 1000);
}

static float last_board_temp;

/// @brief
/// @return -100 if no sigmastar found
static int GetTempSigmaStar() {
	// https://wx.comake.online/doc/doc/SigmaStarDocs-SSD220-SIGMASTAR-202305231834/platform/BSP/Ikayaki/frequency_en.html
	FILE *file = fopen("/sys/devices/virtual/mstar/msys/TEMP_R", "r"); // Temperature 62
	if (file == NULL) {
		if (verbose)
			printf("No temp data at %s\n", WfbLogFile);
		return -100;
	}

	last_board_temp = -100;
	char buff[200];
	// Read lines from the file and parse for dropped packets
	if (fgets(buff, sizeof(buff), file) != NULL) {
		// Find "Temperature"
		char *temperature_str = strstr(buff, "Temperature");
		if (temperature_str != NULL)
			// Extract temperature value
			last_board_temp = atoi(temperature_str + 12);
	}

	fclose(file);

	if (verbose && last_board_temp < -90)
		printf("No temp data in file %s\n", WfbLogFile);

	return last_board_temp;
}

static uint64_t LastTempSent;

static int SendTempToGround(unsigned char *mavbuf) {
	// Once a second
	if (abs(get_current_time_ms() - LastTempSent) < 1000)
		return 0;

	LastTempSent = abs(get_current_time_ms());

	// read the temperature only once per second
	if (temp == 2)
		last_board_temp = GetTempSigmaStar();

	mavlink_message_t message;
	mavlink_msg_raw_imu_pack_chan(system_id, MAV_COMP_ID_SYSTEM_CONTROL, MAVLINK_COMM_1, &message,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, last_board_temp * 100);

	return mavlink_msg_to_send_buffer(mavbuf, &message);
}

bool version_shown = false;
static void ShowVersionOnce(mavlink_message_t *msg, uint8_t header) {
	if (version_shown)
		return;

	version_shown = true;
	/*
	The major version can be determined from the packet start marker byte:
	MAVLink 1: 0xFE
	MAVLink 2: 0xFD
	*/
	if (header == 0xFE)
		printf("Detected MAVLink ver: 1.0  (%d)\n", msg->magic);
	if (header == 0xFD)
		printf("Detected MAVLink version: 2.0  (%d)\n", msg->magic);

	printf("System_id = %d \n", system_id);
}

bool fc_shown = false;

void handle_heartbeat(const mavlink_message_t *message) {
	if (fc_shown)
		return;

	fc_shown = true;
	mavlink_heartbeat_t heartbeat;
	mavlink_msg_heartbeat_decode(message, &heartbeat);

	printf("Flight Controller Type :");

	switch (heartbeat.autopilot) {
	case MAV_AUTOPILOT_GENERIC:
		printf("Generic/INAV");
		break;

	case MAV_AUTOPILOT_ARDUPILOTMEGA:
		printf("ArduPilot");
		break;

	case MAV_AUTOPILOT_PX4:
		printf("PX4");
		break;

	default:
		printf("other");
		break;
	}

	printf("\n");
}

void handle_statustext(const mavlink_message_t *message) {
	mavlink_statustext_t statustext;
	mavlink_msg_statustext_decode(message, &statustext);
	printf("FC message:%s", statustext.text);
}

unsigned long long get_current_time_ms_simple() {
	clock_t current_clock_ticks = clock();
	return (unsigned long long)(current_clock_ticks * 1000 / CLOCKS_PER_SEC);
}

uint16_t channels[18];

// how long a RC value should stay at one level to issue a command
static uint64_t LastStart = 0;
static unsigned long LastValue = 0;
uint16_t NewValue;
static uint64_t NewValueStart = 0;
unsigned int ChannelCmds = 0;
static uint64_t mavpckts_ttl = 0;

void ProcessChannels() {
	// ch_count , not zero based, 1 is first
	uint16_t val = 0;
	if (ch_count < 1 ||
		// wait in the beginning for the values to settle
		ch_count > 16 /* || (mavpckts_ttl<100*/)
		return;

	if (abs(get_current_time_ms() - LastStart) < wait_after_bash)
		return;

	val = channels[ch_count - 1];

	if (abs(val - NewValue) > 32 && ChannelPersistPeriodmMS > 0) {
		// We have a new value, let us wait for it to persist
		NewValue = val;
		NewValueStart = get_current_time_ms();
		return;
	} else if (abs(get_current_time_ms() - NewValueStart) < ChannelPersistPeriodmMS)
		// New value should remain "stable" for a second before being approved
		return;
	else {
	} // New value persisted for more THAN ChannelPersistPeriodmMS

	// the change is too small
	if (abs(val - LastValue) < 32)
		return;

	NewValue = val;
	LastValue = val;

	char buff[60];
	sprintf(buff, "/usr/bin/channels.sh %d %d &", ch_count, val);

	printf("Starting(%d): %s n", ChannelCmds, buff);
	LastStart = get_current_time_ms();

	// intentionally skip the first command, since when stating mavfwd it
	// will always receive some channel value and execute the script
	if (ChannelCmds > 0) {
		system(buff);
	}

	ChannelCmds++;
}

void showchannels(int count) {
	if (verbose) {
		printf("Channels :");
		for (int i = 0; i < count; i++)
			printf("| %02d", channels[i]);
		printf("\n");
	}
}

void handle_msg_id_rc_channels_raw(const mavlink_message_t *message) {
	mavlink_rc_channels_raw_t rc_channels;
	mavlink_msg_rc_channels_raw_decode(message, &rc_channels);
	memcpy(&channels[0], &rc_channels.chan1_raw, 8 * sizeof(uint16_t));
	showchannels(8);
	ProcessChannels();
}

void handle_msg_id_rc_channels_override(const mavlink_message_t *message) {
	mavlink_rc_channels_override_t rc_channels;
	mavlink_msg_rc_channels_override_decode(message, &rc_channels);
	memcpy(&channels[0], &rc_channels.chan1_raw, 18 * sizeof(uint16_t));
	showchannels(18);
	ProcessChannels();
}

void handle_msg_id_rc_channels(const mavlink_message_t *message) {
	mavlink_rc_channels_t rc_channels;
	mavlink_msg_rc_channels_decode(message, &rc_channels);
	memcpy(&channels[0], &rc_channels.chan1_raw, 18 * sizeof(uint16_t));
	showchannels(18);
	ProcessChannels();
}

unsigned char mavbuf[2048];
unsigned int mavbuff_offset = 0;
unsigned int mavpckts_count = 0;

static void process_mavlink(uint8_t *buffer, int count, void *arg) {
	mavlink_message_t message;
	mavlink_status_t status;
	for (int i = 0; i < count; ++i) {
		if (mavbuff_offset > 2000) {
			printf("Mavlink buffer overflowed! Packed lost!\n");
			mavbuff_offset = 0;
		}

		mavbuf[mavbuff_offset] = buffer[i];
		mavbuff_offset++;
		if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &message, &status) == 1) {
			mavpckts_ttl++;
			system_id = message.sysid;
			ShowVersionOnce(&message, (uint8_t)buffer[0]);
			if (verbose)
				printf("Mavlink msg %d no: %d\n", message.msgid, message.seq);

			switch (message.msgid) {
			case MAVLINK_MSG_ID_RC_CHANNELS_RAW: // 35 Used by INAV
				handle_msg_id_rc_channels_raw(&message);
				break;

			case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: // 70
				handle_msg_id_rc_channels_override(&message);
				break;

			case MAVLINK_MSG_ID_RC_CHANNELS: // 65 used by ArduPilot
				handle_msg_id_rc_channels(&message);
				break;

			case MAVLINK_MSG_ID_HEARTBEAT: // Msg info from the FC
				handle_heartbeat(&message);
				break;

			case MAVLINK_MSG_ID_STATUSTEXT: // Msg info from the FC
				// handle_statustext(&message);
				break;
			}

			mavpckts_count++;
			if (aggregate > 0) {
				// We will send whole packets only if packets more than treshold
				if (((aggregate >= 1 && aggregate < 50) && mavpckts_count >= aggregate) ||
					// if buffer more than treshold MAVLINK_MSG_ID_ATTITUDE will always cause the buffer flushed
					((aggregate > 50 && aggregate < 2000) && mavbuff_offset >= aggregate) ||
						((mavpckts_count >= 3) && message.msgid == MAVLINK_MSG_ID_ATTITUDE)) {
					// flush and send all data
					if (sendto(out_sock, mavbuf, mavbuff_offset, 0, (struct sockaddr *)&sin_out,
							sizeof(sin_out)) == -1) {
						perror("sendto()");
						event_base_loopbreak(arg);
					}

					if (verbose)
						printf("%d Pckts / %d bytes sent\n", mavpckts_count, mavbuff_offset);

					mavbuff_offset = 0;
					mavpckts_count = 0;

					SendInfoToGround();
					SendWfbLogToGround();

					if (last_board_temp > -100) {
						mavbuff_offset = SendTempToGround(mavbuf);
					}

					if (mavbuff_offset > 0) {
						mavpckts_count++;
					}
				}
			}
		} // if mavlink_parse_char
	}
}

static long ttl_packets = 0;
static long ttl_bytes = 0;

static void serial_read_cb(struct bufferevent *bev, void *arg) {
	struct evbuffer *input = bufferevent_get_input(bev);
	int packet_len, in_len;
	struct event_base *base = arg;

	while ((in_len = evbuffer_get_length(input))) {
		unsigned char *data = evbuffer_pullup(input, in_len);
		if (data == NULL) {
			return;
		}
		packet_len = in_len;

		// First forward all serial input to UDP.
		ttl_packets++;
		ttl_bytes += packet_len;

		// If garbage only, give some feedback do diagnose
		if (!version_shown && ttl_packets % 10 == 3)
			printf("Packets:%ld  Bytes:%ld\n", ttl_packets, ttl_bytes);

		if (aggregate == 0) {
			if (sendto(out_sock, data, packet_len, 0, (struct sockaddr *)&sin_out,
					sizeof(sin_out)) == -1) {
				perror("sendto()");
				event_base_loopbreak(base);
			}
		}

		// Let's try to parse the stream
		// if no RC channel control needed, only forward the data
		if (aggregate > 0 || ch_count > 0)
			// Let's try to parse the stream
			process_mavlink(data, packet_len, arg);

		evbuffer_drain(input, packet_len);
	}
}

// Signal handler function
void sendtestmsg(int signum) {
	printf("Sending test mavlink msg.\n");
	char buff[200];
	sprintf(buff, "echo Hello_From_OpenIPC > %s", MavLinkMsgFile);
	system(buff);
	SendInfoToGround();
}

static void serial_event_cb(struct bufferevent *bev, short events, void *arg) {
	(void)bev;
	struct event_base *base = arg;

	if (events & (BEV_EVENT_EOF | BEV_EVENT_ERROR | BEV_EVENT_TIMEOUT)) {
		printf("Serial connection closed\n");
		event_base_loopbreak(base);
	}
}

static void in_read(evutil_socket_t sock, short event, void *arg) {
	(void)event;
	unsigned char buf[MAX_MTU];
	struct event_base *base = arg;
	ssize_t nread;

	nread = recvfrom(sock, &buf, sizeof(buf) - 1, 0, NULL, NULL);
	if (nread == -1) {
		perror("recvfrom()");
		event_base_loopbreak(base);
	}

	if (nread > 6) {
		dump_mavlink_packet(buf, "<<");
		bufferevent_write(serial_bev, buf, nread);
	}
}

static void *setup_temp_mem(off_t base, size_t size) {
	int mem_fd;

	mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
	if (mem_fd < 0) {
		fprintf(stderr, "can't open /dev/mem\n");
		return NULL;
	}

	char *mapped_area = mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, base);
	if (mapped_area == MAP_FAILED) {
		fprintf(stderr, "read_mem_reg mmap error: %s (%d)\n", strerror(errno), errno);
		return NULL;
	}

	uint32_t MISC_CTRL45 = 0;

	// Set the T-Sensor cyclic capture mode by configuring MISC_CTRL45 bit[30]
	MISC_CTRL45 |= 1 << 30;

	// Set the capture period by configuring MISC_CTRL45 bit[27:20]
	// The formula for calculating the cyclic capture periodis as follows:
	//	T = N x 2 (ms)
	//	N is the value of MISC_CTRL45 bit[27:20]
	MISC_CTRL45 |= 50 << 20;

	// Enable the T-Sensor by configuring MISC_CTRL45 bit[31] and start to collect the temperature
	MISC_CTRL45 |= 1 << 31;

	*(volatile uint32_t *)(mapped_area + 0xB4) = MISC_CTRL45;

	return mapped_area;
}

static void temp_read(evutil_socket_t sock, short event, void *arg) {
	(void)sock;
	(void)event;
	char *mapped_area = arg;

	uint32_t val = *(volatile uint32_t *)(mapped_area + 0xBC);
	float tempo = val & ((1 << 16) - 1);
	tempo = ((tempo - 117) / 798) * 165 - 40;

	// only once
	if (last_board_temp == -100)
		printf("Temp read %f C\n", tempo);

	last_board_temp = tempo;
}

static int handle_data(
	const char *port_name, int baudrate, const char *out_addr, const char *in_addr) {
	struct event_base *base = NULL;
	struct event *sig_int = NULL, *in_ev = NULL, *temp_tmr = NULL;
	int ret = EXIT_SUCCESS;

	int serial_fd = open(port_name, O_RDWR | O_NOCTTY);
	if (serial_fd < 0) {
		printf("Error while openning port %s: %s\n", port_name, strerror(errno));
		return EXIT_FAILURE;
	};
	evutil_make_socket_nonblocking(serial_fd);

	struct termios options;
	tcgetattr(serial_fd, &options);
	cfsetspeed(&options, speed_by_value(baudrate));

	options.c_cflag &= ~CSIZE;	// Mask the character size bits
	options.c_cflag |= CS8;		// 8 bit data
	options.c_cflag &= ~PARENB; // set parity to no
	options.c_cflag &= ~PARODD; // set parity to no
	options.c_cflag &= ~CSTOPB; // set one stop bit

	options.c_cflag |= (CLOCAL | CREAD);
	options.c_oflag &= ~OPOST;
	options.c_lflag &= 0;
	options.c_iflag &= 0; // disable software flow controll
	options.c_oflag &= 0;

	cfmakeraw(&options);
	tcsetattr(serial_fd, TCSANOW, &options);

	out_sock = socket(AF_INET, SOCK_DGRAM, 0);

	int in_sock = 0;

	printf("Listening on %s...\n", port_name);

	struct sockaddr_in sin_in = {
		.sin_family = AF_INET,
	};

	if (!parse_host_port(in_addr, (struct in_addr *)&sin_in.sin_addr.s_addr, &sin_in.sin_port))
		goto err;
	if (!parse_host_port(out_addr, (struct in_addr *)&sin_out.sin_addr.s_addr, &sin_out.sin_port))
		goto err;

	if (sin_in.sin_port > 0)
		in_sock = socket(AF_INET, SOCK_DGRAM, 0);

	if (in_sock > 0 &&
		bind(in_sock, (struct sockaddr *)&sin_in, sizeof(sin_in))) { // we may not need this
		perror("bind()");
		exit(EXIT_FAILURE);
	}
	if (in_sock > 0)
		printf("Listening on %s...\n", in_addr);

	base = event_base_new();

	sig_int = evsignal_new(base, SIGINT, signal_cb, base);
	event_add(sig_int, NULL);
	// it's recommended by libevent authors to ignore SIGPIPE
	signal(SIGPIPE, SIG_IGN);

	// Test inject a simple packet to test malvink communication Camera to Ground
	signal(SIGUSR1, sendtestmsg);

	serial_bev = bufferevent_socket_new(base, serial_fd, 0);
	bufferevent_setcb(serial_bev, serial_read_cb, NULL, serial_event_cb, base);
	bufferevent_enable(serial_bev, EV_READ);

	if (in_sock > 0) {
		in_ev = event_new(base, in_sock, EV_READ | EV_PERSIST, in_read, NULL);
		event_add(in_ev, NULL);
	}
	if (temp) {
		// SigmaStar
		if (GetTempSigmaStar() > -90) {
			temp = 2;
			printf("Found SigmaStart temp sensor\n");
		// Goke/Hisilicon method
		} else {
			void *mem = setup_temp_mem(0x12028000, 0xFFFF);
			temp_tmr = event_new(base, -1, EV_PERSIST, temp_read, mem);
			evtimer_add(temp_tmr, &(struct timeval){.tv_sec = 1});
		}
	}

	event_base_dispatch(base);

err:
	if (temp_tmr) {
		event_del(temp_tmr);
		event_free(temp_tmr);
	}
	if (out_sock > 0)
		close(out_sock);

	if (in_sock > 0)
		close(in_sock);

	if (serial_fd >= 0)
		close(serial_fd);

	if (serial_bev)
		bufferevent_free(serial_bev);

	if (in_ev) {
		event_del(in_ev);
		event_free(in_ev);
	}

	if (sig_int)
		event_free(sig_int);

	if (base)
		event_base_free(base);

	libevent_global_shutdown();

	return ret;
}

int main(int argc, char **argv) {
	const struct option long_options[] = {
		{"master", required_argument, NULL, 'm'},
		{"baudrate", required_argument, NULL, 'b'},
		{"out", required_argument, NULL, 'o'},
		{"in", required_argument, NULL, 'i'},
		{"channels", required_argument, NULL, 'c'},
		{"wait", required_argument, NULL, 'w'},
		{"persist", required_argument, NULL, 'p'},
		{"aggregate", required_argument, NULL, 'a'},
		{"folder", required_argument, NULL, 'f'},
		{"temp", no_argument, NULL, 't'},
		{"wfb", no_argument, NULL, 'j'},
		{"verbose", no_argument, NULL, 'v'},
		{"help", no_argument, NULL, 'h'},
		{NULL, 0, NULL, 0}
	};

	const char *port_name = default_master;
	int baudrate = default_baudrate;
	const char *out_addr = default_out_addr;
	const char *in_addr = default_in_addr;
	int opt = 0, long_index = 0;
	last_board_temp = -100;

	while ((opt = getopt_long(argc, argv, "m:b:o:i:c:w:p:a:f:tvjh", long_options, &long_index)) != -1) {
		switch (opt) {
		case 'm':
			port_name = optarg;
			break;

		case 'b':
			baudrate = atoi(optarg);
			break;

		case 'o':
			out_addr = optarg;
			break;

		case 'i':
			in_addr = optarg;
			break;

		case 'c':
			ch_count = atoi(optarg);
			if (ch_count == 0)
				printf("rc_channels  monitoring disabled\n");
			else
				printf("Monitoring RC channel %d\n", ch_count);

			LastStart = get_current_time_ms();
			break;

		case 'w':
			wait_after_bash = atoi(optarg);
			LastStart = get_current_time_ms();
			break;

		case 'p':
			ChannelPersistPeriodmMS = atoi(optarg);
			LastStart = get_current_time_ms();
			break;

		case 'a':
			aggregate = atoi(optarg);
			if (aggregate > 2000)
				aggregate = 2000;
			if (aggregate == 0)
				printf("No parsing, raw UART to UDP only\n");
			else if (aggregate < 50)
				printf("Aggregate mavlink pckts in packs of %ld\n", aggregate);
			else if (aggregate > 50)
				printf("Aggregate mavlink pckts till buffer reaches %ld bytes\n", aggregate);
			break;

		case 'f':
			if (optarg != NULL) {
				snprintf(MavLinkMsgFile, sizeof(MavLinkMsgFile), "%smavlink.msg", optarg);
				snprintf(WfbLogFile, sizeof(MavLinkMsgFile), "%swfb.log", optarg);
			}
			break;

		case 't':
			// 1 HiSilicon/Goke, 2 SigmaStar
			temp = 1;
			break;

		case 'j':
			monitor_wfb = true;
			break;

		case 'v':
			verbose = true;
			printf("Verbose mode!\n");
			break;

		case 'h':
		default:
			print_usage();
			return EXIT_SUCCESS;
		}
	}

	return handle_data(port_name, baudrate, out_addr, in_addr);
}