From cf7f3d8876822680b5e489a707d2f97af4fbbe5d Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Wed, 25 Oct 2023 15:04:56 +0200 Subject: [PATCH] ekf2: fix optical_flow_vel publication We should otherwise call this publication before the aid_src publisher that sets the timestamp. Having it separate avoids this ordering constraint. --- src/modules/ekf2/EKF2.cpp | 4 +++- src/modules/ekf2/EKF2.hpp | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 72d82441483b..18b5e5b3cb4a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2184,7 +2184,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) { const hrt_abstime timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample; - if ((timestamp_sample != 0) && (timestamp_sample > _status_optical_flow_pub_last)) { + if ((timestamp_sample != 0) && (timestamp_sample > _optical_flow_vel_pub_last)) { vehicle_optical_flow_vel_s flow_vel{}; flow_vel.timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample; @@ -2205,6 +2205,8 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_optical_flow_vel_pub.publish(flow_vel); + + _optical_flow_vel_pub_last = timestamp_sample; } } #endif // CONFIG_EKF2_OPTICAL_FLOW diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 4e48b4a85a48..88d1e59cbfe9 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -377,6 +377,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; hrt_abstime _status_optical_flow_pub_last{0}; + hrt_abstime _optical_flow_vel_pub_last{0}; perf_counter_t _msg_missed_optical_flow_perf{nullptr}; #endif // CONFIG_EKF2_OPTICAL_FLOW