diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 525b7b29c4..3f064b606d 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -26,6 +26,14 @@ public: void reset_I(); + /* + reduce the integrator, used when we have a low scale factor in a quadplane hover + */ + void decay_I() { + // this reduces integrator by 95% over 2s + _pid_info.I *= 0.995f; + } + void autotune_start(void) { autotune.start(); } void autotune_restore(void) { autotune.stop(); } diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index c0878f9d86..a32928ed9d 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -26,6 +26,14 @@ public: void reset_I(); + /* + reduce the integrator, used when we have a low scale factor in a quadplane hover + */ + void decay_I() { + // this reduces integrator by 95% over 2s + _pid_info.I *= 0.995f; + } + void autotune_start(void) { autotune.start(); } void autotune_restore(void) { autotune.stop(); } diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h index 8748dae5a3..46a04c71d4 100644 --- a/libraries/APM_Control/AP_YawController.h +++ b/libraries/APM_Control/AP_YawController.h @@ -26,6 +26,14 @@ public: void reset_I(); + /* + reduce the integrator, used when we have a low scale factor in a quadplane hover + */ + void decay_I() { + // this reduces integrator by 95% over 2s + _pid_info.I *= 0.995f; + } + const AP_Logger::PID_Info& get_pid_info(void) const {return _pid_info; } static const struct AP_Param::GroupInfo var_info[];