diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 43617189d46dfbf83eed94c5a7693712bcd7edab..f53572ba7b98ff1f878d820d9e1dca1237dd709e 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -90,14 +90,126 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
     AP_GROUPINFO("MPU6K_FILTER", 4, AP_InertialSensor, _mpu6000_filter,  0),
 
 #if INS_MAX_INSTANCES > 1
+    // @Param: ACC2SCAL_X
+    // @DisplayName: Accelerometer2 scaling of X axis
+    // @Description: Accelerometer2 scaling of X axis.  Calculated during acceleration calibration routine
+    // @Range: 0.8 1.2
+    // @User: Advanced
+
+    // @Param: ACC2SCAL_Y
+    // @DisplayName: Accelerometer2 scaling of Y axis
+    // @Description: Accelerometer2 scaling of Y axis  Calculated during acceleration calibration routine
+    // @Range: 0.8 1.2
+    // @User: Advanced
+
+    // @Param: ACC2SCAL_Z
+    // @DisplayName: Accelerometer2 scaling of Z axis
+    // @Description: Accelerometer2 scaling of Z axis  Calculated during acceleration calibration routine
+    // @Range: 0.8 1.2
+    // @User: Advanced
     AP_GROUPINFO("ACC2SCAL",    5, AP_InertialSensor, _accel_scale[1],   0),
+
+    // @Param: ACC2OFFS_X
+    // @DisplayName: Accelerometer2 offsets of X axis
+    // @Description: Accelerometer2 offsets of X axis. This is setup using the acceleration calibration or level operations
+    // @Units: m/s/s
+    // @Range: -300 300
+    // @User: Advanced
+
+    // @Param: ACC2OFFS_Y
+    // @DisplayName: Accelerometer2 offsets of Y axis
+    // @Description: Accelerometer2 offsets of Y axis. This is setup using the acceleration calibration or level operations
+    // @Units: m/s/s
+    // @Range: -300 300
+    // @User: Advanced
+
+    // @Param: ACC2OFFS_Z
+    // @DisplayName: Accelerometer2 offsets of Z axis
+    // @Description: Accelerometer2 offsets of Z axis. This is setup using the acceleration calibration or level operations
+    // @Units: m/s/s
+    // @Range: -300 300
+    // @User: Advanced
     AP_GROUPINFO("ACC2OFFS",    6, AP_InertialSensor, _accel_offset[1],  0),
+
+    // @Param: GYR2OFFS_X
+    // @DisplayName: Gyro2 offsets of X axis
+    // @Description: Gyro2 sensor offsets of X axis. This is setup on each boot during gyro calibrations
+    // @Units: rad/s
+    // @User: Advanced
+
+    // @Param: GYR2OFFS_Y
+    // @DisplayName: Gyro2 offsets of Y axis
+    // @Description: Gyro2 sensor offsets of Y axis. This is setup on each boot during gyro calibrations
+    // @Units: rad/s
+    // @User: Advanced
+
+    // @Param: GYR2OFFS_Z
+    // @DisplayName: Gyro2 offsets of Z axis
+    // @Description: Gyro2 sensor offsets of Z axis. This is setup on each boot during gyro calibrations
+    // @Units: rad/s
+    // @User: Advanced
     AP_GROUPINFO("GYR2OFFS",    7, AP_InertialSensor, _gyro_offset[1],   0),
 #endif
 
 #if INS_MAX_INSTANCES > 2
+    // @Param: ACC3SCAL_X
+    // @DisplayName: Accelerometer3 scaling of X axis
+    // @Description: Accelerometer3 scaling of X axis.  Calculated during acceleration calibration routine
+    // @Range: 0.8 1.2
+    // @User: Advanced
+
+    // @Param: ACC3SCAL_Y
+    // @DisplayName: Accelerometer3 scaling of Y axis
+    // @Description: Accelerometer3 scaling of Y axis  Calculated during acceleration calibration routine
+    // @Range: 0.8 1.2
+    // @User: Advanced
+
+    // @Param: ACC3SCAL_Z
+    // @DisplayName: Accelerometer3 scaling of Z axis
+    // @Description: Accelerometer3 scaling of Z axis  Calculated during acceleration calibration routine
+    // @Range: 0.8 1.2
+    // @User: Advanced
     AP_GROUPINFO("ACC3SCAL",    8, AP_InertialSensor, _accel_scale[2],   0),
+
+    // @Param: ACC3OFFS_X
+    // @DisplayName: Accelerometer3 offsets of X axis
+    // @Description: Accelerometer3 offsets of X axis. This is setup using the acceleration calibration or level operations
+    // @Units: m/s/s
+    // @Range: -300 300
+    // @User: Advanced
+
+    // @Param: ACC3OFFS_Y
+    // @DisplayName: Accelerometer3 offsets of Y axis
+    // @Description: Accelerometer3 offsets of Y axis. This is setup using the acceleration calibration or level operations
+    // @Units: m/s/s
+    // @Range: -300 300
+    // @User: Advanced
+
+    // @Param: ACC3OFFS_Z
+    // @DisplayName: Accelerometer3 offsets of Z axis
+    // @Description: Accelerometer3 offsets of Z axis. This is setup using the acceleration calibration or level operations
+    // @Units: m/s/s
+    // @Range: -300 300
+    // @User: Advanced
     AP_GROUPINFO("ACC3OFFS",    9, AP_InertialSensor, _accel_offset[2],  0),
+
+    // @Param: GYR3OFFS_X
+    // @DisplayName: Gyro3 offsets of X axis
+    // @Description: Gyro3 sensor offsets of X axis. This is setup on each boot during gyro calibrations
+    // @Units: rad/s
+    // @User: Advanced
+
+    // @Param: GYR3OFFS_Y
+    // @DisplayName: Gyro3 offsets of Y axis
+    // @Description: Gyro3 sensor offsets of Y axis. This is setup on each boot during gyro calibrations
+    // @Units: rad/s
+    // @User: Advanced
+
+    // @Param: GYR3OFFS_Z
+    // @DisplayName: Gyro3 offsets of Z axis
+    // @Description: Gyro3 sensor offsets of Z axis. This is setup on each boot during gyro calibrations
+    // @Units: rad/s
+    // @User: Advanced
     AP_GROUPINFO("GYR3OFFS",   10, AP_InertialSensor, _gyro_offset[2],   0),
 #endif
 
@@ -105,30 +217,154 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
 };
 
 AP_InertialSensor::AP_InertialSensor() :
+    _gyro_count(0),
+    _accel_count(0),
+    _backend_count(0),
     _accel(),
     _gyro(),
-    _board_orientation(ROTATION_NONE)
+    _board_orientation(ROTATION_NONE),
+    _hil_mode(false),
+    _have_3D_calibration(false)
 {
     AP_Param::setup_object_defaults(this, var_info);        
+    for (uint8_t i=0; i<INS_MAX_BACKENDS; i++) {
+        _backends[i] = NULL;
+    }
+    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+        _accel_error_count[i] = 0;
+        _gyro_error_count[i] = 0;
+    }
+}
+
+
+/*
+  register a new gyro instance
+ */
+uint8_t AP_InertialSensor::register_gyro(void)
+{
+    if (_gyro_count == INS_MAX_INSTANCES) {
+        hal.scheduler->panic(PSTR("Too many gyros"));
+    }
+    return _gyro_count++;
+}
+
+/*
+  register a new accel instance
+ */
+uint8_t AP_InertialSensor::register_accel(void)
+{
+    if (_accel_count == INS_MAX_INSTANCES) {
+        hal.scheduler->panic(PSTR("Too many accels"));
+    }
+    return _accel_count++;
 }
 
 void
 AP_InertialSensor::init( Start_style style,
                          Sample_rate sample_rate)
 {
-    _product_id = _init_sensor(sample_rate);
+    // remember the sample rate
+    _sample_rate = sample_rate;
 
-    // check scaling
+    if (_gyro_count == 0 && _accel_count == 0) {
+        // detect available backends. Only called once
+        _detect_backends();
+    }
+
+    _product_id = 0; // FIX
+
+    // initialise accel scale if need be. This is needed as we can't
+    // give non-zero default values for vectors in AP_Param
     for (uint8_t i=0; i<get_accel_count(); i++) {
         if (_accel_scale[i].get().is_zero()) {
             _accel_scale[i].set(Vector3f(1,1,1));
         }
     }
 
+    // remember whether we have 3D calibration so this can be used for
+    // AHRS health
+    check_3D_calibration();
+
     if (WARM_START != style) {
         // do cold-start calibration for gyro only
         _init_gyro();
     }
+
+    switch (sample_rate) {
+    case RATE_50HZ:
+        _sample_period_usec = 20000;
+        break;
+    case RATE_100HZ:
+        _sample_period_usec = 10000;
+        break;
+    case RATE_200HZ:
+        _sample_period_usec = 5000;
+        break;
+    case RATE_400HZ:
+    default:
+        _sample_period_usec = 2500;
+        break;
+    }
+
+    // establish the baseline time between samples
+    _delta_time = 0;
+    _next_sample_usec = 0;
+    _last_sample_usec = 0;
+    _have_sample = false;
+}
+
+/*
+  try to load a backend
+ */
+void AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &))
+{
+    if (_backend_count == INS_MAX_BACKENDS) {
+        hal.scheduler->panic(PSTR("Too many INS backends"));
+    }
+    _backends[_backend_count] = detect(*this);
+    if (_backends[_backend_count] != NULL) {
+        _backend_count++;
+    }
+}
+
+
+/*
+  detect available backends for this board
+ */
+void 
+AP_InertialSensor::_detect_backends(void)
+{
+#if HAL_INS_DEFAULT == HAL_INS_HIL
+    _add_backend(AP_InertialSensor_HIL::detect);
+#elif HAL_INS_DEFAULT == HAL_INS_MPU6000
+    _add_backend(AP_InertialSensor_MPU6000::detect);
+#elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN
+    _add_backend(AP_InertialSensor_PX4::detect);
+#elif HAL_INS_DEFAULT == HAL_INS_OILPAN
+    _add_backend(AP_InertialSensor_Oilpan::detect);
+#elif HAL_INS_DEFAULT == HAL_INS_MPU9250
+    _add_backend(AP_InertialSensor_MPU9250::detect);
+#elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE
+    _add_backend(AP_InertialSensor_Flymaple::detect);
+#else
+    #error Unrecognised HAL_INS_TYPE setting
+#endif
+
+#if 0 // disabled due to broken hardware on some PXF capes
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
+    // the PXF also has a MPU6000
+    _add_backend(AP_InertialSensor_MPU6000::detect);
+#endif
+#endif
+
+    if (_backend_count == 0 ||
+        _gyro_count == 0 ||
+        _accel_count == 0) {
+        hal.scheduler->panic(PSTR("No INS backends available"));
+    }
+
+    // set the product ID to the ID of the first backend
+    _product_id.set(_backends[0]->product_id());
 }
 
 void
@@ -138,6 +374,8 @@ AP_InertialSensor::init_accel()
 
     // save calibration
     _save_parameters();
+
+    check_3D_calibration();
 }
 
 #if !defined( __AVR_ATmega1280__ )
@@ -213,10 +451,7 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
         }
         uint8_t num_samples = 0;
         while (num_samples < 32) {
-            if (!wait_for_sample(1000)) {
-                interact->printf_P(PSTR("Failed to get INS sample\n"));
-                goto failed;
-            }
+            wait_for_sample();
             // read samples from ins
             update();
             // capture sample
@@ -254,6 +489,8 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
         }
         _save_parameters();
 
+        check_3D_calibration();
+
         // calculate the trims as well from primary accels and pass back to caller
         _calculate_trim(samples[0][0], trim_roll, trim_pitch);
 
@@ -271,18 +508,37 @@ failed:
 }
 #endif
 
-/// calibrated - returns true if the accelerometers have been calibrated
-/// @note this should not be called while flying because it reads from the eeprom which can be slow
-bool AP_InertialSensor::calibrated()
+/*
+  check if the accelerometers are calibrated in 3D. Called on startup
+  and any accel cal
+ */
+void AP_InertialSensor::check_3D_calibration()
 {
+    _have_3D_calibration = false;
     // check each accelerometer has offsets saved
     for (uint8_t i=0; i<get_accel_count(); i++) {
-        if (!_accel_offset[i].load()) {
-            return false;
+        // exactly 0.0 offset is extremely unlikely
+        if (_accel_offset[i].get().is_zero()) {
+            return;
+        }
+        // exactly 1.0 scaling is extremely unlikely
+        const Vector3f &scaling = _accel_scale[i].get();
+        if (fabsf(scaling.x - 1.0f) < 0.00001f &&
+            fabsf(scaling.y - 1.0f) < 0.00001f &&
+            fabsf(scaling.z - 1.0f) < 0.00001f) {
+            return;
         }
     }
     // if we got this far the accelerometers must have been calibrated
-    return true;
+    _have_3D_calibration = true;
+}
+
+/*
+  return true if we have 3D calibration values
+ */
+bool AP_InertialSensor::calibrated()
+{
+    return _have_3D_calibration;
 }
 
 void
@@ -463,9 +719,9 @@ AP_InertialSensor::_init_gyro()
 
     uint8_t num_converged = 0;
 
-    // we try to get a good calibration estimate for up to 10 seconds
+    // we try to get a good calibration estimate for up to 30 seconds
     // if the gyros are stable, we should get it in 1 second
-    for (int16_t j = 0; j <= 20 && num_converged < num_gyros; j++) {
+    for (int16_t j = 0; j <= 30*4 && num_converged < num_gyros; j++) {
         Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES];
         float diff_norm[INS_MAX_INSTANCES];
         uint8_t i;
@@ -717,3 +973,177 @@ void AP_InertialSensor::_save_parameters()
         _gyro_offset[i].save();
     }
 }
+
+
+/*
+  update gyro and accel values from backends
+ */
+void AP_InertialSensor::update(void)
+{
+    // during initialisation update() may be called without
+    // wait_for_sample(), and a wait is implied
+    wait_for_sample();
+
+    if (!_hil_mode) {
+        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+            // mark sensors unhealthy and let update() in each backend
+            // mark them healthy via _rotate_and_offset_gyro() and
+            // _rotate_and_offset_accel() 
+            _gyro_healthy[i] = false;
+            _accel_healthy[i] = false;
+        }
+        for (uint8_t i=0; i<_backend_count; i++) {
+            _backends[i]->update();
+        }
+
+        // adjust health status if a sensor has a non-zero error count
+        // but another sensor doesn't. 
+        bool have_zero_accel_error_count = false;
+        bool have_zero_gyro_error_count = false;
+        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+            if (_accel_healthy[i] && _accel_error_count[i] == 0) {
+                have_zero_accel_error_count = true;
+            }
+            if (_gyro_healthy[i] && _gyro_error_count[i] == 0) {
+                have_zero_gyro_error_count = true;
+            }
+        }
+
+        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+            if (_gyro_healthy[i] && _gyro_error_count[i] != 0 && have_zero_gyro_error_count) {
+                // we prefer not to use a gyro that has had errors
+                _gyro_healthy[i] = false;
+            }
+            if (_accel_healthy[i] && _accel_error_count[i] != 0 && have_zero_accel_error_count) {
+                // we prefer not to use a accel that has had errors
+                _accel_healthy[i] = false;
+            }
+        }
+
+        // set primary to first healthy accel and gyro
+        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+            if (_gyro_healthy[i]) {
+                _primary_gyro = i;
+                break;
+            }
+        }
+        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+            if (_accel_healthy[i]) {
+                _primary_accel = i;
+                break;
+            }
+        }
+    }
+
+    _have_sample = false;
+}
+
+/*
+  wait for a sample to be available. This is the function that
+  determines the timing of the main loop in ardupilot. 
+
+  Ideally this function would return at exactly the rate given by the
+  sample_rate argument given to AP_InertialSensor::init(). 
+
+  The key output of this function is _delta_time, which is the time
+  over which the gyro and accel integration will happen for this
+  sample. We want that to be a constant time if possible, but if
+  delays occur we need to cope with them. The long term sum of
+  _delta_time should be exactly equal to the wall clock elapsed time
+ */
+void AP_InertialSensor::wait_for_sample(void)
+{
+    if (_have_sample) {
+        // the user has called wait_for_sample() again without
+        // consuming the sample with update()
+        return;
+    }
+
+    uint32_t now = hal.scheduler->micros();
+
+    if (_next_sample_usec == 0 && _delta_time <= 0) {
+        // this is the first call to wait_for_sample()
+        _last_sample_usec = now - _sample_period_usec;
+        _next_sample_usec = now + _sample_period_usec;
+        goto check_sample;
+    }
+
+    // see how long it is till the next sample is due
+    if (_next_sample_usec - now <=_sample_period_usec) {
+        // we're ahead on time, schedule next sample at expected period
+        uint32_t wait_usec = _next_sample_usec - now;
+        if (wait_usec > 200) {
+            hal.scheduler->delay_microseconds(wait_usec);
+        }
+        _next_sample_usec += _sample_period_usec;
+    } else if (now - _next_sample_usec < _sample_period_usec/8) {
+        // we've overshot, but only by a small amount, keep on
+        // schedule with no delay
+        _next_sample_usec += _sample_period_usec;
+    } else {
+        // we've overshot by a larger amount, re-zero scheduling with
+        // no delay
+        _next_sample_usec = now + _sample_period_usec;
+    }
+
+check_sample:
+    if (!_hil_mode) {
+        // we also wait for at least one backend to have a sample of both
+        // accel and gyro. This normally completes immediately.
+        bool gyro_available = false;
+        bool accel_available = false;
+        while (!gyro_available || !accel_available) {
+            for (uint8_t i=0; i<_backend_count; i++) {
+                gyro_available |= _backends[i]->gyro_sample_available();
+                accel_available |= _backends[i]->accel_sample_available();
+            }
+            if (!gyro_available || !accel_available) {
+                hal.scheduler->delay_microseconds(100);
+            }
+        }
+    }
+
+    now = hal.scheduler->micros();
+    _delta_time = (now - _last_sample_usec) * 1.0e-6f;
+    _last_sample_usec = now;
+
+#if 0
+    {
+        static uint64_t delta_time_sum;
+        static uint16_t counter;
+        if (delta_time_sum == 0) {
+            delta_time_sum = _sample_period_usec;
+        }
+        delta_time_sum += _delta_time * 1.0e6f;
+        if (counter++ == 400) {
+            counter = 0;
+            hal.console->printf("now=%lu _delta_time_sum=%lu diff=%ld\n",
+                                (unsigned long)now, 
+                                (unsigned long)delta_time_sum,
+                                (long)(now - delta_time_sum));
+        }
+    }
+#endif
+
+    _have_sample = true;
+}
+
+/*
+  support for setting accel and gyro vectors, for use by HIL
+ */
+void AP_InertialSensor::set_accel(uint8_t instance, const Vector3f &accel)
+{
+    if (instance < INS_MAX_INSTANCES) {
+        _accel[instance] = accel;
+        _accel_healthy[instance] = true;
+    }
+}
+
+void AP_InertialSensor::set_gyro(uint8_t instance, const Vector3f &gyro)
+{
+    if (instance < INS_MAX_INSTANCES) {
+        _gyro[instance] = gyro;
+        _gyro_healthy[instance] = true;
+    }
+}
+
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 3eaba837557bdf8dc35f2bbab03bb8d391a53720..12004e1edeb87db2c83d1443524deccbbb318b2e 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -11,18 +11,22 @@
    maximum number of INS instances available on this platform. If more
    than 1 then redundent sensors may be available
  */
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
-#define INS_MAX_INSTANCES 3
-#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
 #define INS_MAX_INSTANCES 3
