aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_RCProtocol: added timeout handling to SUMD

Browse Source
This commit is contained in:
Andrew Tridgell 2018-11-05 15:50:11 +11:00
parent b1e5e0aba9
commit 7996259726
2 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp
View File

@ -86,12 +86,15 @@ void AP_RCProtocol_SUMD::process_pulse(uint32_t width_s0, uint32_t width_s1)
{ {
uint8_t b; uint8_t b;
if (ss.process_pulse(width_s0, width_s1, b)) { if (ss.process_pulse(width_s0, width_s1, b)) {
_process_byte(b); _process_byte(ss.get_byte_timestamp_us(), b);
} }
} }
void AP_RCProtocol_SUMD::_process_byte(uint8_t byte) void AP_RCProtocol_SUMD::_process_byte(uint32_t timestamp_us, uint8_t byte)
{ {
if (timestamp_us - last_packet_us > 3000U) {
_decode_state = SUMD_DECODE_STATE_UNSYNCED;
}
switch (_decode_state) { switch (_decode_state) {
case SUMD_DECODE_STATE_UNSYNCED: case SUMD_DECODE_STATE_UNSYNCED:
#ifdef SUMD_DEBUG #ifdef SUMD_DEBUG
@ -111,6 +114,7 @@ void AP_RCProtocol_SUMD::_process_byte(uint8_t byte)
#ifdef SUMD_DEBUG #ifdef SUMD_DEBUG
hal.console->printf(" SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ; hal.console->printf(" SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ;
#endif #endif
last_packet_us = timestamp_us;
} }
break; break;
@ -335,6 +339,6 @@ void AP_RCProtocol_SUMD::process_byte(uint8_t byte, uint32_t baudrate)
if (baudrate != 115200) { if (baudrate != 115200) {
return; return;
} }
_process_byte(byte); _process_byte(AP_HAL::micros(), byte);
} }

3
libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h
View File

@ -29,7 +29,7 @@ public:
void process_byte(uint8_t byte, uint32_t baudrate) override; void process_byte(uint8_t byte, uint32_t baudrate) override;
private: private:
void _process_byte(uint8_t byte); void _process_byte(uint32_t timestamp_us, uint8_t byte);
static uint16_t sumd_crc16(uint16_t crc, uint8_t value); static uint16_t sumd_crc16(uint16_t crc, uint8_t value);
static uint8_t sumd_crc8(uint8_t crc, uint8_t value); static uint8_t sumd_crc8(uint8_t crc, uint8_t value);
@ -65,6 +65,7 @@ private:
uint16_t _crc16 = 0x0000; uint16_t _crc16 = 0x0000;
bool _sumd = true; bool _sumd = true;
bool _crcOK = false; bool _crcOK = false;
uint32_t last_packet_us;
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1}; SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
}; };