A440

opendbc_repo/opendbc/car/hyundai/carcontroller.py

@@ -48,6 +48,7 @@ IONIQ_6_DYNAMIC_LOWER_JERK_BP = [-2.0, -1.5, -1.0, -0.25, -0.1, -0.025, -0.01, -
 IONIQ_6_DYNAMIC_LOWER_JERK_V = [3.3, 1.5, 1.0, 0.8, 0.7, 0.65, 0.55, 0.5]
 IONIQ_6_LAUNCH_HOLD_SPEED_BP = [0.0, 0.6, 1.25, 2.5]
 IONIQ_6_LAUNCH_HOLD_SPEED_V = [0.75, 0.6, 0.4, 0.0]
+IONIQ_6_STOP_BRAKE_CAP_MAX_SPEED = 2.0
 IONIQ_6_STOP_BRAKE_CAP_SPEED_BP = [0.0, 0.08, 0.25, 0.6, 1.2, 2.0, 3.0]
 IONIQ_6_STOP_BRAKE_CAP_ACCEL_V = [-0.09, -0.10, -0.11, -0.22, -0.50, -0.95, -1.40]
 IONIQ_6_STOP_HOLD_JERK_BP = [0.0, 0.15, 0.6, 1.2, 2.0, 3.0]
@@ -133,9 +134,12 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc
   state.jerk_lower = min(dynamic_lower_jerk, lower_speed_limit)
 
   if state.stopping:
-    stop_brake_cap = float(np.interp(v_ego, IONIQ_6_STOP_BRAKE_CAP_SPEED_BP, IONIQ_6_STOP_BRAKE_CAP_ACCEL_V))
-    state.desired_accel = min(0.0, max(accel_cmd, stop_brake_cap))
-    state.jerk_upper = min(state.jerk_upper, float(np.interp(v_ego, IONIQ_6_STOP_HOLD_JERK_BP, IONIQ_6_STOP_HOLD_JERK_V)) * IONIQ_6_RESPONSE_MULTIPLIER)
+    if v_ego <= IONIQ_6_STOP_BRAKE_CAP_MAX_SPEED:
+      stop_brake_cap = float(np.interp(v_ego, IONIQ_6_STOP_BRAKE_CAP_SPEED_BP, IONIQ_6_STOP_BRAKE_CAP_ACCEL_V))
+      state.desired_accel = min(0.0, max(accel_cmd, stop_brake_cap))
+      state.jerk_upper = min(state.jerk_upper, float(np.interp(v_ego, IONIQ_6_STOP_HOLD_JERK_BP, IONIQ_6_STOP_HOLD_JERK_V)) * IONIQ_6_RESPONSE_MULTIPLIER)
+    else:
+      state.desired_accel = float(np.clip(accel_cmd, CarControllerParams.ACCEL_MIN, 0.0))
   else:
     state.desired_accel = float(np.clip(accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
     if state.launch_active:

opendbc_repo/opendbc/car/hyundai/interface.py

@@ -208,8 +208,8 @@ class CarInterface(CarInterfaceBase):
       ret.vEgoStopping = 0.35
 
     if candidate == CAR.KIA_NIRO_PHEV_2022:
-      ret.stopAccel = -1.5
-      ret.stoppingDecelRate = 0.55
+      ret.stopAccel = -1.4
+      ret.stoppingDecelRate = 0.5
       ret.vEgoStopping = 0.7
 
     if candidate == CAR.KIA_OPTIMA_G4_FL:

opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py

@@ -183,9 +183,9 @@ class TestHyundaiFingerprint:
     toggles = get_test_toggles()
     CP = CarInterface.get_params(CAR.KIA_NIRO_PHEV_2022, gen_empty_fingerprint(), [], True, False, False, toggles)
 
-    assert CP.stopAccel == pytest.approx(-1.5)
+    assert CP.stopAccel == pytest.approx(-1.4)
     assert CP.vEgoStopping == pytest.approx(0.7)
-    assert CP.stoppingDecelRate == pytest.approx(0.55)
+    assert CP.stoppingDecelRate == pytest.approx(0.5)
 
   def test_kia_forte_no_scc_fw_match(self):
     car_fw = [
@@ -390,20 +390,30 @@ class TestHyundaiFingerprint:
     assert state.jerk_upper == pytest.approx(0.42)
     assert state.actual_accel == pytest.approx(-0.099)
 
-  def test_ioniq_6_longitudinal_tuning_helper_caps_late_low_speed_stop_brake(self):
+  def test_ioniq_6_longitudinal_tuning_helper_caps_final_low_speed_stop_brake(self):
     state = Ioniq6LongitudinalTuningState(actual_accel=-2.82, accel_last=-2.82,
                                           long_control_state_last=LongCtrlState.pid)
 
-    state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=-2.82, v_ego=2.5, a_ego=-2.4,
+    state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=-2.82, v_ego=1.8, a_ego=-2.4,
                                                long_control_state=LongCtrlState.stopping, long_active=True)
     assert state.stopping
-    assert state.desired_accel == pytest.approx(-1.175)
+    assert state.desired_accel == pytest.approx(-0.8375)
 
     for _ in range(10):
-      state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=-2.82, v_ego=2.5, a_ego=-2.4,
+      state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=-2.82, v_ego=1.8, a_ego=-2.4,
                                                  long_control_state=LongCtrlState.stopping, long_active=True)
     assert state.actual_accel == pytest.approx(-2.49)
 
+  def test_ioniq_6_longitudinal_tuning_helper_keeps_full_stop_authority_above_final_band(self):
+    state = Ioniq6LongitudinalTuningState(actual_accel=-1.8, accel_last=-1.8,
+                                          long_control_state_last=LongCtrlState.pid)
+
+    state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=-2.82, v_ego=3.8, a_ego=-1.8,
+                                               long_control_state=LongCtrlState.stopping, long_active=True)
+    assert state.stopping
+    assert state.desired_accel == pytest.approx(-2.82)
+    assert state.actual_accel < -1.8
+
   def test_genesis_g90_longitudinal_tuning_softens_final_stop_hold(self):
     state = GenesisG90LongitudinalTuningState()
 

selfdrive/controls/lib/latcontrol_torque.py

@@ -66,16 +66,16 @@ CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT = 0.02
 CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
 CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT = 0.26
 CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT = 0.40
-CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = 0.00
-CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.00
+CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = -0.03
+CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.05
 CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = 0.00
-CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.00
-CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.04
-CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.10
+CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.04
+CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.10
+CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.04
 CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00
-CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
-CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.03
-CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.07
+CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.03
+CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.08
+CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.03
 CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_MAX = 0.12
 CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT = 0.24
 CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT_WIDTH = 0.05
@@ -116,6 +116,10 @@ GENESIS_G90_CARS = (
 IONIQ_5_CARS = (
   HYUNDAI_CAR.HYUNDAI_IONIQ_5,
 )
+IONIQ_EV_OLD_CARS = (
+  HYUNDAI_CAR.HYUNDAI_IONIQ_EV_LTD,
+  HYUNDAI_CAR.HYUNDAI_IONIQ_EV_2020,
+)
 IONIQ_6_CARS = (
   HYUNDAI_CAR.HYUNDAI_IONIQ_6,
 )
@@ -234,18 +238,18 @@ VOLT_STANDARD_CENTER_TAPER_SPEED = 20.0
 VOLT_STANDARD_CENTER_TAPER_SPEED_WIDTH = 2.5
 
 SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT = 1.04
-SONATA_HYBRID_FF_REDUCTION_LEFT = 0.09
-SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.20
+SONATA_HYBRID_FF_REDUCTION_LEFT = 0.07
+SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.22
 SONATA_HYBRID_FF_ONSET = 0.22
 SONATA_HYBRID_FF_ONSET_WIDTH = 0.08
 SONATA_HYBRID_FF_CUTOFF = 1.35
 SONATA_HYBRID_FF_CUTOFF_WIDTH = 0.40
 SONATA_HYBRID_TRANSITION_SPEED = 8.0
 SONATA_HYBRID_PHASE_SCALE = 0.12
-SONATA_HYBRID_TURN_IN_BOOST_LEFT = 0.08
+SONATA_HYBRID_TURN_IN_BOOST_LEFT = 0.12
 SONATA_HYBRID_TURN_IN_BOOST_RIGHT = 0.00
-SONATA_HYBRID_UNWIND_TAPER_LEFT = 0.14
-SONATA_HYBRID_UNWIND_TAPER_RIGHT = 0.12
+SONATA_HYBRID_UNWIND_TAPER_LEFT = 0.18
+SONATA_HYBRID_UNWIND_TAPER_RIGHT = 0.10
 SONATA_HYBRID_CENTER_TAPER_MAX = 0.05
 SONATA_HYBRID_CENTER_TAPER_LAT = 0.14
 SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH = 0.025
@@ -304,6 +308,25 @@ IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
 IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.15
 IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.26
 
+IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT = 1.08
+IONIQ_EV_OLD_FF_REDUCTION_LEFT = 0.08
+IONIQ_EV_OLD_FF_REDUCTION_RIGHT = 0.16
+IONIQ_EV_OLD_FF_ONSET = 0.14
+IONIQ_EV_OLD_FF_ONSET_WIDTH = 0.05
+IONIQ_EV_OLD_FF_CUTOFF = 1.10
+IONIQ_EV_OLD_FF_CUTOFF_WIDTH = 0.30
+IONIQ_EV_OLD_TRANSITION_SPEED = 10.0
+IONIQ_EV_OLD_PHASE_SCALE = 0.10
+IONIQ_EV_OLD_TURN_IN_BOOST_LEFT = 0.06
+IONIQ_EV_OLD_TURN_IN_BOOST_RIGHT = 0.00
+IONIQ_EV_OLD_UNWIND_TAPER_LEFT = 0.18
+IONIQ_EV_OLD_UNWIND_TAPER_RIGHT = 0.08
+IONIQ_EV_OLD_CENTER_TAPER_MAX = 0.10
+IONIQ_EV_OLD_CENTER_TAPER_LAT = 0.14
+IONIQ_EV_OLD_CENTER_TAPER_LAT_WIDTH = 0.03
+IONIQ_EV_OLD_CENTER_TAPER_SPEED = 24.0
+IONIQ_EV_OLD_CENTER_TAPER_SPEED_WIDTH = 2.2
+
 IONIQ_6_FF_GAIN_LEFT = 0.045
 IONIQ_6_FF_GAIN_RIGHT = 0.015
 IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT = 1.22
@@ -1060,6 +1083,48 @@ def get_ioniq_5_friction_scale(v_ego: float, desired_lateral_accel: float, desir
   return min(max(friction_scale, 0.86), 1.04)
 
 
+def _ioniq_ev_old_sigmoid(x: float) -> float:
+  return _sigmoid(x)
+
+
+def _ioniq_ev_old_low_speed_factor(v_ego: float) -> float:
+  return 1.0 / (1.0 + (max(v_ego, 0.0) / IONIQ_EV_OLD_TRANSITION_SPEED) ** 2)
+
+
+def _ioniq_ev_old_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
+  return math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_EV_OLD_PHASE_SCALE)
+
+
+def _ioniq_ev_old_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
+  return left_value if desired_lateral_accel >= 0.0 else right_value
+
+
+def get_ioniq_ev_old_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
+  if desired_lateral_accel == 0.0:
+    return 1.0
+
+  abs_lateral_accel = abs(desired_lateral_accel)
+  onset = _ioniq_ev_old_sigmoid((abs_lateral_accel - IONIQ_EV_OLD_FF_ONSET) / IONIQ_EV_OLD_FF_ONSET_WIDTH)
+  cutoff = _ioniq_ev_old_sigmoid((IONIQ_EV_OLD_FF_CUTOFF - abs_lateral_accel) / IONIQ_EV_OLD_FF_CUTOFF_WIDTH)
+  base_reduction = _ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_FF_REDUCTION_LEFT, IONIQ_EV_OLD_FF_REDUCTION_RIGHT) * onset * cutoff
+  phase = _ioniq_ev_old_transition_phase(desired_lateral_accel, desired_lateral_jerk)
+  turn_in_weight = max(phase, 0.0)
+  unwind_weight = max(-phase, 0.0)
+  low_speed_factor = _ioniq_ev_old_low_speed_factor(v_ego)
+  turn_in_boost = 1.0 + (_ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_TURN_IN_BOOST_LEFT, IONIQ_EV_OLD_TURN_IN_BOOST_RIGHT) *
+                         turn_in_weight * (0.35 + 0.65 * low_speed_factor))
+  unwind_taper = 1.0 - (_ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_UNWIND_TAPER_LEFT, IONIQ_EV_OLD_UNWIND_TAPER_RIGHT) *
+                        unwind_weight * (0.35 + 0.65 * low_speed_factor))
+  return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)
+
+
+def get_ioniq_ev_old_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
+  speed_weight = _ioniq_ev_old_sigmoid((v_ego - IONIQ_EV_OLD_CENTER_TAPER_SPEED) / IONIQ_EV_OLD_CENTER_TAPER_SPEED_WIDTH)
+  center_weight = _ioniq_ev_old_sigmoid((IONIQ_EV_OLD_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_EV_OLD_CENTER_TAPER_LAT_WIDTH)
+  reduction = IONIQ_EV_OLD_CENTER_TAPER_MAX * speed_weight * center_weight
+  return 1.0 - reduction
+
+
 def _ioniq_6_sigmoid(x: float) -> float:
   return _sigmoid(x)
 
@@ -1352,6 +1417,7 @@ class LatControlTorque(LatControl):
     self.is_volt_standard = CP.carFingerprint in VOLT_STANDARD_CARS
     self.is_genesis_g90 = CP.carFingerprint in GENESIS_G90_CARS
     self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS
+    self.is_ioniq_ev_old = CP.carFingerprint in IONIQ_EV_OLD_CARS
     self.is_ioniq_6 = CP.carFingerprint in IONIQ_6_CARS
     self.is_sonata_hybrid = CP.carFingerprint in SONATA_HYBRID_CARS
     self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS
@@ -1366,6 +1432,8 @@ class LatControlTorque(LatControl):
     self.torque_ki_mult = 1.0
     if self.is_ioniq_5:
       self.torque_params.latAccelFactor *= IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT
+    if self.is_ioniq_ev_old:
+      self.torque_params.latAccelFactor *= IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT
     if self.is_ioniq_6:
       self.torque_params.latAccelFactor *= IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT
     if self.is_sonata_hybrid:
@@ -1389,6 +1457,8 @@ class LatControlTorque(LatControl):
   def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
     if self.is_ioniq_5:
       latAccelFactor *= IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT
+    if self.is_ioniq_ev_old:
+      latAccelFactor *= IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT
     if self.is_ioniq_6:
       latAccelFactor *= IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT
     if self.is_sonata_hybrid:
@@ -1467,11 +1537,13 @@ class LatControlTorque(LatControl):
       volt_standard_test_active = self.is_volt_standard and volt_standard_lateral_testing_ground_active()
       genesis_g90_test_active = self.is_genesis_g90 and genesis_g90_lateral_testing_ground_active()
       ioniq_5_active = self.is_ioniq_5
+      ioniq_ev_old_active = self.is_ioniq_ev_old
       ioniq_6_active = self.is_ioniq_6
       sonata_hybrid_active = self.is_sonata_hybrid
       kia_ev6_test_active = self.is_kia_ev6 and kia_ev6_lateral_testing_ground_active()
       volt_plexy_test_active = self.is_volt_cc and volt_plexy_lateral_testing_ground_active()
       volt_standard_center_taper = get_volt_standard_center_taper_scale(setpoint, CS.vEgo) if volt_standard_test_active else 1.0
+      ioniq_ev_old_center_taper = get_ioniq_ev_old_center_taper_scale(setpoint, CS.vEgo) if ioniq_ev_old_active else 1.0
       ioniq_6_center_taper = get_ioniq_6_center_taper_scale(setpoint, CS.vEgo) if ioniq_6_active else 1.0
       sonata_hybrid_center_taper = get_sonata_hybrid_center_taper_scale(setpoint, CS.vEgo) if sonata_hybrid_active else 1.0
       civic_bosch_modified_a_center_taper = get_civic_bosch_modified_a_center_taper_scale(setpoint, CS.vEgo) if (
@@ -1499,6 +1571,9 @@ class LatControlTorque(LatControl):
         ff *= get_ioniq_5_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
         friction_threshold = get_ioniq_5_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
         friction_scale = get_ioniq_5_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
+      elif ioniq_ev_old_active:
+        ff *= get_ioniq_ev_old_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_ev_old_center_taper
+        friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_ev_old_center_taper)
       elif ioniq_6_active:
         ff *= get_ioniq_6_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_6_center_taper
         friction_threshold = get_ioniq_6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) / max(ioniq_6_center_taper, 1e-3)

