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, 22 insertions, 4 deletions

diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c
index 2c2e09905..744dac3d6 100644
--- a/apps/px4io/dsm.c
+++ b/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;
+	}
 }