Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Michael du Breuil 2023-06-22 17:20:37 -07:00 committed by Andrew Tridgell
parent 8b13413f43
commit bbcc31ecec
3 changed files with 20 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp
View File

@ -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)) {

159
libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp
View File

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

14
libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h
View File

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