Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_WPNav: pivot only when destination set

Browse Source 
this avoids unhelpful pivots as the vehicle reaches the waypoint when WP_RADIUS is very small
This commit is contained in:
Randy Mackay 2020-07-31 14:02:40 +09:00
parent 98b953eb80
commit da746d6e8c
2 changed files with 33 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -24,6 +24,7 @@ extern const AP_HAL::HAL& hal;
#define AR_WPNAV_RADIUS_DEFAULT 2.0f
#define AR_WPNAV_OVERSHOOT_DEFAULT 2.0f
#define AR_WPNAV_PIVOT_ANGLE_DEFAULT 60
#define AR_WPNAV_PIVOT_ANGLE_ACCURACY 10 // vehicle will pivot to within this many degrees of destination
#define AR_WPNAV_PIVOT_RATE_DEFAULT 90
const AP_Param::GroupInfo AR_WPNav::var_info[] = {
@ -141,6 +142,11 @@ void AR_WPNav::update(float dt)
update_distance_and_bearing_to_destination();
// if object avoidance is active check if vehicle should pivot towards destination
if (_oa_active) {
update_pivot_active_flag();
}
// check if vehicle is in recovering state after switching off OA
if (!_oa_active && _oa_was_active) {
if (oa->get_options() & AP_OAPathPlanner::OA_OPTION_WP_RESET) {
@ -192,7 +198,10 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, float ne
_reached_destination = false;
update_distance_and_bearing_to_destination();
// set final desired speed
// determine if we should pivot immediately
update_pivot_active_flag();
// set final desired speed and whether vehicle should pivot
_desired_speed_final = 0.0f;
if (!is_equal(next_leg_bearing_cd, AR_WPNAV_HEADING_UNKNOWN)) {
const float curr_leg_bearing_cd = _origin.get_bearing_to(_destination);
@ -273,7 +282,7 @@ bool AR_WPNav::get_stopping_location(Location& stopping_loc)
bool AR_WPNav::use_pivot_steering_at_next_WP(float yaw_error_cd) const
{
// check cases where we clearly cannot use pivot steering
if (!_pivot_possible || _pivot_angle <= 0) {
if (!_pivot_possible || _pivot_angle <= AR_WPNAV_PIVOT_ANGLE_ACCURACY) {
return false;
}
@ -285,32 +294,32 @@ bool AR_WPNav::use_pivot_steering_at_next_WP(float yaw_error_cd) const
return false;
}
// returns true if vehicle should pivot immediately (because heading error is too large)
bool AR_WPNav::use_pivot_steering(float yaw_error_cd)
// updates _pivot_active flag based on heading error to destination
// relies on update_distance_and_bearing_to_destination having been called first
// to update _oa_wp_bearing and _reversed variables
void AR_WPNav::update_pivot_active_flag()
{
// check cases where we clearly cannot use pivot steering
if (!_pivot_possible || (_pivot_angle <= 0)) {
if (!_pivot_possible || (_pivot_angle <= AR_WPNAV_PIVOT_ANGLE_ACCURACY)) {
_pivot_active = false;
return false;
return;
}
// calc bearing error
const float yaw_error = fabsf(yaw_error_cd) * 0.01f;
// calc yaw error
const float yaw_cd = _reversed ? wrap_360_cd(_oa_wp_bearing_cd + 18000) : _oa_wp_bearing_cd;
const float yaw_error = fabsf(wrap_180_cd(yaw_cd - AP::ahrs().yaw_sensor)) * 0.01f;
// if error is larger than _pivot_angle start pivot steering
if (yaw_error > _pivot_angle) {
_pivot_active = true;
return true;
return;
}
// if within 10 degrees of the target heading, exit pivot steering
if (yaw_error < 10.0f) {
if (yaw_error < AR_WPNAV_PIVOT_ANGLE_ACCURACY) {
_pivot_active = false;
return false;
return;
}
// by default stay in
return _pivot_active;
}
// true if update has been called recently
@ -350,16 +359,15 @@ void AR_WPNav::update_distance_and_bearing_to_destination()
// relies on update_distance_and_bearing_to_destination being called first so _wp_bearing_cd has been updated
void AR_WPNav::update_steering(const Location& current_loc, float current_speed)
{
// calculate yaw error for determining if vehicle should pivot towards waypoint
float yaw_cd = _reversed ? wrap_360_cd(_oa_wp_bearing_cd + 18000) : _oa_wp_bearing_cd;
float yaw_error_cd = wrap_180_cd(yaw_cd - AP::ahrs().yaw_sensor);
// calculate desired turn rate and update desired heading
if (use_pivot_steering(yaw_error_cd)) {
if (_pivot_active) {
_cross_track_error = 0.0f;
_desired_heading_cd = yaw_cd;
_desired_heading_cd = _reversed ? wrap_360_cd(_oa_wp_bearing_cd + 18000) : _oa_wp_bearing_cd;;
_desired_lat_accel = 0.0f;
_desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(yaw_cd * 0.01f), radians(_pivot_rate));
_desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(_desired_heading_cd * 0.01f), radians(_pivot_rate));
// update flag so that it can be cleared
update_pivot_active_flag();
} else {
// run L1 controller
_nav_controller.set_reverse(_reversed);

6
libraries/AR_WPNav/AR_WPNav.h
View File

@ -104,8 +104,10 @@ private:
// returns true if vehicle should pivot turn at next waypoint
bool use_pivot_steering_at_next_WP(float yaw_error_cd) const;
// returns true if vehicle should pivot immediately (because heading error is too large)
bool use_pivot_steering(float yaw_error_cd);
// updates _pivot_active flag based on heading error to destination
// relies on update_distance_and_bearing_to_destination having been called first
// to update _oa_wp_bearing and _reversed variables
void update_pivot_active_flag();
// adjust speed to ensure it does not fall below value held in SPEED_MIN
void apply_speed_min(float &desired_speed);