Rover: stop circle mode if unable to track

ArduPilot · Sep 2, 2024 · f7234ef · f7234ef
1 parent 94336a5
commit f7234ef
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/Rover/mode_circle.cpp b/Rover/mode_circle.cpp
@@ -155,7 +155,11 @@ void ModeCircle::update()
     // Update depending on stage
     if (!reached_edge) {
         update_drive_to_radius();
-
+    } else if (dist_to_edge_m > config.radius * 0.2) {
+        // if more than 20% inside or outside circle radius, stop vehicle
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Circle: vehicle unable to turn fast enough to track circle. Stopping vehicle.");
+        stop_vehicle();
+        return;
     } else {
         update_circling();
     }