diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 86e93ba845..e65a82bb79 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -171,13 +171,6 @@ class PathPlanner(): y_pts = np.interp(self.t_idxs[:MPC_N+1], np.linalg.norm(d_path_xyz, axis=1)/v_ego, d_path_xyz[:,1]) heading_pts = np.interp(self.t_idxs[:MPC_N+1], np.linalg.norm(self.path_xyz, axis=1)/v_ego, self.plan_yaw) - # init state - self.cur_state.x = 0.0 - self.cur_state.y = 0.0 - self.cur_state.psi = 0.0 - # TODO negative sign, still run mpc in ENU, make NED - self.cur_state.delta = -math.radians(angle_steers - angle_offset) / VM.sR - v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed v_poly = [0.0, 0.0, 0.0, v_ego_mpc] assert len(v_poly) == 4 @@ -189,6 +182,11 @@ class PathPlanner(): CAR_ROTATION_RADIUS, list(y_pts), list(heading_pts)) + # init state for next + self.cur_state.x = 0.0 + self.cur_state.y = 0.0 + self.cur_state.psi = 0.0 + self.cur_state.delta = interp(DT_MDL, self.t_idxs[:MPC_N+1], self.mpc_solution.delta) # TODO this needs more thought, use .2s extra for now to estimate other delays delay = CP.steerActuatorDelay + .2 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 82eed1a1b7..665fe8b7fa 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -61b7527b95d7a573a8c11ff8c260a4f01cc15113 \ No newline at end of file +57d9562d7e6103746090570c6be200c54296c95f \ No newline at end of file