From 909a46a33b3ea8f312a84a09d015c4b8fb99a6de Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 26 Apr 2016 15:50:59 +1000 Subject: [PATCH] Replay: pass delta angle time if available --- Tools/Replay/LR_MsgHandler.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index 631ce2f02a..d839966d9b 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -242,7 +242,11 @@ void LR_MsgHandler_IMT_Base::update_from_msg_imt(uint8_t imu_offset, uint8_t *ms if (gyro_mask & this_imu_mask) { Vector3f d_angle; require_field(msg, "DelA", d_angle); - ins.set_delta_angle(imu_offset, d_angle); + float d_angle_dt; + if (!field_value(msg, "DelaT", d_angle_dt)) { + d_angle_dt = 0; + } + ins.set_delta_angle(imu_offset, d_angle, d_angle_dt); } if (accel_mask & this_imu_mask) { float dvt = 0;