Fix breakage to the DSM parser introduced with the input prioritisation logic. Back out to a "any input wins" strategy; connecting multiple receivers to I/O at the same time is currently not supported (read: strange things will happen).

7 files changed, 84 insertions, 88 deletions

diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 40ea38cf7..480e3f5cc 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.
@@ -132,13 +132,7 @@ comms_main(void)
 			/* populate the report */
 			for (int 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..6cf3c80ac 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,62 @@ 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.
+		 */
 		if (fds[0].revents & POLLIN)
 			dsm_input();
 		if (fds[1].revents & POLLIN)
 			sbus_input();
+		ppm_input();
+
+		/*
+		 * 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;
 
-		/* XXX do ppm processing, bypass mode, etc. here */
+			/* 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..5841f29a3 100644
--- a/apps/px4io/dsm.c
+++ b/apps/px4io/dsm.c
@@ -275,10 +275,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 +296,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 +310,22 @@ 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;
+		/* XXX check actual values */
+		system_state.rc_channel_data[channel] = 988 + value;
 	}
 
-	/* DSM input is valid */
-	system_state.dsm_input_ok = true;
+	/* 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;
 
-	/* check if no S.BUS data is available */
-	if (!system_state.sbus_input_ok) {
-
-		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..63a55d840 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -69,12 +69,8 @@
 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)
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c
index c3949f2b0..25fe0cf38 100644
--- a/apps/px4io/sbus.c
+++ b/apps/px4io/sbus.c
@@ -195,17 +195,16 @@ 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;
+	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 +227,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;
 }