Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-20 14:54:09 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: use thread per bus in more drivers

Browse Source 
and removed use of timer suspend
This commit is contained in:
Andrew Tridgell 2016-11-04 11:27:36 +11:00
parent 974827aa82
commit 9afd51350e
6 changed files with 57 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -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);
}

7
libraries/AP_Compass/AP_Compass_AK8963.h
View File

@ -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();

8
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -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;
}

32
libraries/AP_Compass/AP_Compass_LSM9DS1.cpp
View File

@ -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();
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

19
libraries/AP_Compass/AP_Compass_LSM9DS1.h
View File

@ -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;
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<AP_HAL::Device> _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;
};

2
libraries/AP_Compass/AP_Compass_qflight.cpp
View File

@ -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();
}
}