diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 48c98e7350..10e3edbebe 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/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 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 0e3cdf797f..78367235e6 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -e0ffcae8def2fd9c82c547d1f257d4f06a48a3c3 +f9536e41a6a160bdaa29d42bb164b0e4033857e5