From 046bcfa0a94be8d97758d6d00e46726bb5d0a708 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_SmartRTL: use WITH_SEMAPHORE()

and removed usage of hal.util->new_semaphore()
---
 libraries/AP_SmartRTL/AP_SmartRTL.cpp | 34 +++++++++++----------------
 libraries/AP_SmartRTL/AP_SmartRTL.h   |  2 +-
 2 files changed, 15 insertions(+), 21 deletions(-)

diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.cpp b/libraries/AP_SmartRTL/AP_SmartRTL.cpp
index 5531e1e056..fda1957969 100644
--- a/libraries/AP_SmartRTL/AP_SmartRTL.cpp
+++ b/libraries/AP_SmartRTL/AP_SmartRTL.cpp
@@ -95,12 +95,6 @@ void AP_SmartRTL::init()
         return;
     }
 
-    // create semaphore
-    _path_sem = hal.util->new_semaphore();
-    if (_path_sem == nullptr) {
-        return;
-    }
-
     // allocate arrays
     _path = (Vector3f*)calloc(_points_max, sizeof(Vector3f));
 
@@ -144,14 +138,14 @@ bool AP_SmartRTL::pop_point(Vector3f& point)
     }
 
     // get semaphore
-    if (!_path_sem->take_nonblocking()) {
+    if (!_path_sem.take_nonblocking()) {
         log_action(SRTL_POP_FAILED_NO_SEMAPHORE);
         return false;
     }
 
     // check we have another point
     if (_path_points_count == 0) {
-        _path_sem->give();
+        _path_sem.give();
         return false;
     }
 
@@ -161,7 +155,7 @@ bool AP_SmartRTL::pop_point(Vector3f& point)
     // record count of last point popped
     _path_points_completed_limit = _path_points_count;
 
-    _path_sem->give();
+    _path_sem.give();
     return true;
 }
 
@@ -287,7 +281,7 @@ void AP_SmartRTL::cancel_request_for_thorough_cleanup()
 bool AP_SmartRTL::add_point(const Vector3f& point)
 {
     // get semaphore
-    if (!_path_sem->take_nonblocking()) {
+    if (!_path_sem.take_nonblocking()) {
         log_action(SRTL_ADD_FAILED_NO_SEMAPHORE, point);
         return false;
     }
@@ -296,14 +290,14 @@ bool AP_SmartRTL::add_point(const Vector3f& point)
     if (_path_points_count > 0) {
         const Vector3f& last_pos = _path[_path_points_count-1];
         if (last_pos.distance_squared(point) < sq(_accuracy.get())) {
-            _path_sem->give();
+            _path_sem.give();
             return true;
         }
     }
 
     // check we have space in the path
     if (_path_points_count >= _path_points_max) {
-        _path_sem->give();
+        _path_sem.give();
         log_action(SRTL_ADD_FAILED_PATH_FULL, point);
         return false;
     }
@@ -312,7 +306,7 @@ bool AP_SmartRTL::add_point(const Vector3f& point)
     _path[_path_points_count++] = point;
     log_action(SRTL_POINT_ADD, point);
 
-    _path_sem->give();
+    _path_sem.give();
     return true;
 }
 
@@ -324,14 +318,14 @@ void AP_SmartRTL::run_background_cleanup()
     }
 
     // get semaphore
-    if (!_path_sem->take_nonblocking()) {
+    if (!_path_sem.take_nonblocking()) {
         return;
     }
     // local copy of _path_points_count and _path_points_completed_limit
     const uint16_t path_points_count = _path_points_count;
     const uint16_t path_points_completed_limit = _path_points_completed_limit;
     _path_points_completed_limit = SMARTRTL_POINTS_MAX;
-    _path_sem->give();
+    _path_sem.give();
 
     // check if thorough cleanup is required
     if (_thorough_clean_request_ms > 0) {
@@ -623,7 +617,7 @@ void AP_SmartRTL::reset_pruning()
 void AP_SmartRTL::remove_points_by_simplify_bitmask()
 {
     // get semaphore before modifying path
-    if (!_path_sem->take_nonblocking()) {
+    if (!_path_sem.take_nonblocking()) {
         return;
     }
     uint16_t dest = 1;
@@ -648,7 +642,7 @@ void AP_SmartRTL::remove_points_by_simplify_bitmask()
         deactivate(SRTL_DEACTIVATED_PROGRAM_ERROR, "program error");
     }
 
-    _path_sem->give();
+    _path_sem.give();
 
     // flag point removal is complete
     _simplify.bitmask.setall();
@@ -666,7 +660,7 @@ bool AP_SmartRTL::remove_points_by_loops(uint16_t num_points_to_remove)
     }
 
     // get semaphore before modifying path
-    if (!_path_sem->take_nonblocking()) {
+    if (!_path_sem.take_nonblocking()) {
         return false;
     }
 
@@ -692,7 +686,7 @@ bool AP_SmartRTL::remove_points_by_loops(uint16_t num_points_to_remove)
         } else {
             // this is an error that should never happen so deactivate
             deactivate(SRTL_DEACTIVATED_PROGRAM_ERROR, "program error");
-            _path_sem->give();
+            _path_sem.give();
             // we return true so thorough_cleanup does not get stuck
             return true;
         }
@@ -712,7 +706,7 @@ bool AP_SmartRTL::remove_points_by_loops(uint16_t num_points_to_remove)
         _prune.loops_count--;
     }
 
-    _path_sem->give();
+    _path_sem.give();
     return true;
 }
 
diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.h b/libraries/AP_SmartRTL/AP_SmartRTL.h
index d77848e29a..de8281f675 100644
--- a/libraries/AP_SmartRTL/AP_SmartRTL.h
+++ b/libraries/AP_SmartRTL/AP_SmartRTL.h
@@ -184,7 +184,7 @@ private:
     uint16_t _path_points_max;  // after the array has been allocated, we will need to know how big it is. We can't use the parameter, because a user could change the parameter in-flight
     uint16_t _path_points_count;// number of points in the path array
     uint16_t _path_points_completed_limit;  // set by main thread to the path_point_count when a point is popped.  used by simplify and prune algorithms to detect path shrinking
-    AP_HAL::Semaphore *_path_sem;   // semaphore for updating path
+    HAL_Semaphore _path_sem;   // semaphore for updating path
 
     // Simplify
     // structure and buffer to hold the "to-do list" for the simplify algorithm.