+#define INS_MAX_BACKENDS  6
 #else
 #define INS_MAX_INSTANCES 1
+#define INS_MAX_BACKENDS  1
 #endif
 
+
 #include <stdint.h>
 #include <AP_HAL.h>
 #include <AP_Math.h>
 #include "AP_InertialSensor_UserInteract.h"
+
+class AP_InertialSensor_Backend;
+
 /* AP_InertialSensor is an abstraction for gyro and accel measurements
  * which are correctly aligned to the body axes and scaled to SI units.
  *
@@ -32,12 +36,11 @@
  */
 class AP_InertialSensor
 {
+    friend class AP_InertialSensor_Backend;
+
 public:
     AP_InertialSensor();
 
-    // empty virtual destructor
-    virtual ~AP_InertialSensor() {}
-
     enum Start_style {
         COLD_START = 0,
         WARM_START
@@ -64,22 +67,28 @@ public:
     ///
     /// @param style	The initialisation startup style.
     ///
-    virtual void init( Start_style style,
-                       Sample_rate sample_rate);
+    void init( Start_style style,
+               Sample_rate sample_rate);
 
     /// Perform cold startup initialisation for just the accelerometers.
     ///
     /// @note This should not be called unless ::init has previously
     ///       been called, as ::init may perform other work.
     ///
-    virtual void init_accel();
+    void init_accel();
+
+
+    /// Register a new gyro/accel driver, allocating an instance
+    /// number
+    uint8_t register_gyro(void);
+    uint8_t register_accel(void);
 
 #if !defined( __AVR_ATmega1280__ )
     // perform accelerometer calibration including providing user instructions
     // and feedback
-    virtual bool calibrate_accel(AP_InertialSensor_UserInteract *interact,
-                                 float& trim_roll,
-                                 float& trim_pitch);
+    bool calibrate_accel(AP_InertialSensor_UserInteract *interact,
+                         float& trim_roll,
+                         float& trim_pitch);
 #endif
 
     /// calibrated - returns true if the accelerometers have been calibrated
@@ -93,65 +102,66 @@ public:
     /// @note This should not be called unless ::init has previously
     ///       been called, as ::init may perform other work
     ///
-    virtual void init_gyro(void);
+    void init_gyro(void);
 
     /// Fetch the current gyro values
     ///
     /// @returns	vector of rotational rates in radians/sec
     ///
     const Vector3f     &get_gyro(uint8_t i) const { return _gyro[i]; }
-    const Vector3f     &get_gyro(void) const { return get_gyro(_get_primary_gyro()); }
-    virtual void       set_gyro(uint8_t instance, const Vector3f &gyro) {}
+    const Vector3f     &get_gyro(void) const { return get_gyro(_primary_gyro); }
+    void               set_gyro(uint8_t instance, const Vector3f &gyro);
 
     // set gyro offsets in radians/sec
     const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; }
-    const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_get_primary_gyro()); }
+    const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_primary_gyro); }
 
     /// Fetch the current accelerometer values
     ///
     /// @returns	vector of current accelerations in m/s/s
     ///
     const Vector3f     &get_accel(uint8_t i) const { return _accel[i]; }
-    const Vector3f     &get_accel(void) const { return get_accel(get_primary_accel()); }
-    virtual void       set_accel(uint8_t instance, const Vector3f &accel) {}
+    const Vector3f     &get_accel(void) const { return get_accel(_primary_accel); }
+    void               set_accel(uint8_t instance, const Vector3f &accel);
+
+    uint32_t get_gyro_error_count(uint8_t i) const { return _gyro_error_count[i]; }
+    uint32_t get_accel_error_count(uint8_t i) const { return _accel_error_count[i]; }
 
     // multi-device interface
-    virtual bool get_gyro_health(uint8_t instance) const { return true; }
-    bool get_gyro_health(void) const { return get_gyro_health(_get_primary_gyro()); }
+    bool get_gyro_health(uint8_t instance) const { return _gyro_healthy[instance]; }
+    bool get_gyro_health(void) const { return get_gyro_health(_primary_gyro); }
     bool get_gyro_health_all(void) const;
-    virtual uint8_t get_gyro_count(void) const { return 1; };
+    uint8_t get_gyro_count(void) const { return _gyro_count; }
     bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; }
     bool gyro_calibrated_ok_all() const;
 
-    virtual bool get_accel_health(uint8_t instance) const { return true; }
-    bool get_accel_health(void) const { return get_accel_health(get_primary_accel()); }
+    bool get_accel_health(uint8_t instance) const { return _accel_healthy[instance]; }
+    bool get_accel_health(void) const { return get_accel_health(_primary_accel); }
     bool get_accel_health_all(void) const;
-    virtual uint8_t get_accel_count(void) const { return 1; };
+    uint8_t get_accel_count(void) const { return _accel_count; };
 
     // get accel offsets in m/s/s
     const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; }
-    const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(get_primary_accel()); }
+    const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_primary_accel); }
 
     // get accel scale
     const Vector3f &get_accel_scale(uint8_t i) const { return _accel_scale[i]; }
-    const Vector3f &get_accel_scale(void) const { return get_accel_scale(get_primary_accel()); }
-
-    /* Update the sensor data, so that getters are nonblocking.
-     * Returns a bool of whether data was updated or not.
-     */
-    virtual bool update() = 0;
+    const Vector3f &get_accel_scale(void) const { return get_accel_scale(_primary_accel); }
 
     /* get_delta_time returns the time period in seconds
      * overwhich the sensor data was collected
      */
-    virtual float get_delta_time() const = 0;
+    float get_delta_time() const { return _delta_time; }
 
     // return the maximum gyro drift rate in radians/s/s. This
     // depends on what gyro chips are being used
-    virtual float get_gyro_drift_rate(void) = 0;
+    float get_gyro_drift_rate(void) const { return ToRad(0.5f/60); }
+
+    // update gyro and accel values from accumulated samples
+    void update(void);
 
-    // wait for a sample to be available, with timeout in milliseconds
-    virtual bool wait_for_sample(uint16_t timeout_ms) = 0;
+    // wait for a sample to be available
+    void wait_for_sample(void);
 
     // class level parameters
     static const struct AP_Param::GroupInfo var_info[];
@@ -169,24 +179,28 @@ public:
     }
 
     // get_filter - return filter in hz
-    virtual uint8_t get_filter() const { return _mpu6000_filter.get(); }
+    uint8_t get_filter() const { return _mpu6000_filter.get(); }
 
-    virtual uint16_t error_count(void) const { return 0; }
-    virtual bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
+    // return the selected sample rate
+    Sample_rate get_sample_rate(void) const { return _sample_rate; }
 
-    virtual uint8_t get_primary_accel(void) const { return 0; }
+    uint16_t error_count(void) const { return 0; }
+    bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
 
-protected:
+    uint8_t get_primary_accel(void) const { return 0; }
 
-    virtual uint8_t _get_primary_gyro(void) const { return 0; }
+    // enable HIL mode
+    void set_hil_mode(void) { _hil_mode = true; }
 
-    // sensor specific init to be overwritten by descendant classes
-    virtual uint16_t        _init_sensor( Sample_rate sample_rate ) = 0;
+private:
 
-    // no-save implementations of accel and gyro initialisation routines
-    virtual void  _init_accel();
+    // load backend drivers
+    void _add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &));
+    void _detect_backends(void);
 
-    virtual void _init_gyro();
+    // accel and gyro initialisation
+    void _init_accel();
+    void _init_gyro();
 
 #if !defined( __AVR_ATmega1280__ )
     // Calibration routines borrowed from Rolfe Schmidt
@@ -194,53 +208,98 @@ protected:
     // original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
 
     // _calibrate_accel - perform low level accel calibration
-    virtual bool            _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale);
-    virtual void            _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]);
-    virtual void            _calibrate_reset_matrices(float dS[6], float JS[6][6]);
-    virtual void            _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]);
-    virtual void            _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch);
+    bool _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale);
+    void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]);
+    void _calibrate_reset_matrices(float dS[6], float JS[6][6]);
+    void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]);
+    void _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch);
 #endif
 
+    // check if we have 3D accel calibration
+    void check_3D_calibration(void);
+
     // save parameters to eeprom
     void  _save_parameters();
 
-    // Most recent accelerometer reading obtained by ::update
-    Vector3f _accel[INS_MAX_INSTANCES];
+    // backend objects
+    AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS];
 
-    // previous accelerometer reading obtained by ::update
-    Vector3f _previous_accel[INS_MAX_INSTANCES];
+    // number of gyros and accel drivers. Note that most backends
+    // provide both accel and gyro data, so will increment both
+    // counters on initialisation
+    uint8_t _gyro_count;
+    uint8_t _accel_count;
+    uint8_t _backend_count;
 
-    // Most recent gyro reading obtained by ::update
+    // the selected sample rate
+    Sample_rate _sample_rate;
+    
+    // Most recent accelerometer reading
+    Vector3f _accel[INS_MAX_INSTANCES];
+
+    // Most recent gyro reading
     Vector3f _gyro[INS_MAX_INSTANCES];
 
     // product id
     AP_Int16 _product_id;
 
     // accelerometer scaling and offsets
-    AP_Vector3f             _accel_scale[INS_MAX_INSTANCES];
-    AP_Vector3f             _accel_offset[INS_MAX_INSTANCES];
-    AP_Vector3f             _gyro_offset[INS_MAX_INSTANCES];
+    AP_Vector3f _accel_scale[INS_MAX_INSTANCES];
+    AP_Vector3f _accel_offset[INS_MAX_INSTANCES];
+    AP_Vector3f _gyro_offset[INS_MAX_INSTANCES];
 
     // filtering frequency (0 means default)
-    AP_Int8                 _mpu6000_filter;
+    AP_Int8     _mpu6000_filter;
 
     // board orientation from AHRS
-    enum Rotation			_board_orientation;
+    enum Rotation _board_orientation;
 
     // calibrated_ok flags
-    bool                    _gyro_cal_ok[INS_MAX_INSTANCES];
+    bool _gyro_cal_ok[INS_MAX_INSTANCES];
+
+    // primary accel and gyro
+    uint8_t _primary_gyro;
+    uint8_t _primary_accel;
+
+    // has wait_for_sample() found a sample?
+    bool _have_sample:1;
+
+    // are we in HIL mode?
+    bool _hil_mode:1;
+
+    // do we have offsets/scaling from a 3D calibration?
+    bool _have_3D_calibration:1;
+
+    // the delta time in seconds for the last sample
+    float _delta_time;
+
+    // last time a wait_for_sample() returned a sample
+    uint32_t _last_sample_usec;
+
+    // target time for next wait_for_sample() return
+    uint32_t _next_sample_usec;
+    
+    // time between samples in microseconds
+    uint32_t _sample_period_usec;
+
+    // health of gyros and accels
+    bool _gyro_healthy[INS_MAX_INSTANCES];
+    bool _accel_healthy[INS_MAX_INSTANCES];
+
+    uint32_t _accel_error_count[INS_MAX_INSTANCES];
+    uint32_t _gyro_error_count[INS_MAX_INSTANCES];
 };
 
-#include "AP_InertialSensor_Oilpan.h"
+#include "AP_InertialSensor_Backend.h"
 #include "AP_InertialSensor_MPU6000.h"
-#include "AP_InertialSensor_HIL.h"
 #include "AP_InertialSensor_PX4.h"
-#include "AP_InertialSensor_VRBRAIN.h"
-#include "AP_InertialSensor_UserInteract_Stream.h"
-#include "AP_InertialSensor_UserInteract_MAVLink.h"
-#include "AP_InertialSensor_Flymaple.h"
+#include "AP_InertialSensor_Oilpan.h"
+#include "AP_InertialSensor_MPU9250.h"
 #include "AP_InertialSensor_L3G4200D.h"
+#include "AP_InertialSensor_Flymaple.h"
 #include "AP_InertialSensor_MPU9150.h"
-#include "AP_InertialSensor_MPU9250.h"
+#include "AP_InertialSensor_HIL.h"
+#include "AP_InertialSensor_UserInteract_Stream.h"
+#include "AP_InertialSensor_UserInteract_MAVLink.h"
 
 #endif // __AP_INERTIAL_SENSOR_H__
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
index ccc481c086338bf9b8287d119f7e5b523797a7dd..684cb327e98682e4056648f05411f6ed6db77c60 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
@@ -14,7 +14,7 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 /*
-  Flymaple port by Mike McCauley
+  Flymaple IMU driver by Mike McCauley
  */
 
 // Interface to the Flymaple sensors:
@@ -28,20 +28,6 @@
 
 const extern AP_HAL::HAL& hal;
 
-/// Statics
-Vector3f AP_InertialSensor_Flymaple::_accel_filtered;
-uint32_t AP_InertialSensor_Flymaple::_accel_samples;
-Vector3f AP_InertialSensor_Flymaple::_gyro_filtered;
-uint32_t AP_InertialSensor_Flymaple::_gyro_samples;
-uint64_t AP_InertialSensor_Flymaple::_last_accel_timestamp;
-uint64_t AP_InertialSensor_Flymaple::_last_gyro_timestamp;
-LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_x(800, 10);
-LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_y(800, 10);
-LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_z(800, 10);
-LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_x(800, 10);
-LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_y(800, 10);
-LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_z(800, 10);
-
 // This is how often we wish to make raw samples of the sensors in Hz
 const uint32_t  raw_sample_rate_hz = 800;
 // And the equivalent time between samples in microseconds
@@ -77,25 +63,39 @@ const uint32_t  raw_sample_interval_us = (1000000 / raw_sample_rate_hz);
 // Result wil be radians/sec
 #define FLYMAPLE_GYRO_SCALE_R_S (1.0f / 14.375f) * (3.1415926f / 180.0f)
 
-uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate ) 
+AP_InertialSensor_Flymaple::AP_InertialSensor_Flymaple(AP_InertialSensor &imu) :
+    AP_InertialSensor_Backend(imu),
+    _have_gyro_sample(false),
+    _have_accel_sample(false),
+    _accel_filter_x(raw_sample_rate_hz, 10),
+    _accel_filter_y(raw_sample_rate_hz, 10),
+    _accel_filter_z(raw_sample_rate_hz, 10),
+    _gyro_filter_x(raw_sample_rate_hz, 10),
+    _gyro_filter_y(raw_sample_rate_hz, 10),
+    _gyro_filter_z(raw_sample_rate_hz, 10),
+    _last_gyro_timestamp(0),
+    _last_accel_timestamp(0)
+{}
+
+/*
+  detect the sensor
+ */
+AP_InertialSensor_Backend *AP_InertialSensor_Flymaple::detect(AP_InertialSensor &_imu)
 {
-    // Sensors are raw sampled at 800Hz.
-    // Here we figure the divider to get the rate that update should be called
-    switch (sample_rate) {
-    case RATE_50HZ:
-        _sample_divider = raw_sample_rate_hz / 50;
-        _default_filter_hz = 10;
-        break;
-    case RATE_100HZ:
-        _sample_divider = raw_sample_rate_hz / 100;
-        _default_filter_hz = 20;
-        break;
-    case RATE_200HZ:
-    default:
-        _sample_divider = raw_sample_rate_hz / 200;
-        _default_filter_hz = 20;
-        break;
+    AP_InertialSensor_Flymaple *sensor = new AP_InertialSensor_Flymaple(_imu);
+    if (sensor == NULL) {
+        return NULL;
     }
+    if (!sensor->_init_sensor()) {
+        delete sensor;
+        return NULL;
+    }
+    return sensor;
+}
+
+bool AP_InertialSensor_Flymaple::_init_sensor(void) 
+{
+    _default_filter_hz = _default_filter();
 
     // get pointer to i2c bus semaphore
     AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
@@ -146,12 +146,17 @@ uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
     hal.scheduler->delay(1);
 
     // Set up the filter desired
-    _set_filter_frequency(_mpu6000_filter);
+    _set_filter_frequency(_imu.get_filter());
 
-   // give back i2c semaphore
+    // give back i2c semaphore
     i2c_sem->give();
 
-    return AP_PRODUCT_ID_FLYMAPLE;
+    _gyro_instance = _imu.register_gyro();
+    _accel_instance = _imu.register_accel();
+
+    _product_id = AP_PRODUCT_ID_FLYMAPLE;
+
+    return true;
 }
 
 /*
@@ -170,109 +175,46 @@ void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
     _gyro_filter_z.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
 }
 
-/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
 
 // This takes about 20us to run
 bool AP_InertialSensor_Flymaple::update(void) 
 {
-    if (!wait_for_sample(100)) {
-        return false;
-    }
-    Vector3f accel_scale = _accel_scale[0].get();
+    Vector3f accel, gyro;
 
-    // Not really needed since Flymaple _accumulate runs in the main thread
     hal.scheduler->suspend_timer_procs();
-
-    // base the time on the gyro timestamp, as that is what is
-    // multiplied by time to integrate in DCM
-    _delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f;
-    _last_update_usec = _last_gyro_timestamp;
-
-    _previous_accel[0] = _accel[0];
-
-    _accel[0] = _accel_filtered;
-    _accel_samples = 0;
-
-    _gyro[0] = _gyro_filtered;
-    _gyro_samples = 0;
-
+    accel = _accel_filtered;
+    gyro = _gyro_filtered;
+    _have_gyro_sample = false;
+    _have_accel_sample = false;
     hal.scheduler->resume_timer_procs();
 
-    // add offsets and rotation
-    _accel[0].rotate(_board_orientation);
-
     // Adjust for chip scaling to get m/s/s
-    _accel[0] *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
-
-    // Now the calibration scale factor
-    _accel[0].x *= accel_scale.x;
-    _accel[0].y *= accel_scale.y;
-    _accel[0].z *= accel_scale.z;
-    _accel[0]   -= _accel_offset[0];
-
-    _gyro[0].rotate(_board_orientation);
+    accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
+    _rotate_and_offset_accel(_accel_instance, accel);
 
     // Adjust for chip scaling to get radians/sec
-    _gyro[0] *= FLYMAPLE_GYRO_SCALE_R_S;
-    _gyro[0] -= _gyro_offset[0];
+    gyro *= FLYMAPLE_GYRO_SCALE_R_S;
+    _rotate_and_offset_gyro(_gyro_instance, gyro);
 
-    if (_last_filter_hz != _mpu6000_filter) {
-        _set_filter_frequency(_mpu6000_filter);
-        _last_filter_hz = _mpu6000_filter;
+    if (_last_filter_hz != _imu.get_filter()) {
+        _set_filter_frequency(_imu.get_filter());
+        _last_filter_hz = _imu.get_filter();
     }
 
     return true;
 }
 
-bool AP_InertialSensor_Flymaple::get_gyro_health(void) const
-{
-    if (_last_gyro_timestamp == 0) {
-        // not initialised yet, show as healthy to prevent scary GCS
-        // warnings
-        return true;
-    }
-    uint64_t now = hal.scheduler->micros();
-    if ((now - _last_gyro_timestamp) >= (2 * raw_sample_interval_us)) {
-        // gyros have not updated
-        return false;
-    }
-    return true;
-}
-
-bool AP_InertialSensor_Flymaple::get_accel_health(void) const
-{
-    if (_last_accel_timestamp == 0) {
-        // not initialised yet, show as healthy to prevent scary GCS
-        // warnings
-        return true;
-    }
-    uint64_t now = hal.scheduler->micros();
-    if ((now - _last_accel_timestamp) >= (2 * raw_sample_interval_us)) {
-        // gyros have not updated
-        return false;
-    }
-    return true;
-}
-
-float AP_InertialSensor_Flymaple::get_delta_time(void) const
-{
-    return _delta_time;
-}
-
-float AP_InertialSensor_Flymaple::get_gyro_drift_rate(void) 
-{
-    // Dont really know this for the ITG-3200.
-    // 0.5 degrees/second/minute
-    return ToRad(0.5/60);
-}
-
 // This needs to get called as often as possible.
 // Its job is to accumulate samples as fast as is reasonable for the accel and gyro
 // sensors.
-// Cant call this from within the system timers, since the long I2C reads (up to 1ms) 
-// with interrupts disabled breaks lots of things
-// Therefore must call this as often as possible from
-// within the mainline and thropttle the reads here to suit the sensors
+// Note that this is called from gyro_sample_available() and
+// accel_sample_available(), which is really not good enough for
+// 800Hz, as it is common for the main loop to take more than 1.5ms
+// before wait_for_sample() is called again. We can't just call this
+// from a timer as timers run with interrupts disabled, and the I2C
+// operations take too long
+// So we are stuck with a suboptimal solution. The results are not so
+// good in terms of timing. It may be better with the FIFOs enabled
 void AP_InertialSensor_Flymaple::_accumulate(void)
 {
     // get pointer to i2c bus semaphore
@@ -285,7 +227,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
     // Read accelerometer
     // ADXL345 is in the default FIFO bypass mode, so the FIFO is not used
     uint8_t buffer[6];
-    uint64_t now = hal.scheduler->micros();
+    uint32_t now = hal.scheduler->micros();
     // This takes about 250us at 400kHz I2C speed
     if ((now - _last_accel_timestamp) >= raw_sample_interval_us
         && hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0)
@@ -300,7 +242,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
         _accel_filtered = Vector3f(_accel_filter_x.apply(x),
                                    _accel_filter_y.apply(y),
                                    _accel_filter_z.apply(z));
-        _accel_samples++;
+        _have_accel_sample = true;
         _last_accel_timestamp = now;
     }
 
@@ -317,7 +259,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
         _gyro_filtered = Vector3f(_gyro_filter_x.apply(x),
                                   _gyro_filter_y.apply(y),
                                   _gyro_filter_z.apply(z));
-        _gyro_samples++;
+        _have_gyro_sample = true;
         _last_gyro_timestamp = now;
     }
 
@@ -325,26 +267,4 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
     i2c_sem->give();
 }
 
-bool AP_InertialSensor_Flymaple::_sample_available(void)
-{
-    _accumulate();
-    return min(_accel_samples, _gyro_samples) / _sample_divider > 0;
-}
-
-bool AP_InertialSensor_Flymaple::wait_for_sample(uint16_t timeout_ms)
-{
-    if (_sample_available()) {
-        return true;
-    }
-    uint32_t start = hal.scheduler->millis();
-    while ((hal.scheduler->millis() - start) < timeout_ms) {
-        hal.scheduler->delay_microseconds(100);
-        if (_sample_available()) {
-            return true;
-        }
-    }
-    return false;
-}
-
 #endif // CONFIG_HAL_BOARD
-
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
index b08546dd64db0a8433655cb5c694e700d29a48db..724e03dbb4991cb11424347178be8900be60c1e8 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
@@ -6,39 +6,31 @@
 #include <AP_HAL.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
 
-#include <AP_Progmem.h>
 #include "AP_InertialSensor.h"
 #include <Filter.h>
 #include <LowPassFilter2p.h>
 
-class AP_InertialSensor_Flymaple : public AP_InertialSensor
+class AP_InertialSensor_Flymaple : public AP_InertialSensor_Backend
 {
 public:
+    AP_InertialSensor_Flymaple(AP_InertialSensor &imu);
 
-    AP_InertialSensor_Flymaple() : AP_InertialSensor() {}
+    /* update accel and gyro state */
+    bool update();
 
-    /* Concrete implementation of AP_InertialSensor functions: */
-    bool            update();
-    float        	get_delta_time() const;
-    float           get_gyro_drift_rate();
-    bool            wait_for_sample(uint16_t timeout_ms);
-    bool            get_gyro_health(void) const;
-    bool            get_accel_health(void) const;
-    bool            healthy(void) const { return get_gyro_health() && get_accel_health(); }
+    bool gyro_sample_available(void) { _accumulate(); return _have_gyro_sample; }
+    bool accel_sample_available(void) { _accumulate(); return _have_accel_sample; }
+
+    // detect the sensor
+    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
 
 private:
-    uint16_t        _init_sensor( Sample_rate sample_rate );
-    static          void _accumulate(void);
-    bool            _sample_available();
-    uint64_t        _last_update_usec;
-    float           _delta_time;
-    static Vector3f	_accel_filtered;
-    static uint32_t _accel_samples;
-    static Vector3f	_gyro_filtered;
-    static uint32_t _gyro_samples;
-    static uint64_t _last_accel_timestamp;
-    static uint64_t _last_gyro_timestamp;
-    uint8_t  _sample_divider;
+    bool        _init_sensor(void);
+    void        _accumulate(void);
+    Vector3f	_accel_filtered;
+    Vector3f	_gyro_filtered;
+    bool        _have_gyro_sample;
+    bool        _have_accel_sample;
 
     // support for updating filter at runtime
     uint8_t _last_filter_hz;
@@ -46,12 +38,18 @@ private:
 
     void _set_filter_frequency(uint8_t filter_hz);
     // Low Pass filters for gyro and accel 
-    static LowPassFilter2p _accel_filter_x;
-    static LowPassFilter2p _accel_filter_y;
-    static LowPassFilter2p _accel_filter_z;
-    static LowPassFilter2p _gyro_filter_x;
-    static LowPassFilter2p _gyro_filter_y;
-    static LowPassFilter2p _gyro_filter_z;
+    LowPassFilter2p _accel_filter_x;
+    LowPassFilter2p _accel_filter_y;
+    LowPassFilter2p _accel_filter_z;
+    LowPassFilter2p _gyro_filter_x;
+    LowPassFilter2p _gyro_filter_y;
+    LowPassFilter2p _gyro_filter_z;
+
+    uint8_t _gyro_instance;
+    uint8_t _accel_instance;
+
+    uint32_t _last_gyro_timestamp;
+    uint32_t _last_accel_timestamp;
 };
 #endif
 #endif // __AP_INERTIAL_SENSOR_FLYMAPLE_H__
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
index bbb5e8d8ee7dfd4a777b133b26acae7e0ab0a26a..50bfb4d6b6fabcbf5f1514eaee3ee3286159afa8 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
@@ -1,130 +1,46 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#include "AP_InertialSensor_HIL.h"
 #include <AP_HAL.h>
-const extern AP_HAL::HAL& hal;
-
-AP_InertialSensor_HIL::AP_InertialSensor_HIL() :
-    AP_InertialSensor(),
-    _sample_period_usec(0),
-    _last_sample_usec(0)
-{
-    _accel[0] = Vector3f(0, 0, -GRAVITY_MSS);
-}
-
-uint16_t AP_InertialSensor_HIL::_init_sensor( Sample_rate sample_rate ) {
-    switch (sample_rate) {
-    case RATE_50HZ:
-        _sample_period_usec = 20000;
-        break;
-    case RATE_100HZ:
-        _sample_period_usec = 10000;
-        break;
-    case RATE_200HZ:
-        _sample_period_usec = 5000;
-        break;
-    case RATE_400HZ:
-        _sample_period_usec = 2500;
-        break;
-    }
-    return AP_PRODUCT_ID_NONE;
-}
-
-/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
-
-bool AP_InertialSensor_HIL::update( void ) {
-    uint32_t now = hal.scheduler->micros();
-    _last_sample_usec = now;
-    return true;
-}
-
-float AP_InertialSensor_HIL::get_delta_time() const {
-    return _sample_period_usec * 1.0e-6f;
-}
-
-float AP_InertialSensor_HIL::get_gyro_drift_rate(void) {
-    // 0.5 degrees/second/minute
-    return ToRad(0.5/60);
-}
+#include "AP_InertialSensor_HIL.h"
 
-bool AP_InertialSensor_HIL::_sample_available()
-{
-    uint32_t tnow = hal.scheduler->micros();
-    bool have_sample = false;
-    while (tnow - _last_sample_usec > _sample_period_usec) {
-        have_sample = true;
-        _last_sample_usec += _sample_period_usec;
-    }
-    return have_sample;
-}
+const extern AP_HAL::HAL& hal;
 
-bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
+AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu) :
+    AP_InertialSensor_Backend(imu)
 {
-    if (_sample_available()) {
-        return true;
-    }
-    uint32_t start = hal.scheduler->millis();
-    while ((hal.scheduler->millis() - start) < timeout_ms) {
-        uint32_t tnow = hal.scheduler->micros();
-        uint32_t tdelay = (_last_sample_usec + _sample_period_usec) - tnow;
-        if (tdelay < 100000) {
-            hal.scheduler->delay_microseconds(tdelay);
-        }
-        if (_sample_available()) {
-            return true;
-        }
-    }
-    return false;
 }
 
-void AP_InertialSensor_HIL::set_accel(uint8_t instance, const Vector3f &accel)
+/*
+  detect the sensor
+ */
+AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu)
 {
-    if (instance >= INS_MAX_INSTANCES) {
-        return;
+    AP_InertialSensor_HIL *sensor = new AP_InertialSensor_HIL(_imu);
+    if (sensor == NULL) {
+        return NULL;
     }
-    _previous_accel[instance] = _accel[instance];
-    _accel[instance] = accel;
-    _last_accel_usec[instance] = hal.scheduler->micros();
-}
-
-void AP_InertialSensor_HIL::set_gyro(uint8_t instance, const Vector3f &gyro)
-{
-    if (instance >= INS_MAX_INSTANCES) {
-        return;
+    if (!sensor->_init_sensor()) {
+        delete sensor;
+        return NULL;
     }
-    _gyro[instance] = gyro;
-    _last_gyro_usec[instance] = hal.scheduler->micros();
+    return sensor;
 }
 
-bool AP_InertialSensor_HIL::get_gyro_health(uint8_t instance) const
+bool AP_InertialSensor_HIL::_init_sensor(void) 
 {
-    if (instance >= INS_MAX_INSTANCES) {
-        return false;
-    }
-    return (hal.scheduler->micros() - _last_gyro_usec[instance]) < 40000;
-}
+    // grab the used instances
+    _imu.register_gyro();
+    _imu.register_accel();
 
-bool AP_InertialSensor_HIL::get_accel_health(uint8_t instance) const
-{
-    if (instance >= INS_MAX_INSTANCES) {
-        return false;
-    }
-    return (hal.scheduler->micros() - _last_accel_usec[instance]) < 40000;
-}
+    _product_id = AP_PRODUCT_ID_NONE;
+    _imu.set_hil_mode();
 
-uint8_t AP_InertialSensor_HIL::get_gyro_count(void) const
-{
-    if (get_gyro_health(1)) {
-        return 2;
-    }
-    return 1;
+    return true;
 }
 
-uint8_t AP_InertialSensor_HIL::get_accel_count(void) const
+bool AP_InertialSensor_HIL::update(void) 
 {
-    if (get_accel_health(1)) {
-        return 2;
-    }
-    return 1;
+    // the data is stored directly in the frontend, so update()
+    // doesn't need to do anything
+    return true;
 }
-
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
index 55ae828ad835c6ed77475d8cfde181aa3d3276d0..9c1422c662ffadcdaab44bafadcd28d8c63a3e87 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
@@ -1,36 +1,26 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#ifndef __AP_INERTIAL_SENSOR_STUB_H__
-#define __AP_INERTIAL_SENSOR_STUB_H__
+#ifndef __AP_INERTIALSENSOR_HIL_H__
+#define __AP_INERTIALSENSOR_HIL_H__
 
-#include <AP_Progmem.h>
 #include "AP_InertialSensor.h"
 
-class AP_InertialSensor_HIL : public AP_InertialSensor
+class AP_InertialSensor_HIL : public AP_InertialSensor_Backend
 {
 public:
+    AP_InertialSensor_HIL(AP_InertialSensor &imu);
 
-    AP_InertialSensor_HIL();
+    /* update accel and gyro state */
+    bool update();
 
-    /* Concrete implementation of AP_InertialSensor functions: */
-    bool            update();
-    float	        get_delta_time() const;
-    float           get_gyro_drift_rate();
-    bool            wait_for_sample(uint16_t timeout_ms);
-    void            set_accel(uint8_t instance, const Vector3f &accel);
-    void            set_gyro(uint8_t instance, const Vector3f &gyro);
-    bool            get_gyro_health(uint8_t instance) const;
-    bool            get_accel_health(uint8_t instance) const;
-    uint8_t         get_gyro_count(void) const;
-    uint8_t         get_accel_count(void) const;
+    bool gyro_sample_available(void) { return true; }
+    bool accel_sample_available(void) { return true; }
+
+    // detect the sensor
+    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
 
 private:
-    bool            _sample_available();
-    uint16_t        _init_sensor( Sample_rate sample_rate );
-    uint32_t         _sample_period_usec;
-    uint32_t        _last_sample_usec;
-    uint32_t        _last_accel_usec[INS_MAX_INSTANCES];
-    uint32_t        _last_gyro_usec[INS_MAX_INSTANCES];
+    bool _init_sensor(void);
 };
 
-#endif // __AP_INERTIAL_SENSOR_STUB_H__
+#endif // __AP_INERTIALSENSOR_HIL_H__
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
index 60281bd55980d91a944adfdd3bb374e0075f0681..e1e4c5ce25b68078c53f933e5e872922b8f3e815 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
@@ -16,9 +16,17 @@
 /*
   This is an INS driver for the combination L3G4200D gyro and ADXL345 accelerometer.
   This combination is available as a cheap 10DOF sensor on ebay
- */
-// ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf
-// L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf
+
+  This sensor driver is an example only - it should not be used in any
+  serious autopilot as the latencies on I2C prevent good timing at
+  high sample rates. It is useful when doing an initial port of
+  ardupilot to a board where only I2C is available, and a cheap sensor
+  can be used.
+
+Datasheets:
+  ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf
+  L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf
+*/
 
 #include <AP_HAL.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
@@ -103,8 +111,10 @@ const extern AP_HAL::HAL& hal;
 #define L3G4200D_GYRO_SCALE_R_S (DEG_TO_RAD * 70.0f * 0.001f)
 
 // constructor
-AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D() :
-    AP_InertialSensor(),
+AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu) :
+    AP_InertialSensor_Backend(imu),
+    _have_gyro_sample(false),
+    _have_accel_sample(false),
     _accel_filter_x(800, 10),
     _accel_filter_y(800, 10),
     _accel_filter_z(800, 10),
@@ -113,27 +123,9 @@ AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D() :
     _gyro_filter_z(800, 10)
 {}
 
-uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate ) 
+bool AP_InertialSensor_L3G4200D::_init_sensor(void) 
 {
-
-    switch (sample_rate) {
-    case RATE_50HZ:
-        _default_filter_hz = 10;
-        _sample_period_usec = (1000*1000) / 50;
-        _gyro_samples_needed = 16;
-        break;
-    case RATE_100HZ:
-        _default_filter_hz = 20;
-        _sample_period_usec = (1000*1000) / 100;
-        _gyro_samples_needed = 8;
-        break;
-    case RATE_200HZ:
-    default:
-        _default_filter_hz = 20;
-        _sample_period_usec = (1000*1000) / 200;
-        _gyro_samples_needed = 4;
-        break;
-    }
+    _default_filter_hz = _default_filter();
 
     // get pointer to i2c bus semaphore
     AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
@@ -219,7 +211,7 @@ uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate )
                            
 
     // Set up the filter desired
