From 31580d82eea900ca627a9f1d8d9b0bca666d133f Mon Sep 17 00:00:00 2001 From: bnsgeyer <bnsgeyer@aol.com> Date: Tue, 1 Dec 2020 10:02:54 -0500 Subject: [PATCH] AC_PID: add support to smoothly reset the integrator --- libraries/AC_PID/AC_PID.cpp | 17 +++++++++++++++++ libraries/AC_PID/AC_PID.h | 6 ++++++ 2 files changed, 23 insertions(+) diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 8ea4a5fb09..088dc78974 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -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(); diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index 631eb1c28f..2bf1dea4d5 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -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; };