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

AP_NavEKF: Corrected height update timeout error

Browse Source
This commit is contained in:
Paul Riseborough 2013-12-28 20:41:07 +11:00 committed by Andrew Tridgell
parent 0f3ebb8e31
commit 4a7f81e50a
2 changed files with 11 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF/NavEKF.cpp
View File

@ -118,18 +118,22 @@ void SelectVelPosFusion()
fuseVelData = false;
fusePosData = false;
}
// Fuse height measurements with GPS measurements for efficiency
// Don't wait longer than HGTmsecTgt msec between height fusion steps
// Fuse height measurements at the same time as GPS measurements for efficiency
// Don't wait longer than HGTmsecTgt msec between height updates
if (statesInitialised && (newDataGps || ((IMUmsec - HGTmsecPrev) >= HGTmsecTgt)))
{
HGTmsecPrev = IMUmsec;
fuseHgtData = true;
readHgtData();
}
else
{
fuseHgtData = false;
// protect against wrap-around
if(IMUmsec < HGTmsecPrev) HGTmsecPrev = IMUmsec;
}
FuseVelposNED();
FuseVelPosNED();
fuseHgtData = false;
}
@ -1050,7 +1054,7 @@ void CovariancePrediction()
zeroCols(nextP,16,17);
}
// If the total position variance exceds 1E6 (1000m), then stop covariance
// If the total position variance exceeds 1E6 (1000m), then stop covariance
// growth by setting the predicted to the previous values
// This prevent an ill conditioned matrix from occurring for long periods
// without GPS
@ -1079,7 +1083,7 @@ void CovariancePrediction()
}
}
void FuseVelposNED()
void FuseVelPosNED()
{
// declare variables used by fault isolation logic

4
libraries/AP_NavEKF/NavEKF.h
View File

@ -59,11 +59,11 @@ public:
private:
void UpdateStrapdownEquationsNED();
void UpdateStrapdownEquationsNED();
void CovariancePrediction();
void FuseVelposNED();
void FuseVelPosNED();
void FuseMagnetometer();