From 551f7d66c1966a3ba45e3f7f228720c2280636ac Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 1 Aug 2020 10:50:24 +0900 Subject: [PATCH] AR_WPNav: update crosstrack error during pivot turns --- libraries/AR_WPNav/AR_WPNav.cpp | 27 ++++++++++++++++++++++++++- libraries/AR_WPNav/AR_WPNav.h | 3 +++ 2 files changed, 29 insertions(+), 1 deletion(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index d4776a2f0e..9bd02f72a7 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -361,7 +361,7 @@ void AR_WPNav::update_steering(const Location& current_loc, float current_speed) { // calculate desired turn rate and update desired heading if (_pivot_active) { - _cross_track_error = 0.0f; + _cross_track_error = calc_crosstrack_error(current_loc); _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(_desired_heading_cd * 0.01f), radians(_pivot_rate)); @@ -458,3 +458,28 @@ void AR_WPNav::apply_speed_min(float &desired_speed) desired_speed = is_negative(desired_speed) ? -speed_min : speed_min; } } + +// calculate the crosstrack error (does not rely on L1 controller) +float AR_WPNav::calc_crosstrack_error(const Location& current_loc) const +{ + if (!_orig_and_dest_valid) { + return 0.0f; + } + + // calculate the NE position of destination relative to origin + Vector2f dest_from_origin = _oa_origin.get_distance_NE(_oa_destination); + + // return distance to origin if length of track is very small + if (dest_from_origin.length() < 1.0e-6f) { + return current_loc.get_distance_NE(_oa_destination).length(); + } + + // convert to a vector indicating direction only + dest_from_origin.normalize(); + + // calculate the NE position of the vehicle relative to origin + const Vector2f veh_from_origin = _oa_origin.get_distance_NE(current_loc); + + // calculate distance to target track, for reporting + return veh_from_origin % dest_from_origin; +} diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index eaf9b12bef..2bf7c8338e 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -112,6 +112,9 @@ private: // adjust speed to ensure it does not fall below value held in SPEED_MIN void apply_speed_min(float &desired_speed); + // calculate the crosstrack error (does not rely on L1 controller) + float calc_crosstrack_error(const Location& current_loc) const; + private: // parameters