Merge pull request #94 from PX4/DSM-decoder-fix

Untangle the DSM decoder from the input source priority logic, clean up input handling some more.

8 files changed, 187 insertions, 103 deletions

diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 456564ba7..9f3dba047 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -88,6 +88,8 @@ public:
 
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
 
+	bool			dump_one;
+
 private:
 	static const unsigned	_max_actuators = PX4IO_OUTPUT_CHANNELS;
 
@@ -175,6 +177,7 @@ PX4IO	*g_dev;
 
 PX4IO::PX4IO() :
 	CDev("px4io", "/dev/px4io"),
+	dump_one(false),
 	_serial_fd(-1),
 	_io_stream(nullptr),
 	_task(-1),
@@ -478,6 +481,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
 
 	_send_needed = true;
 
+	/* if monitoring, dump the received info */
+	if (dump_one) {
+		dump_one = false;
+
+		printf("IO: %s armed ", rep->armed ? "" : "not");
+		for (unsigned i = 0; i < rep->channel_count; i++)
+			printf("%d: %d ", i, rep->rc_channel[i]);
+		printf("\n");
+	}
+
 out:
 	unlock();
 }
@@ -665,6 +678,30 @@ test(void)
 	exit(0);
 }
 
+void
+monitor(void)
+{
+	unsigned cancels = 4;
+	printf("Hit <enter> three times to exit monitor mode\n");
+
+	for (;;) {
+		pollfd fds[1];
+
+		fds[0].fd = 0;
+		fds[0].events = POLLIN;
+		poll(fds, 1, 500);
+
+		if (fds[0].revents == POLLIN) {
+			int c;
+			read(0, &c, 1);
+			if (cancels-- == 0)
+				exit(0);
+		}
+
+		if (g_dev != nullptr)
+			g_dev->dump_one = true;
+	}
+}
 }
 
 int
@@ -740,8 +777,11 @@ px4io_main(int argc, char *argv[])
 	    !strcmp(argv[1], "rx_sbus") ||
 	    !strcmp(argv[1], "rx_ppm"))
 		errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]);
+
 	if (!strcmp(argv[1], "test"))
 		test();
+	if (!strcmp(argv[1], "monitor"))
+		monitor();
 
-	errx(1, "need a command, try 'start', 'test', 'rx_ppm', 'rx_dsm', 'rx_sbus' or 'update'");
+	errx(1, "need a command, try 'start', 'test', 'monitor' or 'update'");
 }
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 40ea38cf7..83a006d43 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -100,8 +100,8 @@ comms_main(void)
 	debug("FMU: ready");
 
 	for (;;) {
-		/* wait for serial data, but no more than 100ms */
-		poll(&fds, 1, 100);
+		/* wait for serial data, but no more than 10ms */
+		poll(&fds, 1, 10);
 
 		/*
 		 * Pull bytes from FMU and feed them to the HX engine.
@@ -130,15 +130,9 @@ comms_main(void)
 			last_report_time = now;
 
 			/* populate the report */
-			for (int i = 0; i < system_state.rc_channels; i++)
+			for (unsigned i = 0; i < system_state.rc_channels; i++)
 				report.rc_channel[i] = system_state.rc_channel_data[i];
-
-			if (system_state.sbus_input_ok || system_state.dsm_input_ok || system_state.ppm_input_ok) {
-				report.channel_count = system_state.rc_channels;
-			} else {
-				report.channel_count = 0;
-			}
-			
+			report.channel_count = system_state.rc_channels;
 			report.armed = system_state.armed;
 
 			/* and send it */
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index d4eace3df..3b3782918 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -55,19 +55,23 @@
 #include <drivers/drv_hrt.h>
 #include <systemlib/hx_stream.h>
 #include <systemlib/perf_counter.h>
+#include <systemlib/ppm_decode.h>
 
 #define DEBUG
 #include "px4io.h"
 
+static void	ppm_input(void);
+
 void
 controls_main(void)
 {
 	struct pollfd fds[2];
 
+	/* DSM input */
 	fds[0].fd = dsm_init("/dev/ttyS0");
 	fds[0].events = POLLIN;
 
-
+	/* S.bus input */
 	fds[1].fd = sbus_init("/dev/ttyS2");
 	fds[1].events = POLLIN;
 
@@ -75,14 +79,76 @@ controls_main(void)
 		/* run this loop at ~100Hz */
 		poll(fds, 2, 10);
 
+		/*
+		 * Gather R/C control inputs from supported sources.
+		 *
+		 * Note that if you're silly enough to connect more than
+		 * one control input source, they're going to fight each
+		 * other.  Don't do that.
+		 */
+		bool locked = false;
+
 		if (fds[0].revents & POLLIN)
-			dsm_input();
+			locked |= dsm_input();
 		if (fds[1].revents & POLLIN)
-			sbus_input();
+			locked |= sbus_input();
+
+		/*
+		 * If we don't have lock from one of the serial receivers,
+		 * look for PPM. It shares an input with S.bus, so there's
+		 * a possibility it will mis-parse an S.bus frame.
+		 *
+		 * XXX each S.bus frame will cause a PPM decoder interrupt
+		 * storm (lots of edges).  It might be sensible to actually
+		 * disable the PPM decoder completely if we have an alternate
+		 * receiver lock.
+		 */
+		if (!locked)
+			ppm_input();
 
-		/* XXX do ppm processing, bypass mode, etc. here */
+		/*
+		 * If we haven't seen any new control data in 200ms, assume we
+		 * have lost input and tell FMU.
+		 */
+		if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
+
+			/* set the number of channels to zero - no inputs */
+			system_state.rc_channels = 0;
+
+			/* trigger an immediate report to the FMU */
+			system_state.fmu_report_due = true;
+		}
+
+		/* XXX do bypass mode, etc. here */
 
 		/* do PWM output updates */
 		mixer_tick();
 	}
 }
