AP_InertialSensor: first steps in frontend/backend split

This converts the MPU6000 driver to a frontend/backend structure, and
disables all other drivers. They will be progressively re-enabled as
each is converted

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -219,18 +219,52 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
 AP_InertialSensor::AP_InertialSensor() :
     _accel(),
     _gyro(),
-    _board_orientation(ROTATION_NONE)
+    _board_orientation(ROTATION_NONE),
+    _gyro_count(0),
+    _accel_count(0)
 {
     AP_Param::setup_object_defaults(this, var_info);        
+    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+        _backends[i] = NULL;
+    }
+}
+
+
+/*
+  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);
+    if (_gyro_count == 0 && _accel_count == 0) {
+        // detect available backends. Only called once
+        _detect_backends(sample_rate);
+    }
 
-    // check scaling
+    _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));
@@ -241,6 +275,37 @@ AP_InertialSensor::init( Start_style style,
         // do cold-start calibration for gyro only
         _init_gyro();
     }
+
+    switch (sample_rate) {
+    case RATE_50HZ:
+        _delta_time = 1.0f/50;
+        break;
+    case RATE_100HZ:
+        _delta_time = 1.0f/100;
+        break;
+    case RATE_200HZ:
+        _delta_time = 1.0f/200;
+        break;
+    case RATE_400HZ:
+    default:
+        _delta_time = 1.0f/400;
+        break;
+    }
+}
+
+
+/*
+  detect available backends for this board
+ */
+void 
+AP_InertialSensor::_detect_backends(Sample_rate sample_rate)
+{
+    _backends[0] = AP_InertialSensor_MPU6000::detect(*this, sample_rate, _gyro[_gyro_count], _accel[_accel_count]);
+    if (_backends[0] == NULL ||
+        _gyro_count == 0 ||
+        _accel_count == 0) {
+        hal.scheduler->panic(PSTR("No INS backends available"));
+    }
 }
 
 void
@@ -325,10 +390,7 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
         }
         uint8_t num_samples = 0;
         while (num_samples < 32) {
-            if (!wait_for_sample(1000)) {
+            wait_for_sample();
-                interact->printf_P(PSTR("Failed to get INS sample\n"));
-                goto failed;
-            }
             // read samples from ins
             update();
             // capture sample
@@ -829,3 +891,35 @@ void AP_InertialSensor::_save_parameters()
         _gyro_offset[i].save();
     }
 }
+
+
+/*
+  update gyro and accel values from backends
+ */
+void AP_InertialSensor::update(void)
+{
+    for (int8_t i=INS_MAX_INSTANCES-1; i>=0; i--) {
+        if (_backends[i] != NULL) {
+            _backends[i]->update();
+        }
+    }    
+}
+
+/*
+  wait for a sample to be available
+ */
+void AP_InertialSensor::wait_for_sample(void)
+{
+    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) {
+                gyro_available |= _backends[i]->gyro_sample_available();
+                accel_available |= _backends[i]->gyro_sample_available();
+            }
+        }
+        hal.scheduler->delay_microseconds(100);
+    }
+}

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -11,9 +11,7 @@
    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
+#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
-#define INS_MAX_INSTANCES 3
-#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
 #define INS_MAX_INSTANCES 3
 #else
 #define INS_MAX_INSTANCES 1
@@ -23,6 +21,9 @@
 #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 +33,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 +64,28 @@ public:
     ///
     /// @param style	The initialisation startup style.
     ///
-    virtual void init( Start_style style,
+    void init( Start_style style,
-                       Sample_rate sample_rate);
+               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,
+    bool calibrate_accel(AP_InertialSensor_UserInteract *interact,
-                                 float& trim_roll,
+                         float& trim_roll,
-                                 float& trim_pitch);
+                         float& trim_pitch);
 #endif
 
     /// calibrated - returns true if the accelerometers have been calibrated
@@ -93,65 +99,63 @@ 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()); }
+    const Vector3f     &get_gyro(void) const { return get_gyro(_primary_gyro); }
-    virtual void       set_gyro(uint8_t instance, const Vector3f &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()); }
+    const Vector3f     &get_accel(void) const { return get_accel(_primary_accel); }
-    virtual void       set_accel(uint8_t instance, const Vector3f &accel) {}
+    void               set_accel(uint8_t instance, const Vector3f &accel);
 
     // multi-device interface
