From 193799346c594532ca0c70608a73dd919e6fff4a Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 8 Jan 2020 20:18:20 +0900
Subject: [PATCH] AP_SmartRTL: peek_point method peeks at next point

includes peek point takes semaphore
---
 libraries/AP_SmartRTL/AP_SmartRTL.cpp | 27 +++++++++++++++++++++++++++
 libraries/AP_SmartRTL/AP_SmartRTL.h   |  5 +++++
 2 files changed, 32 insertions(+)

diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.cpp b/libraries/AP_SmartRTL/AP_SmartRTL.cpp
index 3b493e82cb..0a9ecf3a85 100644
--- a/libraries/AP_SmartRTL/AP_SmartRTL.cpp
+++ b/libraries/AP_SmartRTL/AP_SmartRTL.cpp
@@ -170,6 +170,33 @@ bool AP_SmartRTL::pop_point(Vector3f& point)
     return true;
 }
 
+// peek at next point on the path without removing it form the path. Returns true on success
+bool AP_SmartRTL::peek_point(Vector3f& point)
+{
+    // check we are active
+    if (!_active) {
+        return false;
+    }
+
+    // get semaphore
+    if (!_path_sem.take_nonblocking()) {
+        log_action(SRTL_PEEK_FAILED_NO_SEMAPHORE);
+        return false;
+    }
+
+    // check we have another point
+    if (_path_points_count == 0) {
+        _path_sem.give();
+        return false;
+    }
+
+    // return last point
+    point = _path[_path_points_count-1];
+
+    _path_sem.give();
+    return true;
+}
+
 // clear return path and set home location.  This should be called as part of the arming procedure
 void AP_SmartRTL::set_home(bool position_ok)
 {
diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.h b/libraries/AP_SmartRTL/AP_SmartRTL.h
index a838f425ea..a83cad692a 100644
--- a/libraries/AP_SmartRTL/AP_SmartRTL.h
+++ b/libraries/AP_SmartRTL/AP_SmartRTL.h
@@ -43,6 +43,10 @@ public:
     // get next point on the path to home, returns true on success
     bool pop_point(Vector3f& point);
 
+    // peek at next point on the path without removing it form the path. Returns true on success
+    // this may fail if the IO thread has taken the path semaphore
+    bool peek_point(Vector3f& point);
+
     // clear return path and set return location if position_ok is true.  This should be called as part of the arming procedure
     // if position_ok is false, SmartRTL will not be available.
     // example sketches use the method that allows providing vehicle position directly
@@ -90,6 +94,7 @@ private:
         SRTL_ADD_FAILED_NO_SEMAPHORE,
         SRTL_ADD_FAILED_PATH_FULL,
         SRTL_POP_FAILED_NO_SEMAPHORE,
+        SRTL_PEEK_FAILED_NO_SEMAPHORE,
         SRTL_DEACTIVATED_INIT_FAILED,
         SRTL_DEACTIVATED_BAD_POSITION,
         SRTL_DEACTIVATED_BAD_POSITION_TIMEOUT,