+
+static void
+ppm_input(void)
+{
+	/*
+	 * Look for new PPM input.
+	 */
+	if (ppm_last_valid_decode != 0) {
+
+		/* avoid racing with PPM updates */
+		irqstate_t state = irqsave();
+
+		/* PPM data exists, copy it */
+		system_state.rc_channels = ppm_decoded_channels;
+		for (unsigned i = 0; i < ppm_decoded_channels; i++)
+			system_state.rc_channel_data[i] = ppm_buffer[i];
+
+		/* copy the timestamp and clear it */
+		system_state.rc_channels_timestamp = ppm_last_valid_decode;
+		ppm_last_valid_decode = 0;
+
+		irqrestore(state);
+
+		/* trigger an immediate report to the FMU */
+		system_state.fmu_report_due = true;
+	}	
+}
diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c
index 744dac3d6..2611f3a03 100644
--- a/apps/px4io/dsm.c
+++ b/apps/px4io/dsm.c
@@ -104,7 +104,7 @@ dsm_init(const char *device)
 	return dsm_fd;
 }
 
-void
+bool
 dsm_input(void)
 {
 	ssize_t		ret;
@@ -141,7 +141,7 @@ dsm_input(void)
 
 	/* if the read failed for any reason, just give up here */
 	if (ret < 1)
-		return;
+		goto out;
 	last_rx_time = now;
 
 	/*
@@ -153,7 +153,7 @@ dsm_input(void)
 	 * If we don't have a full frame, return
 	 */
 	if (partial_frame_count < DSM_FRAME_SIZE)
-	 	return;
+	 	goto out;
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
@@ -161,6 +161,12 @@ dsm_input(void)
 	 */
 	dsm_decode(now);
 	partial_frame_count = 0;
+
+out:
+	/*
+	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked' 
+	 */
+	return (now - last_frame_time) < 200000;
 }
 
 static bool
@@ -275,10 +281,13 @@ dsm_decode(hrt_abstime frame_time)
 	 */
 	if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
 		dsm_guess_format(true);
+
+	/* we have received something we think is a frame */
 	last_frame_time = frame_time;
+
+	/* if we don't know the frame format, update the guessing state machine */
 	if (channel_shift == 0) {
 		dsm_guess_format(false);
-		system_state.dsm_input_ok = false;
 		return;
 	}
 
@@ -293,10 +302,6 @@ dsm_decode(hrt_abstime frame_time)
 	 * seven channels are being transmitted.
 	 */
 
-	const unsigned dsm_chancount = (DSM_FRAME_CHANNELS < PX4IO_INPUT_CHANNELS) ? DSM_FRAME_CHANNELS : PX4IO_INPUT_CHANNELS;
-
-	uint16_t dsm_channels[dsm_chancount];
-
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
 		uint8_t *dp = &frame[2 + (2 * i)];
