AP_Compass: use WITH_SEMAPHORE()

and removed usage of hal.util->new_semaphore()

libraries/AP_Compass/AP_Compass_AK8963.cpp

@@ -17,6 +17,7 @@
 
 #include <AP_Math/AP_Math.h>
 #include <AP_HAL/AP_HAL.h>
+#include <AP_Common/Semaphore.h>
 
 #include "AP_Compass_AK8963.h"
 #include <AP_InertialSensor/AP_InertialSensor_Invensense.h>
@@ -179,19 +180,19 @@ void AP_Compass_AK8963::read()
         return;
     }
 
-    if (_sem->take_nonblocking()) {
+    if (_accum_count == 0) {
-        if (_accum_count == 0) {
+        return;
-            /* We're not ready to publish */
+    }
-            _sem->give();
-            return;
-        }
 
-        Vector3f field = Vector3f(_mag_x_accum, _mag_y_accum, _mag_z_accum) / _accum_count;
+    Vector3f field;
+    {
+        WITH_SEMAPHORE(_sem);
+        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);
     }
+
+    publish_filtered_field(field, _compass_instance);
 }
 
 void AP_Compass_AK8963::_make_adc_sensitivity_adjustment(Vector3f& field) const
@@ -242,18 +243,17 @@ void AP_Compass_AK8963::_update()
     // correct raw_field for known errors
     correct_field(raw_field, _compass_instance);
 
-    if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    WITH_SEMAPHORE(_sem);
-        _mag_x_accum += raw_field.x;
+
-        _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;
-        }
-        _sem->give();
     }
 }
 

libraries/AP_Compass/AP_Compass_Backend.cpp

@@ -11,7 +11,6 @@ extern const AP_HAL::HAL& hal;
 AP_Compass_Backend::AP_Compass_Backend()
     : _compass(AP::compass())
 {
-    _sem = hal.util->new_semaphore();
 }
 
 void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
