diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp index 7abed65105..eec468e59f 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp @@ -77,73 +77,14 @@ uint8_t AP_RCProtocol_ST24::st24_crc8(uint8_t *ptr, uint8_t len) void AP_RCProtocol_ST24::process_pulse(uint32_t width_s0, uint32_t width_s1) { - // convert to bit widths, allowing for up to about 4usec error, assuming 115200 bps - uint16_t bits_s0 = ((width_s0+4)*(uint32_t)115200) / 1000000; - uint16_t bits_s1 = ((width_s1+4)*(uint32_t)115200) / 1000000; - uint8_t bit_ofs, byte_ofs; - uint16_t nbits; - - if (bits_s0 == 0 || bits_s1 == 0) { - // invalid data - goto reset; + uint8_t b; + if (ss.process_pulse(width_s0, width_s1, b)) { + _process_byte(b); } - - byte_ofs = st24_state.bit_ofs/10; - bit_ofs = st24_state.bit_ofs%10; - if (byte_ofs >= ST24_MAX_FRAMELEN) { - goto reset; - } - // pull in the high bits - nbits = bits_s0; - if (nbits+bit_ofs > 10) { - nbits = 10 - bit_ofs; - } - st24_state.bytes[byte_ofs] |= ((1U< 10) { - // we have a full frame - uint8_t byte; - uint8_t i; - for (i=0; i <= byte_ofs; i++) { - // get raw data - uint16_t v = st24_state.bytes[i]; - - // check start bit - if ((v & 1) != 0) { - break; - } - // check stop bits - if ((v & 0x200) != 0x200) { - break; - } - byte = ((v>>1) & 0xFF); - process_byte(byte, 115200); - } - memset(&st24_state, 0, sizeof(st24_state)); - } - - byte_ofs = st24_state.bit_ofs/10; - bit_ofs = st24_state.bit_ofs%10; - - if (bits_s1+bit_ofs > 10) { - // invalid data - goto reset; - } - - // pull in the low bits - st24_state.bit_ofs += bits_s1; - return; -reset: - memset(&st24_state, 0, sizeof(st24_state)); } -void AP_RCProtocol_ST24::process_byte(uint8_t byte, uint32_t baudrate) +void AP_RCProtocol_ST24::_process_byte(uint8_t byte) { - if (baudrate != 115200) { - return; - } - switch (_decode_state) { case ST24_DECODE_STATE_UNSYNCED: if (byte == ST24_STX1) { @@ -282,3 +223,11 @@ void AP_RCProtocol_ST24::process_byte(uint8_t byte, uint32_t baudrate) break; } } + +void AP_RCProtocol_ST24::process_byte(uint8_t byte, uint32_t baudrate) +{ + if (baudrate != 115200) { + return; + } + _process_byte(byte); +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h index 96423908f1..1c0c937049 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h @@ -18,6 +18,7 @@ #pragma once #include "AP_RCProtocol.h" +#include "SoftSerial.h" #define ST24_DATA_LEN_MAX 64 #define ST24_MAX_FRAMELEN 70 @@ -41,6 +42,7 @@ public: void process_pulse(uint32_t width_s0, uint32_t width_s1) override; void process_byte(uint8_t byte, uint32_t baudrate) override; private: + void _process_byte(uint8_t byte); static uint8_t st24_crc8(uint8_t *ptr, uint8_t len); enum ST24_PACKET_TYPE { ST24_PACKET_TYPE_CHANNELDATA12 = 0, @@ -145,9 +147,5 @@ private: ReceiverFcPacket _rxpacket; - struct { - uint16_t bytes[70]; - uint16_t bit_ofs; - bool packet_parsed; - } st24_state; + SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1}; }; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp index 08d4c03b56..6856c74ef8 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp @@ -84,72 +84,14 @@ uint8_t AP_RCProtocol_SUMD::sumd_crc8(uint8_t crc, uint8_t value) void AP_RCProtocol_SUMD::process_pulse(uint32_t width_s0, uint32_t width_s1) { - // convert to bit widths, allowing for up to about 4usec error, assuming 115200 bps - uint16_t bits_s0 = ((width_s0+4)*(uint32_t)115200) / 1000000; - uint16_t bits_s1 = ((width_s1+4)*(uint32_t)115200) / 1000000; - uint8_t bit_ofs, byte_ofs; - uint16_t nbits; - - if (bits_s0 == 0 || bits_s1 == 0) { - // invalid data - goto reset; + uint8_t b; + if (ss.process_pulse(width_s0, width_s1, b)) { + _process_byte(b); } - - byte_ofs = sumd_state.bit_ofs/10; - bit_ofs = sumd_state.bit_ofs%10; - if (byte_ofs >= SUMD_FRAME_MAXLEN) { - goto reset; - } - // pull in the high bits - nbits = bits_s0; - if (nbits+bit_ofs > 10) { - nbits = 10 - bit_ofs; - } - sumd_state.bytes[byte_ofs] |= ((1U< 10) { - // we have a full frame - uint8_t byte; - uint8_t i; - for (i=0; i <= byte_ofs; i++) { - // get raw data - uint16_t v = sumd_state.bytes[i]; - - // check start bit - if ((v & 1) != 0) { - break; - } - // check stop bits - if ((v & 0x200) != 0x200) { - break; - } - byte = ((v>>1) & 0xFF); - process_byte(byte, 115200); - } - memset(&sumd_state, 0, sizeof(sumd_state)); - } - - byte_ofs = sumd_state.bit_ofs/10; - bit_ofs = sumd_state.bit_ofs%10; - - if (bits_s1+bit_ofs > 10) { - // invalid data - goto reset; - } - - // pull in the low bits - sumd_state.bit_ofs += bits_s1; - return; -reset: - memset(&sumd_state, 0, sizeof(sumd_state)); } -void AP_RCProtocol_SUMD::process_byte(uint8_t byte, uint32_t baudrate) +void AP_RCProtocol_SUMD::_process_byte(uint8_t byte) { - if (baudrate != 115200) { - return; - } switch (_decode_state) { case SUMD_DECODE_STATE_UNSYNCED: #ifdef SUMD_DEBUG @@ -387,3 +329,12 @@ void AP_RCProtocol_SUMD::process_byte(uint8_t byte, uint32_t baudrate) break; } } + +void AP_RCProtocol_SUMD::process_byte(uint8_t byte, uint32_t baudrate) +{ + if (baudrate != 115200) { + return; + } + _process_byte(byte); +} + diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h index 3eff2e5cdb..554751930b 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h @@ -18,6 +18,8 @@ #pragma once #include "AP_RCProtocol.h" +#include "SoftSerial.h" + #define SUMD_MAX_CHANNELS 32 #define SUMD_FRAME_MAXLEN 40 class AP_RCProtocol_SUMD : public AP_RCProtocol_Backend { @@ -25,7 +27,9 @@ public: AP_RCProtocol_SUMD(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} void process_pulse(uint32_t width_s0, uint32_t width_s1) override; void process_byte(uint8_t byte, uint32_t baudrate) override; + private: + void _process_byte(uint8_t byte); static uint16_t sumd_crc16(uint16_t crc, uint8_t value); static uint8_t sumd_crc8(uint8_t crc, uint8_t value); @@ -61,9 +65,6 @@ private: uint16_t _crc16 = 0x0000; bool _sumd = true; bool _crcOK = false; - struct { - uint16_t bytes[40]; - uint16_t bit_ofs; - bool packet_parsed; - } sumd_state; + + SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1}; };