Hotfix for S.Bus systems with more than 8 channels

3 files changed, 12 insertions, 10 deletions

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index b17276a31..617b536f4 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -108,9 +108,11 @@ controls_tick() {
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
-	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count);
-	if (sbus_updated)
+	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */);
+	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
+		r_raw_rc_count = 8;
+	}
 	perf_end(c_gather_sbus);
 
 	/*
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index dea67043e..11f4d053d 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -51,7 +51,7 @@
  */
 #define PX4IO_SERVO_COUNT		8
 #define PX4IO_CONTROL_CHANNELS		8
-#define PX4IO_INPUT_CHANNELS		12
+#define PX4IO_INPUT_CHANNELS		8 // XXX this should be 18 channels
 
 /*
  * Debug logging
@@ -200,7 +200,7 @@ extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
 extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
-extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
+extern bool	sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels);
 
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 073ddeaba..c523df6ca 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -66,7 +66,7 @@ static unsigned partial_frame_count;
 
 unsigned sbus_frame_drops;
 
-static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels);
 
 int
 sbus_init(const char *device)
@@ -97,7 +97,7 @@ sbus_init(const char *device)
 }
 
 bool
-sbus_input(uint16_t *values, uint16_t *num_values)
+sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -154,7 +154,7 @@ sbus_input(uint16_t *values, uint16_t *num_values)
 	 * decode it.
 	 */
 	partial_frame_count = 0;
-	return sbus_decode(now, values, num_values);
+	return sbus_decode(now, values, num_values, max_channels);
 }
 
 /*
@@ -194,7 +194,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
 };
 
 static bool
-sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values)
 {
 	/* check frame boundary markers to avoid out-of-sync cases */
 	if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
@@ -214,8 +214,8 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 	/* 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_values > SBUS_INPUT_CHANNELS) ?
+			     SBUS_INPUT_CHANNELS : max_values;
 
 	/* use the decoder matrix to extract channel data */
 	for (unsigned channel = 0; channel < chancount; channel++) {