AP_InertialSensor: use thread-per-bus for lsm303d and mpu6000

libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp

@@ -15,7 +15,6 @@
 
 #include <AP_HAL/AP_HAL.h>
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 #include "AP_InertialSensor_LSM9DS0.h"
 
 #include <utility>
@@ -497,7 +496,7 @@ bool AP_InertialSensor_LSM9DS0::_hardware_init()
     _set_accel_max_abs_offset(_accel_instance, 5.0f);
 
     /* start the timer process to read samples */
-    hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS0::_poll_data, void));
+    _dev_gyro->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS0::_poll_data, bool));
 
     return true;
 
@@ -664,35 +663,15 @@ void AP_InertialSensor_LSM9DS0::_set_accel_scale(accel_scale scale)
 /**
  * Timer process to poll for new data from the LSM9DS0.
  */
-void AP_InertialSensor_LSM9DS0::_poll_data()
+bool AP_InertialSensor_LSM9DS0::_poll_data()
 {
-    bool drdy_is_from_reg = _drdy_pin_num_a < 0 || _drdy_pin_num_g < 0;
-    bool gyro_ready;
-    bool accel_ready;
-
-    if (drdy_is_from_reg && !_spi_sem->take_nonblocking()) {
-        return;
+    if (_gyro_data_ready()) {
+        _read_data_transaction_g();
     }
-
-    gyro_ready = _gyro_data_ready();
-    accel_ready = _accel_data_ready();
-
-    if (gyro_ready || accel_ready) {
-        if (!drdy_is_from_reg && !_spi_sem->take_nonblocking()) {
-            return;
-        }
-
-        if (gyro_ready) {
-            _read_data_transaction_g();
-        }
-        if (accel_ready) {
-            _read_data_transaction_a();
-        }
-
-        _spi_sem->give();
-    } else if(drdy_is_from_reg) {
-        _spi_sem->give();
+    if (_accel_data_ready()) {
+        _read_data_transaction_a();
     }
+    return true;
 }
 
 bool AP_InertialSensor_LSM9DS0::_accel_data_ready()
@@ -797,4 +776,3 @@ void AP_InertialSensor_LSM9DS0::_dump_registers(void)
 }
 #endif
 
-#endif

libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h

@@ -47,7 +47,7 @@ private:
     bool _accel_data_ready();
     bool _gyro_data_ready();
 
-    void _poll_data();
+    bool _poll_data();
 
     bool _init_sensor();
     bool _hardware_init();

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -388,8 +388,7 @@ void AP_InertialSensor_MPU6000::start()
     hal.scheduler->resume_timer_procs();
 
     // start the timer process to read samples
-    hal.scheduler->register_timer_process(
-        FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, void));
+    _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, bool));
 }
 
 /*
@@ -434,21 +433,16 @@ bool AP_InertialSensor_MPU6000::_data_ready()
 }
 
 /*
- * Timer process to poll for new data from the MPU6000.
+ * Timer process to poll for new data from the MPU6000. Called from bus thread with semaphore held
  */
-void AP_InertialSensor_MPU6000::_poll_data()
+bool AP_InertialSensor_MPU6000::_poll_data()
 {
-    if (!_dev->get_semaphore()->take_nonblocking()) {
-        return;
-    }
-
     if (_use_fifo) {
         _read_fifo();
     } else if (_data_ready()) {
         _read_sample();
     }
-
-    _dev->get_semaphore()->give();
+    return true;
 }
 
 void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -77,7 +77,7 @@ private:
     bool _data_ready();
 
     /* Poll for new data (non-blocking) */
-    void _poll_data();
+    bool _poll_data();
 
     /* Read and write functions taking the differences between buses into
      * account */