selfdrive/controls/lib/longcontrol.py

@@ -13,6 +13,9 @@ clip = np.clip
 interp = np.interp
 STOPPING_RELEASE_HYSTERESIS = 0.35
 STOPPING_RELEASE_MIN_ACCEL = 0.15
+MOVING_STOP_FOLLOW_MIN_GAP = 0.25
+NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35
+NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40
 
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
@@ -184,6 +187,17 @@ class LongControl:
 
     return self.stop_release_counter >= int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL))
 
+  @staticmethod
+  def _apply_moving_stop_target_follow(output_accel, a_target, should_stop, CS, starpilot_toggles):
+    follow_min_speed = max(1.5, starpilot_toggles.vEgoStopping + 1.0)
+    if not should_stop or CS.brakePressed or CS.vEgo <= follow_min_speed:
+      return output_accel
+    if a_target >= output_accel - MOVING_STOP_FOLLOW_MIN_GAP:
+      return output_accel
+
+    follow_step = interp(CS.vEgo, [follow_min_speed, 3.0, 6.0, 10.0], [0.02, 0.03, 0.05, 0.07])
+    return max(float(a_target), output_accel - float(follow_step))
+
   def _get_pedal_long_freeze(self, a_target, error, v_ego, accel_limits):
     volt_test_tune_handoff = self.is_volt and testing_ground.use_2
 