-    _set_filter_frequency(_mpu6000_filter);
+    _set_filter_frequency(_imu.get_filter());
 
     // give back i2c semaphore
     i2c_sem->give();
@@ -227,15 +219,12 @@ uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate )
     // start the timer process to read samples
     hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_L3G4200D::_accumulate));
 
-    clock_gettime(CLOCK_MONOTONIC, &_next_sample_ts);
+    _gyro_instance = _imu.register_gyro();
+    _accel_instance = _imu.register_accel();
 
-    _next_sample_ts.tv_nsec += _sample_period_usec * 1000UL;
-    if (_next_sample_ts.tv_nsec >= 1.0e9) {
-        _next_sample_ts.tv_nsec -= 1.0e9;
-        _next_sample_ts.tv_sec++;
-    }
+    _product_id = AP_PRODUCT_ID_L3G4200D;
 
-    return AP_PRODUCT_ID_L3G4200D;
+    return true;
 }
 
 /*
@@ -254,58 +243,36 @@ void AP_InertialSensor_L3G4200D::_set_filter_frequency(uint8_t filter_hz)
     _gyro_filter_z.set_cutoff_frequency(800, filter_hz);
 }
 
-/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
-
+/*
+  copy filtered data to the frontend
+ */
 bool AP_InertialSensor_L3G4200D::update(void) 
 {
-    Vector3f accel_scale = _accel_scale[0].get();
-
-    _previous_accel[0] = _accel[0];
+    Vector3f accel, gyro;
 
     hal.scheduler->suspend_timer_procs();
-    _accel[0] = _accel_filtered;
-    _gyro[0] = _gyro_filtered;
-    _delta_time = _gyro_samples_available * (1.0f/800);    
-    _gyro_samples_available = 0;
+    accel = _accel_filtered;
+    gyro = _gyro_filtered;
+    _have_gyro_sample = false;
+    _have_accel_sample = false;
     hal.scheduler->resume_timer_procs();
 
-    // add offsets and rotation
-    _accel[0].rotate(_board_orientation);
-
     // Adjust for chip scaling to get m/s/s
-    _accel[0] *= ADXL345_ACCELEROMETER_SCALE_M_S;
-
-    // Now the calibration scale factor
-    _accel[0].x *= accel_scale.x;
-    _accel[0].y *= accel_scale.y;
-    _accel[0].z *= accel_scale.z;
-    _accel[0]   -= _accel_offset[0];
-
-    _gyro[0].rotate(_board_orientation);
+    accel *= ADXL345_ACCELEROMETER_SCALE_M_S;
+    _rotate_and_offset_accel(_accel_instance, accel);
 
     // Adjust for chip scaling to get radians/sec
-    _gyro[0] *= L3G4200D_GYRO_SCALE_R_S;
-    _gyro[0] -= _gyro_offset[0];
+    gyro *= L3G4200D_GYRO_SCALE_R_S;
+    _rotate_and_offset_gyro(_gyro_instance, gyro);
 
-    if (_last_filter_hz != _mpu6000_filter) {
-        _set_filter_frequency(_mpu6000_filter);
-        _last_filter_hz = _mpu6000_filter;
+    if (_last_filter_hz != _imu.get_filter()) {
+        _set_filter_frequency(_imu.get_filter());
+        _last_filter_hz = _imu.get_filter();
     }
 
     return true;
 }
 
-float AP_InertialSensor_L3G4200D::get_delta_time(void) const
-{
-    return _delta_time;
-}
-
-float AP_InertialSensor_L3G4200D::get_gyro_drift_rate(void) 
-{
-    // 0.5 degrees/second/minute (a guess)
-    return ToRad(0.5/60);
-}
-
 // Accumulate values from accels and gyros
 void AP_InertialSensor_L3G4200D::_accumulate(void)
 {
@@ -344,19 +311,17 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
                 _gyro_filtered = Vector3f(_gyro_filter_x.apply(buffer[i][0]), 
                                           _gyro_filter_y.apply(-buffer[i][1]), 
                                           _gyro_filter_z.apply(-buffer[i][2]));
-                _gyro_samples_available++;
+                _have_gyro_sample = true;
             }
         }
     }
 
-
     // Read accelerometer FIFO to find out how many samples are available
     hal.i2c->readRegister(ADXL345_ACCELEROMETER_ADDRESS,
                           ADXL345_ACCELEROMETER_ADXLREG_FIFO_STATUS,
                           &fifo_status);
     num_samples_available = fifo_status & 0x3F;
 
-#if 1
     // read the samples and apply the filter
     if (num_samples_available > 0) {
         int16_t buffer[num_samples_available][3];
@@ -368,35 +333,14 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
                 _accel_filtered = Vector3f(_accel_filter_x.apply(buffer[i][0]),
                                            _accel_filter_y.apply(-buffer[i][1]),
                                            _accel_filter_z.apply(-buffer[i][2]));
+                _have_accel_sample = true;
             }
         }
     }
-#endif
 
     // give back i2c semaphore
     i2c_sem->give();
 }
 
-bool AP_InertialSensor_L3G4200D::_sample_available(void)
-{
-    return (_gyro_samples_available >= _gyro_samples_needed);
-}
-
-bool AP_InertialSensor_L3G4200D::wait_for_sample(uint16_t timeout_ms)
-{
-    uint32_t start_us = hal.scheduler->micros();
-    while (clock_nanosleep(CLOCK_MONOTONIC, 
-                           TIMER_ABSTIME, &_next_sample_ts, NULL) == -1 && errno == EINTR) ;
-    _next_sample_ts.tv_nsec += _sample_period_usec * 1000UL;
-    if (_next_sample_ts.tv_nsec >= 1.0e9) {
-        _next_sample_ts.tv_nsec -= 1.0e9;
-        _next_sample_ts.tv_sec++;
-
-    }
-    //_accumulate();
-    return true;
-
-}
-
 #endif // CONFIG_HAL_BOARD
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
index 5aa035a6aa6b62db131cac00fe36ac3318e14f71..674dc87c15f36057e85c243b94ecd0318b6ab918 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
@@ -6,40 +6,39 @@
 #include <AP_HAL.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 
-#include <AP_Progmem.h>
 #include "AP_InertialSensor.h"
 #include <Filter.h>
 #include <LowPassFilter2p.h>
 
-class AP_InertialSensor_L3G4200D : public AP_InertialSensor
+class AP_InertialSensor_L3G4200D : public AP_InertialSensor_Backend
 {
 public:
 
-    AP_InertialSensor_L3G4200D();
+    AP_InertialSensor_L3G4200D(AP_InertialSensor &imu);
 
-    /* Concrete implementation of AP_InertialSensor functions: */
-    bool            update();
-    float        	get_delta_time() const;
-    float           get_gyro_drift_rate();
-    bool            wait_for_sample(uint16_t timeout_ms);
+    /* update accel and gyro state */
+    bool update();
+
+    bool gyro_sample_available(void) { return _have_gyro_sample; }
+    bool accel_sample_available(void) { return _have_accel_sample; }
+
+    // detect the sensor
+    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
+
+    // return product ID
+    int16_t product_id(void) const { return AP_PRODUCT_ID_L3G4200D; }
 
 private:
-    uint16_t        _init_sensor( Sample_rate sample_rate );
-    void             _accumulate(void);
-    bool            _sample_available();
-    uint64_t        _last_update_usec;
+    bool            _init_sensor(void);
+    void            _accumulate(void);
     Vector3f        _accel_filtered;
     Vector3f        _gyro_filtered;
-    uint32_t        _sample_period_usec;
-    uint32_t        _last_sample_time;
-    volatile uint32_t _gyro_samples_available;
-    volatile uint8_t  _gyro_samples_needed;
-    float           _delta_time;
-    struct timespec _next_sample_ts;
+    volatile bool   _have_gyro_sample;
+    volatile bool   _have_accel_sample;
 
     // support for updating filter at runtime
     uint8_t         _last_filter_hz;
-    uint8_t          _default_filter_hz;
+    uint8_t         _default_filter_hz;
 
     void _set_filter_frequency(uint8_t filter_hz);
 
@@ -50,6 +49,10 @@ private:
     LowPassFilter2p _gyro_filter_x;
     LowPassFilter2p _gyro_filter_y;
     LowPassFilter2p _gyro_filter_z;
+
+    // gyro and accel instances
+    uint8_t _gyro_instance;
+    uint8_t _accel_instance;
 };
 #endif
 #endif // __AP_INERTIAL_SENSOR_L3G4200D_H__
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index a79e01dd99b1438b1b7926a00bf75b9012d5009c..849851fb883d609d6de3889ff6017883490b035d 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -173,26 +173,51 @@ const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532f / 16.4f);
  *  variants however
  */
 
-AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000() : 
-	AP_InertialSensor(),
+AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu) :
+    AP_InertialSensor_Backend(imu),
     _drdy_pin(NULL),
     _spi(NULL),
     _spi_sem(NULL),
-    _num_samples(0),
-    _last_sample_time_micros(0),
-    _initialised(false),
-    _mpu6000_product_id(AP_PRODUCT_ID_NONE),
-    _sample_shift(0),
     _last_filter_hz(0),
-    _error_count(0)
+    _error_count(0),
+#if MPU6000_FAST_SAMPLING
+    _accel_filter_x(1000, 15),
+    _accel_filter_y(1000, 15),
+    _accel_filter_z(1000, 15),
+    _gyro_filter_x(1000, 15),
+    _gyro_filter_y(1000, 15),
+    _gyro_filter_z(1000, 15),    
+#else
+    _sample_count(0),
+    _accel_sum(),
+    _gyro_sum(),
+#endif
+    _sum_count(0)
 {
 }
 
-uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate )
+/*
+  detect the sensor
+ */
+AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect(AP_InertialSensor &_imu)
 {
-    if (_initialised) return _mpu6000_product_id;
-    _initialised = true;
+    AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu);
+    if (sensor == NULL) {
+        return NULL;
+    }
+    if (!sensor->_init_sensor()) {
+        delete sensor;
+        return NULL;
+    }
 
+    return sensor;
+}
+
+/*
+  initialise the sensor
+ */
+bool AP_InertialSensor_MPU6000::_init_sensor(void)
+{
     _spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
     _spi_sem = _spi->get_semaphore();
 
@@ -205,103 +230,85 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate )
 
     uint8_t tries = 0;
     do {
-        bool success = _hardware_init(sample_rate);
+        bool success = _hardware_init();
         if (success) {
             hal.scheduler->delay(5+2);
             if (!_spi_sem->take(100)) {
-                hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
+                return false;
             }
             if (_data_ready()) {
                 _spi_sem->give();
                 break;
             } else {
-                hal.console->println_P(
-                        PSTR("MPU6000 startup failed: no data ready"));
+                return false;
             }
             _spi_sem->give();
         }
         if (tries++ > 5) {
-            hal.scheduler->panic(PSTR("PANIC: failed to boot MPU6000 5 times")); 
+            hal.console->print_P(PSTR("failed to boot MPU6000 5 times")); 
+            return false;
         }
     } while (1);
 
+    // grab the used instances
+    _gyro_instance = _imu.register_gyro();
+    _accel_instance = _imu.register_accel();
+
     hal.scheduler->resume_timer_procs();
     
-
-    /* read the first lot of data.
-     * _read_data_transaction requires the spi semaphore to be taken by
-     * its caller. */
-    _last_sample_time_micros = hal.scheduler->micros();
-    hal.scheduler->delay(10);
-    if (_spi_sem->take(100)) {
-        _read_data_transaction();
-        _spi_sem->give();
-    }
-
     // start the timer process to read samples
     hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU6000::_poll_data));
 
 #if MPU6000_DEBUG
     _dump_registers();
 #endif
-    return _mpu6000_product_id;
-}
 
-/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
-
-bool AP_InertialSensor_MPU6000::wait_for_sample(uint16_t timeout_ms)
-{
-    if (_sample_available()) {
-        return true;
-    }
-    uint32_t start = hal.scheduler->millis();
-    while ((hal.scheduler->millis() - start) < timeout_ms) {
-        hal.scheduler->delay_microseconds(100);
-        if (_sample_available()) {
-            return true;
-        }
-    }
-    return false;
+    return true;
 }
 
+
+/*
+  process any 
+ */
 bool AP_InertialSensor_MPU6000::update( void )
-{
-    // wait for at least 1 sample
-    if (!wait_for_sample(1000)) {
+{    
+#if !MPU6000_FAST_SAMPLING
+    if (_sum_count < _sample_count) {
+        // we don't have enough samples yet
         return false;
     }
+#endif
 
-    _previous_accel[0] = _accel[0];
+    // we have a full set of samples
+    uint16_t num_samples;
+    Vector3f accel, gyro;
 
-    // disable timer procs for mininum time
     hal.scheduler->suspend_timer_procs();
-    _gyro[0]  = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
-    _accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z);
-    _num_samples = _sum_count;
+#if MPU6000_FAST_SAMPLING
+    gyro = _gyro_filtered;
+    accel = _accel_filtered;
+    num_samples = 1;
+#else
+    gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
+    accel(_accel_sum.x, _accel_sum.y, _accel_sum.z);
+    num_samples = _sum_count;
     _accel_sum.zero();
     _gyro_sum.zero();
+#endif
     _sum_count = 0;
     hal.scheduler->resume_timer_procs();
 
-    _gyro[0].rotate(_board_orientation);
-    _gyro[0] *= _gyro_scale / _num_samples;
-    _gyro[0] -= _gyro_offset[0];
-
-    _accel[0].rotate(_board_orientation);
-    _accel[0] *= MPU6000_ACCEL_SCALE_1G / _num_samples;
+    gyro *= _gyro_scale / num_samples;
+    _rotate_and_offset_gyro(_gyro_instance, gyro);
 
-    Vector3f accel_scale = _accel_scale[0].get();
-    _accel[0].x *= accel_scale.x;
-    _accel[0].y *= accel_scale.y;
-    _accel[0].z *= accel_scale.z;
-    _accel[0] -= _accel_offset[0];
+    accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
+    _rotate_and_offset_accel(_accel_instance, accel);
 
-    if (_last_filter_hz != _mpu6000_filter) {
+    if (_last_filter_hz != _imu.get_filter()) {
         if (_spi_sem->take(10)) {
             _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
-            _set_filter_register(_mpu6000_filter, 0);
+            _set_filter_register(_imu.get_filter());
             _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
-            _error_count = 0;
             _spi_sem->give();
         }
     }
@@ -331,35 +338,13 @@ bool AP_InertialSensor_MPU6000::_data_ready()
  */
 void AP_InertialSensor_MPU6000::_poll_data(void)
 {
-    if (hal.scheduler->in_timerprocess()) {
-        if (!_spi_sem->take_nonblocking()) {
-            /*
-              the semaphore being busy is an expected condition when the
-              mainline code is calling wait_for_sample() which will
-              grab the semaphore. We return now and rely on the mainline
-              code grabbing the latest sample.
-            */
-            return;
-        }   
-        if (_data_ready()) {
-            _last_sample_time_micros = hal.scheduler->micros();
-            _read_data_transaction(); 
-        }
-        _spi_sem->give();
-    } else {
-        /* Synchronous read - take semaphore */
-        if (_spi_sem->take(10)) {
-            if (_data_ready()) {
-                _last_sample_time_micros = hal.scheduler->micros();
-                _read_data_transaction(); 
-            }
-            _spi_sem->give();
-        } else {
-            hal.scheduler->panic(
-                PSTR("PANIC: AP_InertialSensor_MPU6000::_poll_data "
-                     "failed to take SPI semaphore synchronously"));
-        }
+    if (!_spi_sem->take_nonblocking()) {
+        return;
+    }   
+    if (_data_ready()) {
+        _read_data_transaction(); 
     }
+    _spi_sem->give();
 }
 
 
@@ -390,19 +375,31 @@ void AP_InertialSensor_MPU6000::_read_data_transaction() {
     }
 
 #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
+#if MPU6000_FAST_SAMPLING
+    _accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)), 
+                               _accel_filter_y.apply(int16_val(rx.v, 0)), 
+                               _accel_filter_z.apply(-int16_val(rx.v, 2)));
+
+    _gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)), 
+                              _gyro_filter_y.apply(int16_val(rx.v, 4)), 
+                              _gyro_filter_z.apply(-int16_val(rx.v, 6)));
+#else
     _accel_sum.x += int16_val(rx.v, 1);
     _accel_sum.y += int16_val(rx.v, 0);
     _accel_sum.z -= int16_val(rx.v, 2);
     _gyro_sum.x  += int16_val(rx.v, 5);
     _gyro_sum.y  += int16_val(rx.v, 4);
     _gyro_sum.z  -= int16_val(rx.v, 6);
+#endif
     _sum_count++;
 
+#if !MPU6000_FAST_SAMPLING
     if (_sum_count == 0) {
         // rollover - v unlikely
         _accel_sum.zero();
         _gyro_sum.zero();
     }
+#endif
 }
 
 uint8_t AP_InertialSensor_MPU6000::_register_read( uint8_t reg )
@@ -448,36 +445,30 @@ void AP_InertialSensor_MPU6000::_register_write_check(uint8_t reg, uint8_t val)
 /*
   set the DLPF filter frequency. Assumes caller has taken semaphore
  */
