diff --git a/libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde b/libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde
index bd4e3825c6..0b6d1bef52 100644
--- a/libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde
+++ b/libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde
@@ -39,6 +39,7 @@
 #include <AP_Compass.h>
 #include <AP_Baro.h>
 #include <AP_InertialSensor.h>
+#include <AP_InertialNav.h>
 #include <AP_NavEKF.h>
 #include <stdio.h>
 
@@ -51,21 +52,25 @@ static AP_Baro_HIL barometer;
 static AP_GPS_HIL gps_driver;
 static GPS *g_gps = &gps_driver;
 static AP_Compass_HIL compass;
-static AP_AHRS_DCM ahrs(ins, barometer, g_gps);
+static AP_AHRS_NavEKF ahrs(ins, barometer, g_gps);
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 SITL sitl;
 #endif
 
-static NavEKF NavEKF(&ahrs, barometer);
+static const NavEKF &NavEKF = ahrs.get_NavEKF();
+
 static LogReader LogReader(ins, barometer, compass, g_gps);
 
 static FILE *plotf;
 static FILE *plotf2;
 
+static uint32_t gps_fix_count;
+
 void setup()
 {
     ::printf("Starting\n");
+
     LogReader.open_log("log.bin");
 
     LogReader.wait_type(LOG_GPS_MSG);
@@ -81,29 +86,49 @@ void setup()
     compass.init();
     ins.init(AP_InertialSensor::WARM_START, AP_InertialSensor::RATE_50HZ);
 
-    ::printf("Waiting for 3D fix\n");
-    uint8_t goodFrameCount = 0;
-    while (goodFrameCount <= 10) // wait for readings to stabilise
-    {
-        LogReader.wait_type(LOG_GPS_MSG);
-        g_gps->update();
-        compass.read();
-        barometer.read();
-        LogReader.wait_type(LOG_IMU_MSG);
-        ahrs.update();
-        if ((g_gps->status() >= GPS::GPS_OK_FIX_3D) && (ahrs.yaw_sensor != 0)) goodFrameCount +=1;
-        printf("%u\n",hal.scheduler->millis());
-    }
-
-    barometer.calibrate();
-    compass.set_initial_location(g_gps->latitude, g_gps->longitude);
-
-    NavEKF.InitialiseFilter();
-
     plotf = fopen("plot.dat", "w");
     plotf2 = fopen("plot2.dat", "w");
-    fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw ATT.Roll ATT.Pitch ATT.Yaw AHRS.Roll AHRS.Pitch AHRS.Yaw EKF.Roll EKF.Pitch EKF.Yaw\n");
+    fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw ATT.Roll ATT.Pitch ATT.Yaw DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw\n");
     fprintf(plotf2, "time E1 E2 E3 VN VE VD PN PE PD GX GY GZ AX AY AZ MN ME MD MX MY MZ E1ref E2ref E3ref\n");
+
+    ahrs.set_ekf_use(true);
+
+    ::printf("Waiting for InertialNav to start\n");
+    while (!ahrs.have_inertial_nav()) {
+        uint8_t type;
+        if (!LogReader.update(type)) break;
+        read_sensors(type);
+        if (type == LOG_GPS_MSG && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
+            gps_fix_count++;
+            if (gps_fix_count == 2) {
+                ::printf("GPS Lock at %.7f %.7f %.2fm\n", 
+                         g_gps->latitude*1.0e-7f, 
+                         g_gps->longitude*1.0e-7f,
+                         g_gps->altitude_cm*0.01f);
+                ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm);
+                barometer.calibrate();
+                compass.set_initial_location(g_gps->latitude, g_gps->longitude);            
+            }
+        }
+    }
+
+    if (!ahrs.have_inertial_nav()) {
+        ::printf("Failed to start NavEKF\n");
+        exit(1);
+    }
+}
+
+static void read_sensors(uint8_t type)
+{
+    if (type == LOG_GPS_MSG) {
+        g_gps->update();
+        barometer.read();
+    } else if (type == LOG_IMU_MSG) {
+        ahrs.update();
+    } else if ((type == LOG_PLANE_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) ||
+               (type == LOG_COPTER_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER)) {
+        compass.read();
+    }
 }
 
 void loop()
@@ -115,13 +140,10 @@ void loop()
             fclose(plotf);
             exit(0);
         }