@@ -228,7 +242,7 @@ class LongControl:
       return
     if a_target >= -0.05 or error >= -0.25:
       return
-    if CS.vEgo <= 8.0 or CS.aEgo <= 0.15:
+    if CS.vEgo <= NEGATIVE_TARGET_CREEP_GUARD_SPEED and a_target > -NEGATIVE_TARGET_CREEP_GUARD_DECEL:
       return
 
     # If the planner has already crossed into decel but the car is still
@@ -260,12 +274,12 @@ class LongControl:
       return output_accel
     if a_target >= -0.10 or error >= -0.35:
       return output_accel
-    if CS.vEgo <= 8.0 or CS.aEgo <= 0.15:
+    if CS.vEgo <= NEGATIVE_TARGET_CREEP_GUARD_SPEED and a_target > -NEGATIVE_TARGET_CREEP_GUARD_DECEL:
       return output_accel
 
     # Once the planner is asking for real decel, don't keep feeding positive
     # drive torque while we're still accelerating away from the target.
-    positive_cap = interp(a_target, [-0.6, -0.1], [0.0, 0.05])
+    positive_cap = interp(a_target, [-1.5, -0.6, -0.1], [0.0, 0.0, 0.05])
     return min(output_accel, float(positive_cap))
 
   def update(self, active, CS, a_target, should_stop, accel_limits, starpilot_toggles):
@@ -287,6 +301,7 @@ class LongControl:
       if output_accel > starpilot_toggles.stopAccel:
         output_accel = min(output_accel, 0.0)
         output_accel -= starpilot_toggles.stoppingDecelRate * DT_CTRL
+      output_accel = self._apply_moving_stop_target_follow(output_accel, a_target, should_stop, CS, starpilot_toggles)
       self.reset(preserve_stop_release=True)
 
     elif self.long_control_state == LongCtrlState.starting:

selfdrive/controls/lib/longitudinal_planner.py

@@ -33,6 +33,11 @@ RAW_LEAD_SAFETY_TTC = 7.0
 RAW_LEAD_SAFETY_DISTANCE = 40.0
 STANDSTILL_LEAD_NUDGE_ACCEL = 0.05
 STANDSTILL_LEAD_NUDGE_MIN_SPEED = 0.0
+STANDSTILL_LEAD_DEPART_MIN_ACCEL = 0.20
+STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED = 1.5
+STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED = 0.6
+STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN = 1.5
+STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL = 0.08
 CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0
 VISION_LEAD_APPROACH_MIN_CLOSING_SPEED = 2.0
 VISION_LEAD_APPROACH_TRIGGER_TIME = 4.5
@@ -113,6 +118,9 @@ VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN = 0.9
 VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME = 0.8
 VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 1.25
 VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN = 0.25
+MANUAL_STOP_RESUME_OVERRIDE_TIME = 3.0
+MANUAL_STOP_RESUME_OVERRIDE_MAX_SPEED = 2.0
+MANUAL_STOP_RESUME_OVERRIDE_MIN_ACCEL = 0.2
 LEAD_CATCHUP_ACCEL_MIN_EGO = 8.0
 LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA = -0.5
 LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN = 4.0
@@ -172,6 +180,30 @@ FAR_LEAD_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN = 1.00
 FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DECEL = 0.05
 FAR_LEAD_COMFORT_BRAKE_CAP_MAX_DECEL = 0.18
 FAR_LEAD_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL = 0.05