-void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz, uint8_t default_filter)
+void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz)
 {
-    uint8_t filter = default_filter;
+    if (filter_hz == 0) {
+        filter_hz = _default_filter();
+    }
+    uint8_t filter;
     // choose filtering frequency
-    switch (filter_hz) {
-    case 5:
+    if (filter_hz <= 5) {
         filter = BITS_DLPF_CFG_5HZ;
-        break;
-    case 10:
+    } else if (filter_hz <= 10) {
         filter = BITS_DLPF_CFG_10HZ;
-        break;
-    case 20:
+    } else if (filter_hz <= 20) {
         filter = BITS_DLPF_CFG_20HZ;
-        break;
-    case 42:
+    } else if (filter_hz <= 42) {
         filter = BITS_DLPF_CFG_42HZ;
-        break;
-    case 98:
+    } else {
         filter = BITS_DLPF_CFG_98HZ;
-        break;
-    }
-
-    if (filter != 0) {
-        _last_filter_hz = filter_hz;
-        _register_write(MPUREG_CONFIG, filter);
     }
+    _last_filter_hz = filter_hz;
+    _register_write(MPUREG_CONFIG, filter);
 }
 
 
-bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate)
+bool AP_InertialSensor_MPU6000::_hardware_init(void)
 {
     if (!_spi_sem->take(100)) {
         hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
@@ -519,46 +510,53 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate)
     _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
     hal.scheduler->delay(1);
 
-    uint8_t default_filter;
-
+#if MPU6000_FAST_SAMPLING
+    _sample_count = 1;
+#else
     // sample rate and filtering
     // to minimise the effects of aliasing we choose a filter
     // that is less than half of the sample rate
-    switch (sample_rate) {
-    case RATE_50HZ:
+    switch (_imu.get_sample_rate()) {
+    case AP_InertialSensor::RATE_50HZ:
         // this is used for plane and rover, where noise resistance is
         // more important than update rate. Tests on an aerobatic plane
         // show that 10Hz is fine, and makes it very noise resistant
-        default_filter = BITS_DLPF_CFG_10HZ;
-        _sample_shift = 2;
+        _sample_count = 4;
         break;
-    case RATE_100HZ:
-        default_filter = BITS_DLPF_CFG_20HZ;
-        _sample_shift = 1;
+    case AP_InertialSensor::RATE_100HZ:
+        _sample_count = 2;
         break;
-    case RATE_200HZ:
-    default:
-        default_filter = BITS_DLPF_CFG_20HZ;
-        _sample_shift = 0;
+    case AP_InertialSensor::RATE_200HZ:
+        _sample_count = 1;
         break;
+    default:
+        return false;
     }
+#endif
 
-    _set_filter_register(_mpu6000_filter, default_filter);
+    _set_filter_register(_imu.get_filter());
 
+#if MPU6000_FAST_SAMPLING
+    // set sample rate to 1000Hz and apply a software filter
+    _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_1000HZ);
+#else
     // set sample rate to 200Hz, and use _sample_divider to give
     // the requested rate to the application
     _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ);
+#endif
     hal.scheduler->delay(1);
 
     _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS);  // Gyro scale 2000º/s
     hal.scheduler->delay(1);
 
     // read the product ID rev c has 1/2 the sensitivity of rev d
-    _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
+    _product_id = _register_read(MPUREG_PRODUCT_ID);
     //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
 
-    if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) ||
-        (_mpu6000_product_id == MPU6000_REV_C4)   || (_mpu6000_product_id == MPU6000_REV_C5)) {
+    if ((_product_id == MPU6000ES_REV_C4) || 
+        (_product_id == MPU6000ES_REV_C5) ||
+        (_product_id == MPU6000_REV_C4)   || 
+        (_product_id == MPU6000_REV_C5)) {
         // Accel scale 8g (4096 LSB/g)
         // Rev C has different scaling than rev D
         _register_write(MPUREG_ACCEL_CONFIG,1<<3);
@@ -585,22 +583,6 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate)
     return true;
 }
 
-// return the MPU6k gyro drift rate in radian/s/s
-// note that this is much better than the oilpan gyros
-float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void)
-{
-    // 0.5 degrees/second/minute
-    return ToRad(0.5/60);
-}
-
-// return true if a sample is available
-bool AP_InertialSensor_MPU6000::_sample_available()
-{
-    _poll_data();
-    return (_sum_count >> _sample_shift) > 0;
-}
-
-
 #if MPU6000_DEBUG
 // dump all config registers - used for debug
 void AP_InertialSensor_MPU6000::_dump_registers(void)
@@ -619,11 +601,3 @@ void AP_InertialSensor_MPU6000::_dump_registers(void)
     }
 }
 #endif
-
-
-// get_delta_time returns the time period in seconds overwhich the sensor data was collected
-float AP_InertialSensor_MPU6000::get_delta_time() const
-{
-    // the sensor runs at 200Hz
-    return 0.005 * _num_samples;
-}
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index f6567d849da99d6a45d95eb61098ddd3ea79c8f4..3fa58d04c1f04ba338b4150119a3166e981bee3d 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
@@ -12,33 +12,44 @@
 // enable debug to see a register dump on startup
 #define MPU6000_DEBUG 0
 
-class AP_InertialSensor_MPU6000 : public AP_InertialSensor
+// on fast CPUs we sample at 1kHz and use a software filter
+#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
+#define MPU6000_FAST_SAMPLING 1
+#else
+#define MPU6000_FAST_SAMPLING 0
+#endif
+
+#if MPU6000_FAST_SAMPLING
+#include <Filter.h>
+#include <LowPassFilter2p.h>
+#endif
+
+class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
 {
 public:
+    AP_InertialSensor_MPU6000(AP_InertialSensor &imu);
 
-    AP_InertialSensor_MPU6000();
-
-    /* Concrete implementation of AP_InertialSensor functions: */
-    bool                update();
-    float               get_gyro_drift_rate();
+    /* update accel and gyro state */
+    bool update();
 
-    // wait for a sample to be available, with timeout in milliseconds
-    bool                wait_for_sample(uint16_t timeout_ms);
+    bool gyro_sample_available(void) { return _sum_count >= _sample_count; }
+    bool accel_sample_available(void) { return _sum_count >= _sample_count; }
 
-    // get_delta_time returns the time period in seconds overwhich the sensor data was collected
-    float            	get_delta_time() const;
+    // detect the sensor
+    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
 
-    uint16_t error_count(void) const { return _error_count; }
-    bool healthy(void) const { return _error_count <= 4; }
-    bool get_gyro_health(uint8_t instance) const { return healthy(); }
-    bool get_accel_health(uint8_t instance) const { return healthy(); }
+private:
+#if MPU6000_DEBUG
+    void _dump_registers(void);
+#endif
 
-protected:
-    uint16_t                    _init_sensor( Sample_rate sample_rate );
+    // instance numbers of accel and gyro data
+    uint8_t _gyro_instance;
+    uint8_t _accel_instance;
 
-private:
     AP_HAL::DigitalSource *_drdy_pin;
 
+    bool                 _init_sensor(void);
     bool                 _sample_available();
     void                 _read_data_transaction();
     bool                 _data_ready();
@@ -46,41 +57,42 @@ private:
     uint8_t              _register_read( uint8_t reg );
     void                 _register_write( uint8_t reg, uint8_t val );
     void                 _register_write_check(uint8_t reg, uint8_t val);
-    bool                 _hardware_init(Sample_rate sample_rate);
+    bool                 _hardware_init(void);
 
     AP_HAL::SPIDeviceDriver *_spi;
     AP_HAL::Semaphore *_spi_sem;
 
-    uint16_t					_num_samples;
     static const float          _gyro_scale;
 
-    uint32_t _last_sample_time_micros;
-
-    // ensure we can't initialise twice
-    bool                        _initialised;
-    int16_t              _mpu6000_product_id;
-
-    // how many hardware samples before we report a sample to the caller
-    uint8_t _sample_shift;
-
     // support for updating filter at runtime
     uint8_t _last_filter_hz;
 
-    void _set_filter_register(uint8_t filter_hz, uint8_t default_filter);
+    void _set_filter_register(uint8_t filter_hz);
 
+    // count of bus errors
     uint16_t _error_count;
 
+    // how many hardware samples before we report a sample to the caller
+    uint8_t _sample_count;
+
+#if MPU6000_FAST_SAMPLING
+    Vector3f _accel_filtered;
+    Vector3f _gyro_filtered;
+
+    // Low Pass filters for gyro and accel 
+    LowPassFilter2p _accel_filter_x;
+    LowPassFilter2p _accel_filter_y;
+    LowPassFilter2p _accel_filter_z;
+    LowPassFilter2p _gyro_filter_x;
+    LowPassFilter2p _gyro_filter_y;
+    LowPassFilter2p _gyro_filter_z;
+#else
     // accumulation in timer - must be read with timer disabled
     // the sum of the values since last read
     Vector3l _accel_sum;
     Vector3l _gyro_sum;
-    volatile int16_t _sum_count;
-
-public:
-
-#if MPU6000_DEBUG
-    void						_dump_registers(void);
 #endif
+    volatile uint16_t _sum_count;
 };
 
 #endif // __AP_INERTIAL_SENSOR_MPU6000_H__
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
index 44cd2b117a0298fd4c5cccc2caf64913b6ccea13..95130f12eb9635da29061d69b463e055a264ac13 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
@@ -19,6 +19,10 @@
     Please check the following links for datasheets and documentation:
      - http://www.invensense.com/mems/gyro/documents/PS-MPU-9150A-00v4_3.pdf
      - http://www.invensense.com/mems/gyro/documents/RM-MPU-9150A-00v4_2.pdf
+
+     Note that this is an experimental driver. It is not used by any
+     actively maintained board and should be considered untested and
+     unmaintained 
 */
 
 #include <AP_HAL.h>
@@ -320,19 +324,33 @@ static struct gyro_state_s st = {
 /**
  *  @brief      Constructor
  */
-AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150() :
-    AP_InertialSensor(),
+AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150(AP_InertialSensor &imu) :
+    AP_InertialSensor_Backend(imu),
+    _have_sample_available(false),
     _accel_filter_x(800, 10),
     _accel_filter_y(800, 10),
     _accel_filter_z(800, 10),
     _gyro_filter_x(800, 10),
     _gyro_filter_y(800, 10),
-    _gyro_filter_z(800, 10)    
-    // _mag_filter_x(800, 10),    
-    // _mag_filter_y(800, 10),    
-    // _mag_filter_z(800, 10)    
+    _gyro_filter_z(800, 10)
 {
+}
 
+
+/*
+  detect the sensor
+ */
+AP_InertialSensor_Backend *AP_InertialSensor_MPU9150::detect(AP_InertialSensor &_imu)
+{
+    AP_InertialSensor_MPU9150 *sensor = new AP_InertialSensor_MPU9150(_imu);
+    if (sensor == NULL) {
+        return NULL;
+    }
+    if (!sensor->_init_sensor()) {
+        delete sensor;
+        return NULL;
+    }
+    return sensor;
 }
 
 /*
@@ -352,41 +370,21 @@ void AP_InertialSensor_MPU9150::_set_filter_frequency(uint8_t filter_hz)
 }
 
 /**
- *  @brief      Init method
- *  @param[in] Sample_rate  The sample rate, check the struct def.
- *  @return     AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE if successful.
+ *  Init method
  */
-uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate ) 
+bool AP_InertialSensor_MPU9150::_init_sensor(void) 
 {
     // Sensors pushed to the FIFO.
     uint8_t sensors;
 
-    switch (sample_rate) {
-    case RATE_50HZ:
-        _default_filter_hz = 10;
-        _sample_period_usec = (1000*1000) / 50;
-        break;
-    case RATE_100HZ:
-        _default_filter_hz = 20;
-        _sample_period_usec = (1000*1000) / 100;
-        break;
-    case RATE_200HZ:
-        _default_filter_hz = 20;
-        _sample_period_usec = 5000;
-        break;
-    case RATE_400HZ:
-    default:
-        _default_filter_hz = 20;
-        _sample_period_usec = 2500;
-        break;
-    }
+    _default_filter_hz = _default_filter();
 
     // get pointer to i2c bus semaphore
     AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
 
     // take i2c bus sempahore
     if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)){
-        return -1;
+        return false;
     }        
 
     // Init the sensor
@@ -405,8 +403,8 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate )
     //  This registers are not documented in the register map.
     uint8_t buff[6];
     if (hal.i2c->readRegisters(st.hw->addr, st.reg->accel_offs, 6, buff) != 0) {
-        hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: couldn't read the registers to determine revision"));
-        goto failed;          
+        hal.console->printf("AP_InertialSensor_MPU9150: couldn't read the registers to determine revision");
+        goto failed;
     }    
     uint8_t rev;
     rev = ((buff[5] & 0x01) << 2) | ((buff[3] & 0x01) << 1) |
@@ -432,28 +430,28 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate )
 
     // Set gyro full-scale range [250, 500, 1000, 2000]
     if (mpu_set_gyro_fsr(2000)){
-        hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_gyro_fsr.\n"));
+        hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_gyro_fsr.\n");
         goto failed;
     }
     // Set the accel full-scale range
     if (mpu_set_accel_fsr(2)){
-        hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_accel_fsr.\n"));
+        hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_accel_fsr.\n");
         goto failed;    
     }
     // Set digital low pass filter (using _default_filter_hz, 20 for 100 Hz of sample rate)
     if (mpu_set_lpf(_default_filter_hz)){
-        hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_lpf.\n"));
+        hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_lpf.\n");
         goto failed;    
     }
     // Set sampling rate (value must be between 4Hz and 1KHz)
     if (mpu_set_sample_rate(800)){
-        hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_sample_rate.\n"));
+        hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_sample_rate.\n");
         goto failed;    
     }
     // Select which sensors are pushed to FIFO.
     sensors = INV_XYZ_ACCEL| INV_XYZ_GYRO;
     if (mpu_configure_fifo(sensors)){
-        hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_configure_fifo.\n"));
+        hal.console->printf("AP_InertialSensor_MPU9150: mpu_configure_fifo.\n");
         goto failed;    
     }
 
@@ -467,18 +465,23 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate )
     mpu_set_sensors(sensors);
 
     // Set the filter frecuency (_mpu6000_filter configured to the default value, check AP_InertialSensor.cpp)
-    _set_filter_frequency(_mpu6000_filter);
+    _set_filter_frequency(_imu.get_filter());
 
     // give back i2c semaphore
     i2c_sem->give();
+
+    _gyro_instance = _imu.register_gyro();
+    _accel_instance = _imu.register_accel();
+
     // start the timer process to read samples    
     hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9150::_accumulate));
-    return AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE;
 
-    failed:
-        // give back i2c semaphore
-        i2c_sem->give();
-        return -1;
+    return true;
+
+failed:
+    // give back i2c semaphore
+    i2c_sem->give();
+    return false;
 }
 
 /**
@@ -1017,9 +1020,9 @@ int16_t AP_InertialSensor_MPU9150::mpu_read_fifo(int16_t *gyro, int16_t *accel,
  *  @brief      Accumulate values from accels and gyros. 
  *
  *     This method is called periodically by the scheduler.
- *
  */
-void AP_InertialSensor_MPU9150::_accumulate(void){
+void AP_InertialSensor_MPU9150::_accumulate(void)
+{
     // get pointer to i2c bus semaphore
     AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
 
@@ -1094,102 +1097,36 @@ void AP_InertialSensor_MPU9150::_accumulate(void){
             _gyro_filter_y.apply(gyro_y), 
             _gyro_filter_z.apply(gyro_z));
 
-        _gyro_samples_available++;
+        _have_sample_available = true;
     }
 
     // give back i2c semaphore
     i2c_sem->give();
 }
 
