diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index d60f0687a5..951d49d806 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -46,6 +46,7 @@ #include #include #include +#include #include "dsm.h" #include @@ -57,7 +58,7 @@ #define dsm_udelay(arg) up_udelay(arg) #endif -//#define DSM_DEBUG +// #define DSM_DEBUG static enum DSM_DECODE_STATE { DSM_DECODE_STATE_DESYNC = 0, @@ -69,6 +70,7 @@ static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last rece static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last valid dsm frame */ static uint8_t dsm_frame[DSM_BUFFER_SIZE]; /**< DSM dsm frame receive buffer */ static uint8_t dsm_buf[DSM_FRAME_SIZE * 2]; +static uint16_t dsm_chan_buf[DSM_MAX_CHANNEL_COUNT]; static unsigned dsm_partial_frame_count; /**< Count of bytes received for current dsm frame */ static unsigned dsm_channel_shift = 0; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */ static unsigned dsm_frame_drops = 0; /**< Count of incomplete DSM frames */ @@ -263,6 +265,19 @@ dsm_config(int fd) return ret; } +void +dsm_proto_init() +{ + dsm_channel_shift = 0; + dsm_frame_drops = 0; + dsm_chan_count = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + + for (unsigned i = 0; i < DSM_MAX_CHANNEL_COUNT; i++) { + dsm_chan_buf[i] = 0; + } +} + /** * Initialize the DSM receive functionality * @@ -278,10 +293,7 @@ dsm_init(const char *device) dsm_fd = open(device, O_RDONLY | O_NONBLOCK); } - dsm_channel_shift = 0; - dsm_frame_drops = 0; - dsm_chan_count = 0; - dsm_decode_state = DSM_DECODE_STATE_DESYNC; + dsm_proto_init(); int ret = dsm_config(dsm_fd); @@ -494,7 +506,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool /* if the value is unrealistic, fail the parsing entirely */ if (values[i] < 600 || values[i] > 2400) { #ifdef DSM_DEBUG - printf("DSM: VALUE RANGE FAIL\n"); + printf("DSM: VALUE RANGE FAIL: %d: %d\n", (int)i, (int)values[i]); #endif dsm_chan_count = 0; return false; @@ -581,6 +593,11 @@ dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, */ bool decode_ret = false; + /* ensure there can be no overflows */ + if (max_channels > sizeof(dsm_chan_buf) / sizeof(dsm_chan_buf[0])) { + max_channels = sizeof(dsm_chan_buf) / sizeof(dsm_chan_buf[0]); + } + /* keep decoding until we have consumed the buffer */ for (unsigned d = 0; d < len; d++) { @@ -636,7 +653,7 @@ dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, * Great, it looks like we might have a frame. Go ahead and * decode it. */ - decode_ret = dsm_decode(now, values, &dsm_chan_count, dsm_11_bit, max_channels); + decode_ret = dsm_decode(now, &dsm_chan_buf[0], &dsm_chan_count, dsm_11_bit, max_channels); /* we consumed the partial frame, reset */ dsm_partial_frame_count = 0; @@ -665,6 +682,14 @@ dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, if (decode_ret) { *num_values = dsm_chan_count; + memcpy(&values[0], &dsm_chan_buf[0], dsm_chan_count * sizeof(dsm_chan_buf[0])); +#ifdef DSM_DEBUG + + for (unsigned i = 0; i < dsm_chan_count; i++) { + printf("dsm_decode: %u: %u\n", i, values[0]); + } + +#endif } dsm_last_rx_time = now; diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h index 669b319017..6793abb224 100644 --- a/src/lib/rc/dsm.h +++ b/src/lib/rc/dsm.h @@ -54,6 +54,7 @@ __BEGIN_DECLS #define DSM_BUFFER_SIZE (DSM_FRAME_SIZE + DSM_FRAME_SIZE / 2) __EXPORT int dsm_init(const char *device); +__EXPORT void dsm_proto_init(void); __EXPORT int dsm_config(int dsm_fd); __EXPORT bool dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values); diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp index 9d86d0fa07..8155d6eea1 100644 --- a/src/lib/rc/rc_tests/RCTest.cpp +++ b/src/lib/rc/rc_tests/RCTest.cpp @@ -48,12 +48,12 @@ bool RCTest::run_tests(void) bool RCTest::dsmTest10Ch() { - return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 6, 0); + return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 6, 1500); } bool RCTest::dsmTest12Ch() { - return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 12, 11, 1500); + return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 12, 6, 1500); } bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0) @@ -76,7 +76,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned } // Init the parser - uint8_t frame[20]; + uint8_t frame[30]; uint16_t rc_values[18]; uint16_t num_values; bool dsm_11_bit; @@ -86,6 +86,8 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned int rate_limiter = 0; unsigned last_drop = 0; + dsm_proto_init(); + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { ut_test(ret > 0); @@ -100,9 +102,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned if (result) { ut_test(num_values == expected_chancount); - if (chan0 > 0) { - ut_test((chan0 - rc_values[0]) < 3); - } + ut_test(abs((int)chan0 - (int)rc_values[0]) < 30); //PX4_INFO("decoded packet with %d channels and %s encoding:", num_values, (dsm_11_bit) ? "11 bit" : "10 bit"); @@ -112,7 +112,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned } if (last_drop != (dsm_frame_drops)) { - //PX4_INFO("frame dropped, now #%d", (dsm_frame_drops)); + PX4_INFO("frame dropped, now #%d", (dsm_frame_drops)); last_drop = dsm_frame_drops; }