From c7cb98049809839d469c5510680a99580aaaafa2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 15 Jun 2015 18:13:23 +1000 Subject: [PATCH] Replay: support IMT messages --- Tools/Replay/LR_MsgHandler.cpp | 38 +++++++++++++++++++++ Tools/Replay/LR_MsgHandler.h | 60 +++++++++++++++++++++++++++++++++- Tools/Replay/LogReader.cpp | 12 +++++++ Tools/Replay/Replay.cpp | 20 ++++++++++-- 4 files changed, 127 insertions(+), 3 deletions(-) diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index e1c271752d..7e94f3ed21 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -203,6 +203,44 @@ void LR_MsgHandler_IMU::process_message(uint8_t *msg) update_from_msg_imu(0, msg); } +void LR_MsgHandler_IMT_Base::update_from_msg_imt(uint8_t imu_offset, uint8_t *msg) +{ + wait_timestamp_from_msg(msg); + + uint8_t this_imu_mask = 1 << imu_offset; + + float delta_time; + require_field(msg, "DelT", delta_time); + ins.set_delta_time(delta_time); + + if (gyro_mask & this_imu_mask) { + Vector3f d_angle; + require_field(msg, "DelA", d_angle); + ins.set_delta_angle(imu_offset, d_angle); + } + if (accel_mask & this_imu_mask) { + float dvt; + require_field(msg, "DelvT", dvt); + Vector3f d_velocity; + require_field(msg, "DelV", d_velocity); + ins.set_delta_velocity(imu_offset, dvt, d_velocity); + } +} + +void LR_MsgHandler_IMT::process_message(uint8_t *msg) +{ + update_from_msg_imt(0, msg); +} + +void LR_MsgHandler_IMT2::process_message(uint8_t *msg) +{ + update_from_msg_imt(1, msg); +} + +void LR_MsgHandler_IMT3::process_message(uint8_t *msg) +{ + update_from_msg_imt(2, msg); +} void LR_MsgHandler_MAG2::process_message(uint8_t *msg) { diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h index 8e3aeeb945..e854492bd3 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -187,7 +187,7 @@ public: accel_mask(_accel_mask), gyro_mask(_gyro_mask), ins(_ins) { }; - void update_from_msg_imu(uint8_t gps_offset, uint8_t *msg); + void update_from_msg_imu(uint8_t imu_offset, uint8_t *msg); private: uint8_t &accel_mask; @@ -235,6 +235,64 @@ public: }; +class LR_MsgHandler_IMT_Base : public LR_MsgHandler +{ +public: + LR_MsgHandler_IMT_Base(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, + uint8_t &_accel_mask, uint8_t &_gyro_mask, + AP_InertialSensor &_ins) : + LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), + accel_mask(_accel_mask), + gyro_mask(_gyro_mask), + ins(_ins) { }; + void update_from_msg_imt(uint8_t imu_offset, uint8_t *msg); + +private: + uint8_t &accel_mask; + uint8_t &gyro_mask; + AP_InertialSensor &ins; +}; + +class LR_MsgHandler_IMT : public LR_MsgHandler_IMT_Base +{ +public: + LR_MsgHandler_IMT(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, + uint8_t &_accel_mask, uint8_t &_gyro_mask, + AP_InertialSensor &_ins) + : LR_MsgHandler_IMT_Base(_f, _dataflash, _last_timestamp_usec, + _accel_mask, _gyro_mask, _ins) { }; + + void process_message(uint8_t *msg); +}; + +class LR_MsgHandler_IMT2 : public LR_MsgHandler_IMT_Base +{ +public: + LR_MsgHandler_IMT2(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, + uint8_t &_accel_mask, uint8_t &_gyro_mask, + AP_InertialSensor &_ins) + : LR_MsgHandler_IMT_Base(_f, _dataflash, _last_timestamp_usec, + _accel_mask, _gyro_mask, _ins) { }; + + void process_message(uint8_t *msg); +}; + +class LR_MsgHandler_IMT3 : public LR_MsgHandler_IMT_Base +{ +public: + LR_MsgHandler_IMT3(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, + uint8_t &_accel_mask, uint8_t &_gyro_mask, + AP_InertialSensor &_ins) + : LR_MsgHandler_IMT_Base(_f, _dataflash, _last_timestamp_usec, + _accel_mask, _gyro_mask, _ins) { }; + + void process_message(uint8_t *msg); +}; + class LR_MsgHandler_MAG_Base : public LR_MsgHandler { diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index f5fa387dc1..b980cbce94 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -132,6 +132,18 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) { msgparser[f.type] = new LR_MsgHandler_IMU3(formats[f.type], dataflash, last_timestamp_usec, accel_mask, gyro_mask, ins); + } else if (streq(name, "IMT")) { + msgparser[f.type] = new LR_MsgHandler_IMT(formats[f.type], dataflash, + last_timestamp_usec, + accel_mask, gyro_mask, ins); + } else if (streq(name, "IMT2")) { + msgparser[f.type] = new LR_MsgHandler_IMT2(formats[f.type], dataflash, + last_timestamp_usec, + accel_mask, gyro_mask, ins); + } else if (streq(name, "IMT3")) { + msgparser[f.type] = new LR_MsgHandler_IMT3(formats[f.type], dataflash, + last_timestamp_usec, + accel_mask, gyro_mask, ins); } else if (streq(name, "SIM")) { msgparser[f.type] = new LR_MsgHandler_SIM(formats[f.type], dataflash, last_timestamp_usec, diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index 0da931331b..e352e022f9 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -114,8 +114,10 @@ private: uint16_t update_rate = 0; int32_t arm_time_ms = -1; bool ahrs_healthy; - bool have_imu2; - bool have_fram; + bool have_imu2 = false; + bool have_imt = false; + bool have_imt2 = false; + bool have_fram = false; void _parse_command_line(uint8_t argc, char * const argv[]); @@ -473,6 +475,12 @@ void Replay::read_sensors(const char *type) if (streq(type,"IMU2")) { have_imu2 = true; } + if (streq(type,"IMT")) { + have_imt = true; + } + if (streq(type,"IMT2")) { + have_imt2 = true; + } if (streq(type,"GPS")) { gps.update(); @@ -501,8 +509,16 @@ void Replay::read_sensors(const char *type) run_ahrs = true; } + if (have_imt) { + if ((streq(type,"IMT") && !have_imt2) || + (streq(type,"IMT2") && have_imt2)) { + run_ahrs = true; + } + } + // special handling of IMU messages as these trigger an ahrs.update() if (!have_fram && + !have_imt && ((streq(type,"IMU") && !have_imu2) || (streq(type, "IMU2") && have_imu2))) { run_ahrs = true; }