aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_WPNav: cleanup position control APIs

Browse Source 
use Vector2 for xy, float for z
This commit is contained in:
Andrew Tridgell 2021-06-21 17:26:40 +10:00
parent fd68233fed
commit bf91168cd6
5 changed files with 19 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -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)) {

4
libraries/AC_WPNav/AC_Loiter.cpp
View File

@ -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);
}

4
libraries/AC_WPNav/AC_Loiter.h
View File

@ -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(); }

14
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -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

2
libraries/AC_WPNav/AC_WPNav.h
View File

@ -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