diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 4120bb56d3..5b3ed90d9b 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -42,6 +42,7 @@ #include #include #include +#include #include "sbus.h" #include @@ -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; +} diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index 685c9d515f..222cb5e4b9 100644 --- a/src/lib/rc/sbus.h +++ b/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);