fixed zero speed division

ShisatoYano · Jan 24, 2025 · 96520ce · 96520ce
1 parent 02f5e12
commit 96520ce
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/src/components/control/stanley/stanley_controller.py b/src/components/control/stanley/stanley_controller.py
@@ -104,7 +104,10 @@ def _calculate_control_input(self, state, error_lat_m, error_yaw_rad):
 
         # calculate steering angle input
         curr_spd = state.get_speed_mps()
-        error_steer_rad = atan2(self.CONTROL_GAIN * error_lat_m, curr_spd)
+        if abs(curr_spd) != 0.0:
+            error_steer_rad = atan2(self.CONTROL_GAIN * error_lat_m, curr_spd)
+        else:
+            error_steer_rad = 0.0
         self.target_steer_rad = -1 * (error_steer_rad + error_yaw_rad)
 
         # calculate yaw rate input