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

AP_NavEKF: prevent a possible numerical error on startup

Browse Source 
fixes issue #1294
This commit is contained in:
Andrew Tridgell 2014-08-08 15:55:20 +10:00
parent e22ab50b16
commit 3ee3a71644
1 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -511,13 +511,16 @@ void NavEKF::InitialiseFilterBootstrap(void)
readMagData();
// normalise the acceleration vector
initAccVec.normalize();
float pitch=0, roll=0;
if (initAccVec.length() > 0.001f) {
initAccVec.normalize();
// calculate initial pitch angle
float pitch = asinf(initAccVec.x);
// calculate initial pitch angle
pitch = asinf(initAccVec.x);
// calculate initial roll angle
float roll = -asinf(initAccVec.y / cosf(pitch));
// calculate initial roll angle
roll = -asinf(initAccVec.y / cosf(pitch));
}
// calculate initial orientation and earth magnetic field states
Quaternion initQuat;