From dcdb53584ba637142e817af15c2e3b032833a121 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 21 Aug 2014 07:48:44 +1000
Subject: [PATCH] AP_InertialSensor: use lockless structures in MPU9250 driver

this avoids suspending timers when transferring data between main
thread and SPI read thread
---
 .../AP_InertialSensor_MPU9250.cpp             | 32 +++++++++++--------
 .../AP_InertialSensor_MPU9250.h               | 12 +++++--
 2 files changed, 28 insertions(+), 16 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
index c962395fc..7e9e733b5 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
@@ -177,6 +177,7 @@ AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250() :
     _initialised(false),
     _mpu9250_product_id(AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE),
     _last_filter_hz(-1),
+    _shared_data_idx(0),
     _accel_filter_x(1000, 15),
     _accel_filter_y(1000, 15),
     _accel_filter_z(1000, 15),
@@ -197,6 +198,8 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
     _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
     _spi_sem = _spi->get_semaphore();
 
+    // we need to suspend timers to prevent other SPI drivers grabbing
+    // the bus while we do the long initialisation
     hal.scheduler->suspend_timer_procs();
 
     uint8_t whoami = _register_read(MPUREG_WHOAMI);
@@ -232,7 +235,6 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
 
     hal.scheduler->resume_timer_procs();
 
-
     /* read the first lot of data.
      * _read_data_transaction requires the spi semaphore to be taken by
      * its caller. */
@@ -298,11 +300,10 @@ bool AP_InertialSensor_MPU9250::update( void )
 
     _previous_accel[0] = _accel[0];
 
-    // disable timer procs for mininum time
-    hal.scheduler->suspend_timer_procs();
-    _gyro[0]  = _gyro_filtered;
-    _accel[0] = _accel_filtered;
-    hal.scheduler->resume_timer_procs();
+    // pull the data from the timer shared data buffer
+    uint8_t idx = _shared_data_idx;
+    _gyro[0]  = _shared_data[idx]._gyro_filtered;
+    _accel[0] = _shared_data[idx]._accel_filtered;
 
     _gyro[0].rotate(_board_orientation);
     _gyro[0] *= GYRO_SCALE;
@@ -374,13 +375,18 @@ void AP_InertialSensor_MPU9250::_read_data_transaction()
 
 #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
 
-    _accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)), 
-                               _accel_filter_y.apply(int16_val(rx.v, 0)), 
-                               _accel_filter_z.apply(-int16_val(rx.v, 2)));
-
-    _gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)), 
-                              _gyro_filter_y.apply(int16_val(rx.v, 4)), 
-                              _gyro_filter_z.apply(-int16_val(rx.v, 6)));
+    Vector3f _accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)), 
+                                        _accel_filter_y.apply(int16_val(rx.v, 0)), 
+                                        _accel_filter_z.apply(-int16_val(rx.v, 2)));
+
+    Vector3f _gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)), 
+                                       _gyro_filter_y.apply(int16_val(rx.v, 4)), 
+                                       _gyro_filter_z.apply(-int16_val(rx.v, 6)));
+    // update the shared buffer
+    uint8_t idx = _shared_data_idx ^ 1;
+    _shared_data[idx]._accel_filtered = _accel_filtered;
+    _shared_data[idx]._gyro_filtered = _gyro_filtered;
+    _shared_data_idx = idx;
 }
 
 /*
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
index 82dd7b3e2..27812ba73 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
@@ -57,9 +57,15 @@ private:
     // change the filter frequency
     void _set_filter(uint8_t filter_hz);
 
-    // output of accel/gyro filters
-    Vector3f _accel_filtered;
-    Vector3f _gyro_filtered;
+    // This structure is used to pass data from the timer which reads
+    // the sensor to the main thread. The _shared_data_idx is used to
+    // prevent race conditions by ensuring the data is fully updated
+    // before being used by the consumer
+    struct {
+        Vector3f _accel_filtered;
+        Vector3f _gyro_filtered;
+    } _shared_data[2];
+    volatile uint8_t _shared_data_idx;
 
     // Low Pass filters for gyro and accel 
     LowPassFilter2p _accel_filter_x;
-- 
GitLab