AP_OpticalFlow: use WITH_SEMAPHORE()

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

libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp

@@ -71,12 +71,10 @@ bool AP_OpticalFlow_PX4Flow::scan_buses(void)
             if (!tdev) {
                 continue;
             }
-            if (!tdev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-                continue;
-            }
+            WITH_SEMAPHORE(tdev->get_semaphore());
+
             struct i2c_integral_frame frame;
             success = tdev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame));
-            tdev->get_semaphore()->give();
             if (success) {
                 printf("Found PX4Flow on bus %u\n", bus);
                 dev = std::move(tdev);

libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp

@@ -107,9 +107,7 @@ bool AP_OpticalFlow_Pixart::setup_sensor(void)
     if (!_dev) {
         return false;
     }
-    if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        AP_HAL::panic("Unable to get bus semaphore");
-    }
+    WITH_SEMAPHORE(_dev->get_semaphore());
 
     uint8_t id;
     uint16_t crc;
@@ -133,7 +131,7 @@ bool AP_OpticalFlow_Pixart::setup_sensor(void)
         model = PIXART_3901;
     } else {
         debug("Not a recognised device\n");
-        goto failed;
+        return false;
     }
 
     if (model == PIXART_3900) {
@@ -142,7 +140,7 @@ bool AP_OpticalFlow_Pixart::setup_sensor(void)
         id = reg_read(PIXART_REG_SROM_ID);
         if (id != srom_id) {
             debug("Pixart: bad SROM ID: 0x%02x\n", id);
-            goto failed;
+            return false;
         }
         
         reg_write(PIXART_REG_SROM_EN, 0x15);
@@ -151,7 +149,7 @@ bool AP_OpticalFlow_Pixart::setup_sensor(void)
         crc = reg_read16u(PIXART_REG_DOUT_L);
         if (crc != 0xBEEF) {
             debug("Pixart: bad SROM CRC: 0x%04x\n", crc);
-            goto failed;
+            return false;
         }
     }
 
@@ -167,16 +165,10 @@ bool AP_OpticalFlow_Pixart::setup_sensor(void)
 
     debug("Pixart %s ready\n", model==PIXART_3900?"3900":"3901");
 
-    _dev->get_semaphore()->give();
-
     integral.last_frame_us = AP_HAL::micros();
 
     _dev->register_periodic_callback(2000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Pixart::timer, void));
     return true;
-
-failed:
-    _dev->get_semaphore()->give();
-    return false;
 }
 
 
@@ -299,13 +291,14 @@ void AP_OpticalFlow_Pixart::timer(void)
     float dt = dt_us * 1.0e-6;
     const Vector3f &gyro = get_ahrs().get_gyro();
 
-    if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    {
+        WITH_SEMAPHORE(_sem);
+
         integral.sum.x += burst.delta_x;
         integral.sum.y += burst.delta_y;
         integral.sum_us += dt_us;
         integral.last_frame_us = last_burst_us;
         integral.gyro += Vector2f(gyro.x, gyro.y) * dt;
-        _sem->give();
     }
     
 #if 0
@@ -344,7 +337,9 @@ void AP_OpticalFlow_Pixart::update(void)
     state.device_id = 1;
     state.surface_quality = burst.squal;
     
-    if (integral.sum_us > 0 && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    if (integral.sum_us > 0) {
+        WITH_SEMAPHORE(_sem);
+
         const Vector2f flowScaler = _flowScaler();
         float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
         float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
@@ -362,7 +357,6 @@ void AP_OpticalFlow_Pixart::update(void)
         integral.sum.zero();
         integral.sum_us = 0;
         integral.gyro.zero();
-        _sem->give();
     } else {
         state.flowRate.zero();
         state.bodyRate.zero();

libraries/AP_OpticalFlow/OpticalFlow_backend.cpp

@@ -20,14 +20,10 @@ extern const AP_HAL::HAL& hal;
 OpticalFlow_backend::OpticalFlow_backend(OpticalFlow &_frontend) :
     frontend(_frontend)
 {
-    _sem = hal.util->new_semaphore();    
 }
 
 OpticalFlow_backend::~OpticalFlow_backend(void)
 {
-    if (_sem) {
-        delete _sem;
-    }
 }
 
 // update the frontend

libraries/AP_OpticalFlow/OpticalFlow_backend.h

@@ -58,5 +58,5 @@ protected:
     uint8_t get_address(void) const { return frontend._address; }
     
     // semaphore for access to shared frontend data
-    AP_HAL::Semaphore *_sem;
+    HAL_Semaphore _sem;
 };