AP_Compass: use thread per bus in more drivers

and removed use of timer suspend

libraries/AP_Compass/AP_Compass_AK8963.cpp

@@ -115,7 +115,6 @@ AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass, uint8_t m
 
 bool AP_Compass_AK8963::init()
 {
-    hal.scheduler->suspend_timer_procs();
     AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
 
     if (!bus_sem || !_bus->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
@@ -155,17 +154,18 @@ bool AP_Compass_AK8963::init()
     set_dev_id(_compass_instance, _dev_id);
 
     /* timer needs to be called every 10ms so set the freq_div to 10 */
-    _timesliced = hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void), 10);
+    if (!_bus->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, bool))) {
+        // fallback to timer
+        _timesliced = hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update_timer, void), 10);
+    }
 
     bus_sem->give();
-    hal.scheduler->resume_timer_procs();
 
     return true;
 
 fail:
     bus_sem->give();
 fail_sem:
-    hal.scheduler->resume_timer_procs();
 
     return false;
 }
@@ -181,11 +181,9 @@ void AP_Compass_AK8963::read()
         return;
     }
 
-    hal.scheduler->suspend_timer_procs();
     auto field = _get_filtered_field();
 
     _reset_filter();
-    hal.scheduler->resume_timer_procs();
     publish_filtered_field(field, _compass_instance);
 }
 
@@ -217,21 +215,12 @@ void AP_Compass_AK8963::_make_factory_sensitivity_adjustment(Vector3f& field) co
     field.z *= _magnetometer_ASA[2];
 }
 
-void AP_Compass_AK8963::_update()
+bool AP_Compass_AK8963::_update()
 {
     struct sample_regs regs;
     Vector3f raw_field;
     uint32_t time_us = AP_HAL::micros();
 
-    if (!_timesliced &&
-        AP_HAL::micros() - _last_update_timestamp < 10000) {
-        goto end;
-    }
-
-    if (!_bus->get_semaphore()->take_nonblocking()) {
-        goto end;
-    }
-
     if (!_bus->block_read(AK8963_HXL, (uint8_t *) &regs, sizeof(regs))) {
         goto fail;
     }
@@ -272,12 +261,30 @@ void AP_Compass_AK8963::_update()
         _accum_count = 5;
     }
 
-    _last_update_timestamp = AP_HAL::micros();
-
 fail:
+    return true;
+}
+
+/*
+  update from timer callback
+ */
+void AP_Compass_AK8963::_update_timer()
+{
+    uint32_t now = AP_HAL::micros();
+
+    if (!_timesliced && now - _last_update_timestamp < 10000) {
+        return;
+    }
+
+    if (!_bus->get_semaphore()->take_nonblocking()) {
+        return;
+    }
+
+    _update();
+    
+    _last_update_timestamp = now;
+    
     _bus->get_semaphore()->give();
-end:
-    return;
 }
 
 bool AP_Compass_AK8963::_check_id()
@@ -348,6 +355,11 @@ AP_HAL::Semaphore *AP_AK8963_BusDriver_HALDevice::get_semaphore()
     return _dev->get_semaphore();
 }
 
+AP_HAL::Device::PeriodicHandle AP_AK8963_BusDriver_HALDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
+{
+    return _dev->register_periodic_callback(period_usec, cb);
+}
+
 /* AK8963 on an auxiliary bus of IMU driver */
 AP_AK8963_BusDriver_Auxiliary::AP_AK8963_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id,
                                                              uint8_t backend_instance, uint8_t addr)
@@ -424,3 +436,8 @@ bool AP_AK8963_BusDriver_Auxiliary::start_measurements()
     return true;
 }
 
+AP_HAL::Device::PeriodicHandle AP_AK8963_BusDriver_Auxiliary::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
+{
+    return _bus->register_periodic_callback(period_usec, cb);
+}
+

libraries/AP_Compass/AP_Compass_AK8963.h

@@ -49,7 +49,8 @@ private:
     bool _check_id();
     bool _calibrate();
 
-    void _update();
+    bool _update();
+    void _update_timer();
 
     AP_AK8963_BusDriver *_bus;
 
@@ -79,6 +80,7 @@ public:
 
     virtual bool configure() { return true; }
     virtual bool start_measurements() { return true; }
+    virtual AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t, AP_HAL::Device::PeriodicCb) = 0;
 };
 
 class AP_AK8963_BusDriver_HALDevice: public AP_AK8963_BusDriver
@@ -91,6 +93,7 @@ public:
     virtual bool register_write(uint8_t reg, uint8_t val) override;
 
     virtual AP_HAL::Semaphore  *get_semaphore() override;
+    AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
 
 private:
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
@@ -107,6 +110,8 @@ public:
     bool register_read(uint8_t reg, uint8_t *val) override;
     bool register_write(uint8_t reg, uint8_t val) override;
 
