Copter: mode_smart_rtl uses peek_point

ArduCopter/mode_smart_rtl.cpp

@@ -97,16 +97,21 @@ void ModeSmartRTL::path_follow_run()
         // path semaphore.
         if (g2.smart_rtl.pop_point(next_point)) {
             path_follow_last_pop_fail_ms = 0;
-            bool fast_waypoint = true;
             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
                 next_point.z -= 2.0f;
                 smart_rtl_state = SmartRTL_PreLandPosition;
-                fast_waypoint = false;
+                wp_nav->set_wp_destination_NED(next_point);
+            } else {
+                // peek at the next point
+                Vector3f next_next_point;
+                if (g2.smart_rtl.peek_point(next_next_point)) {
+                    wp_nav->set_wp_destination_NED(next_point, next_next_point);
+                } else {
+                    // this should never happen but send next point anyway
+                    wp_nav->set_wp_destination_NED(next_point);
+                }
             }
-            // send target to waypoint controller
-            wp_nav->set_wp_destination_NED(next_point);
-            wp_nav->set_fast_waypoint(fast_waypoint);
         } 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.