From 52d20cedad5ffa9aa4b3ccadf30fb16d9399e18e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Jun 2021 17:26:40 +1000 Subject: [PATCH] AC_WPNav: cleanup position control APIs use Vector2 for xy, float for z --- libraries/AC_WPNav/AC_Circle.cpp | 17 ++++++++--------- libraries/AC_WPNav/AC_Loiter.cpp | 4 ++-- libraries/AC_WPNav/AC_Loiter.h | 4 ++-- libraries/AC_WPNav/AC_WPNav.cpp | 14 ++++++-------- libraries/AC_WPNav/AC_WPNav.h | 2 +- 5 files changed, 19 insertions(+), 22 deletions(-) diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index d5237b2385..72a07291e1 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/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)) { diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 9337c545e8..22fdf21d5a 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/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); } diff --git a/libraries/AC_WPNav/AC_Loiter.h b/libraries/AC_WPNav/AC_Loiter.h index 5d5861d58d..9418c3a136 100644 --- a/libraries/AC_WPNav/AC_Loiter.h +++ b/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(); } diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index b44ec8543a..020e66a0e3 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/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 diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 8edaba537a..dbecae917a 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/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