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             : #pragma once
      16             : 
      17             : #include "AP_RCTelemetry_config.h"
      18             : 
      19             : #if AP_GHST_TELEM_ENABLED
      20             : 
      21             : #include <AP_RCProtocol/AP_RCProtocol_GHST.h>
      22             : #include "AP_RCTelemetry.h"
      23             : #include <AP_HAL/utility/sparse-endian.h>
      24             : 
      25             : class AP_GHST_Telem : public AP_RCTelemetry {
      26             : public:
      27             :     AP_GHST_Telem();
      28             :     ~AP_GHST_Telem() override;
      29             : 
      30             :     /* Do not allow copies */
      31             :     CLASS_NO_COPY(AP_GHST_Telem);
      32             : 
      33             :     // init - perform required initialisation
      34             :     virtual bool init() override;
      35             : 
      36             :     static AP_GHST_Telem *get_singleton(void);
      37             : 
      38             :     // Broadcast frame definitions courtesy of TBS
      39             :     struct PACKED GPSFrame {
      40             :         uint32_t latitude; // ( degree * 1e7 )
      41             :         uint32_t longitude; // (degree * 1e7 )
      42             :         int16_t altitude; // ( meter )
      43             :     };
      44             : 
      45             :     struct PACKED GPSSecondaryFrame {
      46             :         uint16_t groundspeed; // ( cm/s )
      47             :         uint16_t gps_heading; // ( degree * 10 )
      48             :         uint8_t satellites; // in use ( counter )
      49             :         uint16_t home_dist; // ( m / 10 )
      50             :         uint16_t home_heading; // ( degree * 10 )
      51             :         uint8_t flags; // GPS_FLAGS_FIX 0x01, GPS_FLAGS_FIX_HOME 0x2
      52             :     };
      53             : 
      54             :     struct PACKED BatteryFrame {
      55             :         uint16_t voltage; // ( mV / 10 )
      56             :         uint16_t current; // ( mA / 10 )
      57             :         uint16_t consumed; // ( mAh / 10 )
      58             :         uint8_t rx_voltage; // ( mV / 100 )
      59             :         uint8_t spare[3];
      60             :     };
      61             : 
      62             :     struct PACKED VTXFrame {
      63             : #if __BYTE_ORDER != __LITTLE_ENDIAN
      64             : #error "Only supported on little-endian architectures"
      65             : #endif
      66             :         uint8_t flags;
      67             :         uint16_t frequency;         // frequency in Mhz
      68             :         uint16_t power;              // power in mw, 0 == off
      69             :         uint8_t band : 4;               // A, B, E, AirWave, Race
      70             :         uint8_t channel : 4;            // 1x-8x
      71             :         uint8_t spare[3];
      72             :     };
      73             : 
      74             :     struct PACKED SensorFrame {
      75             :         uint16_t compass_heading; // ( deg * 10 )
      76             :         int16_t baro_alt; // ( m )
      77             :         int16_t vario; // ( m/s * 100 )
      78             :         uint8_t spare[3];
      79             :         uint8_t flags; // MISC_FLAGS_MAGHEAD 0x01, MISC_FLAGS_BAROALT 0x02, MISC_FLAGS_VARIO 0x04
      80             :     };
      81             : 
      82             :     union PACKED TelemetryPayload {
      83             :         GPSFrame gps;
      84             :         GPSSecondaryFrame gps2;
      85             :         BatteryFrame battery;
      86             :         VTXFrame vtx;
      87             :         SensorFrame sensor;
      88             :     };
      89             : 
      90             :     // get the protocol string
      91           0 :     const char* get_protocol_string() const { return AP::ghost()->get_protocol_string(); }
      92             : 
      93             :     // Process a frame from the CRSF protocol decoder
      94             :     static bool process_frame(AP_RCProtocol_GHST::FrameType frame_type, void* data);
      95             :     // process any changed settings and schedule for transmission
      96             :     void update();
      97             :     // get next telemetry data for external consumers of SPort data
      98             :     static bool get_telem_data(AP_RCProtocol_GHST::Frame* frame, bool is_tx_active);
      99             : 
     100             : private:
     101             : 
     102             :     enum SensorType {
     103             :         ATTITUDE,
     104             :         VTX_PARAMETERS,
     105             :         BATTERY,
     106             :         GPS,
     107             :         GPS2,
     108             :         NUM_SENSORS
     109             :     };
     110             : 
     111             :     // passthrough WFQ scheduler
     112             :     bool is_packet_ready(uint8_t idx, bool queue_empty) override;
     113             :     void process_packet(uint8_t idx) override;
     114             :     void setup_custom_telemetry();
     115             :     void update_custom_telemetry_rates(const AP_RCProtocol_GHST::RFMode rf_mode);
     116             : 
     117             :     void calc_battery();
     118             :     void calc_gps();
     119             :     void calc_gps2();
     120             :     void calc_attitude();
     121             :     void process_pending_requests();
     122             :     bool process_rf_mode_changes();
     123             :     AP_RCProtocol_GHST::RFMode get_rf_mode() const;
     124             :     uint16_t get_telemetry_rate() const;
     125             :     bool is_high_speed_telemetry(const AP_RCProtocol_GHST::RFMode rf_mode) const;
     126             : 
     127             :     // setup ready for passthrough operation
     128             :     void setup_wfq_scheduler(void) override;
     129             : 
     130             :     // get next telemetry data for external consumers
     131             :     bool _get_telem_data(AP_RCProtocol_GHST::Frame* data, bool is_tx_active);
     132             :     bool _process_frame(AP_RCProtocol_GHST::FrameType frame_type, void* data);
     133             : 
     134             :     TelemetryPayload _telem;
     135             :     uint8_t _telem_size;
     136             :     uint8_t _telem_type;
     137             : 
     138             :     AP_RCProtocol_GHST::RFMode _rf_mode;
     139             :     bool _enable_telemetry;
     140             : 
     141             :     // reporting telemetry rate
     142             :     uint32_t _telem_last_report_ms;
     143             :     uint16_t _telem_last_avg_rate;
     144             :     // do we need to report the initial state
     145             :     bool _telem_bootstrap_msg_pending;
     146             : 
     147             :     bool _telem_is_high_speed;
     148             :     bool _telem_pending;
     149             :     // used to limit telemetry when in a failsafe condition
     150             :     bool _is_tx_active;
     151             : 
     152             :     struct {
     153             :         uint8_t destination = 0;
     154             :         uint8_t frame_type;
     155             :     } _pending_request;
     156             : 
     157             :     bool _noted_lq_as_rssi_active;
     158             : 
     159             :     static AP_GHST_Telem *singleton;
     160             : };
     161             : 
     162             : namespace AP {
     163             :     AP_GHST_Telem *ghost_telem();
     164             : };
     165             : 
     166             : #endif // AP_GHST_TELEM_ENABLED