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_HAL/Semaphores.h>
      18             : #include <AP_HAL/utility/RingBuffer.h>
      19             : #include <AP_Math/AP_Math.h>
      20             : #include <GCS_MAVLink/GCS_MAVLink.h>
      21             : #include <AP_Common/Location.h>
      22             : 
      23             : #define TELEM_PAYLOAD_STATUS_CAPACITY          5 // size of the message buffer queue (max number of messages waiting to be sent)
      24             : 
      25             : // for fair scheduler
      26             : #define TELEM_TIME_SLOT_MAX               20
      27             : //#define TELEM_DEBUG
      28             : 
      29             : class AP_RCTelemetry {
      30             : public:
      31        2105 :     AP_RCTelemetry(uint8_t time_slots) : _time_slots(time_slots) {}
      32           0 :     virtual ~AP_RCTelemetry() {};
      33             : 
      34             :     /* Do not allow copies */
      35             :     CLASS_NO_COPY(AP_RCTelemetry);
      36             : 
      37             :     // add statustext message to message queue
      38             :     virtual void queue_message(MAV_SEVERITY severity, const char *text);
      39             : 
      40             :     // scheduler entry helpers
      41           0 :     void enable_scheduler_entry(const uint8_t slot) {
      42           0 :         if (slot >= TELEM_TIME_SLOT_MAX) {
      43           0 :             return;
      44             :         }
      45           0 :         BIT_CLEAR(_disabled_scheduler_entries_bitmask, slot);
      46             :     }
      47           1 :     void disable_scheduler_entry(const uint8_t slot) {
      48           1 :         if (slot >= TELEM_TIME_SLOT_MAX) {
      49           0 :             return;
      50             :         }
      51           1 :         BIT_SET(_disabled_scheduler_entries_bitmask, slot);
      52             :     }
      53           0 :     void set_scheduler_entry_min_period(const uint8_t slot, const uint32_t min_period_ms)
      54             :     {
      55           0 :         if (slot >= TELEM_TIME_SLOT_MAX) {
      56           0 :             return;
      57             :         }
      58           0 :         _scheduler.packet_min_period[slot] = min_period_ms;
      59             :     }
      60       11296 :     bool is_scheduler_entry_enabled(const uint8_t slot) const {
      61       11296 :         if (slot >= TELEM_TIME_SLOT_MAX) {
      62           0 :             return false;
      63             :         }
      64       11296 :         return !BIT_IS_SET(_disabled_scheduler_entries_bitmask, slot);
      65             :     }
      66             :     // each derived class might provide a way to reset telemetry rates to default
      67           0 :     virtual void reset_scheduler_entry_min_periods() {}
      68             : 
      69             :     // update error mask of sensors and subsystems. The mask uses the
      70             :     // MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
      71             :     // indicates that the sensor or subsystem is present but not
      72             :     // functioning correctly
      73             :     uint32_t unhealthy_sensors() const;
      74           0 :     uint16_t get_avg_packet_rate() const {
      75           0 :         return _scheduler.avg_packet_rate;
      76             :     }
      77           0 :     uint16_t get_max_packet_rate() const {
      78           0 :         return _scheduler.max_packet_rate;
      79             :     }
      80             : 
      81             :     static float get_vspeed_ms(void);
      82             :     static float get_nav_alt_m(Location::AltFrame frame = Location::AltFrame::ABSOLUTE);
      83             : 
      84             : protected:
      85             :     uint8_t run_wfq_scheduler(const bool use_shaper = true);
      86             :     // process a specific entry
      87             :     bool process_scheduler_entry(const uint8_t slot );
      88             :     // set an entry in the scheduler table
      89         143 :     void set_scheduler_entry(uint8_t slot, uint32_t weight, uint32_t min_period_ms) {
      90         143 :         if (slot >= TELEM_TIME_SLOT_MAX) {
      91           0 :             return;
      92             :         }
      93         143 :         _scheduler.packet_weight[slot] = weight;
      94         143 :         _scheduler.packet_min_period[slot] = min_period_ms;
      95             :     }
      96             :     // add an entry to the scheduler table
      97          14 :     void add_scheduler_entry(uint32_t weight, uint32_t min_period_ms) {
      98          14 :         if (_time_slots >= TELEM_TIME_SLOT_MAX) {
      99           0 :             return;
     100             :         }
     101          14 :         set_scheduler_entry(_time_slots++, weight, min_period_ms);
     102             :     }
     103             :     // setup ready for passthrough operation
     104             :     virtual bool init(void);
     105             : 
     106             :     uint8_t _time_slots;
     107             : 
     108             :     struct
     109             :     {
     110             :         uint32_t last_poll_timer;
     111             :         uint32_t avg_packet_counter;
     112             :         uint32_t packet_timer[TELEM_TIME_SLOT_MAX];
     113             :         uint32_t packet_weight[TELEM_TIME_SLOT_MAX];
     114             :         uint32_t packet_min_period[TELEM_TIME_SLOT_MAX];
     115             :         uint16_t avg_packet_rate;
     116             :         uint16_t max_packet_rate;
     117             : #ifdef TELEM_DEBUG
     118             :         uint8_t packet_rate[TELEM_TIME_SLOT_MAX];
     119             : #endif
     120             :     } _scheduler;
     121             : 
     122             :     struct {
     123             :         HAL_Semaphore sem;
     124             :         ObjectBuffer<mavlink_statustext_t> queue{TELEM_PAYLOAD_STATUS_CAPACITY};
     125             :         mavlink_statustext_t next;
     126             :         bool available;
     127             :     } _statustext;
     128             : 
     129             : private:
     130             :     uint32_t check_sensor_status_timer;
     131             :     uint32_t check_ekf_status_timer;
     132             :     uint32_t _disabled_scheduler_entries_bitmask;
     133             : 
     134             :     // passthrough WFQ scheduler
     135             :     virtual void setup_wfq_scheduler() = 0;
     136           0 :     virtual bool get_next_msg_chunk(void) { return false; }
     137           0 :     virtual bool is_packet_ready(uint8_t idx, bool queue_empty) { return true; }
     138             :     virtual void process_packet(uint8_t idx) = 0;
     139           0 :     virtual void adjust_packet_weight(bool queue_empty) {};
     140       23672 :     bool check_scheduler_entry_time_constraints(const uint32_t now, uint8_t slot, const bool use_shaper) const {
     141       23672 :         if (!use_shaper) {
     142           0 :             return true;
     143             :         }
     144       23672 :         return ((now - _scheduler.packet_timer[slot]) >= _scheduler.packet_min_period[slot]);
     145             :     }
     146             : 
     147             :     void update_avg_packet_rate();
     148        2717 :     void update_max_packet_rate() {
     149        2717 :         _scheduler.max_packet_rate = MAX(_scheduler.avg_packet_rate, _scheduler.max_packet_rate);
     150        2717 :     }
     151             : 
     152             :     // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format
     153             :     void check_sensor_status_flags(void);
     154             :     void check_ekf_status(void);
     155             : };