Line data    Source code

       1             : /*
       2             :  * This file is free software: you can redistribute it and/or modify it
       3             :  * under the terms of the GNU General Public License as published by the
       4             :  * Free Software Foundation, either version 3 of the License, or
       5             :  * (at your option) any later version.
       6             :  *
       7             :  * This file is distributed in the hope that it will be useful, but
       8             :  * WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
      10             :  * See the GNU General Public License for more details.
      11             :  *
      12             :  * You should have received a copy of the GNU General Public License along
      13             :  * with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : #pragma once
      16             : 
      17             : #include "AP_RCProtocol_config.h"
      18             : 
      19             : #if AP_RCPROTOCOL_GHST_ENABLED
      20             : 
      21             : #include "AP_RCProtocol.h"
      22             : #include <AP_Math/AP_Math.h>
      23             : #include <RC_Channel/RC_Channel.h>
      24             : #include "SoftSerial.h"
      25             : 
      26             : #define GHST_MAX_CHANNELS   16U      // Maximum number of channels from GHST datastream
      27             : #define GHST_FRAMELEN_MAX   14U      // maximum possible framelength
      28             : #define GHST_HEADER_LEN     2U       // header length
      29             : #define GHST_FRAME_PAYLOAD_MAX (GHST_FRAMELEN_MAX - GHST_HEADER_LEN)     // maximum size of the frame length field in a packet
      30             : #define GHST_BAUDRATE      420000U
      31             : #define GHST_TX_TIMEOUT    500000U   // the period after which the transmitter is considered disconnected (matches copters failsafe)
      32             : #define GHST_RX_TIMEOUT    150000U   // the period after which the receiver is considered disconnected (>ping frequency)
      33             : 
      34             : class AP_RCProtocol_GHST : public AP_RCProtocol_Backend {
      35             : public:
      36             :     AP_RCProtocol_GHST(AP_RCProtocol &_frontend);
      37             :     virtual ~AP_RCProtocol_GHST();
      38             :     void process_byte(uint8_t byte, uint32_t baudrate) override;
      39             :     void process_handshake(uint32_t baudrate) override;
      40             :     void update(void) override;
      41             : 
      42             :     // is the receiver active, used to detect power loss and baudrate changes
      43           0 :     bool is_rx_active() const override {
      44           0 :         return AP_HAL::micros() < _last_rx_frame_time_us + GHST_RX_TIMEOUT;
      45             :     }
      46             : 
      47             :     // is the transmitter active, used to adjust telemetry data
      48           0 :     bool is_tx_active() const {
      49             :         // this is the same as the Copter failsafe timeout
      50           0 :         return AP_HAL::micros() < _last_tx_frame_time_us + GHST_TX_TIMEOUT;
      51             :     }
      52             : 
      53             :     // get singleton instance
      54           0 :     static AP_RCProtocol_GHST* get_singleton() {
      55           0 :         return _singleton;
      56             :     }
      57             : 
      58             :     enum FrameType {
      59             :         GHST_UL_RC_CHANS_HS4_5TO8 = 0x10, // Control packet with 4 primary channels + CH5-8
      60             :         GHST_UL_RC_CHANS_HS4_9TO12 = 0x11, // Control packet with 4 primary channels + CH9-12
      61             :         GHST_UL_RC_CHANS_HS4_13TO16 = 0x12, // Control packet with 4 primary channels + CH13-16
      62             :         GHST_UL_RC_CHANS_RSSI = 0x13, // Control packet with RSSI and LQ data
      63             :         GHST_UL_RC_VTX_CTRL = 0x14, // Goggle/FC channel changing
      64             :         // -> 0x1F reserved
      65             :         GHST_UL_VTX_SETUP = 0x20, // vTx Setup w/o 4 primary channels (GECO Only)
      66             :         GHST_UL_MSP_REQ = 0x21, // MSP frame, Request
      67             :         GHST_UL_MSP_WRITE = 0x22, // MSP frame, Write
      68             : 
      69             :         GHST_DL_PACK_STAT = 0x23, // Battery Status
      70             :         GHST_DL_GPS_PRIMARY = 0x25, // Primary GPS Data
      71             :         GHST_DL_GPS_SECONDARY = 0x26, // Secondary GPS Data
      72             :         GHST_DL_MAGBARO = 0x27, // Magnetometer, Barometer (and Vario) Data
      73             :         GHST_DL_MSP_RESP = 0x28, // MSP Response
      74             : 
      75             :         GHST_UL_RC_CHANS_HS4_12_5TO8 = 0x30, // Control packet with 4 primary channels + CH5-8
      76             :         GHST_UL_RC_CHANS_HS4_12_9TO12 = 0x31, // Control packet with 4 primary channels + CH9-12
      77             :         GHST_UL_RC_CHANS_HS4_12_13TO16 = 0x32, // Control packet with 4 primary channels + CH13-16
      78             :         GHST_UL_RC_CHANS_12_RSSI = 0x33, // Control packet with RSSI and LQ data
      79             :         // 0x30 -> 0x3f - raw 12 bit packets
      80             :     };
      81             : 
      82             :     enum DeviceAddress {
      83             :         GHST_ADDRESS_FLIGHT_CONTROLLER = 0x82,
      84             :         GHST_ADDRESS_GOGGLES = 0x83,
      85             :         GHST_ADDRESS_GHST_RECEIVER = 0x89,
      86             :     };
      87             : 
      88             :     struct Frame {
      89             :         uint8_t device_address;
      90             :         uint8_t length;
      91             :         uint8_t type;
      92             :         uint8_t payload[GHST_FRAME_PAYLOAD_MAX - 1]; // type is already accounted for
      93             :     } PACKED;
      94             : 
      95             :     struct Channels12Bit_4Chan {
      96             : #if __BYTE_ORDER != __LITTLE_ENDIAN
      97             : #error "Only supported on little-endian architectures"
      98             : #endif
      99             :         uint32_t ch0 : 12;
     100             :         uint32_t ch1 : 12;
     101             :         uint32_t ch2 : 12;
     102             :         uint32_t ch3 : 12;
     103             :     } PACKED;
     104             : 
     105             :     struct RadioFrame {
     106             : #if __BYTE_ORDER != __LITTLE_ENDIAN
     107             : #error "Only supported on little-endian architectures"
     108             : #endif
     109             :         Channels12Bit_4Chan channels;   // high-res channels
     110             :         uint8_t lowres_channels[4];     // low-res channels
     111             :     } PACKED;
     112             : 
     113             :     struct LinkStatisticsFrame {
     114             : #if __BYTE_ORDER != __LITTLE_ENDIAN
     115             : #error "Only supported on little-endian architectures"
     116             : #endif
     117             :         Channels12Bit_4Chan channels;
     118             :         uint8_t link_quality; // ( 0 - 100)
     119             :         uint8_t rssi_dbm; // ( dBm * -1 )
     120             :         uint8_t protocol : 5;
     121             :         uint8_t telemetry : 1;
     122             :         uint8_t alt_scale : 1;
     123             :         uint8_t reserved : 1;
     124             :         int8_t tx_power;
     125             :     } PACKED;
     126             : 
     127             :     enum RFMode {
     128             :         GHST_RF_MODE_NORMAL = 5,    // 55Hz
     129             :         GHST_RF_MODE_RACE = 6,      // 160Hz
     130             :         GHST_RF_MODE_PURERACE = 7,  // 250Hz
     131             :         GHST_RF_MODE_LR = 8,        // 19Hz
     132             :         GHST_RF_MODE_RACE250 = 10,  // 250Hz
     133             :         GHST_RF_MODE_RACE500 = 11,  // 500Hz
     134             :         GHTS_RF_MODE_SOLID150 = 12, // 150Hz
     135             :         GHST_RF_MODE_SOLID250 = 13, // 250Hz
     136             :         RF_MODE_MAX_MODES,
     137             :         RF_MODE_UNKNOWN,
     138             :     };
     139             : 
     140             :     struct LinkStatus {
     141             :         int16_t rssi = -1;
     142             :         int16_t link_quality = -1;
     143             :         uint8_t rf_mode;
     144             :     };
     145             : 
     146             :     bool is_telemetry_supported() const;
     147             : 
     148             :     // this will be used by AP_GHST_Telem to access link status data
     149             :     // from within AP_RCProtocol_GHST thread so no need for cross-thread synch
     150           0 :     const volatile LinkStatus& get_link_status() const {
     151           0 :         return _link_status;
     152             :     }
     153             : 
     154             :     // return the link rate as defined by the LinkStatistics
     155             :     uint16_t get_link_rate()  const;
     156             : 
     157             :     // return the protocol string
     158             :     const char* get_protocol_string() const;
     159             : 
     160             : private:
     161             :     struct Frame _frame;
     162             :     struct Frame _telemetry_frame;
     163             :     uint8_t _frame_ofs;
     164             :     uint8_t _frame_crc;
     165             : 
     166        2094 :     const uint8_t MAX_CHANNELS = MIN((uint8_t)GHST_MAX_CHANNELS, (uint8_t)MAX_RCIN_CHANNELS);
     167             : 
     168             :     static AP_RCProtocol_GHST* _singleton;
     169             : 
     170             :     void _process_byte(uint32_t timestamp_us, uint8_t byte);
     171             :     bool decode_ghost_packet();
     172             :     bool process_telemetry(bool check_constraint = true);
     173             :     void process_link_stats_frame(const void* data);
     174             :     bool write_frame(Frame* frame);
     175           4 :     AP_HAL::UARTDriver* get_current_UART() { return get_available_UART(); }
     176             : 
     177             :     uint16_t _channels[GHST_MAX_CHANNELS];    /* buffer for extracted RC channel data as pulsewidth in microseconds */
     178             : 
     179           0 :     void add_to_buffer(uint8_t index, uint8_t b) { ((uint8_t*)&_frame)[index] = b; }
     180             : 
     181             :     uint32_t _last_frame_time_us;
     182             :     uint32_t _last_tx_frame_time_us;
     183             :     uint32_t _last_rx_frame_time_us;
     184             :     uint32_t _start_frame_time_us;
     185             :     bool telem_available;
     186             :     bool _use_lq_for_rssi;
     187             :     int16_t derive_scaled_lq_value(uint8_t uplink_lq);
     188             : 
     189             :     volatile struct LinkStatus _link_status;
     190             : 
     191             :     static const uint16_t RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES];
     192             : };
     193             : 
     194             : namespace AP {
     195             :     AP_RCProtocol_GHST* ghost();
     196             : };
     197             : 
     198             : #endif  // AP_RCPROTOCOL_GHST_ENABLED