+TRACKED_VISION_MODEL_FLOOR_MIN_SPEED = 10.0
+TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_PROB = 0.95
+TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_DECEL = 0.80
+TRACKED_VISION_MODEL_FLOOR_MIN_CLOSING_SPEED = 1.0
+TRACKED_VISION_MODEL_FLOOR_MAX_TTC = 22.0
+TRACKED_VISION_MODEL_FLOOR_MIN_GAP_MARGIN = -2.0
+TRACKED_VISION_MODEL_FLOOR_MAX_GAP_BUFFER_MIN = 4.0
+TRACKED_VISION_MODEL_FLOOR_MAX_GAP_BUFFER_GAIN = 0.25
+TRACKED_VISION_MODEL_FLOOR_MIN_DECEL = 0.35
+TRACKED_VISION_MODEL_FLOOR_MAX_DECEL = 1.10
+TRACKED_VISION_MODEL_FLOOR_LEAD_BRAKE_MAX = 0.18
+TRACKED_VISION_MODEL_CAP_MIN_SPEED = 14.0
+TRACKED_VISION_MODEL_CAP_MAX_SPEED = 32.0
+TRACKED_VISION_MODEL_CAP_MIN_MODEL_PROB = 0.95
+TRACKED_VISION_MODEL_CAP_MAX_MODEL_DECEL = 0.75
+TRACKED_VISION_MODEL_CAP_MIN_CLOSING_SPEED = 0.75
+TRACKED_VISION_MODEL_CAP_MAX_CLOSING_SPEED = 4.0
+TRACKED_VISION_MODEL_CAP_MAX_LEAD_BRAKE = 0.55
+TRACKED_VISION_MODEL_CAP_MIN_TTC = 7.0
+TRACKED_VISION_MODEL_CAP_MIN_GAP_MARGIN = -1.5
+TRACKED_VISION_MODEL_CAP_MAX_GAP_BUFFER_MIN = 4.0
+TRACKED_VISION_MODEL_CAP_MAX_GAP_BUFFER_GAIN = 0.25
+TRACKED_VISION_MODEL_CAP_MIN_DECEL = 0.45
+TRACKED_VISION_MODEL_CAP_MAX_DECEL = 1.10
 
 # Lookup table for turns
 _A_TOTAL_MAX_V = [3.5, 3.5, 3.2]
@@ -345,6 +377,7 @@ class LongitudinalPlanner:
     self.vision_low_speed_stop_hold_until = 0.0
     self.vision_lead_approach_confirm_t = 0.0
     self.untracked_slow_lead_confirm_t = 0.0
+    self.manual_stop_resume_override_until = 0.0
 
     if self.is_preap:
       try:
@@ -680,6 +713,29 @@ class LongitudinalPlanner:
     min_stop_brake = VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE + VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN * float(v_ego)
     return max(accel_min, -min_stop_brake), True
 
+  def _update_manual_stop_resume_override(self, sm):
+    now_t = time.monotonic()
+    lead = sm["radarState"].leadOne
+    no_lead = not bool(getattr(lead, "status", False))
+    try:
+      starpilot_car_state = sm["starpilotCarState"]
+    except KeyError:
+      starpilot_car_state = None
+    accel_pressed = bool(getattr(starpilot_car_state, "accelPressed", False))
+    model_should_stop = bool(getattr(sm["modelV2"].action, "shouldStop", False))
+    standstill = bool(getattr(sm["carState"], "standstill", False))
+    forcing_stop = bool(getattr(sm["starpilotPlan"], "forcingStop", False))
+    red_light = bool(getattr(sm["starpilotPlan"], "redLight", False))
+
+    if standstill and no_lead and accel_pressed and (forcing_stop or red_light or model_should_stop):
+      self.manual_stop_resume_override_until = now_t + MANUAL_STOP_RESUME_OVERRIDE_TIME
+
+    return bool(
+      no_lead and
+      float(getattr(sm["carState"], "vEgo", 0.0)) < MANUAL_STOP_RESUME_OVERRIDE_MAX_SPEED and
+      now_t < self.manual_stop_resume_override_until
+    )
+
   def get_lead_catchup_accel_cap(self, lead, v_ego, t_follow):
     if lead is None or not lead.status:
       return None
@@ -902,6 +958,91 @@ class LongitudinalPlanner:
     ))
     return -max(0.0, cap_decel - relax_decel)
 
+  def get_tracked_vision_model_brake_floor(self, lead, v_ego, accel_min, t_follow, model_desired):
+    if lead is None or not lead.status or bool(getattr(lead, "radar", False)):
+      return None
+    if float(v_ego) < TRACKED_VISION_MODEL_FLOOR_MIN_SPEED:
+      return None
+
+    lead_prob = float(getattr(lead, "modelProb", 0.0))
+    if lead_prob < TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_PROB:
+      return None
+
+    model_brake = max(0.0, -float(model_desired))
+    if model_brake < TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_DECEL:
+      return None
+
+    lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
+    reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt)
+    projected_closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) + lead_brake * reaction_t
+    if projected_closing_speed < TRACKED_VISION_MODEL_FLOOR_MIN_CLOSING_SPEED:
+      return None
+
+    desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow))
+    gap_margin = float(lead.dRel) - desired_gap
+    max_gap_margin = max(TRACKED_VISION_MODEL_FLOOR_MAX_GAP_BUFFER_MIN,
+                         TRACKED_VISION_MODEL_FLOOR_MAX_GAP_BUFFER_GAIN * float(v_ego))
+    if gap_margin < TRACKED_VISION_MODEL_FLOOR_MIN_GAP_MARGIN or gap_margin > max_gap_margin:
+      return None
+
+    projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1)
+    if projected_ttc > TRACKED_VISION_MODEL_FLOOR_MAX_TTC:
+      return None
+
+    floor_decel = float(np.interp(
+      model_brake,
+      [TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_DECEL, 1.6],
+      [TRACKED_VISION_MODEL_FLOOR_MIN_DECEL, TRACKED_VISION_MODEL_FLOOR_MAX_DECEL],
+    ))
+    floor_decel += float(np.interp(lead_brake, [0.0, 0.8], [0.0, TRACKED_VISION_MODEL_FLOOR_LEAD_BRAKE_MAX]))
+    return max(accel_min, -min(TRACKED_VISION_MODEL_FLOOR_MAX_DECEL, floor_decel))
+
+  def get_tracked_vision_model_brake_cap(self, lead, v_ego, t_follow, model_desired):
+    if lead is None or not lead.status or bool(getattr(lead, "radar", False)):
+      return None
+    if not (TRACKED_VISION_MODEL_CAP_MIN_SPEED <= float(v_ego) <= TRACKED_VISION_MODEL_CAP_MAX_SPEED):
+      return None
+
+    lead_prob = float(getattr(lead, "modelProb", 0.0))
+    if lead_prob < TRACKED_VISION_MODEL_CAP_MIN_MODEL_PROB:
+      return None
+
+    model_brake = max(0.0, -float(model_desired))
+    if model_brake > TRACKED_VISION_MODEL_CAP_MAX_MODEL_DECEL:
+      return None
+
+    lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
+    if lead_brake > TRACKED_VISION_MODEL_CAP_MAX_LEAD_BRAKE:
+      return None
+
+    reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt)
+    projected_closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) + lead_brake * reaction_t
+    if not (TRACKED_VISION_MODEL_CAP_MIN_CLOSING_SPEED <= projected_closing_speed <= TRACKED_VISION_MODEL_CAP_MAX_CLOSING_SPEED):
+      return None
+
+    desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow))
+    gap_margin = float(lead.dRel) - desired_gap
+    max_gap_margin = max(TRACKED_VISION_MODEL_CAP_MAX_GAP_BUFFER_MIN,
+                         TRACKED_VISION_MODEL_CAP_MAX_GAP_BUFFER_GAIN * float(v_ego))
+    if gap_margin < TRACKED_VISION_MODEL_CAP_MIN_GAP_MARGIN or gap_margin > max_gap_margin:
+      return None
+
+    projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1)
+    if projected_ttc < TRACKED_VISION_MODEL_CAP_MIN_TTC:
+      return None
+
+    cap_decel = float(np.interp(
+      model_brake,
+      [0.0, TRACKED_VISION_MODEL_CAP_MAX_MODEL_DECEL],
+      [TRACKED_VISION_MODEL_CAP_MIN_DECEL, TRACKED_VISION_MODEL_CAP_MAX_DECEL],
+    ))
+    cap_decel += float(np.interp(
+      projected_closing_speed,
+      [TRACKED_VISION_MODEL_CAP_MIN_CLOSING_SPEED, TRACKED_VISION_MODEL_CAP_MAX_CLOSING_SPEED],
+      [0.0, 0.08],
+    ))
+    return -min(TRACKED_VISION_MODEL_CAP_MAX_DECEL, cap_decel)
+
   @staticmethod
   def raw_close_lead_needs_control(lead, v_ego):
     if lead is None or not lead.status:
