Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   Base class for FrSky D telemetry
      17             : */
      18             : 
      19             : #include "SIM_Frsky_D.h"
      20             : 
      21             : #include <AP_HAL/AP_HAL.h>
      22             : 
      23             : extern const AP_HAL::HAL& hal;
      24             : 
      25             : using namespace SITL;
      26             : 
      27             : // sadly, this pulls START_STOP_D etc in from the frsky header.
      28             : #include <GCS_MAVLink/GCS.h>
      29             : 
      30             : static const uint8_t START_STOP_D              = 0x5E;
      31             : static const uint8_t BYTESTUFF_D               = 0x5D;
      32             : 
      33           0 : void Frsky_D::handle_data(uint8_t id, uint16_t data)
      34             : {
      35           0 :     hal.console->printf(
      36             :               "Frsky: id=%s (0x%02X) data=%u\n",
      37             :               dataid_string((DataID)id),
      38           0 :               (unsigned)_id,
      39             :               (unsigned)data);
      40           0 : }
      41             : 
      42           0 : void Frsky_D::update()
      43             : {
      44           0 :     const ssize_t n = read_from_autopilot(&_buffer[_buflen], ARRAY_SIZE(_buffer) - _buflen - 1);
      45           0 :     if (n != -1) {
      46           0 :         _buflen += n;
      47             :     }
      48             : 
      49           0 :     if (_buflen == 0) {
      50           0 :         return;
      51             :     }
      52             : 
      53           0 :     while (_buflen) {
      54           0 :         switch (_state) {
      55           0 :         case State::WANT_START_STOP_D:
      56           0 :             if (_buffer[0] != START_STOP_D) {
      57           0 :                 AP_HAL::panic("Corrupt?");
      58             :                 // _lost_bytes++;
      59             :                 continue;
      60             :             }
      61           0 :             memcpy(&_buffer[0], &_buffer[1], --_buflen); //srsly?!
      62           0 :             _state = State::WANT_ID;
      63           0 :             break;
      64           0 :         case State::WANT_ID:
      65           0 :             _id = _buffer[0];
      66           0 :             memcpy(&_buffer[0], &_buffer[1], --_buflen); //srsly?!
      67           0 :             _state = State::WANT_BYTE1;
      68           0 :             break;
      69           0 :         case State::WANT_BYTE1:
      70             :         case State::WANT_BYTE2: {
      71             :             uint8_t byte;
      72           0 :             uint8_t consume = 1;
      73           0 :             if (_buffer[0] == 0x5D) {
      74             :                 // byte-stuffed
      75           0 :                 if (_buflen < 2) {
      76           0 :                     return;
      77             :                 }
      78           0 :                 if (_buffer[1] == 0x3E) {
      79           0 :                     byte = START_STOP_D;
      80           0 :                 } else if (_buffer[1] == 0x3D) {
      81           0 :                     byte = BYTESTUFF_D;
      82             :                 } else {
      83           0 :                     AP_HAL::panic("Unknown stuffed byte");
      84             :                 }
      85           0 :                 consume = 2;
      86             :             } else {
      87           0 :                 byte = _buffer[0];
      88             :             }
      89             : 
      90           0 :             memcpy(&_buffer[0], &_buffer[consume], _buflen-consume);
      91           0 :             _buflen -= consume;
      92             : 
      93           0 :             switch (_state) {
      94           0 :             case State::WANT_ID:
      95             :             case State::WANT_START_STOP_D:
      96           0 :                 AP_HAL::panic("Should not get here");
      97           0 :             case State::WANT_BYTE1:
      98           0 :                 _data = byte;
      99           0 :                 _state = State::WANT_BYTE2;
     100           0 :                 break;
     101           0 :             case State::WANT_BYTE2:
     102           0 :                 _data |= byte << 8;
     103           0 :                 handle_data(_id, _data);
     104           0 :                 _state = State::WANT_START_STOP_D;
     105           0 :                 break;
     106             :             }
     107             :         }
     108             :         }
     109             :     }
     110             : }