From 0199a6342740d5769b51990798a790b1272c97f0 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 27 Feb 2024 08:33:00 +0000 Subject: [PATCH] AP_L1_Control: disable loiter radius scaling in guided path navigation Signed-off-by: Rhys Mainwaring --- libraries/AP_L1_Control/AP_L1_Control.cpp | 7 +++++++ libraries/AP_L1_Control/AP_L1_Control.h | 3 +++ 2 files changed, 10 insertions(+) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index f49a786ed490d1..a2383e01eb4c69 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -147,6 +147,10 @@ float AP_L1_Control::turn_distance(float wp_radius, float turn_angle) const float AP_L1_Control::loiter_radius(const float radius) const { + if (_disable_loiter_radius_scaling) { + return radius; + } + // prevent an insane loiter bank limit float sanitized_bank_limit = constrain_float(_loiter_bank_limit, 0.0f, 89.0f); float lateral_accel_sea_level = tanf(radians(sanitized_bank_limit)) * GRAVITY_MSS; @@ -529,7 +533,10 @@ void AP_L1_Control::update_path(const class Location &position_on_path, Vector2f auto ofs_ned = dn_ned.cross(tangent_ned) * radius_m * direction; center_wp.offset(ofs_ned); + // disable loiter radius scaling while updating loiter + _disable_loiter_radius_scaling = true; update_loiter(center_wp, radius_m, direction); + _disable_loiter_radius_scaling = false; } else { // moving along a line segment - navigate to wp ahead of closest point // in direction of path tangent diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h index b47fc07b7a5cc6..7bf77efbf2c895 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.h +++ b/libraries/AP_L1_Control/AP_L1_Control.h @@ -129,4 +129,7 @@ class AP_L1_Control : public AP_Navigation { bool _reverse = false; float get_yaw() const; int32_t get_yaw_sensor() const; + + // allow loiter radius scaling to be disabled (in path guidance). + bool _disable_loiter_radius_scaling = false; };