ekf2: revent yaw spikes on reset (#25972)

ttechnick · web-flow · commit c51502781f63 · 2025-12-18T09:08:44.000-09:00
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
@@ -748,7 +748,6 @@ void EKF2::Run()
 
 		// push imu data into estimator
 		_ekf.setIMUData(imu_sample_new);
-		PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor)
 
 		// integrate time to monitor time slippage
 		if (_start_time_us > 0) {
@@ -855,6 +854,9 @@ void EKF2::Run()
 #endif // CONFIG_EKF2_MAGNETOMETER
 		}
 
+		PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor)
+
+
 		// publish ekf2_timestamps
 		_ekf2_timestamps_pub.publish(ekf2_timestamps);
 	}

Original file line number	Diff line number	Diff line change
`@@ -748,7 +748,6 @@ void EKF2::Run()`
`748`	`748`
`749`	`749`	`// push imu data into estimator`
`750`	`750`	`_ekf.setIMUData(imu_sample_new);`
`751`		`- PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor)`
`752`	`751`
`753`	`752`	`// integrate time to monitor time slippage`
`754`	`753`	`if (_start_time_us > 0) {`
`@@ -855,6 +854,9 @@ void EKF2::Run()`
`855`	`854`	`#endif // CONFIG_EKF2_MAGNETOMETER`
`856`	`855`	`}`
`857`	`856`
	`857`	`+ PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor)`
	`858`	`+`
	`859`	`+`
`858`	`860`	`// publish ekf2_timestamps`
`859`	`861`	`_ekf2_timestamps_pub.publish(ekf2_timestamps);`
`860`	`862`	`}`