fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 10:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: auxiliary position fusion

Browse Source 
Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
bresch
2023-11-29 12:19:18 +01:00
committed by Mathieu Bresciani
parent aaefc36cad
commit fe7988672f
22 changed files with 379 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/replay/ReplayEkf2.cpp
+6
View File
@@ -45,6 +45,7 @@
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_magnetometer.h>
@@ -104,6 +105,9 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
} else if (sub.orb_meta == ORB_ID(vehicle_visual_odometry)) {
_vehicle_visual_odometry_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(aux_global_position)) {
_aux_global_position_msg_id = msg_id;
}
// the main loop should only handle publication of the following topics, the sensor topics are
@@ -130,6 +134,7 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs
handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id);
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);
// sensor_combined: publish last because ekf2 is polling on this
if (!findTimestampAndPublish(ekf2_timestamps.timestamp / 100, _sensor_combined_msg_id, replay_file)) {
@@ -214,6 +219,7 @@ ReplayEkf2::onExitMainLoop()
print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data");
print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer");
print_sensor_statistics(_vehicle_visual_odometry_msg_id, "vehicle_visual_odometry");
print_sensor_statistics(_aux_global_position_msg_id, "aux_global_position");
}
} // namespace px4