diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index c1552a058e..aa19bdb3f9 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -77,11 +77,6 @@ #define SBUS_TARGET_MIN 1000.0f #define SBUS_TARGET_MAX 2000.0f -#define SBUS_SYNC_PHASE0 0 -#define SBUS_SYNC_PHASE1 1 - -#define SBUS_DEBUG - #ifdef SBUS_DEBUG #include #endif @@ -113,7 +108,6 @@ static uint8_t sbus_frame[SBUS_FRAME_SIZE + (SBUS_FRAME_SIZE / 2)]; static unsigned partial_frame_count; static unsigned sbus_frame_drops; -static unsigned sbus_sync = SBUS_SYNC_PHASE0; unsigned sbus_dropped_frames() @@ -123,7 +117,7 @@ sbus_dropped_frames() 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); + bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values); int sbus_init(const char *device, bool singlewire) @@ -238,6 +232,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails */ uint8_t buf[SBUS_FRAME_SIZE]; bool decoded = false; + do { ret = read(sbus_fd, &buf[0], SBUS_FRAME_SIZE); @@ -249,7 +244,8 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails /* * Try to decode something with what we got */ - decoded = decoded || sbus_parse(now, buf, ret, values, num_values, sbus_failsafe, sbus_frame_drop, &sbus_frame_drops, max_channels); + decoded = decoded + || sbus_parse(now, buf, ret, values, num_values, sbus_failsafe, sbus_frame_drop, &sbus_frame_drops, max_channels); } while (ret > 0); @@ -258,7 +254,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails bool sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, - uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels) + uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels) { last_rx_time = now; @@ -275,179 +271,186 @@ sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, if (partial_frame_count == sizeof(sbus_frame) / sizeof(sbus_frame[0])) { partial_frame_count = 0; sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; - #ifdef SBUS_DEBUG +#ifdef SBUS_DEBUG printf("SBUS2: RESET (BUF LIM)\n"); - #endif +#endif } if (partial_frame_count == SBUS_FRAME_SIZE) { partial_frame_count = 0; sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; - #ifdef SBUS_DEBUG +#ifdef SBUS_DEBUG printf("SBUS2: RESET (PACKET LIM)\n"); - #endif +#endif } - #ifdef SBUS_DEBUG - #if 0 +#ifdef SBUS_DEBUG +#if 0 printf("sbus state: %s%s%s%s%s%s, count: %d, val: %02x\n", - (sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) ? "SBUS2_DECODE_STATE_DESYNC" : "", - (sbus_decode_state == SBUS2_DECODE_STATE_SBUS_START) ? "SBUS2_DECODE_STATE_SBUS_START" : "", - (sbus_decode_state == SBUS2_DECODE_STATE_SBUS1_SYNC) ? "SBUS2_DECODE_STATE_SBUS1_SYNC" : "", - (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_SYNC) ? "SBUS2_DECODE_STATE_SBUS2_SYNC" : "", - (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE) ? "SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE" : "", - (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_GPS) ? "SBUS2_DECODE_STATE_SBUS2_GPS" : "", - partial_frame_count, - (unsigned)frame[d]); - #endif - #endif + (sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) ? "SBUS2_DECODE_STATE_DESYNC" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS_START) ? "SBUS2_DECODE_STATE_SBUS_START" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS1_SYNC) ? "SBUS2_DECODE_STATE_SBUS1_SYNC" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_SYNC) ? "SBUS2_DECODE_STATE_SBUS2_SYNC" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE) ? "SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_GPS) ? "SBUS2_DECODE_STATE_SBUS2_GPS" : "", + partial_frame_count, + (unsigned)frame[d]); +#endif +#endif switch (sbus_decode_state) { - case SBUS2_DECODE_STATE_DESYNC: - /* we are de-synced and only interested in the frame marker */ - if (frame[d] == SBUS_START_SYMBOL) { - sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; + case SBUS2_DECODE_STATE_DESYNC: + + /* we are de-synced and only interested in the frame marker */ + if (frame[d] == SBUS_START_SYMBOL) { + sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; + partial_frame_count = 0; + sbus_frame[partial_frame_count++] = frame[d]; + } + + break; + + /* fall through */ + case SBUS2_DECODE_STATE_SBUS_START: + case SBUS2_DECODE_STATE_SBUS1_SYNC: + + /* fall through */ + case SBUS2_DECODE_STATE_SBUS2_SYNC: { + sbus_frame[partial_frame_count++] = frame[d]; + + /* decode whatever we got and expect */ + if (partial_frame_count < SBUS_FRAME_SIZE) { + break; + } + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + decode_ret = sbus_decode(now, sbus_frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); + + /* + * Offset recovery: If decoding failed, check if there is a second + * start marker in the packet. + */ + unsigned start_index = 0; + + if (!decode_ret && sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) { + + for (unsigned i = 1; i < partial_frame_count; i++) { + if (sbus_frame[i] == SBUS_START_SYMBOL) { + start_index = i; + break; + } + } + + /* we found a second start marker */ + if (start_index != 0) { + /* shift everything in the buffer and reset the state machine */ + for (unsigned i = 0; i < partial_frame_count - start_index; i++) { + sbus_frame[i] = sbus_frame[i + start_index]; + } + + partial_frame_count -= start_index; + sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; + +#ifdef SBUS_DEBUG + printf("DECODE RECOVERY: %d\n", start_index); +#endif + } + } + + /* if there has been no successful attempt at saving a failed + * decoding run, reset the frame count for successful and + * unsuccessful decode runs. + */ + if (start_index == 0) { partial_frame_count = 0; - sbus_frame[partial_frame_count++] = frame[d]; } - break; - /* fall through */ - case SBUS2_DECODE_STATE_SBUS_START: - case SBUS2_DECODE_STATE_SBUS1_SYNC: - /* fall through */ - case SBUS2_DECODE_STATE_SBUS2_SYNC: - { - sbus_frame[partial_frame_count++] = frame[d]; - /* decode whatever we got and expect */ - if (partial_frame_count < SBUS_FRAME_SIZE) { - break; - } - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. - */ - decode_ret = sbus_decode(now, sbus_frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); + } + break; - /* - * Offset recovery: If decoding failed, check if there is a second - * start marker in the packet. - */ - unsigned start_index = 0; - - if (!decode_ret && sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) { - - for (unsigned i = 1; i < partial_frame_count; i++) { - if (sbus_frame[i] == SBUS_START_SYMBOL) { - start_index = i; - break; - } - } - - /* we found a second start marker */ - if (start_index != 0) { - /* shift everything in the buffer and reset the state machine */ - for (unsigned i = 0; i < partial_frame_count - start_index; i++) { - sbus_frame[i] = sbus_frame[i + start_index]; - } - - partial_frame_count -= start_index; - sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; - - #ifdef SBUS_DEBUG - printf("DECODE RECOVERY: %d\n", start_index); - #endif - } - } - - /* if there has been no successful attempt at saving a failed - * decoding run, reset the frame count for successful and - * unsuccessful decode runs. - */ - if (start_index == 0) { - partial_frame_count = 0; - } + case SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE: { + sbus_frame[partial_frame_count++] = frame[d]; + if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) { + /* this slot is unused and in fact S.BUS2 sync */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; } - break; - case SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE: - { - sbus_frame[partial_frame_count++] = frame[d]; - - if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) { - /* this slot is unused and in fact S.BUS2 sync */ - sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; - } - - if (partial_frame_count < SBUS2_FRAME_SIZE_RX_VOLTAGE) { - break; - } - - /* find out which payload we're dealing with in this slot */ - switch(sbus_frame[0]) { - case 0x03: - { - // Observed values: - // (frame[0] == 0x3 && frame[1] == 0x84 && frame[2] == 0x0) - // (frame[0] == 0x3 && frame[1] == 0xc4 && frame[2] == 0x0) - // (frame[0] == 0x3 && frame[1] == 0x80 && frame[2] == 0x2f) - // (frame[0] == 0x3 && frame[1] == 0xc0 && frame[2] == 0x2f) - uint16_t rx_voltage = (sbus_frame[1] << 8) | sbus_frame[2]; - #ifdef SBUS_DEBUG - //printf("rx_voltage %d\n", (int)rx_voltage); - #endif - } - partial_frame_count = 0; - break; - default: - /* this is not what we expect it to be, go back to sync */ - sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; - sbus_frame_drops++; - } + if (partial_frame_count < SBUS2_FRAME_SIZE_RX_VOLTAGE) { + break; } - break; - case SBUS2_DECODE_STATE_SBUS2_GPS: - { - sbus_frame[partial_frame_count++] = frame[d]; - if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) { - /* this slot is unused and in fact S.BUS2 sync */ - sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + /* find out which payload we're dealing with in this slot */ + switch (sbus_frame[0]) { + case 0x03: { + // Observed values: + // (frame[0] == 0x3 && frame[1] == 0x84 && frame[2] == 0x0) + // (frame[0] == 0x3 && frame[1] == 0xc4 && frame[2] == 0x0) + // (frame[0] == 0x3 && frame[1] == 0x80 && frame[2] == 0x2f) + // (frame[0] == 0x3 && frame[1] == 0xc0 && frame[2] == 0x2f) +#ifdef SBUS_DEBUG + //uint16_t rx_voltage = (sbus_frame[1] << 8) | sbus_frame[2]; + //printf("rx_voltage %d\n", (int)rx_voltage); +#endif } - if (partial_frame_count < 24) { - decode_ret = false; - break; - } + partial_frame_count = 0; + break; - /* find out which payload we're dealing with in this slot */ - switch(sbus_frame[0]) { - case 0x13: - { - uint16_t gps_something = (frame[1] << 8) | frame[2]; - #ifdef SBUS_DEBUG - printf("gps_something %d\n", (int)gps_something); - #endif - } - partial_frame_count = 0; - break; - default: - /* this is not what we expect it to be, go back to sync */ - sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; - sbus_frame_drops++; - /* throw unknown bytes away */ - } + default: + /* this is not what we expect it to be, go back to sync */ + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + sbus_frame_drops++; } - break; - default: - #ifdef SBUS_DEBUG - printf("UNKNOWN PROTO STATE"); - #endif - decode_ret = false; + + } + break; + + case SBUS2_DECODE_STATE_SBUS2_GPS: { + sbus_frame[partial_frame_count++] = frame[d]; + + if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) { + /* this slot is unused and in fact S.BUS2 sync */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + } + + if (partial_frame_count < 24) { + decode_ret = false; + break; + } + + /* find out which payload we're dealing with in this slot */ + switch (sbus_frame[0]) { + case 0x13: { +#ifdef SBUS_DEBUG + uint16_t gps_something = (frame[1] << 8) | frame[2]; + printf("gps_something %d\n", (int)gps_something); +#endif + } + + partial_frame_count = 0; + break; + + default: + /* this is not what we expect it to be, go back to sync */ + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + sbus_frame_drops++; + /* throw unknown bytes away */ + } + } + break; + + default: +#ifdef SBUS_DEBUG + printf("UNKNOWN PROTO STATE"); +#endif + decode_ret = false; } - + } *frame_drops = sbus_frame_drops; @@ -494,50 +497,57 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { 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) + bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != SBUS_START_SYMBOL)) { sbus_frame_drops++; - #ifdef SBUS_DEBUG +#ifdef SBUS_DEBUG printf("DECODE FAIL: "); + for (unsigned i = 0; i < SBUS_FRAME_SIZE; i++) { printf("%0x ", frame[i]); } + printf("\n"); - #endif +#endif sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; 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: - #ifdef SBUS_DEBUG - printf("DECODE FAIL: END MARKER\n"); - #endif - sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; - return false; + 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: +#ifdef SBUS_DEBUG + printf("DECODE FAIL: END MARKER\n"); +#endif + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + return false; } /* we have received something we think is a frame */ @@ -565,7 +575,7 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num /* 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; + values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; } /* decode switch channels if data fields are wide enough */ @@ -586,16 +596,17 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num /* 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 */ + + } 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, + * + * 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; diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index 8bc6e808b3..7b13f27f79 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -73,7 +73,7 @@ __EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bo bool *sbus_frame_drop, uint16_t max_channels); __EXPORT bool sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, - uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels); + 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);