Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_HELI_PID: Add Acceleration FF method.

This commit is contained in:
Robert Lefebvre 2015-05-22 20:00:17 -04:00 committed by Andrew Tridgell
parent 7ab628521e
commit 366f87c91c
2 changed files with 43 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AC_PID/AC_HELI_PID.cpp
View File

@ -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: FC
// @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, _ff, 0),
// @Param: VFF
// @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),
// @Param: IMAX
// @DisplayName: PID Integral Maximum
@ -36,23 +36,46 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] PROGMEM = {
// @DisplayName: PID Input filter frequency in Hz
// @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
};
/// Constructor for PID
AC_HELI_PID::AC_HELI_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_ff) :
AC_HELI_PID::AC_HELI_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_vff) :
AC_PID(initial_p, initial_i, initial_d, initial_imax, initial_filt_hz, dt)
{
_ff = initial_ff;
_vff = initial_vff;
_aff = 0;
_last_requested_rate = 0;
}
float AC_HELI_PID::get_ff(float requested_rate)
float AC_HELI_PID::get_vff(float requested_rate)
{
_pid_info.FF = (float)requested_rate * _ff;
_pid_info.desired = requested_rate;
_pid_info.FF = (float)requested_rate * _vff;
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;
_pid_info.desired = requested_rate;
return _pid_info.AFF;
}
// This is an integrator which tends to decay to zero naturally
// if the error is zero.

16
libraries/AC_PID/AC_HELI_PID.h
View File

@ -20,20 +20,26 @@ public:
/// Constructor for PID
AC_HELI_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_ff);
/// get_ff - return FeedForward Term
float get_ff(float requested_rate);
/// 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);
// accessors
float ff() const { return _ff.get(); }
void ff(const float v) { _ff.set(v); }
float ff() const { return _vff.get(); }
void ff(const float v) { _vff.set(v); }
static const struct AP_Param::GroupInfo var_info[];
private:
AP_Float _ff;
AP_Float _vff;
AP_Float _aff;
float _last_requested_rate; // Requested rate from last iteration, used to calculate rate change of requested rate
};