AP_Compass: cleanup use of backend semaphores

fixed drivers that didn't protect accumulation counters

libraries/AP_Compass/AP_Compass_AK8963.cpp

@@ -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;
-    }
-
-    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;
-}
+    if (_sem->take_nonblocking()) {
+        if (_accum_count == 0) {
+            /* We're not ready to publish */
+            _sem->give();
+            return;
+        }
 
-void AP_Compass_AK8963::_reset_filter()
-{
-    _mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
-    _accum_count = 0;
+        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);
+    }
 }
 
 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:

libraries/AP_Compass/AP_Compass_AK8963.h

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

libraries/AP_Compass/AP_Compass_BMM150.cpp

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

libraries/AP_Compass/AP_Compass_BMM150.h

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

libraries/AP_Compass/AP_Compass_Backend.cpp

@@ -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)
 {

libraries/AP_Compass/AP_Compass_Backend.h

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

libraries/AP_Compass/AP_Compass_HMC5843.cpp

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

libraries/AP_Compass/AP_Compass_HMC5843.h

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

libraries/AP_Compass/AP_Compass_LSM303D.cpp

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

libraries/AP_Compass/AP_Compass_LSM303D.h

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

libraries/AP_Compass/AP_Compass_LSM9DS1.cpp

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

libraries/AP_Compass/AP_Compass_LSM9DS1.h

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