AP_InertialSensor: changed read of sensor from MPU6000 to happen immediately

This reduces the delay between when data arrives and when it is used
by up to 1ms.

Added num_samples_available method to all InertialSensors to allow
main loop timing to be synced with sensors.

libraries/AP_ADC/AP_ADC_ADS7844.cpp

@@ -67,7 +67,9 @@ static volatile uint32_t _sum[8];
 // how many values we've accumulated since last read
 static volatile uint16_t _count[8];
 
-static uint32_t last_ch6_micros;
+// variables to calculate time period over which a group of samples were collected
+static volatile uint32_t _ch6_delta_time_start_micros = 0;  // time we start collecting sample (reset on update)
+static volatile uint32_t _ch6_last_sample_time_micros = 0;  // time latest sample was collected
 
 // TCNT2 values for various interrupt rates,
 // assuming 256 prescaler. Note that these values
@@ -122,6 +124,8 @@ void AP_ADC_ADS7844::read(uint32_t tnow)
 
     bit_set(PORTC, 4);                                          // Disable Chip Select (PIN PC4)
 
+    // record time of this sample
+    _ch6_last_sample_time_micros = micros();
 }
 
 
@@ -158,7 +162,7 @@ void AP_ADC_ADS7844::Init( AP_PeriodicProcess * scheduler )
         _sum[i]   = adc_tmp;
     }
 
-    last_ch6_micros = micros();
+    _ch6_last_sample_time_micros = micros();
 
     scheduler->resume_timer();
     scheduler->register_process( AP_ADC_ADS7844::read );
@@ -223,6 +227,12 @@ uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, float *result)
         _count[channel_numbers[i]] = 0;
         _sum[channel_numbers[i]]   = 0;
     }
+
+    // calculate the delta time.
+    // we do this before re-enabling interrupts because another sensor read could fire immediately and change the _last_sensor_time value
+    uint32_t ret = _ch6_last_sample_time_micros - _ch6_delta_time_start_micros;
+    _ch6_delta_time_start_micros = _ch6_last_sample_time_micros;
+
     sei();
 
     // calculate averages. We keep this out of the cli region
@@ -234,8 +244,5 @@ uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, float *result)
     }
 
     // return number of microseconds since last call
-    uint32_t us = micros();
-    uint32_t ret = us - last_ch6_micros;
-    last_ch6_micros = us;
     return ret;
 }

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -53,16 +53,18 @@ public:
     /* Temperature, in degrees celsius, of the gyro. */
     virtual float           temperature() = 0;
 
-    /* sample_time returns the delta in microseconds since the
+    /* sample_time returns the time period in microseconds
-     * last call to reset_sample_time.
+     * overwhich the sensor data was collected
      */
     virtual uint32_t        sample_time() = 0;
-    virtual void            reset_sample_time() = 0;
 
     // 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;
 
+    // get number of samples read from the sensors
+    virtual uint16_t     num_samples_available() = 0;
+
 };
 
 #include "AP_InertialSensor_Oilpan.h"

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -190,7 +190,10 @@ const int8_t AP_InertialSensor_MPU6000::_accel_data_sign[3]  = { 1, 1, -1 };
 
 const uint8_t AP_InertialSensor_MPU6000::_temp_data_index = 3;
 
-static volatile uint8_t _new_data;
+// variables to calculate time period over which a group of samples were collected
+static volatile uint32_t _delta_time_micros = 1;   // time period overwhich samples were collected (initialise to non-zero number but will be overwritten on 2nd read in any case)
+static volatile uint32_t _delta_time_start_micros = 0;  // time we start collecting sample (reset on update)
+static volatile uint32_t _last_sample_time_micros = 0;  // time latest sample was collected
 
 static uint8_t _product_id;
 
@@ -200,6 +203,7 @@ uint8_t AP_InertialSensor_MPU6000::_received_packet[DMP_FIFO_BUFFER_SIZE];    //
 uint8_t AP_InertialSensor_MPU6000::_fifoCountH;                 // high byte of number of elements in fifo buffer
 uint8_t AP_InertialSensor_MPU6000::_fifoCountL;                 // low byte of number of elements in fifo buffer
 Quaternion AP_InertialSensor_MPU6000::quaternion;                       // holds the 4 quaternions representing attitude taken directly from the DMP
+AP_PeriodicProcess*  AP_InertialSensor_MPU6000::_scheduler = NULL;
 
 AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000( uint8_t cs_pin )
 {
@@ -219,10 +223,10 @@ uint16_t AP_InertialSensor_MPU6000::init( AP_PeriodicProcess * scheduler )
 {
     if (_initialised) return _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();
     scheduler->resume_timer();
-    scheduler->register_process( &AP_InertialSensor_MPU6000::read );
     return _product_id;
 }
 
@@ -253,6 +257,10 @@ bool AP_InertialSensor_MPU6000::update( void )
     }
     count = _count;
     _count = 0;
+
+    // record sample time
+    _delta_time_micros = _last_sample_time_micros - _delta_time_start_micros;
+    _delta_time_start_micros = _last_sample_time_micros;
     sei();
 
     count_scale = 1.0 / count;
@@ -325,15 +333,7 @@ float AP_InertialSensor_MPU6000::temperature() {
 
 uint32_t AP_InertialSensor_MPU6000::sample_time()
 {
-    uint32_t us = micros();
+    return _delta_time_micros;
-    uint32_t delta = us - _last_sample_micros;
-    reset_sample_time();
-    return delta;
-}
-
-void AP_InertialSensor_MPU6000::reset_sample_time()
-{
-    _last_sample_micros = micros();
 }
 
 /*================ HARDWARE FUNCTIONS ==================== */
@@ -347,16 +347,16 @@ static int16_t spi_transfer_16(void)
 }
 
 /*
- *  this is called from a timer interrupt to read data from the MPU6000
+ *  this is called from the data_interrupt which fires when the MPU6000 has new sensor data available
  *  and add it to _sum[]
+ *  Note: it is critical that no other devices on the same SPI bus attempt to read at the same time
+ *        to ensure this is the case, these other devices must perform their SPI reads after being
+ *        called by the AP_TimerProcess.
  */