-        switch (type) {
-        case LOG_GPS_MSG:
-            g_gps->update();
-            barometer.read();
-            break;
+        read_sensors(type);
 
-        case LOG_ATTITUDE_MSG: {
+        if ((type == LOG_PLANE_ATTITUDE_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) ||
+            (type == LOG_COPTER_ATTITUDE_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER)) {
             Vector3f ekf_euler;
             Vector3f velNED;
             Vector3f posNED;
@@ -129,6 +151,8 @@ void loop()
             Vector3f accelBias;
             Vector3f magNED;
             Vector3f magXYZ;
+            Vector3f DCM_attitude;
+            ahrs.get_secondary_attitude(DCM_attitude);
             NavEKF.getEulerAngles(ekf_euler);
             NavEKF.getVelNED(velNED);
             NavEKF.getPosNED(posNED);
@@ -146,9 +170,9 @@ void loop()
                     LogReader.get_attitude().x,
                     LogReader.get_attitude().y,
                     LogReader.get_attitude().z,
-                    ahrs.roll_sensor*0.01f, 
-                    ahrs.pitch_sensor*0.01f,
-                    ahrs.yaw_sensor*0.01f,
+                    degrees(DCM_attitude.x),
+                    degrees(DCM_attitude.y),
+                    degrees(DCM_attitude.z),
                     degrees(ekf_euler.x),
                     degrees(ekf_euler.y),
                     degrees(ekf_euler.z));
@@ -178,35 +202,6 @@ void loop()
                     LogReader.get_attitude().x,
                     LogReader.get_attitude().y,
                     LogReader.get_attitude().z);
-
-#if 0
-            ::printf("t=%.3f ATT: (%.1f %.1f %.1f) AHRS: (%.1f %.1f %.1f) EKF: (%.1f %.1f %.1f) ALT: %.1f GPS: %u %f %f\n", 
-                     hal.scheduler->millis() * 0.001f,
-                     LogReader.get_attitude().x,
-                     LogReader.get_attitude().y,
-                     LogReader.get_attitude().z,
-                     ahrs.roll_sensor*0.01f, 
-                     ahrs.pitch_sensor*0.01f,
-                     ahrs.yaw_sensor*0.01f,
-                     degrees(ekf_euler.x),
-                     degrees(ekf_euler.y),
-                     temp,
-                     barometer.get_altitude(),
-                     (unsigned)g_gps->status(),
-                     g_gps->latitude*1.0e-7f,
-                     g_gps->longitude*1.0e-7f);
-#endif
-                break;
-        }
-
-        case LOG_COMPASS_MSG:
-            compass.read();
-            break;
-
-        case LOG_IMU_MSG:
-            ahrs.update();
-            NavEKF.UpdateFilter();
-            break;
         }
     }
 }
diff --git a/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp b/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp
index c437e0509f..5fee8c16c8 100644
--- a/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp
+++ b/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp
@@ -18,7 +18,10 @@
 
 extern const AP_HAL::HAL& hal;
 
+extern uint32_t gps_fix_count;
+
 LogReader::LogReader(AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, GPS *&_gps) :
+    vehicle(VEHICLE_UNKNOWN),
     fd(-1),
     ins(_ins),
     baro(_baro),
@@ -36,7 +39,7 @@ bool LogReader::open_log(const char *logfile)
 }
 
 
