libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.cpp

   1 #include "AP_Frsky_MAVlite_SPortToMAVlite.h"
   2
   3 #include "AP_Frsky_MAVlite.h"
   4
   5 #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
   6 void AP_Frsky_MAVlite_SPortToMAVlite::reset(void)
   7 {
   8     checksum = 0;
   9
  10     expected_seq = 0;
  11     payload_next_byte = 0;
  12     parse_state = State::WANT_LEN;
  13 }
  14
  15 void AP_Frsky_MAVlite_SPortToMAVlite::update_checksum(const uint8_t c)
  16 {
  17     checksum += c; //0-1FF
  18     checksum += checksum >> 8;
  19     checksum &= 0xFF;
  20 }
  21
  22 /*
  23  Parses sport packets and if successful fills the rxmsg mavlite struct
  24  */
  25 bool AP_Frsky_MAVlite_SPortToMAVlite::process(AP_Frsky_MAVlite_Message &rxmsg, const AP_Frsky_SPort::sport_packet_t &packet)
  26 {
  27     // the two skipped bytes in packet.raw here are sensor and frame.
  28     // appid and data are used to transport the mavlite message.
  29
  30     // deal with packet sequence number:
  31     const uint8_t received_seq = (packet.raw[2] & 0x3F);
  32     // if the first byte of any sport passthrough packet is zero then we reset:
  33     if (received_seq == 0) {
  34         reset();
  35     }
  36     if (received_seq != expected_seq) {
  37         parse_state = State::ERROR;
  38         return false;
  39     }
  40     update_checksum(received_seq);
  41     expected_seq = received_seq + 1;
  42
  43     // deal with the remainder (post-sequence) of the packet:
  44     for (uint8_t i=3; i<ARRAY_SIZE(packet.raw); i++) {
  45         parse(packet.raw[i]);
  46     }
  47     if (parse_state == State::MESSAGE_RECEIVED) {
  48         rxmsg = _rxmsg;
  49         return true;
  50     }
  51     return false;
  52 }
  53
  54 void AP_Frsky_MAVlite_SPortToMAVlite::parse(uint8_t byte)
  55 {
  56     switch (parse_state) {
  57
  58     case State::IDLE:
  59         // it is an error to receive anything but offset==0 byte=0xx0
  60         // in this state
  61         parse_state = State::ERROR;
  62         return;
  63
  64     case State::ERROR:
  65         // waiting for offset==0 && byte==0x00 to bump us into WANT_LEN
  66         return;
  67
  68     case State::WANT_LEN:
  69         _rxmsg.len = byte;
  70         update_checksum(byte);
  71         parse_state = State::WANT_MSGID;
  72         return;
  73
  74     case State::WANT_MSGID:
  75         _rxmsg.msgid = byte;
  76         update_checksum(byte);
  77         if (_rxmsg.len == 0) {
  78             parse_state = State::WANT_CHECKSUM;
  79         } else {
  80             parse_state = State::WANT_PAYLOAD;
  81         }
  82         return;
  83
  84     case State::WANT_PAYLOAD:
  85         // add byte to payload
  86         _rxmsg.payload[payload_next_byte++] = byte;
  87         update_checksum(byte);
  88
  89         if (payload_next_byte >= _rxmsg.len) {
  90             parse_state = State::WANT_CHECKSUM;
  91         }
  92         return;
  93
  94     case State::WANT_CHECKSUM:
  95         if (checksum != byte) {
  96             parse_state = State::ERROR;
  97             return;
  98         }
  99         parse_state = State::MESSAGE_RECEIVED;
 100         return;
 101
 102     case State::MESSAGE_RECEIVED:
 103         return;
 104     }
 105 }
 106 #endif