Divide by 0 bug fix lateral planner (#25995)

* Divide by speed correctly

* Update

* Update lateral_planner.py

* Update ref_commit

selfdrive/controls/lib/lateral_planner.py

@@ -75,10 +75,10 @@ class LateralPlanner:
                      y_pts,
                      heading_pts,
                      yaw_rate_pts)
-    # init state for next
-    # mpc.u_sol is the desired curvature rate given x0 curv state.
-    # with x0[3] = measured_curvature, this would be the actual desired rate.
-    # instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control.
+    # init state for next iteration
+    # mpc.u_sol is the desired second derivative of psi given x0 curv state.
+    # with x0[3] = measured_yaw_rate, this would be the actual desired yaw rate.
+    # instead, interpolate x_sol so that x0[3] is the desired yaw rate for lat_control.
     self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])
 
     #  Check for infeasible MPC solution
@@ -105,8 +105,11 @@ class LateralPlanner:
     lateralPlan.modelMonoTime = sm.logMonoTime['modelV2']
     lateralPlan.dPathPoints = self.y_pts.tolist()
     lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
-    lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/sm['carState'].vEgo).tolist()
-    lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
+
+    # clip speed for curv calculation at 1m/s, to prevent low speed extremes
+    clipped_speed = max(1.0, sm['carState'].vEgo)
+    lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/clipped_speed).tolist()
+    lateralPlan.curvatureRates = [float(x/clipped_speed) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
 
     lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
     lateralPlan.solverExecutionTime = self.lat_mpc.solve_time

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-e0ffcae8def2fd9c82c547d1f257d4f06a48a3c3
+f9536e41a6a160bdaa29d42bb164b0e4033857e5