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

AP_NavEKF2: Don't use speed switch criteria when speed estimate is invalid

Browse Source 
Prevent unwanted switches from range finder to Baro height aiding due to triggering of the speed threshold
This commit is contained in:
priseborough 2016-12-09 08:39:17 +11:00 committed by Randy Mackay
parent 75edfdba79
commit 21c07d9abc
1 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -659,8 +659,16 @@ void NavEKF2_core::selectHeightForFusion()
// If the terrain height is consistent and we are moving slowly, then it can be
// used as a height reference in combination with a range finder
float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
bool dontTrustTerrain = (horizSpeed > 2.0f) || !terrainHgtStable;
bool trustTerrain = (horizSpeed < 1.0f) && terrainHgtStable;
bool dontTrustTerrain,trustTerrain;
if (filterStatus.flags.horiz_vel) {
// We can use the velocity
dontTrustTerrain = (horizSpeed > 2.0f) || !terrainHgtStable;
trustTerrain = (horizSpeed < 1.0f) && terrainHgtStable;
} else {
// We cant use the velocity
dontTrustTerrain = !terrainHgtStable;
trustTerrain = terrainHgtStable;
}
/*
* Switch between range finder and primary height source using height above ground and speed thresholds with