From 31f7525a61f1950e515a928010ddf65a7c8bab9c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 16 Jun 2015 14:26:07 +1000 Subject: [PATCH] Replay: added --no-imt flag --- Tools/Replay/LR_MsgHandler.cpp | 4 ++++ Tools/Replay/LR_MsgHandler.h | 36 ++++++++++++++++++++-------------- Tools/Replay/LogReader.cpp | 6 +++--- Tools/Replay/LogReader.h | 2 ++ Tools/Replay/Replay.cpp | 12 ++++++++++-- 5 files changed, 40 insertions(+), 20 deletions(-) diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index 7e94f3ed21..5313069e40 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -207,6 +207,10 @@ void LR_MsgHandler_IMT_Base::update_from_msg_imt(uint8_t imu_offset, uint8_t *ms { wait_timestamp_from_msg(msg); + if (!use_imt) { + return; + } + uint8_t this_imu_mask = 1 << imu_offset; float delta_time; diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h index e854492bd3..656d650520 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -239,18 +239,21 @@ 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) : + uint64_t &_last_timestamp_usec, + uint8_t &_accel_mask, uint8_t &_gyro_mask, + bool &_use_imt, + AP_InertialSensor &_ins) : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), accel_mask(_accel_mask), gyro_mask(_gyro_mask), + use_imt(_use_imt), ins(_ins) { }; void update_from_msg_imt(uint8_t imu_offset, uint8_t *msg); private: uint8_t &accel_mask; uint8_t &gyro_mask; + bool &use_imt; AP_InertialSensor &ins; }; @@ -258,11 +261,12 @@ 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) + uint64_t &_last_timestamp_usec, + uint8_t &_accel_mask, uint8_t &_gyro_mask, + bool &_use_imt, + AP_InertialSensor &_ins) : LR_MsgHandler_IMT_Base(_f, _dataflash, _last_timestamp_usec, - _accel_mask, _gyro_mask, _ins) { }; + _accel_mask, _gyro_mask, _use_imt, _ins) { }; void process_message(uint8_t *msg); }; @@ -271,11 +275,12 @@ 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) + uint64_t &_last_timestamp_usec, + uint8_t &_accel_mask, uint8_t &_gyro_mask, + bool &_use_imt, + AP_InertialSensor &_ins) : LR_MsgHandler_IMT_Base(_f, _dataflash, _last_timestamp_usec, - _accel_mask, _gyro_mask, _ins) { }; + _accel_mask, _gyro_mask, _use_imt, _ins) { }; void process_message(uint8_t *msg); }; @@ -284,11 +289,12 @@ 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) + uint64_t &_last_timestamp_usec, + uint8_t &_accel_mask, uint8_t &_gyro_mask, + bool &_use_imt, + AP_InertialSensor &_ins) : LR_MsgHandler_IMT_Base(_f, _dataflash, _last_timestamp_usec, - _accel_mask, _gyro_mask, _ins) { }; + _accel_mask, _gyro_mask, _use_imt, _ins) { }; void process_message(uint8_t *msg); }; diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index b980cbce94..a55ccf07f3 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -135,15 +135,15 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) { } else if (streq(name, "IMT")) { msgparser[f.type] = new LR_MsgHandler_IMT(formats[f.type], dataflash, last_timestamp_usec, - accel_mask, gyro_mask, ins); + accel_mask, gyro_mask, use_imt, 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); + accel_mask, gyro_mask, use_imt, 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); + accel_mask, gyro_mask, use_imt, 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/LogReader.h b/Tools/Replay/LogReader.h index 36d24195eb..69787df2d1 100644 --- a/Tools/Replay/LogReader.h +++ b/Tools/Replay/LogReader.h @@ -20,6 +20,7 @@ public: void set_accel_mask(uint8_t mask) { accel_mask = mask; } void set_gyro_mask(uint8_t mask) { gyro_mask = mask; } + void set_use_imt(bool _use_imt) { use_imt = _use_imt; } uint64_t last_timestamp_us(void) const { return last_timestamp_usec; } virtual bool handle_log_format_msg(const struct log_Format &f); @@ -36,6 +37,7 @@ private: uint8_t accel_mask; uint8_t gyro_mask; + bool use_imt = true; uint32_t ground_alt_cm; diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index e352e022f9..db54ff9113 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -118,6 +118,7 @@ private: bool have_imt = false; bool have_imt2 = false; bool have_fram = false; + bool use_imt = true; void _parse_command_line(uint8_t argc, char * const argv[]); @@ -196,6 +197,7 @@ void Replay::usage(void) ::printf("\t--accel-mask MASK set accel mask (1=accel1 only, 2=accel2 only, 3=both)\n"); ::printf("\t--gyro-mask MASK set gyro mask (1=gyro1 only, 2=gyro2 only, 3=both)\n"); ::printf("\t--arm-time time arm at time (milliseconds)\n"); + ::printf("\t--no-imt don't use IMT data\n"); } void Replay::_parse_command_line(uint8_t argc, char * const argv[]) @@ -208,6 +210,7 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[]) {"accel-mask", true, 0, 'a'}, {"gyro-mask", true, 0, 'g'}, {"arm-time", true, 0, 'A'}, + {"no-imt", false, 0, 'n'}, {0, false, 0, 0} }; @@ -237,6 +240,11 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[]) arm_time_ms = strtol(gopt.optarg, NULL, 0); break; + case 'n': + use_imt = false; + logreader.set_use_imt(use_imt); + break; + case 'p': const char *eq = strchr(gopt.optarg, '='); if (eq == NULL) { @@ -475,10 +483,10 @@ void Replay::read_sensors(const char *type) if (streq(type,"IMU2")) { have_imu2 = true; } - if (streq(type,"IMT")) { + if (use_imt && streq(type,"IMT")) { have_imt = true; } - if (streq(type,"IMT2")) { + if (use_imt && streq(type,"IMT2")) { have_imt2 = true; }