@@ -126,12 +125,9 @@ void AP_Compass_Backend::accumulate_sample(Vector3f &field, uint8_t instance,
 void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance,
                                                    const Vector3f *scaling)
 {
-    if (!_sem->take_nonblocking()) {
+    WITH_SEMAPHORE(_sem);
-        return;
-    }
 
     if (_accum_count == 0) {
-        _sem->give();
         return;
     }
 
@@ -144,8 +140,6 @@ void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance,
 
     _accum.zero();
     _accum_count = 0;
-
-    _sem->give();
 }
 
 /*

libraries/AP_Compass/AP_Compass_Backend.h

@@ -110,7 +110,7 @@ protected:
     Compass &_compass;
 
     // semaphore for access to shared frontend data
-    AP_HAL::Semaphore *_sem;
+    HAL_Semaphore_Recursive _sem;
 
     // accumulated samples, protected by _sem
     Vector3f    _accum;

libraries/AP_Compass/AP_Compass_SITL.cpp

@@ -1,6 +1,7 @@
 #include "AP_Compass_SITL.h"
 
 #include <AP_HAL/AP_HAL.h>
+#include <AP_Common/Semaphore.h>
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 extern const AP_HAL::HAL& hal;
@@ -123,9 +124,7 @@ void AP_Compass_SITL::_timer()
         _mag_accum[i] += f;
     }
 
-    if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    WITH_SEMAPHORE(_sem);
-        return;
-    }
 
     _accum_count++;
     if (_accum_count == 10) {
@@ -135,28 +134,24 @@ void AP_Compass_SITL::_timer()
         _accum_count = 5;
         _has_sample = true;
     }
-    _sem->give();
 }
 
 void AP_Compass_SITL::read()
 {
-    if (_sem->take_nonblocking()) {
+    WITH_SEMAPHORE(_sem);
-        if (!_has_sample) {
-            _sem->give();
-            return;
-        }
 
-        for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
+    if (!_has_sample) {
-            Vector3f field(_mag_accum[i]);
+        return;
-            field /= _accum_count;
-            _mag_accum[i].zero();
-            publish_filtered_field(field, _compass_instance[i]);
-        }
-        _accum_count = 0;
-
-        _has_sample = false;
-        _sem->give();
     }
 
+    for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
+        Vector3f field(_mag_accum[i]);
+        field /= _accum_count;
+        _mag_accum[i].zero();
+        publish_filtered_field(field, _compass_instance[i]);
+    }
+    _accum_count = 0;
+
+    _has_sample = false;
 }
 #endif

libraries/AP_Compass/AP_Compass_UAVCAN.cpp

@@ -36,7 +36,7 @@ UC_REGISTRY_BINDER(MagCb, uavcan::equipment::ahrs::MagneticFieldStrength);
 UC_REGISTRY_BINDER(Mag2Cb, uavcan::equipment::ahrs::MagneticFieldStrength2);
 
 AP_Compass_UAVCAN::DetectedModules AP_Compass_UAVCAN::_detected_modules[] = {0};
-AP_HAL::Semaphore* AP_Compass_UAVCAN::_sem_registry = nullptr;
+HAL_Semaphore AP_Compass_UAVCAN::_sem_registry;
 
 AP_Compass_UAVCAN::AP_Compass_UAVCAN(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id)
     : _ap_uavcan(ap_uavcan)
@@ -72,15 +72,12 @@ void AP_Compass_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
 
 bool AP_Compass_UAVCAN::take_registry()
 {
-    if (_sem_registry == nullptr) {
+    return _sem_registry.take(HAL_SEMAPHORE_BLOCK_FOREVER);
-        _sem_registry = hal.util->new_semaphore();
-    }
-    return _sem_registry->take(HAL_SEMAPHORE_BLOCK_FOREVER);
 }
 
 void AP_Compass_UAVCAN::give_registry()
 {
-    _sem_registry->give();
+    _sem_registry.give();
 }
 
 AP_Compass_Backend* AP_Compass_UAVCAN::probe()

libraries/AP_Compass/AP_Compass_UAVCAN.h

@@ -48,5 +48,5 @@ private:
         AP_Compass_UAVCAN *driver;
     } _detected_modules[COMPASS_MAX_BACKEND];
 
-    static AP_HAL::Semaphore *_sem_registry;
+    static HAL_Semaphore _sem_registry;
 };

libraries/AP_Compass/Compass_learn.cpp

@@ -48,8 +48,6 @@ void CompassLearn::update(void)
         R.from_euler(0.0f, -ToRad(inclination_deg), ToRad(declination_deg));
         mag_ef = R * mag_ef;
 
-        sem = hal.util->new_semaphore();
-
         have_earth_field = true;
 
         // form eliptical correction matrix and invert it. This is
@@ -85,8 +83,9 @@ void CompassLearn::update(void)
     if (field_change.length() < min_field_change) {
         return;
     }
-    
+
-    if (sem->take_nonblocking()) {
+    {
+        WITH_SEMAPHORE(sem);
         // give a sample to the backend to process
         new_sample.field = field;
         new_sample.offsets = compass.get_offsets(0);
@@ -94,7 +93,6 @@ void CompassLearn::update(void)
         sample_available = true;
         last_field = field;
         num_samples++;
-        sem->give();
     }
 
     if (sample_available) {
@@ -109,7 +107,9 @@ void CompassLearn::update(void)
                                                num_samples);
     }
 
-    if (!converged && sem->take_nonblocking()) {
+    if (!converged) {
+        WITH_SEMAPHORE(sem);
+
         // stop updating the offsets once converged
         compass.set_offsets(0, best_offsets);
         if (num_samples > 30 && best_error < 50 && worst_error > 65) {
@@ -120,7 +120,6 @@ void CompassLearn::update(void)
             compass.set_learn_type(Compass::LEARN_EKF, true);
             converged = true;
         }
-        sem->give();
     }
 }
 
@@ -132,13 +131,14 @@ void CompassLearn::io_timer(void)
     if (!sample_available) {
         return;
     }
+
     struct sample s;
-    if (!sem->take_nonblocking()) {
+
-        return;
+    {
+        WITH_SEMAPHORE(sem);
+        s = new_sample;
+        sample_available = false;
     }
-    s = new_sample;
-    sample_available = false;
-    sem->give();
 
     process_sample(s);
 }
@@ -192,12 +192,11 @@ void CompassLearn::process_sample(const struct sample &s)
         }
     }
 
-    if (sem->take_nonblocking()) {
+    WITH_SEMAPHORE(sem);
-        // pass the current estimate to the front-end
+
-        best_offsets = predicted_offsets[besti];
+    // pass the current estimate to the front-end
-        best_error = bestv;
+    best_offsets = predicted_offsets[besti];
-        worst_error = worstv;
+    best_error = bestv;
-        best_yaw_deg = wrap_360(degrees(s.attitude.z) + besti * (360/num_sectors));
+    worst_error = worstv;
-        sem->give();
+    best_yaw_deg = wrap_360(degrees(s.attitude.z) + besti * (360/num_sectors));
-    }
 }

libraries/AP_Compass/Compass_learn.h

@@ -30,7 +30,7 @@ private:
     Vector3f mag_ef;
 
     // semaphore for access to shared data with IO thread
-    AP_HAL::Semaphore *sem;    
+    HAL_Semaphore sem;
     
     struct sample {
         // milliGauss body field and offsets