ekf-replay: fix airspeed replay

If available, the EKF uses airspeed_validated, not airspeed
PX4 · Jan 27, 2025 · c76e743 · c76e743
1 parent c3ba39f
commit c76e743
Show file tree

Hide file tree

Showing 5 changed files with 13 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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
@@ -39,6 +39,7 @@
 
 // for ekf2 replay
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/airspeed_validated.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/sensor_combined.h>
@@ -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
@@ -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;