AP_InertialSensor: converted PX4 driver to new API

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -278,19 +278,20 @@ AP_InertialSensor::init( Start_style style,
 
     switch (sample_rate) {
     case RATE_50HZ:
-        _delta_time = 1.0f/50;
+        _sample_period_usec = 20000;
         break;
     case RATE_100HZ:
-        _delta_time = 1.0f/100;
+        _sample_period_usec = 10000;
         break;
     case RATE_200HZ:
-        _delta_time = 1.0f/200;
+        _sample_period_usec = 5000;
         break;
     case RATE_400HZ:
     default:
-        _delta_time = 1.0f/400;
+        _sample_period_usec = 2500;
         break;
     }
+    _delta_time = 1.0e-6f * _sample_period_usec;
 }
 
 
@@ -301,9 +302,11 @@ void
 AP_InertialSensor::_detect_backends(Sample_rate sample_rate)
 {
 #if HAL_INS_DEFAULT == HAL_INS_HIL
-    _backends[0] = AP_InertialSensor_HIL::detect(*this, sample_rate, _gyro[_gyro_count], _accel[_accel_count]);
+    _backends[0] = AP_InertialSensor_HIL::detect(*this, sample_rate);
 #elif HAL_INS_DEFAULT == HAL_INS_MPU6000
-    _backends[0] = AP_InertialSensor_MPU6000::detect(*this, sample_rate, _gyro[_gyro_count], _accel[_accel_count]);
+    _backends[0] = AP_InertialSensor_MPU6000::detect(*this, sample_rate);
+#elif HAL_INS_DEFAULT == HAL_INS_PX4
+    _backends[0] = AP_InertialSensor_PX4::detect(*this, sample_rate);
 #else
     #error Unrecognised HAL_INS_TYPE setting
 #endif
@@ -912,13 +915,32 @@ void AP_InertialSensor::update(void)
 }
 
 /*
-  wait for a sample to be available
+  wait for a sample to be available. This waits for at least one new
+  accel and one new gyro sample. It is up to the backend to define
+  what a new sample means. Some backends are based on the sensor
+  providing a sample, some are based on time.
  */
 void AP_InertialSensor::wait_for_sample(void)
 {
+    bool have_sample = false;
+
+    // wait the right amount of time for a sample to be due
+    uint32_t now = hal.scheduler->micros();
+    while (now - _last_sample_usec > _sample_period_usec) {
+        _last_sample_usec += _sample_period_usec;
+        have_sample = true;
+    }
+    if (!have_sample) {
+        uint32_t sample_due = _last_sample_usec + _sample_period_usec;
+        uint32_t wait_usec = sample_due - now;
+        hal.scheduler->delay_microseconds(wait_usec);
+        _last_sample_usec += _sample_period_usec;
+    }
+
+    // but also wait for at least one backend to have a sample of both
+    // accel and gyro
     bool gyro_available = false;
     bool accel_available = false;
-
     while (!gyro_available || !accel_available) {
         for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
             if (_backends[i] != NULL) {
@@ -926,7 +948,9 @@ void AP_InertialSensor::wait_for_sample(void)
                 accel_available |= _backends[i]->accel_sample_available();
             }
         }
-        hal.scheduler->delay(1);
+        if (!gyro_available || !accel_available) {
+            hal.scheduler->delay_microseconds(100);
+        }
     }
 }
 

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -247,10 +247,17 @@ private:
 
     // time between samples
     float _delta_time;
+
+    // last time a wait_for_sample() returned a sample
+    uint32_t _last_sample_usec;
+    
+    // time between samples in microseconds
+    uint32_t _sample_period_usec;
 };
 
 #include "AP_InertialSensor_Backend.h"
 #include "AP_InertialSensor_MPU6000.h"
+#include "AP_InertialSensor_PX4.h"
 #include "AP_InertialSensor_HIL.h"
 #include "AP_InertialSensor_UserInteract_Stream.h"
 #include "AP_InertialSensor_UserInteract_MAVLink.h"

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -4,17 +4,16 @@
 #include "AP_InertialSensor.h"
 #include "AP_InertialSensor_Backend.h"
 
-AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel) :
+AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
-    _imu(imu),
+    _imu(imu)
-    _gyro(gyro),
-    _accel(accel)
 {}
 
 /*
   rotate gyro vector and add the gyro offset
  */
