Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Fix height drift on ground using range finder without GPS

This commit is contained in:
priseborough 2016-08-20 17:06:21 +10:00 committed by Randy Mackay
parent 8797714203
commit 60d8adcca0
1 changed files with 2 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -1336,10 +1336,8 @@ void NavEKF2_core::ConstrainStates()
for (uint8_t i=19; i<=21; i++) statesArray[i] = constrain_float(statesArray[i],-0.5f,0.5f);
// wind velocity limit 100 m/s (could be based on some multiple of max airspeed * EAS2TAS) - TODO apply circular limit
for (uint8_t i=22; i<=23; i++) statesArray[i] = constrain_float(statesArray[i],-100.0f,100.0f);
// constrain the terrain or vertical position state state depending on whether we are using the ground as the height reference
if (inhibitGndState) {
stateStruct.position.z = MIN(stateStruct.position.z, terrainState - rngOnGnd);
} else {
// constrain the terrain state to be below the vehicle height unless we are using terrain as the height datum
if (!inhibitGndState) {
terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
}
}