From c52578426e8bba2a2e3df32d9705fd0cd45b270f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 16 Nov 2014 12:31:05 +1100
Subject: [PATCH] Replay: cope with up to 3 IMUs

---
 Tools/Replay/LogReader.cpp | 30 +++++++++++++++++++++++++-----
 1 file changed, 25 insertions(+), 5 deletions(-)

diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp
index 6b83e09092..4c8ef85212 100644
--- a/Tools/Replay/LogReader.cpp
+++ b/Tools/Replay/LogReader.cpp
@@ -27,8 +27,8 @@ LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro_HIL &_baro
     compass(_compass),
     gps(_gps),
     airspeed(_airspeed),
-    accel_mask(3),
-    gyro_mask(3)
+    accel_mask(7),
+    gyro_mask(7)
 {}
 
 bool LogReader::open_log(const char *logfile)
@@ -258,9 +258,12 @@ void LogReader::process_copter(uint8_t type, uint8_t *data, uint16_t length)
 
 bool LogReader::set_parameter(const char *name, float value)
 {
-    if (strcmp(name, "GPS_TYPE") == 0) {
-        // ignore this one
-        return true;
+    const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_USE" };
+    for (uint8_t i=0; i<sizeof(ignore_parms)/sizeof(ignore_parms[0]); i++) {
+        if (strcmp(name, ignore_parms[i]) == 0) {
+            ::printf("Ignoring set of %s to %f\n", name, value);
+            return true;
+        }
     }
     enum ap_var_type var_type;
     AP_Param *vp = AP_Param::find(name, &var_type);
@@ -386,6 +389,23 @@ bool LogReader::update(uint8_t &type)
         break;
     }
 
+    case LOG_IMU3_MSG: {
+        struct log_IMU msg;
+        if(sizeof(msg) != f.length) {
+            printf("Bad IMU3 length\n");
+            exit(1);
+        }
+        memcpy(&msg, data, sizeof(msg));
+        wait_timestamp(msg.timestamp);
+        if (gyro_mask & 4) {
+            ins.set_gyro(2, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
+        }
+        if (accel_mask & 4) {
+            ins.set_accel(2, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
+        }
+        break;
+    }
+
     case LOG_GPS_MSG: {
         struct log_GPS msg;
         if(sizeof(msg) != f.length) {