Line data    Source code

       1             : #include "AP_Frsky_MAVlite_MAVliteToSPort.h"
       2             : 
       3             : #include "AP_Frsky_MAVlite.h"
       4             : 
       5             : #include "AP_Frsky_SPort.h"
       6             : 
       7             : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
       8           6 : void AP_Frsky_MAVlite_MAVliteToSPort::reset()
       9             : {
      10           6 :     checksum = 0;
      11             : 
      12           6 :     packet_offs = 2;  // someone else sets frame and sensorid
      13           6 :     state = State::WANT_LEN;
      14           6 :     next_seq = 0;
      15             : 
      16           6 :     payload_count = 0;
      17           6 : }
      18             : 
      19          80 : void AP_Frsky_MAVlite_MAVliteToSPort::update_checksum(const uint8_t c)
      20             : {
      21          80 :     checksum += c; //0-1FF
      22          80 :     checksum += checksum >> 8;
      23          80 :     checksum &= 0xFF;
      24          80 : }
      25             : 
      26             : /*
      27             :  Warning:
      28             :  make sure that all packets pushed by this method are sequential and not interleaved by packets inserted by another thread!
      29             :  */
      30           6 : bool AP_Frsky_MAVlite_MAVliteToSPort::process(ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> &queue, const AP_Frsky_MAVlite_Message &msg)
      31             : {
      32             :     // let's check if there's enough room to send it
      33           6 :     if (queue.space() < MAVLITE_MSG_SPORT_PACKETS_COUNT(msg.len)) {
      34           0 :         return false;
      35             :     }
      36           6 :     reset();
      37             : 
      38           6 :     process_byte(msg.len, queue);
      39           6 :     process_byte(msg.msgid, queue);
      40             : 
      41          56 :     for (uint8_t i=0; i<msg.len; i++) {
      42          50 :         process_byte(msg.payload[i], queue);
      43             :     }
      44             : 
      45             :     // byte isn't important in this call; checksum is used in
      46             :     // process_byte in case we need to start a new packet just to hold
      47             :     // it.
      48           6 :     process_byte(0, queue);
      49             : 
      50           6 :     return true;
      51             : }
      52             : 
      53          68 : void AP_Frsky_MAVlite_MAVliteToSPort::process_byte(const uint8_t b, ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> &queue)
      54             : {
      55          68 :     if (packet_offs == 2) {
      56             :         // start of a packet (since we skip setting sensorid and
      57             :         // frame).  Emit a sequence number.
      58          18 :         packet.raw[packet_offs++] = next_seq;
      59          18 :         update_checksum(next_seq);
      60          18 :         next_seq += 1;
      61             :     }
      62          68 :     switch (state) {
      63           6 :     case State::WANT_LEN:
      64           6 :         packet.raw[packet_offs++] = b;
      65           6 :         update_checksum(b);
      66           6 :         payload_len = b;
      67           6 :         state = State::WANT_MSGID;
      68           6 :         break;
      69           6 :     case State::WANT_MSGID:
      70           6 :         packet.raw[packet_offs++] = b;
      71           6 :         update_checksum(b);
      72           6 :         if (b == 0) {
      73           0 :             state = State::WANT_CHECKSUM;
      74             :         } else {
      75           6 :             state = State::WANT_PAYLOAD;
      76             :         }
      77           6 :         break;
      78          50 :     case State::WANT_PAYLOAD:
      79          50 :         packet.raw[packet_offs++] = b;
      80          50 :         update_checksum(b);
      81          50 :         payload_count++;
      82          50 :         if (payload_count >= payload_len) {
      83           6 :             state = State::WANT_CHECKSUM;
      84             :         }
      85          50 :         break;
      86           6 :     case State::WANT_CHECKSUM:
      87           6 :         packet.raw[packet_offs++] = checksum;
      88           6 :         queue.push(packet);
      89           6 :         state = State::DONE;
      90           6 :         break;
      91           0 :     case State::DONE:
      92           0 :         return;
      93             :     }
      94          68 :     if (packet_offs >= ARRAY_SIZE(packet.raw)) {
      95             :         // it's cooked
      96          12 :         queue.push(packet);
      97          12 :         packet_offs = 2;  // skip setting sensorid and frame
      98             :     }
      99             : }
     100             : #endif