-    virtual bool get_gyro_health(uint8_t instance) const { return true; }
+    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(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(uint8_t instance) const { return true; }
-    bool get_accel_health(void) const { return get_accel_health(get_primary_accel()); }
+    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()); }
+    const Vector3f &get_accel_scale(void) const { return get_accel_scale(_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;
 
     /* 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); }
 
-    // wait for a sample to be available, with timeout in milliseconds
+    // update gyro and accel values from accumulated samples
-    virtual bool wait_for_sample(uint16_t timeout_ms) = 0;
+    void update(void);
+
+    // 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 +173,21 @@ 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; }
+    uint16_t error_count(void) const { return 0; }
-    virtual bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
+    bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
 
-    virtual uint8_t get_primary_accel(void) const { return 0; }
+    uint8_t get_primary_accel(void) const { return 0; }
 
-protected:
+private:
 
-    virtual uint8_t _get_primary_gyro(void) const { return 0; }
+    // load backend drivers
+    void _detect_backends(Sample_rate sample_rate);
 
-    // sensor specific init to be overwritten by descendant classes
+    // accel and gyro initialisation
-    virtual uint16_t        _init_sensor( Sample_rate sample_rate ) = 0;
+    void _init_accel();
-
+    void _init_gyro();
-    // no-save implementations of accel and gyro initialisation routines
-    virtual void  _init_accel();
-
-    virtual void _init_gyro();
 
 #if !defined( __AVR_ATmega1280__ )
     // Calibration routines borrowed from Rolfe Schmidt
@@ -194,54 +195,63 @@ 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);
+    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]);
+    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]);
+    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]);
+    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);
+    void _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch);
 #endif
 
     // save parameters to eeprom
     void  _save_parameters();
 
-    // Most recent accelerometer reading obtained by ::update
+    // backend objects
+    AP_InertialSensor_Backend *_backends[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;
+
+    // Most recent accelerometer reading
     Vector3f _accel[INS_MAX_INSTANCES];
 
-    // previous accelerometer reading obtained by ::update
+    // Most recent gyro reading
-    Vector3f _previous_accel[INS_MAX_INSTANCES];
-
-    // Most recent gyro reading obtained by ::update
     Vector3f _gyro[INS_MAX_INSTANCES];
 
+    // timestamp of latest gyro and accel readings
+    uint32_t _last_gyro_sample_time_usec[INS_MAX_INSTANCES];
+    uint32_t _last_accel_sample_time_usec[INS_MAX_INSTANCES];
+
     // product id
     AP_Int16 _product_id;
 
     // accelerometer scaling and offsets
-    AP_Vector3f             _accel_scale[INS_MAX_INSTANCES];
+    AP_Vector3f _accel_scale[INS_MAX_INSTANCES];
-    AP_Vector3f             _accel_offset[INS_MAX_INSTANCES];
+    AP_Vector3f _accel_offset[INS_MAX_INSTANCES];
-    AP_Vector3f             _gyro_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;
+
+    // time between samples
+    float _delta_time;
 };
 
-#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_L3G4200D.h"
-#include "AP_InertialSensor_MPU9150.h"
-#include "AP_InertialSensor_MPU9250.h"
-#include "AP_InertialSensor_L3GD20.h"
 
 #endif // __AP_INERTIAL_SENSOR_H__

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -0,0 +1,36 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL.h>
+#include "AP_InertialSensor.h"
+#include "AP_InertialSensor_Backend.h"
+
+AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel) :
+    _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)
+{
+    _imu._gyro[instance].rotate(_imu._board_orientation);
+    _imu._gyro[instance] -= _imu._gyro_offset[instance];
+    _imu._last_gyro_sample_time_usec[instance] = now;
+}
+
+/*
+  rotate accel vector, scale and add the accel offset
+ */
+void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, uint32_t now)
+{
+    _imu._accel[instance].rotate(_imu._board_orientation);
+
+    const Vector3f &accel_scale = _imu._accel_scale[instance].get();
+    _imu._accel[instance].x *= accel_scale.x;
+    _imu._accel[instance].y *= accel_scale.y;
+    _imu._accel[instance].z *= accel_scale.z;
+    _imu._accel[instance] -= _imu._accel_offset[instance];
+    _imu._last_accel_sample_time_usec[instance] = now;
+}

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -0,0 +1,68 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  IMU driver backend class
+ */
+#ifndef __AP_INERTIALSENSOR_BACKEND_H__
+#define __AP_INERTIALSENSOR_BACKEND_H__
+
+class AP_InertialSensor_Backend
+{
+public:
+    AP_InertialSensor_Backend(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel);
+
+    // we declare a virtual destructor so that drivers can
+    // override with a custom destructor if need be.
+    virtual ~AP_InertialSensor_Backend(void) {}
+
+    /* 
+     * Update the sensor data. Called by the frontend to transfer
+     * accumulated sensor readings to the frontend structure
+     */
+    virtual bool update() = 0;
+
+    /* 
+     * return true if at least one accel sample is available in the backend
+     * since the last call to update()
+     */
+    virtual bool accel_sample_available() = 0;
+
+    /* 
+     * return true if at least one gyro sample is available in the backend
+     * since the last call to update()
+     */
+    virtual bool gyro_sample_available() = 0;
+
+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);
+
+    // rotate accel vector, scale and offset
+    void _rotate_and_offset_accel(uint8_t instance, uint32_t now);
+
+    // note that each backend is also expected to have a detect()
+    // function which instantiates an instance of the backend sensor
+    // driver. 
+};
+
+#endif // __AP_INERTIALSENSOR_BACKEND_H__

libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp

@@ -1,5 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#if NOT_YET
 #include "AP_InertialSensor_HIL.h"
 #include <AP_HAL.h>
 const extern AP_HAL::HAL& hal;
@@ -128,3 +129,4 @@ uint8_t AP_InertialSensor_HIL::get_accel_count(void) const
     return 1;
 }
 
+#endif

libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.cpp

@@ -36,6 +36,8 @@
  ****************************************************************************/
 
 #include <AP_HAL.h>
+#if NOT_YET
+
 #include "AP_InertialSensor_L3GD20.h"
 
 extern const AP_HAL::HAL& hal;
@@ -630,3 +632,4 @@ float AP_InertialSensor_L3GD20::get_delta_time() const
     // the sensor runs at 200Hz
     return 0.005 * _num_samples;
 }
+#endif

libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.cpp

@@ -1,5 +1,7 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#if NOT_YET
+
 /****************************************************************************
  *
  *	 Coded by Víctor Mayoral Vilches <v.mayoralv@gmail.com> using 
@@ -826,3 +828,4 @@ float AP_InertialSensor_LSM303D::get_delta_time() const
     // the sensor runs at 200Hz
     return 0.005 * _num_samples;
 }
+#endif

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -173,26 +173,47 @@ const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532f / 16.4f);
  *  variants however
  */
 
-AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000() : 
+AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, 
-	AP_InertialSensor(),
+                                                     Vector3f &gyro, 
+                                                     Vector3f &accel) : 
+    AP_InertialSensor_Backend(imu, gyro, accel),
     _drdy_pin(NULL),
     _spi(NULL),
     _spi_sem(NULL),
-    _num_samples(0),
+    _sample_count(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),
+    _sum_count(0)
 {
+    _accel_sum.zero();
+    _gyro_sum.zero();
 }
 
-uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate )
+/*
+  detect the sensor
+ */
+AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect(AP_InertialSensor &_imu, 
+                                                             AP_InertialSensor::Sample_rate sample_rate,
+                                                             Vector3f &gyro, 
+                                                             Vector3f &accel)
 {
-    if (_initialised) return _mpu6000_product_id;
+    AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu, gyro, accel);
-    _initialised = true;
+    if (sensor == NULL) {
+        return NULL;
+    }
+    if (!sensor->_init_sensor(sample_rate)) {
+        delete sensor;
+        return NULL;
+    }
 
+    return sensor;
+}
+
+/*
+  initialise the sensor
+ */
+bool AP_InertialSensor_MPU6000::_init_sensor(AP_InertialSensor::Sample_rate sample_rate)
+{
     _spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
     _spi_sem = _spi->get_semaphore();
 
@@ -209,99 +230,73 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate )
         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(
+                return false;
-                        PSTR("MPU6000 startup failed: no data ready"));
             }
             _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;
+
+    return true;
 }
 
-/*================ 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;
-}
 
+/*
+  process any 
+ */
 bool AP_InertialSensor_MPU6000::update( void )
-{
+{    
-    // wait for at least 1 sample
+    if (_sum_count < _sample_count) {
-    if (!wait_for_sample(1000)) {
+        // we don't have enough samples yet
         return false;
     }
 
-    _previous_accel[0] = _accel[0];
+    // we have a full set of samples
+    uint16_t num_samples;
+    uint32_t now = hal.scheduler->micros();
 
-    // disable timer procs for mininum time
     hal.scheduler->suspend_timer_procs();
-    _gyro[0]  = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
+    _gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
-    _accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z);
+    _accel(_accel_sum.x, _accel_sum.y, _accel_sum.z);
-    _num_samples = _sum_count;
+    num_samples = _sum_count;
     _accel_sum.zero();
     _gyro_sum.zero();
     _sum_count = 0;
     hal.scheduler->resume_timer_procs();
 
-    _gyro[0].rotate(_board_orientation);
+    _gyro *= _gyro_scale / num_samples;
-    _gyro[0] *= _gyro_scale / _num_samples;
+    _rotate_and_offset_gyro(_gyro_instance, now);
-    _gyro[0] -= _gyro_offset[0];
 
-    _accel[0].rotate(_board_orientation);
+    _accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
-    _accel[0] *= MPU6000_ACCEL_SCALE_1G / _num_samples;
+    _rotate_and_offset_accel(_accel_instance, now);
 
-    Vector3f accel_scale = _accel_scale[0].get();
+    if (_last_filter_hz != _imu.get_filter()) {
-    _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);
+            _set_filter_register(_imu.get_filter(), 0);
             _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
-            _error_count = 0;
             _spi_sem->give();
         }
     }
