AP_SmartRTL: peek_point method peeks at next point

includes peek point takes semaphore

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)
 {

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,