Reverted unwanted S.Bus changes

2 files changed, 51 insertions, 63 deletions

diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 564687b58..297fea017 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -98,17 +98,11 @@ controls_main(void)
 		 */
 		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(fds[1].fd, PX4IO_INPUT_CHANNELS, &system_state.rc_channel_data,
-				&system_state.rc_channels, &system_state.rc_channels_timestamp, &rc_updated);
+			locked |= sbus_input();
 
 		/*
 		 * If we don't have lock from one of the serial receivers,
@@ -138,18 +132,19 @@ controls_main(void)
 		 */
 		if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
 
+			if (system_state.rc_channels > 0) {
+				/*
+				 * If the RC signal status (lost / present) has
+				 * just changed, request an update immediately.
+				 */
+				system_state.fmu_report_due = true;
+			}
+			
 			/* set the number of channels to zero - no inputs */
 			system_state.rc_channels = 0;
-			rc_updated = true;
 		}
 
 		/*
-		 * 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.
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c
index f7a74cd12..f84e5df8a 100644
--- a/apps/px4io/sbus.c
+++ b/apps/px4io/sbus.c
@@ -49,10 +49,13 @@
 
 #define DEBUG
 #include "px4io.h"
+#include "protocol.h"
 #include "debug.h"
 
 #define SBUS_FRAME_SIZE		25
-#define SBUS_INPUT_CHANNELS	16
+#define SBUS_INPUT_CHANNELS	18
+
+static int sbus_fd = -1;
 
 static hrt_abstime last_rx_time;
 static hrt_abstime last_frame_time;
@@ -63,14 +66,11 @@ static unsigned partial_frame_count;
 
 unsigned sbus_frame_drops;
 
-static int sbus_decode(hrt_abstime frame_time, unsigned max_channels,
-	uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time);
+static void sbus_decode(hrt_abstime frame_time);
 
 int
 sbus_init(const char *device)
 {
-	static int sbus_fd = -1;
-
 	if (sbus_fd < 0)
 		sbus_fd = open(device, O_RDONLY);
 
@@ -87,18 +87,16 @@ sbus_init(const char *device)
 		partial_frame_count = 0;
 		last_rx_time = hrt_absolute_time();
 
-		debug("Sbus: ready");
-
+		debug("S.Bus: ready");
 	} else {
-		debug("Sbus: open failed");
+		debug("S.Bus: open failed");
 	}
 
 	return sbus_fd;
 }
 
 bool
-sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count,
-		uint64_t *receive_time, bool *updated)
+sbus_input(void)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -111,7 +109,7 @@ sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *chan
 	 * frame transmission time is ~2ms.
 	 *
 	 * We expect to only be called when bytes arrive for processing,
-	 * and if an interval of more than 3ms passes between calls,
+	 * and if an interval of more than 3ms passes between calls, 
 	 * the first byte we read will be the first byte of a frame.
 	 *
 	 * In the case where byte(s) are dropped from a frame, this also
@@ -119,7 +117,6 @@ sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *chan
 	 * if we didn't drop bytes...
 	 */
 	now = hrt_absolute_time();