@@ -311,31 +316,40 @@ dsm_decode(hrt_abstime frame_time)
 			continue;
 
 		/* update the decoded channel count */
-		if (channel > ppm_decoded_channels)
-			ppm_decoded_channels = channel;
+		if (channel >= system_state.rc_channels)
+			system_state.rc_channels = channel + 1;
 
 		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
 		if (channel_shift == 11)
 			value /= 2;
-
-		/* stuff the decoded channel into the PPM input buffer */
-		dsm_channels[channel] = 988 + value;
+		value += 998;
+
+		/* 
+		 * Store the decoded channel into the R/C input buffer, taking into
+		 * account the different ideas about channel assignement that we have.
+		 *
+		 * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw,
+		 * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw.
+		 */
+		switch (channel) {
+		case 0:
+			channel = 2;
+			break;
+		case 1:
+			channel = 0;
+			break;
+		case 2:
+			channel = 1;
+		default:
+			break;
+		}
+		system_state.rc_channel_data[channel] = value;
 	}
 
-	/* DSM input is valid */
-	system_state.dsm_input_ok = true;
-
-	/* check if no S.BUS data is available */
-	if (!system_state.sbus_input_ok) {
+	/* and note that we have received data from the R/C controller */
+	/* XXX failsafe will cause problems here - need a strategy for detecting it */
+	system_state.rc_channels_timestamp = frame_time;
 
-		for (unsigned i = 0; i < dsm_chancount; i++) {
-			system_state.rc_channel_data[i] = dsm_channels[i];
-		}
-
-		/* and note that we have received data from the R/C controller */
-		/* XXX failsafe will cause problems here - need a strategy for detecting it */
-		system_state.rc_channels_timestamp = frame_time;
-		system_state.rc_channels = dsm_chancount;
-		system_state.fmu_report_due = true;
-	}
+	/* trigger an immediate report to the FMU */
+	system_state.fmu_report_due = true;
 }
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index 483e9fe4d..f02e98ae4 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -50,8 +50,6 @@
 
 #include <drivers/drv_pwm_output.h>
 
-#include <systemlib/ppm_decode.h>
-
 #include "px4io.h"
 
 /*
@@ -59,10 +57,6 @@
  */
 static unsigned fmu_input_drops;
 #define FMU_INPUT_DROP_LIMIT	20
-/*
- * Collect RC input data from the controller source(s).
- */
-static void	mixer_get_rc_input(void);
 
 /*
  * Update a mixer based on the current control signals.
@@ -89,12 +83,6 @@ mixer_tick(void)
 	bool should_arm;
 
 	/*
-	 * Start by looking for R/C control inputs.
-	 * This updates system_state with any control inputs received.
-	 */
-	mixer_get_rc_input();
-
-	/*
 	 * Decide which set of inputs we're using.
 	 */
 	if (system_state.mixer_use_fmu) {
@@ -122,8 +110,10 @@ mixer_tick(void)
 
 	} else {
 		/* we have no control input */
+		/* XXX builtin failsafe would activate here */
 		control_count = 0;
 	}
+
 	/*
 	 * Tickle each mixer, if we have control data.
 	 */
@@ -142,8 +132,7 @@ mixer_tick(void)
 	/*
 	 * Decide whether the servos should be armed right now.
 	 */
-
-	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0) && system_state.mixer_use_fmu;
+	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
@@ -166,30 +155,3 @@ mixer_update(int mixer, uint16_t *inputs, int input_count)
 		mixers[mixer].current_value = 0;
 	}
 }
-
-static void
-mixer_get_rc_input(void)
-{
-	/* if we haven't seen any new data in 200ms, assume we have lost input and tell FMU */
-	if ((hrt_absolute_time() - ppm_last_valid_decode) > 200000) {
-
-		/* input was ok and timed out, mark as update */
-		if (system_state.ppm_input_ok) {
-			system_state.ppm_input_ok = false;
-			system_state.fmu_report_due = true;
-		}
-		return;
-	}
-
-	/* mark PPM as valid */
-	system_state.ppm_input_ok = true;
-
-	/* check if no DSM and S.BUS data is available */
-	if (!system_state.sbus_input_ok && !system_state.dsm_input_ok) {
-		/* otherwise, copy channel data */
-		system_state.rc_channels = ppm_decoded_channels;
-		for (unsigned i = 0; i < ppm_decoded_channels; i++)
-			system_state.rc_channel_data[i] = ppm_buffer[i];
-		system_state.fmu_report_due = true;
-	}
-}
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index 77524797f..a3ac9e3e7 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -44,6 +44,7 @@
 #include <debug.h>
 #include <stdlib.h>
 #include <errno.h>