-struct PACKED log_Compass {
+struct PACKED log_Plane_Compass {
     LOG_PACKET_HEADER;
     uint32_t time_ms;
     int16_t mag_x;
@@ -47,20 +50,20 @@ struct PACKED log_Compass {
     int16_t offset_z;
 };
 
-struct PACKED log_Nav_Tuning {
+struct PACKED log_Copter_Compass {
     LOG_PACKET_HEADER;
-    uint32_t time_ms;
-    uint16_t yaw;
-    uint32_t wp_distance;
-    uint16_t target_bearing_cd;
-    uint16_t nav_bearing_cd;
-    int16_t altitude_error_cm;
-    int16_t airspeed_cm;
-    float   altitude;
-    uint32_t groundspeed_cm;
+    int16_t mag_x;
+    int16_t mag_y;
+    int16_t mag_z;
+    int16_t offset_x;
+    int16_t offset_y;
+    int16_t offset_z;
+    int16_t motor_offset_x;
+    int16_t motor_offset_y;
+    int16_t motor_offset_z;
 };
 
-struct PACKED log_Attitude {
+struct PACKED log_Plane_Attitude {
     LOG_PACKET_HEADER;
     uint32_t time_ms;
     int16_t roll;
@@ -70,6 +73,89 @@ struct PACKED log_Attitude {
     uint16_t error_yaw;
 };
 
+struct PACKED log_Copter_Attitude {
+    LOG_PACKET_HEADER;
+    int16_t control_roll;
+    int16_t roll;
+    int16_t control_pitch;
+    int16_t pitch;
+    uint16_t control_yaw;
+    uint16_t yaw;
+};
+
+struct PACKED log_Copter_Control_Tuning {
+    LOG_PACKET_HEADER;
+    int16_t throttle_in;
+    int16_t sonar_alt;
+    int32_t baro_alt;
+    float   next_wp_alt;
+    int16_t desired_sonar_alt;
+    int16_t angle_boost;
+    int16_t climb_rate;
+    int16_t throttle_out;
+    int16_t desired_climb_rate;
+};
+
+void LogReader::process_plane(uint8_t type, uint8_t *data, uint16_t length)
+{
+    switch (type) {
+    case LOG_PLANE_COMPASS_MSG: {
+        struct log_Plane_Compass msg;
+        if(sizeof(msg) != length) {
+            printf("Bad COMPASS length\n");
+            exit(1);
+        }
+        memcpy(&msg, data, sizeof(msg));
+        wait_timestamp(msg.time_ms);
+        compass.setHIL(Vector3i(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z));
+        compass.set_offsets(msg.offset_x, msg.offset_y, msg.offset_z);
+        break;
+    }
+
+    case LOG_PLANE_ATTITUDE_MSG: {
+        struct log_Plane_Attitude msg;
+        if(sizeof(msg) != length) {
+            printf("Bad ATTITUDE length\n");
+            exit(1);
+        }
+        memcpy(&msg, data, sizeof(msg));
+        wait_timestamp(msg.time_ms);
+        attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
+        break;
+    }
+    }
+}
+
+void LogReader::process_copter(uint8_t type, uint8_t *data, uint16_t length)
+{
+    switch (type) {
+    case LOG_COPTER_COMPASS_MSG: {
+        struct log_Copter_Compass msg;
+        if(sizeof(msg) != length) {
+            printf("Bad COMPASS length\n");
+            exit(1);
+        }
+        memcpy(&msg, data, sizeof(msg));
+        //wait_timestamp(msg.time_ms);
+        compass.setHIL(Vector3i(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z));
+        compass.set_offsets(msg.offset_x, msg.offset_y, msg.offset_z);
+        break;
+    }
+
+    case LOG_COPTER_ATTITUDE_MSG: {
+        struct log_Copter_Attitude msg;
+        if(sizeof(msg) != length) {
+            printf("Bad ATTITUDE length\n");
+            exit(1);
+        }
+        memcpy(&msg, data, sizeof(msg));
+        //wait_timestamp(msg.time_ms);
+        attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
+        break;
+    }
+    }
+}
+
 bool LogReader::update(uint8_t &type)
 {
     uint8_t hdr[3];
@@ -77,6 +163,7 @@ bool LogReader::update(uint8_t &type)
         return false;
     }
     if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {
+        printf("bad log header\n");
         return false;
     }
 
@@ -107,8 +194,32 @@ bool LogReader::update(uint8_t &type)
     }
 
     switch (f.type) {
+    case LOG_MESSAGE_MSG: {
+        struct log_Message msg;
+        if(sizeof(msg) != f.length) {
+            printf("Bad MESSAGE length\n");
+            exit(1);
+        }
+        memcpy(&msg, data, sizeof(msg));
+        if (strncmp(msg.msg, "ArduPlane", strlen("ArduPlane")) == 0) {
+            vehicle = VEHICLE_PLANE;
+            ::printf("Detected Plane\n");
+        } else if (strncmp(msg.msg, "ArduCopter", strlen("ArduCopter")) == 0) {
+            vehicle = VEHICLE_COPTER;
+            ::printf("Detected Copter\n");
+        } else if (strncmp(msg.msg, "APMRover2", strlen("APMRover2")) == 0) {
+            vehicle = VEHICLE_ROVER;
+            ::printf("Detected Rover\n");
+        }
+        break;
+    }
+
     case LOG_IMU_MSG: {
         struct log_IMU msg;
+        if(sizeof(msg) != f.length) {
+            printf("Bad IMU length\n");
+            exit(1);
+        }
         memcpy(&msg, data, sizeof(msg));
         wait_timestamp(msg.timestamp);
         ins.set_gyro(Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
@@ -118,6 +229,10 @@ bool LogReader::update(uint8_t &type)
 
     case LOG_GPS_MSG: {
         struct log_GPS msg;
+        if(sizeof(msg) != f.length) {
+            printf("Bad GPS length\n");
+            exit(1);
+        }
         memcpy(&msg, data, sizeof(msg));
         wait_timestamp(msg.apm_time);
         gps->setHIL(msg.apm_time,
@@ -135,30 +250,25 @@ bool LogReader::update(uint8_t &type)
         break;
     }
 
-    case LOG_COMPASS_MSG: {
-        struct log_Compass msg;
-        memcpy(&msg, data, sizeof(msg));
-        wait_timestamp(msg.time_ms);
-        //compass.setHIL(Vector3i(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z));
-        compass.setHIL(Vector3i(msg.mag_x, msg.mag_y, msg.mag_z));
-        break;
-    }
-
-    case LOG_ATTITUDE_MSG: {
-        struct log_Attitude msg;
-        memcpy(&msg, data, sizeof(msg));
-        wait_timestamp(msg.time_ms);
-        attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
-        break;
-    }
-
     case LOG_SIMSTATE_MSG: {
         struct log_AHRS msg;
+        if(sizeof(msg) != f.length) {
+            printf("Bad SIMSTATE length\n");
+            exit(1);
+        }
         memcpy(&msg, data, sizeof(msg));
         wait_timestamp(msg.time_ms);
         sim_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
         break;
     }
+
+    default:
+        if (vehicle == VEHICLE_PLANE) {
+            process_plane(f.type, data, f.length);
+        } else if (vehicle == VEHICLE_COPTER) {
+            process_copter(f.type, data, f.length);
+        }
+        break;
     }
 
     type = f.type;
diff --git a/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h b/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h
index 94aaf6b20a..335dbeae84 100644
--- a/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h
+++ b/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h
@@ -1,8 +1,14 @@
 
 enum log_messages {
-    LOG_NTUN_MSG     = 2,
-    LOG_ATTITUDE_MSG = 10,
-    LOG_COMPASS_MSG  = 12
+    // plane specific messages
+    LOG_PLANE_NTUN_MSG     = 2,
+    LOG_PLANE_ATTITUDE_MSG = 10,
+    LOG_PLANE_COMPASS_MSG  = 12,
+
+    // copter specific messages
+    LOG_COPTER_ATTITUDE_MSG = 1,
+    LOG_COPTER_COMPASS_MSG  = 15,
+    LOG_COPTER_CONTROL_TUNING_MSG = 4
 };
 
 
@@ -16,6 +22,10 @@ public:
     const Vector3f &get_attitude(void) const { return attitude; }
     const Vector3f &get_sim_attitude(void) const { return sim_attitude; }
 
+    enum vehicle_type { VEHICLE_UNKNOWN, VEHICLE_COPTER, VEHICLE_PLANE, VEHICLE_ROVER };
+
+    vehicle_type vehicle;
+
 private:
     int fd;
     AP_InertialSensor &ins;
@@ -26,10 +36,13 @@ private:
     uint32_t ground_alt_cm;
 
     uint8_t num_formats;
-    struct log_Format formats[32];
+    struct log_Format formats[100];
 
     Vector3f attitude;
     Vector3f sim_attitude;
 
     void wait_timestamp(uint32_t timestamp);
+
+    void process_plane(uint8_t type, uint8_t *data, uint16_t length);
+    void process_copter(uint8_t type, uint8_t *data, uint16_t length);
 };
diff --git a/libraries/AP_NavEKF/examples/AP_NavEKF/pitch.gnu b/libraries/AP_NavEKF/examples/AP_NavEKF/pitch.gnu
index 1bbfd6acd2..8c0371d827 100644
--- a/libraries/AP_NavEKF/examples/AP_NavEKF/pitch.gnu
+++ b/libraries/AP_NavEKF/examples/AP_NavEKF/pitch.gnu
@@ -1,3 +1,3 @@
 set style data lines
-plot "plot.dat" using "SIM.Pitch", "" using "AHRS.Pitch", "" using "EKF.Pitch"
+plot "plot.dat" using "SIM.Pitch", "" using "DCM.Pitch", "" using "EKF.Pitch"
 pause -1 "hit return to exit"
diff --git a/libraries/AP_NavEKF/examples/AP_NavEKF/roll.gnu b/libraries/AP_NavEKF/examples/AP_NavEKF/roll.gnu
index a9622852ef..2fbd899136 100644
--- a/libraries/AP_NavEKF/examples/AP_NavEKF/roll.gnu
+++ b/libraries/AP_NavEKF/examples/AP_NavEKF/roll.gnu
@@ -1,3 +1,3 @@
 set style data lines
-plot "plot.dat" using "SIM.Roll", "" using "AHRS.Roll", "" using "EKF.Roll"
+plot "plot.dat" using "SIM.Roll", "" using "DCM.Roll", "" using "EKF.Roll"
 pause -1 "hit return to exit"