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

AC_PID: add support to smoothly reset the integrator

This commit is contained in:
bnsgeyer 2020-12-01 10:02:54 -05:00 committed by Andrew Tridgell
parent ff6621f3b6
commit 31580d82ee
2 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AC_PID/AC_PID.cpp
View File

@ -249,6 +249,23 @@ void AC_PID::reset_I()
_integrator = 0;
}
void AC_PID::reset_I_smoothly()
{
float reset_time = AC_PID_RESET_TC * 3.0f;
uint64_t now = AP_HAL::micros64();
if ((now - _reset_last_update) > 5e5 ) {
_reset_counter = 0;
}
if ((float)_reset_counter < (reset_time/_dt)) {
_integrator = _integrator - (_dt / (_dt + AC_PID_RESET_TC)) * _integrator;
_reset_counter++;
} else {
_integrator = 0;
}
_reset_last_update = now;
}
void AC_PID::load_gains()
{
_kp.load();

6
libraries/AC_PID/AC_PID.h
View File

@ -12,6 +12,7 @@
#define AC_PID_TFILT_HZ_DEFAULT 0.0f // default input filter frequency
#define AC_PID_EFILT_HZ_DEFAULT 0.0f // default input filter frequency
#define AC_PID_DFILT_HZ_DEFAULT 20.0f // default input filter frequency
#define AC_PID_RESET_TC 0.16f // Time constant for integrator reset decay to zero
/// @class AC_PID
/// @brief Copter PID control class
@ -56,6 +57,9 @@ public:
// reset_I - reset the integrator
void reset_I();
// reset_I - reset the integrator smoothly to zero within 0.5 seconds
void reset_I_smoothly();
// reset_filter - input filter will be reset to the next value provided to set_input()
void reset_filter() {
_flags._reset_filter = true;
@ -131,6 +135,8 @@ protected:
float _target; // target value to enable filtering
float _error; // error value to enable filtering
float _derivative; // derivative value to enable filtering
uint16_t _reset_counter; // loop counter for reset decay
uint64_t _reset_last_update; //time in microseconds of last update to reset_I
AP_Logger::PID_Info _pid_info;
};