aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: cleanup use of backend semaphores

Browse Source 
fixed drivers that didn't protect accumulation counters
This commit is contained in:
Andrew Tridgell 2016-11-04 11:47:43 +11:00
parent 9afd51350e
commit 1c631ea037
12 changed files with 72 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -58,7 +58,6 @@ AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus,
, _bus(bus)
, _dev_id(dev_id)
{
_reset_filter();
}
AP_Compass_AK8963::~AP_Compass_AK8963()
@ -119,7 +118,7 @@ bool AP_Compass_AK8963::init()
if (!bus_sem || !_bus->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.console->printf("AK8963: Unable to get bus semaphore\n");
goto fail_sem;
return false;
}
if (!_bus->configure()) {
@ -153,20 +152,18 @@ bool AP_Compass_AK8963::init()
_compass_instance = register_compass();
set_dev_id(_compass_instance, _dev_id);
bus_sem->give();
/* timer needs to be called every 10ms so set the freq_div to 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();
return true;
fail:
bus_sem->give();
fail_sem:
return false;
}
@ -176,29 +173,19 @@ void AP_Compass_AK8963::read()
return;
}
if (_accum_count == 0) {
/* We're not ready to publish*/
return;
if (_sem->take_nonblocking()) {
if (_accum_count == 0) {
/* We're not ready to publish */
_sem->give();
return;
}
Vector3f field = Vector3f(_mag_x_accum, _mag_y_accum, _mag_z_accum) / _accum_count;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_count = 0;
_sem->give();
publish_filtered_field(field, _compass_instance);
}
auto field = _get_filtered_field();
_reset_filter();
publish_filtered_field(field, _compass_instance);
}
Vector3f AP_Compass_AK8963::_get_filtered_field() const
{
Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum);
field /= _accum_count;
return field;
}
void AP_Compass_AK8963::_reset_filter()
{
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_count = 0;
}
void AP_Compass_AK8963::_make_adc_sensitivity_adjustment(Vector3f& field) const
@ -250,15 +237,18 @@ bool AP_Compass_AK8963::_update()
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 10) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
if (_sem->take(0)) {
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 10) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count = 5;
}
_sem->give();
}
fail:

2
libraries/AP_Compass/AP_Compass_AK8963.h
View File

@ -41,9 +41,7 @@ private:
void _make_factory_sensitivity_adjustment(Vector3f &field) const;
void _make_adc_sensitivity_adjustment(Vector3f &field) const;
Vector3f _get_filtered_field() const;
void _reset_filter();
bool _reset();
bool _setup_mode();
bool _check_id();

24
libraries/AP_Compass/AP_Compass_BMM150.cpp
View File

@ -18,8 +18,6 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include <utility>
#include <AP_HAL/utility/sparse-endian.h>
@ -126,12 +124,6 @@ bool AP_Compass_BMM150::init()
uint8_t val = 0;
bool ret;
_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)) {
hal.console->printf("BMM150: Unable to get bus semaphore\n");
return false;
@ -244,10 +236,6 @@ bool AP_Compass_BMM150::_update()
{
const uint32_t time_usec = AP_HAL::micros();
if (time_usec - _last_update_timestamp < MEASURE_TIME_USEC) {
return true;
}
le16_t data[4];
bool ret = _dev->read_registers(DATA_X_LSB_REG, (uint8_t *) &data, sizeof(data));
@ -279,7 +267,7 @@ bool AP_Compass_BMM150::_update()
/* correct raw_field for known errors */
correct_field(raw_field, _compass_instance);
if (!_accum_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return true;
}
_mag_accum += raw_field;
@ -288,18 +276,17 @@ bool AP_Compass_BMM150::_update()
_mag_accum /= 2;
_accum_count = 5;
}
_last_update_timestamp = time_usec;
_accum_sem->give();
_sem->give();
return true;
}
void AP_Compass_BMM150::read()
{
if (!_accum_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (!_sem->take_nonblocking()) {
return;
}
if (_accum_count == 0) {
_accum_sem->give();
_sem->give();
return;
}
@ -307,9 +294,8 @@ void AP_Compass_BMM150::read()
field /= _accum_count;
_mag_accum.zero();
_accum_count = 0;
_accum_sem->give();
_sem->give();
publish_filtered_field(field, _compass_instance);
}
#endif

6
libraries/AP_Compass/AP_Compass_BMM150.h
View File

@ -48,10 +48,8 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
AP_HAL::Semaphore *_accum_sem;
Vector3f _mag_accum = Vector3f();
uint32_t _accum_count = 0;
uint32_t _last_update_timestamp = 0;
Vector3f _mag_accum;
uint32_t _accum_count;
uint8_t _compass_instance;

4
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -7,7 +7,9 @@ extern const AP_HAL::HAL& hal;
AP_Compass_Backend::AP_Compass_Backend(Compass &compass) :
_compass(compass)
{}
{
_sem = hal.util->new_semaphore();
}
void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
{

3
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -77,6 +77,9 @@ protected:
// access to frontend
Compass &_compass;
// semaphore for access to shared frontend data
AP_HAL::Semaphore *_sem;
private:
void apply_corrections(Vector3f &mag, uint8_t i);
};

12
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -176,8 +176,6 @@ bool AP_Compass_HMC5843::init()
bus_sem->give();
_accum_sem = hal.util->new_semaphore();
// perform an initial read
read();
@ -237,7 +235,7 @@ bool AP_Compass_HMC5843::_timer()
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
if (_accum_sem->take(0)) {
if (_sem->take(0)) {
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
@ -249,7 +247,7 @@ bool AP_Compass_HMC5843::_timer()
_accum_count = 7;
}
_last_accum_time = tnow;
_accum_sem->give();
_sem->give();
}
return true;
@ -270,12 +268,12 @@ void AP_Compass_HMC5843::read()
return;
}
if (!_accum_sem->take_nonblocking()) {
if (!_sem->take_nonblocking()) {
return;
}
if (_accum_count == 0) {
_accum_sem->give();
_sem->give();
return;
}
@ -287,7 +285,7 @@ void AP_Compass_HMC5843::read()
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_sem->give();
_sem->give();
publish_filtered_field(field, _compass_instance);
}

