Harmonized PPM, S.BUS and DSM input (order: first preference S.Bus, then DSM, then PPM, first available and valid source is chosen), tested with FMU, valid channel inputs

1 files changed, 21 insertions, 5 deletions

diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c
index 497818c94..3d8d7c18b 100644
--- a/apps/px4io/sbus.c
+++ b/apps/px4io/sbus.c
@@ -50,6 +50,7 @@
 #define DEBUG
 #include "px4io.h"
 #include "protocol.h"
+#include "debug.h"
 
 #define SBUS_FRAME_SIZE		25
 
@@ -191,17 +192,22 @@ static void
 sbus_decode(hrt_abstime frame_time)
 {
 	/* check frame boundary markers to avoid out-of-sync cases */
-	if ((frame[0] != 0xf0) || (frame[24] != 0x00)) {
+	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 (frame[23] & (1 << 4))
+	if (frame[23] & (1 << 4)) {
+		system_state.sbus_input_ok = false;
 		return;
+	}
+
+	unsigned chancount = (PX4IO_INPUT_CHANNELS > 16) ? 16 : PX4IO_INPUT_CHANNELS;
 
 	/* use the decoder matrix to extract channel data */
-	for (unsigned channel = 0; channel < 16; channel++) {
+	for (unsigned channel = 0; channel < chancount; channel++) {
 		unsigned value = 0;
 
 		for (unsigned pick = 0; pick < 3; pick++) {
@@ -217,8 +223,18 @@ sbus_decode(hrt_abstime frame_time)
 			}
 		}
 		/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
-		ppm_buffer[channel] = (value / 2) + 998;
+		system_state.rc_channel_data[channel] = (value / 2) + 998;
+	}
+
+	if (PX4IO_INPUT_CHANNELS >= 18) {
+		/* decode two switch channels */
+		chancount = 18;
 	}
+
+	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 */
-	ppm_last_valid_decode = frame_time;	
+	system_state.rc_channels_timestamp = frame_time;	
 }