aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_HELI_PID: Add Leak-Min param and functionality.

Browse Source
This commit is contained in:
Robert Lefebvre 2015-09-23 19:27:26 -04:00 committed by Randy Mackay
parent b487d66d9e
commit 6cc40f9760
2 changed files with 18 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_PID/AC_HELI_PID.cpp
View File

@ -37,6 +37,13 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] PROGMEM = {
// @Description: // @Description:
AP_GROUPINFO("FILT_HZ", 6, AC_HELI_PID, _filt_hz, AC_PID_FILT_HZ_DEFAULT), AP_GROUPINFO("FILT_HZ", 6, AC_HELI_PID, _filt_hz, AC_PID_FILT_HZ_DEFAULT),
// @Param: I_L_MIN
// @DisplayName: I-term Leak Minimum
// @Description: Point below which I-term will not leak down
// @Range: 0 4500
// @User: Advanced
AP_GROUPINFO("I_L_MIN", 7, AC_HELI_PID, _leak_min, AC_PID_LEAK_MIN),
AP_GROUPEND AP_GROUPEND
}; };
@ -59,7 +66,14 @@ float AC_HELI_PID::get_vff(float requested_rate)
float AC_HELI_PID::get_leaky_i(float leak_rate) float AC_HELI_PID::get_leaky_i(float leak_rate)
{ {
if(!is_zero(_ki) && !is_zero(_dt)){ if(!is_zero(_ki) && !is_zero(_dt)){
_integrator -= (float)_integrator * leak_rate;
// integrator does not leak down below Leak Min
if (_integrator > _leak_min){
_integrator -= (float)(_integrator - _leak_min) * leak_rate;
} else if (_integrator < -_leak_min) {
_integrator -= (float)(_integrator + _leak_min) * leak_rate;
}
_integrator += ((float)_input * _ki) * _dt; _integrator += ((float)_input * _ki) * _dt;
if (_integrator < -_imax) { if (_integrator < -_imax) {
_integrator = -_imax; _integrator = -_imax;

3
libraries/AC_PID/AC_HELI_PID.h
View File

@ -12,6 +12,8 @@
#include <math.h> #include <math.h>
#include "AC_PID.h" #include "AC_PID.h"
#define AC_PID_LEAK_MIN 500.0 // Default I-term Leak Minimum
/// @class AC_HELI_PID /// @class AC_HELI_PID
/// @brief Heli PID control class /// @brief Heli PID control class
class AC_HELI_PID : public AC_PID { class AC_HELI_PID : public AC_PID {
@ -34,6 +36,7 @@ public:
private: private:
AP_Float _vff; AP_Float _vff;
AP_Float _leak_min;
}; };