2012-01-29 01:21:43 -04:00
/// @file AC_PID.cpp
/// @brief Generic PID algorithm
2015-08-11 03:28:41 -03:00
# include <AP_Math/AP_Math.h>
2012-01-29 01:21:43 -04:00
# include "AC_PID.h"
2023-07-27 10:58:50 -03:00
# define AC_PID_DEFAULT_NOTCH_ATTENUATION 40
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AC_PID : : var_info [ ] = {
2012-12-10 09:28:39 -04:00
// @Param: P
// @DisplayName: PID Proportional Gain
// @Description: P Gain which produces an output value that is proportional to the current error value
2022-12-29 22:20:32 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER ( " P " , 0 , AC_PID , _kp , default_kp ) ,
2015-02-11 08:06:04 -04:00
2012-12-10 09:28:39 -04: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
2022-12-29 22:20:32 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER ( " I " , 1 , AC_PID , _ki , default_ki ) ,
2015-02-11 08:06:04 -04:00
2012-12-10 09:28:39 -04: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
2022-12-29 22:20:32 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER ( " D " , 2 , AC_PID , _kd , default_kd ) ,
2015-02-11 08:06:04 -04:00
// 3 was for uint16 IMAX
2019-06-27 06:33:15 -03:00
// @Param: FF
// @DisplayName: FF FeedForward Gain
// @Description: FF Gain which produces an output value that is proportional to the demanded input
2022-12-29 22:20:32 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER ( " FF " , 4 , AC_PID , _kff , default_kff ) ,
2015-02-11 08:06:04 -04:00
2012-12-10 09:28:39 -04:00
// @Param: IMAX
// @DisplayName: PID Integral Maximum
// @Description: The maximum/minimum value that the I term can output
2022-12-29 22:20:32 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER ( " IMAX " , 5 , AC_PID , _kimax , default_kimax ) ,
2019-06-27 06:33:15 -03:00
// 6 was for float FILT
// 7 is for float ILMI and FF
2015-02-11 08:06:04 -04:00
2019-06-27 06:33:15 -03:00
// index 8 was for AFF
// @Param: FLTT
// @DisplayName: PID Target filter frequency in Hz
// @Description: Target filter frequency in Hz
2016-02-17 22:10:53 -04:00
// @Units: Hz
2022-12-29 22:20:32 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER ( " FLTT " , 9 , AC_PID , _filt_T_hz , default_filt_T_hz ) ,
2019-06-27 06:33:15 -03:00
// @Param: FLTE
// @DisplayName: PID Error filter frequency in Hz
// @Description: Error filter frequency in Hz
// @Units: Hz
2022-12-29 22:20:32 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER ( " FLTE " , 10 , AC_PID , _filt_E_hz , default_filt_E_hz ) ,
2019-06-27 06:33:15 -03:00
// @Param: FLTD
// @DisplayName: PID Derivative term filter frequency in Hz
// @Description: Derivative filter frequency in Hz
// @Units: Hz
2022-12-29 22:20:32 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER ( " FLTD " , 11 , AC_PID , _filt_D_hz , default_filt_D_hz ) ,
2015-02-11 08:06:04 -04: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
2022-12-29 22:20:32 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER ( " SMAX " , 12 , AC_PID , _slew_rate_max , default_slew_rate_max ) ,
2020-09-24 00:28:24 -03:00
2023-09-18 22:46:00 -03:00
// @Param: PDMX
// @DisplayName: PD sum maximum
// @Description: The maximum/minimum value that the sum of the P and D term can output
// @User: Advanced
AP_GROUPINFO ( " PDMX " , 13 , AC_PID , _kpdmax , 0 ) ,
2023-07-27 10:58:50 -03:00
# if AC_PID_ADVANCED_ENABLED
// @Param: ADV
// @DisplayName: Advanced parameters enable
// @Description: Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO_FLAGS ( " ADV " , 32 , AC_PID , _adv_enable , 0 , AP_PARAM_FLAG_ENABLE ) ,
// @Param: D_FF
// @DisplayName: PID Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
// @Range: 0 0.02
// @Increment: 0.0001
// @User: Advanced
AP_GROUPINFO_FLAGS_DEFAULT_POINTER ( " D_FF " , 33 , AC_PID , _kdff , default_kdff ) ,
// @Param: NTF
// @DisplayName: PID Target notch Filter center frequency
// @Description: PID Target notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @User: Advanced
AP_GROUPINFO ( " NTF " , 34 , AC_PID , _notch_T_center_freq_hz , 0 ) ,
// @Param: NEF
// @DisplayName: PID Error notch Filter center frequency
// @Description: PID Error notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @User: Advanced
AP_GROUPINFO ( " NEF " , 35 , AC_PID , _notch_E_center_freq_hz , 0 ) ,
// @Param: NBW
// @DisplayName: PID notch Filter bandwidth
// @Description: PID notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
AP_GROUPINFO ( " NBW " , 36 , AC_PID , _notch_bandwidth_hz , 0 ) ,
// @Param: NATT
// @DisplayName: PID notch Filter attenuation
// @Description: PID notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @User: Advanced
AP_GROUPINFO ( " NATT " , 37 , AC_PID , _notch_attenuation_dB , 40 ) ,
# endif
2012-08-17 02:37:26 -03:00
AP_GROUPEND
2012-02-12 07:25:50 -04:00
} ;
2012-01-29 01:21:43 -04:00
2015-02-11 08:06:04 -04:00
// Constructor
2020-09-24 00:28:24 -03:00
AC_PID : : AC_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 ,
2023-07-27 10:58:50 -03:00
float initial_srmax , float initial_srtau , float initial_dff ) :
2022-12-29 22:20:32 -04:00
default_kp ( initial_p ) ,
default_ki ( initial_i ) ,
default_kd ( initial_d ) ,
default_kff ( initial_ff ) ,
default_kimax ( initial_imax ) ,
default_filt_T_hz ( initial_filt_T_hz ) ,
default_filt_E_hz ( initial_filt_E_hz ) ,
default_filt_D_hz ( initial_filt_D_hz ) ,
2023-07-27 10:58:50 -03:00
default_slew_rate_max ( initial_srmax ) ,
default_kdff ( initial_dff )
2015-02-11 08:06:04 -04:00
{
// load parameter values from eeprom
AP_Param : : setup_object_defaults ( this , var_info ) ;
2022-12-29 22:20:32 -04:00
// this param is not in the table, so its default is no loaded in the call above
_slew_rate_tau . set ( initial_srtau ) ;
2015-02-11 08:06:04 -04:00
// reset input filter to first value received
_flags . _reset_filter = true ;
2015-05-21 22:43:43 -03:00
memset ( & _pid_info , 0 , sizeof ( _pid_info ) ) ;
2021-03-31 21:12:11 -03:00
// slew limit scaler allows for plane to use degrees/sec slew
// limit
_slew_limit_scale = 1 ;
2015-02-11 08:06:04 -04:00
}
2019-06-27 06:33:15 -03:00
// filt_T_hz - set target filter hz
void AC_PID : : filt_T_hz ( float hz )
{
_filt_T_hz . set ( fabsf ( hz ) ) ;
}
// filt_E_hz - set error filter hz
void AC_PID : : filt_E_hz ( float hz )
2015-02-11 08:06:04 -04:00
{
2019-06-27 06:33:15 -03:00
_filt_E_hz . set ( fabsf ( hz ) ) ;
}
2015-03-11 02:16:26 -03:00
2019-06-27 06:33:15 -03:00
// filt_D_hz - set derivative filter hz
void AC_PID : : filt_D_hz ( float hz )
{
_filt_D_hz . set ( fabsf ( hz ) ) ;
2015-02-11 08:06:04 -04:00
}
2021-11-05 17:52:42 -03:00
// slew_limit - set slew limit
void AC_PID : : slew_limit ( float smax )
{
_slew_rate_max . set ( fabsf ( smax ) ) ;
}
2023-07-27 10:58:50 -03:00
void AC_PID : : set_notch_sample_rate ( float sample_rate )
{
# if AC_PID_ADVANCED_ENABLED
if ( ! _adv_enable ) {
return ;
}
if ( ! is_zero ( _notch_T_center_freq_hz . get ( ) ) & &
( ! is_equal ( sample_rate , _target_notch . sample_freq_hz ( ) )
| | ! is_equal ( _notch_T_center_freq_hz . get ( ) , _target_notch . center_freq_hz ( ) ) ) ) {
_target_notch . init ( sample_rate , _notch_T_center_freq_hz , _notch_bandwidth_hz , _notch_attenuation_dB ) ;
}
if ( ! is_zero ( _notch_E_center_freq_hz . get ( ) ) & &
( ! is_equal ( sample_rate , _error_notch . sample_freq_hz ( ) )
| | ! is_equal ( _notch_E_center_freq_hz . get ( ) , _error_notch . center_freq_hz ( ) ) ) ) {
_error_notch . init ( sample_rate , _notch_E_center_freq_hz , _notch_bandwidth_hz , _notch_attenuation_dB ) ;
}
# endif
}
2019-06-27 06:33:15 -03:00
// update_all - set target and measured inputs to PID controller and calculate outputs
// target and error are filtered
// the derivative is then calculated and filtered
// the integral is then updated based on the setting of the limit flag
2022-05-28 12:58:25 -03:00
float AC_PID : : update_all ( float target , float measurement , float dt , bool limit , float boost )
2015-02-11 08:06:04 -04:00
{
2015-04-09 00:06:07 -03:00
// don't process inf or NaN
2019-06-27 06:33:15 -03:00
if ( ! isfinite ( target ) | | ! isfinite ( measurement ) ) {
return 0.0f ;
2015-04-09 00:06:07 -03:00
}
2015-03-29 07:02:46 -03:00
2015-04-09 00:06:07 -03:00
// reset input filter to value received
if ( _flags . _reset_filter ) {
_flags . _reset_filter = false ;
2019-06-27 06:33:15 -03:00
_target = target ;
_error = _target - measurement ;
2015-04-09 00:06:07 -03:00
_derivative = 0.0f ;
2023-07-27 10:58:50 -03:00
_target_derivative = 0.0f ;
# if AC_PID_ADVANCED_ENABLED
if ( _adv_enable ) {
if ( ! is_zero ( _notch_T_center_freq_hz . get ( ) ) ) {
_target_notch . reset ( ) ;
_target = _target_notch . apply ( _target ) ;
}
if ( ! is_zero ( _notch_E_center_freq_hz . get ( ) ) ) {
_error_notch . reset ( ) ;
_error = _error_notch . apply ( _error ) ;
}
}
# endif
2019-06-27 06:33:15 -03:00
} else {
float error_last = _error ;
2023-07-27 10:58:50 -03:00
float target_last = _target ;
float error = _target - measurement ;
# if AC_PID_ADVANCED_ENABLED
if ( _adv_enable ) {
// apply notch filters before FTLD/FLTE to avoid shot noise
if ( ! is_zero ( _notch_T_center_freq_hz . get ( ) ) ) {
target = _target_notch . apply ( target ) ;
}
if ( ! is_zero ( _notch_E_center_freq_hz . get ( ) ) ) {
error = _error_notch . apply ( error ) ;
}
}
# endif
2022-12-02 08:34:53 -04:00
_target + = get_filt_T_alpha ( dt ) * ( target - _target ) ;
2023-07-27 10:58:50 -03:00
_error + = get_filt_E_alpha ( dt ) * ( error - _error ) ;
2019-06-27 06:33:15 -03:00
// calculate and filter derivative
2022-12-02 08:34:53 -04:00
if ( is_positive ( dt ) ) {
float derivative = ( _error - error_last ) / dt ;
_derivative + = get_filt_D_alpha ( dt ) * ( derivative - _derivative ) ;
2023-07-27 10:58:50 -03:00
_target_derivative = ( _target - target_last ) / dt ;
2019-06-27 06:33:15 -03:00
}
2015-04-09 00:06:07 -03:00
}
2019-06-27 06:33:15 -03:00
// update I term
2022-12-02 08:34:53 -04:00
update_i ( dt , limit ) ;
2019-06-27 06:33:15 -03:00
float P_out = ( _error * _kp ) ;
float D_out = ( _derivative * _kd ) ;
2020-09-24 00:28:24 -03:00
// calculate slew limit modifier for P+D
2022-12-02 08:34:53 -04:00
_pid_info . Dmod = _slew_limiter . modifier ( ( _pid_info . P + _pid_info . D ) * _slew_limit_scale , dt ) ;
2021-04-04 06:49:31 -03:00
_pid_info . slew_rate = _slew_limiter . get_slew_rate ( ) ;
2020-09-24 00:28:24 -03:00
P_out * = _pid_info . Dmod ;
D_out * = _pid_info . Dmod ;
2022-05-28 12:58:25 -03:00
// boost output if required
P_out * = boost ;
D_out * = boost ;
2023-10-11 15:02:55 -03:00
_pid_info . PD_limit = false ;
2023-09-12 16:51:54 -03:00
// Apply PD sum limit if enabled
if ( is_positive ( _kpdmax ) ) {
const float PD_sum_abs = fabsf ( P_out + D_out ) ;
if ( PD_sum_abs > _kpdmax ) {
const float PD_scale = _kpdmax / PD_sum_abs ;
P_out * = PD_scale ;
D_out * = PD_scale ;
_pid_info . PD_limit = true ;
}
}
2019-06-27 06:33:15 -03:00
_pid_info . target = _target ;
_pid_info . actual = measurement ;
_pid_info . error = _error ;
_pid_info . P = P_out ;
_pid_info . D = D_out ;
2023-09-18 22:46:00 -03:00
return P_out + D_out + _integrator ;
2015-02-11 08:06:04 -04:00
}
2019-06-27 06:33:15 -03:00
// update_error - set error input to PID controller and calculate outputs
// target is set to zero and error is set and filtered
// the derivative then is calculated and filtered
// the integral is then updated based on the setting of the limit flag
// Target and Measured must be set manually for logging purposes.
// todo: remove function when it is no longer used.
2022-12-02 08:34:53 -04:00
float AC_PID : : update_error ( float error , float dt , bool limit )
2012-01-29 01:21:43 -04:00
{
2015-04-09 00:06:07 -03:00
// don't process inf or NaN
2019-06-27 06:33:15 -03:00
if ( ! isfinite ( error ) ) {
return 0.0f ;
2015-04-09 00:06:07 -03:00
}
2023-09-23 14:42:47 -03:00
// Reuse update all code path, zero target and pass negative error as measurement
// Passing as measurement bypasses any target filtering to maintain behaviour
// Negate as update all calculates error as target - measurement
_target = 0.0 ;
const float output = update_all ( 0.0 , - error , dt , limit ) ;
2019-06-27 06:33:15 -03:00
2023-09-23 14:42:47 -03:00
// Make sure logged target and actual are still 0 to maintain behaviour
_pid_info . target = 0.0 ;
_pid_info . actual = 0.0 ;
2023-09-12 16:51:54 -03:00
2023-09-23 14:42:47 -03:00
return output ;
2015-02-11 08:06:04 -04:00
}
2019-06-27 06:33:15 -03:00
// update_i - update the integral
// If the limit flag is set the integral is only allowed to shrink
2022-12-02 08:34:53 -04:00
void AC_PID : : update_i ( float dt , bool limit )
2015-02-11 08:06:04 -04:00
{
2022-12-02 08:34:53 -04:00
if ( ! is_zero ( _ki ) & & is_positive ( dt ) ) {
2019-06-27 06:33:15 -03:00
// Ensure that integrator can only be reduced if the output is saturated
if ( ! limit | | ( ( is_positive ( _integrator ) & & is_negative ( _error ) ) | | ( is_negative ( _integrator ) & & is_positive ( _error ) ) ) ) {
2022-12-02 08:34:53 -04:00
_integrator + = ( ( float ) _error * _ki ) * dt ;
2019-06-27 06:33:15 -03:00
_integrator = constrain_float ( _integrator , - _kimax , _kimax ) ;
2012-08-17 02:37:26 -03:00
}
2019-06-27 06:33:15 -03:00
} else {
_integrator = 0.0f ;
2012-08-17 02:37:26 -03:00
}
2019-06-27 06:33:15 -03:00
_pid_info . I = _integrator ;
2021-01-02 14:04:11 -04:00
_pid_info . limit = limit ;
2012-01-29 01:21:43 -04:00
}
2019-06-27 06:33:15 -03:00
float AC_PID : : get_p ( ) const
2012-01-29 01:21:43 -04:00
{
2019-06-27 06:33:15 -03:00
return _error * _kp ;
2012-01-29 01:21:43 -04:00
}
2019-06-27 06:33:15 -03:00
float AC_PID : : get_i ( ) const
2017-02-10 01:25:27 -04:00
{
2019-06-27 06:33:15 -03:00
return _integrator ;
2017-02-10 01:25:27 -04:00
}
2019-06-27 06:33:15 -03:00
float AC_PID : : get_d ( ) const
{
return _kd * _derivative ;
}
2017-02-10 01:25:27 -04:00
2019-06-27 06:33:15 -03:00
float AC_PID : : get_ff ( )
2012-01-29 01:21:43 -04:00
{
2023-07-27 10:58:50 -03:00
_pid_info . FF = _target * _kff
# if AC_PID_ADVANCED_ENABLED
+ _target_derivative * _kdff
# endif
;
return _pid_info . FF ;
2012-01-29 01:21:43 -04:00
}
2014-02-14 02:52:24 -04:00
void AC_PID : : reset_I ( )
2012-01-29 01:21:43 -04:00
{
2022-01-08 07:10:48 -04:00
_integrator = 0.0 ;
2020-12-01 11:02:54 -04:00
}
2014-02-14 02:52:24 -04:00
void AC_PID : : load_gains ( )
2012-01-29 01:21:43 -04:00
{
2012-08-17 02:37:26 -03:00
_kp . load ( ) ;
_ki . load ( ) ;
_kd . load ( ) ;
2019-06-27 06:33:15 -03:00
_kff . load ( ) ;
_kimax . load ( ) ;
2022-07-05 00:08:55 -03:00
_kimax . set ( fabsf ( _kimax ) ) ;
2023-09-18 22:46:00 -03:00
_kpdmax . load ( ) ;
_kpdmax . set ( fabsf ( _kpdmax ) ) ;
2019-06-27 06:33:15 -03:00
_filt_T_hz . load ( ) ;
_filt_E_hz . load ( ) ;
_filt_D_hz . load ( ) ;
2023-07-27 10:58:50 -03:00
# if AC_PID_ADVANCED_ENABLED
_kdff . load ( ) ;
_notch_T_center_freq_hz . load ( ) ;
_notch_E_center_freq_hz . load ( ) ;
_notch_bandwidth_hz . load ( ) ;
_notch_attenuation_dB . load ( ) ;
# endif
2012-01-29 01:21:43 -04:00
}
2015-02-11 08:06:04 -04:00
// save_gains - save gains to eeprom
2014-02-14 02:52:24 -04:00
void AC_PID : : save_gains ( )
2012-01-29 01:21:43 -04:00
{
2012-08-17 02:37:26 -03:00
_kp . save ( ) ;
_ki . save ( ) ;
_kd . save ( ) ;
2019-06-27 06:33:15 -03:00
_kff . save ( ) ;
_kimax . save ( ) ;
_filt_T_hz . save ( ) ;
_filt_E_hz . save ( ) ;
_filt_D_hz . save ( ) ;
2023-07-27 10:58:50 -03:00
# if AC_PID_ADVANCED_ENABLED
_kdff . save ( ) ;
_notch_T_center_freq_hz . save ( ) ;
_notch_E_center_freq_hz . save ( ) ;
_notch_bandwidth_hz . save ( ) ;
_notch_attenuation_dB . save ( ) ;
# endif
2015-02-11 08:06:04 -04:00
}
/// Overload the function call operator to permit easy initialisation
2023-07-27 10:58:50 -03:00
void AC_PID : : operator ( ) ( float p_val , float i_val , float d_val , float ff_val , float imax_val , float input_filt_T_hz , float input_filt_E_hz , float input_filt_D_hz , float dff_val )
2015-02-11 08:06:04 -04:00
{
2022-07-05 00:08:55 -03:00
_kp . set ( p_val ) ;
_ki . set ( i_val ) ;
_kd . set ( d_val ) ;
_kff . set ( ff_val ) ;
_kimax . set ( fabsf ( imax_val ) ) ;
_filt_T_hz . set ( input_filt_T_hz ) ;
_filt_E_hz . set ( input_filt_E_hz ) ;
_filt_D_hz . set ( input_filt_D_hz ) ;
2023-07-27 10:58:50 -03:00
# if AC_PID_ADVANCED_ENABLED
_kdff . set ( dff_val ) ;
# endif
2012-01-29 01:21:43 -04:00
}
2014-04-17 18:03:53 -03:00
2019-06-27 06:33:15 -03:00
// get_filt_T_alpha - get the target filter alpha
2022-12-02 08:34:53 -04:00
float AC_PID : : get_filt_T_alpha ( float dt ) const
2019-06-27 06:33:15 -03:00
{
2022-12-02 08:34:53 -04:00
return calc_lowpass_alpha_dt ( dt , _filt_T_hz ) ;
2019-06-27 06:33:15 -03:00
}
// get_filt_E_alpha - get the error filter alpha
2022-12-02 08:34:53 -04:00
float AC_PID : : get_filt_E_alpha ( float dt ) const
2015-03-11 02:16:26 -03:00
{
2022-12-02 08:34:53 -04:00
return calc_lowpass_alpha_dt ( dt , _filt_E_hz ) ;
2019-06-27 06:33:15 -03:00
}
// get_filt_D_alpha - get the derivative filter alpha
2022-12-02 08:34:53 -04:00
float AC_PID : : get_filt_D_alpha ( float dt ) const
2019-06-27 06:33:15 -03:00
{
2022-12-02 08:34:53 -04:00
return calc_lowpass_alpha_dt ( dt , _filt_D_hz ) ;
2014-04-17 18:03:53 -03:00
}
2019-06-27 06:33:15 -03:00
2022-01-08 07:10:48 -04:00
void AC_PID : : set_integrator ( float target , float measurement , float integrator )
2019-06-27 06:33:15 -03:00
{
2022-01-08 07:10:48 -04:00
set_integrator ( target - measurement , integrator ) ;
}
void AC_PID : : set_integrator ( float error , float integrator )
{
_integrator = constrain_float ( integrator - error * _kp , - _kimax , _kimax ) ;
2019-06-27 06:33:15 -03:00
}
2022-01-08 07:10:48 -04:00
void AC_PID : : set_integrator ( float integrator )
2019-06-27 06:33:15 -03:00
{
2022-01-08 07:10:48 -04:00
_integrator = constrain_float ( integrator , - _kimax , _kimax ) ;
2019-10-16 05:10:28 -03:00
}
2022-12-02 08:34:53 -04:00
void AC_PID : : relax_integrator ( float integrator , float dt , float time_constant )
2019-10-16 05:10:28 -03:00
{
2022-01-08 07:10:48 -04:00
integrator = constrain_float ( integrator , - _kimax , _kimax ) ;
2022-12-02 08:34:53 -04:00
if ( is_positive ( dt ) ) {
_integrator = _integrator + ( integrator - _integrator ) * ( dt / ( dt + time_constant ) ) ;
}
2019-06-27 06:33:15 -03:00
}