diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.cpp deleted file mode 100644 index 3072dab31e..0000000000 --- a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.cpp +++ /dev/null @@ -1,323 +0,0 @@ -#include -#include - -#include "AP_Frsky_MAVlite.h" - -extern const AP_HAL::HAL& hal; - -void AP_Frsky_MAVlite::update_checksum(mavlite_message_t &msg, const uint8_t c) -{ - msg.checksum += c; //0-1FF - msg.checksum += msg.checksum >> 8; - msg.checksum &= 0xFF; -} - -void AP_Frsky_MAVlite::parser_reset(void) -{ - _rxmsg.checksum = 0; - _rxmsg.len = 0; - _rxmsg.msgid = 0; - - _rxstatus.current_rx_seq = 0; - _rxstatus.payload_next_byte = 0; - _rxstatus.parse_state = ParseState::IDLE; -} - -void AP_Frsky_MAVlite::encoder_reset(mavlite_message_t &txmsg) -{ - txmsg.checksum = 0; - - _txstatus.current_rx_seq = 0; - _txstatus.payload_next_byte = 0; - _txstatus.parse_state = ParseState::IDLE; -} - -/* - Parses sport packets and if successfull fills the rxmsg mavlite struct - */ -bool AP_Frsky_MAVlite::parse(mavlite_message_t &rxmsg, const AP_Frsky_SPort::sport_packet_t packet) -{ - for (uint8_t i=0; i<6; i++) { - parse(packet.raw[i+2], i); - } - if (_rxstatus.parse_state == ParseState::MESSAGE_RECEIVED) { - rxmsg = _rxmsg; - return true; - } - return false; -} - -/* - Warning: - make sure that all packets pushed by this method are sequential and not interleaved by packets inserted by another thread! - */ -bool AP_Frsky_MAVlite::encode(ObjectBuffer_TS &queue, mavlite_message_t &msg) -{ - // let's check if there's enough room to send it - if (queue.space() < MAVLITE_MSG_SPORT_PACKETS_COUNT(msg.len)) { - return false; - } - encoder_reset(msg); - // prevent looping forever - uint8_t packet_count = 0; - while (_txstatus.parse_state != ParseState::MESSAGE_RECEIVED && packet_count++ < MAVLITE_MSG_SPORT_PACKETS_COUNT(MAVLITE_MAX_PAYLOAD_LEN)) { - - AP_Frsky_SPort::sport_packet_t packet {0}; - - for (uint8_t i=0; i<6; i++) { - // read msg and fill packet one byte at the time, ignore sensor and frame byte - encode(packet.raw[i+2], i, msg); - } - - if (_txstatus.parse_state == ParseState::ERROR) { - break; - } - - queue.push(packet); - } - _txstatus.parse_state = ParseState::IDLE; - return true; -} - -bool AP_Frsky_MAVlite::parse(uint8_t byte, uint8_t offset) -{ - switch (_rxstatus.parse_state) { - case ParseState::IDLE: - case ParseState::ERROR: - if ( offset == 0 && byte == 0x00 ) { - parser_reset(); - _rxstatus.parse_state = ParseState::GOT_START; - } else { - _rxstatus.parse_state = ParseState::ERROR; - } - break; - case ParseState::GOT_START: - if ( offset == 0 && byte == 0x00 ) { - parser_reset(); - _rxstatus.parse_state = ParseState::GOT_START; - } else { - _rxmsg.len = byte; - _rxstatus.parse_state = ParseState::GOT_LEN; - update_checksum(_rxmsg, byte); - } - break; - case ParseState::GOT_LEN: - if ( offset == 0 && byte == 0x00 ) { - parser_reset(); - _rxstatus.parse_state = ParseState::GOT_START; - } else { - _rxmsg.msgid = byte; - _rxstatus.parse_state = ParseState::GOT_MSGID; - update_checksum(_rxmsg, byte); - } - break; - case ParseState::GOT_MSGID: - if ( offset == 0 && byte == 0x00 ) { - parser_reset(); - _rxstatus.parse_state = ParseState::GOT_START; - } else { - _rxmsg.payload[_rxstatus.payload_next_byte++] = byte; - _rxstatus.parse_state = ParseState::GOT_PAYLOAD; - update_checksum(_rxmsg, byte); - } - break; - case ParseState::GOT_SEQ: - if ( offset == 0 && byte == 0x00 ) { - parser_reset(); - _rxstatus.parse_state = ParseState::GOT_START; - } else { - if ( _rxstatus.payload_next_byte < _rxmsg.len ) { - _rxmsg.payload[_rxstatus.payload_next_byte++] = byte; - _rxstatus.parse_state = ParseState::GOT_PAYLOAD; - update_checksum(_rxmsg, byte); - } else { - if ( _rxmsg.checksum == byte ) { - _rxstatus.parse_state = ParseState::MESSAGE_RECEIVED; - return true; - } else { - _rxstatus.parse_state = ParseState::ERROR; - } - } - } - break; - case ParseState::GOT_PAYLOAD: - if ( offset == 0) { - if ( byte == 0x00 ) { - parser_reset(); - _rxstatus.parse_state = ParseState::GOT_START; - } else { - if ((byte & 0x3F) != _rxstatus.current_rx_seq + 1) { - _rxstatus.parse_state = ParseState::ERROR; - } else { - _rxstatus.current_rx_seq = (byte & 0x3F); - _rxstatus.parse_state = ParseState::GOT_SEQ; - } - update_checksum(_rxmsg, byte); - } - } else { - if ( _rxstatus.payload_next_byte < _rxmsg.len ) { - _rxmsg.payload[_rxstatus.payload_next_byte++] = byte; - update_checksum(_rxmsg, byte); - } else { - if ( _rxmsg.checksum == byte ) { - _rxstatus.parse_state = ParseState::MESSAGE_RECEIVED; - return true; - } else { - _rxstatus.parse_state = ParseState::ERROR; - } - } - } - break; - case ParseState::MESSAGE_RECEIVED: - if ( offset == 0 && byte == 0x00 ) { - parser_reset(); - _rxstatus.parse_state = ParseState::GOT_START; - } - break; - } - return false; -} - -bool AP_Frsky_MAVlite::encode(uint8_t &byte, const uint8_t offset, mavlite_message_t &txmsg) -{ - switch (_txstatus.parse_state) { - case ParseState::IDLE: - case ParseState::ERROR: - if ( offset == 0 ) { - byte = 0x00; - encoder_reset(txmsg); - _txstatus.parse_state = ParseState::GOT_START; - } else { - _txstatus.parse_state = ParseState::ERROR; - } - break; - case ParseState::GOT_START: - byte = txmsg.len; - _txstatus.parse_state = ParseState::GOT_LEN; - update_checksum(txmsg, byte); - break; - case ParseState::GOT_LEN: - byte = txmsg.msgid; - _txstatus.parse_state = ParseState::GOT_MSGID; - update_checksum(txmsg, byte); - break; - case ParseState::GOT_MSGID: - byte = txmsg.payload[_txstatus.payload_next_byte++]; - _txstatus.parse_state = ParseState::GOT_PAYLOAD; - update_checksum(txmsg, byte); - break; - case ParseState::GOT_SEQ: - if ( _txstatus.payload_next_byte < txmsg.len ) { - byte = txmsg.payload[_txstatus.payload_next_byte++]; - _txstatus.parse_state = ParseState::GOT_PAYLOAD; - update_checksum(txmsg, byte); - } else { - byte = txmsg.checksum; - _txstatus.parse_state = ParseState::MESSAGE_RECEIVED; - return true; - } - break; - case ParseState::GOT_PAYLOAD: - if ( offset == 0) { - byte = ++_txstatus.current_rx_seq; - update_checksum(txmsg, byte); - _txstatus.parse_state = ParseState::GOT_SEQ; - } else { - if ( _txstatus.payload_next_byte < txmsg.len ) { - byte = txmsg.payload[_txstatus.payload_next_byte++]; - update_checksum(txmsg, byte); - } else { - byte = (uint8_t)txmsg.checksum; - _txstatus.parse_state = ParseState::MESSAGE_RECEIVED; - return true; - } - } - break; - case ParseState::MESSAGE_RECEIVED: - break; - } - return false; -} - -bool AP_Frsky_MAVlite::mavlite_msg_get_bytes(uint8_t *bytes, const mavlite_message_t &msg, const uint8_t offset, const uint8_t count) -{ - if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) { - return false; - } - memcpy(bytes, &msg.payload[offset], count); - return true; -} - -bool AP_Frsky_MAVlite::mavlite_msg_set_bytes(mavlite_message_t &msg, const uint8_t *bytes, const uint8_t offset, const uint8_t count) -{ - if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) { - return false; - } - memcpy(&msg.payload[offset], bytes, count); - msg.len += count; - return true; -} - - -bool AP_Frsky_MAVlite::mavlite_msg_get_float(float &value, const mavlite_message_t &msg, const uint8_t offset) -{ - return mavlite_msg_get_bytes((uint8_t*)&value, msg, offset, 4); -} - -bool AP_Frsky_MAVlite::mavlite_msg_get_uint16(uint16_t &value, const mavlite_message_t &msg, const uint8_t offset) -{ - return mavlite_msg_get_bytes((uint8_t*)&value, msg, offset, 2); -} - -bool AP_Frsky_MAVlite::mavlite_msg_get_uint8(uint8_t &value, const mavlite_message_t &msg, const uint8_t offset) -{ - return mavlite_msg_get_bytes((uint8_t*)&value, msg, offset, 1); -} - -uint8_t AP_Frsky_MAVlite::bit8_unpack(const uint8_t value, const uint8_t bit_count, const uint8_t bit_offset) -{ - uint8_t mask = 0; - for (uint8_t i=bit_offset; i<=bit_count; i++) { - mask |= 1 << i; - } - return (value & mask) >> bit_offset; -} - -void AP_Frsky_MAVlite::bit8_pack(uint8_t &value, const uint8_t bit_value, const uint8_t bit_count, const uint8_t bit_offset) -{ - uint8_t mask = 0; - for (uint8_t i=bit_offset; i<=bit_count; i++) { - mask |= 1 << i; - } - value |= (bit_value< +/* -#include "AP_Frsky_SPort.h" + Wire Protocol: + + Several SPort packets make up a MAVlite message. + + A maximum of six relevant data bytes are present in an SPort packet. + + Each SPort packet starts with a sequence number, starting with zero. + + If the sequence number is zero then the parser is reset. + + The first sport packet contains len at offset 1. It is the + *payload* length - does not include checksum, for example. msgid is + at offset 2, then payload bytes. + + Subsequent SPort packets contain a sequence number at offset zero, + followed by more payload bytes. + + When sufficient payload bytes have been received (based on "len"), a + single checksum byte arrives. Sometimes this checksum byte goes + into an SPort packet all on its own, sharing space only with the + sequence number. + +*/ -#include #define MAVLITE_MAX_PAYLOAD_LEN 31 // 7 float params + cmd_id + options #define MAVLITE_MSG_SPORT_PACKETS_COUNT(LEN) static_cast(1 + ceilf((LEN-2)/5.0f)) // number of sport packets required to transport a message with LEN payload #define SPORT_PACKET_QUEUE_LENGTH static_cast(30U*MAVLITE_MSG_SPORT_PACKETS_COUNT(MAVLITE_MAX_PAYLOAD_LEN)) - -class AP_Frsky_MAVlite -{ -public: - typedef struct { - uint8_t msgid = 0; // ID of message in payload - uint8_t len = 0; // Length of payload - uint8_t payload[MAVLITE_MAX_PAYLOAD_LEN]; - int16_t checksum = 0; // sent at end of packet - } mavlite_message_t; - - // public parsing methods - bool parse(mavlite_message_t &rxmsg, const AP_Frsky_SPort::sport_packet_t packet) ; - bool encode(ObjectBuffer_TS &queue, mavlite_message_t &msg); - - // public mavlite message helpers - static bool mavlite_msg_get_float(float &value, const mavlite_message_t &msg, const uint8_t offset); - static bool mavlite_msg_set_float(mavlite_message_t &msg, const float value, const uint8_t offset); - - static bool mavlite_msg_get_string(char* value, const mavlite_message_t &msg, const uint8_t offset); - static bool mavlite_msg_set_string(mavlite_message_t &msg, const char* value, const uint8_t offset); - - static bool mavlite_msg_get_uint16(uint16_t &value, const mavlite_message_t &msg, const uint8_t offset); - static bool mavlite_msg_set_uint16(mavlite_message_t &msg, const uint16_t value, const uint8_t offset); - - static bool mavlite_msg_get_uint8(uint8_t &value, const mavlite_message_t &msg, const uint8_t offset); - static bool mavlite_msg_set_uint8(mavlite_message_t &msg, const uint8_t value, const uint8_t offset); - - static void bit8_pack(uint8_t &value, const uint8_t bit_value, const uint8_t bit_count, const uint8_t bit_offset); - static uint8_t bit8_unpack(const uint8_t value, const uint8_t bit_count, const uint8_t bit_offset); - -private: - enum class ParseState : uint8_t { - IDLE=0, - ERROR, - GOT_START, - GOT_LEN, - GOT_SEQ, - GOT_MSGID, - GOT_PAYLOAD, - MESSAGE_RECEIVED, - }; // state machine for mavlite messages - - typedef struct { - ParseState parse_state = ParseState::IDLE; - uint8_t current_rx_seq = 0; - uint8_t payload_next_byte = 0; - } mavlite_status_t; - - mavlite_status_t _txstatus; - mavlite_status_t _rxstatus; - - mavlite_message_t _rxmsg; - - static bool mavlite_msg_get_bytes(uint8_t *bytes, const mavlite_message_t &msg, const uint8_t offset, const uint8_t count); - static bool mavlite_msg_set_bytes(mavlite_message_t &msg, const uint8_t *bytes, const uint8_t offset, const uint8_t count); - - void encoder_reset(mavlite_message_t &txmsg); - void parser_reset(); - bool parse(const uint8_t byte, const uint8_t offset); - bool encode(uint8_t &byte, uint8_t offset, mavlite_message_t &txmsg); - void update_checksum(mavlite_message_t &msg, const uint8_t c); -}; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.cpp new file mode 100644 index 0000000000..32c820d1a9 --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.cpp @@ -0,0 +1,98 @@ +#include "AP_Frsky_MAVlite_MAVliteToSPort.h" + +#include "AP_Frsky_MAVlite.h" + +#include "AP_Frsky_SPort.h" + +void AP_Frsky_MAVlite_MAVliteToSPort::reset() +{ + checksum = 0; + + packet_offs = 2; // someone else sets frame and sensorid + state = State::WANT_LEN; + next_seq = 0; + + payload_count = 0; +} + +void AP_Frsky_MAVlite_MAVliteToSPort::update_checksum(const uint8_t c) +{ + checksum += c; //0-1FF + checksum += checksum >> 8; + checksum &= 0xFF; +} + +/* + Warning: + make sure that all packets pushed by this method are sequential and not interleaved by packets inserted by another thread! + */ +bool AP_Frsky_MAVlite_MAVliteToSPort::process(ObjectBuffer_TS &queue, const AP_Frsky_MAVlite_Message &msg) +{ + // let's check if there's enough room to send it + if (queue.space() < MAVLITE_MSG_SPORT_PACKETS_COUNT(msg.len)) { + return false; + } + reset(); + + process_byte(msg.len, queue); + process_byte(msg.msgid, queue); + + for (uint8_t i=0; i &queue) +{ + if (packet_offs == 2) { + // start of a packet (since we skip setting sensorid and + // frame). Emit a sequence number. + packet.raw[packet_offs++] = next_seq; + update_checksum(next_seq); + next_seq += 1; + } + switch (state) { + case State::WANT_LEN: + packet.raw[packet_offs++] = b; + update_checksum(b); + payload_len = b; + state = State::WANT_MSGID; + break; + case State::WANT_MSGID: + packet.raw[packet_offs++] = b; + update_checksum(b); + if (b == 0) { + state = State::WANT_CHECKSUM; + } else { + state = State::WANT_PAYLOAD; + } + break; + case State::WANT_PAYLOAD: + packet.raw[packet_offs++] = b; + update_checksum(b); + payload_count++; + if (payload_count >= payload_len) { + state = State::WANT_CHECKSUM; + } + break; + case State::WANT_CHECKSUM: + packet.raw[packet_offs++] = checksum; + queue.push(packet); + state = State::DONE; + break; + case State::DONE: + return; + } + if (packet_offs >= ARRAY_SIZE(packet.raw)) { + // it's cooked + queue.push(packet); + packet_offs = 2; // skip setting sensorid and frame + } +} diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.h b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.h new file mode 100644 index 0000000000..0efbf254de --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.h @@ -0,0 +1,52 @@ +#pragma once + +#include "AP_Frsky_MAVlite_Message.h" +#include "AP_Frsky_SPort.h" + +#include + +#include + +/* + * An instance of this class encodes a MAVlite message into several + * SPort packets, and pushes them onto the supplied queue. + * + * process() will return false if there is insufficient room in the + * queue for all SPort packets. + * + * See AP_Frsky_MAVlite.h for a description of the encoding of a + * MAVlite message in SPort packets. + */ + +class AP_Frsky_MAVlite_MAVliteToSPort { +public: + + // insert sport packets calculated from mavlite msg into queue + bool process(ObjectBuffer_TS &queue, + const AP_Frsky_MAVlite_Message &msg) WARN_IF_UNUSED; + +private: + + enum class State : uint8_t { + WANT_LEN, + WANT_MSGID, + WANT_PAYLOAD, + WANT_CHECKSUM, + DONE, + }; + State state = State::WANT_LEN; + + void reset(); + + void process_byte(uint8_t byte, ObjectBuffer_TS &queue); + + AP_Frsky_SPort::sport_packet_t packet {}; + uint8_t packet_offs = 0; + + uint8_t next_seq = 0; + uint8_t payload_count = 0; + uint8_t payload_len; + + int16_t checksum; // sent at end of packet + void update_checksum(const uint8_t c); +}; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_Message.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_Message.cpp new file mode 100644 index 0000000000..7ada1d68e7 --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_Message.cpp @@ -0,0 +1,55 @@ +#include "AP_Frsky_MAVlite_Message.h" + +#include + +bool AP_Frsky_MAVlite_Message::get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const +{ + if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) { + return false; + } + memcpy(bytes, &payload[offset], count); + return true; +} + +bool AP_Frsky_MAVlite_Message::set_bytes(const uint8_t *bytes, const uint8_t offset, const uint8_t count) +{ + if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) { + return false; + } + memcpy(&payload[offset], bytes, count); + len += count; + return true; +} + +bool AP_Frsky_MAVlite_Message::get_string(char* value, const uint8_t offset) const +{ + if (get_bytes((uint8_t*)value, offset, MIN((uint8_t)16, len - offset))) { + value[MIN((uint8_t)16, len - offset)] = 0x00; // terminator + return true; + } + return false; +} + +bool AP_Frsky_MAVlite_Message::set_string(const char* value, const uint8_t offset) +{ + return set_bytes((uint8_t*)value, offset, MIN((uint8_t)16, strlen(value))); +} + + +uint8_t AP_Frsky_MAVlite_Message::bit8_unpack(const uint8_t value, const uint8_t bit_count, const uint8_t bit_offset) +{ + uint8_t mask = 0; + for (uint8_t i=bit_offset; i<=bit_count; i++) { + mask |= 1 << i; + } + return (value & mask) >> bit_offset; +} + +void AP_Frsky_MAVlite_Message::bit8_pack(uint8_t &value, const uint8_t bit_value, const uint8_t bit_count, const uint8_t bit_offset) +{ + uint8_t mask = 0; + for (uint8_t i=bit_offset; i<=bit_count; i++) { + mask |= 1 << i; + } + value |= (bit_value< + +#include + +class AP_Frsky_MAVlite_Message { +public: + // helpers + bool get_float(float &value, const uint8_t offset) const WARN_IF_UNUSED { + return get_bytes((uint8_t*)&value, offset, 4); + } + bool set_float(const float value, const uint8_t offset) WARN_IF_UNUSED { + return set_bytes((uint8_t*)&value, offset, 4); + } + + bool get_string(char* value, const uint8_t offset) const WARN_IF_UNUSED; + bool set_string(const char* value, const uint8_t offset) WARN_IF_UNUSED; + + bool get_uint16(uint16_t &value, const uint8_t offset) const WARN_IF_UNUSED { + return get_bytes((uint8_t*)&value, offset, 2); + } + bool set_uint16(const uint16_t value, const uint8_t offset) WARN_IF_UNUSED { + return set_bytes((uint8_t*)&value, offset, 2); + } + + bool get_uint8(uint8_t &value, const uint8_t offset) const WARN_IF_UNUSED { + return get_bytes((uint8_t*)&value, offset, 1);; + } + bool set_uint8(const uint8_t value, const uint8_t offset) WARN_IF_UNUSED { + return set_bytes((uint8_t*)&value, offset, 1); + } + + uint8_t msgid = 0; // ID of message in payload + uint8_t len = 0; // Length of payload + uint8_t payload[MAVLITE_MAX_PAYLOAD_LEN]; + + static void bit8_pack(uint8_t &value, const uint8_t bit_value, const uint8_t bit_count, const uint8_t bit_offset); + static uint8_t bit8_unpack(const uint8_t value, const uint8_t bit_count, const uint8_t bit_offset); + +private: + bool get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const WARN_IF_UNUSED; + bool set_bytes(const uint8_t *bytes, const uint8_t offset, const uint8_t count) WARN_IF_UNUSED; +}; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.cpp new file mode 100644 index 0000000000..73309e30f5 --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.cpp @@ -0,0 +1,104 @@ +#include "AP_Frsky_MAVlite_SPortToMAVlite.h" + +#include "AP_Frsky_MAVlite.h" + +void AP_Frsky_MAVlite_SPortToMAVlite::reset(void) +{ + checksum = 0; + + expected_seq = 0; + payload_next_byte = 0; + parse_state = State::WANT_LEN; +} + +void AP_Frsky_MAVlite_SPortToMAVlite::update_checksum(const uint8_t c) +{ + checksum += c; //0-1FF + checksum += checksum >> 8; + checksum &= 0xFF; +} + +/* + Parses sport packets and if successfull fills the rxmsg mavlite struct + */ +bool AP_Frsky_MAVlite_SPortToMAVlite::process(AP_Frsky_MAVlite_Message &rxmsg, const AP_Frsky_SPort::sport_packet_t &packet) +{ + // the two skipped bytes in packet.raw here are sensor and frame. + // appid and data are used to transport the mavlite message. + + // deal with packet sequence number: + const uint8_t received_seq = (packet.raw[2] & 0x3F); + // if the first byte of any sport passthrough packet is zero then we reset: + if (received_seq == 0) { + reset(); + } + if (received_seq != expected_seq) { + parse_state = State::ERROR; + return false; + } + update_checksum(received_seq); + expected_seq = received_seq + 1; + + // deal with the remainder (post-sequence) of the packet: + for (uint8_t i=3; i= _rxmsg.len) { + parse_state = State::WANT_CHECKSUM; + } + return; + + case State::WANT_CHECKSUM: + if (checksum != byte) { + parse_state = State::ERROR; + return; + } + parse_state = State::MESSAGE_RECEIVED; + return; + + case State::MESSAGE_RECEIVED: + return; + } +} diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.h b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.h new file mode 100644 index 0000000000..9a2e981172 --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.h @@ -0,0 +1,48 @@ +#pragma once + +#include "AP_Frsky_MAVlite_Message.h" +#include "AP_Frsky_SPort.h" + +#include + +/* + * An instance of this class decodes a stream of SPort packets into a + * MAVlite message (see AP_Frsky_MAVlite_Message.h). It is expected + * that the same rxmsg is passed into process() multiple times, each + * time with a new sport packet. If a packet is successfully decodes + * then process() will return true and rxmsg can be used as a MAVlite + * message. + * + * See AP_Frsky_MAVlite.h for a description of the encoding of a + * MAVlite message in SPort packets. + */ +class AP_Frsky_MAVlite_SPortToMAVlite { +public: + + bool process(AP_Frsky_MAVlite_Message &rxmsg, + const AP_Frsky_SPort::sport_packet_t &packet) WARN_IF_UNUSED; + +private: + + void reset(); + + uint8_t expected_seq; + uint8_t payload_next_byte; + + enum class State : uint8_t { + IDLE=0, + ERROR, + WANT_LEN, + WANT_MSGID, + WANT_PAYLOAD, + WANT_CHECKSUM, + MESSAGE_RECEIVED, + }; + State parse_state = State::IDLE; + + AP_Frsky_MAVlite_Message _rxmsg; + void parse(const uint8_t byte); + + int16_t checksum; // sent at end of packet + void update_checksum(const uint8_t c); +}; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h index 8c8ef26270..9a24ef5e72 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h @@ -33,6 +33,8 @@ protected: uint32_t calc_gps_latlng(bool &send_latitude); + static uint8_t calc_sensor_id(const uint8_t physical_id); + private: struct { diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index f0659e84a9..6fde8c035a 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -674,9 +674,9 @@ void AP_Frsky_SPort_Passthrough::process_rx_queue() AP_Frsky_SPort::sport_packet_t packet; uint8_t loop_count = 0; // prevent looping forever while (_SPort_bidir.rx_packet_queue.pop(packet) && loop_count++ < MAVLITE_MSG_SPORT_PACKETS_COUNT(MAVLITE_MAX_PAYLOAD_LEN)) { - AP_Frsky_MAVlite::mavlite_message_t rxmsg; + AP_Frsky_MAVlite_Message rxmsg; - if (_mavlite_handler.parse(rxmsg, packet)) { + if (sport_to_mavlite.process(rxmsg, packet)) { mavlite_process_message(rxmsg); break; // process only 1 mavlite message each call } @@ -711,19 +711,26 @@ void AP_Frsky_SPort_Passthrough::process_tx_queue() * Handle the COMMAND_LONG mavlite message * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -void AP_Frsky_SPort_Passthrough::mavlite_handle_command_long(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg) +void AP_Frsky_SPort_Passthrough::mavlite_handle_command_long(const AP_Frsky_MAVlite_Message &rxmsg) { mavlink_command_long_t mav_command_long {}; uint8_t cmd_options; float params[7] {}; - AP_Frsky_MAVlite::mavlite_msg_get_uint16(mav_command_long.command, rxmsg, 0); - AP_Frsky_MAVlite::mavlite_msg_get_uint8(cmd_options, rxmsg, 2); - uint8_t param_count = AP_Frsky_MAVlite::bit8_unpack(cmd_options, 3, 0); // first 3 bits + if (!rxmsg.get_uint16(mav_command_long.command, 0)) { + return; + } + if (!rxmsg.get_uint8(cmd_options, 2)) { + return; + } + uint8_t param_count = AP_Frsky_MAVlite_Message::bit8_unpack(cmd_options, 3, 0); // first 3 bits for (uint8_t cmd_idx=0; cmd_idx