Copter: support for SCurves and position controller changes

wp_start provides next_dest_loc
send next_destination to wp_nav instead of setting fast_waypoint
fixup zigzag for S-curve changes
fixup guided
auto spline fixes
smart rtl rename of next_point to dest_NED
loc_from_cmd accepts default location
auto mode stops before starting land command
auto do_next_wp accepts default location
rename do_next_wp to set_next_wp
also rename get_spline_from_cmd argument
also improve failure to set next waypoint due to missing terrain data
also fixup comment in set_next_wp
also auto stops when moving from straight to spline segments
also auto mode spline fix
also auto mode calls AC_WPNav::set_spline_destination_next

Copter: AutoYaw provides rate from WPNav

ArduCopter/autoyaw.cpp

@@ -215,8 +215,21 @@ float Mode::AutoYaw::yaw()
 // messages (positive is clockwise, negative is counter clockwise)
 float Mode::AutoYaw::rate_cds() const
 {
-    if (_mode == AUTO_YAW_RATE) {
+    switch (_mode) {
+
+    case AUTO_YAW_HOLD:
+    case AUTO_YAW_ROI:
+    case AUTO_YAW_FIXED:
+    case AUTO_YAW_LOOK_AHEAD:
+    case AUTO_YAW_RESETTOARMEDYAW:
+    case AUTO_YAW_CIRCLE:
+        return 0.0f;
+
+    case AUTO_YAW_RATE:
         return _rate_cds;
+
+    case AUTO_YAW_LOOK_AT_NEXT_WP:
+        return copter.wp_nav->get_yaw_rate();
     }
 
     // return zero turn rate (this should never happen)

ArduCopter/defines.h

@@ -91,7 +91,6 @@ enum AutoMode {
     Auto_RTL,
     Auto_CircleMoveToEdge,
     Auto_Circle,
-    Auto_Spline,
     Auto_NavGuided,
     Auto_Loiter,
     Auto_LoiterToAlt,

ArduCopter/mode.h

@@ -369,8 +369,6 @@ public:
     void land_start(const Vector3f& destination);
     void circle_movetoedge_start(const Location &circle_center, float radius_m);
     void circle_start();
-    void spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination);
-    void spline_start(const Location& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location& next_destination);
     void nav_guided_start();
 
     bool is_landing() const override;
@@ -419,7 +417,6 @@ private:
 
     void takeoff_run();
     void wp_run();
-    void spline_run();
     void land_run();
     void rtl_run();
     void circle_run();
@@ -427,7 +424,7 @@ private:
     void loiter_run();
     void loiter_to_alt_run();
 
-    Location loc_from_cmd(const AP_Mission::Mission_Command& cmd) const;
+    Location loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const;
 
     void payload_place_start(const Vector3f& destination);
     void payload_place_run();
@@ -442,12 +439,14 @@ private:
 
     void do_takeoff(const AP_Mission::Mission_Command& cmd);
     void do_nav_wp(const AP_Mission::Mission_Command& cmd);
+    bool set_next_wp(const AP_Mission::Mission_Command& current_cmd, const Location &default_loc);
     void do_land(const AP_Mission::Mission_Command& cmd);
     void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
     void do_circle(const AP_Mission::Mission_Command& cmd);
     void do_loiter_time(const AP_Mission::Mission_Command& cmd);
     void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
     void do_spline_wp(const AP_Mission::Mission_Command& cmd);
+    void get_spline_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc, Location& dest_loc, Location& next_dest_loc, bool& next_dest_loc_is_spline);
 #if NAV_GUIDED == ENABLED
     void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
     void do_guided_limits(const AP_Mission::Mission_Command& cmd);

ArduCopter/mode_auto.cpp

@@ -93,10 +93,6 @@ void ModeAuto::run()
         circle_run();
         break;
 
-    case Auto_Spline:
-        spline_run();
-        break;
-
     case Auto_NavGuided:
 #if NAV_GUIDED == ENABLED
         nav_guided_run();
@@ -190,7 +186,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
     }
 
     // set waypoint controller target