+    AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
+    
     AP_HAL::Semaphore  *get_semaphore() override;
 
     bool configure();

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -141,12 +141,11 @@ AP_Compass_Backend *AP_Compass_HMC5843::probe_mpu6000(Compass &compass)
 
 bool AP_Compass_HMC5843::init()
 {
-    hal.scheduler->suspend_timer_procs();
     AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
 
     if (!bus_sem || !bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
         hal.console->printf("HMC5843: Unable to get bus semaphore\n");
-        goto fail_sem;
+        return false;
     }
 
     if (!_bus->configure()) {
@@ -176,7 +175,6 @@ bool AP_Compass_HMC5843::init()
     _initialised = true;
 
     bus_sem->give();
-    hal.scheduler->resume_timer_procs();
 
     _accum_sem = hal.util->new_semaphore();    
 
@@ -198,10 +196,6 @@ bool AP_Compass_HMC5843::init()
 
 errout:
     bus_sem->give();
-
-fail_sem:
-    hal.scheduler->resume_timer_procs();
-
     return false;
 }
 

libraries/AP_Compass/AP_Compass_LSM9DS1.cpp

@@ -71,14 +71,11 @@ AP_Compass_Backend *AP_Compass_LSM9DS1::probe(Compass &compass,
 
 bool AP_Compass_LSM9DS1::init()
 {
-    _last_update_timestamp = AP_HAL::micros();
-
-    hal.scheduler->suspend_timer_procs();
     AP_HAL::Semaphore *bus_sem = _dev->get_semaphore();
 
     if (!bus_sem || !bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
         hal.console->printf("LSM9DS1: Unable to get bus semaphore\n");
-        goto fail_sem;
+        return false;
     }
 
     if (!_check_id()) {
@@ -99,19 +96,14 @@ bool AP_Compass_LSM9DS1::init()
     _compass_instance = register_compass();
     set_dev_id(_compass_instance, _dev_id);
 
-    hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_LSM9DS1::_update, void));
+    _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM9DS1::_update, bool));
 
-    hal.scheduler->resume_timer_procs();
     bus_sem->give();
 
     return true;
 
 errout:
     bus_sem->give();
-
-fail_sem:
-    hal.scheduler->resume_timer_procs();
-
     return false;
 }
 
@@ -128,20 +120,12 @@ void AP_Compass_LSM9DS1::_dump_registers()
     hal.console->println();
 }
 
-void AP_Compass_LSM9DS1::_update(void)
+bool AP_Compass_LSM9DS1::_update(void)
 {
     struct sample_regs regs;
     Vector3f raw_field;
     uint32_t time_us = AP_HAL::micros();
 
-    if (AP_HAL::micros() - _last_update_timestamp < 10000) {
-        goto end;
-    }
-
-    if (!_dev->get_semaphore()->take_nonblocking()) {
-        goto end;
-    }
-
     if (!_block_read(LSM9DS1M_STATUS_REG_M, (uint8_t *) &regs, sizeof(regs))) {
         goto fail;
     }
@@ -178,12 +162,8 @@ void AP_Compass_LSM9DS1::_update(void)
         _accum_count = 5;
     }
 
-    _last_update_timestamp = AP_HAL::micros();
-
 fail:
-    _dev->get_semaphore()->give();
+    return true;
-end:
-    return;
 }
 
 void AP_Compass_LSM9DS1::read()
@@ -193,13 +173,9 @@ void AP_Compass_LSM9DS1::read()
         return;
     }
 
-    hal.scheduler->suspend_timer_procs();
-
     auto field = _get_filtered_field();
     _reset_filter();
 
-    hal.scheduler->resume_timer_procs();
-
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2
     field.rotate(ROTATION_ROLL_180);
 #endif

libraries/AP_Compass/AP_Compass_LSM9DS1.h

@@ -26,7 +26,7 @@ private:
     bool _check_id(void);
     bool _configure(void);
     bool _set_scale(void);
-    void _update(void);
+    bool _update(void);
     void _reset_filter(void);
     Vector3f _get_filtered_field() const;
 
@@ -36,13 +36,12 @@ private:
     bool _block_read(uint8_t reg, uint8_t *buf, uint32_t size);
     void _dump_registers();
 
-    AP_HAL::OwnPtr<AP_HAL::Device> _dev = nullptr;
+    AP_HAL::OwnPtr<AP_HAL::Device> _dev;
-    uint32_t _dev_id = 0;
+    uint32_t _dev_id;
-    uint8_t _compass_instance = 0;
+    uint8_t _compass_instance;
-    float _scaling = 0.0f;
+    float _scaling;
-    float _mag_x_accum = 0.0f;
+    float _mag_x_accum;
-    float _mag_y_accum = 0.0f;
+    float _mag_y_accum;
-    float _mag_z_accum = 0.0f;
+    float _mag_z_accum;
-    uint32_t _accum_count = 0;
+    uint32_t _accum_count;
-    uint32_t _last_update_timestamp;
 };

libraries/AP_Compass/AP_Compass_qflight.cpp

@@ -56,11 +56,9 @@ bool AP_Compass_QFLIGHT::init(void)
 void AP_Compass_QFLIGHT::read(void)
 {
     if (count > 0) {
-        hal.scheduler->suspend_timer_procs();
         publish_filtered_field(sum/count, instance);
         sum.zero();
         count = 0;
-        hal.scheduler->resume_timer_procs();
     }
 }