Lock out the PPM decoder if the DSM or S.bus decoders have seen a frame recently.

4 files changed, 42 insertions, 12 deletions

diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 6cf3c80ac..3b3782918 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -86,15 +86,29 @@ controls_main(void)
 		 * 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();
-		ppm_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();
 
 		/*
 		 * If we haven't seen any new control data in 200ms, assume we
-		 * have lost input and tell FMU 
+		 * have lost input and tell FMU.
 		 */
 		if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
 
diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c
index 5841f29a3..04aca709b 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
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 63a55d840..0032e6d80 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -165,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 25fe0cf38..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;
 }
 
 /*
@@ -203,6 +210,9 @@ sbus_decode(hrt_abstime frame_time)
 		return;
 	}
 
+	/* 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;