Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: apply pitch limiting at high roll to rate

Browse Source 
this fixes an issue with a large elevator trim error when rolled at
close to 90 degrees
This commit is contained in:
Andrew Tridgell 2021-04-03 14:12:46 +11:00
parent 4694820ac3
commit ccd7b15d06
1 changed files with 19 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/APM_Control/AP_PitchController.cpp
View File

@ -197,26 +197,6 @@ 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;
/*
when we are past the users defined roll limit for the
aircraft our priority should be to bring the aircraft back
within the roll limit. Using elevator for pitch control at
large roll angles is ineffective, and can be counter
productive as it induces earth-frame yaw which can reduce
the ability to roll. We linearly reduce elevator input when
beyond the configured roll limit, reducing to zero at 90
degrees
*/
float roll_wrapped = labs(_ahrs.roll_sensor);
if (roll_wrapped > 9000) {
roll_wrapped = 18000 - roll_wrapped;
}
if (roll_wrapped > aparm.roll_limit_cd + 500 && aparm.roll_limit_cd < 8500 &&
labs(_ahrs.pitch_sensor) < 7000) {
float roll_prop = (roll_wrapped - (aparm.roll_limit_cd+500)) / (float)(9000 - aparm.roll_limit_cd);
_last_out *= (1 - roll_prop);
}
// remember the last output to trigger the I limit
_last_out = out;
@ -334,6 +314,25 @@ int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool
// Apply the turn correction offset
desired_rate = desired_rate + rate_offset;
/*
when we are past the users defined roll limit for the aircraft
our priority should be to bring the aircraft back within the
roll limit. Using elevator for pitch control at large roll
angles is ineffective, and can be counter productive as it
induces earth-frame yaw which can reduce the ability to roll. We
linearly reduce pitch demanded rate when beyond the configured
roll limit, reducing to zero at 90 degrees
*/
float roll_wrapped = labs(_ahrs.roll_sensor);
if (roll_wrapped > 9000) {
roll_wrapped = 18000 - roll_wrapped;
}
const float roll_limit_margin = MIN(aparm.roll_limit_cd + 500.0, 8500.0);
if (roll_wrapped > roll_limit_margin && labs(_ahrs.pitch_sensor) < 7000) {
float roll_prop = (roll_wrapped - roll_limit_margin) / (float)(9000 - roll_limit_margin);
desired_rate *= (1 - roll_prop);
}
return _get_rate_out(desired_rate, scaler, disable_integrator, aspeed);
}