@@ -1263,6 +1404,7 @@ class LongitudinalPlanner:
     comfort_output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0]
     vision_cap_accel_min = min(comfort_output_accel_min, get_vehicle_min_accel(self.CP, v_ego))
     output_accel_min = comfort_output_accel_min
+    model_desired_accel = float(sm['modelV2'].action.desiredAcceleration)
 
     if not tracking_lead:
       pretracking_vision_caps = []
@@ -1341,12 +1483,33 @@ class LongitudinalPlanner:
       self.a_desired = min(self.a_desired, close_lead_brake_cap)
       output_a_target = min(output_a_target, close_lead_brake_cap)
 
-    if lead_control_active and sm['carState'].standstill:
-      standstill_nudge_gap = max(float(getattr(starpilot_toggles, "stop_distance", STOP_DISTANCE)), STOP_DISTANCE) - 0.5
-      moving_leads = [lead for lead in (self.lead_one, self.lead_two)
-                      if lead.status and lead.vLead > STANDSTILL_LEAD_NUDGE_MIN_SPEED and lead.dRel >= standstill_nudge_gap]
-      if moving_leads:
-        output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL)
+    standstill_nudge_gap = max(float(getattr(starpilot_toggles, "stop_distance", STOP_DISTANCE)), STOP_DISTANCE) - 0.5
+    moving_leads = [lead for lead in (self.lead_one, self.lead_two)
+                    if lead.status and lead.vLead > STANDSTILL_LEAD_NUDGE_MIN_SPEED and lead.dRel >= standstill_nudge_gap]
+    lead_depart_ready = any(
+      lead.status and
+      lead.vLead >= STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED and
+      lead.dRel >= standstill_nudge_gap + STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN
+      for lead in (self.lead_one, self.lead_two)
+    )
+
+    if lead_control_active and sm['carState'].standstill and moving_leads:
+      output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL)
+
+    if (
+      lead_control_active and
+      sm['carState'].standstill and
+      lead_depart_ready and
+      not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and
+      not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and
+      model_desired_accel >= STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL
+    ):
+      vision_low_speed_stop_active = False
+      output_should_stop = False
+      output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL)
+
+    if lead_control_active and lead_depart_ready and not output_should_stop and float(sm['carState'].vEgo) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED:
+      output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL)
 
     if lead_one_active:
       lead_catchup_accel_cap = self.get_lead_catchup_accel_cap(self.lead_one, scene_v_ego, effective_t_follow)
@@ -1365,6 +1528,18 @@ class LongitudinalPlanner:
 
     follow_control_lead = self.get_follow_control_lead(lead_control_active, scene_v_ego, effective_t_follow)
     if follow_control_lead is not None and not panic_bypass:
+      if not output_should_stop and not vision_low_speed_stop_active:
+        tracked_vision_model_brake_floor = self.get_tracked_vision_model_brake_floor(
+          follow_control_lead,
+          scene_v_ego,
+          output_accel_min,
+          effective_t_follow,
+          model_desired_accel,
+        )
+        if tracked_vision_model_brake_floor is not None:
+          self.a_desired = min(self.a_desired, tracked_vision_model_brake_floor)
+          output_a_target = min(output_a_target, tracked_vision_model_brake_floor)
+
       matched_follow_brake_cap = self.get_matched_follow_brake_cap(follow_control_lead, scene_v_ego, effective_t_follow)
       if matched_follow_brake_cap is not None:
         self.a_desired = max(self.a_desired, matched_follow_brake_cap)
@@ -1389,9 +1564,25 @@ class LongitudinalPlanner:
         self.a_desired = max(self.a_desired, far_lead_brake_cap)
         output_a_target = max(output_a_target, far_lead_brake_cap)
 
+    if follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active:
+      tracked_vision_model_brake_cap = self.get_tracked_vision_model_brake_cap(
+        follow_control_lead,
+        scene_v_ego,
+        effective_t_follow,
+        model_desired_accel,
+      )
+      if tracked_vision_model_brake_cap is not None:
+        self.a_desired = max(self.a_desired, tracked_vision_model_brake_cap)
+        output_a_target = max(output_a_target, tracked_vision_model_brake_cap)
+
     output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1]
     output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max))
 
+    manual_stop_resume_override = self._update_manual_stop_resume_override(sm)
+    if manual_stop_resume_override:
+      output_a_target = max(output_a_target, MANUAL_STOP_RESUME_OVERRIDE_MIN_ACCEL)
+      output_should_stop = False
+
     self.output_a_target = output_a_target
     self.output_should_stop = bool(output_should_stop or vision_low_speed_stop_active)
 
@@ -1414,7 +1605,13 @@ class LongitudinalPlanner:
     longitudinalPlan.fcw = self.fcw
 
     longitudinalPlan.aTarget = float(self.output_a_target)
-    longitudinalPlan.shouldStop = bool(self.output_should_stop) or (sm['starpilotPlan'].forcingStop and sm['starpilotPlan'].forcingStopLength < 1)
+    force_stop_handoff = bool(
+      sm['starpilotPlan'].forcingStop and (
+        sm['starpilotPlan'].forcingStopLength < 1.0 or
+        sm['starpilotPlan'].vCruise <= 0.0
+      )
+    )
+    longitudinalPlan.shouldStop = bool(self.output_should_stop) or force_stop_handoff
     longitudinalPlan.allowBrake = True
     longitudinalPlan.allowThrottle = bool(self.allow_throttle)
 