-
 	if ((now - last_rx_time) > 3000) {
 		if (partial_frame_count > 0) {
 			sbus_frame_drops++;
@@ -131,12 +128,11 @@ sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *chan
 	 * Fetch bytes, but no more than we would need to complete
 	 * the current frame.
 	 */
-	ret = read(fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count);
+	ret = read(sbus_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)
 		goto out;
-
 	last_rx_time = now;
 
 	/*
@@ -148,18 +144,18 @@ sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *chan
 	 * If we don't have a full frame, return
 	 */
 	if (partial_frame_count < SBUS_FRAME_SIZE)
-		goto out;
+	 	goto out;
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
-	 * decode it, report if the receiver got something.
+	 * decode it.
 	 */
-	*updated = (sbus_decode(now, max_channels, channel_data, channel_count, receive_time) == OK);
+	sbus_decode(now);
 	partial_frame_count = 0;
 
 out:
 	/*
-	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked'
+	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked' 
 	 */
 	return (now - last_frame_time) < 200000;
 }
@@ -181,51 +177,48 @@ struct sbus_bit_pick {
 	uint8_t mask;
 	uint8_t lshift;
 };
-static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
-	/*  0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00,  0} },
-	/*  1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00,  0} },
-	/*  2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} },
-	/*  3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00,  0} },
-	/*  4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00,  0} },
-	/*  5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03,  9} },
-	/*  6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00,  0} },
-	/*  7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00,  0} },
-	/*  8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00,  0} },
-	/*  9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00,  0} },
-	/* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} },
-	/* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00,  0} },
-	/* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00,  0} },
-	/* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03,  9} },
-	/* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00,  0} },
-	/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00,  0} }
+static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {	
+/*  0 */ { { 0, 0, 0xff, 0},{ 1, 0, 0x07, 8},{ 0, 0, 0x00,  0} },
+/*  1 */ { { 1, 3, 0x1f, 0},{ 2, 0, 0x3f, 5},{ 0, 0, 0x00,  0} },
+/*  2 */ { { 2, 6, 0x03, 0},{ 3, 0, 0xff, 2},{ 4, 0, 0x01, 10} },
+/*  3 */ { { 4, 1, 0x7f, 0},{ 5, 0, 0x0f, 7},{ 0, 0, 0x00,  0} },
+/*  4 */ { { 5, 4, 0x0f, 0},{ 6, 0, 0x7f, 4},{ 0, 0, 0x00,  0} },
+/*  5 */ { { 6, 7, 0x01, 0},{ 7, 0, 0xff, 1},{ 8, 0, 0x03,  9} },
+/*  6 */ { { 8, 2, 0x3f, 0},{ 9, 0, 0x1f, 6},{ 0, 0, 0x00,  0} },
+/*  7 */ { { 9, 5, 0x07, 0},{10, 0, 0xff, 3},{ 0, 0, 0x00,  0} },
+/*  8 */ { {11, 0, 0xff, 0},{12, 0, 0x07, 8},{ 0, 0, 0x00,  0} },
+/*  9 */ { {12, 3, 0x1f, 0},{13, 0, 0x3f, 5},{ 0, 0, 0x00,  0} },
+/* 10 */ { {13, 6, 0x03, 0},{14, 0, 0xff, 2},{15, 0, 0x01, 10} },
+/* 11 */ { {15, 1, 0x7f, 0},{16, 0, 0x0f, 7},{ 0, 0, 0x00,  0} },
+/* 12 */ { {16, 4, 0x0f, 0},{17, 0, 0x7f, 4},{ 0, 0, 0x00,  0} },
+/* 13 */ { {17, 7, 0x01, 0},{18, 0, 0xff, 1},{19, 0, 0x03,  9} },
+/* 14 */ { {19, 2, 0x3f, 0},{20, 0, 0x1f, 6},{ 0, 0, 0x00,  0} },
+/* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00,  0} }
 };
 
-static int
-sbus_decode(hrt_abstime frame_time, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time)
+static void
+sbus_decode(hrt_abstime frame_time)
 {
 	/* check frame boundary markers to avoid out-of-sync cases */
 	if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
 		sbus_frame_drops++;
-		return 1;
+		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 */
+		(frame[23] & (1 << 3))) { /* failsafe */
 
 		/* actively announce signal loss */
-		*channel_count = 0;
-		
+		system_state.rc_channels = 0;
 		return 1;
 	}
 
-	/* decode failsafe and RC status */
-
 	/* we have received something we think is a frame */
 	last_frame_time = frame_time;
 
-	unsigned chancount = (max_channels > SBUS_INPUT_CHANNELS) ?
-		SBUS_INPUT_CHANNELS : max_channels;
+	unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? 
+		SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
 
 	/* use the decoder matrix to extract channel data */
 	for (unsigned channel = 0; channel < chancount; channel++) {
@@ -243,7 +236,6 @@ sbus_decode(hrt_abstime frame_time, unsigned max_channels, uint16_t *channel_dat
 				value |= piece;
 			}
 		}
-
 		/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
 		system_state.rc_channel_data[channel] = (value / 2) + 998;
 	}
@@ -257,10 +249,11 @@ sbus_decode(hrt_abstime frame_time, unsigned max_channels, uint16_t *channel_dat
 	}
 
 	/* note the number of channels decoded */
-	*channel_count = chancount;
+	system_state.rc_channels = chancount;
 
 	/* and note that we have received data from the R/C controller */
-	*receive_time = frame_time;
+	system_state.rc_channels_timestamp = frame_time;	
 
-	return 0;
+	/* trigger an immediate report to the FMU */
+	system_state.fmu_report_due = true;
 }