Browse Source

APM_Control: added decay_I() function

used by VTOL planes to decay integrator on fixed wing components when
at very low airspeed
master
Andrew Tridgell 6 years ago
parent
commit
05bd0cb9f4
  1. 8
      libraries/APM_Control/AP_PitchController.h
  2. 8
      libraries/APM_Control/AP_RollController.h
  3. 8
      libraries/APM_Control/AP_YawController.h

8
libraries/APM_Control/AP_PitchController.h

@ -26,6 +26,14 @@ public: @@ -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(); }

8
libraries/APM_Control/AP_RollController.h

@ -26,6 +26,14 @@ public: @@ -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(); }

8
libraries/APM_Control/AP_YawController.h

@ -26,6 +26,14 @@ public: @@ -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[];

Loading…
Cancel
Save