Line data    Source code

       1             : /// @file       PID.cpp
       2             : /// @brief      Generic PID algorithm
       3             : 
       4             : #include <cmath>
       5             : 
       6             : #include "PID.h"
       7             : #include <AP_HAL/AP_HAL.h>
       8             : #include <AP_Math/AP_Math.h>
       9             : 
      10             : extern const AP_HAL::HAL& hal;
      11             : 
      12             : const AP_Param::GroupInfo PID::var_info[] = {
      13             : 
      14             :     // @Param: P
      15             :     // @DisplayName: PID Proportional Gain
      16             :     // @Description: P Gain which produces an output value that is proportional to the current error value
      17             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P",    0, PID, _kp, default_kp),
      18             : 
      19             :     // @Param: I
      20             :     // @DisplayName: PID Integral Gain
      21             :     // @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error
      22             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("I",    1, PID, _ki, default_ki),
      23             : 
      24             :     // @Param: D
      25             :     // @DisplayName: PID Derivative Gain
      26             :     // @Description: D Gain which produces an output that is proportional to the rate of change of the error
      27             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("D",    2, PID, _kd, default_kd),
      28             : 
      29             :     // @Param: IMAX
      30             :     // @DisplayName: PID Integral Maximum
      31             :     // @Description: The maximum/minimum value that the I term can output
      32             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("IMAX", 3, PID, _imax, default_kimax),
      33             : 
      34             :     AP_GROUPEND
      35             : };
      36             : 
      37         778 : float PID::get_pid(float error, float scaler)
      38             : {
      39         778 :     uint32_t tnow = AP_HAL::millis();
      40         778 :     uint32_t dt = tnow - _last_t;
      41         778 :     float output            = 0;
      42             :     float delta_time;
      43             : 
      44         778 :     if (_last_t == 0 || dt > 1000) {
      45           1 :         dt = 0;
      46             : 
      47             :                 // if this PID hasn't been used for a full second then zero
      48             :                 // the intergator term. This prevents I buildup from a
      49             :                 // previous fight mode from causing a massive return before
      50             :                 // the integrator gets a chance to correct itself
      51           1 :                 reset_I();
      52             :     }
      53         778 :     _last_t = tnow;
      54             : 
      55         778 :     delta_time = (float)dt * 0.001f;
      56             : 
      57             :     // Compute proportional component
      58         778 :     _pid_info.P = error * _kp;
      59         778 :     output += _pid_info.P;
      60             : 
      61             :     // Compute derivative component if time has elapsed
      62         778 :     if ((fabsf(_kd) > 0) && (dt > 0)) {
      63             :         float derivative;
      64             : 
      65           0 :                 if (isnan(_last_derivative)) {
      66             :                         // we've just done a reset, suppress the first derivative
      67             :                         // term as we don't want a sudden change in input to cause
      68             :                         // a large D output change                      
      69           0 :                         derivative = 0;
      70           0 :                         _last_derivative = 0;
      71             :                 } else {
      72           0 :                         derivative = (error - _last_error) / delta_time;
      73             :                 }
      74             : 
      75             :         // discrete low pass filter, cuts out the
      76             :         // high frequency noise that can drive the controller crazy
      77           0 :         float RC = 1/(2*M_PI*_fCut);
      78           0 :         derivative = _last_derivative +
      79           0 :                      ((delta_time / (RC + delta_time)) *
      80           0 :                       (derivative - _last_derivative));
      81             : 
      82             :         // update state
      83           0 :         _last_error             = error;
      84           0 :         _last_derivative    = derivative;
      85             : 
      86             :         // add in derivative component
      87           0 :         _pid_info.D = _kd * derivative;
      88           0 :         output                          += _pid_info.D;
      89             :     }
      90             : 
      91             :     // scale the P and D components
      92         778 :     output *= scaler;
      93         778 :     _pid_info.D *= scaler;
      94         778 :     _pid_info.P *= scaler;
      95             : 
      96             :     // Compute integral component if time has elapsed
      97         778 :     if ((fabsf(_ki) > 0) && (dt > 0)) {
      98           0 :         _integrator             += (error * _ki) * scaler * delta_time;
      99           0 :         if (_integrator < -_imax) {
     100           0 :             _integrator = -_imax;
     101           0 :         } else if (_integrator > _imax) {
     102           0 :             _integrator = _imax;
     103             :         }
     104           0 :         _pid_info.I = _integrator;
     105           0 :         output                          += _integrator;
     106             :     }
     107             : 
     108         778 :     _pid_info.target = output;
     109         778 :     return output;
     110             : }
     111             : 
     112             : void
     113           2 : PID::reset_I()
     114             : {
     115           2 :     _integrator = 0;
     116             :         // we use NAN (Not A Number) to indicate that the last 
     117             :         // derivative value is not valid
     118           2 :     _last_derivative = NAN;
     119           2 :     _pid_info.I = 0;
     120           2 : }
     121             : 
     122           1 : void PID::reset() {
     123           1 :     memset(&_pid_info, 0, sizeof(_pid_info));
     124           1 :     reset_I();
     125           1 : }
     126             : 
     127             : void
     128           0 : PID::load_gains()
     129             : {
     130           0 :     _kp.load();
     131           0 :     _ki.load();
     132           0 :     _kd.load();
     133           0 :     _imax.load();
     134           0 : }
     135             : 
     136             : void
     137           0 : PID::save_gains()
     138             : {
     139           0 :     _kp.save();
     140           0 :     _ki.save();
     141           0 :     _kd.save();
     142           0 :     _imax.save();
     143           0 : }