Line data    Source code

       1             : /*
       2             :   Soaring Controller class by Samuel Tabor
       3             : 
       4             :   Provides a layer between the thermal centring algorithm and the main
       5             :   code for managing navigation targets, data logging, tuning parameters,
       6             :   algorithm inputs and eventually other soaring strategies such as
       7             :   speed-to-fly. AP_TECS library used for reference.
       8             : */
       9             : 
      10             : #pragma once
      11             : 
      12             : #include "AP_Soaring_config.h"
      13             : #if HAL_SOARING_ENABLED
      14             : 
      15             : #include <AP_Param/AP_Param.h>
      16             : #include <AP_Math/AP_Math.h>
      17             : #include "ExtendedKalmanFilter.h"
      18             : #include "Variometer.h"
      19             : #include "SpeedToFly.h"
      20             : 
      21             : static constexpr float INITIAL_THERMAL_RADIUS = 80.0;
      22             : static constexpr float INITIAL_STRENGTH_COVARIANCE = 0.0049;
      23             : static constexpr float INITIAL_RADIUS_COVARIANCE = 400.0;
      24             : static constexpr float INITIAL_POSITION_COVARIANCE = 400.0;
      25             : 
      26             : 
      27             : class SoaringController {
      28             :     Variometer::PolarParams _polarParams;
      29             :     ExtendedKalmanFilter _ekf{};
      30             :     class AP_TECS &_tecs;
      31             :     Variometer _vario;
      32             :     SpeedToFly _speedToFly;
      33             : 
      34             :     const AP_FixedWing &_aparm;
      35             : 
      36             :     // store aircraft location at last update
      37             :     Vector3f _prev_update_location;
      38             : 
      39             :     // store time thermal was entered for hysteresis
      40             :     uint64_t _thermal_start_time_us;
      41             : 
      42             :     // store position thermal was entered as a backup check
      43             :     Vector3f _thermal_start_pos;
      44             : 
      45             :     // store time cruise was entered for hysteresis
      46             :     uint64_t _cruise_start_time_us;
      47             : 
      48             :     // store time of last update
      49             :     uint64_t _prev_update_time;
      50             : 
      51             :     // store time of last NVT publish
      52             : #if HAL_SOARING_NVF_EKF_ENABLED
      53             :     uint32_t _prev_nvf_pub_time_ms;
      54             : #endif // #if HAL_SOARING_NVF_EKF_ENABLED
      55             : 
      56             :     bool _throttle_suppressed;
      57             : 
      58             :     float McCready(float alt);
      59             : 
      60             :     float _thermalability;
      61             : 
      62         692 :     LowPassFilter<float> _position_x_filter{1/60.0};
      63         692 :     LowPassFilter<float> _position_y_filter{1/60.0};
      64             : 
      65             : protected:
      66             :     AP_Int8 soar_active;
      67             :     AP_Float thermal_vspeed;
      68             :     AP_Float thermal_q1;
      69             :     AP_Float thermal_q2;
      70             :     AP_Float thermal_r;
      71             :     AP_Float thermal_distance_ahead;
      72             :     AP_Int16 min_thermal_s;
      73             :     AP_Int16 min_cruise_s;
      74             :     AP_Float alt_max;
      75             :     AP_Float alt_min;
      76             :     AP_Float alt_cutoff;
      77             :     AP_Float max_drift;
      78             :     AP_Float thermal_bank;
      79             :     AP_Float soar_thermal_airspeed;
      80             :     AP_Float soar_cruise_airspeed;
      81             :     AP_Float soar_thermal_flap;
      82             : 
      83             : public:
      84             :     SoaringController(class AP_TECS &tecs, const AP_FixedWing &parms);
      85             : 
      86             :     enum class LoiterStatus {
      87             :         DISABLED,
      88             :         ALT_TOO_HIGH,
      89             :         ALT_TOO_LOW,
      90             :         THERMAL_WEAK,
      91             :         ALT_LOST,
      92             :         DRIFT_EXCEEDED,
      93             :         GOOD_TO_KEEP_LOITERING,
      94             :         EXIT_COMMANDED,
      95             :     };
      96             : 
      97             :     enum class ActiveStatus {
      98             :         SOARING_DISABLED,
      99             :         MANUAL_MODE_CHANGE,
     100             :         AUTO_MODE_CHANGE
     101             :     };
     102             : 
     103             :     AP_Float max_radius;
     104             : 
     105             :     // this supports the TECS_* user settable parameters
     106             :     static const struct AP_Param::GroupInfo var_info[];
     107             :     void get_target(Location & wp);
     108             :     bool suppress_throttle();
     109             :     bool check_thermal_criteria();
     110             :     LoiterStatus check_cruise_criteria(Vector2f prev_wp, Vector2f next_wp);
     111             :     void init_thermalling();
     112             :     void init_cruising();
     113             :     void update_thermalling();
     114             :     void update_cruising();
     115             :     void set_throttle_suppressed(bool suppressed);
     116             : 
     117       25500 :     bool get_throttle_suppressed() const
     118             :     {
     119       25500 :         return _throttle_suppressed;
     120             :     }
     121             : 
     122       13750 :     float get_vario_reading() const
     123             :     {
     124       13750 :         return _vario.get_displayed_value();
     125             :     }
     126             : 
     127             :     void update_vario();
     128             : 
     129             :     bool check_drift(Vector2f prev_wp, Vector2f next_wp);
     130             : 
     131             :     void update_active_state(bool override_disable);
     132             : 
     133     4332498 :     bool is_active() const {return _last_update_status>=SoaringController::ActiveStatus::MANUAL_MODE_CHANGE;};
     134             : 
     135          17 :     void set_pilot_desired_state(ActiveStatus pilot_desired_state) {_pilot_desired_state = pilot_desired_state;};
     136             : 
     137         321 :     float get_alt_cutoff() const {return alt_cutoff;}
     138             : 
     139           1 :     float get_circling_time() const {return _vario.tau;}
     140             : 
     141             :     float get_thermalling_radius() const;
     142             : 
     143             :     float get_thermalling_target_airspeed();
     144             : 
     145             :     float get_cruising_target_airspeed();
     146             : 
     147       13618 :     float get_thermalling_flap() const
     148             :     {
     149       13618 :         return soar_thermal_flap;
     150             :     }
     151             : 
     152             : private:
     153             : 
     154             :     ActiveStatus _last_update_status;
     155             : 
     156             :     ActiveStatus _pilot_desired_state = ActiveStatus::AUTO_MODE_CHANGE;
     157             : 
     158             :     ActiveStatus active_state(bool override_disable) const;
     159             : 
     160             :     bool _exit_commanded;
     161             : };
     162             : 
     163             : #endif // HAL_SOARING_ENABLED