-bool AP_InertialSensor_MPU9150::_sample_available(void)
-{
-    uint64_t tnow =  hal.scheduler->micros();
-    while (tnow - _last_sample_timestamp > _sample_period_usec) {
-        _have_sample_available = true;
-        _last_sample_timestamp += _sample_period_usec;
-    }
-    return _have_sample_available;    
-}
-
-bool AP_InertialSensor_MPU9150::wait_for_sample(uint16_t timeout_ms)
-{
-    if (_sample_available()) {
-        return true;
-    }
-    uint32_t start = hal.scheduler->millis();
-    while ((hal.scheduler->millis() - start) < timeout_ms) {
-        uint64_t tnow = hal.scheduler->micros(); 
-        // we spin for the last timing_lag microseconds. Before that
-        // we yield the CPU to allow IO to happen
-        const uint16_t timing_lag = 400;
-        if (_last_sample_timestamp + _sample_period_usec > tnow+timing_lag) {
-            hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_period_usec - (tnow+timing_lag));
-        }
-        if (_sample_available()) {
-            return true;
-        }
-    }
-    return false;
-}
-
 bool AP_InertialSensor_MPU9150::update(void) 
 {
-    if (!wait_for_sample(1000)) {
-        return false;
-    }
-    Vector3f accel_scale = _accel_scale[0].get();
+    Vector3f accel, gyro;
 
-    _previous_accel[0] = _accel[0];
-
-    // hal.scheduler->suspend_timer_procs();
-    _accel[0] = _accel_filtered;
-    _gyro[0] = _gyro_filtered;
-    // hal.scheduler->resume_timer_procs();
-
-    // add offsets and rotation
-    _accel[0].rotate(_board_orientation);
-
-    // Adjust for chip scaling to get m/s/s
-    ////////////////////////////////////////////////
-    _accel[0] *= MPU9150_ACCEL_SCALE_2G/_gyro_samples_available;
-
-    // Now the calibration scale factor
-    _accel[0].x *= accel_scale.x;
-    _accel[0].y *= accel_scale.y;
-    _accel[0].z *= accel_scale.z;
-    _accel[0]   -= _accel_offset[0];
-
-    _gyro[0].rotate(_board_orientation);
+    hal.scheduler->suspend_timer_procs();
+    accel = _accel_filtered;
+    gyro = _gyro_filtered;
+    _have_sample_available = false;
+    hal.scheduler->resume_timer_procs();
 
-    // Adjust for chip scaling to get radians/sec
-    _gyro[0] *= MPU9150_GYRO_SCALE_2000 / _gyro_samples_available;
-    _gyro[0] -= _gyro_offset[0];
-    ////////////////////////////////////////////////
+    accel *= MPU9150_ACCEL_SCALE_2G;
+    _rotate_and_offset_accel(_accel_instance, accel);
 
-    _gyro_samples_available = 0;
+    gyro *= MPU9150_GYRO_SCALE_2000;
+    _rotate_and_offset_gyro(_gyro_instance, gyro);
 
-    if (_last_filter_hz != _mpu6000_filter) {
-        _set_filter_frequency(_mpu6000_filter);
-        _last_filter_hz = _mpu6000_filter;
+    if (_last_filter_hz != _imu.get_filter()) {
+        _set_filter_frequency(_imu.get_filter());
+        _last_filter_hz = _imu.get_filter();
     }
 
-    _have_sample_available = false;
-
     return true;
 }
 
-// TODO review to make sure it matches
-float AP_InertialSensor_MPU9150::get_gyro_drift_rate(void) 
-{
-    // 0.5 degrees/second/minute (a guess)
-    return ToRad(0.5/60);
-}
-
-// TODO review to make sure it matches
-float AP_InertialSensor_MPU9150::get_delta_time(void) const
-{
-    return _sample_period_usec * 1.0e-6f;
-}
-
 
 #endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h
index a1d31fadab1da01c553d5c47d6e0233c6e568f32..52de4f9d71fc37694ee20590c1ce2273de248cca 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h
@@ -12,33 +12,30 @@
 #include <LowPassFilter2p.h>
 
 
-class AP_InertialSensor_MPU9150 : public AP_InertialSensor
+class AP_InertialSensor_MPU9150 : public AP_InertialSensor_Backend
 {
 public:
+    AP_InertialSensor_MPU9150(AP_InertialSensor &imu);
 
-    AP_InertialSensor_MPU9150();
+    /* update accel and gyro state */
+    bool update();
 
-    /* Implementation of AP_InertialSensor functions: */
-    bool            update();
-    float        	get_delta_time() const;
-    float           get_gyro_drift_rate();
-    bool            wait_for_sample(uint16_t timeout_ms);
+    bool gyro_sample_available(void) { return _have_sample_available; }
+    bool accel_sample_available(void) { return _have_sample_available; }
+
+    // detect the sensor
+    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
 
 private:
-    uint16_t        _init_sensor( Sample_rate sample_rate );
+    bool            _init_sensor();
     void             _accumulate(void);
-    bool            _sample_available();
-    // uint64_t        _last_update_usec;
     Vector3f        _accel_filtered;
     Vector3f        _gyro_filtered;
-    uint32_t        _sample_period_usec;
-    volatile uint32_t _gyro_samples_available;
-    uint64_t        _last_sample_timestamp;    
-    bool     _have_sample_available;    
+    bool            _have_sample_available;
 
     // // support for updating filter at runtime
     uint8_t         _last_filter_hz;
-    uint8_t          _default_filter_hz;
+    uint8_t         _default_filter_hz;
 
     int16_t mpu_set_gyro_fsr(uint16_t fsr);
     int16_t mpu_set_accel_fsr(uint8_t fsr);
@@ -52,7 +49,6 @@ private:
     int16_t mpu_set_int_latched(uint8_t enable);
     int16_t mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t timestamp, uint8_t *sensors, uint8_t *more);
 
-    // Filter (specify which one)
     void _set_filter_frequency(uint8_t filter_hz);
 
     // Low Pass filters for gyro and accel 
@@ -62,11 +58,9 @@ private:
     LowPassFilter2p _gyro_filter_x;
     LowPassFilter2p _gyro_filter_y;
     LowPassFilter2p _gyro_filter_z;
-    // LowPassFilter2p _mag_filter_x;
-    // LowPassFilter2p _mag_filter_y;
-    // LowPassFilter2p _mag_filter_z;
-
 
+    uint8_t _gyro_instance;
+    uint8_t _accel_instance;
 };
 #endif
 #endif // __AP_INERTIAL_SENSOR_MPU9150_H__
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
index 6eefd8f2cd27b9034287eb79755a196e5637ec49..0c8cd321126e47e68e8b2b68e9e671969164707d 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
@@ -21,7 +21,6 @@
 
 #include "AP_InertialSensor_MPU9250.h"
 #include "../AP_HAL_Linux/GPIO.h"
-#include <stdio.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -158,29 +157,11 @@ extern const AP_HAL::HAL& hal;
 #define BITS_DLPF_CFG_2100HZ_NOLPF              0x07
 #define BITS_DLPF_CFG_MASK                              0x07
 
-
-// // TODO Remove
-// // Product ID Description for MPU6000
-// // high 4 bits  low 4 bits
-// // Product Name	Product Revision
-// #define MPU6000ES_REV_C4                        0x14    // 0001			0100
-// #define MPU6000ES_REV_C5                        0x15    // 0001			0101
-// #define MPU6000ES_REV_D6                        0x16    // 0001			0110
-// #define MPU6000ES_REV_D7                        0x17    // 0001			0111
-// #define MPU6000ES_REV_D8                        0x18    // 0001			1000
-// #define MPU6000_REV_C4                          0x54    // 0101			0100
-// #define MPU6000_REV_C5                          0x55    // 0101			0101
-// #define MPU6000_REV_D6                          0x56    // 0101			0110
-// #define MPU6000_REV_D7                          0x57    // 0101			0111
-// #define MPU6000_REV_D8                          0x58    // 0101			1000
-// #define MPU6000_REV_D9                          0x59    // 0101			1001
-
-
 /*
  *  PS-MPU-9250A-00.pdf, page 8, lists LSB sensitivity of
  *  gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3)
  */
-const float AP_InertialSensor_MPU9250::_gyro_scale = (0.0174532f / 16.4f);
+#define GYRO_SCALE (0.0174532f / 16.4f)
 
 /*
  *  PS-MPU-9250A-00.pdf, page 9, lists LSB sensitivity of
@@ -190,27 +171,48 @@ const float AP_InertialSensor_MPU9250::_gyro_scale = (0.0174532f / 16.4f);
  *  variants however
  */
 
-AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250() :
-	AP_InertialSensor(),
-    _drdy_pin(NULL),
-    _initialised(false),
-    _mpu9250_product_id(AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE)
+AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu) :
+	AP_InertialSensor_Backend(imu),
+    _last_filter_hz(-1),
+    _shared_data_idx(0),
+    _accel_filter_x(1000, 15),
+    _accel_filter_y(1000, 15),
+    _accel_filter_z(1000, 15),
+    _gyro_filter_x(1000, 15),
+    _gyro_filter_y(1000, 15),
+    _gyro_filter_z(1000, 15),
+    _have_sample_available(false)
 {
 }
 
-uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
+
+/*
+  detect the sensor
+ */
+AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &_imu)
 {
-    if (_initialised) return _mpu9250_product_id;
-    _initialised = true;
+    AP_InertialSensor_MPU9250 *sensor = new AP_InertialSensor_MPU9250(_imu);
+    if (sensor == NULL) {
+        return NULL;
+    }
+    if (!sensor->_init_sensor()) {
+        delete sensor;
+        return NULL;
+    }
 
+    return sensor;
+}
+
+/*
+  initialise the sensor
+ */
+bool AP_InertialSensor_MPU9250::_init_sensor(void)
+{
     _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
     _spi_sem = _spi->get_semaphore();
 
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE
-    _drdy_pin = hal.gpio->channel(BBB_P8_7);
-    _drdy_pin->mode(HAL_GPIO_INPUT);
-#endif
-
+    // we need to suspend timers to prevent other SPI drivers grabbing
+    // the bus while we do the long initialisation
     hal.scheduler->suspend_timer_procs();
 
     uint8_t whoami = _register_read(MPUREG_WHOAMI);
@@ -218,43 +220,36 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
         // TODO: we should probably accept multiple chip
         // revisions. This is the one on the PXF
         hal.console->printf("MPU9250: unexpected WHOAMI 0x%x\n", (unsigned)whoami);
-        hal.scheduler->panic("MPU9250: bad WHOAMI");
+        return false;
     }
 
     uint8_t tries = 0;
     do {
-        bool success = _hardware_init(sample_rate);
+        bool success = _hardware_init();
         if (success) {
-            hal.scheduler->delay(5+2);
+            hal.scheduler->delay(10);
             if (!_spi_sem->take(100)) {
-                hal.scheduler->panic(PSTR("MPU9250: Unable to get semaphore"));
+                hal.console->printf("MPU9250: Unable to get semaphore");
+                return false;
             }
-            if (_data_ready()) {
+            uint8_t status = _register_read(MPUREG_INT_STATUS);
+            if ((status & BIT_RAW_RDY_INT) != 0) {
                 _spi_sem->give();
                 break;
-            } else {
-                hal.console->println_P(
-                        PSTR("MPU9250 startup failed: no data ready"));
             }
             _spi_sem->give();
         }
         if (tries++ > 5) {
-            hal.scheduler->panic(PSTR("PANIC: failed to boot MPU9250 5 times"));
+            return false;
         }
     } while (1);
 
     hal.scheduler->resume_timer_procs();
 
+    _gyro_instance = _imu.register_gyro();
+    _accel_instance = _imu.register_accel();
 
-    /* read the first lot of data.
-     * _read_data_transaction requires the spi semaphore to be taken by
-     * its caller. */
-    _last_sample_time_micros = hal.scheduler->micros();
-    hal.scheduler->delay(10);
-    if (_spi_sem->take(100)) {
-        _read_data_transaction();
-        _spi_sem->give();
-    }
+    _product_id = AP_PRODUCT_ID_MPU9250;
 
     // start the timer process to read samples
     hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9250::_poll_data));
@@ -262,70 +257,45 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
 #if MPU9250_DEBUG
     _dump_registers();
 #endif
-    return _mpu9250_product_id;
-}
-
-/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
-
-bool AP_InertialSensor_MPU9250::wait_for_sample(uint16_t timeout_ms)
-{
-    if (_sample_available()) {
-        return true;
-    }
-    uint32_t start = hal.scheduler->millis();
-    while ((hal.scheduler->millis() - start) < timeout_ms) {
-        hal.scheduler->delay_microseconds(100);
-        if (_sample_available()) {
-            return true;
-        }
-    }
-    return false;
+    return true;
 }
 
+/*
+  update the accel and gyro vectors
+ */
 bool AP_InertialSensor_MPU9250::update( void )
 {
-    // wait for at least 1 sample
-    if (!wait_for_sample(1000)) {
-        return false;
-    }
+    // pull the data from the timer shared data buffer
+    uint8_t idx = _shared_data_idx;
+    Vector3f gyro = _shared_data[idx]._gyro_filtered;
+    Vector3f accel = _shared_data[idx]._accel_filtered;
 
-    _previous_accel[0] = _accel[0];
+    _have_sample_available = false;
 
-    // disable timer procs for mininum time
-    hal.scheduler->suspend_timer_procs();
-    _gyro[0]  = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
-    _accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z);
-    _num_samples = _sum_count;
-    _accel_sum.zero();
-    _gyro_sum.zero();
-    _sum_count = 0;
-    hal.scheduler->resume_timer_procs();
+    accel *= MPU9250_ACCEL_SCALE_1G;
+    gyro *= GYRO_SCALE;
 
-    _gyro[0].rotate(_board_orientation);
-    _gyro[0] *= _gyro_scale / _num_samples;
-    _gyro[0] -= _gyro_offset[0];
+    // rotate for bbone default
+    accel.rotate(ROTATION_ROLL_180_YAW_90);
+    gyro.rotate(ROTATION_ROLL_180_YAW_90);
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
+    // PXF has an additional YAW 180
+    accel.rotate(ROTATION_YAW_180);
+    gyro.rotate(ROTATION_YAW_180);
+#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
+    // NavIO has different orientation, assuming RaspberryPi is right
+    // way up, and PWM pins on NavIO are at the back of the aircraft
+    accel.rotate(ROTATION_ROLL_180_YAW_90);
+    gyro.rotate(ROTATION_ROLL_180_YAW_90);
+#endif
 
-    _accel[0].rotate(_board_orientation);
-    _accel[0] *= MPU9250_ACCEL_SCALE_1G / _num_samples;
+    _rotate_and_offset_gyro(_gyro_instance, gyro);
+    _rotate_and_offset_accel(_accel_instance, accel);
 
-    // rotate for bbone default
-    _accel[0].rotate(ROTATION_ROLL_180_YAW_90);
-    _gyro[0].rotate(ROTATION_ROLL_180_YAW_90);
-
-    Vector3f accel_scale = _accel_scale[0].get();
-    _accel[0].x *= accel_scale.x;
-    _accel[0].y *= accel_scale.y;
-    _accel[0].z *= accel_scale.z;
-    _accel[0] -= _accel_offset[0];
-
-    if (_last_filter_hz != _mpu6000_filter) {
-        if (_spi_sem->take(10)) {
-            _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
-            _set_filter_register(_mpu6000_filter, 0);
-            _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
-            _error_count = 0;
-            _spi_sem->give();
-        }
+    if (_last_filter_hz != _imu.get_filter()) {
+        _set_filter(_imu.get_filter());
+        _last_filter_hz = _imu.get_filter();
     }
 
     return true;
@@ -333,59 +303,30 @@ bool AP_InertialSensor_MPU9250::update( void )
 
 /*================ HARDWARE FUNCTIONS ==================== */
 
-/**
- * Return true if the MPU9250 has new data available for reading.
- *
- * We use the data ready pin if it is available.  Otherwise, read the
- * status register.
- */
-bool AP_InertialSensor_MPU9250::_data_ready()
-{
-    if (_drdy_pin) {
-        return _drdy_pin->read() != 0;
-    }
-    uint8_t status = _register_read(MPUREG_INT_STATUS);
-    return (status & BIT_RAW_RDY_INT) != 0;
-}
-
 /**
  * Timer process to poll for new data from the MPU9250.
  */
 void AP_InertialSensor_MPU9250::_poll_data(void)
 {
-    if (hal.scheduler->in_timerprocess()) {
-        if (!_spi_sem->take_nonblocking()) {
-            /*
-              the semaphore being busy is an expected condition when the
-              mainline code is calling wait_for_sample() which will
-              grab the semaphore. We return now and rely on the mainline
-              code grabbing the latest sample.
-            */
-            return;
-        }
-        if (_data_ready()) {
-            _last_sample_time_micros = hal.scheduler->micros();
-            _read_data_transaction();
-        }
-        _spi_sem->give();
-    } else {
-        /* Synchronous read - take semaphore */
-        if (_spi_sem->take(10)) {
-            if (_data_ready()) {
-                _last_sample_time_micros = hal.scheduler->micros();
-                _read_data_transaction();
-            }
-            _spi_sem->give();
-        } else {
-            hal.scheduler->panic(
-                PSTR("PANIC: AP_InertialSensor_MPU9250::_poll_data "
-                     "failed to take SPI semaphore synchronously"));
-        }
+    if (!_spi_sem->take_nonblocking()) {
+        /*
+          the semaphore being busy is an expected condition when the
+          mainline code is calling wait_for_sample() which will
+          grab the semaphore. We return now and rely on the mainline
+          code grabbing the latest sample.
+        */
+        return;
     }
+    _read_data_transaction();
+    _spi_sem->give();
 }
 
 
-void AP_InertialSensor_MPU9250::_read_data_transaction() {
+/*
+  read from the data registers and update filtered data
+ */
+void AP_InertialSensor_MPU9250::_read_data_transaction() 
+{
     /* one resister address followed by seven 2-byte registers */
     struct PACKED {
         uint8_t cmd;
@@ -395,45 +336,27 @@ void AP_InertialSensor_MPU9250::_read_data_transaction() {
 
     _spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx));
 
-    if (_drdy_pin) {
-        if (_drdy_pin->read() != 0) {
-            // data ready should have gone low after a read
-            printf("MPU9250: DRDY didn't go low\n");
-        }
-    }
+#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
 
-    /*
-      detect a bad SPI bus transaction by looking for all 14 bytes
-      zero, or the wrong INT_STATUS register value. This is used to
-      detect a too high SPI bus speed.
-     */
-    uint8_t i;
-    for (i=0; i<14; i++) {
-        if (rx.v[i] != 0) break;
-    }
-    if ((rx.int_status&~0x6) != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT) || i == 14) {
-        // likely a bad bus transaction
-        if (++_error_count > 4) {
-            _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
-        }
-    }
+    Vector3f _accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)), 
+                                        _accel_filter_y.apply(int16_val(rx.v, 0)), 
+                                        _accel_filter_z.apply(-int16_val(rx.v, 2)));
 
-#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
-    _accel_sum.x += int16_val(rx.v, 1);
-    _accel_sum.y += int16_val(rx.v, 0);
-    _accel_sum.z -= int16_val(rx.v, 2);
-    _gyro_sum.x  += int16_val(rx.v, 5);
-    _gyro_sum.y  += int16_val(rx.v, 4);
-    _gyro_sum.z  -= int16_val(rx.v, 6);
-    _sum_count++;
-
-    if (_sum_count == 0) {
-        // rollover - v unlikely
-        _accel_sum.zero();
-        _gyro_sum.zero();
-    }
+    Vector3f _gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)), 
+                                       _gyro_filter_y.apply(int16_val(rx.v, 4)), 
+                                       _gyro_filter_z.apply(-int16_val(rx.v, 6)));
+    // update the shared buffer
+    uint8_t idx = _shared_data_idx ^ 1;
+    _shared_data[idx]._accel_filtered = _accel_filtered;
+    _shared_data[idx]._gyro_filtered = _gyro_filtered;
+    _shared_data_idx = idx;
+
+    _have_sample_available = true;
 }
 
+/*
+  read an 8 bit register
+ */
 uint8_t AP_InertialSensor_MPU9250::_register_read( uint8_t reg )
 {
     uint8_t addr = reg | 0x80; // Set most significant bit
@@ -444,10 +367,12 @@ uint8_t AP_InertialSensor_MPU9250::_register_read( uint8_t reg )
     tx[0] = addr;
     tx[1] = 0;
     _spi->transaction(tx, rx, 2);
-
     return rx[1];
 }
 
