AC_WPNav: cleanup position control APIs

use Vector2 for xy, float for z

libraries/AC_WPNav/AC_Circle.cpp

@@ -213,10 +213,11 @@ bool AC_Circle::update(float climb_rate_cms)
     }
 
     // update position controller target
-    Vector3f zero;
-    _pos_control.input_pos_vel_accel_xy(target, zero, zero);
-    if(_terrain_alt) {
-        _pos_control.input_pos_vel_accel_z(target, zero, zero);
+    Vector2f zero;
+    _pos_control.input_pos_vel_accel_xy(target.xy(), zero, zero);
+    if (_terrain_alt) {
+        float zero2 = 0;
+        _pos_control.input_pos_vel_accel_z(target.z, zero2, 0);
     } else {
         _pos_control.set_pos_target_z_from_climb_rate_cm(climb_rate_cms,  false);
     }
@@ -244,14 +245,12 @@ void AC_Circle::get_closest_point_on_circle(Vector3f &result) const
     }
 
     // get current position
-    Vector3f stopping_point;
+    Vector2f stopping_point;
     _pos_control.get_stopping_point_xy_cm(stopping_point);
 
     // calc vector from stopping point to circle center
-    Vector2f vec;   // vector from circle center to current location
-    vec.x = (stopping_point.x - _center.x);
-    vec.y = (stopping_point.y - _center.y);
-    float dist = norm(vec.x, vec.y);
+    Vector2f vec = stopping_point - _center.xy();
+    float dist = vec.length();
 
     // if current location is exactly at the center of the circle return edge directly behind vehicle
     if (is_zero(dist)) {

libraries/AC_WPNav/AC_Loiter.cpp

@@ -86,7 +86,7 @@ AC_Loiter::AC_Loiter(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_Po
 }
 
 /// init_target to a position in cm from ekf origin
-void AC_Loiter::init_target(const Vector3f& position)
+void AC_Loiter::init_target(const Vector2f& position)
 {
     sanity_check_params();
 
@@ -172,7 +172,7 @@ void AC_Loiter::set_pilot_desired_acceleration(float euler_roll_angle_cd, float
 }
 
 /// get vector to stopping point based on a horizontal position and velocity
-void AC_Loiter::get_stopping_point_xy(Vector3f& stopping_point) const
+void AC_Loiter::get_stopping_point_xy(Vector2f& stopping_point) const
 {
     _pos_control.get_stopping_point_xy_cm(stopping_point);
 }

libraries/AC_WPNav/AC_Loiter.h

@@ -17,7 +17,7 @@ public:
     AC_Loiter(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control);
 
     /// init_target to a position in cm from ekf origin
-    void init_target(const Vector3f& position);
+    void init_target(const Vector2f& position);
 
     /// initialize's position and feed-forward velocity from current pos and velocity
     void init_target();
@@ -36,7 +36,7 @@ public:
     void clear_pilot_desired_acceleration() { _desired_accel.zero(); }
 
     /// get vector to stopping point based on a horizontal position and velocity
-    void get_stopping_point_xy(Vector3f& stopping_point) const;
+    void get_stopping_point_xy(Vector2f& stopping_point) const;
 
     /// get horizontal distance to loiter target in cm
     float get_distance_to_target() const { return _pos_control.get_pos_error_xy_cm(); }

libraries/AC_WPNav/AC_WPNav.cpp

@@ -384,21 +384,19 @@ void AC_WPNav::shift_wp_origin_and_destination_to_stopping_point_xy()
     _pos_control.relax_velocity_controller_xy();
 
     // get current and target locations
-    Vector3f stopping_point;
+    Vector2f stopping_point;
     get_wp_stopping_point_xy(stopping_point);
 
     // shift origin and destination horizontally
-    _origin.x = stopping_point.x;
-    _origin.y = stopping_point.y;
-    _destination.x = stopping_point.x;
-    _destination.y = stopping_point.y;
+    _origin.xy() = stopping_point;
+    _destination.xy() = stopping_point;
 
     // move pos controller target horizontally
     _pos_control.set_pos_target_xy_cm(stopping_point.x, stopping_point.y);
 }
 
 /// get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
-void AC_WPNav::get_wp_stopping_point_xy(Vector3f& stopping_point) const
+void AC_WPNav::get_wp_stopping_point_xy(Vector2f& stopping_point) const
 {
 	_pos_control.get_stopping_point_xy_cm(stopping_point);
 }
@@ -406,8 +404,8 @@ void AC_WPNav::get_wp_stopping_point_xy(Vector3f& stopping_point) const
 /// get_wp_stopping_point - returns vector to stopping point based on 3D position and velocity
 void AC_WPNav::get_wp_stopping_point(Vector3f& stopping_point) const
 {
-    _pos_control.get_stopping_point_xy_cm(stopping_point);
-    _pos_control.get_stopping_point_z_cm(stopping_point);
+    _pos_control.get_stopping_point_xy_cm(stopping_point.xy());
+    _pos_control.get_stopping_point_z_cm(stopping_point.z);
 }
 
 /// advance_wp_target_along_track - move target location along track from origin to destination

libraries/AC_WPNav/AC_WPNav.h

@@ -128,7 +128,7 @@ public:
 
     /// get_wp_stopping_point_xy - calculates stopping point based on current position, velocity, waypoint acceleration
     ///		results placed in stopping_position vector
-    void get_wp_stopping_point_xy(Vector3f& stopping_point) const;
+    void get_wp_stopping_point_xy(Vector2f& stopping_point) const;
     void get_wp_stopping_point(Vector3f& stopping_point) const;
 
     /// get_wp_distance_to_destination - get horizontal distance to destination in cm