-    if (!wp_nav->set_wp_destination(dest)) {
+    if (!wp_nav->set_wp_destination_loc(dest)) {
         // failure to set destination can only be because of missing terrain data
         copter.failsafe_terrain_on_event();
         return;
@@ -212,7 +208,7 @@ void ModeAuto::wp_start(const Location& dest_loc)
     _mode = Auto_WP;
 
     // send target to waypoint controller
-    if (!wp_nav->set_wp_destination(dest_loc)) {
+    if (!wp_nav->set_wp_destination_loc(dest_loc)) {
         // failure to set destination can only be because of missing terrain data
         copter.failsafe_terrain_on_event();
         return;
@@ -293,7 +289,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi
         circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame());
 
         // initialise wpnav to move to edge of circle
-        if (!wp_nav->set_wp_destination(circle_edge)) {
+        if (!wp_nav->set_wp_destination_loc(circle_edge)) {
             // failure to set destination can only be because of missing terrain data
             copter.failsafe_terrain_on_event();
         }
@@ -331,28 +327,6 @@ void ModeAuto::circle_start()
     }
 }
 
-// auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
-//  seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE.  If Straight or Spline the next_destination should be provided
-void ModeAuto::spline_start(const Location& destination, bool stopped_at_start,
-                               AC_WPNav::spline_segment_end_type seg_end_type, 
-                               const Location& next_destination)
-{
-    _mode = Auto_Spline;
-
-    // initialise wpnav
-    if (!wp_nav->set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
-        // failure to set destination can only be because of missing terrain data
-        copter.failsafe_terrain_on_event();
-        return;
-    }
-
-    // initialise yaw
-    // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    if (auto_yaw.mode() != AUTO_YAW_ROI) {
-        auto_yaw.set_mode_to_default(false);
-    }
-}
-
 #if NAV_GUIDED == ENABLED
 // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
 void ModeAuto::nav_guided_start()
@@ -782,47 +756,7 @@ void ModeAuto::wp_run()
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
     } else {
         // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
+        attitude_control->input_euler_angle_roll_pitch_yaw_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), auto_yaw.rate_cds());
-    }
-}
-
-// auto_spline_run - runs the auto spline controller
-//      called by auto_run at 100hz or more
-void ModeAuto::spline_run()
-{
-    // if not armed set throttle to zero and exit immediately
-    if (is_disarmed_or_landed()) {
-        make_safe_spool_down();
-        wp_nav->wp_and_spline_init();
-        return;
-    }
-
-    // process pilot's yaw input
-    float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
-    }
-
-    // set motors to full range
-    motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
-
-    // run waypoint controller
-    wp_nav->update_spline();
-
-    // call z-axis position controller (wpnav should have already updated it's alt target)
-    pos_control->update_z_controller();
-
-    // call attitude controller
-    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
-        // roll & pitch from waypoint controller, yaw rate from pilot
-        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
-    } else {
-        // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
     }
 }
 
@@ -1096,25 +1030,25 @@ void ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd)
     takeoff_start(cmd.content.location);
 }
 
-Location ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd) const
+Location ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const
 {
     Location ret(cmd.content.location);
 
     // use current lat, lon if zero
     if (ret.lat == 0 && ret.lng == 0) {
-        ret.lat = copter.current_loc.lat;
+        ret.lat = default_loc.lat;
-        ret.lng = copter.current_loc.lng;
+        ret.lng = default_loc.lng;
     }
     // use current altitude if not provided
     if (ret.alt == 0) {
-        // set to current altitude but in command's alt frame
+        // set to default_loc's altitude but in command's alt frame
-        int32_t curr_alt;
+        // note that this may use the terrain database
-        if (copter.current_loc.get_alt_cm(ret.get_alt_frame(),curr_alt)) {
+        int32_t default_alt;
-            ret.set_alt_cm(curr_alt, ret.get_alt_frame());
+        if (default_loc.get_alt_cm(ret.get_alt_frame(), default_alt)) {
+            ret.set_alt_cm(default_alt, ret.get_alt_frame());
         } else {
-            // default to current altitude as alt-above-home
+            // default to default_loc's altitude and frame
-            ret.set_alt_cm(copter.current_loc.alt,
+            ret.set_alt_cm(default_loc.alt, default_loc.get_alt_frame());
-                           copter.current_loc.get_alt_frame());
         }
     }
     return ret;
@@ -1123,46 +1057,90 @@ Location ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd) const
 // do_nav_wp - initiate move to next waypoint
 void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
-    Location target_loc = loc_from_cmd(cmd);
+    // calculate default location used when lat, lon or alt is zero
+    Location default_loc = copter.current_loc;
+    if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
+        if (!wp_nav->get_wp_destination_loc(default_loc)) {
+            // this should never happen
+            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        }
+    }
+
+    // get waypoint's location from command and send to wp_nav
+    const Location dest_loc = loc_from_cmd(cmd, default_loc);
+    if (!wp_nav->set_wp_destination_loc(dest_loc)) {
+        // failure to set destination can only be because of missing terrain data
+        copter.failsafe_terrain_on_event();
+        return;
+    }
+
+    _mode = Auto_WP;
 
     // this will be used to remember the time in millis after we reach or pass the WP.
     loiter_time = 0;
     // this is the delay, stored in seconds
     loiter_time_max = cmd.p1;
 