-void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, uint32_t now)
+void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro, uint32_t now)
 {
+    _imu._gyro[instance] = gyro;
     _imu._gyro[instance].rotate(_imu._board_orientation);
     _imu._gyro[instance] -= _imu._gyro_offset[instance];
     _imu._last_gyro_sample_time_usec[instance] = now;
@@ -23,8 +22,9 @@ void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, uint32
 /*
   rotate accel vector, scale and add the accel offset
  */
-void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, uint32_t now)
+void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, const Vector3f &accel, uint32_t now)
 {
+    _imu._accel[instance] = accel;
     _imu._accel[instance].rotate(_imu._board_orientation);
 
     const Vector3f &accel_scale = _imu._accel_scale[instance].get();

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -23,7 +23,7 @@
 class AP_InertialSensor_Backend
 {
 public:
-    AP_InertialSensor_Backend(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel);
+    AP_InertialSensor_Backend(AP_InertialSensor &imu);
 
     // we declare a virtual destructor so that drivers can
     // override with a custom destructor if need be.
@@ -50,15 +50,11 @@ public:
 protected:
     AP_InertialSensor &_imu;      ///< access to frontend
 
-    // references to instance vectors
-    Vector3f &_gyro;
-    Vector3f &_accel;
-
     // rotate gyro vector and offset
-    void _rotate_and_offset_gyro(uint8_t instance, uint32_t now);
+    void _rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro, uint32_t now);
 
     // rotate accel vector, scale and offset
-    void _rotate_and_offset_accel(uint8_t instance, uint32_t now);
+    void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel, uint32_t now);
 
     // note that each backend is also expected to have a detect()
     // function which instantiates an instance of the backend sensor

libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp

@@ -5,10 +5,9 @@
 
 const extern AP_HAL::HAL& hal;
 
-AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel) :
+AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu) :
-    AP_InertialSensor_Backend(imu, gyro, accel),
+    AP_InertialSensor_Backend(imu),
-    _sample_period_usec(0),
+    _sample_period_usec(0)
-    _last_sample_usec(0)
 {
 }
 
@@ -16,11 +15,9 @@ AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu, Vector3f &g
   detect the sensor
  */
 AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu, 
-                                                         AP_InertialSensor::Sample_rate sample_rate,
+                                                         AP_InertialSensor::Sample_rate sample_rate)
-                                                         Vector3f &gyro, 
-                                                         Vector3f &accel)
 {
-    AP_InertialSensor_HIL *sensor = new AP_InertialSensor_HIL(_imu, gyro, accel);
+    AP_InertialSensor_HIL *sensor = new AP_InertialSensor_HIL(_imu);
     if (sensor == NULL) {
         return NULL;
     }
@@ -57,15 +54,10 @@ bool AP_InertialSensor_HIL::_init_sensor(AP_InertialSensor::Sample_rate sample_r
 
 bool AP_InertialSensor_HIL::update(void) 
 {
-    uint32_t now = hal.scheduler->micros();
-    while (now - _last_sample_usec > _sample_period_usec) {
-        _last_sample_usec += _sample_period_usec;
-    }
     return true;
 }
 
 bool AP_InertialSensor_HIL::_sample_available()
 {
-    return (hal.scheduler->micros() - _last_sample_usec > _sample_period_usec);
+    return true;
 }
-

libraries/AP_InertialSensor/AP_InertialSensor_HIL.h

@@ -8,7 +8,7 @@
 class AP_InertialSensor_HIL : public AP_InertialSensor_Backend
 {
 public:
-    AP_InertialSensor_HIL(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel);
+    AP_InertialSensor_HIL(AP_InertialSensor &imu);
 
     /* update accel and gyro state */
     bool update();
@@ -18,15 +18,12 @@ public:
 
     // detect the sensor
     static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
-                                             AP_InertialSensor::Sample_rate sample_rate,
+                                             AP_InertialSensor::Sample_rate sample_rate);
-                                             Vector3f &gyro,
-                                             Vector3f &accel);
 
 private:
     bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
     bool _sample_available(void);
     uint32_t _sample_period_usec;
-    uint32_t _last_sample_usec;
 };
 
 #endif // __AP_INERTIALSENSOR_HIL_H__

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -173,10 +173,8 @@ const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532f / 16.4f);
  *  variants however
  */
 
-AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, 
+AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu) :
-                                                     Vector3f &gyro, 
+    AP_InertialSensor_Backend(imu),
-                                                     Vector3f &accel) : 
-    AP_InertialSensor_Backend(imu, gyro, accel),
     _drdy_pin(NULL),
     _spi(NULL),
     _spi_sem(NULL),
@@ -193,11 +191,9 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu,
   detect the sensor
  */
 AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect(AP_InertialSensor &_imu, 
-                                                             AP_InertialSensor::Sample_rate sample_rate,
+                                                             AP_InertialSensor::Sample_rate sample_rate)
-                                                             Vector3f &gyro, 
-                                                             Vector3f &accel)
 {
-    AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu, gyro, accel);
+    AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu);
     if (sensor == NULL) {
         return NULL;
     }
@@ -276,21 +272,22 @@ bool AP_InertialSensor_MPU6000::update( void )
     // we have a full set of samples
     uint16_t num_samples;
     uint32_t now = hal.scheduler->micros();
+    Vector3f accel, gyro;
 
     hal.scheduler->suspend_timer_procs();
-    _gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
+    gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
-    _accel(_accel_sum.x, _accel_sum.y, _accel_sum.z);
+    accel(_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();
 
-    _gyro *= _gyro_scale / num_samples;
+    gyro *= _gyro_scale / num_samples;
-    _rotate_and_offset_gyro(_gyro_instance, now);
+    _rotate_and_offset_gyro(_gyro_instance, gyro, now);
 
-    _accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
+    accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
-    _rotate_and_offset_accel(_accel_instance, now);
+    _rotate_and_offset_accel(_accel_instance, accel, now);
 
     if (_last_filter_hz != _imu.get_filter()) {
         if (_spi_sem->take(10)) {

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -15,7 +15,7 @@
 class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
 {
 public:
-    AP_InertialSensor_MPU6000(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel);
+    AP_InertialSensor_MPU6000(AP_InertialSensor &imu);
 
     /* update accel and gyro state */
     bool update();
@@ -25,9 +25,8 @@ public:
 
     // detect the sensor
     static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
-                                             AP_InertialSensor::Sample_rate sample_rate,
+                                             AP_InertialSensor::Sample_rate sample_rate);
-                                             Vector3f &gyro,
+
-                                             Vector3f &accel);
 private:
 #if MPU6000_DEBUG
     void _dump_registers(void);

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -2,6 +2,7 @@
 
 #include <AP_HAL.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+
 #include "AP_InertialSensor_PX4.h"
 
 const extern AP_HAL::HAL& hal;
@@ -15,11 +16,35 @@ const extern AP_HAL::HAL& hal;
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_hrt.h>
 
-#include <AP_Notify.h>
+#include <stdio.h>
 
-uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) 
+AP_InertialSensor_PX4::AP_InertialSensor_PX4(AP_InertialSensor &imu) :
+    AP_InertialSensor_Backend(imu),
+    _last_get_sample_timestamp(0),
+    _sample_time_usec(0)
 {
-    // assumes max 2 instances
+}
+
+/*
+  detect the sensor
+ */
+AP_InertialSensor_Backend *AP_InertialSensor_PX4::detect(AP_InertialSensor &_imu, 
+                                                         AP_InertialSensor::Sample_rate sample_rate)
+{
+    AP_InertialSensor_PX4 *sensor = new AP_InertialSensor_PX4(_imu);
+    if (sensor == NULL) {
+        return NULL;
+    }
+    if (!sensor->_init_sensor(sample_rate)) {
+        delete sensor;
+        return NULL;
+    }
+    return sensor;
+}
+
+bool AP_InertialSensor_PX4::_init_sensor(AP_InertialSensor::Sample_rate sample_rate) 
+{
+    // 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 +57,44 @@ 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);
+        return false;
     }
 
     switch (sample_rate) {
-    case RATE_50HZ:
+    case AP_InertialSensor::RATE_50HZ:
         _default_filter_hz = 15;
         _sample_time_usec = 20000;
         break;
-    case RATE_100HZ:
+    case AP_InertialSensor::RATE_100HZ:
         _default_filter_hz = 30;
         _sample_time_usec = 10000;
         break;
-    case RATE_200HZ:
+    case AP_InertialSensor::RATE_200HZ:
         _default_filter_hz = 30;
         _sample_time_usec = 5000;
         break;
-    case RATE_400HZ:
+    case AP_InertialSensor::RATE_400HZ:
-    default:
         _default_filter_hz = 30;
         _sample_time_usec = 2500;
         break;
+    default:
+        return false;
     }
 
-    _set_filter_frequency(_mpu6000_filter);
+    _set_filter_frequency(_imu.get_filter());
 
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+    return true;
-    return AP_PRODUCT_ID_PX4_V2;
-#else
-    return AP_PRODUCT_ID_PX4;
-#endif
 }
 
 /*
@@ -89,109 +113,33 @@ 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();
 
+    uint32_t now = hal.scheduler->micros();
 
     for (uint8_t k=0; k<_num_accel_instances; k++) {
-        _previous_accel[k] = _accel[k];
+        Vector3f accel = _accel_in[k];
-        _accel[k] = _accel_in[k];
+        _rotate_and_offset_accel(_accel_instance[k], accel, now);
-        _accel[k].rotate(_board_orientation);
+        _last_accel_update_timestamp[k] = _last_accel_timestamp[k];
-        _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];
     }
 
     for (uint8_t k=0; k<_num_gyro_instances; k++) {
-        _gyro[k] = _gyro_in[k];
+        Vector3f gyro = _gyro_in[k];
-        _gyro[k].rotate(_board_orientation);
+        _rotate_and_offset_gyro(_gyro_instance[k], gyro, now);
-        _gyro[k] -= _gyro_offset[k];
+        _last_gyro_update_timestamp[k] = _last_gyro_timestamp[k];
     }
 
-    if (_last_filter_hz != _mpu6000_filter) {
+    if (_last_filter_hz != _imu.get_filter()) {
-        _set_filter_frequency(_mpu6000_filter);
+        _set_filter_frequency(_imu.get_filter());
-        _last_filter_hz = _mpu6000_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++) {
@@ -215,59 +163,26 @@ void AP_InertialSensor_PX4::_get_sample(void)
     _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 = hal.scheduler->micros64();
+    _get_sample();
-    while (tnow - _last_sample_timestamp > _sample_time_usec) {
+    for (uint8_t i=0; i<_num_gyro_instances; i++) {
-        _have_sample_available = true;
+        if (_last_gyro_timestamp[i] != _last_gyro_update_timestamp[i]) {
-        _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;
-    }
-    uint64_t start = hal.scheduler->millis64();
-    while ((hal.scheduler->millis64() - start) < timeout_ms) {
-        uint64_t tnow = hal.scheduler->micros64();
-        // 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()) {
             return true;
         }
-    }
+    }    
     return false;
 }
 
-/**
+bool AP_InertialSensor_PX4::accel_sample_available(void)
-   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;
-}
-
-uint8_t AP_InertialSensor_PX4::get_primary_accel(void) const 
 {
+    _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

libraries/AP_InertialSensor/AP_InertialSensor_PX4.h

@@ -13,47 +13,35 @@
 #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_PX4(AP_InertialSensor &imu);
-        AP_InertialSensor(),
-        _last_get_sample_timestamp(0),
-        _sample_time_usec(0)
-        {
-        }
 
-    /* Concrete implementation of AP_InertialSensor functions: */
+    /* update accel and gyro state */
-    bool            update();
+    bool update();
-    float        	get_delta_time() const;
-    float           get_gyro_drift_rate();
-    bool            wait_for_sample(uint16_t timeout_ms);
-    bool            healthy(void) const;
 
-    // multi-device interface
+    // detect the sensor
-    bool get_gyro_health(uint8_t instance) const;
+    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
-    uint8_t get_gyro_count(void) const;
+                                             AP_InertialSensor::Sample_rate sample_rate);
 
-    bool get_accel_health(uint8_t instance) const;
+    bool gyro_sample_available(void);
-    uint8_t get_accel_count(void) const;
+    bool accel_sample_available(void);
-
-    uint8_t get_primary_accel(void) const;
 
 private:
-    uint8_t _get_primary_gyro(void) const;
+    bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
-
-    uint16_t _init_sensor( Sample_rate sample_rate );
     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 +52,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 // __AP_INERTIAL_SENSOR_PX4_H__