AP_InertialSensor: fixed threading and locking in remaining backends

use a backend semaphore for shared data and use thread per bus where
available

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -596,7 +596,7 @@ AP_InertialSensor::detect_backends(void)
                                                       hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
                                                       hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_ROLL_180));
     } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_TEST_V4) {
-        _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
+        _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
     }
     // also add any PX4 backends (eg. canbus sensors)
     _add_backend(AP_InertialSensor_PX4::detect(*this));

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -9,7 +9,9 @@ const extern AP_HAL::HAL& hal;
 AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
     _imu(imu),
     _product_id(AP_PRODUCT_ID_NONE)
-{}
+{
+    _sem = hal.util->new_semaphore();
+}
 
 void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vector3f &accel) 
 {
@@ -103,13 +105,15 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
     _imu._last_delta_angle[instance] = delta_angle;
     _imu._last_raw_gyro[instance] = gyro;
 
-    _imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
-    if (_imu._gyro_filtered[instance].is_nan() || _imu._gyro_filtered[instance].is_inf()) {
-        _imu._gyro_filter[instance].reset();
+    if (_sem->take(0)) {
+        _imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
+        if (_imu._gyro_filtered[instance].is_nan() || _imu._gyro_filtered[instance].is_inf()) {
+            _imu._gyro_filter[instance].reset();
+        }
+        _imu._new_gyro_data[instance] = true;
+        _sem->give();
     }
 
-    _imu._new_gyro_data[instance] = true;
-
     DataFlash_Class *dataflash = get_dataflash();
     if (dataflash != nullptr) {
         uint64_t now = AP_HAL::micros64();
@@ -184,14 +188,17 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
     _imu._delta_velocity_acc[instance] += accel * dt;
     _imu._delta_velocity_acc_dt[instance] += dt;
 
-    _imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel);
-    if (_imu._accel_filtered[instance].is_nan() || _imu._accel_filtered[instance].is_inf()) {
-        _imu._accel_filter[instance].reset();
-    }
+    if (_sem->take(0)) {
+        _imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel);
+        if (_imu._accel_filtered[instance].is_nan() || _imu._accel_filtered[instance].is_inf()) {
+            _imu._accel_filter[instance].reset();
+        }
 
-    _imu.set_accel_peak_hold(instance, _imu._accel_filtered[instance]);
+        _imu.set_accel_peak_hold(instance, _imu._accel_filtered[instance]);
 
-    _imu._new_accel_data[instance] = true;
+        _imu._new_accel_data[instance] = true;
+        _sem->give();
+    }
 
     DataFlash_Class *dataflash = get_dataflash();
     if (dataflash != nullptr) {
@@ -251,7 +258,9 @@ void AP_InertialSensor_Backend::_publish_temperature(uint8_t instance, float tem
  */
 void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
 {    
-    hal.scheduler->suspend_timer_procs();
+    if (!_sem->take_nonblocking()) {
+        return;
+    }
 
     if (_imu._new_gyro_data[instance]) {
         _publish_gyro(instance, _imu._gyro_filtered[instance]);
@@ -264,7 +273,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
         _last_gyro_filter_hz[instance] = _gyro_filter_cutoff();
     }
 
-    hal.scheduler->resume_timer_procs();
+    _sem->give();
 }
 
 /*
@@ -272,7 +281,9 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
  */
 void AP_InertialSensor_Backend::update_accel(uint8_t instance)
 {    
-    hal.scheduler->suspend_timer_procs();
+    if (!_sem->take_nonblocking()) {
+        return;
+    }
 
     if (_imu._new_accel_data[instance]) {
         _publish_accel(instance, _imu._accel_filtered[instance]);
@@ -285,5 +296,5 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance)
         _last_accel_filter_hz[instance] = _accel_filter_cutoff();
     }
 
-    hal.scheduler->resume_timer_procs();
+    _sem->give();
 }

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -75,6 +75,9 @@ protected:
     // access to frontend
     AP_InertialSensor &_imu;
 
+    // semaphore for access to shared frontend data
+    AP_HAL::Semaphore *_sem;
+
     void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel);
     void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro);
 

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp

@@ -194,16 +194,23 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
                          L3G4200D_REG_FIFO_CTL_STREAM);
     hal.scheduler->delay(1);
 
+    _product_id = AP_PRODUCT_ID_L3G4200D;
+    
     _dev->get_semaphore()->give();
 
+    return true;
+}
+
+/*
+  startup the sensor
+ */
+void AP_InertialSensor_L3G4200D::start(void)
+{
     _gyro_instance = _imu.register_gyro(800);
     _accel_instance = _imu.register_accel(800);
-    _product_id = AP_PRODUCT_ID_L3G4200D;
 
     // start the timer process to read samples
-    hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, void));
-
-    return true;
+    _dev->register_periodic_callback(1250, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, bool));
 }
 
 /*
@@ -218,12 +225,8 @@ bool AP_InertialSensor_L3G4200D::update(void)
 }
 
 // Accumulate values from accels and gyros
-void AP_InertialSensor_L3G4200D::_accumulate(void)
+bool AP_InertialSensor_L3G4200D::_accumulate(void)
 {
-    if (!_dev->get_semaphore()->take_nonblocking()) {
-        return;
-    }
-
     uint8_t num_samples_available;
     uint8_t fifo_status = 0;
 
@@ -276,9 +279,6 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
             }
         }
     }
-
-    // give back i2c semaphore
-    _dev->get_semaphore()->give();
 }
 
 #endif

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h

@@ -22,14 +22,16 @@ public:
                                             AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
 
     /* update accel and gyro state */
-    bool update();
+    bool update() override;
 
     // return product ID
-    int16_t product_id() const { return AP_PRODUCT_ID_L3G4200D; }
+    int16_t product_id() const override { return AP_PRODUCT_ID_L3G4200D; }
+
+    void start(void) override;
 
 private:
     bool _init_sensor();
-    void _accumulate();
+    bool _accumulate();
 
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
 

libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp

@@ -436,9 +436,7 @@ bool AP_InertialSensor_LSM9DS0::_init_sensor()
         _drdy_pin_g->mode(HAL_GPIO_INPUT);
     }
 
-    hal.scheduler->suspend_timer_procs();
     bool success = _hardware_init();
-    hal.scheduler->resume_timer_procs();
 
 #if LSM9DS0_DEBUG
     _dump_registers();

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -288,9 +288,7 @@ bool AP_InertialSensor_MPU6000::_init()
     _drdy_pin->mode(HAL_GPIO_INPUT);
 #endif
 
-    hal.scheduler->suspend_timer_procs();
     bool success = _hardware_init();
-    hal.scheduler->resume_timer_procs();
 
 #if MPU6000_DEBUG
     _dump_registers();
@@ -321,8 +319,6 @@ bool AP_InertialSensor_MPU6000::_has_auxiliary_bus()
 
 void AP_InertialSensor_MPU6000::start()
 {
-    hal.scheduler->suspend_timer_procs();
-
     if (!_dev->get_semaphore()->take(100)) {
         AP_HAL::panic("MPU6000: Unable to get semaphore");
     }
@@ -393,8 +389,6 @@ void AP_InertialSensor_MPU6000::start()
     set_gyro_orientation(_gyro_instance, _rotation);
     set_accel_orientation(_accel_instance, _rotation);
     
-    hal.scheduler->resume_timer_procs();
-
     // start the timer process to read samples
     if (_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, bool)) == nullptr) {
         // cope with AuxiliaryBus which does not support register_periodic_callback yet

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp

@@ -14,13 +14,12 @@
 */
 #include <AP_HAL/AP_HAL.h>
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 #include "AP_InertialSensor_MPU9250.h"
 
 #include <assert.h>
 #include <utility>
 
-#include <AP_HAL_Linux/GPIO.h>
+#include <AP_HAL/GPIO.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -264,9 +263,7 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i
 
 bool AP_InertialSensor_MPU9250::_init()
 {
-    hal.scheduler->suspend_timer_procs();
     bool success = _hardware_init();
-    hal.scheduler->resume_timer_procs();
 
 #if MPU9250_DEBUG
     _dump_registers();
@@ -282,8 +279,6 @@ bool AP_InertialSensor_MPU9250::_has_auxiliary_bus()
 
 void AP_InertialSensor_MPU9250::start()
 {
-    hal.scheduler->suspend_timer_procs();
-
     if (!_dev->get_semaphore()->take(100)) {
         AP_HAL::panic("MPU92500: Unable to get semaphore");
     }
@@ -330,11 +325,8 @@ void AP_InertialSensor_MPU9250::start()
     _gyro_instance = _imu.register_gyro(DEFAULT_SAMPLE_RATE);
     _accel_instance = _imu.register_accel(DEFAULT_SAMPLE_RATE);
 
-    hal.scheduler->resume_timer_procs();
-
     // start the timer process to read samples
-    hal.scheduler->register_timer_process(
-        FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_poll_data, void));
+    _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_read_sample, bool));
 }
 
 /*
@@ -375,20 +367,6 @@ bool AP_InertialSensor_MPU9250::_data_ready(uint8_t int_status)
     return (int_status & BIT_RAW_RDY_INT) != 0;
 }
 
-/*
- * Timer process to poll for new data from the MPU9250.
- */
-void AP_InertialSensor_MPU9250::_poll_data()
-{
-    if (!_dev->get_semaphore()->take_nonblocking()) {
-        return;
-    }
-
-    _read_sample();
-
-    _dev->get_semaphore()->give();
-}
-
 void AP_InertialSensor_MPU9250::_accumulate(uint8_t *rx)
 {
     Vector3f accel, gyro;
@@ -414,7 +392,7 @@ void AP_InertialSensor_MPU9250::_accumulate(uint8_t *rx)
 /*
  * read from the data registers and update filtered data
  */
-void AP_InertialSensor_MPU9250::_read_sample()
+bool AP_InertialSensor_MPU9250::_read_sample()
 {
     /* one register address followed by seven 2-byte registers */
     struct PACKED {
@@ -424,14 +402,15 @@ void AP_InertialSensor_MPU9250::_read_sample()
 
     if (!_block_read(MPUREG_INT_STATUS, (uint8_t *) &rx, sizeof(rx))) {
         hal.console->printf("MPU9250: error reading sample\n");
-        return;
+        return true;
     }
 
     if (!_data_ready(rx.int_status)) {
-        return;
+        return true;
     }
 
     _accumulate(rx.d);
+    return true;
 }
 
 bool AP_InertialSensor_MPU9250::_block_read(uint8_t reg, uint8_t *buf,
@@ -700,4 +679,4 @@ int AP_MPU9250_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slave,
 
     return 0;
 }
-#endif
+

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h

@@ -62,15 +62,12 @@ private:
     bool _has_auxiliary_bus();
 
     /* Read a single sample */
-    void _read_sample();
+    bool _read_sample();
 
     /* Check if there's data available by reading register */
     bool _data_ready();
     bool _data_ready(uint8_t int_status);
 
-    /* Poll for new data (non-blocking) */
-    void _poll_data();
-
     /* Read and write functions taking the differences between buses into
      * account */
     bool _block_read(uint8_t reg, uint8_t *buf, uint32_t size);