diff --git a/libraries/AC_PID/AC_HELI_PID.cpp b/libraries/AC_PID/AC_HELI_PID.cpp index 0b1971f112..4b5cb56884 100644 --- a/libraries/AC_PID/AC_HELI_PID.cpp +++ b/libraries/AC_PID/AC_HELI_PID.cpp @@ -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. diff --git a/libraries/AC_PID/AC_HELI_PID.h b/libraries/AC_PID/AC_HELI_PID.h index 235931fabd..fc8bb29e5e 100644 --- a/libraries/AC_PID/AC_HELI_PID.h +++ b/libraries/AC_PID/AC_HELI_PID.h @@ -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 };