Line data    Source code

       1             : #pragma once
       2             : 
       3             : /// @file       AC_PID_Basic.h
       4             : /// @brief      Lightweight PID controller with error and derivative filtering, integrator limit, and EEPROM gain storage.
       5             : 
       6             : #include <AP_Common/AP_Common.h>
       7             : #include <AP_Param/AP_Param.h>
       8             : #include "AP_PIDInfo.h"
       9             : 
      10             : /// @class      AC_PID_Basic
      11             : /// @brief      Copter PID control class
      12             : class AC_PID_Basic {
      13             : public:
      14             : 
      15             :     /// Constructor for PID controller with EEPROM-backed gain.
      16             :     /// Parameters are initialized from defaults or EEPROM at runtime.
      17             :     AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz);
      18             : 
      19             :     // Computes the PID output using a target and measurement input.
      20             :     // Applies filters to the error and derivative, then updates the integrator.
      21             :     // If `limit` is true, the integrator is allowed to shrink but not grow.
      22             :     float update_all(float target, float measurement, float dt, bool limit = false) WARN_IF_UNUSED;
      23             :     float update_all(float target, float measurement, float dt, bool limit_neg, bool limit_pos) WARN_IF_UNUSED;
      24             : 
      25             :     // Updates the integrator using current error and dt.
      26             :     // If `limit_neg` is true, integrator may only increase.
      27             :     // If `limit_pos` is true, integrator may only decrease.
      28             :     void update_i(float dt, bool limit_neg, bool limit_pos);
      29             : 
      30             :     // get results from pid controller
      31             :     float get_p() const WARN_IF_UNUSED { return _error * _kp; }
      32             :     float get_i() const WARN_IF_UNUSED { return _integrator; }
      33             :     float get_d() const WARN_IF_UNUSED { return _derivative * _kd; }
      34             :     float get_ff() const WARN_IF_UNUSED { return _target * _kff; }
      35    94323556 :     float get_error() const WARN_IF_UNUSED { return _error; }
      36             : 
      37             :     // Resets the integrator to zero.
      38             :     void reset_I();
      39             : 
      40             :     // Flags the filter to reset on the next call to update_all().
      41     6184170 :     void reset_filter() { _reset_filter = true; }
      42             : 
      43             :     // Saves controller configuration from EEPROM, including gains and filter frequencies. (not used)
      44             :     void save_gains();
      45             : 
      46             :     // get accessors
      47      110219 :     AP_Float &kP() WARN_IF_UNUSED { return _kp; }
      48             :     AP_Float &kI() WARN_IF_UNUSED { return _ki; }
      49             :     AP_Float &kD() WARN_IF_UNUSED { return _kd; }
      50             :     AP_Float &ff() WARN_IF_UNUSED { return _kff;}
      51             :     AP_Float &filt_E_hz() WARN_IF_UNUSED { return _filt_E_hz; }
      52             :     AP_Float &filt_D_hz() WARN_IF_UNUSED { return _filt_D_hz; }
      53             :     float imax() const WARN_IF_UNUSED { return _kimax.get(); }
      54             :     float get_filt_E_alpha(float dt) const WARN_IF_UNUSED;
      55             :     float get_filt_D_alpha(float dt) const WARN_IF_UNUSED;
      56             : 
      57             :     // set accessors
      58           0 :     void set_kP(float v) { _kp.set(v); }
      59             :     void set_kI(float v) { _ki.set(v); }
      60             :     void set_kD(float v) { _kd.set(v); }
      61             :     void set_ff(float v) { _kff.set(v); }
      62             :     void set_imax(float v) { _kimax.set(fabsf(v)); }
      63             :     void set_filt_E_hz(float hz) { _filt_E_hz.set(fabsf(hz)); }
      64             :     void set_filt_D_hz(float hz) { _filt_D_hz.set(fabsf(hz)); }
      65             : 
      66             :     // Sets the integrator directly, with overloads supporting raw I value, target + measurement, or error.
      67             :     // Internally clamps to IMAX.
      68             :     void set_integrator(float target, float measurement, float i);
      69             :     void set_integrator(float error, float i);
      70             :     void set_integrator(float i);
      71             : 
      72       14401 :     const AP_PIDInfo& get_pid_info(void) const WARN_IF_UNUSED { return _pid_info; }
      73             : 
      74             :     // parameter var table
      75             :     static const struct AP_Param::GroupInfo var_info[];
      76             : 
      77             : protected:
      78             : 
      79             :     // parameters
      80             :     AP_Float _kp;
      81             :     AP_Float _ki;
      82             :     AP_Float _kd;
      83             :     AP_Float _kff;
      84             :     AP_Float _kimax;
      85             :     AP_Float _filt_E_hz;         // PID error filter frequency in Hz
      86             :     AP_Float _filt_D_hz;         // PID derivative filter frequency in Hz
      87             : 
      88             :     // internal variables
      89             :     float _target;      // target value to enable filtering
      90             :     float _error;       // error value to enable filtering
      91             :     float _derivative;  // last derivative for low-pass filter
      92             :     float _integrator;  // integrator value
      93             :     bool _reset_filter; // true when input filter should be reset during next call to set_input
      94             : 
      95             :     AP_PIDInfo _pid_info;
      96             : 
      97             : private:
      98             :     const float default_kp;
      99             :     const float default_ki;
     100             :     const float default_kd;
     101             :     const float default_kff;
     102             :     const float default_kimax;
     103             :     const float default_filt_E_hz;
     104             :     const float default_filt_D_hz;
     105             : };