From aab580fce2319dddc25177e2389711c8cf47cac7 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 14 Feb 2024 20:41:08 +0000 Subject: [PATCH 1/3] AP_RCProtocol: add tests for CRSF and fix protocol test fixed RCProtocolTest on SITL and make it pass/fail with an exit code Co-authored-by: Andrew Tridgell --- .../RCProtocolTest/RCProtocolTest.cpp | 133 +++++++++++++++++- 1 file changed, 129 insertions(+), 4 deletions(-) diff --git a/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp b/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp index 5f9ecea11a159..851069522560d 100644 --- a/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp +++ b/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp @@ -19,48 +19,101 @@ #include #include #include +#include #include #include void setup(); void loop(); +class RC_Channel_Example : public RC_Channel {}; + +class RC_Channels_Example : public RC_Channels +{ +public: + RC_Channel_Example obj_channels[NUM_RC_CHANNELS]; + + RC_Channel_Example *channel(const uint8_t chan) override { + if (chan >= NUM_RC_CHANNELS) { + return nullptr; + } + return &obj_channels[chan]; + } + +protected: + int8_t flight_mode_channel_number() const override { return 5; } +}; + +#define RC_CHANNELS_SUBCLASS RC_Channels_Example +#define RC_CHANNEL_SUBCLASS RC_Channel_Example + +#include + +static RC_Channels_Example rchannels; +static AP_SerialManager serial_manager; + const AP_HAL::HAL& hal = AP_HAL::get_HAL(); static AP_VideoTX vtx; // for set_vtx functions static AP_RCProtocol *rcprot; +static uint32_t test_count; +static uint32_t test_failures; + +static void delay_ms(uint32_t ms) +{ +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + hal.scheduler->stop_clock(AP_HAL::micros64()+ms*1000); +#else + hal.scheduler->delay(ms); +#endif +} + // setup routine void setup() { // introduction hal.console->printf("ArduPilot RC protocol test\n"); - hal.scheduler->delay(100); + delay_ms(100); } static bool check_result(const char *name, bool bytes, const uint16_t *values, uint8_t nvalues) { char label[20]; snprintf(label, 20, "%s(%s)", name, bytes?"bytes":"pulses"); - if (!rcprot->new_input()) { + const bool have_input = rcprot->new_input(); + if (values == nullptr) { + if (have_input) { + printf("%s: got input, none expected\n", label); + return false; + } else { + printf("%s: OK (no input)\n", label); + return true; + } + } + if (!have_input) { printf("%s: No new input\n", label); + test_failures++; return false; } const char *pname = rcprot->protocol_name(); if (strncmp(pname, name, strlen(pname)) != 0) { printf("%s: wrong protocol detected %s\n", label, rcprot->protocol_name()); + test_failures++; return false; } uint8_t n = rcprot->num_channels(); if (n != nvalues) { printf("%s: wrong number of channels %u should be %u\n", label, n, nvalues); + test_failures++; return false; } for (uint8_t i=0; iread(i); if (values[i] != v) { printf("%s: chan %u wrong value %u should be %u\n", label, i+1, v, values[i]); + test_failures++; return false; } } @@ -81,11 +134,11 @@ static bool test_byte_protocol(const char *name, uint32_t baudrate, for (uint8_t repeat=0; repeat 0 && i > 0 && ((i % pause_at) == 0)) { - hal.scheduler->delay(10); + delay_ms(10); } rcprot->process_byte(bytes[i], baudrate); } - hal.scheduler->delay(10); + delay_ms(10); if (repeat > repeats) { ret &= check_result(name, true, values, nvalues); } @@ -202,7 +255,33 @@ static bool test_protocol(const char *name, uint32_t baudrate, return ret; } +/* + test a protocol handler where we only expected byte input to work + */ +static bool test_protocol_bytesonly(const char *name, uint32_t baudrate, + const uint8_t *bytes, uint8_t nbytes, + const uint16_t *values, uint8_t nvalues, + uint8_t repeats=1, + int8_t pause_at=0, + bool inverted=false) +{ + bool ret = true; + rcprot = new AP_RCProtocol(); + rcprot->init(); + + ret &= test_byte_protocol(name, baudrate, bytes, nbytes, values, nvalues, repeats, pause_at); + delete rcprot; + + rcprot = new AP_RCProtocol(); + rcprot->init(); + ret &= test_pulse_protocol(name, baudrate, bytes, nbytes, nullptr, 0, repeats, pause_at, inverted); + delete rcprot; + + return ret; +} + //Main loop where the action takes place +#pragma GCC diagnostic error "-Wframe-larger-than=2000" void loop() { const uint8_t srxl_bytes[] = { 0xa5, 0x03, 0x0c, 0x04, 0x2f, 0x6c, 0x10, 0xb4, 0x26, @@ -340,6 +419,35 @@ void loop() // we only decode up to 18ch const uint16_t fport2_24ch_output[] = {1495, 1495, 986, 1495, 982, 1495, 982, 982, 1495, 2006, 982, 1495, 1495, 1495, 1495, 1495, 1495, 1495}; + const uint8_t crsf_bytes[] = {0xC8, 0x14, 0x17, 0x20, 0x03, 0x0C, 0xA0, 0x00, 0xF6, 0xB7, 0x6E, 0x94, 0xFC, + 0x1F, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE, 0x0F, 0x6E }; + const uint16_t crsf_output[] = {1501, 1500, 989, 1497, 1873, 1136, 2011, 988, 988, 988, 988, 2011, 0, 0, 0, 0, 0, 0}; + // CRSF partial frame followed by full frame + const uint8_t crsf_bad_bytes1[] = {0xC8, 0x14, 0x17, 0x20, 0x03, 0x0C, 0xA0, 0xC8, 0x14, 0x17, 0x20, 0x03, 0x0C, 0xA0, 0x00, 0xF6, 0xB7, 0x6E, 0x94, 0xFC, + 0x1F, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE, 0x0F, 0x6E }; + const uint16_t crsf_bad_output1[] = {1501, 1500, 989, 1497, 1873, 1136, 2011, 988, 988, 988, 988, 2011, 0, 0, 0, 0, 0, 0}; + // CRSF full frame with bad CRC followed by full frame + const uint8_t crsf_bad_bytes2[] = {0xC8, 0x14, 0x17, 0x20, 0x03, 0x0C, 0xA0, 0x00, 0xF6, 0xB7, 0x6E, 0x94, 0xFC, + 0x1F, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE, 0x0F, 0x6F, + 0xC8, 0x14, 0x17, 0x20, 0x03, 0x0C, 0xA0, 0x00, 0xF6, 0xB7, 0x6E, 0x94, 0xFC, + 0x1F, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE, 0x0F, 0x6E }; + const uint16_t crsf_bad_output2[] = {1501, 1500, 989, 1497, 1873, 1136, 2011, 988, 988, 988, 988, 2011, 0, 0, 0, 0, 0, 0}; + + // CRSF with lots of start markers followed by full frame + const uint8_t crsf_bad_bytes3[] = {0xC8, 0x14, 0xC8, 0x14, 0xC8, 0x0C, 0xA0, + 0xC8, 0x14, 0x17, 0x20, 0x03, 0x0C, 0xA0, 0x00, 0xF6, 0xB7, 0x6E, 0x94, 0xFC, + 0x1F, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE, 0x0F, 0x6E, }; + const uint16_t crsf_bad_output3[] = {1501, 1500, 989, 1497, 1873, 1136, 2011, 988, 988, 988, 988, 2011, 0, 0, 0, 0, 0, 0}; + + // CRSF with a partial frame followed by a full frame + const uint8_t crsf_bad_bytes4[] = { + 0xC8, 0x14, 0x17, 0x20, 0x03, 0x0C, 0xA0, 0x00, 0xF6, 0xB7, 0x6E, 0x94, 0xFC, + 0x1F, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE, + 0xC8, 0x14, 0x17, 0x20, 0x03, 0x0C, 0xA0, 0x00, 0xF6, 0xB7, 0x6E, 0x94, 0xFC, + 0x1F, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE, 0x0F, 0x6E }; + const uint16_t crsf_bad_output4[] = {1501, 1500, 989, 1497, 1873, 1136, 2011, 988, 988, 988, 988, 2011, 0, 0, 0, 0, 0, 0}; + + test_protocol("SRXL", 115200, srxl_bytes, sizeof(srxl_bytes), srxl_output, ARRAY_SIZE(srxl_output), 1); test_protocol("SUMD", 115200, sumd_bytes, sizeof(sumd_bytes), sumd_output, ARRAY_SIZE(sumd_output), 1); test_protocol("SUMD2", 115200, sumd_bytes2, sizeof(sumd_bytes2), sumd_output2, ARRAY_SIZE(sumd_output2), 1); @@ -349,6 +457,13 @@ void loop() // SBUS needs 3 repeats to pass the RCProtocol 3 frames test test_protocol("SBUS", 100000, sbus_bytes, sizeof(sbus_bytes), sbus_output, ARRAY_SIZE(sbus_output), 3, 0, true); + // CRSF needs 3 repeats to pass the RCProtocol 3 frames test + test_protocol_bytesonly("CRSF", 416666, crsf_bytes, sizeof(crsf_bytes), crsf_output, ARRAY_SIZE(crsf_output), 3, 0, true); + test_protocol_bytesonly("CRSF2", 416666, crsf_bad_bytes1, sizeof(crsf_bad_bytes1), crsf_bad_output1, ARRAY_SIZE(crsf_bad_output1), 3, 0, true); + test_protocol_bytesonly("CRSF3", 416666, crsf_bad_bytes2, sizeof(crsf_bad_bytes2), crsf_bad_output2, ARRAY_SIZE(crsf_bad_output2), 3, 0, true); + test_protocol_bytesonly("CRSF4", 416666, crsf_bad_bytes3, sizeof(crsf_bad_bytes3), crsf_bad_output3, ARRAY_SIZE(crsf_bad_output3), 3, 0, true); + test_protocol_bytesonly("CRSF5", 416666, crsf_bad_bytes4, sizeof(crsf_bad_bytes4), crsf_bad_output4, ARRAY_SIZE(crsf_bad_output4), 3, 0, true); + // DSM needs 8 repeats, 5 to guess the format, then 3 to pass the RCProtocol 3 frames test test_protocol("DSM1", 115200, dsm_bytes, sizeof(dsm_bytes), dsm_output, ARRAY_SIZE(dsm_output), 9); test_protocol("DSM2", 115200, dsm_bytes2, sizeof(dsm_bytes2), dsm_output2, ARRAY_SIZE(dsm_output2), 9, 16); @@ -363,6 +478,16 @@ void loop() test_protocol("FPORT", 115200, fport_bytes, sizeof(fport_bytes), fport_output, ARRAY_SIZE(fport_output), 3, 0, true); test_protocol("FPORT2_16CH", 115200, fport2_16ch_bytes, sizeof(fport2_16ch_bytes), fport2_16ch_output, ARRAY_SIZE(fport2_16ch_output), 3, 0, true); test_protocol("FPORT2_24CH", 115200, fport2_24ch_bytes, sizeof(fport2_24ch_bytes), fport2_24ch_output, ARRAY_SIZE(fport2_24ch_output), 3, 0, true); + + if (test_count++ == 10) { + if (test_failures == 0) { + printf("Test PASSED\n"); + ::exit(0); + } + printf("Test FAILED - %u failures\n", unsigned(test_failures)); + ::exit(1); + } + printf("Test count %u - %u failures\n", unsigned(test_count), unsigned(test_failures)); } AP_HAL_MAIN(); From c051bbe949a47a37d1ff7857ac19c4eb980f7d20 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 10 Feb 2024 14:57:24 +0000 Subject: [PATCH 2/3] AP_RCProtocol: decode high duty-cycle CRSF frames using frame markers rather than timeouts Co-authored-by: Andrew Tridgell --- .../AP_RCProtocol/AP_RCProtocol_CRSF.cpp | 144 ++++++++++++------ libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h | 8 +- 2 files changed, 99 insertions(+), 53 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index eb454752d5541..065795acb331a 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -146,7 +146,7 @@ static const char* get_frame_type(uint8_t byte, uint8_t subtype = 0) # define debug(fmt, args...) do {} while(0) #endif -#define CRSF_FRAME_TIMEOUT_US 10000U // 10ms to account for scheduling delays +#define CRSF_FRAME_TIMEOUT_US 50000U // 50ms to account for failure of the frame sync and long scheduling delays #define CRSF_INTER_FRAME_TIME_US_250HZ 4000U // At fastest, frames are sent by the transmitter every 4 ms, 250 Hz #define CRSF_INTER_FRAME_TIME_US_150HZ 6667U // At medium, frames are sent by the transmitter every 6.667 ms, 150 Hz #define CRSF_INTER_FRAME_TIME_US_50HZ 20000U // At slowest, frames are sent by the transmitter every 20ms, 50 Hz @@ -212,76 +212,132 @@ uint16_t AP_RCProtocol_CRSF::get_link_rate(ProtocolType protocol) const { } } -void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte) +// process a byte provided by a uart from rc stack +void AP_RCProtocol_CRSF::process_byte(uint8_t byte, uint32_t baudrate) +{ + // reject RC data if we have been configured for standalone mode + if ((baudrate != CRSF_BAUDRATE && baudrate != CRSF_BAUDRATE_1MBIT && baudrate != CRSF_BAUDRATE_2MBIT) || _uart) { + return; + } + _process_byte(byte); +} + +// process a byte provided by a uart +void AP_RCProtocol_CRSF::_process_byte(uint8_t byte) { //debug("process_byte(0x%x)", byte); - // we took too long decoding, start again - the RX will only send complete frames so this is unlikely to fail, - // however thread scheduling can introduce longer delays even when the data has been received - if (_frame_ofs > 0 && (timestamp_us - _start_frame_time_us) > CRSF_FRAME_TIMEOUT_US) { + const uint32_t now = AP_HAL::micros(); + + // extra check for overflow, should never happen since it will have been handled in check_frame() + if (_frame_ofs >= sizeof(_frame)) { _frame_ofs = 0; } - // overflow check - if (_frame_ofs >= CRSF_FRAMELEN_MAX) { + // check for long frame gaps + // we took too long decoding, start again - the RX will only send complete frames so this is unlikely to fail, + // however thread scheduling can introduce longer delays even when the data has been received + if (_frame_ofs > 0 && (now - _start_frame_time_us) > CRSF_FRAME_TIMEOUT_US) { _frame_ofs = 0; } // start of a new frame if (_frame_ofs == 0) { - _start_frame_time_us = timestamp_us; + _start_frame_time_us = now; } + + _frame_bytes[_frame_ofs++] = byte; + + if (!check_frame(now)) { + skip_to_next_frame(now); + } +} - add_to_buffer(_frame_ofs++, byte); +// check if a frame is valid. Return false if the frame is definitely +// invalid. Return true if we need more bytes +bool AP_RCProtocol_CRSF::check_frame(uint32_t timestamp_us) +{ + // overflow check + if (_frame_ofs >= sizeof(_frame)) { + return false; + } // need a header to get the length if (_frame_ofs < CRSF_HEADER_TYPE_LEN) { - return; + return true; } if (_frame.device_address != DeviceAddress::CRSF_ADDRESS_FLIGHT_CONTROLLER) { - return; - } - - // parse the length - if (_frame_ofs == CRSF_HEADER_TYPE_LEN) { - _frame_crc = crc8_dvb_s2(0, _frame.type); - // check for garbage frame - if (_frame.length > CRSF_FRAME_PAYLOAD_MAX) { - _frame_ofs = 0; - } - return; - } - - // update crc - if (_frame_ofs < _frame.length + CRSF_HEADER_LEN) { - _frame_crc = crc8_dvb_s2(_frame_crc, byte); + return false; } - // overflow check - if (_frame_ofs > _frame.length + CRSF_HEADER_LEN) { - _frame_ofs = 0; - return; + // check validity of the length byte if we have received it + if (_frame_ofs >= CRSF_HEADER_TYPE_LEN && + _frame.length > CRSF_FRAME_PAYLOAD_MAX) { + return false; } // decode whatever we got and expect - if (_frame_ofs == _frame.length + CRSF_HEADER_LEN) { - log_data(AP_RCProtocol::CRSF, timestamp_us, (const uint8_t*)&_frame, _frame_ofs - CRSF_HEADER_LEN); + if (_frame_ofs >= _frame.length + CRSF_HEADER_LEN) { + const uint8_t crc = crc8_dvb_s2_update(0, &_frame_bytes[CRSF_HEADER_LEN], _frame.length - 1); - // we consumed the partial frame, reset - _frame_ofs = 0; + //debug("check_frame(0x%x, 0x%x)", _frame.device_address, _frame.length); - // bad CRC (payload start is +1 from frame start, so need to subtract that from frame length to get index) - if (_frame_crc != _frame.payload[_frame.length - 2]) { - return; + if (crc != _frame.payload[_frame.length - 2]) { + return false; } - _last_frame_time_us = _last_rx_frame_time_us = timestamp_us; + log_data(AP_RCProtocol::CRSF, timestamp_us, (const uint8_t*)&_frame, _frame.length + CRSF_HEADER_LEN); + // decode here if (decode_crsf_packet()) { _last_tx_frame_time_us = timestamp_us; // we have received a frame from the transmitter add_input(MAX_CHANNELS, _channels, false, _link_status.rssi, _link_status.link_quality); } + + // we consumed the frame + const auto len = _frame.length + CRSF_HEADER_LEN; + _frame_ofs -= len; + if (_frame_ofs > 0) { + memmove(_frame_bytes, _frame_bytes+len, _frame_ofs); + } + + _last_frame_time_us = _last_rx_frame_time_us = timestamp_us; + + return true; + } + + // more bytes to come + return true; +} + +// called when parsing or CRC fails on a frame +void AP_RCProtocol_CRSF::skip_to_next_frame(uint32_t timestamp_us) +{ + if (_frame_ofs <= 1) { + return; } + + /* + look for a frame header in the remaining bytes + */ + const uint8_t *new_header = (const uint8_t *)memchr(&_frame_bytes[1], DeviceAddress::CRSF_ADDRESS_FLIGHT_CONTROLLER, _frame_ofs - 1); + if (new_header == nullptr) { + _frame_ofs = 0; + return; + } + + /* + setup the current state as the remaining bytes, if any + */ + _frame_ofs -= (new_header - _frame_bytes); + if (_frame_ofs > 0) { + memmove(_frame_bytes, new_header, _frame_ofs); + } + + _start_frame_time_us = timestamp_us; + + // we could now have a good frame + check_frame(timestamp_us); } void AP_RCProtocol_CRSF::update(void) @@ -299,7 +355,7 @@ void AP_RCProtocol_CRSF::update(void) for (uint8_t i = 0; i < n; i++) { int16_t b = _uart->read(); if (b >= 0) { - _process_byte(AP_HAL::micros(), uint8_t(b)); + _process_byte(uint8_t(b)); } } } @@ -570,16 +626,6 @@ void AP_RCProtocol_CRSF::process_link_stats_tx_frame(const void* data) } } -// process a byte provided by a uart -void AP_RCProtocol_CRSF::process_byte(uint8_t byte, uint32_t baudrate) -{ - // reject RC data if we have been configured for standalone mode - if ((baudrate != CRSF_BAUDRATE && baudrate != CRSF_BAUDRATE_1MBIT && baudrate != CRSF_BAUDRATE_2MBIT) || _uart) { - return; - } - _process_byte(AP_HAL::micros(), byte); -} - // start the uart if we have one void AP_RCProtocol_CRSF::start_uart() { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h index 3546ed484cc9e..18d69a8e69889 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h @@ -289,15 +289,17 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend { private: struct Frame _frame; + uint8_t *_frame_bytes = (uint8_t*)&_frame; struct Frame _telemetry_frame; uint8_t _frame_ofs; - uint8_t _frame_crc; const uint8_t MAX_CHANNELS = MIN((uint8_t)CRSF_MAX_CHANNELS, (uint8_t)MAX_RCIN_CHANNELS); static AP_RCProtocol_CRSF* _singleton; - void _process_byte(uint32_t timestamp_us, uint8_t byte); + void _process_byte(uint8_t byte); + bool check_frame(uint32_t timestamp_us); + void skip_to_next_frame(uint32_t timestamp_us); bool decode_crsf_packet(); bool process_telemetry(bool check_constraint = true); void process_link_stats_frame(const void* data); @@ -312,8 +314,6 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend { uint16_t _channels[CRSF_MAX_CHANNELS]; /* buffer for extracted RC channel data as pulsewidth in microseconds */ - void add_to_buffer(uint8_t index, uint8_t b) { ((uint8_t*)&_frame)[index] = b; } - uint32_t _last_frame_time_us; uint32_t _last_tx_frame_time_us; uint32_t _last_uart_start_time_ms; From b464712888106721f874459726fa3909334e6e2b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 21 Feb 2024 10:25:49 +1100 Subject: [PATCH 3/3] AP_RCProtocol: CRSF: use subtraction with times, not time+timedelta --- libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h index 18d69a8e69889..fe065d0071359 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h @@ -58,13 +58,13 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend { bool is_rx_active() const override { // later versions of CRSFv3 will send link rate frames every 200ms // but only before an initial failsafe - return AP_HAL::micros() < _last_rx_frame_time_us + CRSF_RX_TIMEOUT; + return _last_rx_frame_time_us != 0 && AP_HAL::micros() - _last_rx_frame_time_us < CRSF_RX_TIMEOUT; } // is the transmitter active, used to adjust telemetry data bool is_tx_active() const { // this is the same as the Copter failsafe timeout - return AP_HAL::micros() < _last_tx_frame_time_us + CRSF_TX_TIMEOUT; + return _last_tx_frame_time_us != 0 && AP_HAL::micros() - _last_tx_frame_time_us < CRSF_TX_TIMEOUT; } // get singleton instance