Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

ekf2_replay: log groundtruth messages

Browse Source
This commit is contained in:
bresch
2024-12-06 15:03:55 +01:00
committed by Mathieu Bresciani
parent 3165a77e26
commit 3941b19968
2 changed files with 15 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/replay/ReplayEkf2.cpp
+12
View File
@@ -108,6 +108,15 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
} else if (sub.orb_meta == ORB_ID(aux_global_position)) {
_aux_global_position_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(vehicle_local_position_groundtruth)) {
_vehicle_local_position_groundtruth_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(vehicle_attitude_groundtruth)) {
_vehicle_attitude_groundtruth_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(vehicle_global_position_groundtruth)) {
_vehicle_global_position_groundtruth_msg_id = msg_id;
}
// the main loop should only handle publication of the following topics, the sensor topics are
@@ -135,6 +144,9 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs
handle_sensor_publication(ekf2_timestamps.vehicle_magnetometer_timestamp_rel, _vehicle_magnetometer_msg_id);
handle_sensor_publication(ekf2_timestamps.visual_odometry_timestamp_rel, _vehicle_visual_odometry_msg_id);
handle_sensor_publication(0, _aux_global_position_msg_id);
handle_sensor_publication(0, _vehicle_local_position_groundtruth_msg_id);
handle_sensor_publication(0, _vehicle_global_position_groundtruth_msg_id);
handle_sensor_publication(0, _vehicle_attitude_groundtruth_msg_id);
// sensor_combined: publish last because ekf2 is polling on this
if (!findTimestampAndPublish(ekf2_timestamps.timestamp / 100, _sensor_combined_msg_id, replay_file)) {
src/modules/replay/ReplayEkf2.hpp
+3
View File
@@ -89,6 +89,9 @@ private:
uint16_t _vehicle_magnetometer_msg_id = msg_id_invalid;
uint16_t _vehicle_visual_odometry_msg_id = msg_id_invalid;
uint16_t _aux_global_position_msg_id = msg_id_invalid;
uint16_t _vehicle_local_position_groundtruth_msg_id = msg_id_invalid;
uint16_t _vehicle_global_position_groundtruth_msg_id = msg_id_invalid;
uint16_t _vehicle_attitude_groundtruth_msg_id = msg_id_invalid;
};
} //namespace px4