From 096f459148fbe0af30b85c6e49880e21eda58e05 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 21 Oct 2021 09:18:22 +0900 Subject: [PATCH] AP_Torqeedo: handle incoming escape characters --- libraries/AP_Torqeedo/AP_Torqeedo.cpp | 14 +++++++++++++- libraries/AP_Torqeedo/AP_Torqeedo.h | 1 + 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Torqeedo/AP_Torqeedo.cpp b/libraries/AP_Torqeedo/AP_Torqeedo.cpp index 7420bcbdce..cb17eac899 100644 --- a/libraries/AP_Torqeedo/AP_Torqeedo.cpp +++ b/libraries/AP_Torqeedo/AP_Torqeedo.cpp @@ -25,7 +25,9 @@ #define TORQEEDO_SERIAL_BAUD 19200 // communication is always at 19200 #define TORQEEDO_PACKET_HEADER 0xAC // communication packet header -#define TORQEEDO_PACKET_FOOTER 0xAD // communication packer footer +#define TORQEEDO_PACKET_FOOTER 0xAD // communication packet footer +#define TORQEEDO_PACKET_ESCAPE 0xAE // escape character for handling occurrences of header, footer and this escape bytes in original message +#define TORQEEDO_PACKET_ESCAPE_MASK 0x80 // byte after ESCAPE character should be XOR'd with this value #define TORQEEDO_LOG_TRQD_INTERVAL_MS 5000// log TRQD message at this interval in milliseconds #define TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_MS 100 // motor speed sent at 10hz if connected to motor #define TORQEEDO_SEND_MOTOR_STATUS_REQUEST_INTERVAL_MS 400 // motor status requested every 0.4sec if connected to motor @@ -404,6 +406,7 @@ bool AP_Torqeedo::parse_byte(uint8_t b) _parse_state = ParseState::WAITING_FOR_FOOTER; } _received_buff_len = 0; + _parse_escape_received = false; break; case ParseState::WAITING_FOR_FOOTER: if (b == TORQEEDO_PACKET_FOOTER) { @@ -429,6 +432,15 @@ bool AP_Torqeedo::parse_byte(uint8_t b) } complete_msg_received = true; } else { + // escape character handling + if (_parse_escape_received) { + b ^= TORQEEDO_PACKET_ESCAPE_MASK; + _parse_escape_received = false; + } else if (b == TORQEEDO_PACKET_ESCAPE) { + // escape character received, record this and ignore this byte + _parse_escape_received = true; + break; + } // add to buffer _received_buff[_received_buff_len] = b; _received_buff_len++; diff --git a/libraries/AP_Torqeedo/AP_Torqeedo.h b/libraries/AP_Torqeedo/AP_Torqeedo.h index e5383f4f64..61dbda7ecc 100644 --- a/libraries/AP_Torqeedo/AP_Torqeedo.h +++ b/libraries/AP_Torqeedo/AP_Torqeedo.h @@ -229,6 +229,7 @@ private: // message parsing members ParseState _parse_state; // current state of parsing + bool _parse_escape_received; // true if the escape character has been received so we must XOR the next byte uint32_t _parse_error_count; // total number of parsing errors (for reporting) uint32_t _parse_success_count; // number of messages successfully parsed (for reporting) uint8_t _received_buff[TORQEEDO_MESSAGE_LEN_MAX]; // characters received