mag_fusion: prevent fusing default value of 0

PX4 · Jul 23, 2021 · 698c2aa · 698c2aa
1 parent b747307
commit 698c2aa
Showing 1 changed file with 10 additions and 0 deletions.
diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp
@@ -781,6 +781,11 @@ void Ekf::fuseHeading()
 				// Vehicle is at rest so use the last moving prediction as an observation
 				// to prevent the heading from drifting and to enable yaw gyro bias learning
 				// before takeoff.
+
+				if (fabsf(_last_static_yaw) < 1e-6f) {
+					_last_static_yaw = predicted_hdg;
+				}
+
 				measured_hdg = _last_static_yaw;
 
 			}
@@ -836,6 +841,11 @@ void Ekf::fuseHeading()
 				// Vehicle is at rest so use the last moving prediction as an observation
 				// to prevent the heading from drifting and to enable yaw gyro bias learning
 				// before takeoff.
+
+				if (fabsf(_last_static_yaw) < 1e-6f) {
+					_last_static_yaw = predicted_hdg;
+				}
+
 				measured_hdg = _last_static_yaw;
 
 			}