-void AP_InertialSensor_MPU6000::read(uint32_t )
+void AP_InertialSensor_MPU6000::read()
 {
-    if (_new_data == 0) {
+    // record time that data was available
-        // no new data is ready from the MPU6000
+    _last_sample_time_micros = micros();
-        return;
-    }
-    _new_data = 0;
 
     // now read the data
     digitalWrite(_cs_pin, LOW);
@@ -365,6 +365,7 @@ void AP_InertialSensor_MPU6000::read(uint32_t )
     for (uint8_t i=0; i<7; i++) {
         _sum[i] += spi_transfer_16();
     }
+    digitalWrite(_cs_pin, HIGH);
 
     _count++;
     if (_count == 0) {
@@ -372,8 +373,6 @@ void AP_InertialSensor_MPU6000::read(uint32_t )
         memset((void*)_sum, 0, sizeof(_sum));
     }
 
-    digitalWrite(_cs_pin, HIGH);
-
     // should also read FIFO data if enabled
     if( _dmp_initialised ) {
         if( FIFO_ready() ) {
@@ -408,8 +407,17 @@ void AP_InertialSensor_MPU6000::register_write(uint8_t reg, uint8_t val)
 // MPU6000 new data interrupt on INT6
 void AP_InertialSensor_MPU6000::data_interrupt(void)
 {
-    // tell the timer routine that there is data to be read
+    // stop the timer firing, to prevent SPI bus accesses by other drivers
-    _new_data = 1;
+    _scheduler->suspend_timer();
+
+    // re-enable interrupts
+    sei();
+
+    // read in samples from the MPU6000's data registers
+    read();
+
+    // resume the timer
+    _scheduler->resume_timer();
 }
 
 void AP_InertialSensor_MPU6000::hardware_init()
@@ -477,6 +485,18 @@ float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void)
     return ToRad(0.5/60);
 }
 
+// get number of samples read from the sensors
+uint16_t AP_InertialSensor_MPU6000::num_samples_available()
+{
+    return _count;
+}
+
+// get time (in microseconds) that last sample was captured
+uint32_t AP_InertialSensor_MPU6000::last_sample_time()
+{
+    return _last_sample_time_micros;
+}
+
 // Update gyro offsets with new values.  Offsets provided in as scaled deg/sec values
 void AP_InertialSensor_MPU6000::set_gyro_offsets_scaled(float offX, float offY, float offZ)
 {
@@ -662,7 +682,6 @@ void AP_InertialSensor_MPU6000::FIFO_reset()
     temp = register_read(MPUREG_USER_CTRL);
     temp = temp | BIT_USER_CTRL_FIFO_RESET;             // FIFO RESET BIT
     register_write(MPUREG_USER_CTRL, temp);
-    _new_data = 0;                                                              // clear new data flag
 }
 
 // FIFO_getPacket - read an attitude packet from FIFO buffer

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -39,7 +39,6 @@ public:
     void                get_sensors( float * );
     float               temperature();
     uint32_t            sample_time();
-    void                reset_sample_time();
     float               get_gyro_drift_rate();
 
     // set_gyro_offsets - updates gyro offsets in mpu6000 registers
@@ -50,9 +49,15 @@ public:
     static void         set_accel_offsets_scaled(float offX, float offY, float offZ);
     static void         set_accel_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ);
 
+    // get number of samples read from the sensors
+    uint16_t     num_samples_available();
+
+    // get time (in microseconds) that last sample was captured
+    uint32_t     last_sample_time();
+
 private:
 
-    static void                 read(uint32_t);
+    static void                 read();
     static void                 data_interrupt(void);
     static uint8_t              register_read( uint8_t reg );
     static void                 register_write( uint8_t reg, uint8_t val );
@@ -62,8 +67,6 @@ private:
     Vector3f                    _accel;
     float                       _temp;
 
-    uint32_t                    _last_sample_micros;
-
     float                       _temp_to_celsius( uint16_t );
 
     static const float          _accel_scale;
@@ -77,6 +80,8 @@ private:
 
     static const uint8_t        _temp_data_index;
 
+    static AP_PeriodicProcess*  _scheduler;             // pointer to scheduler so that we can suspend/resume scheduler when we pull data from the MPU6000
+
     /* TODO deprecate _cs_pin */
     static uint8_t              _cs_pin;
 

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp

@@ -167,8 +167,6 @@ float AP_InertialSensor_Oilpan::temperature() {
 uint32_t AP_InertialSensor_Oilpan::sample_time() {
     return _sample_time;
 }
-void AP_InertialSensor_Oilpan::reset_sample_time() {
-}
 
 /* ------ Private functions -------------------------------------------*/
 
@@ -190,3 +188,9 @@ float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void)
     // 3.0 degrees/second/minute
     return ToRad(3.0/60);
 }
+
+// get number of samples read from the sensors
+uint16_t AP_InertialSensor_Oilpan::num_samples_available()
+{
+    return _adc->num_samples_available(_sensors);
+}

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h

@@ -31,9 +31,11 @@ public:
     void            get_sensors( float * );
     float           temperature();
     uint32_t        sample_time();
-    void            reset_sample_time();
     float           get_gyro_drift_rate();
 
+    // get number of samples read from the sensors
+    uint16_t        num_samples_available();
+
     static const struct AP_Param::GroupInfo        var_info[];
 
     AP_Int16                    _x_high;