From 9afd51350e96f909fbcd1afb9d94974a3fa1c037 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Nov 2016 11:27:36 +1100 Subject: [PATCH] AP_Compass: use thread per bus in more drivers and removed use of timer suspend --- libraries/AP_Compass/AP_Compass_AK8963.cpp | 57 +++++++++++++-------- libraries/AP_Compass/AP_Compass_AK8963.h | 7 ++- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 8 +-- libraries/AP_Compass/AP_Compass_LSM9DS1.cpp | 32 ++---------- libraries/AP_Compass/AP_Compass_LSM9DS1.h | 19 ++++--- libraries/AP_Compass/AP_Compass_qflight.cpp | 2 - 6 files changed, 57 insertions(+), 68 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 3f2bccd322..5851e6da72 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/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 *) ®s, 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); +} + diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index 38ee06fba4..390665ed3a 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/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 _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(); diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 12a11f5d37..c1bb93f8c0 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/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; } diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp index 34117dbf6d..c1ff0c0324 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp +++ b/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 *) ®s, 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(); -end: - return; + return true; } 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 diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.h b/libraries/AP_Compass/AP_Compass_LSM9DS1.h index 459c7e2055..eb5ac4a0ec 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.h +++ b/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 _dev = nullptr; - uint32_t _dev_id = 0; - uint8_t _compass_instance = 0; - float _scaling = 0.0f; - float _mag_x_accum = 0.0f; - float _mag_y_accum = 0.0f; - float _mag_z_accum = 0.0f; - uint32_t _accum_count = 0; - uint32_t _last_update_timestamp; + AP_HAL::OwnPtr _dev; + uint32_t _dev_id; + uint8_t _compass_instance; + float _scaling; + float _mag_x_accum; + float _mag_y_accum; + float _mag_z_accum; + uint32_t _accum_count; }; diff --git a/libraries/AP_Compass/AP_Compass_qflight.cpp b/libraries/AP_Compass/AP_Compass_qflight.cpp index 20b02772a9..cab2d99f14 100644 --- a/libraries/AP_Compass/AP_Compass_qflight.cpp +++ b/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(); } }