mirror of https://github.com/ArduPilot/ardupilot
AC_Heli_PID: Deprecate Accel Feedforward.
This commit is contained in:
parent
bfc10c1969
commit
0615d7a058
|
@ -22,10 +22,10 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] PROGMEM = {
|
|||
// @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),
|
||||
|
||||
// @Param: VFF
|
||||
// @Param: FF
|
||||
// @DisplayName: Velocity FF FeedForward Gain
|
||||
// @Description: Velocity FF Gain which produces an output value that is proportional to the demanded input
|
||||
AP_GROUPINFO("VFF", 4, AC_HELI_PID, _vff, 0),
|
||||
AP_GROUPINFO("FF", 4, AC_HELI_PID, _vff, 0),
|
||||
|
||||
// @Param: IMAX
|
||||
// @DisplayName: PID Integral Maximum
|
||||
|
@ -37,10 +37,6 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] PROGMEM = {
|
|||
// @Description:
|
||||
AP_GROUPINFO("FILT_HZ", 6, AC_HELI_PID, _filt_hz, AC_PID_FILT_HZ_DEFAULT),
|
||||
|
||||
// @Param: AFF
|
||||
// @DisplayName: Acceleration FF FeedForward Gain
|
||||
// @Description: Acceleration FF Gain which produces an output value that is proportional to the change in demanded input
|
||||
AP_GROUPINFO("AFF", 7, AC_HELI_PID, _aff, 0),
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
@ -49,8 +45,6 @@ AC_HELI_PID::AC_HELI_PID(float initial_p, float initial_i, float initial_d, floa
|
|||
AC_PID(initial_p, initial_i, initial_d, initial_imax, initial_filt_hz, dt)
|
||||
{
|
||||
_vff = initial_vff;
|
||||
_aff = 0;
|
||||
_last_requested_rate = 0;
|
||||
}
|
||||
|
||||
float AC_HELI_PID::get_vff(float requested_rate)
|
||||
|
@ -59,22 +53,6 @@ float AC_HELI_PID::get_vff(float requested_rate)
|
|||
return _pid_info.FF;
|
||||
}
|
||||
|
||||
float AC_HELI_PID::get_aff(float requested_rate)
|
||||
{
|
||||
float derivative;
|
||||
|
||||
// calculate derivative
|
||||
if (_dt > 0.0f) {
|
||||
derivative = (requested_rate - _last_requested_rate) / _dt;
|
||||
} else {
|
||||
derivative = 0;
|
||||
}
|
||||
|
||||
_pid_info.AFF = derivative * _aff;
|
||||
_last_requested_rate = requested_rate;
|
||||
return _pid_info.AFF;
|
||||
}
|
||||
|
||||
// This is an integrator which tends to decay to zero naturally
|
||||
// if the error is zero.
|
||||
|
||||
|
|
|
@ -22,9 +22,6 @@ public:
|
|||
|
||||
/// get_vff - return Velocity FeedForward Term
|
||||
float get_vff(float requested_rate);
|
||||
|
||||
/// get_avff - return Acceleration FeedForward Term
|
||||
float get_aff(float requested_rate);
|
||||
|
||||
/// get_leaky_i - replacement for get_i but output is leaded at leak_rate
|
||||
float get_leaky_i(float leak_rate);
|
||||
|
@ -37,9 +34,6 @@ public:
|
|||
|
||||
private:
|
||||
AP_Float _vff;
|
||||
AP_Float _aff;
|
||||
|
||||
float _last_requested_rate; // Requested rate from last iteration, used to calculate rate change of requested rate
|
||||
|
||||
};
|
||||
|
||||
|
|
Loading…
Reference in New Issue