Just for fun, add a (completely untested) S.bus decoder.

1 files changed, 145 insertions, 0 deletions

diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c
index a91e37b5c..497818c94 100644
--- a/apps/px4io/sbus.c
+++ b/apps/px4io/sbus.c
@@ -43,13 +43,28 @@
 #include <unistd.h>
 #include <termios.h>
 
+#include <systemlib/ppm_decode.h>
+
 #include <drivers/drv_hrt.h>
 
 #define DEBUG
 #include "px4io.h"
+#include "protocol.h"
+
+#define SBUS_FRAME_SIZE		25
 
 static int sbus_fd = -1;
 
+static hrt_abstime last_rx_time;
+
+static uint8_t	frame[SBUS_FRAME_SIZE];
+
+static unsigned partial_frame_count;
+
+unsigned sbus_frame_drops;
+
+static void sbus_decode(hrt_abstime frame_time);
+
 int
 sbus_init(const char *device)
 {
@@ -65,6 +80,10 @@ sbus_init(const char *device)
 		t.c_cflag |= (CSTOPB | PARENB);
 		tcsetattr(sbus_fd, TCSANOW, &t);
 
+		/* initialise the decoder */
+		partial_frame_count = 0;
+		last_rx_time = hrt_absolute_time();
+
 		debug("Sbus: ready");
 	} else {
 		debug("Sbus: open failed");
@@ -76,4 +95,130 @@ sbus_init(const char *device)
 void
 sbus_input(void)
 {
+	ssize_t		ret;
+	hrt_abstime	now;
+
+	/*
+	 * The S.bus protocol doesn't provide reliable framing,
+	 * so we detect frame boundaries by the inter-frame delay.
+	 *
+	 * The minimum frame spacing is 7ms; with 25 bytes at 100000bps
+	 * 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, 
+	 * 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
+	 * provides a degree of protection. Of course, it would be better
+	 * if we didn't drop bytes...
+	 */
+	now = hrt_absolute_time();
+	if ((now - last_rx_time) > 3000) {
+		if (partial_frame_count > 0) {
+			sbus_frame_drops++;
+			partial_frame_count = 0;
+		}
+	}
+
+	/*
+	 * Fetch bytes, but no more than we would need to complete
+	 * the current frame.
+	 */
+	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)
+		return;
+	last_rx_time = now;
+
+	/*
+	 * Add bytes to the current frame
+	 */
+	partial_frame_count += ret;
+
+	/*
+	 * If we don't have a full frame, return
+	 */
+	if (partial_frame_count < SBUS_FRAME_SIZE)
+	 	return;
+
+	/*
+	 * Great, it looks like we might have a frame.  Go ahead and
+	 * decode it.
+	 */
+	sbus_decode(now);
+	partial_frame_count = 0;
+}
+
+/*
+ * S.bus decoder matrix.
+ *
+ * Each channel value can come from up to 3 input bytes. Each row in the
+ * matrix describes up to three bytes, and each entry gives:
+ *
+ * - byte offset in the data portion of the frame
+ * - right shift applied to the data byte
+ * - mask for the data byte
+ * - left shift applied to the result into the channel value
+ */
+struct sbus_bit_pick {
+	uint8_t byte;
+	uint8_t rshift;
+	uint8_t mask;
+	uint8_t lshift;
+};
+static struct sbus_bit_pick sbus_decoder[][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 void
+sbus_decode(hrt_abstime frame_time)
+{
+	/* check frame boundary markers to avoid out-of-sync cases */
+	if ((frame[0] != 0xf0) || (frame[24] != 0x00)) {
+		sbus_frame_drops++;
+		return;
+	}
+
+	/* if the failsafe bit is set, we consider that a loss of RX signal */
+	if (frame[23] & (1 << 4))
+		return;
+
+	/* use the decoder matrix to extract channel data */
+	for (unsigned channel = 0; channel < 16; channel++) {
+		unsigned value = 0;
+
+		for (unsigned pick = 0; pick < 3; pick++) {
+			struct sbus_bit_pick *decode = &sbus_decoder[channel][pick];
+
+			if (decode->mask != 0) {
+				unsigned piece = frame[1 + decode->byte];
+				piece >>= decode->rshift;
+				piece &= decode->mask;
+				piece <<= decode->lshift;
+
+				value |= piece;
+			}
+		}
+		/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
+		ppm_buffer[channel] = (value / 2) + 998;
+	}
+	/* and note that we have received data from the R/C controller */
+	ppm_last_valid_decode = frame_time;	
 }