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

ekf2_replay: fix time stamping bug

Browse Source 
If the replay data for the baro or mag data has a zero time stamp, then the corresponding relative timestamp published over the combined sensor topic must be se to RELATIVE_TIMESTAMP_INVALID so that the ekf2 module does not try to use this data.
This commit is contained in:
Paul Riseborough
2016-11-06 14:09:39 +11:00
committed by Beat Küng
parent 7be71459f5
commit 7d0d29f62d
1 changed files with 12 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2_replay/ekf2_replay_main.cpp
+12 -2
View File
@@ -379,8 +379,18 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
_sensors.timestamp = replay_part1.time_ref;
_sensors.gyro_integral_dt = replay_part1.gyro_integral_dt;
_sensors.accelerometer_integral_dt = replay_part1.accelerometer_integral_dt;
_sensors.magnetometer_timestamp_relative = (int32_t)(replay_part1.magnetometer_timestamp - _sensors.timestamp);
_sensors.baro_timestamp_relative = (int32_t)(replay_part1.baro_timestamp - _sensors.timestamp);
// If the magnetometer timestamp is zero, then there is no valid data
if (replay_part1.magnetometer_timestamp == 0) {
_sensors.magnetometer_timestamp_relative = (int32_t)sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
} else {
_sensors.magnetometer_timestamp_relative = (int32_t)(replay_part1.magnetometer_timestamp - _sensors.timestamp);
}
// If the barometer timestamp is zero then there is no valid data
if (replay_part1.baro_timestamp == 0) {
_sensors.baro_timestamp_relative = (int32_t)sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
} else {
_sensors.baro_timestamp_relative = (int32_t)(replay_part1.baro_timestamp - _sensors.timestamp);
}
_sensors.gyro_rad[0] = replay_part1.gyro_x_rad;
_sensors.gyro_rad[1] = replay_part1.gyro_y_rad;
_sensors.gyro_rad[2] = replay_part1.gyro_z_rad;