dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
bresch 2023-10-25 15:04:56 +02:00 committed by Daniel Agar
parent f68f88b97c
commit eed2870fd8
2 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF2.cpp
View File

@ -2184,7 +2184,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
{
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 &timestamp)
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

1
src/modules/ekf2/EKF2.hpp
View File

@ -377,6 +377,7 @@ private:
uORB::PublicationMulti<estimator_aid_source2d_s> _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