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

AC_PID: added feed-forward to AC_PID

This commit is contained in:
Andrew Tridgell 2017-02-10 16:25:27 +11:00
parent 332735d2ee
commit b97bf5d15e
4 changed files with 20 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AC_PID/AC_HELI_PID.cpp
View File

@ -23,7 +23,7 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] = {
// @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),
AP_GROUPINFO("VFF", 4, AC_HELI_PID, _ff, 0),
// @Param: IMAX
// @DisplayName: PID Integral Maximum
@ -48,19 +48,13 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] = {
};
/// 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_vff) :
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_PID(initial_p, initial_i, initial_d, initial_imax, initial_filt_hz, dt)
{
_vff = initial_vff;
_ff = initial_ff;
_last_requested_rate = 0;
}
float AC_HELI_PID::get_vff(float requested_rate)
{
_pid_info.FF = (float)requested_rate * _vff;
return _pid_info.FF;
}
// This is an integrator which tends to decay to zero naturally
// if the error is zero.

3
libraries/AC_PID/AC_HELI_PID.h
View File

@ -19,9 +19,6 @@ 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_vff - return Velocity FeedForward Term
float get_vff(float requested_rate);
/// get_leaky_i - replacement for get_i but output is leaded at leak_rate
float get_leaky_i(float leak_rate);

12
libraries/AC_PID/AC_PID.cpp
View File

@ -34,6 +34,11 @@ const AP_Param::GroupInfo AC_PID::var_info[] = {
// @Units: Hz
AP_GROUPINFO("FILT", 6, AC_PID, _filt_hz, AC_PID_FILT_HZ_DEFAULT),
// @Param: FF
// @DisplayName: FF FeedForward Gain
// @Description: FF Gain which produces an output value that is proportional to the demanded input
AP_GROUPINFO("FF", 7, AC_PID, _ff, 0),
AP_GROUPEND
};
@ -153,6 +158,13 @@ float AC_PID::get_d()
return _pid_info.D;
}
float AC_PID::get_ff(float requested_rate)
{
_pid_info.FF = (float)requested_rate * _ff;
return _pid_info.FF;
}
float AC_PID::get_pi()
{
return get_p() + get_i();

9
libraries/AC_PID/AC_PID.h
View File

@ -39,7 +39,8 @@ public:
float get_p();
float get_i();
float get_d();
float get_ff(float requested_rate);
// reset_I - reset the integrator
void reset_I();
@ -62,7 +63,7 @@ public:
AP_Float &filt_hz() { return _filt_hz; }
float imax() const { return _imax.get(); }
float get_filt_alpha() const;
float ff() const { return _vff.get(); }
float ff() const { return _ff.get(); }
// set accessors
void kP(const float v) { _kp.set(v); }
@ -70,7 +71,7 @@ public:
void kD(const float v) { _kd.set(v); }
void imax(const float v) { _imax.set(fabsf(v)); }
void filt_hz(const float v);
void ff(const float v) { _vff.set(v); }
void ff(const float v) { _ff.set(v); }
float get_integrator() const { return _integrator; }
void set_integrator(float i) { _integrator = i; }
@ -91,7 +92,7 @@ protected:
AP_Float _kd;
AP_Float _imax;
AP_Float _filt_hz; // PID Input filter frequency in Hz
AP_Float _vff; // only used by heli
AP_Float _ff;
// flags
struct ac_pid_flags {