From bbcc31ecec6f8fb21db732b3285e7a01d62dab42 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Thu, 22 Jun 2023 17:20:37 -0700 Subject: [PATCH] AP_RCProtocol: Refactor SUMD to remove legacy cruft Of particular note this removes the partial SUMH support. This has been broken as far back as 2018, as we only called add_input() if we were in the SUMD or SUMD failsafe cases, which meant SUMH didn't work. This commits to that path, and removes all the 8 bit CRC, SUMH support. This also refactors a number of things for readability (condensing white space, simplifying guards around debug code). Many of the debugs could probably be removed, but this simplifies it. This was tested against the RCProtocol example test, and everything there reports a pass. --- .../AP_RCProtocol/AP_RCProtocol_Backend.cpp | 5 + .../AP_RCProtocol/AP_RCProtocol_SUMD.cpp | 159 ++---------------- libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h | 14 +- 3 files changed, 20 insertions(+), 158 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp index 01b0fe9acb..9575ab22f7 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp @@ -179,6 +179,11 @@ void AP_RCProtocol_Backend::configure_vtx(uint8_t band, uint8_t channel, uint8_t void AP_RCProtocol_Backend::log_data(AP_RCProtocol::rcprotocol_t prot, uint32_t timestamp, const uint8_t *data, uint8_t len) const { #if HAL_LOGGING_ENABLED +#if (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX) + if (&rc() == nullptr) { // allow running without RC_Channels if we are doing the examples + return; + } +#endif if (rc().option_is_enabled(RC_Channels::Option::LOG_RAW_DATA)) { uint32_t u32[10] {}; if (len > sizeof(u32)) { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp index e385dd6eba..8f313279f0 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp @@ -56,26 +56,9 @@ #define SUMD_ID_SUMD 0x01 #define SUMD_ID_FAILSAFE 0x81 -/* define range mapping here, -+100% -> 1000..2000 */ -#define SUMD_RANGE_MIN 0.0f -#define SUMD_RANGE_MAX 4096.0f - -#define SUMD_TARGET_MIN 1000.0f -#define SUMD_TARGET_MAX 2000.0f - -/* pre-calculate the floating point stuff as far as possible at compile time */ -#define SUMD_SCALE_FACTOR ((SUMD_TARGET_MAX - SUMD_TARGET_MIN) / (SUMD_RANGE_MAX - SUMD_RANGE_MIN)) -#define SUMD_SCALE_OFFSET (int)(SUMD_TARGET_MIN - (SUMD_SCALE_FACTOR * SUMD_RANGE_MIN + 0.5f)) - // #define SUMD_DEBUG extern const AP_HAL::HAL& hal; -uint8_t AP_RCProtocol_SUMD::sumd_crc8(uint8_t crc, uint8_t value) -{ - crc += value; - return crc; -} - void AP_RCProtocol_SUMD::process_pulse(uint32_t width_s0, uint32_t width_s1) { uint8_t b; @@ -95,14 +78,8 @@ void AP_RCProtocol_SUMD::_process_byte(uint32_t timestamp_us, uint8_t byte) hal.console->printf(" SUMD_DECODE_STATE_UNSYNCED \n") ; #endif if (byte == SUMD_HEADER_ID) { - _rxpacket.header = byte; - _sumd = true; _rxlen = 0; - _crc16 = 0x0000; - _crc8 = 0x00; - _crcOK = false; - _crc16 = crc_xmodem_update(_crc16, byte); - _crc8 = sumd_crc8(_crc8, byte); + _crc16 = crc_xmodem_update(0, byte); _decode_state = SUMD_DECODE_STATE_GOT_HEADER; #ifdef SUMD_DEBUG @@ -113,26 +90,13 @@ void AP_RCProtocol_SUMD::_process_byte(uint32_t timestamp_us, uint8_t byte) break; case SUMD_DECODE_STATE_GOT_HEADER: - if (byte == SUMD_ID_SUMD || byte == SUMD_ID_SUMH) { + if ((byte == SUMD_ID_SUMD) || (byte == SUMD_ID_FAILSAFE)) { _rxpacket.status = byte; - - if (byte == SUMD_ID_SUMH) { - _sumd = false; - } - - if (_sumd) { - _crc16 = crc_xmodem_update(_crc16, byte); - - } else { - _crc8 = sumd_crc8(_crc8, byte); - } - + _crc16 = crc_xmodem_update(_crc16, byte); _decode_state = SUMD_DECODE_STATE_GOT_STATE; - #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_GOT_STATE: %x \n", byte) ; #endif - } else { _decode_state = SUMD_DECODE_STATE_UNSYNCED; } @@ -142,21 +106,13 @@ void AP_RCProtocol_SUMD::_process_byte(uint32_t timestamp_us, uint8_t byte) case SUMD_DECODE_STATE_GOT_STATE: if (byte >= 2 && byte <= SUMD_MAX_CHANNELS) { _rxpacket.length = byte; - - if (_sumd) { - _crc16 = crc_xmodem_update(_crc16, byte); - - } else { - _crc8 = sumd_crc8(_crc8, byte); - } - + _crc16 = crc_xmodem_update(_crc16, byte); _rxlen++; _decode_state = SUMD_DECODE_STATE_GOT_LEN; #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_GOT_LEN: %x (%d) \n", byte, byte) ; #endif - } else { _decode_state = SUMD_DECODE_STATE_UNSYNCED; } @@ -165,118 +121,43 @@ void AP_RCProtocol_SUMD::_process_byte(uint32_t timestamp_us, uint8_t byte) case SUMD_DECODE_STATE_GOT_LEN: _rxpacket.sumd_data[_rxlen] = byte; - - if (_sumd) { - _crc16 = crc_xmodem_update(_crc16, byte); - - } else { - _crc8 = sumd_crc8(_crc8, byte); - } - + _crc16 = crc_xmodem_update(_crc16, byte); _rxlen++; - if (_rxlen <= ((_rxpacket.length * 2))) { #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_GOT_DATA[%d]: %x\n", _rxlen - 2, byte) ; #endif - } else { _decode_state = SUMD_DECODE_STATE_GOT_DATA; - #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_GOT_DATA -- finish --\n") ; #endif - } break; case SUMD_DECODE_STATE_GOT_DATA: _rxpacket.crc16_high = byte; - + _decode_state = SUMD_DECODE_STATE_GOT_CRC; #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC16[1]: %x [%x]\n", byte, ((_crc16 >> 8) & 0xff)) ; #endif - - if (_sumd) { - _decode_state = SUMD_DECODE_STATE_GOT_CRC; - - } else { - _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_1; - } - - break; - - case SUMD_DECODE_STATE_GOT_CRC16_BYTE_1: - _rxpacket.crc16_low = byte; - -#ifdef SUMD_DEBUG - hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC16[2]: %x [%x]\n", byte, (_crc16 & 0xff)) ; -#endif - - _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_2; - - break; - - case SUMD_DECODE_STATE_GOT_CRC16_BYTE_2: - _rxpacket.telemetry = byte; - -#ifdef SUMD_DEBUG - hal.console->printf(" SUMD_DECODE_STATE_GOT_SUMH_TELEMETRY: %x\n", byte) ; -#endif - - _decode_state = SUMD_DECODE_STATE_GOT_CRC; - break; case SUMD_DECODE_STATE_GOT_CRC: - if (_sumd) { - _rxpacket.crc16_low = byte; - #ifdef SUMD_DEBUG - hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC[2]: %x [%x]\n\n", byte, (_crc16 & 0xff)) ; + hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC[2]: %x [%x]\n\n", byte, (_crc16 & 0xff)) ; #endif - if (_crc16 == (uint16_t)(_rxpacket.crc16_high << 8) + _rxpacket.crc16_low) { - _crcOK = true; - } - - } else { - _rxpacket.crc8 = byte; - -#ifdef SUMD_DEBUG - hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC8_SUMH: %x [%x]\n\n", byte, _crc8) ; -#endif - - if (_crc8 == _rxpacket.crc8) { - _crcOK = true; - } - } - log_data(AP_RCProtocol::SUMD, timestamp_us, _rxpacket.sumd_data, _rxlen); - if (_crcOK) { + if (_crc16 == (uint16_t)(_rxpacket.crc16_high << 8) + byte) { #ifdef SUMD_DEBUG hal.console->printf(" CRC - OK \n") ; -#endif - if (_sumd) { -#ifdef SUMD_DEBUG - hal.console->printf(" Got valid SUMD Packet\n") ; -#endif - } else { -#ifdef SUMD_DEBUG - hal.console->printf(" Got valid SUMH Packet\n") ; -#endif - - } - -#ifdef SUMD_DEBUG + hal.console->printf(" Got valid SUMD Packet\n") ; hal.console->printf(" RXLEN: %d [Chans: %d] \n\n", _rxlen - 1, (_rxlen - 1) / 2) ; #endif - - unsigned i; - uint8_t num_values; uint16_t values[SUMD_MAX_CHANNELS]; /* received Channels */ @@ -284,8 +165,6 @@ void AP_RCProtocol_SUMD::_process_byte(uint32_t timestamp_us, uint8_t byte) _rxpacket.length = (uint8_t) SUMD_MAX_CHANNELS; } - num_values = (uint16_t)_rxpacket.length; - /* decode the actual packet */ /* reorder first 4 channels */ @@ -299,29 +178,19 @@ void AP_RCProtocol_SUMD::_process_byte(uint32_t timestamp_us, uint8_t byte) values[3] = (uint16_t)((_rxpacket.sumd_data[3 * 2 + 1] << 8) | _rxpacket.sumd_data[3 * 2 + 2]) >> 3; /* we start at channel 5(index 4) */ - unsigned chan_index = 4; - - for (i = 4; i < _rxpacket.length; i++) { + for (uint8_t i = 4; i < _rxpacket.length; i++) { #ifdef SUMD_DEBUG hal.console->printf("ch[%u] : %x %x [ %x %d ]\n", i + 1, _rxpacket.sumd_data[i * 2 + 1], _rxpacket.sumd_data[i * 2 + 2], ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3, ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3); #endif - values[chan_index] = (uint16_t)((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3; - /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - //channels[chan_index] = (uint16_t)(channels[chan_index] * SUMD_SCALE_FACTOR + .5f) + SUMD_SCALE_OFFSET; - - chan_index++; + values[i] = (uint16_t)((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3; } - if (_rxpacket.status == 0x01) { - add_input(num_values, values, false); - } else if (_rxpacket.status == 0x81) { - add_input(num_values, values, true); - } - } else { + add_input(_rxpacket.length, values, (_rxpacket.status == SUMD_ID_FAILSAFE)); #ifdef SUMD_DEBUG - hal.console->printf(" CRC - fail 0x%X 0x%X\n", _crc16, (uint16_t)(_rxpacket.crc16_high << 8) + _rxpacket.crc16_low); + } else { + hal.console->printf(" CRC - fail 0x%X 0x%X\n", _crc16, (uint16_t)(_rxpacket.crc16_high << 8) + byte); #endif } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h index 943618c478..f9ad0702ba 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h @@ -34,20 +34,13 @@ public: private: void _process_byte(uint32_t timestamp_us, uint8_t byte); - static uint8_t sumd_crc8(uint8_t crc, uint8_t value); -#pragma pack(push, 1) typedef struct { - uint8_t header; ///< 0xA8 for a valid packet uint8_t status; ///< 0x01 valid and live SUMD data frame / 0x00 = SUMH / 0x81 = Failsafe uint8_t length; ///< Channels uint8_t sumd_data[(SUMD_MAX_CHANNELS+1) * 2]; ///< ChannelData (High Byte/ Low Byte) uint8_t crc16_high; ///< High Byte of 16 Bit CRC - uint8_t crc16_low; ///< Low Byte of 16 Bit CRC - uint8_t telemetry; ///< Telemetry request - uint8_t crc8; ///< SUMH CRC8 } ReceiverFcPacketHoTT; -#pragma pack(pop) enum SUMD_DECODE_STATE { @@ -57,17 +50,12 @@ private: SUMD_DECODE_STATE_GOT_LEN, SUMD_DECODE_STATE_GOT_DATA, SUMD_DECODE_STATE_GOT_CRC, - SUMD_DECODE_STATE_GOT_CRC16_BYTE_1, - SUMD_DECODE_STATE_GOT_CRC16_BYTE_2 }; enum SUMD_DECODE_STATE _decode_state = SUMD_DECODE_STATE_UNSYNCED; uint8_t _rxlen; ReceiverFcPacketHoTT _rxpacket; - uint8_t _crc8 = 0x00; - uint16_t _crc16 = 0x0000; - bool _sumd = true; - bool _crcOK = false; + uint16_t _crc16; uint32_t last_packet_us; SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};