Rework the PX4IO software architecture:

 - Use a separate thread for handing R/C inputs and outputs.
 - Remove all PX4IO R/C receiver configuration; it's all automatic now.
 - Rework the main loop, dedicate it to PX4FMU communications after startup.
 - Fix several issues in the px4io driver that would cause a crash if PX4IO was not responding.

1 files changed, 39 insertions, 114 deletions

diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index c44b677d1..a93ef9cb8 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -32,10 +32,11 @@
  ****************************************************************************/
 
 /**
- * @file FMU communication for the PX4IO module.
+ * @file comms.c
+ *
+ * FMU communication for the PX4IO module.
  */
 
-
 #include <nuttx/config.h>
 #include <stdio.h>
 #include <stdbool.h>
@@ -44,7 +45,6 @@
 #include <debug.h>
 #include <stdlib.h>
 #include <errno.h>
-#include <termios.h>
 #include <string.h>
 #include <poll.h>
 
@@ -54,101 +54,50 @@
 #include <systemlib/hx_stream.h>
 #include <systemlib/perf_counter.h>
 
+#define DEBUG
 #include "px4io.h"
 
 #define FMU_MIN_REPORT_INTERVAL	  5000	/*  5ms */
 #define FMU_MAX_REPORT_INTERVAL	100000	/* 100ms */
 
+int frame_rx;
+int frame_bad;
+
 static int			fmu_fd;
 static hx_stream_t		stream;
-static int			rx_fd;
 static struct px4io_report	report;
 
 static void			comms_handle_frame(void *arg, const void *buffer, size_t length);
 
-static struct pollfd pollfds[2];
-static int pollcount;
-
 void
 comms_init(void)
 {
 	/* initialise the FMU interface */
 	fmu_fd = open("/dev/ttyS1", O_RDWR | O_NONBLOCK);
-	if (fmu_fd < 0)
-		lib_lowprintf("COMMS: fmu open failed %d\n", errno);
 	stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
-	pollfds[0].fd = fmu_fd;
-	pollfds[0].events = POLLIN;
-	pollcount = 1;
 
 	/* default state in the report to FMU */
 	report.i2f_magic = I2F_MAGIC;
-
 }
 
-static void
-serial_rx_init(unsigned serial_mode)
+void
+comms_main(void)
 {
-	if (serial_mode == system_state.serial_rx_mode)
-		return;
-	system_state.serial_rx_mode = serial_mode;
-
-	if (serial_mode == RX_MODE_PPM_ONLY) {
-		if (rx_fd != -1) {
-			pollcount = 1;
-			close(rx_fd);
-			rx_fd = -1;
-		}
-		return;
-	}
-
-	/* open the serial port used for DSM and S.bus communication */
-	rx_fd = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK);
-	pollfds[1].fd = rx_fd;
-	pollfds[1].events = POLLIN;
-	pollcount = 2;
-
-	struct termios t;
-	tcgetattr(rx_fd, &t);
 
-	switch (serial_mode) {
-	case RX_MODE_DSM:
-		/* 115200, no parity, one stop bit */
-		cfsetspeed(&t, 115200);
-		t.c_cflag &= ~(CSTOPB | PARENB);
+	struct pollfd fds;
+	fds.fd = fmu_fd;
+	fds.events = POLLIN;
+	debug("FMU: ready");
 
-		dsm_init(serial_mode);
-		break;
+	for (;;) {
+		/* wait for serial data, but no more than 100ms */
+		poll(&fds, 1, 100);
 
-	case RX_MODE_FUTABA_SBUS:
-		/* 100000, even parity, two stop bits */
-		cfsetspeed(&t, 100000);
-		t.c_cflag |= (CSTOPB | PARENB);
-
-		sbus_init(serial_mode);
-		break;
-
-	default:
-		break;
-	}
-
-	tcsetattr(rx_fd, TCSANOW, &t);
-}
-
-void
-comms_check(void)
-{
-	/*
-	 * Check for serial data
-	 */
-	int ret = poll(pollfds, pollcount, 0);
-
-	if (ret > 0) {
 		/*
 		 * Pull bytes from FMU and feed them to the HX engine.
 		 * Limit the number of bytes we actually process on any one iteration.
 		 */
-		if (pollfds[0].revents & POLLIN) {
+		if (fds.revents & POLLIN) {
 			char buf[32];
 			ssize_t count = read(fmu_fd, buf, sizeof(buf));
 			for (int i = 0; i < count; i++)
@@ -156,54 +105,32 @@ comms_check(void)
 		}
 
 		/*
-		 * Pull bytes from the serial RX port and feed them to the decoder
-		 * if we care about serial RX data.
+		 * Decide if it's time to send an update to the FMU.
 		 */
-		if ((pollcount > 1) && (pollfds[1].revents & POLLIN)) {
-			switch (system_state.serial_rx_mode) {
-			case RX_MODE_DSM:
-				dsm_input(rx_fd);
-				break;
-
-			case RX_MODE_FUTABA_SBUS:
-				sbus_input(rx_fd);
-				break;
-
-			default:
-				break;
-			}
+		static hrt_abstime last_report_time;
+		hrt_abstime now, delta;
+
+		/* should we send a report to the FMU? */
+		now = hrt_absolute_time();
+		delta = now - last_report_time;
+		if ((delta > FMU_MIN_REPORT_INTERVAL) && 
+		    (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
+
+			system_state.fmu_report_due = false;
+			last_report_time = now;
+
+			/* populate the report */
+			for (int i = 0; i < system_state.rc_channels; i++)
+				report.rc_channel[i] = system_state.rc_channel_data[i];
+			report.channel_count = system_state.rc_channels;
+			report.armed = system_state.armed;
+
+			/* and send it */
+			hx_stream_send(stream, &report, sizeof(report));
 		}
 	}
-
-	/*
-	 * Decide if it's time to send an update to the FMU.
-	 */
-	static hrt_abstime last_report_time;
-	hrt_abstime now, delta;
-
-	/* should we send a report to the FMU? */
-	now = hrt_absolute_time();
-	delta = now - last_report_time;
-	if ((delta > FMU_MIN_REPORT_INTERVAL) && 
-	    (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
-
-		system_state.fmu_report_due = false;
-		last_report_time = now;
-
-		/* populate the report */
-		for (int i = 0; i < system_state.rc_channels; i++)
-			report.rc_channel[i] = system_state.rc_channel_data[i];
-		report.channel_count = system_state.rc_channels;
-		report.armed = system_state.armed;
-
-		/* and send it */
-		hx_stream_send(stream, &report, sizeof(report));
-	}
 }
 
-int frame_rx;
-int frame_bad;
-
 static void
 comms_handle_config(const void *buffer, size_t length)
 {
@@ -215,8 +142,6 @@ comms_handle_config(const void *buffer, size_t length)
 	}
 
 	frame_rx++;
-
-	serial_rx_init(cfg->serial_rx_mode);
 }
 
 static void
@@ -274,9 +199,9 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
 			comms_handle_config(buffer, length);
 			break;
 		default:
+		    	frame_bad++;
 			break;
 		}
 	}
-    	frame_bad++;
 }