Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: suppress roll/pitch D term in ground_mode

Browse Source 
prevent oscillations which are quite common
This commit is contained in:
Andrew Tridgell 2021-10-29 21:37:15 +11:00
parent dfd7dfc1af
commit 8a73bdcbe6
4 changed files with 20 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/APM_Control/AP_PitchController.cpp
View File

@ -144,7 +144,7 @@ AP_PitchController::AP_PitchController(const AP_Vehicle::FixedWing &parms)
/* /*
AC_PID based rate controller AC_PID based rate controller
*/ */
float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed) float 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 dt = AP::scheduler().get_loop_period_s();
@ -199,6 +199,10 @@ float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool d
// sum components // sum components
float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D; 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 // remember the last output to trigger the I limit
_last_out = out; _last_out = out;
@ -229,7 +233,7 @@ float AP_PitchController::get_rate_out(float desired_rate, float scaler)
// If no airspeed available use average of min and max // If no airspeed available use average of min and max
aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_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);
} }
/* /*
@ -282,7 +286,7 @@ float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inv
// 4) minimum FBW airspeed (metres/sec) // 4) minimum FBW airspeed (metres/sec)
// 5) maximum FBW airspeed (metres/sec) // 5) maximum FBW airspeed (metres/sec)
// //
float AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool disable_integrator) float 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 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 // Calculate ideal turn rate from bank angle and airspeed assuming a level coordinated turn
@ -336,7 +340,7 @@ float AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool di
desired_rate *= (1 - roll_prop); 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() void AP_PitchController::reset_I()

4
libraries/APM_Control/AP_PitchController.h
View File

@ -16,7 +16,7 @@ public:
AP_PitchController &operator=(const AP_PitchController&) = delete; AP_PitchController &operator=(const AP_PitchController&) = delete;
float get_rate_out(float desired_rate, float scaler); float get_rate_out(float desired_rate, float scaler);
float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator); float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode);
void reset_I(); void reset_I();
@ -56,6 +56,6 @@ private:
AP_Logger::PID_Info _pid_info; AP_Logger::PID_Info _pid_info;
float _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed); float _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; float _get_coordination_rate_offset(float &aspeed, bool &inverted) const;
}; };

12
libraries/APM_Control/AP_RollController.cpp
View File

@ -129,7 +129,7 @@ AP_RollController::AP_RollController(const AP_Vehicle::FixedWing &parms)
/* /*
AC_PID based rate controller AC_PID based rate controller
*/ */
float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator) float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, bool ground_mode)
{ {
const AP_AHRS &_ahrs = AP::ahrs(); const AP_AHRS &_ahrs = AP::ahrs();
@ -187,6 +187,10 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di
// sum components // sum components
float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D; 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 // remember the last output to trigger the I limit
_last_out = out; _last_out = out;
@ -209,7 +213,7 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di
*/ */
float AP_RollController::get_rate_out(float desired_rate, float scaler) float 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);
} }
/* /*
@ -221,7 +225,7 @@ float AP_RollController::get_rate_out(float desired_rate, float scaler)
3) boolean which is true when stabilise mode is active 3) boolean which is true when stabilise mode is active
4) minimum FBW airspeed (metres/sec) 4) minimum FBW airspeed (metres/sec)
*/ */
float AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool disable_integrator) float AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode)
{ {
if (gains.tau < 0.05f) { if (gains.tau < 0.05f) {
gains.tau.set(0.05f); gains.tau.set(0.05f);
@ -238,7 +242,7 @@ float AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool dis
desired_rate = gains.rmax_pos; 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() void AP_RollController::reset_I()

4
libraries/APM_Control/AP_RollController.h
View File

@ -16,7 +16,7 @@ public:
AP_RollController &operator=(const AP_RollController&) = delete; AP_RollController &operator=(const AP_RollController&) = delete;
float get_rate_out(float desired_rate, float scaler); float get_rate_out(float desired_rate, float scaler);
float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator); float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode);
void reset_I(); void reset_I();
@ -61,5 +61,5 @@ private:
AP_Logger::PID_Info _pid_info; AP_Logger::PID_Info _pid_info;
float _get_rate_out(float desired_rate, float scaler, bool disable_integrator); float _get_rate_out(float desired_rate, float scaler, bool disable_integrator, bool ground_mode);
}; };