Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Common/AP_Common.h>
       4             : #include <AC_PID/AC_PID.h>
       5             : #include "AP_AutoTune.h"
       6             : 
       7             : class AP_YawController
       8             : {
       9             : public:
      10             :     AP_YawController(const AP_FixedWing &parms);
      11             : 
      12             :     /* Do not allow copies */
      13             :     CLASS_NO_COPY(AP_YawController);
      14             : 
      15             :     // return true if rate control or damping is enabled
      16     1016541 :     bool enabled() const { return rate_control_enabled() || (_K_D > 0.0); } 
      17             : 
      18             :     // return true if rate control is enabled
      19     1023202 :     bool rate_control_enabled(void) const { return _rate_enable != 0; }
      20             : 
      21             :     // get actuator output for sideslip and yaw damping control
      22             :     int32_t get_servo_out(float scaler, bool disable_integrator);
      23             : 
      24             :     // get actuator output for direct rate control
      25             :     // desired_rate is in deg/sec. scaler is the surface speed scaler
      26             :     float get_rate_out(float desired_rate, float scaler, bool disable_integrator);
      27             : 
      28             :     void reset_I();
      29             : 
      30             :     void reset_rate_PID();
      31             : 
      32             :     /*
      33             :       reduce the integrator, used when we have a low scale factor in a quadplane hover
      34             :     */
      35       68104 :     void decay_I()
      36             :     {
      37             :         // this reduces integrator by 95% over 2s
      38       68104 :         _pid_info.I *= 0.995f;
      39       68104 :     }
      40             : 
      41        8420 :     const AP_PIDInfo& get_pid_info(void) const
      42             :     {
      43        8420 :         return _pid_info;
      44             :     }
      45             : 
      46             :     // set the PID notch sample rates
      47       54357 :     void set_notch_sample_rate(float sample_rate) { rate_pid.set_notch_sample_rate(sample_rate); }
      48             : 
      49             :     // start/stop auto tuner
      50             :     void autotune_start(void);
      51             :     void autotune_restore(void);
      52             :     
      53             : 
      54             :     static const struct AP_Param::GroupInfo var_info[];
      55             : 
      56             : private:
      57             :     const AP_FixedWing &aparm;
      58             :     AP_Float _K_A;
      59             :     AP_Float _K_I;
      60             :     AP_Float _K_D;
      61             :     AP_Float _K_FF;
      62             :     AP_Int16 _imax;
      63             :     AP_Int8  _rate_enable;
      64             :     AC_PID rate_pid{0.04, 0.15, 0, 0.15, 0.666, 3, 0, 12, 150, 1};
      65             : 
      66             :     uint32_t _last_t;
      67             :     float _last_out;
      68             :     float _last_rate_hp_out;
      69             :     float _last_rate_hp_in;
      70             :     float _K_D_last;
      71             : 
      72             :     float _integrator;
      73             : 
      74             :     AP_AutoTune::ATGains gains;
      75             :     AP_AutoTune *autotune;
      76             :     bool failed_autotune_alloc;
      77             :     
      78             :     AP_PIDInfo _pid_info;
      79             : };