UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-09-23 10:18:30 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(replay): replay sensor_selection and launch_detection_status

Browse Source 
Neither topic was published, so ekf2 kept its accel and gyro device ids at
0, skipped the bias reset that follows a device change, and reported
estimator_sensor_bias with zeroed ids and accel_bias_valid false.

Assisted-by: Claude:claude-opus-5[1m]
Signed-off-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
bresch
2026-09-01 11:23:10 +02:00
committed by Mathieu Bresciani
parent 8ee00cc618
commit 412723352b
2 changed files with 14 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/replay/ReplayEkf2.cpp
View File

@@ -43,7 +43,9 @@
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/launch_detection_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/sensor_gps.h>
@@ -148,6 +150,12 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
} else if (sub.orb_meta == ORB_ID(vehicle_status)) {
_vehicle_status_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(sensor_selection)) {
_sensor_selection_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(launch_detection_status)) {
_launch_detection_status_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(ekf2_timestamps)) {
_ekf2_timestamps_exists = true;
@@ -178,6 +186,8 @@ ReplayEkf2::publishEkf2Topics(sensor_combined_s &sensor_combined, std::ifstream
findTimestampAndPublish(sensor_combined.timestamp, _vehicle_gps_position_msg_id, replay_file);
findTimestampAndPublish(sensor_combined.timestamp, _vehicle_land_detected_msg_id, replay_file);
findTimestampAndPublish(sensor_combined.timestamp, _vehicle_status_msg_id, replay_file);
findTimestampAndPublish(sensor_combined.timestamp, _sensor_selection_msg_id, replay_file);
findTimestampAndPublish(sensor_combined.timestamp, _launch_detection_status_msg_id, replay_file);
// sensor_combined: publish last because ekf2 is polling on this
if (_last_sensor_combined_timestamp > 0) {
@@ -225,6 +235,8 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs
findTimestampAndPublish(ekf2_timestamps.timestamp, _vehicle_gps_position_msg_id, replay_file);
findTimestampAndPublish(ekf2_timestamps.timestamp, _vehicle_land_detected_msg_id, replay_file);
findTimestampAndPublish(ekf2_timestamps.timestamp, _vehicle_status_msg_id, replay_file);
findTimestampAndPublish(ekf2_timestamps.timestamp, _sensor_selection_msg_id, replay_file);
findTimestampAndPublish(ekf2_timestamps.timestamp, _launch_detection_status_msg_id, replay_file);
publishUnmatchedImuSamples(ekf2_timestamps.timestamp, replay_file);

2
src/modules/replay/ReplayEkf2.hpp
View File

@@ -109,6 +109,8 @@ private:
uint16_t _vehicle_gps_position_msg_id = msg_id_invalid;
uint16_t _vehicle_land_detected_msg_id = msg_id_invalid;
uint16_t _vehicle_status_msg_id = msg_id_invalid;
uint16_t _sensor_selection_msg_id = msg_id_invalid;
uint16_t _launch_detection_status_msg_id = msg_id_invalid;
bool _ekf2_timestamps_exists{false};
uint64_t _last_sensor_combined_timestamp{0};