selfdrive/controls/tests/test_latcontrol.py

@@ -42,6 +42,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
   get_ioniq_5_ff_scale,
   get_ioniq_5_friction_scale,
   get_ioniq_5_friction_threshold,
+  get_ioniq_ev_old_center_taper_scale,
+  get_ioniq_ev_old_ff_scale,
   get_ioniq_6_center_taper_scale,
   get_ioniq_6_directional_taper_scale,
   get_ioniq_6_output_taper_scale,
@@ -296,6 +298,17 @@ class TestLatControl:
     assert unwind_left_scale < 1.0
     assert unwind_right_scale < unwind_left_scale
 
+  def test_ioniq_ev_old_ff_scale_curve(self):
+    assert get_ioniq_ev_old_ff_scale(0.0, 0.0, 20.0) == 1.0
+    assert get_ioniq_ev_old_ff_scale(0.35, 0.0, 20.0) > get_ioniq_ev_old_ff_scale(-0.35, 0.0, 20.0)
+    assert get_ioniq_ev_old_ff_scale(0.35, 0.7, 8.0) > get_ioniq_ev_old_ff_scale(0.35, 0.0, 8.0)
+    assert get_ioniq_ev_old_ff_scale(0.35, -0.7, 8.0) < get_ioniq_ev_old_ff_scale(0.35, 0.0, 8.0)
+    assert get_ioniq_ev_old_ff_scale(-0.35, -0.7, 8.0) <= get_ioniq_ev_old_ff_scale(-0.35, 0.0, 8.0)
+
+  def test_ioniq_ev_old_center_taper_curve(self):
+    assert get_ioniq_ev_old_center_taper_scale(0.0, 30.0) < get_ioniq_ev_old_center_taper_scale(0.0, 15.0)
+    assert get_ioniq_ev_old_center_taper_scale(0.0, 30.0) < get_ioniq_ev_old_center_taper_scale(0.20, 30.0) <= 1.0
+
   def test_ioniq_6_ff_scale_curve(self):
     assert get_ioniq_6_ff_scale(0.0, 0.0, 20.0) == 1.0
     assert get_ioniq_6_ff_scale(0.4, 0.0, 20.0) > get_ioniq_6_ff_scale(-0.4, 0.0, 20.0)
@@ -574,6 +587,8 @@ class TestLatControl:
     base_turn_in_right = get_civic_bosch_modified_b_ff_scale(-0.5, -0.8, 12.0)
     base_unwind_left = get_civic_bosch_modified_b_ff_scale(0.5, -0.8, 12.0)
     base_unwind_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.8, 12.0)
+    base_turn_in_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, -0.8)
+    base_unwind_left_friction = get_civic_bosch_modified_b_friction_scale(12.0, 0.5, -0.8)
     base_unwind_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, 0.8)
 
     monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_a_lateral_testing_ground_active", lambda: True)
@@ -584,15 +599,19 @@ class TestLatControl:
     a_variant_turn_in_right = get_civic_bosch_modified_b_ff_scale(-0.5, -0.8, 12.0)
     a_variant_unwind_left = get_civic_bosch_modified_b_ff_scale(0.5, -0.8, 12.0)
     a_variant_unwind_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.8, 12.0)
+    a_variant_turn_in_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, -0.8)
+    a_variant_unwind_left_friction = get_civic_bosch_modified_b_friction_scale(12.0, 0.5, -0.8)
     a_variant_unwind_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, 0.8)
 
-    assert a_variant_steady_left == pytest.approx(base_steady_left)
-    assert a_variant_steady_right == pytest.approx(base_steady_right)
-    assert a_variant_turn_in_left == pytest.approx(base_turn_in_left)
-    assert a_variant_turn_in_right == pytest.approx(base_turn_in_right)
+    assert a_variant_steady_left < base_steady_left
+    assert a_variant_steady_right > base_steady_right
+    assert a_variant_turn_in_left < base_turn_in_left
+    assert a_variant_turn_in_right > base_turn_in_right
     assert a_variant_unwind_left < base_unwind_left
-    assert a_variant_unwind_right < base_unwind_right
-    assert a_variant_unwind_right_friction < base_unwind_right_friction
+    assert a_variant_unwind_right > base_unwind_right
+    assert a_variant_turn_in_right_friction > base_turn_in_right_friction
+    assert a_variant_unwind_left_friction < base_unwind_left_friction
+    assert a_variant_unwind_right_friction >= 0.82
 
   def test_modified_civic_a_variant_center_taper_curve(self):
     assert get_civic_bosch_modified_a_center_taper_scale(0.0, 25.0) < get_civic_bosch_modified_a_center_taper_scale(0.0, 10.0)

selfdrive/controls/tests/test_longcontrol.py

@@ -264,6 +264,32 @@ def test_update_releases_stopping_with_cruise_standstill_latched():
   assert output_accel > 0.0
 
 
+def test_stopping_state_follows_stronger_moving_stop_target():
+  CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
+  CP.longitudinalTuning.kpBP = [0.0]
+  CP.longitudinalTuning.kpV = [0.1]
+  CP.longitudinalTuning.kiBP = [0.0]
+  CP.longitudinalTuning.kiV = [0.03]
+
+  lc = LongControl(CP)
+  lc.long_control_state = LongCtrlState.stopping
+  lc.last_output_accel = -1.40
+  CS = car.CarState.new_message(vEgo=4.0, aEgo=-1.2, brakePressed=False)
+  CS.cruiseState.standstill = False
+
+  output_accel = lc.update(
+    active=True,
+    CS=CS,
+    a_target=-3.5,
+    should_stop=True,
+    accel_limits=(-3.5, 2.0),
+    starpilot_toggles=make_toggles(stopAccel=-0.5, stoppingDecelRate=0.8, vEgoStopping=0.5),
+  )
+
+  assert lc.long_control_state == LongCtrlState.stopping
+  assert output_accel < -1.43
+
+
 def test_volt_testing_ground_handoff_freezes_integrator(monkeypatch):
   CP = car.CarParams.new_message()
   CP.brand = "gm"
@@ -329,6 +355,44 @@ def test_negative_target_unwinds_positive_accel_command_after_sign_flip():
   assert output_accel <= 0.01
 
 
+def test_negative_target_unwinds_positive_accel_command_at_low_speed():
+  CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
+  CP.longitudinalTuning.kpBP = [0.0]
+  CP.longitudinalTuning.kpV = [0.1]
+  CP.longitudinalTuning.kiBP = [0.0]
+  CP.longitudinalTuning.kiV = [0.03]
+
+  lc = LongControl(CP)
+  lc.long_control_state = LongCtrlState.pid
+  lc.last_output_accel = 0.9
+  lc.pid.i = 0.9
+  CS = car.CarState.new_message(vEgo=1.3, aEgo=0.35, brakePressed=False, gasPressed=False)
+  CS.cruiseState.standstill = False
+
+  output_accel = lc.update(
+    active=True,
+    CS=CS,
+    a_target=-1.2,
+    should_stop=False,
+    accel_limits=(-3.0, 2.0),
+    starpilot_toggles=make_toggles(),
+  )
+
+  assert lc.long_control_state == LongCtrlState.pid
+  assert output_accel <= 0.01
+
+
+def test_negative_target_creep_guard_keeps_mild_crawl_request():
+  capped = LongControl._cap_positive_output_on_negative_target(
+    output_accel=0.18,
+    a_target=-0.2,
+    error=-0.5,
+    CS=car.CarState.new_message(vEgo=0.2, aEgo=0.0),
+  )
+
+  assert capped == pytest.approx(0.18)
+
+
 def test_pedal_long_brake_bias_adds_small_negative_nudge_for_strong_decel_request():
   CP = car.CarParams.new_message()
   CP.brand = "gm"

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -10,6 +10,7 @@ from opendbc.car.honda.interface import CarInterface
 from opendbc.car.honda.values import CAR
 import openpilot.selfdrive.controls.lib.longitudinal_planner as longitudinal_planner_module
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import soften_far_radar_lead_accel, should_trigger_planner_fcw
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
@@ -80,6 +81,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta
       leadTwo=lead_two if lead_two is not None else make_lead(status=False),
     ),
     "selfdriveState": SimpleNamespace(enabled=True, experimentalMode=experimental_mode, personality=0),
