Line data    Source code

       1             : #pragma once
       2             : 
       3             : // VTOL assistance in a forward flight mode
       4             : 
       5             : class QuadPlane;
       6             : class VTOL_Assist {
       7             : public:
       8         692 :     VTOL_Assist(QuadPlane& _quadplane):quadplane(_quadplane) {};
       9             : 
      10             :     // check for assistance needed
      11             :     bool should_assist(float aspeed, bool have_airspeed);
      12             : 
      13             :     // Assistance not needed, reset any state
      14             :     void reset();
      15             : 
      16             :     // speed below which quad assistance is given
      17             :     AP_Float speed;
      18             : 
      19             :     // angular error at which quad assistance is given
      20             :     AP_Int8 angle;
      21             : 
      22             :     // altitude to trigger assistance
      23             :     AP_Int16 alt;
      24             : 
      25             :     // Time hysteresis for triggering of assistance
      26             :     AP_Float delay;
      27             : 
      28             :     // special options
      29             :     AP_Int16 options;
      30             : 
      31             :     // assist options
      32             :     enum class OPTION {
      33             :         FW_FORCE_DISABLED=(1U<<0),
      34             :         SPIN_DISABLED=(1U<<1),
      35             :     };
      36             :     
      37     7418988 :     bool option_is_set(OPTION option) const {
      38     7418988 :         return (options.get() & int32_t(option)) != 0;
      39             :     }
      40             :     
      41             :     // State from pilot
      42             :     enum class STATE {
      43             :         ASSIST_DISABLED,
      44             :         ASSIST_ENABLED,
      45             :         FORCE_ENABLED,
      46             :     };
      47           0 :     void set_state(STATE _state) { state = _state; }
      48             : 
      49             :     // Logging getters for assist types
      50      205625 :     bool in_force_assist() const { return force_assist; }
      51      205625 :     bool in_speed_assist() const { return speed_assist; }
      52      205625 :     bool in_alt_assist() const { return alt_error.is_active(); }
      53      205625 :     bool in_angle_assist() const { return angle_error.is_active(); }
      54             : 
      55             :     // check if we are in VTOL recovery
      56             :     bool check_VTOL_recovery(void);
      57             : 
      58             :     // output rudder and elevator for spin recovery
      59             :     void output_spin_recovery(void);
      60             :     
      61             : private:
      62             : 
      63             :     // Default to enabled
      64             :     STATE state = STATE::ASSIST_ENABLED;
      65             : 
      66             :     class Assist_Hysteresis {
      67             :     public:
      68             :         // Reset state
      69             :         void reset();
      70             : 
      71             :         // Update state, return true when first triggered
      72             :         bool update(const bool trigger, const uint32_t &now_ms, const uint32_t &trigger_delay_ms, const uint32_t &clear_delay_ms);
      73             : 
      74             :         // Return true if the output is active
      75     5321259 :         bool is_active() const { return active; }
      76             : 
      77             :     private:
      78             :         uint32_t start_ms;
      79             :         uint32_t last_ms;
      80             :         bool active;
      81             :     };
      82             :     Assist_Hysteresis angle_error;
      83             :     Assist_Hysteresis alt_error;
      84             : 
      85             :     // Force and speed assist have no hysteresis
      86             :     bool force_assist;
      87             :     bool speed_assist;
      88             : 
      89             :     // Reference to access quadplane
      90             :     QuadPlane& quadplane;
      91             : };