S.BUS2: Initial parsing

src/lib/rc/sbus.c

@@ -42,6 +42,7 @@
 #include <fcntl.h>
 #include <unistd.h>
 #include <termios.h>
+#include <string.h>
 
 #include "sbus.h"
 #include <drivers/drv_hrt.h>
@@ -120,8 +121,9 @@ sbus_dropped_frames()
 	return sbus_frame_drops;
 }
 
-static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe,
-			bool *sbus_frame_drop, uint16_t max_channels);
+static bool
+sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num_values,
+	bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values);
 
 int
 sbus_init(const char *device, bool singlewire)
@@ -201,7 +203,7 @@ sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values)
 }
 
 bool
-sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop,
+sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop,
 	   uint16_t max_channels)
 {
 	ssize_t		ret;
@@ -254,8 +256,8 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
 }
 
 bool
-sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *values,
-	uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t *frame_drops, uint16_t max_channels)
+sbus_parse(uint64_t now, uint8_t *frame, unsigned *partial_count, uint16_t *values,
+	uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels)
 {
 
 	last_rx_time = now;
@@ -326,7 +328,7 @@ sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *v
 						case 0x03:
 							{
 								uint16_t rx_voltage = (frame[1] << 8) | frame[2];
-								isr_debug(30, "rx_voltage %d", (int)rx_voltage);
+								printf("rx_voltage %d", (int)rx_voltage);
 								n_consumed = 3;
 							}
 							break;
@@ -355,7 +357,7 @@ sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *v
 						case 0x13:
 							{
 								uint16_t gps_something = (frame[1] << 8) | frame[2];
-								isr_debug(30, "gps_something %d", (int)gps_something);
+								printf("gps_something %d", (int)gps_something);
 								n_consumed = 24;
 							}
 							break;
@@ -390,3 +392,139 @@ sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *v
 	/* return false as default */
 	return decode_ret;
 }
+
+/*
+ * 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 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} }
+};
+
+bool
+sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num_values,
+	bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
+{
+
+	/* check frame boundary markers to avoid out-of-sync cases */
+	if ((frame[0] != 0x0f)) {
+		sbus_frame_drops++;
+		return false;
+	}
+
+	switch (frame[24]) {
+		case 0x00:
+			/* this is S.BUS 1 */
+			sbus_decode_state = SBUS2_DECODE_STATE_SBUS1_SYNC;
+			break;
+		case 0x04:
+			/* receiver voltage */
+			sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE;
+			break;
+		case 0x14:
+			/* GPS / baro */
+			sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_GPS;
+			break;
+		case 0x24:
+			/* Unknown SBUS2 data */
+			sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
+			break;
+		case 0x34:
+			/* Unknown SBUS2 data */
+			sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
+			break;
+		default:
+			return false;
+	}
+
+	/* we have received something we think is a frame */
+	last_frame_time = frame_time;
+
+	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++) {
+		unsigned value = 0;
+
+		for (unsigned pick = 0; pick < 3; pick++) {
+			const 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 not too sloppy fashion */
+		values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET;
+	}
+
+	/* decode switch channels if data fields are wide enough */
+	if (max_values > 17 && chancount > 15) {
+		chancount = 18;
+
+		/* channel 17 (index 16) */
+		values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998;
+		/* channel 18 (index 17) */
+		values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998;
+	}
+
+	/* note the number of channels decoded */
+	*num_values = chancount;
+
+	/* decode and handle failsafe and frame-lost flags */
+	if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
+		/* report that we failed to read anything valid off the receiver */
+		*sbus_failsafe = true;
+		*sbus_frame_drop = true;
+	}
+	else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
+		/* set a special warning flag
+		 * 
+		 * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this 
+		 * condition as fail-safe greatly reduces the reliability and range of the radio link, 
+		 * e.g. by prematurely issueing return-to-launch!!! */
+
+		*sbus_failsafe = false;
+		*sbus_frame_drop = true;
+	} else {
+		*sbus_failsafe = false;
+		*sbus_frame_drop = false;
+	}
+
+	return true;
+}

src/lib/rc/sbus.h

@@ -46,6 +46,10 @@
 
 __BEGIN_DECLS
 
+#define SBUS_INTER_FRAME_TIMEOUT	3000	/**< 3000 us frame timeout */
+#define SBUS_FRAME_SIZE			25
+#define SBUS_BUFFER_SIZE		(SBUS_FRAME_SIZE + SBUS_FRAME_SIZE / 2)
+
 __EXPORT int	sbus_init(const char *device, bool singlewire);
 
 /**
@@ -68,6 +72,8 @@ __EXPORT int	sbus_init(const char *device, bool singlewire);
 __EXPORT bool	sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe,
 			   bool *sbus_frame_drop,
 			   uint16_t max_channels);
+__EXPORT bool sbus_parse(uint64_t now, uint8_t *frame, unsigned *partial_count, uint16_t *values,
+	uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels);
 __EXPORT void	sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values);
 __EXPORT void	sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values);