Line data    Source code

       1             : /// @file       AC_PI_2D.cpp
       2             : /// @brief      2-axis PI controller
       3             : 
       4             : #include <AP_Math/AP_Math.h>
       5             : #include "AC_PI_2D.h"
       6             : 
       7             : const AP_Param::GroupInfo AC_PI_2D::var_info[] = {
       8             :     // @Param: P
       9             :     // @DisplayName: PI Proportional Gain
      10             :     // @Description: P Gain which produces an output value that is proportional to the current error value
      11             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P",    0, AC_PI_2D, _kp, default_kp),
      12             : 
      13             :     // @Param: I
      14             :     // @DisplayName: PI Integral Gain
      15             :     // @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error
      16             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("I",    1, AC_PI_2D, _ki, default_ki),
      17             : 
      18             :     // @Param: IMAX
      19             :     // @DisplayName: PI Integral Maximum
      20             :     // @Description: The maximum/minimum value that the I term can output
      21             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("IMAX", 2, AC_PI_2D, _imax, default_imax),
      22             : 
      23             :     // @Param: FILT_HZ
      24             :     // @DisplayName: PI Input filter frequency in Hz
      25             :     // @Description: Input filter frequency in Hz
      26             :     // @Units: Hz
      27             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FILT_HZ", 3, AC_PI_2D, _filt_hz, default_filt_hz),
      28             : 
      29             :     AP_GROUPEND
      30             : };
      31             : 
      32             : // Constructor
      33        1001 : AC_PI_2D::AC_PI_2D(float initial_p, float initial_i, float initial_imax, float initial_filt_hz, float dt) :
      34        1001 :     _dt(dt),
      35        1001 :     default_kp(initial_p),
      36        1001 :     default_ki(initial_i),
      37        1001 :     default_imax(initial_imax),
      38        1001 :     default_filt_hz(initial_filt_hz)
      39             : {
      40             :     // load parameter values from eeprom
      41        1001 :     AP_Param::setup_object_defaults(this, var_info);
      42             : 
      43        1001 :     filt_hz(initial_filt_hz);
      44             : 
      45             :     // reset input filter to first value received
      46        1001 :     _flags._reset_filter = true;
      47        1001 : }
      48             : 
      49             : // set_dt - set time step in seconds
      50           1 : void AC_PI_2D::set_dt(float dt)
      51             : {
      52             :     // set dt and calculate the input filter alpha
      53           1 :     _dt = dt;
      54           1 :     calc_filt_alpha();
      55           1 : }
      56             : 
      57             : // filt_hz - set input filter hz
      58        1001 : void AC_PI_2D::filt_hz(float hz)
      59             : {
      60        1001 :     _filt_hz.set(fabsf(hz));
      61             : 
      62             :     // sanity check _filt_hz
      63        1001 :     _filt_hz.set(MAX(_filt_hz, AC_PI_2D_FILT_HZ_MIN));
      64             : 
      65             :     // calculate the input filter alpha
      66        1001 :     calc_filt_alpha();
      67        1001 : }
      68             : 
      69             : // set_input - set input to PID controller
      70             : //  input is filtered before the PID controllers are run
      71             : //  this should be called before any other calls to get_p, get_i or get_d
      72       49116 : void AC_PI_2D::set_input(const Vector2f &input)
      73             : {
      74             :     // don't process inf or NaN
      75       49116 :     if (!isfinite(input.x) || !isfinite(input.y)) {
      76           0 :         return;
      77             :     }
      78             : 
      79             :     // reset input filter to value received
      80       49116 :     if (_flags._reset_filter) {
      81           1 :         _flags._reset_filter = false;
      82           1 :         _input = input;
      83             :     }
      84             : 
      85             :     // update filter and calculate derivative
      86       49116 :     Vector2f input_filt_change = (input - _input) * _filt_alpha;
      87       49116 :     _input = _input + input_filt_change;
      88             : }
      89             : 
      90       70596 : Vector2f AC_PI_2D::get_p() const
      91             : {
      92       70596 :     return (_input * _kp);
      93             : }
      94             : 
      95       21480 : Vector2f AC_PI_2D::get_i()
      96             : {
      97       21480 :     if (!is_zero(_ki) && !is_zero(_dt)) {
      98       21480 :         _integrator += (_input * _ki) * _dt;
      99       21480 :         const float integrator_length = _integrator.length();
     100       21480 :         if ((integrator_length > _imax) && (is_positive(integrator_length))) {
     101           0 :             _integrator *= (_imax / integrator_length);
     102             :         }
     103       21480 :         return _integrator;
     104             :     }
     105           0 :     return Vector2f{};
     106             : }
     107             : 
     108             : // get_i_shrink - get_i but do not allow integrator to grow in length (it may shrink)
     109           0 : Vector2f AC_PI_2D::get_i_shrink()
     110             : {
     111           0 :     if (!is_zero(_ki) && !is_zero(_dt)) {
     112           0 :         const float integrator_length_orig = MIN(_integrator.length(),_imax);
     113           0 :         _integrator += (_input * _ki) * _dt;
     114           0 :         const float integrator_length_new = _integrator.length();
     115           0 :         if ((integrator_length_new > integrator_length_orig) && is_positive(integrator_length_new)) {
     116           0 :             _integrator *= (integrator_length_orig / integrator_length_new);
     117             :         }
     118           0 :         return _integrator;
     119             :     }
     120           0 :     return Vector2f{};
     121             : }
     122             : 
     123       21480 : Vector2f AC_PI_2D::get_pi()
     124             : {
     125       21480 :     return get_p() + get_i();
     126             : }
     127             : 
     128        1601 : void AC_PI_2D::reset_I()
     129             : {
     130        1601 :     _integrator.zero();
     131        1601 : }
     132             : 
     133             : // Loads controller configuration from EEPROM, including gains and filter frequencies. (not used)
     134           0 : void AC_PI_2D::load_gains()
     135             : {
     136           0 :     _kp.load();
     137           0 :     _ki.load();
     138           0 :     _imax.load();
     139           0 :     _imax.set(fabsf(_imax));
     140           0 :     _filt_hz.load();
     141             : 
     142             :     // calculate the input filter alpha
     143           0 :     calc_filt_alpha();
     144           0 : }
     145             : 
     146             : // Saves controller configuration from EEPROM, including gains and filter frequencies. (not used)
     147           0 : void AC_PI_2D::save_gains()
     148             : {
     149           0 :     _kp.save();
     150           0 :     _ki.save();
     151           0 :     _imax.save();
     152           0 :     _filt_hz.save();
     153           0 : }
     154             : 
     155             : // calc_filt_alpha - recalculate the input filter alpha
     156        1002 : void AC_PI_2D::calc_filt_alpha()
     157             : {
     158        1002 :     if (is_zero(_filt_hz)) {
     159           0 :         _filt_alpha = 1.0f;
     160           0 :         return;
     161             :     }
     162             :   
     163             :     // calculate alpha
     164        1002 :     const float rc = 1/(M_2PI*_filt_hz);
     165        1002 :     _filt_alpha = _dt / (_dt + rc);
     166             : }