+/*
+  write an 8 bit register
+ */
 void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val)
 {
     uint8_t tx[2];
@@ -459,42 +384,32 @@ void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val)
 }
 
 /*
-  set the DLPF filter frequency. Assumes caller has taken semaphore
+  set the accel/gyro filter frequency
  */
-void AP_InertialSensor_MPU9250::_set_filter_register(uint8_t filter_hz, uint8_t default_filter)
+void AP_InertialSensor_MPU9250::_set_filter(uint8_t filter_hz)
 {
-    uint8_t filter = default_filter;
-    // choose filtering frequency
-    switch (filter_hz) {
-    case 5:
-        filter = BITS_DLPF_CFG_5HZ;
-        break;
-    case 10:
-        filter = BITS_DLPF_CFG_10HZ;
-        break;
-    case 20:
-        filter = BITS_DLPF_CFG_20HZ;
-        break;
-    case 42:
-        filter = BITS_DLPF_CFG_42HZ;
-        break;
-    case 98:
-        filter = BITS_DLPF_CFG_98HZ;
-        break;
+    if (filter_hz == 0) {
+        filter_hz = _default_filter_hz;
     }
 
-    if (filter != 0) {
-        _last_filter_hz = filter_hz;
+    _accel_filter_x.set_cutoff_frequency(1000, filter_hz);
+    _accel_filter_y.set_cutoff_frequency(1000, filter_hz);
+    _accel_filter_z.set_cutoff_frequency(1000, filter_hz);
 
-        _register_write(MPUREG_CONFIG, filter);
-    }
+    _gyro_filter_x.set_cutoff_frequency(1000, filter_hz);
+    _gyro_filter_y.set_cutoff_frequency(1000, filter_hz);
+    _gyro_filter_z.set_cutoff_frequency(1000, filter_hz);    
 }
 
 
-bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate)
+/*
+  initialise the sensor configuration registers
+ */
+bool AP_InertialSensor_MPU9250::_hardware_init(void)
 {
     if (!_spi_sem->take(100)) {
-        hal.scheduler->panic(PSTR("MPU9250: Unable to get semaphore"));
+        hal.console->printf("MPU9250: Unable to get semaphore");
+        return false;
     }
 
     // initially run the bus at low speed
@@ -503,7 +418,11 @@ bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate)
     // Chip reset
     uint8_t tries;
     for (tries = 0; tries<5; tries++) {
+#if HAL_COMPASS_DEFAULT != HAL_COMPASS_AK8963 
+        /* Prevent reseting if internal AK8963 is selected, because it may corrupt
+         * AK8963's initialisation.  */
         _register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
+#endif
         hal.scheduler->delay(100);
 
         // Wake up device and select GyroZ clock. Note that the
@@ -527,68 +446,30 @@ bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate)
     }
 
     _register_write(MPUREG_PWR_MGMT_2, 0x00);            // only used for wake-up in accelerometer only low power mode
-    hal.scheduler->delay(1);
 
     // Disable I2C bus (recommended on datasheet)
+#if HAL_COMPASS_DEFAULT != HAL_COMPASS_AK8963 
+     /* Prevent disabling if internal AK8963 is selected. If internal AK8963 is not used
+      * it's ok to disable I2C slaves*/
     _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
-    hal.scheduler->delay(1);
-
-    uint8_t default_filter;
-
-    // sample rate and filtering
-    // to minimise the effects of aliasing we choose a filter
-    // that is less than half of the sample rate
-    switch (sample_rate) {
-    case RATE_50HZ:
-        // this is used for plane and rover, where noise resistance is
-        // more important than update rate. Tests on an aerobatic plane
-        // show that 10Hz is fine, and makes it very noise resistant
-        default_filter = BITS_DLPF_CFG_10HZ;
-        _sample_shift = 2;
-        break;
-    case RATE_100HZ:
-        default_filter = BITS_DLPF_CFG_20HZ;
-        _sample_shift = 1;
-        break;
-    case RATE_200HZ:
-    default:
-        default_filter = BITS_DLPF_CFG_20HZ;
-        _sample_shift = 0;
-        break;
-    }
+#endif
 
-    _set_filter_register(_mpu6000_filter, default_filter);
+    _default_filter_hz = _default_filter();
 
-    // set sample rate to 200Hz, and use _sample_divider to give
-    // the requested rate to the application
-    _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ);
-    hal.scheduler->delay(1);
+    // used a fixed filter of 42Hz on the sensor, then filter using
+    // the 2-pole software filter
+    _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_42HZ);
 
+    // set sample rate to 1kHz, and use the 2 pole filter to give the
+    // desired rate
+    _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_1000HZ);
     _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS);  // Gyro scale 2000º/s
-    hal.scheduler->delay(1);
-
-    // // read the product ID rev c has 1/2 the sensitivity of rev d
-    // _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
-    // //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
-
-    // if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) ||
-    //     (_mpu6000_product_id == MPU6000_REV_C4)   || (_mpu6000_product_id == MPU6000_REV_C5)) {
-    //     // Accel scale 8g (4096 LSB/g)
-    //     // Rev C has different scaling than rev D
-    //     _register_write(MPUREG_ACCEL_CONFIG,1<<3);
-    // } else {
-    //     // Accel scale 8g (4096 LSB/g)
-    //     _register_write(MPUREG_ACCEL_CONFIG,2<<3);
-    // }
 
     // RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 8g
     _register_write(MPUREG_ACCEL_CONFIG,2<<3);
 
-    hal.scheduler->delay(1);
-
     // configure interrupt to fire when new data arrives
     _register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);
-    hal.scheduler->delay(1);
 
     // clear interrupt on any read, and hold the data ready pin high
     // until we clear the interrupt
@@ -603,22 +484,6 @@ bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate)
     return true;
 }
 
-// return the MPUXk gyro drift rate in radian/s/s
-// note that this is much better than the oilpan gyros
-float AP_InertialSensor_MPU9250::get_gyro_drift_rate(void)
-{
-    // 0.5 degrees/second/minute
-    return ToRad(0.5/60);
-}
-
-// return true if a sample is available
-bool AP_InertialSensor_MPU9250::_sample_available()
-{
-    _poll_data();
-    return (_sum_count >> _sample_shift) > 0;
-}
-
-
 #if MPU9250_DEBUG
 // dump all config registers - used for debug
 void AP_InertialSensor_MPU9250::_dump_registers(void)
@@ -636,12 +501,4 @@ void AP_InertialSensor_MPU9250::_dump_registers(void)
 #endif
 
 
-// get_delta_time returns the time period in seconds overwhich the sensor data was collected
-float AP_InertialSensor_MPU9250::get_delta_time() const
-{
-    // the sensor runs at 200Hz
-    return 0.005 * _num_samples;
-
-}
-
 #endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
index aeb1dd998c7fd16e84324444e0293a2291fd1b0f..1ce5b1a67feafb8d6393ab88c79c22f40e81ecef 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
@@ -7,75 +7,75 @@
 #include <AP_HAL.h>
 #include <AP_Math.h>
 #include <AP_Progmem.h>
+#include <Filter.h>
+#include <LowPassFilter2p.h>
 #include "AP_InertialSensor.h"
 
 // enable debug to see a register dump on startup
 #define MPU9250_DEBUG 0
 
-class AP_InertialSensor_MPU9250 : public AP_InertialSensor
+class AP_InertialSensor_MPU9250 : public AP_InertialSensor_Backend
 {
 public:
 
-    AP_InertialSensor_MPU9250();
+    AP_InertialSensor_MPU9250(AP_InertialSensor &imu);
 
-    /* Concrete implementation of AP_InertialSensor functions: */
-    bool                update();
-    float               get_gyro_drift_rate();
+    /* update accel and gyro state */
+    bool update();
 
-    // wait for a sample to be available, with timeout in milliseconds
-    bool                wait_for_sample(uint16_t timeout_ms);
+    bool gyro_sample_available(void) { return _have_sample_available; }
+    bool accel_sample_available(void) { return _have_sample_available; }
 
-    // get_delta_time returns the time period in seconds overwhich the sensor data was collected
-    float            	get_delta_time() const;
-
-    uint16_t error_count(void) const { return _error_count; }
-    bool healthy(void) const { return _error_count <= 4; }
-    bool get_gyro_health(uint8_t instance) const { return healthy(); }
-    bool get_accel_health(uint8_t instance) const { return healthy(); }
-
-protected:
-    uint16_t                    _init_sensor( Sample_rate sample_rate );
+    // detect the sensor
+    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
 
 private:
-    AP_HAL::DigitalSource *_drdy_pin;
+    bool                 _init_sensor(void);
 
-    bool                 _sample_available();
     void                 _read_data_transaction();
     bool                 _data_ready();
     void                 _poll_data(void);
     uint8_t              _register_read( uint8_t reg );
     void                 _register_write( uint8_t reg, uint8_t val );
-    bool                 _hardware_init(Sample_rate sample_rate);
+    bool                 _hardware_init(void);
+    bool                 _sample_available();
 
     AP_HAL::SPIDeviceDriver *_spi;
     AP_HAL::Semaphore *_spi_sem;
 
-    uint16_t					_num_samples;
-    static const float          _gyro_scale;
-
-    uint32_t _last_sample_time_micros;
-
-    // ensure we can't initialise twice
-    bool                        _initialised;
-    int16_t              _mpu9250_product_id;
-
-    // how many hardware samples before we report a sample to the caller
-    uint8_t _sample_shift;
-
     // support for updating filter at runtime
-    uint8_t _last_filter_hz;
-
-    void _set_filter_register(uint8_t filter_hz, uint8_t default_filter);
-
-    uint16_t _error_count;
-
-    // accumulation in timer - must be read with timer disabled
-    // the sum of the values since last read
-    Vector3l _accel_sum;
-    Vector3l _gyro_sum;
-    volatile int16_t _sum_count;
-
-public:
+    int16_t _last_filter_hz;
+
+    // change the filter frequency
+    void _set_filter(uint8_t filter_hz);
+
+    // This structure is used to pass data from the timer which reads
+    // the sensor to the main thread. The _shared_data_idx is used to
+    // prevent race conditions by ensuring the data is fully updated
+    // before being used by the consumer
+    struct {
+        Vector3f _accel_filtered;
+        Vector3f _gyro_filtered;
+    } _shared_data[2];
+    volatile uint8_t _shared_data_idx;
+
+    // Low Pass filters for gyro and accel 
+    LowPassFilter2p _accel_filter_x;
+    LowPassFilter2p _accel_filter_y;
+    LowPassFilter2p _accel_filter_z;
+    LowPassFilter2p _gyro_filter_x;
+    LowPassFilter2p _gyro_filter_y;
+    LowPassFilter2p _gyro_filter_z;
+
+    // do we currently have a sample pending?
+    bool _have_sample_available;
+
+    // default filter frequency when set to zero
+    uint8_t _default_filter_hz;
+
+    // gyro and accel instances
+    uint8_t _gyro_instance;
+    uint8_t _accel_instance;
 
 #if MPU9250_DEBUG
     void						_dump_registers(void);
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
index 7070ff8e87ad800097a1d92ec410ab70f94e6c7d..df56b447464ca65f96a24fc496b889c0853b80b9 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
@@ -1,11 +1,16 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 #include <AP_HAL.h>
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
 #include "AP_InertialSensor_Oilpan.h"
+#include <AP_ADC.h>
 
 const extern AP_HAL::HAL& hal;
 
+// this driver assumes an AP_ADC object has been declared globally
+extern AP_ADC_ADS7844 apm1_adc;
+
 // ADC channel mappings on for the APM Oilpan
 // Sensors: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
 const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 };
@@ -14,9 +19,6 @@ const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 };
 const int8_t AP_InertialSensor_Oilpan::_sensor_signs[6] =
 { 1, -1, -1, 1, -1, -1 };
 
-// ADC channel reading the gyro temperature
-const uint8_t AP_InertialSensor_Oilpan::_gyro_temp_ch = 3;
-
 // Maximum possible value returned by an offset-corrected sensor channel
 const float AP_InertialSensor_Oilpan::_adc_constraint = 900;
 
@@ -30,120 +32,94 @@ const float AP_InertialSensor_Oilpan::_adc_constraint = 900;
 #define OILPAN_RAW_ACCEL_OFFSET ((2465.0f + 1617.0f) * 0.5f)
 #define OILPAN_RAW_GYRO_OFFSET  1658.0f
 
-#define ToRad(x) radians(x)      // *pi/180
 // IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s,
 // 0.8mV/ADC step => 0.8/3.33 = 0.4
 // Tested values : 0.4026, ?, 0.4192
-const float AP_InertialSensor_Oilpan::_gyro_gain_x = ToRad(0.4f);
-const float AP_InertialSensor_Oilpan::_gyro_gain_y = ToRad(0.41f);
-const float AP_InertialSensor_Oilpan::_gyro_gain_z = ToRad(0.41f);
+const float AP_InertialSensor_Oilpan::_gyro_gain_x = radians(0.4f);
+const float AP_InertialSensor_Oilpan::_gyro_gain_y = radians(0.41f);
+const float AP_InertialSensor_Oilpan::_gyro_gain_z = radians(0.41f);
 
 /* ------ Public functions -------------------------------------------*/
 
-AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) : 
-    AP_InertialSensor(),
-    _adc(adc)
+AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan(AP_InertialSensor &imu) : 
+    AP_InertialSensor_Backend(imu)
+{
+}
+
+/*
+  detect the sensor
+ */
+AP_InertialSensor_Backend *AP_InertialSensor_Oilpan::detect(AP_InertialSensor &_imu)
 {
+    AP_InertialSensor_Oilpan *sensor = new AP_InertialSensor_Oilpan(_imu);
+    if (sensor == NULL) {
+        return NULL;
+    }
+    if (!sensor->_init_sensor()) {
+        delete sensor;
+        return NULL;
+    }
+
+    return sensor;
 }
 
-uint16_t AP_InertialSensor_Oilpan::_init_sensor( Sample_rate sample_rate)
+bool AP_InertialSensor_Oilpan::_init_sensor(void)
 {
-    _adc->Init();
+    apm1_adc.Init();
 
-    switch (sample_rate) {
-    case RATE_50HZ:
+    switch (_imu.get_sample_rate()) {
+    case AP_InertialSensor::RATE_50HZ:
         _sample_threshold = 20;
         break;
-    case RATE_100HZ:
+    case AP_InertialSensor::RATE_100HZ:
         _sample_threshold = 10;
         break;
-    case RATE_200HZ:
+    case AP_InertialSensor::RATE_200HZ:
         _sample_threshold = 5;
         break;
+    default:
+        // can't do this speed
+        return false;
     }
 
-#if defined(__AVR_ATmega1280__)
-    return AP_PRODUCT_ID_APM1_1280;
-#else
-    return AP_PRODUCT_ID_APM1_2560;
-#endif
+    _gyro_instance = _imu.register_gyro();
+    _accel_instance = _imu.register_accel();
+
+    _product_id = AP_PRODUCT_ID_APM1_2560;
+
+    return true;
 }
 
+/*
+  copy data from ADC to frontend
+ */
 bool AP_InertialSensor_Oilpan::update()
 {
-    if (!wait_for_sample(100)) {
-        return false;
-    }
     float adc_values[6];
-    Vector3f gyro_offset = _gyro_offset[0].get();
-    Vector3f accel_scale = _accel_scale[0].get();
-    Vector3f accel_offset = _accel_offset[0].get();
-
-    _delta_time_micros = _adc->Ch6(_sensors, adc_values);
-    _temp = _adc->Ch(_gyro_temp_ch);
-
-    _gyro[0] = Vector3f(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ),
-                        _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ),
-                        _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ));
-    _gyro[0].rotate(_board_orientation);
-    _gyro[0].x *= _gyro_gain_x;
-    _gyro[0].y *= _gyro_gain_y;
-    _gyro[0].z *= _gyro_gain_z;
-    _gyro[0] -= gyro_offset;
-
-    _previous_accel[0] = _accel[0];
-
-    _accel[0]  = Vector3f(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
-                          _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
-                          _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));
-    _accel[0].rotate(_board_orientation);
-    _accel[0].x *= accel_scale.x;
-    _accel[0].y *= accel_scale.y;
-    _accel[0].z *= accel_scale.z;
-    _accel[0]   *= OILPAN_ACCEL_SCALE_1G;
-    _accel[0] -= accel_offset;
-
-/*
- *  X  = 1619.30 to 2445.69
- *  Y =  1609.45 to 2435.42
- *  Z =  1627.44  to 2434.82
- */
 
-    return true;
-}
+    apm1_adc.Ch6(_sensors, adc_values);
 
-float AP_InertialSensor_Oilpan::get_delta_time() const {
-    return _delta_time_micros * 1.0e-6;
-}
+    // copy gyros to frontend
+    Vector3f v;
+    v(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_x,
+      _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_y,
+      _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_z);
+    _rotate_and_offset_gyro(_gyro_instance, v);
 
-/* ------ Private functions -------------------------------------------*/
+    // copy accels to frontend
+    v(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
+      _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
+      _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));
+    v *= OILPAN_ACCEL_SCALE_1G;
+    _rotate_and_offset_accel(_accel_instance, v);
 
-// return the oilpan gyro drift rate in radian/s/s
-float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void)
-{
-    // 3.0 degrees/second/minute
-    return ToRad(3.0/60);
+    return true;
 }
 
 // return true if a new sample is available
-bool AP_InertialSensor_Oilpan::_sample_available()
-{
-    return (_adc->num_samples_available(_sensors) / _sample_threshold) > 0;
-}
-
-bool AP_InertialSensor_Oilpan::wait_for_sample(uint16_t timeout_ms)
+bool AP_InertialSensor_Oilpan::_sample_available() const
 {
-    if (_sample_available()) {
-        return true;
-    }
-    uint32_t start = hal.scheduler->millis();
-    while ((hal.scheduler->millis() - start) < timeout_ms) {
-        hal.scheduler->delay_microseconds(100);
-        if (_sample_available()) {
-            return true;
-        }
-    }
-    return false;
+    return apm1_adc.num_samples_available(_sensors) >= _sample_threshold;
 }
 
 #endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