+#include <string.h>
 
 #include <nuttx/clock.h>
 
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 483b9bcc8..45b7cf847 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -69,17 +69,13 @@
 struct sys_state_s 
 {
 
-	bool		armed;		/* IO armed */
-	bool		arm_ok;		/* FMU says OK to arm */
-
-	bool		ppm_input_ok;	/* valid PPM input data */
-	bool		dsm_input_ok;	/* valid Spektrum DSM data */
-	bool		sbus_input_ok;	/* valid Futaba S.Bus data */
+	bool		armed;			/* IO armed */
+	bool		arm_ok;			/* FMU says OK to arm */
 
 	/*
 	 * Data from the remote control input(s)
 	 */
-	int		rc_channels;
+	unsigned	rc_channels;
 	uint16_t	rc_channel_data[PX4IO_INPUT_CHANNELS];
 	uint64_t	rc_channels_timestamp;
 
@@ -169,9 +165,9 @@ extern void	comms_main(void) __attribute__((noreturn));
  */
 extern void	controls_main(void);
 extern int	dsm_init(const char *device);
-extern void	dsm_input(void);
+extern bool	dsm_input(void);
 extern int	sbus_init(const char *device);
-extern void	sbus_input(void);
+extern bool	sbus_input(void);
 
 /*
  * Assertion codes
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c
index c3949f2b0..a8f628a84 100644
--- a/apps/px4io/sbus.c
+++ b/apps/px4io/sbus.c
@@ -58,6 +58,7 @@
 static int sbus_fd = -1;
 
 static hrt_abstime last_rx_time;
+static hrt_abstime last_frame_time;
 
 static uint8_t	frame[SBUS_FRAME_SIZE];
 
@@ -94,7 +95,7 @@ sbus_init(const char *device)
 	return sbus_fd;
 }
 
-void
+bool
 sbus_input(void)
 {
 	ssize_t		ret;
@@ -131,7 +132,7 @@ sbus_input(void)
 
 	/* if the read failed for any reason, just give up here */
 	if (ret < 1)
-		return;
+		goto out;
 	last_rx_time = now;
 
 	/*
@@ -143,7 +144,7 @@ sbus_input(void)
 	 * If we don't have a full frame, return
 	 */
 	if (partial_frame_count < SBUS_FRAME_SIZE)
-	 	return;
+	 	goto out;
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
@@ -151,6 +152,12 @@ sbus_input(void)
 	 */
 	sbus_decode(now);
 	partial_frame_count = 0;
+
+out:
+	/*
+	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked' 
+	 */
+	return (now - last_frame_time) < 200000;
 }
 
 /*
@@ -195,17 +202,19 @@ sbus_decode(hrt_abstime frame_time)
 	/* check frame boundary markers to avoid out-of-sync cases */
 	if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
 		sbus_frame_drops++;
-		system_state.sbus_input_ok = false;
 		return;
 	}
 
-	/* if the failsafe bit is set, we consider that a loss of RX signal */
+	/* if the failsafe bit is set, we consider the frame invalid */
 	if (frame[23] & (1 << 4)) {
-		system_state.sbus_input_ok = false;
 		return;
 	}
 
-	unsigned chancount = (PX4IO_INPUT_CHANNELS > 16) ? 16 : PX4IO_INPUT_CHANNELS;
+	/* we have received something we think is a frame */
+	last_frame_time = frame_time;
+
+	unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? 
+		SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
 
 	/* use the decoder matrix to extract channel data */
 	for (unsigned channel = 0; channel < chancount; channel++) {
@@ -228,14 +237,16 @@ sbus_decode(hrt_abstime frame_time)
 	}
 
 	if (PX4IO_INPUT_CHANNELS >= 18) {
-		/* decode two switch channels */
 		chancount = 18;
+		/* XXX decode the two switch channels */
 	}
 
+	/* note the number of channels decoded */
 	system_state.rc_channels = chancount;
-	system_state.sbus_input_ok = true;
-	system_state.fmu_report_due = true;
 
 	/* and note that we have received data from the R/C controller */
 	system_state.rc_channels_timestamp = frame_time;	
+
+	/* trigger an immediate report to the FMU */
+	system_state.fmu_report_due = true;
 }