Line data    Source code

       1             : /// @file       AC_HELI_PID.cpp
       2             : /// @brief      Generic PID algorithm
       3             : 
       4             : #include <AP_Math/AP_Math.h>
       5             : #include "AC_HELI_PID.h"
       6             : 
       7             : const AP_Param::GroupInfo AC_HELI_PID::var_info[] = {
       8             :     // @Param: P
       9             :     // @DisplayName: PID 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_HELI_PID, _kp, default_kp),
      12             : 
      13             :     // @Param: I
      14             :     // @DisplayName: PID 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_HELI_PID, _ki, default_ki),
      17             : 
      18             :     // @Param: D
      19             :     // @DisplayName: PID Derivative Gain
      20             :     // @Description: D Gain which produces an output that is proportional to the rate of change of the error
      21             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("D", 2, AC_HELI_PID, _kd, default_kd),
      22             : 
      23             :     // 3 was for uint16 IMAX
      24             : 
      25             :     // @Param: FF
      26             :     // @DisplayName: FF FeedForward Gain
      27             :     // @Description: FF Gain which produces an output value that is proportional to the demanded input
      28             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FF", 4, AC_HELI_PID, _kff, default_kff),
      29             : 
      30             :     // @Param: IMAX
      31             :     // @DisplayName: PID Integral Maximum
      32             :     // @Description: The maximum/minimum value that the I term can output
      33             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("IMAX", 5, AC_HELI_PID, _kimax, default_kimax),
      34             : 
      35             :     // 6 was for float FILT
      36             : 
      37             :     // @Param: ILMI
      38             :     // @DisplayName: I-term Leak Minimum
      39             :     // @Description: Point below which I-term will not leak down
      40             :     // @Range: 0 1
      41             :     // @User: Advanced
      42             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("ILMI", 7, AC_HELI_PID, _leak_min, default_ilmi),
      43             : 
      44             :     // 8 was for float AFF
      45             : 
      46             :     // @Param: FLTT
      47             :     // @DisplayName: PID Target filter frequency in Hz
      48             :     // @Description: Low-pass filter frequency applied to the target input (Hz)
      49             :     // @Units: Hz
      50             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTT", 9, AC_HELI_PID, _filt_T_hz, default_filt_T_hz),
      51             : 
      52             :     // @Param: FLTE
      53             :     // @DisplayName: PID Error filter frequency in Hz
      54             :     // @Description: Low-pass filter frequency applied to the error (Hz)
      55             :     // @Units: Hz
      56             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTE", 10, AC_HELI_PID, _filt_E_hz, default_filt_E_hz),
      57             : 
      58             :     // @Param: FLTD
      59             :     // @DisplayName: PID Derivative term filter frequency in Hz
      60             :     // @Description: Low-pass filter frequency applied to the derivative (Hz)
      61             :     // @Units: Hz
      62             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTD", 11, AC_HELI_PID, _filt_D_hz, default_filt_D_hz),
      63             : 
      64             :     // @Param: SMAX
      65             :     // @DisplayName: Slew rate limit
      66             :     // @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
      67             :     // @Range: 0 200
      68             :     // @Increment: 0.5
      69             :     // @User: Advanced
      70             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("SMAX", 12, AC_HELI_PID, _slew_rate_max, default_slew_rate_max),
      71             : 
      72             :     // @Param: PDMX
      73             :     // @DisplayName: PD sum maximum
      74             :     // @Description: The maximum/minimum value that the sum of the P and D term can output
      75             :     // @User: Advanced
      76             :     AP_GROUPINFO("PDMX", 13, AC_HELI_PID, _kpdmax, 0),
      77             : 
      78             :     // @Param: D_FF
      79             :     // @DisplayName: PID Derivative FeedForward Gain
      80             :     // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
      81             :     // @Range: 0 0.02
      82             :     // @Increment: 0.0001
      83             :     // @User: Advanced
      84             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("D_FF", 14, AC_HELI_PID, _kdff, default_kdff),
      85             : 
      86             : #if AP_FILTER_ENABLED
      87             :     // @Param: NTF
      88             :     // @DisplayName: PID Target notch filter index
      89             :     // @Description: PID Target notch filter index, zero disables
      90             :     // @Range: 0 8
      91             :     // @User: Advanced
      92             :     AP_GROUPINFO("NTF", 15, AC_HELI_PID, _notch_T_filter, 0),
      93             : 
      94             :     // @Param: NEF
      95             :     // @DisplayName: PID Error notch filter index
      96             :     // @Description: PID Error notch filter index, zero disables
      97             :     // @Range: 0 8
      98             :     // @User: Advanced
      99             :     AP_GROUPINFO("NEF", 16, AC_HELI_PID, _notch_E_filter, 0),
     100             : #endif
     101             : 
     102             :     AP_GROUPEND
     103             : };
     104             : 
     105             : // Constructor
     106         249 : AC_HELI_PID::AC_HELI_PID(const AC_PID::Defaults &defaults,  float initial_ilmi) :
     107             :     AC_PID{defaults},
     108         249 :     default_ilmi(initial_ilmi)
     109             : {
     110             : 
     111             :     // load parameter values from eeprom
     112         249 :     AP_Param::setup_object_defaults(this, var_info);
     113             : 
     114         249 :     _last_requested_rate = 0;
     115         249 : }
     116             : 
     117             : // This is an integrator which tends to decay to zero naturally
     118             : // if the error is zero.
     119             : 
     120     6970781 : void AC_HELI_PID::update_leaky_i(float leak_rate)
     121             : {
     122     6970781 :     if (!is_zero(_ki)){
     123             : 
     124             :         // integrator does not leak down below Leak Min
     125     6970781 :         if (_integrator > _leak_min){
     126      138983 :             _integrator -= (float)(_integrator - _leak_min) * leak_rate;
     127     6831798 :         } else if (_integrator < -_leak_min) {
     128      235482 :             _integrator -= (float)(_integrator + _leak_min) * leak_rate;
     129             :         }
     130             : 
     131             :     }
     132     6970781 : }