From 7d0d29f62def7b29d253f4e45cba272d0c4e15e7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 6 Nov 2016 14:09:39 +1100 Subject: [PATCH] ekf2_replay: fix time stamping bug 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. --- src/modules/ekf2_replay/ekf2_replay_main.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index 021245bca4..0ae4ac81aa 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp @@ -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;