Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: Use Pos_Control Heading

This commit is contained in:
Leonard Hall 2021-04-23 10:47:17 +09:30 committed by Randy Mackay
parent 4158c37cfa
commit 361ba989bd
2 changed files with 2 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -520,26 +520,6 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
}
}
// Calculate the turn rate
float turn_rate = 0.0f;
const float target_vel_xy_len = Vector2f(target_vel.x, target_vel.y).length();
if (is_positive(target_vel_xy_len)) {
const float accel_forward = (target_accel.x * target_vel.x + target_accel.y * target_vel.y + target_accel.z * target_vel.z)/target_vel.length();
const Vector3f accel_turn = target_accel - target_vel * accel_forward / target_vel.length();
const float accel_turn_xy_len = Vector2f(accel_turn.x, accel_turn.y).length();
turn_rate = accel_turn_xy_len / target_vel_xy_len;
if ((accel_turn.y * target_vel.x - accel_turn.x * target_vel.y) < 0.0) {
turn_rate = -turn_rate;
}
}
// update the target yaw if origin and destination are at least 2m apart horizontally
const Vector2f target_vel_xy(target_vel.x, target_vel.y);
if (target_vel_xy.length() > WPNAV_YAW_VEL_MIN) {
set_yaw_cd(degrees(target_vel_xy.angle()) * 100.0f);
set_yaw_rate_cds(turn_rate*degrees(100.0f));
}
// successfully advanced along track
return true;
}
@ -610,41 +590,6 @@ bool AC_WPNav::is_active() const
return (AP_HAL::millis() - _wp_last_update) < 200;
}
// returns target yaw in centi-degrees (used for wp and spline navigation)
float AC_WPNav::get_yaw() const
{
if (_flags.wp_yaw_set) {
return _yaw;
} else {
// if yaw has not been set return attitude controller's current target
return _attitude_control.get_att_target_euler_cd().z;
}
}
// returns target yaw rate in centi-degrees / second (used for wp and spline navigation)
float AC_WPNav::get_yaw_rate_cds() const
{
if (_flags.wp_yaw_set) {
return _yaw_rate_cds;
}
// if yaw has not been set return zero turn rate
return 0.0f;
}
// set heading used for spline and waypoint navigation
void AC_WPNav::set_yaw_cd(float heading_cd)
{
_yaw = heading_cd;
_flags.wp_yaw_set = true;
}
// set yaw rate used for spline and waypoint navigation
void AC_WPNav::set_yaw_rate_cds(float yaw_rate_cds)
{
_yaw_rate_cds = yaw_rate_cds;
}
// get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude)
bool AC_WPNav::get_terrain_offset(float& offset_cm)
{

11
libraries/AC_WPNav/AC_WPNav.h
View File

@ -23,7 +23,6 @@
#define WPNAV_WP_SPEED_DOWN 150.0f // default maximum descent velocity
#define WPNAV_WP_ACCEL_Z_DEFAULT 100.0f // default vertical acceleration between waypoints in cm/s/s
#define WPNAV_YAW_VEL_MIN 10 // target velocity must be at least 10cm/s for vehicle's yaw to change
class AC_WPNav
{
@ -162,8 +161,8 @@ public:
///
// get target yaw in centi-degrees (used for wp and spline navigation)
float get_yaw() const;
float get_yaw_rate_cds() const;
float get_yaw() const { return _pos_control.get_yaw_cd(); }
float get_yaw_rate_cds() const { return _pos_control.get_yaw_rate_cds(); }
/// set_spline_destination waypoint using location class
/// returns false if conversion from location to vector from ekf origin cannot be calculated
@ -229,10 +228,6 @@ protected:
// returns false if conversion failed (likely because terrain data was not available)
bool get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_alt);
// set heading used for spline and waypoint navigation
void set_yaw_cd(float heading_cd);
void set_yaw_rate_cds(float yaw_rate_cds);
// helper function to calculate scurve jerk and jerk_time values
// updates _scurve_jerk and _scurve_jerk_time
void calc_scurve_jerk_and_jerk_time();
@ -275,8 +270,6 @@ protected:
Vector3f _destination; // target destination in cm from ekf origin
float _track_error_xy; // horizontal error of the actual position vs the desired position
float _track_scalar_dt; // time compression multiplier to slow the progress along the track
float _yaw; // current yaw heading in centi-degrees based on track direction
float _yaw_rate_cds; // current yaw rate in centi-degrees/second based on track curvature
// terrain following variables
bool _terrain_alt; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin