From d2e94a49b7be19c4d4ec7276fa67d6cc59a3b6eb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 18 Jun 2021 11:19:53 +1000 Subject: [PATCH] AC_WPNav: convert circle, loiter and WPNav to double position --- libraries/AC_WPNav/AC_Circle.cpp | 19 ++++++++++--------- libraries/AC_WPNav/AC_Circle.h | 8 ++++---- libraries/AC_WPNav/AC_Loiter.cpp | 9 +++++---- libraries/AC_WPNav/AC_WPNav.cpp | 18 +++++++++++------- libraries/AC_WPNav/AC_WPNav.h | 2 +- 5 files changed, 31 insertions(+), 25 deletions(-) diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index 72a07291e1..4c1beff7fe 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/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 diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index 5afe222daf..59d4b0acd7 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/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) diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 22fdf21d5a..7d426ec771 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/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); diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 020e66a0e3..c5057fc1d6 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/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) { diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index dbecae917a..f8afa706c8 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/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;