From da746d6e8c22a237bb2f8b900b32e2cfa606a645 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 31 Jul 2020 14:02:40 +0900 Subject: [PATCH] AR_WPNav: pivot only when destination set this avoids unhelpful pivots as the vehicle reaches the waypoint when WP_RADIUS is very small --- libraries/AR_WPNav/AR_WPNav.cpp | 50 +++++++++++++++++++-------------- libraries/AR_WPNav/AR_WPNav.h | 6 ++-- 2 files changed, 33 insertions(+), 23 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index f9d90eebdb..d4776a2f0e 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -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); diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index a84838daef..eaf9b12bef 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -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);