diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp
index 957932d10c..f125b8105c 100644
--- a/libraries/AP_InertialNav/AP_InertialNav.cpp
+++ b/libraries/AP_InertialNav/AP_InertialNav.cpp
@@ -1,12 +1,8 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-#include <FastSerial.h>
+#include <AP_HAL.h>
 #include <AP_InertialNav.h>
 
-#if defined(ARDUINO) && ARDUINO >= 100
- #include "Arduino.h"
-#else
- #include <wiring.h>
-#endif
+extern const AP_HAL::HAL& hal;
 
 // table of user settable parameters
 const AP_Param::GroupInfo AP_InertialNav::var_info[] PROGMEM = {
@@ -119,7 +115,7 @@ void AP_InertialNav::set_time_constant_xy( float time_constant_in_seconds )
 // position_ok - return true if position has been initialised and have received gps data within 3 seconds
 bool AP_InertialNav::position_ok()
 {
-    return _xy_enabled && (millis() - _gps_last_update < 3000);
+    return _xy_enabled && (hal.scheduler->millis() - _gps_last_update < 3000);
 }
 
 // check_gps - check if new gps readings have arrived and use them to correct position estimates
@@ -138,7 +134,7 @@ void AP_InertialNav::check_gps()
     if( gps_time != _gps_last_time ) {
 
         // calculate time since last gps reading
-        now = millis();
+        now = hal.scheduler->millis();
         float dt = (float)(now - _gps_last_update) / 1000.0;
 
         // call position correction method
diff --git a/libraries/AP_InertialNav/examples/AP_InertialNav_test/AP_InertialNav_test.pde b/libraries/AP_InertialNav/examples/AP_InertialNav_test/AP_InertialNav_test.pde
index 20f52a972d..6fd0731da3 100644
--- a/libraries/AP_InertialNav/examples/AP_InertialNav_test/AP_InertialNav_test.pde
+++ b/libraries/AP_InertialNav/examples/AP_InertialNav_test/AP_InertialNav_test.pde
@@ -1,70 +1,100 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
-//
-// Simple test for the AP_InertialSensor MPU6000 driver.
-//
-
-#include <FastSerial.h>
 #include <AP_Common.h>
-#include <Arduino_Mega_ISR_Registry.h>
+#include <AP_Progmem.h>
+#include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
+#include <AP_Param.h>
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
+
 #include <AP_GPS.h>             // ArduPilot GPS library
-#include <I2C.h>                // Arduino I2C lib
-#include <SPI.h>                // Arduino SPI lib
-#include <SPI3.h>               // SPI3 library
 #include <AP_ADC.h>             // ArduPilot Mega Analog to Digital Converter Library
-#include <AP_AnalogSource.h>
 #include <AP_Baro.h>            // ArduPilot Mega Barometer Library
 #include <Filter.h>
 #include <AP_Compass.h>         // ArduPilot Mega Magnetometer Library
-#include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
+#include <AP_Declination.h>
 #include <AP_InertialSensor.h>  // ArduPilot Mega Inertial Sensor (accel & gyro) Library
-#include <AP_PeriodicProcess.h> // Parent header of Timer
-#include <AP_TimerProcess.h>    // TimerProcess is the scheduler for MPU6000 reads.
 #include <AP_AHRS.h>
 #include <AP_Airspeed.h>
 #include <AC_PID.h>             // PID library
 #include <APM_PI.h>             // PID library
-#include <AP_InertialNav.h>
 #include <AP_Buffer.h>          // ArduPilot general purpose FIFO buffer
 
-FastSerialPort(Serial, 0);
+#include <AP_InertialNav.h>
 
-Arduino_Mega_ISR_Registry isr_registry;
-AP_TimerProcess scheduler;
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
+
+#define A_LED_PIN 27
+#define C_LED_PIN 25
+const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
 AP_InertialSensor_MPU6000 ins;
+AP_Baro_MS5611 baro;
+#else
+
+#define A_LED_PIN 37
+#define C_LED_PIN 35
+const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
+AP_ADC_ADS7844 adc;
+AP_InertialSensor_Oilpan ins(&adc);
+AP_Baro_BMP085 baro;
+#endif
+
+GPS *gps;
+AP_GPS_Auto auto_gps(hal.uartB, &gps);
+
+AP_Compass_HMC5843 compass;
+AP_AHRS_DCM ahrs(&ins, gps);
+
+AP_InertialNav inertialnav(&ahrs, &ins, &baro, &gps);
+
+uint32_t last_update;
+
+static void flash_leds(bool on) {
+    hal.gpio->write(A_LED_PIN, on);
+    hal.gpio->write(C_LED_PIN, ~on);
+}
 
 void setup(void)
 {
-    Serial.begin(115200);
-    Serial.println("Doing INS startup...");
+    hal.console->println_P(PSTR("AP_InertialNav test startup..."));
+    hal.gpio->pinMode(A_LED_PIN, GPIO_OUTPUT);
+    hal.gpio->pinMode(C_LED_PIN, GPIO_OUTPUT);
 
-    SPI.begin();
-    SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate
-
-    isr_registry.init();
-    scheduler.init(&isr_registry);
-
-    // we need to stop the barometer from holding the SPI bus
-    pinMode(40, OUTPUT);
-    digitalWrite(40, HIGH);
+    gps = &auto_gps;
+    gps->init(GPS::GPS_ENGINE_AIRBORNE_2G);
 
     ins.init(AP_InertialSensor::COLD_START, 
 			 AP_InertialSensor::RATE_100HZ,
-			 delay, NULL, &scheduler);
+			 flash_leds);
+
+    ahrs.set_compass(&compass);
+    ahrs.set_barometer(&baro);
+
+    last_update = hal.scheduler->millis();
+
+    inertialnav.init();
+    inertialnav.set_velocity_xy(0,0);
+    inertialnav.set_current_position(0,0);
 }
 
 void loop(void)
 {
-    Vector3f accel;
-    Vector3f gyro;
-    float temperature;
+    hal.scheduler->delay(20);
+    gps->update();
+    ahrs.update();
+    uint32_t currtime = hal.scheduler->millis();
+    float dt = (currtime - last_update) / 1000.0f;
+    last_update = currtime;
+    inertialnav.update(dt);
 
-    delay(20);
-    ins.update();
-    gyro = ins.get_gyro();
-    accel = ins.get_accel();
-    temperature = ins.temperature();
+    float dx =  inertialnav.get_latitude_diff();
+    float dy =  inertialnav.get_longitude_diff();
+    float velx =  inertialnav.get_latitude_velocity();
+    float vely =  inertialnav.get_longitude_velocity();
 
-    Serial.printf("AX: %f  AY: %f  AZ: %f  GX: %f  GY: %f  GZ: %f T=%f\n",
-                  accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z, temperature);
+    hal.console->printf_P(
+            PSTR("inertial nav pos: (%f,%f) velocity: (%f, %f)\r\n"),
+            dx, dy, velx, vely);
 }
+
+AP_HAL_MAIN();