AP_NavEKF: support dual sensors in log replay, and fix flight altitude

libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde

@@ -237,7 +237,7 @@ void loop()
                     LogReader.get_attitude().z,
                     LogReader.get_inavpos().x,
                     LogReader.get_inavpos().y,
-                    LogReader.get_inavpos().z,
+                    LogReader.get_relalt(),
                     degrees(DCM_attitude.x),
                     degrees(DCM_attitude.y),
                     degrees(DCM_attitude.z),

libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp

@@ -256,8 +256,21 @@ bool LogReader::update(uint8_t &type)
         }
         memcpy(&msg, data, sizeof(msg));
         wait_timestamp(msg.timestamp);
-        ins.set_gyro(Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
+        ins.set_gyro(0, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
-        ins.set_accel(Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
+        ins.set_accel(0, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
+        break;
+    }
+
+    case LOG_IMU2_MSG: {
+        struct log_IMU msg;
+        if(sizeof(msg) != f.length) {
+            printf("Bad IMU2 length\n");
+            exit(1);
+        }
+        memcpy(&msg, data, sizeof(msg));
+        wait_timestamp(msg.timestamp);
+        ins.set_gyro(1, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
+        ins.set_accel(1, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
         break;
     }
 
@@ -280,6 +293,7 @@ bool LogReader::update(uint8_t &type)
         if (msg.status == 3 && ground_alt_cm == 0) {
             ground_alt_cm = msg.altitude;
         }
+        rel_altitude = msg.rel_altitude*0.01f;
         break;
     }
 

libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h

@@ -23,6 +23,7 @@ public:
     const Vector3f &get_attitude(void) const { return attitude; }
     const Vector3f &get_inavpos(void) const { return inavpos; }
     const Vector3f &get_sim_attitude(void) const { return sim_attitude; }
+    const float &get_relalt(void) const { return rel_altitude; }
 
     enum vehicle_type { VEHICLE_UNKNOWN, VEHICLE_COPTER, VEHICLE_PLANE, VEHICLE_ROVER };
 
@@ -44,6 +45,7 @@ private:
     Vector3f attitude;
     Vector3f sim_attitude;
     Vector3f inavpos;
+    float rel_altitude;
 
     void wait_timestamp(uint32_t timestamp);