mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
APM_Control: suppress roll/pitch D term in ground_mode
prevent oscillations which are quite common
This commit is contained in:
parent
dfd7dfc1af
commit
8a73bdcbe6
@ -144,7 +144,7 @@ AP_PitchController::AP_PitchController(const AP_Vehicle::FixedWing &parms)
|
||||
/*
|
||||
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();
|
||||
|
||||
@ -199,6 +199,10 @@ float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool d
|
||||
|
||||
// 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;
|
||||
@ -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
|
||||
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)
|
||||
// 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 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);
|
||||
}
|
||||
|
||||
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()
|
||||
|
@ -16,7 +16,7 @@ public:
|
||||
AP_PitchController &operator=(const AP_PitchController&) = delete;
|
||||
|
||||
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();
|
||||
|
||||
@ -56,6 +56,6 @@ private:
|
||||
|
||||
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;
|
||||
};
|
||||
|
@ -129,7 +129,7 @@ AP_RollController::AP_RollController(const AP_Vehicle::FixedWing &parms)
|
||||
/*
|
||||
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();
|
||||
|
||||
@ -187,6 +187,10 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di
|
||||
|
||||
// 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;
|
||||
@ -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)
|
||||
{
|
||||
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
|
||||
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) {
|
||||
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;
|
||||
}
|
||||
|
||||
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()
|
||||
|
@ -16,7 +16,7 @@ public:
|
||||
AP_RollController &operator=(const AP_RollController&) = delete;
|
||||
|
||||
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();
|
||||
|
||||
@ -61,5 +61,5 @@ private:
|
||||
|
||||
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);
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user