From dedfa54d5a8bd613a7294883375df229cf3c7b35 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 12 Oct 2018 10:35:04 +1100
Subject: [PATCH] AP_IRLock: use WITH_SEMAPHORE()

and removed usage of hal.util->new_semaphore()
---
 libraries/AP_IRLock/AP_IRLock_I2C.cpp | 23 +++++++++++------------
 libraries/AP_IRLock/AP_IRLock_I2C.h   |  2 +-
 2 files changed, 12 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_IRLock/AP_IRLock_I2C.cpp b/libraries/AP_IRLock/AP_IRLock_I2C.cpp
index d0af466ac1..56170170ba 100644
--- a/libraries/AP_IRLock/AP_IRLock_I2C.cpp
+++ b/libraries/AP_IRLock/AP_IRLock_I2C.cpp
@@ -43,8 +43,6 @@ void AP_IRLock_I2C::init(int8_t bus)
         return;
     }
 
-    sem = hal.util->new_semaphore();
-
     // read at 50Hz
     printf("Starting IRLock on I2C\n");
 
@@ -132,14 +130,15 @@ void AP_IRLock_I2C::read_frames(void)
     pixel_to_1M_plane(corner1_pix_x, corner1_pix_y, corner1_pos_x, corner1_pos_y);
     pixel_to_1M_plane(corner2_pix_x, corner2_pix_y, corner2_pos_x, corner2_pos_y);
 
-    if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    {
+        WITH_SEMAPHORE(sem);
+
         /* convert to angles */
         _target_info.timestamp = AP_HAL::millis();
         _target_info.pos_x = 0.5f*(corner1_pos_x+corner2_pos_x);
         _target_info.pos_y = 0.5f*(corner1_pos_y+corner2_pos_y);
         _target_info.size_x = corner2_pos_x-corner1_pos_x;
         _target_info.size_y = corner2_pos_y-corner1_pos_y;
-        sem->give();
     }
 
 #if 0
@@ -158,17 +157,17 @@ void AP_IRLock_I2C::read_frames(void)
 bool AP_IRLock_I2C::update()
 {
     bool new_data = false;
-    if (!dev || !sem) {
+    if (!dev) {
         return false;
     }
-    if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        if (_last_update_ms != _target_info.timestamp) {
-            new_data = true;
-        }
-        _last_update_ms = _target_info.timestamp;
-        _flags.healthy = (AP_HAL::millis() - _last_read_ms < 100);
-        sem->give();
+    WITH_SEMAPHORE(sem);
+
+    if (_last_update_ms != _target_info.timestamp) {
+        new_data = true;
     }
+    _last_update_ms = _target_info.timestamp;
+    _flags.healthy = (AP_HAL::millis() - _last_read_ms < 100);
+
     // return true if new data found
     return new_data;
 }
diff --git a/libraries/AP_IRLock/AP_IRLock_I2C.h b/libraries/AP_IRLock/AP_IRLock_I2C.h
index 862274beb3..29d2ca46f4 100644
--- a/libraries/AP_IRLock/AP_IRLock_I2C.h
+++ b/libraries/AP_IRLock/AP_IRLock_I2C.h
@@ -35,6 +35,6 @@ private:
 
     void pixel_to_1M_plane(float pix_x, float pix_y, float &ret_x, float &ret_y);
 
-    AP_HAL::Semaphore *sem;
+    HAL_Semaphore sem;
     uint32_t _last_read_ms;
 };