AP_InertialSensor: converted MPU9150 driver

untested conversion

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -278,6 +278,7 @@ private:
 #include "AP_InertialSensor_MPU9250.h"
 #include "AP_InertialSensor_L3G4200D.h"
 #include "AP_InertialSensor_Flymaple.h"
+#include "AP_InertialSensor_MPU9150.h"
 #include "AP_InertialSensor_HIL.h"
 #include "AP_InertialSensor_UserInteract_Stream.h"
 #include "AP_InertialSensor_UserInteract_MAVLink.h"

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,34 @@ static struct gyro_state_s st = {
 /**
  *  @brief      Constructor
  */
-AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150() :
+AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150(AP_InertialSensor &imu) :
-    AP_InertialSensor(),
+    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)    
 {
+}
 
+
+/*
+  detect the sensor
+ */
+AP_InertialSensor_Backend *AP_InertialSensor_MPU9150::detect(AP_InertialSensor &_imu, 
+                                                             AP_InertialSensor::Sample_rate sample_rate)
+{
+    AP_InertialSensor_MPU9150 *sensor = new AP_InertialSensor_MPU9150(_imu);
+    if (sensor == NULL) {
+        return NULL;
+    }
+    if (!sensor->_init_sensor(sample_rate)) {
+        delete sensor;
+        return NULL;
+    }
+    return sensor;
 }
 
 /*
@@ -356,29 +375,26 @@ void AP_InertialSensor_MPU9150::_set_filter_frequency(uint8_t filter_hz)
  *  @param[in] Sample_rate  The sample rate, check the struct def.
  *  @return     AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE if successful.
  */
-uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate ) 
+bool AP_InertialSensor_MPU9150::_init_sensor(AP_InertialSensor::Sample_rate sample_rate) 
 {
     // Sensors pushed to the FIFO.
     uint8_t sensors;
 
     switch (sample_rate) {
-    case RATE_50HZ:
+    case AP_InertialSensor::RATE_50HZ:
         _default_filter_hz = 10;
-        _sample_period_usec = (1000*1000) / 50;
         break;
-    case RATE_100HZ:
+    case AP_InertialSensor::RATE_100HZ:
         _default_filter_hz = 20;
-        _sample_period_usec = (1000*1000) / 100;
         break;
-    case RATE_200HZ:
+    case AP_InertialSensor::RATE_200HZ:
+        _default_filter_hz = 20;
+        break;
+    case AP_InertialSensor::RATE_400HZ:
         _default_filter_hz = 20;
-        _sample_period_usec = 5000;
         break;
-    case RATE_400HZ:
     default:
-        _default_filter_hz = 20;
+        return false;
-        _sample_period_usec = 2500;
-        break;
     }
 
     // get pointer to i2c bus semaphore
@@ -386,7 +402,7 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate )
 
     // take i2c bus sempahore
     if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)){
-        return -1;
+        return false;
     }        
 
     // Init the sensor
@@ -405,7 +421,7 @@ 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"));
+        hal.console->printf("AP_InertialSensor_MPU9150: couldn't read the registers to determine revision");
         goto failed;
     }    
     uint8_t rev;
@@ -432,28 +448,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 +483,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:
+    return true;
+
+failed:
     // give back i2c semaphore
     i2c_sem->give();
-        return -1;
+    return false;
 }
 
 /**
@@ -1017,9 +1038,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 +1115,37 @@ 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)) {
+    Vector3f accel, gyro;
-        return false;
+    uint32_t now = hal.scheduler->micros();
-    }
-    Vector3f accel_scale = _accel_scale[0].get();
-
-    _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);
-
-    // Adjust for chip scaling to get radians/sec
-    _gyro[0] *= MPU9150_GYRO_SCALE_2000 / _gyro_samples_available;
-    _gyro[0] -= _gyro_offset[0];
-    ////////////////////////////////////////////////
-
-    _gyro_samples_available = 0;
-
-    if (_last_filter_hz != _mpu6000_filter) {
-        _set_filter_frequency(_mpu6000_filter);
-        _last_filter_hz = _mpu6000_filter;
-    }
 
+    hal.scheduler->suspend_timer_procs();
+    accel = _accel_filtered;
+    gyro = _gyro_filtered;
     _have_sample_available = false;
+    hal.scheduler->resume_timer_procs();
+
+    accel *= MPU9150_ACCEL_SCALE_2G;
+    _rotate_and_offset_accel(_accel_instance, accel, now);
+
+    gyro *= MPU9150_GYRO_SCALE_2000;
+    _rotate_and_offset_gyro(_gyro_instance, gyro, now);
+
+    if (_last_filter_hz != _imu.get_filter()) {
+        _set_filter_frequency(_imu.get_filter());
+        _last_filter_hz = _imu.get_filter();
+    }
 
     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

libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h

@@ -12,28 +12,26 @@
 #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 */
-
-    /* Implementation of AP_InertialSensor functions: */
     bool update();
-    float        	get_delta_time() const;
+
-    float           get_gyro_drift_rate();
+    bool gyro_sample_available(void) { return _have_sample_available; }
-    bool            wait_for_sample(uint16_t timeout_ms);
+    bool accel_sample_available(void) { return _have_sample_available; }
+
+    // detect the sensor
+    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
+                                             AP_InertialSensor::Sample_rate sample_rate);
 
 private:
-    uint16_t        _init_sensor( Sample_rate sample_rate );
+    bool            _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
     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;
 
     // // support for updating filter at runtime
@@ -52,7 +50,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 +59,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__