AP_InertialSensor: Improvements to Flymaple sensors

Fix a bug that caused hang after 71 minutes. Use I2CDriver semaphore.
Remove test for in_accumulate: not needed.

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp

@@ -33,7 +33,6 @@ Vector3f AP_InertialSensor_Flymaple::_accel_filtered;
 uint32_t AP_InertialSensor_Flymaple::_accel_samples;
 Vector3f AP_InertialSensor_Flymaple::_gyro_filtered;
 uint32_t AP_InertialSensor_Flymaple::_gyro_samples;
-volatile bool AP_InertialSensor_Flymaple::_in_accumulate;
 uint64_t AP_InertialSensor_Flymaple::_last_accel_timestamp;
 uint64_t AP_InertialSensor_Flymaple::_last_gyro_timestamp;
 LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_x(800, 10);
@@ -98,6 +97,13 @@ uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
         break;
     }
 
+    // get pointer to i2c bus semaphore
+    AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
+
+    // take i2c bus sempahore
+    if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER))
+        return false;
+
     // Init the accelerometer
     uint8_t data;
     hal.i2c->readRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DEVID, &data);
@@ -142,6 +148,9 @@ uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
     // Set up the filter desired
     _set_filter_frequency(_mpu6000_filter);
 
+   // give back i2c semaphore
+    i2c_sem->give();
+
     return AP_PRODUCT_ID_FLYMAPLE;
 }
 
@@ -239,18 +248,19 @@ float AP_InertialSensor_Flymaple::get_gyro_drift_rate(void)
 // within the mainline and thropttle the reads here to suit the sensors
 void AP_InertialSensor_Flymaple::_accumulate(void)
 {
-    if (_in_accumulate) {
+    // get pointer to i2c bus semaphore
-        return; // Dont be re-entrant (how can this occur?)
+    AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
-    }
-    _in_accumulate = true;
 
+    // take i2c bus sempahore
+    if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER))
+        return;
 
     // Read accelerometer
     // ADXL345 is in the default FIFO bypass mode, so the FIFO is not used
     uint8_t buffer[6];
     uint64_t now = hal.scheduler->micros();
     // This takes about 500us at 200kHz I2C speed
-    if (now > (_last_accel_timestamp + raw_sample_interval_us) 
+    if ((now - _last_accel_timestamp) >= raw_sample_interval_us
         && hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0)
     {
         // The order is a bit wierd here since the standard we have adopted for Flymaple 
@@ -270,7 +280,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
     // Read gyro
     now = hal.scheduler->micros();
     // This takes about 500us at 200kHz I2C speed
-    if (now > (_last_gyro_timestamp + raw_sample_interval_us) 
+    if ((now - _last_gyro_timestamp) >= raw_sample_interval_us
         && hal.i2c->readRegisters(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_GYROX_H, 6, buffer) == 0)
     {
         // See above re order of samples in buffer
@@ -284,8 +294,8 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
         _last_gyro_timestamp = now;
     }
 
-    _in_accumulate = false;
+    // give back i2c semaphore
-
+    i2c_sem->give();
 }
 
 bool AP_InertialSensor_Flymaple::sample_available(void)

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h

@@ -33,7 +33,6 @@ private:
     static uint32_t _accel_samples;
     static Vector3f	_gyro_filtered;
     static uint32_t _gyro_samples;
-    static volatile bool _in_accumulate;
     static uint64_t _last_accel_timestamp;
     static uint64_t _last_gyro_timestamp;
     uint8_t  _sample_divider;