Copter: SmartRTL: do not consume home position when following path

This home position is explicitly set when set_home is called (e.g. via
the mavlink interface).  It doesn't make any sense to pop that home
position from the list as it won't ever get reset.

ArduCopter/mode.h

@@ -1244,7 +1244,7 @@ class ModeSmartRTL : public ModeRTL {
     // keep track of how long we have failed to get another return
     // point while following our path home.  If we take too long we
     // may choose to land the vehicle.
-    uint32_t path_follow_last_pop_fail_ms;
+    uint32_t path_follow_last_semtake_fail_ms;
 };
 
 

ArduCopter/mode_smart_rtl.cpp

@@ -74,7 +74,7 @@ void ModeSmartRTL::wait_cleanup_run()
 
     // check if return path is computed and if yes, begin journey home
     if (g2.smart_rtl.request_thorough_cleanup()) {
-        path_follow_last_pop_fail_ms = 0;
+        path_follow_last_semtake_fail_ms = 0;
         smart_rtl_state = SmartRTL_PathFollow;
     }
 }
@@ -92,42 +92,38 @@ void ModeSmartRTL::path_follow_run()
 
     // if we are close to current target point, switch the next point to be our target.
     if (wp_nav->reached_wp_destination()) {
-        Vector3f dest_NED;
-        // this pop_point can fail if the IO task currently has the
-        // path semaphore.
-        if (g2.smart_rtl.pop_point(dest_NED)) {
-            path_follow_last_pop_fail_ms = 0;
-            if (g2.smart_rtl.get_num_points() == 0) {
-                // this is the very last point, add 2m to the target alt and move to pre-land state
-                dest_NED.z -= 2.0f;
+        HAL_Semaphore &sem = g2.smart_rtl.path_semaphore();
+        if (sem.take_nonblocking()) {
+            Vector3f dest_NED;
+            const uint32_t num_points = g2.smart_rtl.get_num_points();
+            if (num_points == 0) {
+                // we don't even have a home point set in
+                // SmartRTL... how did we manage to get here?
+                INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
                 smart_rtl_state = SmartRTL_PreLandPosition;
-                wp_nav->set_wp_destination_NED(dest_NED);
-            } else {
-                // peek at the next point.  this can fail if the IO task currently has the path semaphore
-                Vector3f next_dest_NED;
-                if (g2.smart_rtl.peek_point(next_dest_NED)) {
+            } else if (num_points == 1) {
+                // we don't remove the home point
+                if (g2.smart_rtl.peek_point(dest_NED)) {
+                    // this is the very last point, add 2m to the target alt
+                    dest_NED.z -= 2.0f;
+                    smart_rtl_state = SmartRTL_PreLandPosition;
                     wp_nav->set_wp_destination_NED(dest_NED);
-                    if (g2.smart_rtl.get_num_points() == 1) {
-                        // this is the very last point, add 2m to the target alt
-                        next_dest_NED.z -= 2.0f;
-                    }
-                    wp_nav->set_wp_destination_next_NED(next_dest_NED);
                 } else {
-                    // this can only happen if peek failed to take the semaphore
-                    // send next point anyway which will cause the vehicle to slow at the next point
+                    INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+                    smart_rtl_state = SmartRTL_PreLandPosition;
+                }
+            } else {
+                if (g2.smart_rtl.pop_point(dest_NED)) {
                     wp_nav->set_wp_destination_NED(dest_NED);
+                } else {
                     INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
                 }
             }
-        } else if (g2.smart_rtl.get_num_points() == 0) {
-            // We should never get here; should always have at least
-            // two points and the "zero points left" is handled above.
-            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
-            smart_rtl_state = SmartRTL_PreLandPosition;
-        } else if (path_follow_last_pop_fail_ms == 0) {
-            // first time we've failed to pop off (ever, or after a success)
-            path_follow_last_pop_fail_ms = AP_HAL::millis();
-        } else if (AP_HAL::millis() - path_follow_last_pop_fail_ms > 10000) {
+            sem.give();
+        } else if (path_follow_last_semtake_fail_ms == 0) {
+            // first time we've failed to take the semaphore
+            path_follow_last_semtake_fail_ms = AP_HAL::millis();
+        } else if (AP_HAL::millis() - path_follow_last_semtake_fail_ms > 10000) {
             // we failed to pop a point off for 10 seconds.  This is
             // almost certainly a bug.
             INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);

libraries/AP_SmartRTL/AP_SmartRTL.h

@@ -47,6 +47,11 @@ class AP_SmartRTL {
     // this may fail if the IO thread has taken the path semaphore
     bool peek_point(Vector3f& point);
 
+    // returns the semaphore responsible for path protection:
+    HAL_Semaphore &path_semaphore(void) {
+        return _path_sem;
+    }
+
     // 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