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

1 files changed, 13 insertions, 3 deletions

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;