From b4171853b1d33191fc03c5ea51b8562798c23111 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 18 Jan 2014 11:57:59 +1100 Subject: [PATCH] AP_NavEKF: allow initialisation before GPS lock to aid indoor testing --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 4 ++-- libraries/AP_NavEKF/AP_NavEKF.cpp | 16 ++++++++++++---- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 4740d063d4..372ca4dfdc 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -52,8 +52,8 @@ void AP_AHRS_NavEKF::update(void) _dcm_attitude(roll, pitch, yaw); if (!ekf_started) { - // if we have GPS lock and a compass set we can start the EKF - if (get_compass() && _gps && _gps->status() >= GPS::GPS_OK_FIX_3D) { + // if we have a compass set we can start the EKF + if (get_compass()) { if (start_time_ms == 0) { start_time_ms = hal.scheduler->millis(); } diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 8f7f72efbf..7ab30cfdc6 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -383,9 +383,9 @@ void NavEKF::UpdateFilter() 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(); - if (newDataGps) { + if (newDataGps||staticMode) { fuseVelData = true; fusePosData = true; // Calculate the scale factor to be applied to the measurement variance to account for @@ -398,9 +398,9 @@ void NavEKF::SelectVelPosFusion() fuseVelData = 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(); - if (newDataHgt) + if (newDataHgt||staticMode) { fuseHgtData = true; // Calculate the scale factor to be applied to the measurement variance to account for @@ -1337,6 +1337,10 @@ void NavEKF::FuseVelPosNED() { posHealth = true; posFailTime = hal.scheduler->millis(); + if (posTimeout) + { + ResetPosition(); + } } else { @@ -1360,6 +1364,10 @@ void NavEKF::FuseVelPosNED() { hgtHealth = true; hgtFailTime = hal.scheduler->millis(); + if (hgtTimeout) + { + ResetPosition(); + } } else {