Fixed signal loss detection on S.Bus parsing, stripped PX4IO code parts from S.Bus parser to allow FMU / IO parser code sharing. Added S.Bus channels 17 and 18 if channel data struct has enough space. Tested with receiver and PX4FMU.

5 files changed, 72 insertions, 31 deletions

diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 9a786234e..bfdad019d 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -52,6 +52,7 @@
 #include <nuttx/clock.h>
 
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
 #include <systemlib/hx_stream.h>
 #include <systemlib/perf_counter.h>
 
@@ -201,7 +202,12 @@ comms_handle_command(const void *buffer, size_t length)
 		system_state.servo_rate = new_servo_rate;
 	}
 
-	/* update servo values immediately */
+	/*
+	 * update servo values immediately.
+	 * the updates are done in addition also
+	 * in the mainloop, since this function will only
+	 * update with a connected FMU.
+	 */
 	mixer_tick();
 
 	/* XXX do relay changes here */	
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 4bd6fe1a0..48370d9d0 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -92,10 +92,22 @@ controls_main(void)
 		 */
 		bool locked = false;
 
+		/*
+		 * Store RC channel count to detect switch to RC loss sooner
+		 * than just by timeout
+		 */
+		unsigned rc_channels = system_state.rc_channels;
+
+		/*
+		 * Track if any input got an update in this round
+		 */
+		bool rc_updated;
+
 		if (fds[0].revents & POLLIN)
 			locked |= dsm_input();
 		if (fds[1].revents & POLLIN)
-			locked |= sbus_input();
+			locked |= sbus_input(fds[1].fd, PX4IO_INPUT_CHANNELS, &system_state.rc_channel_data,
+				&system_state.rc_channels, &system_state.rc_channels_timestamp, &rc_updated);
 
 		/*
 		 * If we don't have lock from one of the serial receivers,
@@ -127,13 +139,21 @@ controls_main(void)
 
 			/* set the number of channels to zero - no inputs */
 			system_state.rc_channels = 0;
-			system_state.rc_lost = true;
-
-			/* trigger an immediate report to the FMU */
-			system_state.fmu_report_due = true;
+			rc_updated = true;
 		}
 
-		/* PWM output updates are performed directly on each comms update */
+		/*
+		 * If there was a RC update OR the RC signal status (lost / present) has
+		 * just changed, request an update immediately.
+		 */
+		system_state.fmu_report_due |= rc_updated;
+
+		/*
+		 * PWM output updates are performed in addition on each comm update.
+		 * the updates here are required to ensure operation if FMU is not started
+		 * or stopped responding.
+		 */
+		mixer_tick();
 	}
 }
 
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index 318183ef5..135d97bb3 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -48,7 +48,10 @@
 #include <unistd.h>
 #include <fcntl.h>
 
+#include <debug.h>
+
 #include <drivers/drv_pwm_output.h>
+#include <drivers/drv_hrt.h>
 
 #include "px4io.h"
 
@@ -95,6 +98,7 @@ mixer_tick(void)
 			/* too many frames without FMU input, time to go to failsafe */
 			system_state.mixer_manual_override = true;
 			system_state.mixer_fmu_available = false;
+			lib_lowprintf("\nRX timeout\n");
 		}
 
 	} else if (system_state.rc_channels > 0 && system_state.manual_override_ok) {
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index c8699c6c6..9cfd8f716 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -193,7 +193,8 @@ extern void	controls_main(void);
 extern int	dsm_init(const char *device);
 extern bool	dsm_input(void);
 extern int	sbus_init(const char *device);
-extern bool	sbus_input(void);
+extern bool	sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count,
+		uint64_t *receive_time, bool *updated);
 
 /*
  * Assertion codes
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c
index a8f628a84..0de0593e7 100644
--- a/apps/px4io/sbus.c
+++ b/apps/px4io/sbus.c
@@ -49,14 +49,11 @@
 
 #define DEBUG
 #include "px4io.h"
-#include "protocol.h"
 #include "debug.h"
 
 #define SBUS_FRAME_SIZE		25
 #define SBUS_INPUT_CHANNELS	16
 
-static int sbus_fd = -1;
-
 static hrt_abstime last_rx_time;
 static hrt_abstime last_frame_time;
 
@@ -66,11 +63,14 @@ static unsigned partial_frame_count;
 
 unsigned sbus_frame_drops;
 
-static void sbus_decode(hrt_abstime frame_time);
+static int sbus_decode(hrt_abstime frame_time, unsigned max_channels,
+	uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time);
 
 int
 sbus_init(const char *device)
 {
+	static int sbus_fd = -1;
+
 	if (sbus_fd < 0)
 		sbus_fd = open(device, O_RDONLY);
 
@@ -96,7 +96,8 @@ sbus_init(const char *device)
 }
 
 bool
-sbus_input(void)
+sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count,
+		uint64_t *receive_time, bool *updated)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -128,7 +129,7 @@ sbus_input(void)
 	 * Fetch bytes, but no more than we would need to complete
 	 * the current frame.
 	 */
-	ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count);
+	ret = read(fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count);
 
 	/* if the read failed for any reason, just give up here */
 	if (ret < 1)
@@ -148,9 +149,9 @@ sbus_input(void)
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
-	 * decode it.
+	 * decode it, report if the receiver got something.
 	 */
-	sbus_decode(now);
+	*updated = (sbus_decode(now, max_channels, channel_data, channel_count, receive_time) == OK);
 	partial_frame_count = 0;
 
 out:
@@ -196,25 +197,32 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
 /* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00,  0} }
 };
 
-static void
-sbus_decode(hrt_abstime frame_time)
+static int
+sbus_decode(hrt_abstime frame_time, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time)
 {
 	/* check frame boundary markers to avoid out-of-sync cases */
 	if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
 		sbus_frame_drops++;
-		return;
+		return 1;
 	}
 
-	/* if the failsafe bit is set, we consider the frame invalid */
-	if (frame[23] & (1 << 4)) {
-		return;
+	/* if the failsafe or connection lost bit is set, we consider the frame invalid */
+	if ((frame[23] & (1 << 2)) && /* signal lost */
+	    (frame[23] & (1 << 3))) { /* failsafe */
+
+		/* actively announce signal loss */
+		*channel_count = 0;
+		
+		return 1;
 	}
 
+	/* decode failsafe and RC status */
+
 	/* 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;
+	unsigned chancount = (max_channels > SBUS_INPUT_CHANNELS) ?
+		SBUS_INPUT_CHANNELS : max_channels;
 
 	/* use the decoder matrix to extract channel data */
 	for (unsigned channel = 0; channel < chancount; channel++) {
@@ -236,17 +244,19 @@ sbus_decode(hrt_abstime frame_time)
 		system_state.rc_channel_data[channel] = (value / 2) + 998;
 	}
 
-	if (PX4IO_INPUT_CHANNELS >= 18) {
-		chancount = 18;
-		/* XXX decode the two switch channels */
+	/* decode switch channels if data fields are wide enough */
+	if (chancount > 17) {
+		/* channel 17 (index 16) */
+		system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998;
+		/* channel 18 (index 17) */
+		system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998;
 	}
 
 	/* note the number of channels decoded */
-	system_state.rc_channels = chancount;
+	*channel_count = chancount;
 
 	/* and note that we have received data from the R/C controller */
-	system_state.rc_channels_timestamp = frame_time;	
+	*receive_time = frame_time;
 
-	/* trigger an immediate report to the FMU */
-	system_state.fmu_report_due = true;
+	return 0;
 }