-    // Set wp navigation target
+    // set next destination if necessary
-    wp_start(target_loc);
+    if (!set_next_wp(cmd, dest_loc)) {
+        // failure to set next destination can only be because of missing terrain data
+        copter.failsafe_terrain_on_event();
+        return;
+    }
 
-    // if no delay as well as not final waypoint set the waypoint as "fast"
+    // initialise yaw
-    AP_Mission::Mission_Command temp_cmd;
+    // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    bool fast_waypoint = false;
+    if (auto_yaw.mode() != AUTO_YAW_ROI) {
-    if (loiter_time_max == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
+        auto_yaw.set_mode_to_default(false);
+    }
+}
+
+// checks the next mission command and adds it as a destination if necessary
+// supports both straight line and spline waypoints
+// cmd should be the current command
+// default_loc should be the destination from the current_cmd but corrected for cases where user set lat, lon or alt to zero
+// returns true on success, false on failure which should only happen due to a failure to retrieve terrain data
+bool ModeAuto::set_next_wp(const AP_Mission::Mission_Command& current_cmd, const Location &default_loc)
+{
+    // do not add next wp if current command has a delay meaning the vehicle will stop at the destination
+    if (current_cmd.p1 > 0) {
+        return true;
+    }
+
+    // do not add next wp if there are no more navigation commands
+    AP_Mission::Mission_Command next_cmd;
+    if (!mission.get_next_nav_cmd(current_cmd.index+1, next_cmd)) {
+        return true;
+    }
 
     // whether vehicle should stop at the target position depends upon the next command
-        switch (temp_cmd.id) {
+    switch (next_cmd.id) {
     case MAV_CMD_NAV_WAYPOINT:
     case MAV_CMD_NAV_LOITER_UNLIM:
     case MAV_CMD_NAV_LOITER_TURNS:
-            case MAV_CMD_NAV_LOITER_TIME:
+    case MAV_CMD_NAV_LOITER_TIME: {
-            case MAV_CMD_NAV_LAND:
+        const Location dest_loc = loc_from_cmd(current_cmd, default_loc);
-            case MAV_CMD_NAV_SPLINE_WAYPOINT:
+        const Location next_dest_loc = loc_from_cmd(next_cmd, dest_loc);
-                // if next command's lat, lon is specified then do not slowdown at this waypoint
+        return wp_nav->set_wp_destination_next_loc(next_dest_loc);
-                if ((temp_cmd.content.location.lat != 0) || (temp_cmd.content.location.lng != 0)) {
-                    fast_waypoint = true;
     }
-                break;
+    case MAV_CMD_NAV_SPLINE_WAYPOINT: {
+        // get spline's location and next location from command and send to wp_nav
+        Location next_dest_loc, next_next_dest_loc;
+        bool next_next_dest_loc_is_spline;
+        get_spline_from_cmd(next_cmd, default_loc, next_dest_loc, next_next_dest_loc, next_next_dest_loc_is_spline);
+        return wp_nav->set_spline_destination_next_loc(next_dest_loc, next_next_dest_loc, next_next_dest_loc_is_spline);
+    }
+    case MAV_CMD_NAV_LAND:
+        // stop because we may change between rel,abs and terrain alt types
     case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-                // do not stop for RTL
-                fast_waypoint = true;
-                break;
     case MAV_CMD_NAV_TAKEOFF:
+        // always stop for RTL and takeoff commands
     default:
-                // always stop for takeoff commands
         // for unsupported commands it is safer to stop
         break;
     }
-        copter.wp_nav->set_fast_waypoint(fast_waypoint);
+
-    }
+    return true;
 }
 
 // do_land - initiate landing procedure
@@ -1225,7 +1203,7 @@ void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
 // do_circle - initiate moving in a circle
 void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
 {
-    const Location circle_center = loc_from_cmd(cmd);
+    const Location circle_center = loc_from_cmd(cmd, copter.current_loc);
 
     // calculate radius
     uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
@@ -1276,68 +1254,66 @@ void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
     pos_control->set_max_accel_z(wp_nav->get_accel_z());
     pos_control->set_max_speed_z(wp_nav->get_default_speed_down(),
                                  wp_nav->get_default_speed_up());
-
-    if (pos_control->is_active_z()) {
-        pos_control->freeze_ff_z();
-    }
 }
 
-// do_spline_wp - initiate move to next waypoint
+// do_nav_wp - initiate move to next waypoint
 void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
 {
-    const Location target_loc = loc_from_cmd(cmd);
+    // calculate default location used when lat, lon or alt is zero
+    Location default_loc = copter.current_loc;
+    if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
+        if (!wp_nav->get_wp_destination_loc(default_loc)) {
+            // this should never happen
+            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        }
+    }
+
+    // get spline's location and next location from command and send to wp_nav
+    Location dest_loc, next_dest_loc;
+    bool next_dest_loc_is_spline;
+    get_spline_from_cmd(cmd, default_loc, dest_loc, next_dest_loc, next_dest_loc_is_spline);
+    if (!wp_nav->set_spline_destination_loc(dest_loc, next_dest_loc, next_dest_loc_is_spline)) {
+        // failure to set destination can only be because of missing terrain data
+        copter.failsafe_terrain_on_event();
+        return;
+    }
+
+    _mode = Auto_WP;
 
     // this will be used to remember the time in millis after we reach or pass the WP.
     loiter_time = 0;
     // this is the delay, stored in seconds
     loiter_time_max = cmd.p1;
 
-    // determine segment start and end type
+    // set next destination if necessary
-    bool stopped_at_start = true;
+    if (!set_next_wp(cmd, dest_loc)) {
-    AC_WPNav::spline_segment_end_type seg_end_type = AC_WPNav::SEGMENT_END_STOP;
+        // failure to set next destination can only be because of missing terrain data
-    AP_Mission::Mission_Command temp_cmd;
+        copter.failsafe_terrain_on_event();
+        return;
+    }
 
-    // if previous command was a wp_nav command with no delay set stopped_at_start to false
+    // initialise yaw
-    // To-Do: move processing of delay into wp-nav controller to allow it to determine the stopped_at_start value itself?
+    // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    uint16_t prev_cmd_idx = mission.get_prev_nav_cmd_index();
+    if (auto_yaw.mode() != AUTO_YAW_ROI) {
-    if (prev_cmd_idx != AP_MISSION_CMD_INDEX_NONE) {
+        auto_yaw.set_mode_to_default(false);
-        if (mission.read_cmd_from_storage(prev_cmd_idx, temp_cmd)) {
-            if ((temp_cmd.id == MAV_CMD_NAV_WAYPOINT || temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) && temp_cmd.p1 == 0) {
-                stopped_at_start = false;
-            }
     }
 }
 
+// get_spline_from_cmd - Calculates the spline type for a given spline waypoint mission command
+void ModeAuto::get_spline_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc, Location& dest_loc, Location& next_dest_loc, bool& next_dest_loc_is_spline)
+{
+    dest_loc = loc_from_cmd(cmd, default_loc);
+
     // if there is no delay at the end of this segment get next nav command
-    Location next_loc;
+    AP_Mission::Mission_Command temp_cmd;
     if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
-        next_loc = temp_cmd.content.location;
+        next_dest_loc = loc_from_cmd(temp_cmd, dest_loc);
-        // default lat, lon to first waypoint's lat, lon
+        next_dest_loc_is_spline = temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT;
-        if (next_loc.lat == 0 && next_loc.lng == 0) {
-            next_loc.lat = target_loc.lat;
-            next_loc.lng = target_loc.lng;
-        }
-        // default alt to first waypoint's alt but in next waypoint's alt frame
-        if (next_loc.alt == 0) {
-            int32_t next_alt;
-            if (target_loc.get_alt_cm(next_loc.get_alt_frame(), next_alt)) {
-                next_loc.set_alt_cm(next_alt, next_loc.get_alt_frame());
     } else {
-                // default to first waypoints altitude
+        next_dest_loc = dest_loc;
-                next_loc.set_alt_cm(target_loc.alt, target_loc.get_alt_frame());
+        next_dest_loc_is_spline = false;
     }
 }
-        // if the next nav command is a waypoint set end type to spline or straight
-        if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT) {
-            seg_end_type = AC_WPNav::SEGMENT_END_STRAIGHT;
-        } else if (temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) {
-            seg_end_type = AC_WPNav::SEGMENT_END_SPLINE;
-        }
-    }
-
-    // set spline navigation target
-    spline_start(target_loc, stopped_at_start, seg_end_type, next_loc);
-}
 
 #if NAV_GUIDED == ENABLED
 // do_nav_guided_enable - initiate accepting commands from external nav computer

