From 5c4582ccceb886fb2afacee2c027856ccaea8252 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Apr 2021 21:11:11 -0400 Subject: [PATCH] rc: dsm allow full range --- src/lib/rc/dsm.cpp | 37 ++++++++++++++++++---------------- src/lib/rc/rc_tests/RCTest.cpp | 10 +++++---- 2 files changed, 26 insertions(+), 21 deletions(-) diff --git a/src/lib/rc/dsm.cpp b/src/lib/rc/dsm.cpp index 284ca6f9ff..6ed2ec413f 100644 --- a/src/lib/rc/dsm.cpp +++ b/src/lib/rc/dsm.cpp @@ -81,6 +81,7 @@ static unsigned dsm_partial_frame_count; /**< Count of bytes received for curren static unsigned dsm_channel_shift = 0; /**< Channel resolution, 0=unknown, 10=10 bit (1024), 11=11 bit (2048) */ static unsigned dsm_frame_drops = 0; /**< Count of incomplete DSM frames */ static uint16_t dsm_chan_count = 0; /**< DSM channel count */ +static uint16_t dsm_chan_count_prev = 0; /**< last valid DSM channel count */ /** * Attempt to decode a single channel raw channel datum @@ -135,7 +136,7 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u // Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1) // ±100% travel is 1102µs to 1898 µs - if (value < 990 || value > 2010) { + if (value < 903 || value > 2097) { // if the value is unrealistic, fail the parsing entirely PX4_DEBUG("channel %d invalid range %d", channel, value); return false; @@ -190,12 +191,14 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u // Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1) // ±100% travel is 1102µs to 1898 µs - if (value < 990 || value > 2010) { + if (value < 903 || value > 2097) { // if the value is unrealistic, fail the parsing entirely PX4_DEBUG("channel %d invalid range %d", channel, value); return false; } + PX4_DEBUG(stderr, "CH%d=%d(0x%02x), ", channel, value, raw); + return true; } @@ -215,6 +218,7 @@ static bool dsm_guess_format(bool reset) /* reset the 10/11 bit sniffed channel masks */ if (reset) { + PX4_DEBUG("dsm_guess_format reset"); cs10 = 0; cs11 = 0; samples = 0; @@ -283,8 +287,8 @@ static bool dsm_guess_format(bool reset) printf("dsm guess format: samples: %d %s\n", samples, (reset) ? "RESET" : ""); #endif - /* wait until we have seen plenty of frames - 5 should normally be enough */ - if (samples < 5) { + /* wait until we have seen plenty of frames */ + if (samples < 10) { return false; } @@ -597,6 +601,7 @@ bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, // abort if channel already found, no duplicate channels per DSM frame if (channels_found[channel]) { PX4_DEBUG("duplicate channel %d\n\n", channel); + dsm_guess_format(true); return false; } else { @@ -605,6 +610,7 @@ bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, /* reset bit guessing state machine if the channel index is out of bounds */ if (channel > DSM_MAX_CHANNEL_COUNT) { + PX4_DEBUG("channel %d > %d (DSM_MAX_CHANNEL_COUNT)", channel, DSM_MAX_CHANNEL_COUNT); dsm_guess_format(true); return false; } @@ -645,16 +651,6 @@ bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, values[channel] = value; } - /* - * Spektrum likes to send junk in higher channel numbers to fill - * their packets. We don't know about a 13 channel model in their TX - * lines, so if we get a channel count of 13, we'll return 12 (the last - * data index that is stable). - */ - if (*num_values == 13) { - *num_values = 12; - } - /* Set the 11-bit data indicator */ *dsm_11_bit = (dsm_channel_shift == 11); @@ -845,11 +841,18 @@ bool dsm_parse(const uint64_t now, const uint8_t *frame, const unsigned len, uin } if (decode_ret) { - *num_values = dsm_chan_count; + // require stable channel count (dsm_chan_count == dsm_chan_count_prev) before considering the decode valid + if ((dsm_chan_count > 0) && (dsm_chan_count <= DSM_MAX_CHANNEL_COUNT) && (dsm_chan_count == dsm_chan_count_prev)) { + *num_values = dsm_chan_count; + memcpy(&values[0], &dsm_chan_buf[0], dsm_chan_count * sizeof(dsm_chan_buf[0])); + + } else { + decode_ret = false; + } + + dsm_chan_count_prev = dsm_chan_count; - memcpy(&values[0], &dsm_chan_buf[0], dsm_chan_count * sizeof(dsm_chan_buf[0])); #ifdef DSM_DEBUG - printf("PACKET ---------\n"); printf("frame drops: %u, chan #: %u\n", dsm_frame_drops, dsm_chan_count); diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp index 115372eac5..8326850e6d 100644 --- a/src/lib/rc/rc_tests/RCTest.cpp +++ b/src/lib/rc/rc_tests/RCTest.cpp @@ -231,17 +231,17 @@ bool RCTest::ghstTest() bool RCTest::dsmTest10Ch() { - return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 6, 1500); + return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 17, 1500); } bool RCTest::dsmTest16Ch() { - return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 16, 3, 1500); + return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 16, 6, 1500); } bool RCTest::dsmTest22msDSMX16Ch() { - return dsmTest(TEST_DATA_PATH "dsm_x_dx9_px4_binding_data.txt", 16, 6, 1499); + return dsmTest(TEST_DATA_PATH "dsm_x_dx9_px4_binding_data.txt", 16, 11, 1499); } bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0) @@ -290,7 +290,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned &dsm_11_bit, &dsm_frame_drops, nullptr, max_channels); if (result) { - if (count > (16 * 10)) { // need to process enough data to have full channel count + if (count > (16 * 20)) { // need to process enough data to have full channel count ut_compare("num_values == expected_chancount", num_values, expected_chancount); } @@ -313,6 +313,8 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned fclose(fp); + ut_compare("num_values == expected_chancount", num_values, expected_chancount); + ut_test(ret == EOF); //PX4_INFO("drop: %d", (int)last_drop); ut_compare("last_drop == expected_dropcount", last_drop, expected_dropcount);