From c76e74338b289ad9d8ef29ffe7cc229f8af79d0b Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 24 Jan 2025 14:52:30 +0100 Subject: [PATCH] ekf-replay: fix airspeed replay If available, the EKF uses airspeed_validated, not airspeed --- msg/Ekf2Timestamps.msg | 1 + src/modules/ekf2/EKF2.cpp | 4 ++++ src/modules/logger/logged_topics.cpp | 1 + src/modules/replay/ReplayEkf2.cpp | 6 ++++++ src/modules/replay/ReplayEkf2.hpp | 1 + 5 files changed, 13 insertions(+) diff --git a/msg/Ekf2Timestamps.msg b/msg/Ekf2Timestamps.msg index ae3ac067669f..2487cf0200f2 100644 --- a/msg/Ekf2Timestamps.msg +++ b/msg/Ekf2Timestamps.msg @@ -14,6 +14,7 @@ int16 RELATIVE_TIMESTAMP_INVALID = 32767 # (0x7fff) If one of the relative times # difference of +-3.2s to the sensor_combined topic. int16 airspeed_timestamp_rel +int16 airspeed_validated_timestamp_rel int16 distance_sensor_timestamp_rel int16 optical_flow_timestamp_rel int16 vehicle_air_data_timestamp_rel diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 4fcafaceddae..ae7aad04068a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -745,6 +745,7 @@ void EKF2::Run() ekf2_timestamps_s ekf2_timestamps { .timestamp = now, .airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, + .airspeed_validated_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, .distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, .optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, .vehicle_air_data_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, @@ -2080,6 +2081,9 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) } _airspeed_validated_timestamp_last = airspeed_validated.timestamp; + + ekf2_timestamps.airspeed_validated_timestamp_rel = (int16_t)((int64_t)airspeed_validated.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); } } else if (((ekf2_timestamps.timestamp - _airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) { diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3e91b2461975..4b1da7d3a169 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -294,6 +294,7 @@ void LoggedTopics::add_estimator_replay_topics() // current EKF2 subscriptions add_topic("airspeed"); + add_topic("airspeed_validated"); add_topic("vehicle_optical_flow"); add_topic("sensor_combined"); add_topic("sensor_selection"); diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 89f4874ce545..e03072b59e9e 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -39,6 +39,7 @@ // for ekf2 replay #include +#include #include #include #include @@ -91,6 +92,9 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) } else if (sub.orb_meta == ORB_ID(airspeed)) { _airspeed_msg_id = msg_id; + } else if (sub.orb_meta == ORB_ID(airspeed_validated)) { + _airspeed_validated_msg_id = msg_id; + } else if (sub.orb_meta == ORB_ID(distance_sensor)) { _distance_sensor_msg_id = msg_id; @@ -138,6 +142,7 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs }; handle_sensor_publication(ekf2_timestamps.airspeed_timestamp_rel, _airspeed_msg_id); + handle_sensor_publication(ekf2_timestamps.airspeed_validated_timestamp_rel, _airspeed_validated_msg_id); handle_sensor_publication(ekf2_timestamps.distance_sensor_timestamp_rel, _distance_sensor_msg_id); handle_sensor_publication(ekf2_timestamps.optical_flow_timestamp_rel, _optical_flow_msg_id); handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id); @@ -225,6 +230,7 @@ ReplayEkf2::onExitMainLoop() PX4_INFO("Topic, Num Published, Num Error (no timestamp match found):"); print_sensor_statistics(_airspeed_msg_id, "airspeed"); + print_sensor_statistics(_airspeed_validated_msg_id, "airspeed_validated"); print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor"); print_sensor_statistics(_optical_flow_msg_id, "vehicle_optical_flow"); print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined"); diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp index 13cb3d8be5d5..519b4242d20f 100644 --- a/src/modules/replay/ReplayEkf2.hpp +++ b/src/modules/replay/ReplayEkf2.hpp @@ -82,6 +82,7 @@ class ReplayEkf2 : public Replay static constexpr uint16_t msg_id_invalid = 0xffff; uint16_t _airspeed_msg_id = msg_id_invalid; + uint16_t _airspeed_validated_msg_id = msg_id_invalid; uint16_t _distance_sensor_msg_id = msg_id_invalid; uint16_t _optical_flow_msg_id = msg_id_invalid; uint16_t _sensor_combined_msg_id = msg_id_invalid;