Sub: use cleaned up APIs

ArduSub/commands_logic.cpp

@@ -299,7 +299,7 @@ void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
     if (target_loc.lat == 0 && target_loc.lng == 0) {
         // To-Do: make this simpler
         Vector3f temp_pos;
-        wp_nav.get_wp_stopping_point_xy(temp_pos);
+        wp_nav.get_wp_stopping_point_xy(temp_pos.xy());
         const Location temp_loc(temp_pos, Location::AltFrame::ABOVE_ORIGIN);
         target_loc.lat = temp_loc.lat;
         target_loc.lng = temp_loc.lng;

ArduSub/control_guided.cpp

@@ -217,8 +217,8 @@ bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vecto
     posvel_pos_target_cm = destination;
     posvel_vel_target_cms = velocity;
 
-    pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm, posvel_vel_target_cms, Vector3f());
-    pos_control.input_pos_vel_accel_z(posvel_pos_target_cm, posvel_vel_target_cms, Vector3f());
+    pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());
+    pos_control.input_pos_vel_accel_z(posvel_pos_target_cm.z, posvel_vel_target_cms.z, 0);
 
     // log target
     Log_Write_GuidedTarget(guided_mode, destination, velocity);
@@ -421,8 +421,8 @@ void Sub::guided_posvel_control_run()
     posvel_pos_target_cm += posvel_vel_target_cms * pos_control.get_dt();
 
     // send position and velocity targets to position controller
-    pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm, posvel_vel_target_cms, Vector3f());
-    pos_control.input_pos_vel_accel_z(posvel_pos_target_cm, posvel_vel_target_cms, Vector3f());
+    pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());
+    pos_control.input_pos_vel_accel_z(posvel_pos_target_cm.z, posvel_vel_target_cms.z, 0);
 
     // run position controller
     pos_control.update_xy_controller();