Line data    Source code

       1             : #pragma once
       2             : 
       3             : /// @file       AC_PID_2D.h
       4             : /// @brief      2D PID controller with vector support, input filtering, integrator clamping, and EEPROM-backed 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 <AC_PID/AP_PIDInfo.h>
      11             : 
      12             : /// @class      AC_PID_2D
      13             : /// @brief      Copter PID control class
      14             : class AC_PID_2D {
      15             : public:
      16             : 
      17             :     /// Constructor for 2D PID controller with EEPROM-backed gain.
      18             :     /// Parameters are initialized from defaults or EEPROM at runtime.
      19             :     AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, float initial_imax, float initial_filt_hz, float initial_filt_d_hz);
      20             : 
      21             :     CLASS_NO_COPY(AC_PID_2D);
      22             : 
      23             :     // Computes the 2D PID output from target and measurement vectors.
      24             :     // Applies filtering to error and derivative terms.
      25             :     // Integrator is updated only if it does not grow in the direction of the specified limit vector.
      26             :     Vector2f update_all(const Vector2f &target, const Vector2f &measurement, float dt, const Vector2f &limit);
      27             :     Vector2f update_all(const Vector3f &target, const Vector3f &measurement, float dt, const Vector3f &limit);
      28             : 
      29             :     // Updates the 2D integrator using the filtered error.
      30             :     // The integrator is only allowed to grow if it does not push further in the direction of the limit vector.
      31             :     void update_i(float dt, const Vector2f &limit);
      32             : 
      33             :     // get results from pid controller
      34             :     Vector2f get_p() const;
      35             :     const Vector2f& get_i() const;
      36             :     Vector2f get_d() const;
      37             :     Vector2f get_ff();
      38    29187743 :     const Vector2f& get_error() const { return _error; }
      39             : 
      40             :     // reset the integrator
      41             :     void reset_I();
      42             : 
      43             :     // Flags the input and derivative filters for reset on the next call to update_all().
      44     3708955 :     void reset_filter() { _reset_filter = true; }
      45             : 
      46             :     // Saves controller configuration from EEPROM, including gains and filter frequencies. (not used)
      47             :     void save_gains();
      48             : 
      49             :     // get accessors
      50     2432684 :     AP_Float &kP() { return _kp; }
      51           0 :     AP_Float &kI() { return _ki; }
      52             :     AP_Float &kD() { return _kd; }
      53     3708875 :     AP_Float &ff() { return _kff;}
      54             :     AP_Float &filt_E_hz() { return _filt_E_hz; }
      55             :     AP_Float &filt_D_hz() { return _filt_D_hz; }
      56             :     float imax() const { return _kimax.get(); }
      57             :     float get_filt_E_alpha(float dt) const;
      58             :     float get_filt_D_alpha(float dt) const;
      59             : 
      60             :     // set accessors
      61           0 :     void set_kP(float v) { _kp.set(v); }
      62          29 :     void set_kI(float v) { _ki.set(v); }
      63             :     void set_kD(float v) { _kd.set(v); }
      64             :     void set_ff(float v) { _kff.set(v); }
      65             :     void set_imax(float v) { _kimax.set(fabsf(v)); }
      66             :     void set_filt_E_hz(float hz) { _filt_E_hz.set(fabsf(hz)); }
      67             :     void set_filt_D_hz(float hz) { _filt_D_hz.set(fabsf(hz)); }
      68             : 
      69             :     // Sets the integrator directly or based on a target, measurement, or error.
      70             :     // Result is clamped to IMAX length.
      71             :     void set_integrator(const Vector2f& target, const Vector2f& measurement, const Vector2f& i);
      72             :     void set_integrator(const Vector2f& error, const Vector2f& i);
      73             :     void set_integrator(const Vector3f& i) { set_integrator(Vector2f{i.x, i.y}); }
      74             :     void set_integrator(const Vector2f& i);
      75             : 
      76      667193 :     const AP_PIDInfo& get_pid_info_x(void) const { return _pid_info_x; }
      77      667193 :     const AP_PIDInfo& get_pid_info_y(void) const { return _pid_info_y; }
      78             : 
      79             :     // parameter var table
      80             :     static const struct AP_Param::GroupInfo        var_info[];
      81             : 
      82             : protected:
      83             : 
      84             :     // parameters
      85             :     AP_Float _kp;
      86             :     AP_Float _ki;
      87             :     AP_Float _kd;
      88             :     AP_Float _kff;
      89             :     AP_Float _kimax;
      90             :     AP_Float _filt_E_hz;         // PID error filter frequency in Hz
      91             :     AP_Float _filt_D_hz;         // PID derivative filter frequency in Hz
      92             : 
      93             :     // internal variables
      94             :     Vector2f    _target;        // target value to enable filtering
      95             :     Vector2f    _error;         // error value to enable filtering
      96             :     Vector2f    _derivative;    // last derivative from low-pass filter
      97             :     Vector2f    _integrator;    // integrator value
      98             :     bool        _reset_filter;  // true when input filter should be reset during next call to update_all
      99             : 
     100             :     AP_PIDInfo _pid_info_x;
     101             :     AP_PIDInfo _pid_info_y;
     102             : 
     103             : private:
     104             :     const float default_kp;
     105             :     const float default_ki;
     106             :     const float default_kd;
     107             :     const float default_kff;
     108             :     const float default_kimax;
     109             :     const float default_filt_E_hz;
     110             :     const float default_filt_D_hz;
     111             : };