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

AP_NavEKF: allow initialisation before GPS lock to aid indoor testing

This commit is contained in:
Paul Riseborough 2014-01-18 11:57:59 +11:00 committed by Andrew Tridgell
parent 7fb60812c2
commit b4171853b1
2 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -52,8 +52,8 @@ void AP_AHRS_NavEKF::update(void)
_dcm_attitude(roll, pitch, yaw); _dcm_attitude(roll, pitch, yaw);
if (!ekf_started) { if (!ekf_started) {
// if we have GPS lock and a compass set we can start the EKF // if we have a compass set we can start the EKF
if (get_compass() && _gps && _gps->status() >= GPS::GPS_OK_FIX_3D) { if (get_compass()) {
if (start_time_ms == 0) { if (start_time_ms == 0) {
start_time_ms = hal.scheduler->millis(); start_time_ms = hal.scheduler->millis();
} }

16
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -383,9 +383,9 @@ void NavEKF::UpdateFilter()
void NavEKF::SelectVelPosFusion() void NavEKF::SelectVelPosFusion()
{ {
// Command fusion of GPS measurements if new ones available // Command fusion of GPS measurements if new ones available or in static mode
readGpsData(); readGpsData();
if (newDataGps) { if (newDataGps||staticMode) {
fuseVelData = true; fuseVelData = true;
fusePosData = true; fusePosData = true;
// Calculate the scale factor to be applied to the measurement variance to account for // Calculate the scale factor to be applied to the measurement variance to account for
@ -398,9 +398,9 @@ void NavEKF::SelectVelPosFusion()
fuseVelData = false; fuseVelData = false;
fusePosData = false; fusePosData = false;
} }
// Command fusion of height measurements if new ones available // Command fusion of height measurements if new ones available or in static mode
readHgtData(); readHgtData();
if (newDataHgt) if (newDataHgt||staticMode)
{ {
fuseHgtData = true; fuseHgtData = true;
// Calculate the scale factor to be applied to the measurement variance to account for // Calculate the scale factor to be applied to the measurement variance to account for
@ -1337,6 +1337,10 @@ void NavEKF::FuseVelPosNED()
{ {
posHealth = true; posHealth = true;
posFailTime = hal.scheduler->millis(); posFailTime = hal.scheduler->millis();
if (posTimeout)
{
ResetPosition();
}
} }
else else
{ {
@ -1360,6 +1364,10 @@ void NavEKF::FuseVelPosNED()
{ {
hgtHealth = true; hgtHealth = true;
hgtFailTime = hal.scheduler->millis(); hgtFailTime = hal.scheduler->millis();
if (hgtTimeout)
{
ResetPosition();
}
} }
else else
{ {