Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-12 01:23:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Add protection for negative height above ground

Browse Source
This commit is contained in:
priseborough 2014-11-16 16:28:43 +11:00 committed by Andrew Tridgell
parent 963cc4d60a
commit b1d44d4dde
1 changed files with 1 additions and 1 deletions

2
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3628,7 +3628,7 @@ void NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScal
{ {
if (useOptFlow() || gpsInhibitMode == 2) { if (useOptFlow() || gpsInhibitMode == 2) {
// allow 1.0 rad/sec margin for angular motion // allow 1.0 rad/sec margin for angular motion
ekfGndSpdLimit = max((_maxFlowRate - 1.0f), 0.0f) * (flowStates[1] - state.position[2]); ekfGndSpdLimit = max((_maxFlowRate - 1.0f), 0.0f) * max((flowStates[1] - state.position[2]), 0.1f);
// use standard gains up to 5.0 metres height and reduce above that // use standard gains up to 5.0 metres height and reduce above that
ekfNavVelGainScaler = 5.0f / max((flowStates[1] - state.position[2]),5.0f); ekfNavVelGainScaler = 5.0f / max((flowStates[1] - state.position[2]),5.0f);
} else { } else {