AC_WPNav: convert circle, loiter and WPNav to double position

libraries/AC_WPNav/AC_Circle.cpp

@@ -58,7 +58,7 @@ AC_Circle::AC_Circle(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_Po
 /// init - initialise circle controller setting center specifically
 ///     set terrain_alt to true if center.z should be interpreted as an alt-above-terrain
 ///     caller should set the position controller's x,y and z speeds and accelerations before calling this
-void AC_Circle::init(const Vector3f& center, bool terrain_alt)
+void AC_Circle::init(const Vector3p& center, bool terrain_alt)
 {
     _center = center;
     _terrain_alt = terrain_alt;
@@ -86,7 +86,7 @@ void AC_Circle::init()
     _pos_control.init_z_controller_stopping_point();
 
     // get stopping point
-    const Vector3f& stopping_point = _pos_control.get_pos_target_cm();
+    const Vector3p& stopping_point = _pos_control.get_pos_target_cm();
 
     // set circle center to circle_radius ahead of stopping point
     _center = stopping_point;
@@ -189,7 +189,7 @@ bool AC_Circle::update(float climb_rate_cms)
     }
 
     // if the circle_radius is zero we are doing panorama so no need to update loiter target
-    Vector3f target {
+    Vector3p target {
         _center.x,
         _center.y,
         target_z_cm
@@ -200,7 +200,7 @@ bool AC_Circle::update(float climb_rate_cms)
         target.y += - _radius * sinf(-_angle);
 
         // heading is from vehicle to center of circle
-        _yaw = get_bearing_cd(_inav.get_position(), _center);
+        _yaw = get_bearing_cd(_inav.get_position(), _center.tofloat());
 
         if ((_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0) {
             _yaw += is_positive(_rate)?-9000.0f:9000.0f;
@@ -217,7 +217,8 @@ bool AC_Circle::update(float climb_rate_cms)
     _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);
+        float target_zf = target.z;
+        _pos_control.input_pos_vel_accel_z(target_zf, zero2, 0);
     } else {
         _pos_control.set_pos_target_z_from_climb_rate_cm(climb_rate_cms,  false);
     }
@@ -240,16 +241,16 @@ void AC_Circle::get_closest_point_on_circle(Vector3f &result) const
 {
     // return center if radius is zero
     if (_radius <= 0) {
-        result = _center;
+        result = _center.tofloat();
         return;
     }
 
     // get current position
-    Vector2f stopping_point;
+    Vector2p stopping_point;
     _pos_control.get_stopping_point_xy_cm(stopping_point);
 
     // calc vector from stopping point to circle center
-    Vector2f vec = stopping_point - _center.xy();
+    Vector2f vec = (stopping_point - _center.xy()).tofloat();
     float dist = vec.length();
 
     // if current location is exactly at the center of the circle return edge directly behind vehicle
@@ -313,7 +314,7 @@ void AC_Circle::init_start_angle(bool use_heading)
     } else {
         // if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading)
         const Vector3f &curr_pos = _inav.get_position();
-        if (is_equal(curr_pos.x,_center.x) && is_equal(curr_pos.y,_center.y)) {
+        if (is_equal(curr_pos.x,float(_center.x)) && is_equal(curr_pos.y,float(_center.y))) {
             _angle = wrap_PI(_ahrs.yaw-M_PI);
         } else {
             // get bearing from circle center to vehicle in radians

libraries/AC_WPNav/AC_Circle.h

@@ -22,7 +22,7 @@ public:
     /// init - initialise circle controller setting center specifically
     ///     set terrain_alt to true if center.z should be interpreted as an alt-above-terrain
     ///     caller should set the position controller's x,y and z speeds and accelerations before calling this
-    void init(const Vector3f& center, bool terrain_alt);
+    void init(const Vector3p& center, bool terrain_alt);
 
     /// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading
     ///     caller should set the position controller's x,y and z speeds and accelerations before calling this
@@ -33,10 +33,10 @@ public:
 
     /// set_circle_center as a vector from ekf origin
     ///     terrain_alt should be true if center.z is alt is above terrain
-    void set_center(const Vector3f& center, bool terrain_alt) { _center = center; _terrain_alt = terrain_alt; }
+    void set_center(const Vector3f& center, bool terrain_alt) { _center = center.topostype(); _terrain_alt = terrain_alt; }
 
     /// get_circle_center in cm from home
-    const Vector3f& get_center() const { return _center; }
+    const Vector3p& get_center() const { return _center; }
 
     /// returns true if using terrain altitudes
     bool center_is_terrain_alt() const { return _terrain_alt; }
@@ -143,7 +143,7 @@ private:
     AP_Int16    _options;       // stick control enable/disable
 
     // internal variables
-    Vector3f    _center;        // center of circle in cm from home
+    Vector3p    _center;        // center of circle in cm from home
     float       _radius;        // radius of circle in cm
     float       _yaw;           // yaw heading (normally towards circle center)
     float       _angle;         // current angular position around circle in radians (0=directly north of the center of the circle)

libraries/AC_WPNav/AC_Loiter.cpp

@@ -174,7 +174,9 @@ 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(Vector2f& stopping_point) const
 {
-    _pos_control.get_stopping_point_xy_cm(stopping_point);
+    Vector2p stop;
+    _pos_control.get_stopping_point_xy_cm(stop);
+    stopping_point = stop.tofloat();
 }
 
 /// get maximum lean angle when using loiter
@@ -287,11 +289,10 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
     }
 
     // get loiters desired velocity from the position controller where it is being stored.
-    const Vector3f &target_pos_3d = _pos_control.get_pos_target_cm();
-    Vector2f target_pos{target_pos_3d.x, target_pos_3d.y};
+    Vector2p target_pos = _pos_control.get_pos_target_cm().xy();
 
     // update the target position using our predicted velocity
-    target_pos += desired_vel * nav_dt;
+    target_pos += (desired_vel * nav_dt).topostype();
 
     // send adjusted feed forward acceleration and velocity back to the Position Controller
     _pos_control.set_pos_vel_accel_xy(target_pos, desired_vel, _desired_accel);

libraries/AC_WPNav/AC_WPNav.cpp

@@ -398,14 +398,18 @@ void AC_WPNav::shift_wp_origin_and_destination_to_stopping_point_xy()
 /// 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(Vector2f& stopping_point) const
 {
-	_pos_control.get_stopping_point_xy_cm(stopping_point);
+    Vector2p stop;
+    _pos_control.get_stopping_point_xy_cm(stop);
+    stopping_point = stop.tofloat();
 }
 
 /// 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.xy());
-    _pos_control.get_stopping_point_z_cm(stopping_point.z);
+    Vector3p stop;
+    _pos_control.get_stopping_point_xy_cm(stop.xy());
+    _pos_control.get_stopping_point_z_cm(stop.z);
+    stopping_point = stop.tofloat();
 }
 
 /// advance_wp_target_along_track - move target location along track from origin to destination
@@ -463,9 +467,9 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
 
     // input shape the terrain offset
     shape_pos_vel_accel(terr_offset, 0.0f, 0.0f,
-        _pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset,
-        0.0f, _pos_control.get_max_speed_down_cms(), _pos_control.get_max_speed_up_cms(),
-        -_pos_control.get_max_accel_z_cmss(), _pos_control.get_max_accel_z_cmss(), 0.05f, dt);
+                        _pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset,
+                        0.0f, _pos_control.get_max_speed_down_cms(), _pos_control.get_max_speed_up_cms(),
+                        -_pos_control.get_max_accel_z_cmss(), _pos_control.get_max_accel_z_cmss(), 0.05f, dt);
 
     update_pos_vel_accel(_pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset, dt, 0.0f);
 
@@ -475,7 +479,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
     target_accel.z += _accel_terrain_offset;
 
     // pass new target to the position controller
-    _pos_control.set_pos_vel_accel(target_pos, target_vel, target_accel);
+    _pos_control.set_pos_vel_accel(target_pos.topostype(), target_vel, target_accel);
 
     // check if we've reached the waypoint
     if (!_flags.reached_destination) {

libraries/AC_WPNav/AC_WPNav.h

@@ -274,7 +274,7 @@ protected:
     float       _rangefinder_alt_cm;    // latest distance from the rangefinder
 
     // position, velocity and acceleration targets passed to position controller
-    float       _pos_terrain_offset;
+    postype_t   _pos_terrain_offset;
     float       _vel_terrain_offset;
     float       _accel_terrain_offset;