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

AP_NavEKF: Ensure initialisation methods read required sensor data

Browse Source
This commit is contained in:
priseborough 2014-12-22 08:20:09 +11:00 committed by Randy Mackay
parent 5d1de79f08
commit 0dc2356175
1 changed files with 15 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -549,10 +549,15 @@ void NavEKF::InitialiseFilterDynamic(void)
state.accel_zbias1 = 0;
state.accel_zbias2 = 0;
state.wind_vel.zero();
// read the GPS and set the position and velocity states
readGpsData();
ResetVelocity();
ResetPosition();
// read the barometer and set the height state
readHgtData();
ResetHeight();
state.body_magfield.zero();
// set stored states to current state
StoreStatesReset();
@ -621,12 +626,6 @@ void NavEKF::InitialiseFilterBootstrap(void)
Quaternion initQuat;
initQuat = calcQuatAndFieldStates(roll, pitch);
// read the GPS
readGpsData();
// read the barometer
readHgtData();
// check on ground status
SetFlightAndFusionModes();
@ -638,6 +637,15 @@ void NavEKF::InitialiseFilterBootstrap(void)
state.wind_vel.zero();
state.body_magfield.zero();
// read the GPS and set the position and velocity states
readGpsData();
ResetVelocity();
ResetPosition();
// read the barometer and set the height state
readHgtData();
ResetHeight();
// set stored states to current state
StoreStatesReset();