2014-05-02 18:12:24 -03:00
/// @file AC_HELI_PID.cpp
/// @brief Generic PID algorithm
2015-08-11 03:28:41 -03:00
# include <AP_Math/AP_Math.h>
2014-05-02 18:12:24 -03:00
# include "AC_HELI_PID.h"
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AC_HELI_PID : : var_info [ ] = {
2014-05-02 18:12:24 -03:00
// @Param: P
// @DisplayName: PID Proportional Gain
// @Description: P Gain which produces an output value that is proportional to the current error value
AP_GROUPINFO ( " P " , 0 , AC_HELI_PID , _kp , 0 ) ,
2015-01-23 06:05:40 -04:00
2014-05-02 18:12:24 -03:00
// @Param: I
// @DisplayName: PID Integral Gain
// @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error
AP_GROUPINFO ( " I " , 1 , AC_HELI_PID , _ki , 0 ) ,
2015-01-23 06:05:40 -04:00
2014-05-02 18:12:24 -03:00
// @Param: D
// @DisplayName: PID Derivative Gain
// @Description: D Gain which produces an output that is proportional to the rate of change of the error
AP_GROUPINFO ( " D " , 2 , AC_HELI_PID , _kd , 0 ) ,
2015-01-23 06:05:40 -04:00
2019-06-27 06:33:49 -03:00
// 3 was for uint16 IMAX
2022-02-01 19:46:30 -04:00
// @Param: FF
// @DisplayName: FF FeedForward Gain
// @Description: FF Gain which produces an output value that is proportional to the demanded input
AP_GROUPINFO ( " FF " , 4 , AC_HELI_PID , _kff , 0 ) ,
2015-01-23 06:05:40 -04:00
// @Param: IMAX
// @DisplayName: PID Integral Maximum
// @Description: The maximum/minimum value that the I term can output
2019-06-27 06:33:49 -03:00
AP_GROUPINFO ( " IMAX " , 5 , AC_HELI_PID , _kimax , 0 ) ,
2015-01-23 06:05:40 -04:00
2019-06-27 06:33:49 -03:00
// 6 was for float FILT
2015-05-22 21:00:17 -03:00
2016-02-15 02:24:52 -04:00
// @Param: ILMI
2015-09-23 20:27:26 -03:00
// @DisplayName: I-term Leak Minimum
// @Description: Point below which I-term will not leak down
2016-02-17 07:03:37 -04:00
// @Range: 0 1
2015-09-23 20:27:26 -03:00
// @User: Advanced
2016-02-15 02:24:52 -04:00
AP_GROUPINFO ( " ILMI " , 7 , AC_HELI_PID , _leak_min , AC_PID_LEAK_MIN ) ,
2015-09-23 20:27:26 -03:00
2019-06-27 06:33:49 -03:00
// 8 was for float AFF
// @Param: FLTT
// @DisplayName: PID Target filter frequency in Hz
// @Description: Target filter frequency in Hz
// @Units: Hz
AP_GROUPINFO ( " FLTT " , 9 , AC_HELI_PID , _filt_T_hz , AC_PID_TFILT_HZ_DEFAULT ) ,
// @Param: FLTE
// @DisplayName: PID Error filter frequency in Hz
// @Description: Error filter frequency in Hz
// @Units: Hz
AP_GROUPINFO ( " FLTE " , 10 , AC_HELI_PID , _filt_E_hz , AC_PID_EFILT_HZ_DEFAULT ) ,
// @Param: FLTD
// @DisplayName: PID D term filter frequency in Hz
// @Description: Derivative filter frequency in Hz
// @Units: Hz
AP_GROUPINFO ( " FLTD " , 11 , AC_HELI_PID , _filt_D_hz , AC_PID_DFILT_HZ_DEFAULT ) ,
2015-09-23 20:27:26 -03:00
2020-09-24 00:28:24 -03:00
// @Param: SMAX
// @DisplayName: Slew rate limit
// @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.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
AP_GROUPINFO ( " SMAX " , 12 , AC_HELI_PID , _slew_rate_max , 0 ) ,
2014-05-02 18:12:24 -03:00
AP_GROUPEND
} ;
2015-01-23 06:05:40 -04:00
/// Constructor for PID
2022-12-02 08:34:53 -04:00
AC_HELI_PID : : AC_HELI_PID ( float initial_p , float initial_i , float initial_d , float initial_ff , float initial_imax , float initial_filt_T_hz , float initial_filt_E_hz , float initial_filt_D_hz ) :
AC_PID ( initial_p , initial_i , initial_d , initial_ff , initial_imax , initial_filt_T_hz , initial_filt_E_hz , initial_filt_D_hz )
2015-01-23 06:05:40 -04:00
{
2015-05-22 21:00:17 -03:00
_last_requested_rate = 0 ;
2015-01-23 06:05:40 -04:00
}
2014-05-02 18:12:24 -03:00
// This is an integrator which tends to decay to zero naturally
// if the error is zero.
2019-06-27 06:33:49 -03:00
void AC_HELI_PID : : update_leaky_i ( float leak_rate )
2014-05-02 18:12:24 -03:00
{
2022-12-02 08:34:53 -04:00
if ( ! is_zero ( _ki ) ) {
2015-09-23 20:27:26 -03:00
// integrator does not leak down below Leak Min
if ( _integrator > _leak_min ) {
_integrator - = ( float ) ( _integrator - _leak_min ) * leak_rate ;
} else if ( _integrator < - _leak_min ) {
_integrator - = ( float ) ( _integrator + _leak_min ) * leak_rate ;
}
2015-05-21 22:43:43 -03:00
_pid_info . I = _integrator ;
}
2014-05-02 18:12:24 -03:00
}