AP_InertialSensor: allow specification of sample rate in init() call

this lets the caller not need to know the underlying sample rate. They
just ask for what rate updates happen. 

This also changes the MPU6k filtering to be less than half the sample
rate

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -25,11 +25,12 @@ AP_InertialSensor::AP_InertialSensor()
 
 void
 AP_InertialSensor::init( Start_style style,
+                         Sample_rate sample_rate,
                          void (*delay_cb)(unsigned long t),
                          void (*flash_leds_cb)(bool on),
                          AP_PeriodicProcess *  scheduler )
 {
-    _product_id = _init_sensor(scheduler);
+    _product_id = _init_sensor(scheduler, sample_rate);
 
     // check scaling
     Vector3f accel_scale = _accel_scale.get();

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -24,6 +24,13 @@ public:
         WARM_START
     };
 
+    // the rate that updates will be available to the application
+    enum Sample_rate {
+        RATE_50HZ,
+        RATE_100HZ,
+        RATE_200HZ
+    };
+
     /// Perform startup initialisation.
     ///
     /// Called to initialise the state of the IMU.
@@ -38,6 +45,7 @@ public:
     /// @param style	The initialisation startup style.
     ///
     virtual void        init( Start_style           style,
+                              Sample_rate           sample_rate,
                               void                  (*delay_cb)(unsigned long t),
                               void                  (*flash_leds_cb)(bool on),
                               AP_PeriodicProcess *  scheduler );
@@ -135,7 +143,7 @@ public:
 protected:
 
     // sensor specific init to be overwritten by descendant classes
-    virtual uint16_t        _init_sensor( AP_PeriodicProcess * scheduler ) = 0;
+    virtual uint16_t        _init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate ) = 0;
 
     // no-save implementations of accel and gyro initialisation routines
     virtual void            _init_accel(void    (*delay_cb)(unsigned long t),

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -210,13 +210,13 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000()
     _dmp_initialised = false;
 }
 
-uint16_t AP_InertialSensor_MPU6000::_init_sensor( AP_PeriodicProcess * scheduler )
+uint16_t AP_InertialSensor_MPU6000::_init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate )
 {
     if (_initialised) return _mpu6000_product_id;
     _initialised = true;
     _scheduler = scheduler;     // store pointer to scheduler so that we can suspend/resume scheduler when we pull data from the MPU6000
     scheduler->suspend_timer();
-    hardware_init();
+    hardware_init(sample_rate);
     scheduler->resume_timer();
     return _mpu6000_product_id;
 }
@@ -381,7 +381,7 @@ void AP_InertialSensor_MPU6000::data_interrupt(void)
     _scheduler->queue_process( AP_InertialSensor_MPU6000::read );
 }
 
-void AP_InertialSensor_MPU6000::hardware_init()
+void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
 {
     // MPU6000 chip select setup
     pinMode(MPU6000_CS_PIN, OUTPUT);
@@ -401,12 +401,36 @@ void AP_InertialSensor_MPU6000::hardware_init()
     // Disable I2C bus (recommended on datasheet)
     register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
     delay(1);
-    // SAMPLE RATE
-    register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
+
+    uint8_t rate, 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:
+        rate = MPUREG_SMPLRT_50HZ;
+        filter = BITS_DLPF_CFG_20HZ;
+        break;
+    case RATE_100HZ:
+        rate = MPUREG_SMPLRT_100HZ;
+        filter = BITS_DLPF_CFG_42HZ;
+        break;
+    case RATE_200HZ:
+    default:
+        rate = MPUREG_SMPLRT_200HZ;
+        filter = BITS_DLPF_CFG_98HZ;
+        break;
+    }
+
+    // set sample rate
+    register_write(MPUREG_SMPLRT_DIV, rate);
     delay(1);
-    // FS & DLPF   FS=2000º/s, DLPF = 98Hz (low pass filter)
-    register_write(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
+
+    // set low pass filter
+    register_write(MPUREG_CONFIG, filter);
     delay(1);
+
     register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS);  // Gyro scale 2000º/s
     delay(1);
 

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -52,7 +52,7 @@ public:
     uint32_t            get_delta_time_micros();
 
 protected:
-    uint16_t                    _init_sensor( AP_PeriodicProcess * scheduler );
+    uint16_t                    _init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate );
 
 private:
 
@@ -60,7 +60,7 @@ private:
     static void                 data_interrupt(void);
     static uint8_t              register_read( uint8_t reg );
     static void                 register_write( uint8_t reg, uint8_t val );
-    static void                 hardware_init();
+    static void                 hardware_init(Sample_rate sample_rate);
 
     float                       _temp;
 

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp

@@ -41,10 +41,22 @@ AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) :
 {
 }
 
-uint16_t AP_InertialSensor_Oilpan::_init_sensor( AP_PeriodicProcess * scheduler)
+uint16_t AP_InertialSensor_Oilpan::_init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate)
 {
     _adc->Init(scheduler);
 
+    switch (sample_rate) {
+    case RATE_50HZ:
+        _sample_threshold = 20;
+        break;
+    case RATE_100HZ:
+        _sample_threshold = 10;
+        break;
+    case RATE_200HZ:
+        _sample_threshold = 5;
+        break;
+    }
+
 #if defined(DESKTOP_BUILD)
     return AP_PRODUCT_ID_SITL;
 #elif defined(__AVR_ATmega1280__)
@@ -130,5 +142,8 @@ float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void)
 // get number of samples read from the sensors
 uint16_t AP_InertialSensor_Oilpan::num_samples_available()
 {
-    return _adc->num_samples_available(_sensors);
+    if (_adc->num_samples_available(_sensors) >= _sample_threshold) {
+        return 1;
+    }
+    return 0;
 }

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h

@@ -34,7 +34,7 @@ public:
     uint16_t        num_samples_available();
 
 protected:
-    uint16_t        _init_sensor(AP_PeriodicProcess * scheduler);
+    uint16_t        _init_sensor(AP_PeriodicProcess * scheduler, Sample_rate sample_rate);
 
 private:
 
@@ -55,6 +55,8 @@ private:
     static const float          _gyro_gain_z;
 
     static const float          _adc_constraint;
+
+    uint8_t                     _sample_threshold;
 };
 
 #endif // __AP_INERTIAL_SENSOR_OILPAN_H__

libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp

@@ -2,7 +2,7 @@
 
 #include "AP_InertialSensor_Stub.h"
 
-uint16_t AP_InertialSensor_Stub::_init_sensor( AP_PeriodicProcess * scheduler ) {
+uint16_t AP_InertialSensor_Stub::_init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate ) {
     return AP_PRODUCT_ID_NONE;
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_Stub.h

@@ -32,7 +32,7 @@ public:
     uint16_t        num_samples_available();
 
 protected:
-    uint16_t        _init_sensor( AP_PeriodicProcess * scheduler );
+    uint16_t        _init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate );
 };
 
 #endif // __AP_INERTIAL_SENSOR_STUB_H__

libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde

@@ -55,7 +55,9 @@ void setup(void)
 #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
     adc.Init(&scheduler);           // APM ADC library initialization
 #endif
-    ins.init(AP_InertialSensor::COLD_START, delay, NULL, &scheduler);
+    ins.init(AP_InertialSensor::COLD_START, 
+			 AP_InertialSensor::RATE_100HZ,
+			 delay, NULL, &scheduler);
 
     // display initial values
     display_offsets_and_scaling();