From c961dd8bab38c1a8570eaf02b912353641e45df3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 30 Nov 2012 01:34:33 -0800 Subject: Just for fun, add a (completely untested) S.bus decoder. --- apps/px4io/sbus.c | 145 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 145 insertions(+) (limited to 'apps/px4io/sbus.c') 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 #include +#include + #include #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; } -- cgit v1.2.3