Plane: disable loiter radius scaling in guided path navigation

Signed-off-by: Rhys Mainwaring <[email protected]>
ArduPilot · Mar 5, 2024 · 64bba35 · 64bba35
1 parent 3001c0f
commit 64bba35
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 0 deletions.
diff --git 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
@@ -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;
 };