diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index e80abdc75b..d959df31bc 100644 --- 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); }