Merged DSM fixes

1 files changed, 67 insertions, 9 deletions

diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 0e0210373..e64f8b204 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -55,6 +55,7 @@
 #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"
@@ -63,15 +64,18 @@
 #define RC_CHANNEL_HIGH_THRESH		1600
 #define RC_CHANNEL_LOW_THRESH		1400
 
+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;
 
@@ -79,10 +83,32 @@ 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();
 
 		/* force manual input override */
 		if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) {
@@ -92,16 +118,48 @@ controls_main(void)
 			system_state.mixer_use_fmu = true;
 		}
 
-		/* detect rc loss event */
-		if (hrt_absolute_time() - system_state.rc_channels_timestamp > RC_FAILSAFE_TIMEOUT) {
-			system_state.rc_lost = true;
-		}
+		/*
+		 * 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) {
 
-		/* XXX detect fmu loss event */
+			/* set the number of channels to zero - no inputs */
+			system_state.rc_channels = 0;
+			system_state.rc_lost = true;
 
-		/* XXX handle failsave events - RC loss and FMU loss - here */
+			/* trigger an immediate report to the FMU */
+			system_state.fmu_report_due = true;
+		}
 
 		/* 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;
+	}	
+}