Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: BMM150: reduce lock region

Browse Source 
Use a semaphore to update the accumulated values instead of using the
timer semaphore.
This commit is contained in:
Lucas De Marchi 2016-07-19 17:31:21 -03:00
parent c32dc3bc37
commit 667fca60f5
2 changed files with 18 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Compass/AP_Compass_BMM150.cpp
View File

@ -127,11 +127,15 @@ bool AP_Compass_BMM150::init()
uint8_t val = 0; uint8_t val = 0;
bool ret; bool ret;
hal.scheduler->suspend_timer_procs(); _accum_sem = hal.util->new_semaphore();
if (!_accum_sem) {
hal.console->printf("BMM150: Unable to create semaphore\n");
return false;
}
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.console->printf("BMM150: Unable to get bus semaphore\n"); hal.console->printf("BMM150: Unable to get bus semaphore\n");
goto fail_sem; return false;
} }
/* Do a soft reset */ /* Do a soft reset */
@ -184,13 +188,14 @@ bool AP_Compass_BMM150::init()
} }
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
hal.scheduler->resume_timer_procs();
/* register the compass instance in the frontend */ /* register the compass instance in the frontend */
_compass_instance = register_compass(); _compass_instance = register_compass();
set_dev_id(_compass_instance, AP_COMPASS_TYPE_BMM150); set_dev_id(_compass_instance, AP_COMPASS_TYPE_BMM150);
hal.scheduler->suspend_timer_procs();
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, void)); hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, void));
hal.scheduler->resume_timer_procs();
return true; return true;
@ -198,8 +203,6 @@ bus_error:
hal.console->printf("BMM150: Bus communication error\n"); hal.console->printf("BMM150: Bus communication error\n");
fail: fail:
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
fail_sem:
hal.scheduler->resume_timer_procs();
return false; return false;
} }
@ -283,28 +286,34 @@ void AP_Compass_BMM150::_update()
/* correct raw_field for known errors */ /* correct raw_field for known errors */
correct_field(raw_field, _compass_instance); correct_field(raw_field, _compass_instance);
if (!_accum_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return;
}
_mag_accum += raw_field; _mag_accum += raw_field;
_accum_count++; _accum_count++;
if (_accum_count == 10) { if (_accum_count == 10) {
_mag_accum /= 2; _mag_accum /= 2;
_accum_count = 5; _accum_count = 5;
} }
_last_update_timestamp = time_usec; _last_update_timestamp = time_usec;
_accum_sem->give();
} }
void AP_Compass_BMM150::read() void AP_Compass_BMM150::read()
{ {
if (!_accum_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return;
}
if (_accum_count == 0) { if (_accum_count == 0) {
_accum_sem->give();
return; return;
} }
hal.scheduler->suspend_timer_procs();
Vector3f field(_mag_accum); Vector3f field(_mag_accum);
field /= _accum_count; field /= _accum_count;
_mag_accum.zero(); _mag_accum.zero();
_accum_count = 0; _accum_count = 0;
hal.scheduler->resume_timer_procs(); _accum_sem->give();
publish_filtered_field(field, _compass_instance); publish_filtered_field(field, _compass_instance);
} }

1
libraries/AP_Compass/AP_Compass_BMM150.h
View File

@ -46,6 +46,7 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> _dev; AP_HAL::OwnPtr<AP_HAL::Device> _dev;
AP_HAL::Semaphore *_accum_sem;
Vector3f _mag_accum = Vector3f(); Vector3f _mag_accum = Vector3f();
uint32_t _accum_count = 0; uint32_t _accum_count = 0;
uint32_t _last_update_timestamp = 0; uint32_t _last_update_timestamp = 0;