diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index fcebf1b564..beacc518d0 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -10,20 +10,20 @@ LongCtrlState = car.CarControl.Actuators.LongControlState STOPPING_TARGET_SPEED_OFFSET = 0.01 # As per ISO 15622:2018 for all speeds -ACCEL_MIN_ISO = -3.5 # m/s^2 -ACCEL_MAX_ISO = 2.0 # m/s^2 +ACCEL_MIN_ISO = -3.5 # m/s^2 +ACCEL_MAX_ISO = 2.0 # m/s^2 -def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, v_pid, +def long_control_state_trans(CP, active, long_control_state, v_ego, v_target_future, v_pid, output_accel, brake_pressed, cruise_standstill, min_speed_can): """Update longitudinal control state machine""" stopping_target_speed = min_speed_can + STOPPING_TARGET_SPEED_OFFSET stopping_condition = (v_ego < 2.0 and cruise_standstill) or \ (v_ego < CP.vEgoStopping and - ((v_pid < stopping_target_speed and v_target < stopping_target_speed) or + ((v_pid < stopping_target_speed and v_target_future < stopping_target_speed) or brake_pressed)) - starting_condition = v_target > CP.vEgoStarting and not cruise_standstill + starting_condition = v_target_future > CP.vEgoStarting and not cruise_standstill if not active: long_control_state = LongCtrlState.off @@ -75,13 +75,10 @@ class LongControl(): v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], long_plan.speeds) a_target_upper = 2 * (v_target_upper - long_plan.speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0] - - v_target = min(v_target_lower, v_target_upper) a_target = min(a_target_lower, a_target_upper) v_target_future = long_plan.speeds[-1] else: - v_target = 0.0 v_target_future = 0.0 a_target = 0.0 @@ -103,11 +100,11 @@ class LongControl(): # tracking objects and driving elif self.long_control_state == LongCtrlState.pid: - self.v_pid = v_target + self.v_pid = long_plan.speeds[0] # Toyota starts braking more when it thinks you want to stop # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration - prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < v_target + prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV) freeze_integrator = prevent_overshoot diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index b179c09e16..5fff750105 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -eb82d8fc821da3488dffe85f191211ee1fad5904 \ No newline at end of file +e32613484a42234bf896f1205039e9becc91ea3b \ No newline at end of file