+    "starpilotCarState": SimpleNamespace(accelPressed=False),
     "starpilotPlan": SimpleNamespace(
       vCruise=v_ego + 5.0,
       minAcceleration=min_accel,
@@ -91,6 +93,8 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta
       speedJerk=5.0,
       dangerFactor=1.0,
       tFollow=1.45,
+      forcingStop=False,
+      redLight=False,
       forcingStopLength=2,
     ),
   }
@@ -801,6 +805,93 @@ def test_acc_mode_low_speed_vision_stop_buffer_stays_latched_when_closure_soften
   assert cap_held <= -1.25
 
 
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
+def test_acc_mode_tracked_vision_model_brake_floor_prevents_positive_output_on_slower_lead(model_version):
+  v_ego = 19.1
+
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  sm = make_sm(
+    v_ego,
+    desired_accel=-1.18,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=36.5, v_lead=17.2, a_lead=-0.53, radar=False, model_prob=0.993),
+  )
+
+  for _ in range(8):
+    planner.update(sm, make_toggles(model_version))
+
+  assert planner.mode == "acc"
+  assert planner.output_a_target <= -0.35
+
+
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
+def test_tracked_vision_model_brake_cap_relaxes_mild_model_brake_slam_window(model_version):
+  v_ego = 20.56
+
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=38.1, v_lead=19.07, a_lead=-0.30, radar=False, model_prob=0.999)
+
+  cap = planner.get_tracked_vision_model_brake_cap(lead, v_ego, 1.45, -0.35)
+
+  assert cap is not None
+  assert cap > -1.2
+
+
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
+def test_tracked_vision_model_brake_cap_does_not_relax_strong_model_brake(model_version):
+  v_ego = 20.56
+
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=38.1, v_lead=19.07, a_lead=-0.30, radar=False, model_prob=0.999)
+
+  cap = planner.get_tracked_vision_model_brake_cap(lead, v_ego, 1.45, -1.2)
+
+  assert cap is None
+
+
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
+def test_manual_resume_override_clears_no_lead_model_stop_at_standstill(model_version):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.0)
+  sm = make_sm(0.0, desired_accel=0.0, min_accel=-0.5)
+  sm["carState"].standstill = True
+  sm["controlsState"].longControlState = LongCtrlState.stopping
+  sm["modelV2"].action.shouldStop = True
+  sm["starpilotPlan"].forcingStop = True
+  sm["starpilotCarState"] = SimpleNamespace(accelPressed=True)
+
+  planner.update(sm, make_toggles(model_version))
+
+  assert not planner.output_should_stop
+  assert planner.output_a_target >= 0.2
+
+
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
+def test_manual_resume_override_does_not_clear_stopped_lead_stop(model_version):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.0)
+  sm = make_sm(
+    0.0,
+    desired_accel=0.0,
+    min_accel=-0.5,
+    lead_one=make_lead(status=True, d_rel=4.0, v_lead=0.0, radar=False, model_prob=0.99),
+  )
+  sm["carState"].standstill = True
+  sm["controlsState"].longControlState = LongCtrlState.stopping
+  sm["modelV2"].action.shouldStop = True
+  sm["starpilotPlan"].forcingStop = True
+  sm["starpilotCarState"] = SimpleNamespace(accelPressed=True)
+
+  planner.update(sm, make_toggles(model_version))
+
+  assert planner.output_should_stop
+
+
 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
 def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_version):
   v_ego = 0.0
@@ -825,6 +916,32 @@ def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_ve
   assert planner.output_a_target < 0.1
 
 
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
+def test_standstill_moving_lead_applies_resume_floor_once_stop_clears(model_version):
+  v_ego = 0.0
+
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  sm = make_sm(
+    v_ego,
+    desired_accel=0.1,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=15.0, v_lead=2.2, a_lead=0.4, radar=False, model_prob=0.99),
+  )
+  sm["carState"].standstill = True
+  sm["controlsState"].longControlState = LongCtrlState.stopping
+  sm["modelV2"].action.shouldStop = False
+  sm["starpilotPlan"].vCruise = 10.0
+
+  for _ in range(12):
+    planner.update(sm, make_toggles(model_version))
+
+  assert not planner.output_should_stop
+  assert planner.output_a_target >= 0.2
+
+
 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
 def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version):
   far_v_ego = 29.26
@@ -943,6 +1060,43 @@ def test_modeld_action_uses_direct_action_head_for_v14(monkeypatch):
   assert not action.shouldStop
 
 
+def test_publish_force_stop_handoff_sets_should_stop_when_vcruise_zero():
+  class FakePM:
+    def __init__(self):
+      self.sent = {}
+
+    def send(self, name, msg):
+      self.sent[name] = msg
+
+  class FakeSM(dict):
+    def all_checks(self, service_list=None):
+      return True
+
+    logMonoTime = {"modelV2": int(1e9)}
+
+  v_ego = 5.0
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  planner.output_a_target = -0.5
+  planner.output_should_stop = False
+  planner.v_desired_trajectory = np.zeros(CONTROL_N)
+  planner.a_desired_trajectory = np.zeros(CONTROL_N)
+  planner.j_desired_trajectory = np.zeros(CONTROL_N)
+  planner.fcw = False
+  planner.mpc.source = "cruise"
+  planner.mpc.solve_time = 0.0
+  pm = FakePM()
+
+  sm = FakeSM(make_sm(v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False))
+  sm["starpilotPlan"].forcingStop = True
+  sm["starpilotPlan"].forcingStopLength = 5.0
+  sm["starpilotPlan"].vCruise = 0.0
+
+  planner.publish(sm, pm)
+
+  assert pm.sent["longitudinalPlan"].longitudinalPlan.shouldStop
+
+
 def test_allow_throttle_hysteresis_filters_gas_prob_chatter():
   v_ego = 10.0
 

starpilot/common/starpilot_functions.py

@@ -299,4 +299,7 @@ def update_openpilot(thread_manager, params):
     if not update_available():
       break
 
+  while params.get_bool("IsOnroad") or thread_manager.is_thread_alive("lock_doors"):
+    time.sleep(60)
+
   HARDWARE.reboot()

system/manager/manager.py

@@ -751,6 +751,7 @@ def manager_thread() -> None:
 
   started_prev = False
   ignition_prev = False
