fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 22:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Code format fix - replace 1E-x with 1e-x

Browse Source
This commit is contained in:
Paul Riseborough
2020-10-06 20:50:52 +11:00
committed by Paul Riseborough
parent e82d0af6d2
commit f666ebb995
11 changed files with 37 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_fusion.cpp
+8 -8
View File
@@ -436,10 +436,10 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
const float SA2 = SA0*q0 + SA1*q1;
const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
float SA4, SA5_inv;
if (sq(SA3) > 1E-6f) {
if (sq(SA3) > 1e-6f) {
SA4 = 1.0F/sq(SA3);
SA5_inv = sq(SA2)*SA4 + 1;
canUseA = fabsf(SA5_inv) > 1E-6f;
canUseA = fabsf(SA5_inv) > 1e-6f;
}
bool canUseB = false;
@@ -448,10 +448,10 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
const float SB2 = SB0*q3 + SB1*q2;
const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
float SB3, SB5_inv;
if (sq(SB2) > 1E-6f) {
if (sq(SB2) > 1e-6f) {
SB3 = 1.0F/sq(SB2);
SB5_inv = SB3*sq(SB4) + 1;
canUseB = fabsf(SB5_inv) > 1E-6f;
canUseB = fabsf(SB5_inv) > 1e-6f;
}
Vector4f H_YAW;
@@ -516,10 +516,10 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
const float SA2 = SA0*q0 - SA1*q1;
const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
float SA4, SA5_inv;
if (sq(SA3) > 1E-6f) {
if (sq(SA3) > 1e-6f) {
SA4 = 1.0F/sq(SA3);
SA5_inv = sq(SA2)*SA4 + 1;
canUseA = fabsf(SA5_inv) > 1E-6f;
canUseA = fabsf(SA5_inv) > 1e-6f;
}
bool canUseB = false;
@@ -528,10 +528,10 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
const float SB2 = -SB0*q3 + SB1*q2;
const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
float SB3, SB5_inv;
if (sq(SB2) > 1E-6f) {
if (sq(SB2) > 1e-6f) {
SB3 = 1.0F/sq(SB2);
SB5_inv = SB3*sq(SB4) + 1;
canUseB = fabsf(SB5_inv) > 1E-6f;
canUseB = fabsf(SB5_inv) > 1e-6f;
}
Vector4f H_YAW;