From b330a6b24e72e4137d6fc310d8aad2f3a9f1b77d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Oct 2021 18:49:34 +1100 Subject: [PATCH] APM_Control: suppress roll/pitch D term in ground_mode prevent oscillations which are quite common --- libraries/APM_Control/AP_PitchController.cpp | 12 ++++++++---- libraries/APM_Control/AP_PitchController.h | 6 +++--- libraries/APM_Control/AP_RollController.cpp | 12 ++++++++---- libraries/APM_Control/AP_RollController.h | 6 +++--- 4 files changed, 22 insertions(+), 14 deletions(-) diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index 48ef93d2f7..f6331722ad 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -144,7 +144,7 @@ AP_PitchController::AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWin /* AC_PID based rate controller */ -int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed) +int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed, bool ground_mode) { const float dt = AP::scheduler().get_loop_period_s(); const float eas2tas = _ahrs.get_EAS2TAS(); @@ -196,6 +196,10 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool // sum components float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D; + if (ground_mode) { + // when on ground suppress D and half P term to prevent oscillations + out -= pinfo.D + 0.5*pinfo.P; + } // remember the last output to trigger the I limit _last_out = out; @@ -226,7 +230,7 @@ int32_t AP_PitchController::get_rate_out(float desired_rate, float scaler) // If no airspeed available use average of min and max aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max)); } - return _get_rate_out(desired_rate, scaler, false, aspeed); + return _get_rate_out(desired_rate, scaler, false, aspeed, false); } /* @@ -278,7 +282,7 @@ float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inv // 4) minimum FBW airspeed (metres/sec) // 5) maximum FBW airspeed (metres/sec) // -int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool disable_integrator) +int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode) { // Calculate offset to pitch rate demand required to maintain pitch angle whilst banking // Calculate ideal turn rate from bank angle and airspeed assuming a level coordinated turn @@ -331,7 +335,7 @@ int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool desired_rate *= (1 - roll_prop); } - return _get_rate_out(desired_rate, scaler, disable_integrator, aspeed); + return _get_rate_out(desired_rate, scaler, disable_integrator, aspeed, ground_mode); } void AP_PitchController::reset_I() diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index aa81552136..6e341e093f 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -16,8 +16,8 @@ public: AP_PitchController(const AP_PitchController &other) = delete; AP_PitchController &operator=(const AP_PitchController&) = delete; - int32_t get_rate_out(float desired_rate, float scaler); - int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator); + int32_t get_rate_out(float desired_rate, float scaler); + int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode); void reset_I(); @@ -58,7 +58,7 @@ private: AP_Logger::PID_Info _pid_info; - int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed); + int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed, bool ground_mode); float _get_coordination_rate_offset(float &aspeed, bool &inverted) const; AP_AHRS &_ahrs; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index 101bc2750e..fe862c4c81 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -129,7 +129,7 @@ AP_RollController::AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing /* AC_PID based rate controller */ -int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator) +int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, bool ground_mode) { const float dt = AP::scheduler().get_loop_period_s(); const float eas2tas = _ahrs.get_EAS2TAS(); @@ -185,6 +185,10 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool // sum components float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D; + if (ground_mode) { + // when on ground suppress D term to prevent oscillations + out -= pinfo.D + 0.5*pinfo.P; + } // remember the last output to trigger the I limit _last_out = out; @@ -207,7 +211,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool */ int32_t AP_RollController::get_rate_out(float desired_rate, float scaler) { - return _get_rate_out(desired_rate, scaler, false); + return _get_rate_out(desired_rate, scaler, false, false); } /* @@ -219,7 +223,7 @@ int32_t AP_RollController::get_rate_out(float desired_rate, float scaler) 3) boolean which is true when stabilise mode is active 4) minimum FBW airspeed (metres/sec) */ -int32_t AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool disable_integrator) +int32_t AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode) { if (gains.tau < 0.05f) { gains.tau.set(0.05f); @@ -236,7 +240,7 @@ int32_t AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool d desired_rate = gains.rmax_pos; } - return _get_rate_out(desired_rate, scaler, disable_integrator); + return _get_rate_out(desired_rate, scaler, disable_integrator, ground_mode); } void AP_RollController::reset_I() diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index 9e3f8b8652..2a08de985a 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -16,8 +16,8 @@ public: AP_RollController(const AP_RollController &other) = delete; AP_RollController &operator=(const AP_RollController&) = delete; - int32_t get_rate_out(float desired_rate, float scaler); - int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator); + int32_t get_rate_out(float desired_rate, float scaler); + int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode); void reset_I(); @@ -63,7 +63,7 @@ private: AP_Logger::PID_Info _pid_info; - int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator); + int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator, bool ground_mode); AP_AHRS &_ahrs; };