Line data    Source code

       1             : #pragma once
       2             : 
       3             : /// @file       AC_PID.h
       4             : /// @brief      General-purpose PID controller with input, error, and derivative filtering, plus slew rate limiting and EEPROM gain storage.
       5             : 
       6             : #include <AP_Common/AP_Common.h>
       7             : #include <AP_Param/AP_Param.h>
       8             : #include <stdlib.h>
       9             : #include <cmath>
      10             : #include <Filter/SlewLimiter.h>
      11             : #include <Filter/NotchFilter.h>
      12             : #include <Filter/AP_Filter.h>
      13             : 
      14             : #define AC_PID_TFILT_HZ_DEFAULT  0.0f   // default input filter frequency
      15             : #define AC_PID_EFILT_HZ_DEFAULT  0.0f   // default input filter frequency
      16             : #define AC_PID_DFILT_HZ_DEFAULT  20.0f  // default input filter frequency
      17             : #define AC_PID_RESET_TC          0.16f  // Time constant for integrator reset decay to zero
      18             : 
      19             : #include "AP_PIDInfo.h"
      20             : 
      21             : /// @class      AC_PID
      22             : /// @brief      Copter PID control class
      23             : class AC_PID {
      24             : public:
      25             : 
      26             :     struct Defaults {
      27             :         float p;
      28             :         float i;
      29             :         float d;
      30             :         float ff;
      31             :         float imax;
      32             :         float filt_T_hz;
      33             :         float filt_E_hz;
      34             :         float filt_D_hz;
      35             :         float srmax;
      36             :         float srtau;
      37             :         float dff;
      38             :     };
      39             : 
      40             :     /// Constructor for PID controller with EEPROM-backed gain.
      41             :     /// Parameters are initialized from defaults or EEPROM at runtime.
      42             :     AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz,
      43             :            float initial_srmax=0, float initial_srtau=1.0, float initial_dff=0);
      44        5809 :     AC_PID(const AC_PID::Defaults &defaults) :
      45             :         AC_PID(
      46        5809 :             defaults.p,
      47        5809 :             defaults.i,
      48        5809 :             defaults.d,
      49        5809 :             defaults.ff,
      50        5809 :             defaults.imax,
      51        5809 :             defaults.filt_T_hz,
      52        5809 :             defaults.filt_E_hz,
      53        5809 :             defaults.filt_D_hz,
      54        5809 :             defaults.srmax,
      55        5809 :             defaults.srtau,
      56        5809 :             defaults.dff
      57        5809 :             )
      58        5809 :         { }
      59             : 
      60             :     CLASS_NO_COPY(AC_PID);
      61             : 
      62             :     // Computes the PID output using a target and measurement input.
      63             :     // Applies filters to the target and error, calculates the derivative and updates the integrator.
      64             :     // If `limit` is true, the integrator is allowed to shrink but not grow.
      65             :     float update_all(float target, float measurement, float dt, bool limit = false, float pd_scale = 1.0f, float i_scale = 1.0f);
      66             : 
      67             :     // Computes the PID output from an error input only (target assumed to be zero).
      68             :     // Applies error filtering and updates the derivative and integrator.
      69             :     // Target and measurement must be set separately for logging.
      70             :     // todo: remove function when it is no longer used.
      71             :     float update_error(float error, float dt, bool limit = false);
      72             : 
      73             :     // get_pid - get results from pid controller
      74             :     float get_p() const;
      75             :     float get_i() const;
      76             :     float get_d() const;
      77             :     float get_ff() const;
      78             :     float get_ff_component() const;
      79             :     float get_dff_component() const;
      80             : 
      81             :     // Used to fully zero the I term between mode changes or initialization
      82             :     void reset_I();
      83             : 
      84             :     // Flags the input filter for reset. The next call to `update_all()` will reinitialize the filter using the next input.
      85    22303386 :     void reset_filter() {
      86    22303386 :         _flags._reset_filter = true;
      87    22303386 :     }
      88             : 
      89             :     // Loads controller configuration from EEPROM, including gains and filter frequencies. (not used)
      90             :     void load_gains();
      91             : 
      92             :     // Saves controller configuration from EEPROM, including gains and filter frequencies. Used by autotune to save gains before tuning.
      93             :     void save_gains();
      94             : 
      95             :     // get accessors
      96    65929148 :     const AP_Float &kP() const { return _kp; }
      97     6592913 :     AP_Float &kP() { return _kp; }
      98    30813951 :     AP_Float &kI() { return _ki; }
      99      310931 :     AP_Float &kD() { return _kd; }
     100          44 :     AP_Float &kIMAX() { return _kimax; }
     101             :     AP_Float &kPDMAX() { return _kpdmax; }
     102     6485827 :     AP_Float &ff() { return _kff;}
     103    10986227 :     AP_Float &filt_T_hz() { return _filt_T_hz; }
     104    21799520 :     AP_Float &filt_E_hz() { return _filt_E_hz; }
     105        1663 :     AP_Float &filt_D_hz() { return _filt_D_hz; }
     106       37147 :     AP_Float &slew_limit() { return _slew_rate_max; }
     107          24 :     AP_Float &kDff() { return _kdff; }
     108             : 
     109    43962462 :     float imax() const { return _kimax.get(); }
     110             :     float pdmax() const { return _kpdmax.get(); }
     111             : 
     112             :     // Returns alpha value for the target low-pass filter (based on filter frequency and dt)
     113             :     float get_filt_T_alpha(float dt) const;
     114             :     // Returns alpha value for the error low-pass filter (based on filter frequency and dt)
     115             :     float get_filt_E_alpha(float dt) const;
     116             :     // Returns alpha value for the derivative low-pass filter (based on filter frequency and dt)
     117             :     float get_filt_D_alpha(float dt) const;
     118             : 
     119             :     // set accessors
     120        7366 :     void set_kP(const float v) { _kp.set(v); }
     121        7366 :     void set_kI(const float v) { _ki.set(v); }
     122        7360 :     void set_kD(const float v) { _kd.set(v); }
     123        7366 :     void set_ff(const float v) { _kff.set(v); }
     124          16 :     void set_imax(const float v) { _kimax.set(fabsf(v)); }
     125             :     void set_pdmax(const float v) { _kpdmax.set(fabsf(v)); }
     126             :     void set_filt_T_hz(const float v);
     127             :     void set_filt_E_hz(const float v);
     128             :     void set_filt_D_hz(const float v);
     129             :     void set_slew_limit(const float v);
     130        7006 :     void set_kDff(const float v) { _kdff.set(v); }
     131             : 
     132             :     // Sets target and actual rate values for external logging (optional).
     133       35750 :     void set_target_rate(float target) { _pid_info.target = target; }
     134       35750 :     void set_actual_rate(float actual) { _pid_info.actual = actual; }
     135             : 
     136             :     // Sets the integrator directly, clamped to the IMAX bounds. Also flags I-term as externally set.
     137             :     void set_integrator(float i);
     138             : 
     139             :     // Gradually adjust the integrator toward a desired value using a time constant.
     140             :     // Typically used to "relax" the I-term in dynamic conditions.
     141             :     void relax_integrator(float integrator, float dt, float time_constant);
     142             : 
     143             :     // set slew limiter scale factor
     144        2076 :     void set_slew_limit_scale(int8_t scale) { _slew_limit_scale = scale; }
     145             : 
     146             :     // Returns current slew rate from the limiter. Returns 0 if SMAX is zero (disabled).
     147             :     float get_slew_rate(void) const { return _slew_limiter.get_slew_rate(); }
     148             : 
     149    64224331 :     const AP_PIDInfo& get_pid_info(void) const { return _pid_info; }
     150             : 
     151             :     // Configures optional notch filters for target and error signals using the given sample rate.
     152             :     // Filters are dynamically allocated and validated via the AP_Filter API.
     153             :     void set_notch_sample_rate(float);
     154             : 
     155             :     // parameter var table
     156             :     static const struct AP_Param::GroupInfo var_info[];
     157             : 
     158             : protected:
     159             : 
     160             :     // Updates the integrator based on current error and dt.
     161             :     // If `limit` is true, the integrator is only allowed to shrink to avoid wind-up.
     162             :     void update_i(float dt, bool limit, float i_scale = 1.0f);
     163             : 
     164             :     // parameters
     165             :     AP_Float _kp;
     166             :     AP_Float _ki;
     167             :     AP_Float _kd;
     168             :     AP_Float _kff;
     169             :     AP_Float _kimax;
     170             :     AP_Float _kpdmax;
     171             :     AP_Float _filt_T_hz;         // PID target filter frequency in Hz
     172             :     AP_Float _filt_E_hz;         // PID error filter frequency in Hz
     173             :     AP_Float _filt_D_hz;         // PID derivative filter frequency in Hz
     174             :     AP_Float _slew_rate_max;
     175             :     AP_Float _kdff;
     176             : #if AP_FILTER_ENABLED
     177             :     AP_Int8 _notch_T_filter;
     178             :     AP_Int8 _notch_E_filter;
     179             : #endif
     180             : 
     181             :     // Slew rate time constant (tau). Not exposed in this class by default, but defined as an AP_Float so parent classes can make it configurable via param table.
     182             :     AP_Float _slew_rate_tau;
     183             : 
     184             :     SlewLimiter _slew_limiter{_slew_rate_max, _slew_rate_tau};
     185             : 
     186             :     // flags
     187             :     struct ac_pid_flags {
     188             :         bool _reset_filter :1;  // true if the input filter should be reset on the next call to update_all()
     189             :         bool _I_set :1;         // true if the I term has been set externally, including zeroing
     190             :     } _flags;
     191             : 
     192             :     // internal variables
     193             :     float _integrator;        // integrator value
     194             :     float _target;            // target value to enable filtering
     195             :     float _error;             // error value to enable filtering
     196             :     float _derivative;        // derivative value to enable filtering
     197             :     int8_t _slew_limit_scale;
     198             :     float _target_derivative; // target derivative value to enable dff
     199             : #if AP_FILTER_ENABLED
     200             :     NotchFilterFloat* _target_notch;
     201             :     NotchFilterFloat* _error_notch;
     202             : #endif
     203             : 
     204             :     AP_PIDInfo _pid_info;
     205             : 
     206             :     const float default_kp;
     207             :     const float default_ki;
     208             :     const float default_kd;
     209             :     const float default_kff;
     210             :     const float default_kdff;
     211             :     const float default_kimax;
     212             :     const float default_filt_T_hz;
     213             :     const float default_filt_E_hz;
     214             :     const float default_filt_D_hz;
     215             :     const float default_slew_rate_max;
     216             : };