zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Compass: cleanup use of backend semaphores 

fixed drivers that didn't protect accumulation counters
mission-4.1.18
Andrew Tridgell 8 years ago
parent
9afd51350e
commit
1c631ea037
12 changed files with 72 additions and 107 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 62
      libraries/AP_Compass/AP_Compass_AK8963.cpp
  2. 2
      libraries/AP_Compass/AP_Compass_AK8963.h
  3. 24
      libraries/AP_Compass/AP_Compass_BMM150.cpp
  4. 6
      libraries/AP_Compass/AP_Compass_BMM150.h
  5. 4
      libraries/AP_Compass/AP_Compass_Backend.cpp
  6. 3
      libraries/AP_Compass/AP_Compass_Backend.h
  7. 12
      libraries/AP_Compass/AP_Compass_HMC5843.cpp
  8. 1
      libraries/AP_Compass/AP_Compass_HMC5843.h
  9. 12
      libraries/AP_Compass/AP_Compass_LSM303D.cpp
  10. 1
      libraries/AP_Compass/AP_Compass_LSM303D.h
  11. 50
      libraries/AP_Compass/AP_Compass_LSM9DS1.cpp
  12. 2
      libraries/AP_Compass/AP_Compass_LSM9DS1.h

62
libraries/AP_Compass/AP_Compass_AK8963.cpp
Unescape View File

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

2
libraries/AP_Compass/AP_Compass_AK8963.h
Unescape View File

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

24
libraries/AP_Compass/AP_Compass_BMM150.cpp
Unescape View File

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

6
libraries/AP_Compass/AP_Compass_BMM150.h
Unescape View File

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

4
libraries/AP_Compass/AP_Compass_Backend.cpp
Unescape View File

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

3
libraries/AP_Compass/AP_Compass_Backend.h
Unescape View File

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

12
libraries/AP_Compass/AP_Compass_HMC5843.cpp
Unescape View File

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

1
libraries/AP_Compass/AP_Compass_HMC5843.h
Unescape View File

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

12
libraries/AP_Compass/AP_Compass_LSM303D.cpp
Unescape View File

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

1
libraries/AP_Compass/AP_Compass_LSM303D.h
Unescape View File

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

50
libraries/AP_Compass/AP_Compass_LSM9DS1.cpp
Unescape View File

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

2
libraries/AP_Compass/AP_Compass_LSM9DS1.h
Unescape View File

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

Write Preview
Loading…
Cancel
Save