From 20b0444c15869b7fbd1d2d77ea18aa97aa4a4abc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 19 Feb 2014 22:04:12 +1100 Subject: [PATCH] AP_NavEKF: remove sleep on init of EKF this prevents HIL from locking up, and also prevents a possible 1s delay in flight on EKF init --- libraries/AP_NavEKF/AP_NavEKF.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index b1b345dea3..febefe2103 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -389,15 +389,9 @@ void NavEKF::InitialiseFilterBootstrap(void) // body magnetic field vector with offsets removed Vector3f initMagXYZ; - // Take 50 readings at 20msec intervals and average - initAccVec.zero(); - initMagXYZ.zero(); - for (uint8_t i=1; i<=50; i++) { - initAccVec = initAccVec + _ahrs->get_ins().get_accel(); - initMagXYZ = initMagXYZ + _ahrs->get_compass()->get_field() * 0.001f; // convert from Gauss to mGauss - hal.scheduler->delay(20); - } - initMagXYZ = initMagXYZ * 0.02f; + // we should average readings over several calls to this function + initAccVec = _ahrs->get_ins().get_accel(); + initMagXYZ = _ahrs->get_compass()->get_field() * 0.001f; // convert from Gauss to mGauss // Normalise the acceleration vector initAccVec.normalize();