+  warned_onroad_reboot = False
 
   # StarPilot variables
   sm = sm.extend(['starpilotPlan'])
@@ -809,8 +810,15 @@ def manager_thread() -> None:
     # Exit main loop when uninstall/shutdown/reboot is needed
     shutdown = False
     for param in ("DoUninstall", "DoShutdown", "DoReboot"):
+      if param == "DoReboot" and started:
+        if params.get_bool(param):
+          if not warned_onroad_reboot:
+            cloudlog.warning("ignoring DoReboot while onroad; deferring until offroad")
+            warned_onroad_reboot = True
+        continue
       if params.get_bool(param):
         shutdown = True
+        warned_onroad_reboot = False
         params.put("LastManagerExitReason", f"{param} {datetime.datetime.now()}")
         cloudlog.warning(f"Shutting down manager - {param} set")
 

system/updated/tests/test_base.py

@@ -227,6 +227,27 @@ class ParamsBaseUpdateTest(TestBaseUpdate):
       self._test_params("master", False, True)
       self._test_finalized_update("master", *self.MOCK_RELEASES["master"])
 
+  def test_download_blocked_onroad(self):
+    self.setup_remote_release("release3")
+    self.setup_basedir_release("release3")
+
+    with self.additional_context(), processes_context(["updated"]) as [updated]:
+      self.wait_for_idle()
+
+      self.MOCK_RELEASES["release3"] = ("0.1.3", "1.2", "0.1.3 release notes")
+      self.update_remote_release("release3")
+
+      self.send_check_for_updates_signal(updated)
+      self.wait_for_fetch_available()
+      self._test_params("release3", True, False)
+
+      self.params.put_bool("IsOffroad", False)
+      self.send_download_signal(updated)
+      self.wait_for_idle()
+
+      assert not self.params.get_bool("UpdateAvailable")
+      assert not get_consistent_flag(str(self.staging_root / "finalized"))
+
   def test_agnos_update(self, mocker):
     # Start on release3, push an update with an agnos change
     self.setup_remote_release("release3")

system/updated/updated.py

@@ -47,6 +47,11 @@ class UserRequest:
   CHECK = 1
   FETCH = 2
 
+
+class UpdateAborted(Exception):
+  pass
+
+
 class WaitTimeHelper:
   def __init__(self):
     self.ready_event = threading.Event()
@@ -75,6 +80,38 @@ def run(cmd: list[str], cwd: str = None) -> str:
   return subprocess.check_output(cmd, cwd=cwd, stderr=subprocess.STDOUT, encoding='utf8')
 
 
+def run_with_offroad_abort(cmd: list[str], params: Params, cwd: str = None, poll_interval: float = 0.5) -> str:
+  proc = subprocess.Popen(cmd, cwd=cwd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True)
+  output: list[str] = []
+
+  try:
+    while True:
+      try:
+        stdout, _ = proc.communicate(timeout=poll_interval)
+        if stdout:
+          output.append(stdout)
+        if proc.returncode:
+          raise subprocess.CalledProcessError(proc.returncode, cmd, ''.join(output))
+        return ''.join(output)
+      except subprocess.TimeoutExpired:
+        if not params.get_bool("IsOffroad"):
+          proc.terminate()
+          try:
+            stdout, _ = proc.communicate(timeout=5)
+            if stdout:
+              output.append(stdout)
+          except subprocess.TimeoutExpired:
+            proc.kill()
+            stdout, _ = proc.communicate()
+            if stdout:
+              output.append(stdout)
+          raise UpdateAborted(f"aborted {' '.join(cmd)} because vehicle went onroad")
+  finally:
+    if proc.poll() is None:
+      proc.kill()
+      proc.communicate()
+
+
 def set_consistent_flag(consistent: bool) -> None:
   os.sync()
   consistent_file = Path(os.path.join(FINALIZED, ".overlay_consistent"))
@@ -374,7 +411,12 @@ class Updater:
     else:
       cloudlog.info(f"up to date on {cur_branch} ({str(cur_commit)[:7]})")
 
+  def require_offroad(self, context: str) -> None:
+    if not self.params.get_bool("IsOffroad"):
+      raise UpdateAborted(f"{context} blocked because vehicle is onroad")
+
   def fetch_update(self) -> None:
+    self.require_offroad("update fetch")
     cloudlog.info("attempting git fetch inside staging overlay")
 
     self.params.put("UpdaterState", "downloading...")
@@ -388,7 +430,7 @@ class Updater:
     run(["git", "config", "--replace-all", "remote.origin.fetch", "+refs/heads/*:refs/remotes/origin/*"], OVERLAY_MERGED)
 
     branch = self.target_branch
-    git_fetch_output = run(["git", "fetch", "origin", branch], OVERLAY_MERGED)
+    git_fetch_output = run_with_offroad_abort(["git", "fetch", "origin", branch], self.params, OVERLAY_MERGED)
     cloudlog.info("git fetch success: %s", git_fetch_output)
 
     cloudlog.info("git reset in progress")
@@ -401,14 +443,19 @@ class Updater:
       ["git", "submodule", "update", "--init", "--recursive"],
       ["git", "submodule", "foreach", "--recursive", "git", "reset", "--hard"],
     ]
-    r = [run(cmd, OVERLAY_MERGED) for cmd in cmds]
+    r = []
+    for cmd in cmds:
+      self.require_offroad("update apply")
+      r.append(run_with_offroad_abort(cmd, self.params, OVERLAY_MERGED))
     cloudlog.info("git reset success: %s", '\n'.join(r))
 
     # TODO: show agnos download progress
     if AGNOS:
+      self.require_offroad("AGNOS update")
       handle_agnos_update()
 
     # Create the finalized, ready-to-swap update
+    self.require_offroad("update finalization")
     self.params.put("UpdaterState", "finalizing update...")
     finalize_update()
     cloudlog.info("finalize success!")
@@ -484,7 +531,8 @@ def main() -> None:
 
         update_failed_count += 1
 
-        if manual_update_requested or user_requested_action or (params.get_bool("IsOffroad") and automatic_updates_enabled):
+        should_check = manual_update_requested or user_requested_action or (params.get_bool("IsOffroad") and automatic_updates_enabled)
+        if should_check:
           # check for update
           params.put("UpdaterState", "checking...")
           updater.check_for_update()
@@ -497,6 +545,8 @@ def main() -> None:
             cloudlog.info("skipping fetch, connection metered")
           elif wait_helper.user_request == UserRequest.CHECK:
             cloudlog.info("skipping fetch, only checking")
+          elif not params.get_bool("IsOffroad"):
+            cloudlog.info("skipping fetch, vehicle went onroad")
           else:
             updater.fetch_update()
             write_time_to_param(params, "UpdaterLastFetchTime")
@@ -515,6 +565,10 @@ def main() -> None:
         )
         exception = f"command failed: {e.cmd}\n{e.output}"
         OVERLAY_INIT.unlink(missing_ok=True)
+      except UpdateAborted as e:
+        cloudlog.warning(str(e))
+        exception = str(e)
+        OVERLAY_INIT.unlink(missing_ok=True)
       except Exception as e:
         cloudlog.exception("uncaught updated exception, shouldn't happen")
         exception = str(e)