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

L1_Control: Ensure that LIM_BANK passes a sea level sanity check

Browse Source 
This fixes #6637 where the LIM_BANK can be set such that the vehicle demands a radius at sea level that is much higher then the loiter radius was configured to be. This effectively feels like a fly away and is due to bad parameters usually. If this happens just fall back to the simple EAS2TAS scaling of the radius.
This commit is contained in:
Michael du Breuil 2017-07-29 13:50:54 -07:00 committed by Andrew Tridgell
parent 96ad5e8f62
commit 52c98b56cb
1 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -158,8 +158,14 @@ float AP_L1_Control::loiter_radius(const float radius) const
return radius * eas2tas_sq; return radius * eas2tas_sq;
} else { } else {
float sea_level_radius = sq(nominal_velocity_sea_level) / lateral_accel_sea_level; float sea_level_radius = sq(nominal_velocity_sea_level) / lateral_accel_sea_level;
// select the requested radius, or the required altitude scale, whichever is safer if (sea_level_radius > radius) {
return MAX(sea_level_radius * eas2tas_sq, radius); // If we've told the plane that its sea level radius is unachievable fallback to
// straight altitude scaling
return radius * eas2tas_sq;
} else {
// select the requested radius, or the required altitude scale, whichever is safer
return MAX(sea_level_radius * eas2tas_sq, radius);
}
} }
} }