Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -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 // body magnetic field vector with offsets removed
Vector3f initMagXYZ; Vector3f initMagXYZ;
// Take 50 readings at 20msec intervals and average // we should average readings over several calls to this function
initAccVec.zero(); initAccVec = _ahrs->get_ins().get_accel();
initMagXYZ.zero(); initMagXYZ = _ahrs->get_compass()->get_field() * 0.001f; // convert from Gauss to mGauss
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;
// Normalise the acceleration vector // Normalise the acceleration vector
initAccVec.normalize(); initAccVec.normalize();