From a3548b6a21c471573a9fbb5848d661f1388eef67 Mon Sep 17 00:00:00 2001
From: Stephen Dade <stephen_dade@hotmail.com>
Date: Mon, 2 Sep 2024 19:21:55 +1000
Subject: [PATCH] Rover: slow vehicle if unable to track in circle mode

---
 Rover/mode.h          |  1 +
 Rover/mode_circle.cpp | 28 +++++++++++++++++++++-------
 2 files changed, 22 insertions(+), 7 deletions(-)

diff --git a/Rover/mode.h b/Rover/mode.h
index c338d8cea2..6b7b579702 100644
--- a/Rover/mode.h
+++ b/Rover/mode.h
@@ -499,6 +499,7 @@ protected:
     float angle_total_rad;  // total angle in radians that vehicle has circled
     bool reached_edge;      // true once vehicle has reached edge of circle
     float dist_to_edge_m;   // distance to edge of circle in meters (equivalent to crosstrack error)
+    bool tracking_back;     // true if the vehicle is trying to track back onto the circle
 };
 
 class ModeGuided : public Mode
diff --git a/Rover/mode_circle.cpp b/Rover/mode_circle.cpp
index 1635bf099d..c0a7686207 100644
--- a/Rover/mode_circle.cpp
+++ b/Rover/mode_circle.cpp
@@ -91,6 +91,9 @@ bool ModeCircle::_enter()
     // check speed around circle does not lead to excessive lateral acceleration
     check_config_speed();
 
+    // reset tracking_back 
+    tracking_back = false;
+
     // calculate speed, accel and jerk limits
     // otherwise the vehicle uses wp_nav default speed limit
     float atc_accel_max = MIN(g2.attitude_control.get_accel_max(), g2.attitude_control.get_decel_max());
@@ -155,7 +158,14 @@ 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) {
+        // 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) {
+        // if within turn_radius, call the vehicle back on track
+        tracking_back = false;
     } else {
         update_circling();
     }
@@ -193,12 +203,16 @@ void ModeCircle::update_circling()
     const float accel_fb = constrain_float(config.speed - target.speed, -speed_change_max, speed_change_max);
     target.speed += accel_fb;
 
-    // calculate angular rate and update target angle
-    const float circumference = 2.0 * M_PI * config.radius;
-    const float angular_rate_rad = (target.speed / circumference) * M_2PI * (config.dir == Direction::CW ? 1.0 : -1.0);
-    const float angle_dt = angular_rate_rad * rover.G_Dt;
-    target.yaw_rad = wrap_PI(target.yaw_rad + angle_dt);
-    angle_total_rad += angle_dt;
+    // calculate angular rate and update target angle, if the vehicle is not trying to track back
+    if (!tracking_back) {
+        const float circumference = 2.0 * M_PI * config.radius;
+        const float angular_rate_rad = (target.speed / circumference) * M_2PI * (config.dir == Direction::CW ? 1.0 : -1.0);
+        const float angle_dt = angular_rate_rad * rover.G_Dt;
+        target.yaw_rad = wrap_PI(target.yaw_rad + angle_dt);
+        angle_total_rad += angle_dt;
+    } else {
+        init_target_yaw_rad();
+    }
 
     // calculate target point's position, velocity and acceleration
     target.pos = config.center_pos.topostype();