Line data    Source code

       1             : 
       2             : /* Variometer class by Samuel Tabor
       3             : 
       4             : Manages the estimation of aircraft total energy, drag and vertical air velocity.
       5             : */
       6             : #pragma once
       7             : 
       8             : #include <AP_Param/AP_Param.h>
       9             : #include <Filter/AverageFilter.h>
      10             : #include <Filter/LowPassFilter.h>
      11             : #include <AP_Vehicle/AP_FixedWing.h>
      12             : #include <AP_Common/Location.h>
      13             : 
      14             : class Variometer {
      15             : 
      16             :     const AP_FixedWing &_aparm;
      17             : 
      18             :     // store time of last update
      19             :     uint64_t _prev_update_time;
      20             : 
      21             :     float _raw_climb_rate;
      22             : 
      23             :     float _aspd_filt_constrained;
      24             : 
      25             :     float _expected_thermalling_sink;
      26             : 
      27             :     // declares a 5point average filter using floats
      28             :     AverageFilterFloat_Size5 _vdot_filter;
      29             : 
      30             :     AverageFilterFloat_Size5 _sp_filter;
      31             : 
      32             :     /*
      33             :      low pass filters for various purposes.
      34             :      */
      35             :     // Climb rate filter for monitoring progress in thermal.
      36         692 :     LowPassFilter<float> _climb_filter{1/60.0};
      37             : 
      38             :     // Fast filter for mavlink/audio vario output.
      39         692 :     LowPassFilter<float> _audio_filter{1/0.71};
      40             : 
      41             :     // Slower filter for deciding to enter THERMAL mode.
      42         692 :     LowPassFilter<float> _trigger_filter{1/4.06};
      43             : 
      44             :     // Longitudinal acceleration bias filter.
      45         692 :     LowPassFilter<float> _vdotbias_filter{1/60.0};
      46             : 
      47             :     // Speed to fly vario filter.
      48         692 :     LowPassFilter<float> _stf_filter{1/20.0};
      49             : 
      50             : public:
      51             :     struct PolarParams {
      52             :         AP_Float K;
      53             :         AP_Float CD0;
      54             :         AP_Float B;
      55             :     };
      56             : 
      57             :     Variometer(const AP_FixedWing &parms, const PolarParams &polarParams);
      58             : 
      59             :     float alt;
      60             :     float reading;
      61             :     float tau;
      62             : 
      63             :     void update(const float thermal_bank);
      64             :     float calculate_aircraft_sinkrate(float phi) const;
      65             : 
      66           1 :     void reset_climb_filter(float value) { _climb_filter.reset(value);}
      67             : 
      68           5 :     void reset_trigger_filter(float value) { _trigger_filter.reset(value);}
      69             : 
      70             :     float get_airspeed(void) const {return _aspd_filt_constrained;};
      71             : 
      72       13750 :     float get_displayed_value(void) const {return _audio_filter.get();};
      73             : 
      74       25985 :     float get_filtered_climb(void) const {return _climb_filter.get();};
      75             : 
      76       22463 :     float get_trigger_value(void) const {return _trigger_filter.get();};
      77             : 
      78       34129 :     float get_stf_value(void) const {return _stf_filter.get();};
      79             : 
      80       36080 :     float get_exp_thermalling_sink(void) const {return _expected_thermalling_sink;};
      81             : 
      82             :     float calculate_circling_time_constant(const float thermal_bank) const;
      83             : 
      84             : private:
      85             :     const PolarParams &_polarParams;
      86             : };
      87             :