Line data    Source code

       1             : /// @file       PID.h
       2             : /// @brief      Generic PID algorithm, with EEPROM-backed storage of constants.
       3             : #pragma once
       4             : 
       5             : #include <AP_Common/AP_Common.h>
       6             : #include <AP_Param/AP_Param.h>
       7             : #include <AC_PID/AC_PID.h>  // for AP_PIDInfo
       8             : #include <stdlib.h>
       9             : #include <cmath>
      10             : 
      11             : /// @class      PID
      12             : /// @brief      Object managing one PID control
      13             : class PID {
      14             : public:
      15             : 
      16        2076 :     PID(const float &   initial_p = 0.0f,
      17        1384 :         const float &   initial_i = 0.0f,
      18        1384 :         const float &   initial_d = 0.0f,
      19         692 :         const int16_t & initial_imax = 0):
      20         692 :             default_kp(initial_p),
      21         692 :             default_ki(initial_i),
      22         692 :             default_kd(initial_d),
      23         692 :             default_kimax(initial_imax)
      24             :     {
      25         692 :                 AP_Param::setup_object_defaults(this, var_info);
      26             : 
      27             :                 // set _last_derivative as invalid when we startup
      28         692 :                 _last_derivative = NAN;
      29         692 :     }
      30             : 
      31             :     /// Iterate the PID, return the new control value
      32             :     ///
      33             :     /// Positive error produces positive output.
      34             :     ///
      35             :     /// @param error    The measured error value
      36             :     /// @param scaler   An arbitrary scale factor
      37             :     ///
      38             :     /// @returns                The updated control output.
      39             :     ///
      40             :     float        get_pid(float error, float scaler = 1.0);
      41             : 
      42             :     /// Reset the whole PID state
      43             :     //
      44             :     void        reset();
      45             : 
      46             :     /// Reset the PID integrator
      47             :     ///
      48             :     void        reset_I();
      49             : 
      50             :     /// Load gain properties
      51             :     ///
      52             :     void        load_gains();
      53             : 
      54             :     /// Save gain properties
      55             :     ///
      56             :     void        save_gains();
      57             : 
      58             :     /// @name   parameter accessors
      59             :     //@{
      60             : 
      61             :     /// Overload the function call operator to permit relatively easy initialisation
      62             :     void operator        () (const float    p,
      63             :                              const float    i,
      64             :                              const float    d,
      65             :                              const int16_t  imaxval) {
      66             :         _kp.set(p); _ki.set(i); _kd.set(d); _imax.set(imaxval);
      67             :     }
      68             : 
      69             :     float        kP() const {
      70             :         return _kp.get();
      71             :     }
      72             :     float        kI() const {
      73             :         return _ki.get();
      74             :     }
      75             :     float        kD() const {
      76             :         return _kd.get();
      77             :     }
      78             :     int16_t        imax() const {
      79             :         return _imax.get();
      80             :     }
      81             : 
      82             :     void        kP(const float v)               {
      83             :         _kp.set(v);
      84             :     }
      85             :     void        kI(const float v)               {
      86             :         _ki.set(v);
      87             :     }
      88             :     void        kD(const float v)               {
      89             :         _kd.set(v);
      90             :     }
      91             :     void        imax(const int16_t v)   {
      92             :         _imax.set(abs(v));
      93             :     }
      94             : 
      95             :     float        get_integrator() const {
      96             :         return _integrator;
      97             :     }
      98             : 
      99             :     static const struct AP_Param::GroupInfo        var_info[];
     100             : 
     101         905 :     const AP_PIDInfo& get_pid_info(void) const { return _pid_info; }
     102             : 
     103             : private:
     104             :     AP_Float        _kp;
     105             :     AP_Float        _ki;
     106             :     AP_Float        _kd;
     107             :     AP_Int16        _imax;
     108             : 
     109             :     float           _integrator;///< integrator value
     110             :     float           _last_error;///< last error for derivative
     111             :     float           _last_derivative;///< last derivative for low-pass filter
     112             :     uint32_t        _last_t;///< last time get_pid() was called in millis
     113             : 
     114             :     float           _get_pid(float error, uint16_t dt, float scaler);
     115             : 
     116             :     AP_PIDInfo _pid_info {};
     117             : 
     118             :     /// Low pass filter cut frequency for derivative calculation.
     119             :     ///
     120             :     /// 20 Hz because anything over that is probably noise, see
     121             :     /// http://en.wikipedia.org/wiki/Low-pass_filter.
     122             :     ///
     123             :     static const uint8_t        _fCut = 20;
     124             : 
     125             :     const float default_kp;
     126             :     const float default_ki;
     127             :     const float default_kd;
     128             :     const float default_kimax;
     129             : };