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

apps/px4io/dsm.c

@@ -278,6 +278,7 @@ dsm_decode(hrt_abstime frame_time)
 	last_frame_time = frame_time;
 	if (channel_shift == 0) {
 		dsm_guess_format(false);
+		system_state.dsm_input_ok = false;
 		return;
 	}
 
@@ -292,6 +293,10 @@ dsm_decode(hrt_abstime frame_time)
 	 * seven channels are being transmitted.
 	 */
 
+	const unsigned dsm_chancount = (DSM_FRAME_CHANNELS < PX4IO_INPUT_CHANNELS) ? DSM_FRAME_CHANNELS : PX4IO_INPUT_CHANNELS;
+
+	uint16_t dsm_channels[dsm_chancount];
+
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
 		uint8_t *dp = &frame[2 + (2 * i)];
@@ -314,10 +319,23 @@ dsm_decode(hrt_abstime frame_time)
 			value /= 2;
 
 		/* stuff the decoded channel into the PPM input buffer */
-		ppm_buffer[channel] = 988 + value;
+		dsm_channels[channel] = 988 + value;
 	}
 
-	/* and note that we have received data from the R/C controller */
-	/* XXX failsafe will cause problems here - need a strategy for detecting it */
-	ppm_last_valid_decode = frame_time;
+	/* DSM input is valid */
+	system_state.dsm_input_ok = true;
+
+	/* check if no S.BUS data is available */
+	if (!system_state.sbus_input_ok) {
+
+		for (unsigned i = 0; i < dsm_chancount; i++) {
+			system_state.rc_channel_data[i] = dsm_channels[i];
+		}
+
+		/* and note that we have received data from the R/C controller */
+		/* XXX failsafe will cause problems here - need a strategy for detecting it */
+		system_state.rc_channels_timestamp = frame_time;
+		system_state.rc_channels = dsm_chancount;
+		system_state.fmu_report_due = true;
+	}
 }

apps/px4io/mixer.c

@@ -172,17 +172,26 @@ mixer_update(int mixer, uint16_t *inputs, int input_count)
 static void
 mixer_get_rc_input(void)
 {
-
 	/* if we haven't seen any new data in 200ms, assume we have lost input and tell FMU */
 	if ((hrt_absolute_time() - ppm_last_valid_decode) > 200000) {
-		system_state.rc_channels = 0;
-		system_state.fmu_report_due = true;
+
+		/* input was ok and timed out, mark as update */
+		if (system_state.ppm_input_ok) {
+			system_state.ppm_input_ok = false;
+			system_state.fmu_report_due = true;
+		}
 		return;
 	}
 
-	/* otherwise, copy channel data */
-	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];
-	system_state.fmu_report_due = true;
+	/* mark PPM as valid */
+	system_state.ppm_input_ok = true;
+
+	/* check if no DSM and S.BUS data is available */
+	if (!system_state.sbus_input_ok && !system_state.dsm_input_ok) {
+		/* otherwise, copy channel data */
+		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];
+		system_state.fmu_report_due = true;
+	}
 }

apps/px4io/px4io.c

@@ -58,6 +58,8 @@ struct sys_state_s 	system_state;
 
 int user_start(int argc, char *argv[])
 {
+	/* reset all to zero */
+	memset(&system_state, 0, sizeof(system_state));
 
 	/* configure the high-resolution time/callout interface */
 	hrt_init();

apps/px4io/px4io.h

@@ -72,11 +72,16 @@ struct sys_state_s
 	bool		armed;		/* IO armed */
 	bool		arm_ok;		/* FMU says OK to arm */
 
+	bool		ppm_input_ok;	/* valid PPM input data */
+	bool		dsm_input_ok;	/* valid Spektrum DSM data */
+	bool		sbus_input_ok;	/* valid Futaba S.Bus data */
+
 	/*
 	 * Data from the remote control input(s)
 	 */
 	int		rc_channels;
 	uint16_t	rc_channel_data[PX4IO_INPUT_CHANNELS];
+	uint64_t	rc_channels_timestamp;
 
 	/*
 	 * Control signals from FMU.

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;	
 }