1
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -66,7 +66,6 @@ private:
bool _initialised;
bool _force_external;
AP_HAL::Semaphore *_accum_sem;
};
class AP_HMC5843_BusDriver

12
libraries/AP_Compass/AP_Compass_LSM303D.cpp
View File

@ -275,7 +275,7 @@ bool AP_Compass_LSM303D::init()
set_external(_compass_instance, false);
#endif
_accum_sem = hal.util->new_semaphore();
_sem = hal.util->new_semaphore();
// read at 100Hz
_dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, bool));
@ -354,7 +354,7 @@ bool AP_Compass_LSM303D::_update()
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
if (_accum_sem->take(0)) {
if (_sem->take(0)) {
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
@ -365,7 +365,7 @@ bool AP_Compass_LSM303D::_update()
_mag_z_accum /= 2;
_accum_count = 5;
}
_accum_sem->give();
_sem->give();
}
return true;
}
@ -377,13 +377,13 @@ void AP_Compass_LSM303D::read()
return;
}
if (!_accum_sem->take_nonblocking()) {
if (!_sem->take_nonblocking()) {
return;
}
if (_accum_count == 0) {
/* We're not ready to publish*/
_accum_sem->give();
_sem->give();
return;
}
@ -393,7 +393,7 @@ void AP_Compass_LSM303D::read()
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_sem->give();
_sem->give();
publish_filtered_field(field, _compass_instance);
}

1
libraries/AP_Compass/AP_Compass_LSM303D.h
View File

@ -56,5 +56,4 @@ private:
uint8_t _mag_range_ga;
uint8_t _mag_samplerate;
uint8_t _reg7_expected;
AP_HAL::Semaphore *_accum_sem;
};

50
libraries/AP_Compass/AP_Compass_LSM9DS1.cpp
View File

@ -151,15 +151,18 @@ bool AP_Compass_LSM9DS1::_update(void)
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 10) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count = 5;
if (_sem->take(0)) {
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 10) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count = 5;
}
_sem->give();
}
fail:
@ -168,37 +171,29 @@ fail:
void AP_Compass_LSM9DS1::read()
{
if (!_sem->take_nonblocking()) {
return;
}
if (_accum_count == 0) {
/* We're not ready to publish*/
_sem->give();
return;
}
auto field = _get_filtered_field();
_reset_filter();
Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum);
field /= _accum_count;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_count = 0;
_sem->give();
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2
field.rotate(ROTATION_ROLL_180);
#endif
publish_filtered_field(field, _compass_instance);
}
void AP_Compass_LSM9DS1::_reset_filter()
{
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_count = 0;
}
Vector3f AP_Compass_LSM9DS1::_get_filtered_field() const
{
Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum);
field /= _accum_count;
return field;
}
bool AP_Compass_LSM9DS1::_check_id(void)
{
// initially run the bus at low speed
@ -240,7 +235,6 @@ AP_Compass_LSM9DS1::AP_Compass_LSM9DS1(Compass &compass, AP_HAL::OwnPtr<AP_HAL::
, _dev(std::move(dev))
, _dev_id(dev_id)
{
_reset_filter();
}
uint8_t AP_Compass_LSM9DS1::_register_read(uint8_t reg)

2
libraries/AP_Compass/AP_Compass_LSM9DS1.h
View File

@ -27,8 +27,6 @@ private:
bool _configure(void);
bool _set_scale(void);
bool _update(void);
void _reset_filter(void);
Vector3f _get_filtered_field() const;
uint8_t _register_read(uint8_t reg);
void _register_write(uint8_t reg, uint8_t val);