index bffb4f21146ce1abfe8e2b3af41d05d1b4b33f3d..e00169424c697d49becc7c443c04e422df9754ff 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
@@ -3,50 +3,35 @@
 #ifndef __AP_INERTIAL_SENSOR_OILPAN_H__
 #define __AP_INERTIAL_SENSOR_OILPAN_H__
 
-#include <stdint.h>
-
-#include <AP_ADC.h>
-#include <AP_Math.h>
+#include <AP_HAL.h>
 #include "AP_InertialSensor.h"
 
-class AP_InertialSensor_Oilpan : public AP_InertialSensor
+class AP_InertialSensor_Oilpan : public AP_InertialSensor_Backend
 {
 public:
+    AP_InertialSensor_Oilpan(AP_InertialSensor &imu);
 
-    AP_InertialSensor_Oilpan( AP_ADC * adc );
-
-    /* Concrete implementation of AP_InertialSensor functions: */
-    bool            update();
-    float        	get_delta_time() const;
-    float           get_gyro_drift_rate();
+    /* update accel and gyro state */
+    bool update();
 
-    // wait for a sample to be available, with timeout in milliseconds
-    bool            wait_for_sample(uint16_t timeout_ms);
+    bool gyro_sample_available(void) { return _sample_available(); }
+    bool accel_sample_available(void) { return _sample_available(); }
 
-protected:
-    uint16_t        _init_sensor(Sample_rate sample_rate);
+    // detect the sensor
+    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
 
 private:
-
-    bool            			_sample_available();
-
-    AP_ADC *                    _adc;
-
-    float                       _temp;
-
-    uint32_t                    _delta_time_micros;
-
+    bool                        _init_sensor(void);
+    bool            			_sample_available() const;
     static const uint8_t        _sensors[6];
     static const int8_t         _sensor_signs[6];
-    static const uint8_t        _gyro_temp_ch;
-
     static const float          _gyro_gain_x;
     static const float          _gyro_gain_y;
     static const float          _gyro_gain_z;
-
     static const float          _adc_constraint;
-
     uint8_t                     _sample_threshold;
+    uint8_t                     _gyro_instance;
+    uint8_t                     _accel_instance;
 };
 
 #endif // __AP_INERTIAL_SENSOR_OILPAN_H__
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
index 4f473f5637e0f2558afe8c7fb69c9f828afdac98..c36e1264141fa949b2ec1e880bdc12437bf03763 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
@@ -1,7 +1,8 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 #include <AP_HAL.h>
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+
 #include "AP_InertialSensor_PX4.h"
 
 const extern AP_HAL::HAL& hal;
@@ -15,11 +16,33 @@ const extern AP_HAL::HAL& hal;
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_hrt.h>
 
-#include <AP_Notify.h>
+#include <stdio.h>
+
+AP_InertialSensor_PX4::AP_InertialSensor_PX4(AP_InertialSensor &imu) :
+    AP_InertialSensor_Backend(imu),
+    _last_get_sample_timestamp(0)
+{
+}
+
+/*
+  detect the sensor
+ */
+AP_InertialSensor_Backend *AP_InertialSensor_PX4::detect(AP_InertialSensor &_imu)
+{
+    AP_InertialSensor_PX4 *sensor = new AP_InertialSensor_PX4(_imu);
+    if (sensor == NULL) {
+        return NULL;
+    }
+    if (!sensor->_init_sensor()) {
+        delete sensor;
+        return NULL;
+    }
+    return sensor;
+}
 
-uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) 
+bool AP_InertialSensor_PX4::_init_sensor(void) 
 {
-    // assumes max 2 instances
+    // assumes max 3 instances
     _accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY);
     _accel_fd[1] = open(ACCEL_DEVICE_PATH "1", O_RDONLY);
     _accel_fd[2] = open(ACCEL_DEVICE_PATH "2", O_RDONLY);
@@ -32,45 +55,30 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
     for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
         if (_accel_fd[i] >= 0) {
             _num_accel_instances = i+1;
+            _accel_instance[i] = _imu.register_accel();
         }
         if (_gyro_fd[i] >= 0) {
             _num_gyro_instances = i+1;
+            _gyro_instance[i] = _imu.register_gyro();
         }
     }    
 	if (_num_accel_instances == 0) {
-        hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH);
+        return false;
     }
 	if (_num_gyro_instances == 0) {
-        hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH);
-    }
-
-    switch (sample_rate) {
-    case RATE_50HZ:
-        _default_filter_hz = 15;
-        _sample_time_usec = 20000;
-        break;
-    case RATE_100HZ:
-        _default_filter_hz = 30;
-        _sample_time_usec = 10000;
-        break;
-    case RATE_200HZ:
-        _default_filter_hz = 30;
-        _sample_time_usec = 5000;
-        break;
-    case RATE_400HZ:
-    default:
-        _default_filter_hz = 30;
-        _sample_time_usec = 2500;
-        break;
+        return false;
     }
 
-    _set_filter_frequency(_mpu6000_filter);
+    _default_filter_hz = _default_filter();
+    _set_filter_frequency(_imu.get_filter());
 
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
-    return AP_PRODUCT_ID_PX4_V2;
+    _product_id = AP_PRODUCT_ID_PX4_V2;
 #else
-    return AP_PRODUCT_ID_PX4;
+    _product_id = AP_PRODUCT_ID_PX4;
 #endif
+
+    return true;
 }
 
 /*
@@ -89,109 +97,39 @@ void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
     }
 }
 
-/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
-
-// multi-device interface
-bool AP_InertialSensor_PX4::get_gyro_health(uint8_t instance) const
-{
-    if (_sample_time_usec == 0 || _last_get_sample_timestamp == 0) {
-        // not initialised yet, show as healthy to prevent scary GCS
-        // warnings
-        return true;
-    }
-    if (instance >= _num_gyro_instances) {
-        return false;
-    }
-
-    if ((_last_get_sample_timestamp - _last_gyro_timestamp[instance]) > 2*_sample_time_usec) {
-        // gyros have not updated
-        return false;
-    }
-    return true;
-}
-
-uint8_t AP_InertialSensor_PX4::get_gyro_count(void) const
-{
-    return _num_gyro_instances;
-}
-
-bool AP_InertialSensor_PX4::get_accel_health(uint8_t k) const
-{
-    if (_sample_time_usec == 0 || _last_get_sample_timestamp == 0) {
-        // not initialised yet, show as healthy to prevent scary GCS
-        // warnings
-        return true;
-    }
-    if (k >= _num_accel_instances) {
-        return false;
-    }
-
-    if ((_last_get_sample_timestamp - _last_accel_timestamp[k]) > 2*_sample_time_usec) {
-        // accels have not updated
-        return false;
-    }
-    if (fabsf(_accel[k].x) > 30 && fabsf(_accel[k].y) > 30 && fabsf(_accel[k].z) > 30 &&
-        (_previous_accel[k] - _accel[k]).length() < 0.01f) {
-        // unchanging accel, large in all 3 axes. This is a likely
-        // accelerometer failure of the LSM303d
-        return false;
-    }
-    return true;
-    
-}
-
-uint8_t AP_InertialSensor_PX4::get_accel_count(void) const
-{
-    return _num_accel_instances;
-}
-
 bool AP_InertialSensor_PX4::update(void) 
 {
-    if (!wait_for_sample(100)) {
-        return false;
-    }
-
     // get the latest sample from the sensor drivers
     _get_sample();
 
-
     for (uint8_t k=0; k<_num_accel_instances; k++) {
-        _previous_accel[k] = _accel[k];
-        _accel[k] = _accel_in[k];
-        _accel[k].rotate(_board_orientation);
-        _accel[k].x *= _accel_scale[k].get().x;
-        _accel[k].y *= _accel_scale[k].get().y;
-        _accel[k].z *= _accel_scale[k].get().z;
-        _accel[k]   -= _accel_offset[k];
+        Vector3f accel = _accel_in[k];
+        // calling _rotate_and_offset_accel sets the sensor healthy,
+        // so we only want to do this if we have new data from it
+        if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) {
+            _rotate_and_offset_accel(_accel_instance[k], accel);
+            _last_accel_update_timestamp[k] = _last_accel_timestamp[k];
+        }
     }
 
     for (uint8_t k=0; k<_num_gyro_instances; k++) {
-        _gyro[k] = _gyro_in[k];
-        _gyro[k].rotate(_board_orientation);
-        _gyro[k] -= _gyro_offset[k];
+        Vector3f gyro = _gyro_in[k];
+        // calling _rotate_and_offset_accel sets the sensor healthy,
+        // so we only want to do this if we have new data from it
+        if (_last_gyro_timestamp[k] != _last_gyro_update_timestamp[k]) {
+            _rotate_and_offset_gyro(_gyro_instance[k], gyro);
+            _last_gyro_update_timestamp[k] = _last_gyro_timestamp[k];
+        }
     }
 
-    if (_last_filter_hz != _mpu6000_filter) {
-        _set_filter_frequency(_mpu6000_filter);
-        _last_filter_hz = _mpu6000_filter;
+    if (_last_filter_hz != _imu.get_filter()) {
+        _set_filter_frequency(_imu.get_filter());
+        _last_filter_hz = _imu.get_filter();
     }
 
-    _have_sample_available = false;
-
     return true;
 }
 
-float AP_InertialSensor_PX4::get_delta_time(void) const
-{
-    return _sample_time_usec * 1.0e-6f;
-}
-
-float AP_InertialSensor_PX4::get_gyro_drift_rate(void) 
-{
-    // assume 0.5 degrees/second/minute
-    return ToRad(0.5/60);
-}
-
 void AP_InertialSensor_PX4::_get_sample(void)
 {
     for (uint8_t i=0; i<_num_accel_instances; i++) {
@@ -201,6 +139,7 @@ void AP_InertialSensor_PX4::_get_sample(void)
                accel_report.timestamp != _last_accel_timestamp[i]) {        
             _accel_in[i] = Vector3f(accel_report.x, accel_report.y, accel_report.z);
             _last_accel_timestamp[i] = accel_report.timestamp;
+            _set_accel_error_count(_accel_instance[i], accel_report.error_count);
         }
     }
     for (uint8_t i=0; i<_num_gyro_instances; i++) {
@@ -210,64 +149,32 @@ void AP_InertialSensor_PX4::_get_sample(void)
                gyro_report.timestamp != _last_gyro_timestamp[i]) {        
             _gyro_in[i] = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
             _last_gyro_timestamp[i] = gyro_report.timestamp;
+            _set_gyro_error_count(_gyro_instance[i], gyro_report.error_count);
         }
     }
-    _last_get_sample_timestamp = hrt_absolute_time();
+    _last_get_sample_timestamp = hal.scheduler->micros64();
 }
 
-bool AP_InertialSensor_PX4::_sample_available(void)
+bool AP_InertialSensor_PX4::gyro_sample_available(void)
 {
-    uint64_t tnow = hrt_absolute_time();
-    while (tnow - _last_sample_timestamp > _sample_time_usec) {
-        _have_sample_available = true;
-        _last_sample_timestamp += _sample_time_usec;
-    }
-    return _have_sample_available;
-}
-
-bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
-{
-    if (_sample_available()) {
-        return true;
-    }
-    uint32_t start = hal.scheduler->millis();
-    while ((hal.scheduler->millis() - start) < timeout_ms) {
-        uint64_t tnow = hrt_absolute_time();
-        // we spin for the last timing_lag microseconds. Before that
-        // we yield the CPU to allow IO to happen
-        const uint16_t timing_lag = 400;
-        if (_last_sample_timestamp + _sample_time_usec > tnow+timing_lag) {
-            hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_time_usec - (tnow+timing_lag));
-        }
-        if (_sample_available()) {
+    _get_sample();
+    for (uint8_t i=0; i<_num_gyro_instances; i++) {
+        if (_last_gyro_timestamp[i] != _last_gyro_update_timestamp[i]) {
             return true;
         }
-    }
-    return false;
-}
-
-/**
-   try to detect bad accel/gyro sensors
- */
-bool AP_InertialSensor_PX4::healthy(void) const
-{
-    return get_gyro_health(0) && get_accel_health(0);
-}
-
-uint8_t AP_InertialSensor_PX4::_get_primary_gyro(void) const 
-{
-    for (uint8_t i=0; i<_num_gyro_instances; i++) {
-        if (get_gyro_health(i)) return i;
     }    
-    return 0;
+    return false;
 }
 
-uint8_t AP_InertialSensor_PX4::get_primary_accel(void) const 
+bool AP_InertialSensor_PX4::accel_sample_available(void)
 {
+    _get_sample();
     for (uint8_t i=0; i<_num_accel_instances; i++) {
-        if (get_accel_health(i)) return i;
+        if (_last_accel_timestamp[i] != _last_accel_update_timestamp[i]) {
+            return true;
+        }
     }    
-    return 0;
+    return false;
 }
 
 #endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
index 1950910c4ec69528ed0db51e8c684d336983c655..262608baaa201dd14cdd2a70e96660cb761c7f52 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
@@ -4,7 +4,7 @@
 #define __AP_INERTIAL_SENSOR_PX4_H__
 
 #include <AP_HAL.h>
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
 
 #include <AP_Progmem.h>
 #include "AP_InertialSensor.h"
@@ -13,47 +13,33 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
 
-class AP_InertialSensor_PX4 : public AP_InertialSensor
+class AP_InertialSensor_PX4 : public AP_InertialSensor_Backend
 {
 public:
 
-    AP_InertialSensor_PX4() : 
-        AP_InertialSensor(),
-        _last_get_sample_timestamp(0),
-        _sample_time_usec(0)
-        {
-        }
+    AP_InertialSensor_PX4(AP_InertialSensor &imu);
 
-    /* Concrete implementation of AP_InertialSensor functions: */
-    bool            update();
-    float        	get_delta_time() const;
-    float           get_gyro_drift_rate();
-    bool            wait_for_sample(uint16_t timeout_ms);
-    bool            healthy(void) const;
+    /* update accel and gyro state */
+    bool update();
 
-    // multi-device interface
-    bool get_gyro_health(uint8_t instance) const;
-    uint8_t get_gyro_count(void) const;
+    // detect the sensor
+    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
 
-    bool get_accel_health(uint8_t instance) const;
-    uint8_t get_accel_count(void) const;
-
-    uint8_t get_primary_accel(void) const;
+    bool gyro_sample_available(void);
+    bool accel_sample_available(void);
 
 private:
-    uint8_t _get_primary_gyro(void) const;
-
-    uint16_t _init_sensor( Sample_rate sample_rate );
+    bool     _init_sensor(void);
     void     _get_sample(void);
     bool     _sample_available(void);
     Vector3f _accel_in[INS_MAX_INSTANCES];
     Vector3f _gyro_in[INS_MAX_INSTANCES];
     uint64_t _last_accel_timestamp[INS_MAX_INSTANCES];
     uint64_t _last_gyro_timestamp[INS_MAX_INSTANCES];
+    uint64_t _last_accel_update_timestamp[INS_MAX_INSTANCES];
+    uint64_t _last_gyro_update_timestamp[INS_MAX_INSTANCES];
     uint64_t _last_get_sample_timestamp;
     uint64_t _last_sample_timestamp;
-    uint32_t _sample_time_usec;
-    bool     _have_sample_available;
 
     // support for updating filter at runtime
     uint8_t _last_filter_hz;
@@ -64,8 +50,14 @@ private:
     // accelerometer and gyro driver handles
     uint8_t _num_accel_instances;
     uint8_t _num_gyro_instances;
+
     int _accel_fd[INS_MAX_INSTANCES];
     int _gyro_fd[INS_MAX_INSTANCES];
+
+    // indexes in frontend object. Note that these could be different
+    // from the backend indexes
+    uint8_t _accel_instance[INS_MAX_INSTANCES];
+    uint8_t _gyro_instance[INS_MAX_INSTANCES];
 };
-#endif
+#endif // CONFIG_HAL_BOARD
 #endif // __AP_INERTIAL_SENSOR_PX4_H__
diff --git a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde
index c28dc8e7d2bc22c42f1a398de7e87b9c2aad9b67..81b0e7bcc0c8467c939424b404e69b242adf2db6 100644
--- a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde
+++ b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde
@@ -36,28 +36,12 @@
 #include <AP_Declination.h>
 #include <AP_NavEKF.h>
 #include <AP_HAL_Linux.h>
+#include <AP_Rally.h>
+#include <AP_Scheduler.h>
 
 const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
 
-#define CONFIG_INS_TYPE HAL_INS_DEFAULT
-
-#if CONFIG_INS_TYPE == HAL_INS_MPU6000
-AP_InertialSensor_MPU6000 ins;
-#elif CONFIG_INS_TYPE == HAL_INS_PX4
-AP_InertialSensor_PX4 ins;
-#elif CONFIG_INS_TYPE == HAL_INS_VRBRAIN
-AP_InertialSensor_VRBRAIN ins;
-#elif CONFIG_INS_TYPE == HAL_INS_HIL
-AP_InertialSensor_HIL ins;
-#elif CONFIG_INS_TYPE == HAL_INS_FLYMAPLE
-AP_InertialSensor_Flymaple ins;
-#elif CONFIG_INS_TYPE == HAL_INS_L3G4200D
-AP_InertialSensor_L3G4200D ins;
-#elif CONFIG_INS_TYPE == HAL_INS_MPU9250
-AP_InertialSensor_MPU9250 ins;
-#else
-  #error Unrecognised CONFIG_INS_TYPE setting.
-#endif // CONFIG_INS_TYPE
+AP_InertialSensor ins;
 
 void setup(void)
 {
@@ -208,7 +192,7 @@ void run_test()
     while( !hal.console->available() ) {
 
         // wait until we have a sample
-        ins.wait_for_sample(1000);
+        ins.wait_for_sample();
 
         // read samples from ins
         ins.update();