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

AP_NavEKF: remove sleep on init of EKF

Browse Source 
this prevents HIL from locking up, and also prevents a possible 1s
delay in flight on EKF init
This commit is contained in:
Andrew Tridgell 2014-02-19 22:04:12 +11:00
parent b434c11215
commit 20b0444c15
1 changed files with 3 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -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();