Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM_Control: suppress roll/pitch D term in ground_mode 

prevent oscillations which are quite common
This commit is contained in:
Andrew Tridgell 2021-10-30 18:49:34 +11:00 committed by Randy Mackay
parent 17e646add9
commit b330a6b24e
4 changed files with 22 additions and 14 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(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()

6
libraries/APM_Control/AP_PitchController.h
View File

@ -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;

12
libraries/APM_Control/AP_RollController.cpp
View File

@ -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()

6
libraries/APM_Control/AP_RollController.h
View File

@ -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;
};