aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM_Control: reduce elevator when past LIM_ROLL_CD

Browse Source 
this reduces elevator control when rolled over hard in fixed
wing. Using the elevator when on the side just caused earth frame yaw
and is counter productive. It can also prevent some aircraft from
recovering from inverted flight.
This commit is contained in:
Andrew Tridgell 2016-06-23 22:41:32 +10:00
parent a8e5255b90
commit 8a03b3ba22
1 changed files with 21 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/APM_Control/AP_PitchController.cpp
View File

@ -201,7 +201,27 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
}
_last_out += _pid_info.I;
/*
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 = fabsf(_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);
}
// Convert to centi-degrees and constrain
return constrain_float(_last_out * 100, -4500, 4500);
}