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

AP_NavEKF: Protect against possible div by 0

Browse Source
This commit is contained in:
Paul Riseborough 2016-03-01 17:51:04 +11:00 committed by Lucas De Marchi
parent 7d6b926749
commit 2888bdd6d5
1 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -628,7 +628,12 @@ void NavEKF2_core::fuseEulerYaw()
float t7 = q0*q3*2.0f;
float t8 = q1*q2*2.0f;
float t9 = t7+t8;
float t10 = 1.0f/(t6*t6);
float t10 = sq(t6);
if (t10 > 1e-6f) {
t10 = 1.0f / t10;
} else {
return;
}
float t11 = t9*t9;
float t12 = t10*t11;
float t13 = t12+1.0f;
@ -638,12 +643,7 @@ void NavEKF2_core::fuseEulerYaw()
} else {
return;
}
float t15;
if (fabsf(t6) > 1e-6) {
t15 = 1.0f/t6;
} else {
return;
}
float t15 = 1.0f/t6;
H_YAW[0] = 0.0f;
H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
@ -666,7 +666,12 @@ void NavEKF2_core::fuseEulerYaw()
float t7 = q0*q3*2.0f;
float t10 = q1*q2*2.0f;
float t8 = t7-t10;
float t9 = 1.0f/(t6*t6);
float t9 = sq(t6);
if (t9 > 1e-6f) {
t9 = 1.0f/t9;
} else {
return;
}
float t11 = t8*t8;
float t12 = t9*t11;
float t13 = t12+1.0f;
@ -676,12 +681,7 @@ void NavEKF2_core::fuseEulerYaw()
} else {
return;
}
float t15;
if (fabsf(t6) > 1e-6) {
t15 = 1.0f/t6;
} else {
return;
}
float t15 = 1.0f/t6;
H_YAW[0] = -t14*(t15*(q0*q2*2.0+q1*q3*2.0)-t8*t9*(q0*q1*2.0-q2*q3*2.0));
H_YAW[1] = 0.0f;
H_YAW[2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10));