@@ -331,35 +326,13 @@ bool AP_InertialSensor_MPU6000::_data_ready()
  */
 void AP_InertialSensor_MPU6000::_poll_data(void)
 {
-    if (hal.scheduler->in_timerprocess()) {
+    if (!_spi_sem->take_nonblocking()) {
-        if (!_spi_sem->take_nonblocking()) {
+        return;
-            /*
+    }   
-              the semaphore being busy is an expected condition when the
+    if (_data_ready()) {
-              mainline code is calling wait_for_sample() which will
+        _read_data_transaction(); 
-              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"));
-        }
     }
+    _spi_sem->give();
 }
 
 
@@ -477,7 +450,7 @@ void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz, uint8_t
 }
 
 
-bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate)
+bool AP_InertialSensor_MPU6000::_hardware_init(AP_InertialSensor::Sample_rate sample_rate)
 {
     if (!_spi_sem->take(100)) {
         hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
@@ -525,25 +498,25 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate)
     // 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:
+    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:
+    case AP_InertialSensor::RATE_100HZ:
         default_filter = BITS_DLPF_CFG_20HZ;
-        _sample_shift = 1;
+        _sample_count = 2;
         break;
-    case RATE_200HZ:
+    case AP_InertialSensor::RATE_200HZ:
     default:
         default_filter = BITS_DLPF_CFG_20HZ;
-        _sample_shift = 0;
+        _sample_count = 1;
         break;
     }
 
-    _set_filter_register(_mpu6000_filter, default_filter);
+    _set_filter_register(_imu.get_filter(), default_filter);
 
     // set sample rate to 200Hz, and use _sample_divider to give
     // the requested rate to the application
@@ -554,11 +527,13 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate)
     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);
+    uint8_t 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) ||
+    if ((product_id == MPU6000ES_REV_C4) || 
-        (_mpu6000_product_id == MPU6000_REV_C4)   || (_mpu6000_product_id == MPU6000_REV_C5)) {
+        (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 +560,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 +578,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;
-}

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -12,33 +12,34 @@
 // enable debug to see a register dump on startup
 #define MPU6000_DEBUG 0
 
-class AP_InertialSensor_MPU6000 : public AP_InertialSensor
+class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
 {
 public:
+    AP_InertialSensor_MPU6000(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel);
 
-    AP_InertialSensor_MPU6000();
+    /* update accel and gyro state */
+    bool update();
 
-    /* Concrete implementation of AP_InertialSensor functions: */
+    bool gyro_sample_available(void) { return _sum_count >= _sample_count; }
-    bool                update();
+    bool accel_sample_available(void) { return _sum_count >= _sample_count; }
-    float               get_gyro_drift_rate();
-
-    // wait for a sample to be available, with timeout in milliseconds
-    bool                wait_for_sample(uint16_t timeout_ms);
-
-    // 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,
+                                             AP_InertialSensor::Sample_rate sample_rate,
+                                             Vector3f &gyro,
+                                             Vector3f &accel);
 private:
+#if MPU6000_DEBUG
+    void _dump_registers(void);
+#endif
+
+    // instance numbers of accel and gyro data
+    uint8_t _gyro_instance;
+    uint8_t _accel_instance;
+
     AP_HAL::DigitalSource *_drdy_pin;
 
+    bool                 _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
     bool                 _sample_available();
     void                 _read_data_transaction();
     bool                 _data_ready();
@@ -46,41 +47,29 @@ 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(AP_InertialSensor::Sample_rate sample_rate);
 
     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;
+    uint8_t _sample_count;
 
     // support for updating filter at runtime
     uint8_t _last_filter_hz;
 
     void _set_filter_register(uint8_t filter_hz, uint8_t default_filter);
 
+    // count of bus errors
     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;
+    volatile uint16_t _sum_count;
-
-public:
-
-#if MPU6000_DEBUG
-    void						_dump_registers(void);
-#endif
 };
 
 #endif // __AP_INERTIAL_SENSOR_MPU6000_H__