Line data    Source code

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