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};