#include "AP_Frsky_MAVlite_SPortToMAVlite.h" #include "AP_Frsky_MAVlite.h" #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL 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 successful 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<ARRAY_SIZE(packet.raw); i++) { parse(packet.raw[i]); } if (parse_state == State::MESSAGE_RECEIVED) { rxmsg = _rxmsg; return true; } return false; } void AP_Frsky_MAVlite_SPortToMAVlite::parse(uint8_t byte) { switch (parse_state) { case State::IDLE: // it is an error to receive anything but offset==0 byte=0xx0 // in this state parse_state = State::ERROR; return; case State::ERROR: // waiting for offset==0 && byte==0x00 to bump us into WANT_LEN return; case State::WANT_LEN: _rxmsg.len = byte; update_checksum(byte); parse_state = State::WANT_MSGID; return; case State::WANT_MSGID: _rxmsg.msgid = byte; update_checksum(byte); if (_rxmsg.len == 0) { parse_state = State::WANT_CHECKSUM; } else { parse_state = State::WANT_PAYLOAD; } return; case State::WANT_PAYLOAD: // add byte to payload _rxmsg.payload[payload_next_byte++] = byte; update_checksum(byte); if (payload_next_byte >= _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; } } #endif