From ac2ef4c5a211afbec06ef404636fd84334b7b6e0 Mon Sep 17 00:00:00 2001
From: Stephen Dade <stephen_dade@hotmail.com>
Date: Fri, 20 Dec 2024 16:34:30 +1100
Subject: [PATCH] Rover: Reduce min circle radius to 0.1m

---
 Rover/mode.h          |  3 +++
 Rover/mode_circle.cpp | 18 +++++++++++++-----
 2 files changed, 16 insertions(+), 5 deletions(-)

diff --git a/Rover/mode.h b/Rover/mode.h
index 6b7b579702..9aa9a2d27a 100644
--- a/Rover/mode.h
+++ b/Rover/mode.h
@@ -418,6 +418,9 @@ public:
     Number mode_number() const override { return Number::CIRCLE; }
     const char *name4() const override { return "CIRC"; }
 
+    // return the distance at which the vehicle is considered to be on track along the circle
+    float get_reached_distance() const;
+
     // initialise with specific center location, radius (in meters) and direction
     // replaces use of _enter when initialised from within Auto mode
     bool set_center(const Location& center_loc, float radius_m, bool dir_ccw);
diff --git a/Rover/mode_circle.cpp b/Rover/mode_circle.cpp
index c0a7686207..7f89c781ea 100644
--- a/Rover/mode_circle.cpp
+++ b/Rover/mode_circle.cpp
@@ -1,8 +1,7 @@
 #include "Rover.h"
 
 #define AR_CIRCLE_ACCEL_DEFAULT         1.0 // default acceleration in m/s/s if not specified by user
-#define AR_CIRCLE_RADIUS_MIN            0.5 // minimum radius in meters
-#define AR_CIRCLE_REACHED_EDGE_DIST     0.2 // vehicle has reached edge if within 0.2m
+#define AR_CIRCLE_RADIUS_MIN            0.1 // minimum radius in meters
 
 const AP_Param::GroupInfo ModeCircle::var_info[] = {
 
@@ -39,6 +38,15 @@ ModeCircle::ModeCircle() : Mode()
     AP_Param::setup_object_defaults(this, var_info);
 }
 
+// get the distance at which the vehicle is considered to be on track along the circle
+float ModeCircle::get_reached_distance() const
+{
+    if (config.radius >= 0.5) {
+        return 0.2;
+    }
+    return 0.01;
+}
+
 // initialise with specific center location, radius (in meters) and direction
 // replaces use of _enter when initialised from within Auto mode
 bool ModeCircle::set_center(const Location& center_loc, float radius_m, bool dir_ccw)
@@ -158,12 +166,12 @@ void ModeCircle::update()
     // Update depending on stage
     if (!reached_edge) {
         update_drive_to_radius();
-    } else if (dist_to_edge_m > 2 * MAX(g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST) && !tracking_back) {
+    } else if (dist_to_edge_m > 2 * MAX(g2.turn_radius, get_reached_distance()) && !tracking_back) {
         // if more than 2* turn_radius outside circle radius, slow vehicle by 20%
         config.speed = 0.8 * config.speed;
         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Circle: cannot keep up, slowing to %.1fm/s", config.speed);
         tracking_back = true;
-    } else if (dist_to_edge_m < MAX(g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST) && tracking_back) {
+    } else if (dist_to_edge_m < MAX(g2.turn_radius, get_reached_distance()) && tracking_back) {
         // if within turn_radius, call the vehicle back on track
         tracking_back = false;
     } else {
@@ -184,7 +192,7 @@ void ModeCircle::update()
 void ModeCircle::update_drive_to_radius()
 {
     // check if vehicle has reached edge of circle
-    const float dist_thresh_m = MAX(g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST);
+    const float dist_thresh_m = MAX(g2.turn_radius, get_reached_distance());
     reached_edge |= dist_to_edge_m <= dist_thresh_m;
 
     // calculate target point's position, velocity and acceleration