diff --git a/openpilot/selfdrive/controls/lib/longcontrol.py b/openpilot/selfdrive/controls/lib/longcontrol.py index c7322c0940..3a20601c48 100644 --- a/openpilot/selfdrive/controls/lib/longcontrol.py +++ b/openpilot/selfdrive/controls/lib/longcontrol.py @@ -9,8 +9,6 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] LongCtrlState = car.CarControl.Actuators.LongControlState -STOP_JERK = 1.0 - def long_control_state_trans(CP_SP, active, long_control_state, should_stop, brake_pressed, cruise_standstill): @@ -55,7 +53,7 @@ class LongControl: def update(self, active, CS, a_target, should_stop, accel_limits): """Update longitudinal control. This updates the state machine and runs a PID loop""" - self.pid.neg_limit = max(accel_limits[0], -np.sqrt(2.0 * STOP_JERK * max(CS.vEgo, 0.0))) + self.pid.neg_limit = accel_limits[0] self.pid.pos_limit = accel_limits[1] self.long_control_state = long_control_state_trans(self.CP_SP, active, self.long_control_state, @@ -66,9 +64,11 @@ class LongControl: output_accel = 0. elif self.long_control_state == LongCtrlState.stopping: - output_accel = min(self.last_output_accel, 0.0) - stop_target = np.interp(CS.vEgo, [0.0, 1.0], [-0.5, self.CP.stopAccel]) - output_accel = np.clip(stop_target, output_accel - STOP_JERK * DT_CTRL, output_accel + STOP_JERK * DT_CTRL) + output_accel = self.last_output_accel + if output_accel > self.CP.stopAccel: + output_accel = min(output_accel, 0.0) + # TODO: can we just go straight to stopAccel? + output_accel -= 1.0 * DT_CTRL # m/s^2/s while trying to stop self.reset() else: # LongCtrlState.pid