ArduCopter/mode_guided.cpp

@@ -116,7 +116,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
     }
     target_loc.set_alt_cm(takeoff_alt_cm, frame);
 
-    if (!wp_nav->set_wp_destination(target_loc)) {
+    if (!wp_nav->set_wp_destination_loc(target_loc)) {
         // failure to set destination can only be because of missing terrain data
         AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
         // failure is propagated to GCS with NAK
@@ -296,7 +296,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
         pos_control_start();
     }
 
-    if (!wp_nav->set_wp_destination(dest_loc)) {
+    if (!wp_nav->set_wp_destination_loc(dest_loc)) {
         // failure to set destination can only be because of missing terrain data
         AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
         // failure is propagated to GCS with NAK

ArduCopter/mode_rtl.cpp

@@ -123,14 +123,13 @@ void ModeRTL::climb_start()
     }
 
     // set the destination
-    if (!wp_nav->set_wp_destination(rtl_path.climb_target)) {
+    if (!wp_nav->set_wp_destination_loc(rtl_path.climb_target) || !wp_nav->set_wp_destination_next_loc(rtl_path.return_target)) {
         // this should not happen because rtl_build_path will have checked terrain data was available
         gcs().send_text(MAV_SEVERITY_CRITICAL,"RTL: unexpected error setting climb target");
         AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
         copter.set_mode(Mode::Number::LAND, ModeReason::TERRAIN_FAILSAFE);
         return;
     }
-    wp_nav->set_fast_waypoint(true);
 
     // hold current yaw during initial climb
     auto_yaw.set_mode(AUTO_YAW_HOLD);
@@ -142,7 +141,7 @@ void ModeRTL::return_start()
     _state = RTL_ReturnHome;
     _state_complete = false;
 
-    if (!wp_nav->set_wp_destination(rtl_path.return_target)) {
+    if (!wp_nav->set_wp_destination_loc(rtl_path.return_target)) {
         // failure must be caused by missing terrain data, restart RTL
         restart_without_terrain();
     }

ArduCopter/mode_smart_rtl.cpp

@@ -92,24 +92,26 @@ 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 next_point;
+        Vector3f dest_NED;
         // this pop_point can fail if the IO task currently has the
         // path semaphore.
-        if (g2.smart_rtl.pop_point(next_point)) {
+        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
-                next_point.z -= 2.0f;
+                dest_NED.z -= 2.0f;
                 smart_rtl_state = SmartRTL_PreLandPosition;
-                wp_nav->set_wp_destination_NED(next_point);
+                wp_nav->set_wp_destination_NED(dest_NED);
             } else {
                 // peek at the next point
-                Vector3f next_next_point;
+                Vector3f next_dest_NED;
-                if (g2.smart_rtl.peek_point(next_next_point)) {
+                if (g2.smart_rtl.peek_point(next_dest_NED)) {
-                    wp_nav->set_wp_destination_NED(next_point, next_next_point);
+                    wp_nav->set_wp_destination_NED(dest_NED);
+                    wp_nav->set_wp_destination_next_NED(next_dest_NED);
                 } else {
                     // this should never happen but send next point anyway
-                    wp_nav->set_wp_destination_NED(next_point);
+                    wp_nav->set_wp_destination_NED(dest_NED);
+                    INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
                 }
             }
         } else if (g2.smart_rtl.get_num_points() == 0) {
@@ -181,7 +183,7 @@ bool ModeSmartRTL::get_wp(Location& destination)
     case SmartRTL_PathFollow:
     case SmartRTL_PreLandPosition:
     case SmartRTL_Descend:
-        return wp_nav->get_wp_destination(destination);
+        return wp_nav->get_wp_destination_loc(destination);
     case SmartRTL_Land:
         return false;
     }

ArduCopter/tuning.cpp

@@ -72,7 +72,7 @@ void Copter::tuning()
         break;
 
     case TUNING_THROTTLE_RATE_KP:
-        pos_control->get_vel_z_p().kP(tuning_value);
+        pos_control->get_vel_z_pid().kP(tuning_value);
         break;
 
     case TUNING_ACCEL_Z_KP: