Enemy of Hex Wrench

2026-08-22 08:43:54 +08:00 · 2026-08-04 22:33:51 -05:00
parent 6334045800
commit d21e13ba8f
15 changed files with 176 additions and 26 deletions
@@ -621,8 +621,7 @@ class CarState(CarStateBase):
      ("LFAHDA_CLUSTER", 0),  # optional: carries cluster icon state on some variants
      ("BLINKER_STALKS", 0),  # optional: some trims publish live stalk/light state on ECAN during turn camera events
    ]
-    if CP.carFingerprint in CANFD_ANGLE_LONGITUDINAL_CAR and CP.enableBsm:
-      # Keep the suppressed ADAS BSM output optional.
+    if CP.enableBsm:
      msgs.append(("BLINDSPOTS_REAR_CORNERS", 0))
    if CP.carFingerprint in CANFD_ANGLE_LONGITUDINAL_CAR:
      msgs.append(("BLINDSPOTS_FRONT_CORNER_2", 0))
@@ -280,6 +280,13 @@ class CarInterface(CarInterfaceBase):
    if candidate == CAR.HYUNDAI_IONIQ_6:
      ret.longitudinalActuatorDelay = 0.6

+    if candidate == CAR.HYUNDAI_SANTA_FE_2022:
+      ret.longitudinalActuatorDelay = 0.4
+      ret.longitudinalTuning.kpBP = [0.0, 8.0, 20.0, 35.0]
+      ret.longitudinalTuning.kpV = [0.20, 0.17, 0.12, 0.08]
+      ret.longitudinalTuning.kiBP = [0.0, 8.0, 20.0, 35.0]
+      ret.longitudinalTuning.kiV = [0.02, 0.03, 0.05, 0.07]
+
    if candidate == CAR.KIA_EV9 and ret.openpilotLongitudinalControl:
      apply_kia_ev9_longitudinal_params(ret)

@@ -120,6 +120,17 @@ def get_test_toggles() -> SimpleNamespace:


 class TestHyundaiFingerprint:
+  def test_canfd_torque_bsm_parser_registers_rear_blindspots(self):
+    CP = CarParams.new_message()
+    CP.carFingerprint = CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN
+    CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.EV | HyundaiFlags.CANFD_ALT_GEARS_2)
+    CP.enableBsm = True
+
+    parsers = CarState(CP, None).get_can_parsers(CP)
+    pt_messages = {state.name for state in parsers[Bus.pt].message_states.values()}
+
+    assert "BLINDSPOTS_REAR_CORNERS" in pt_messages
+
  def test_feature_detection(self):
    # LKA steering
    for candidate in (CAR.KIA_EV6, CAR.HYUNDAI_IONIQ_6):
@@ -777,6 +788,16 @@ class TestHyundaiFingerprint:

    assert CP.longitudinalActuatorDelay == pytest.approx(0.22)

+  def test_santa_fe_2022_longitudinal_tune_tracks_slow_scc_response(self):
+    toggles = get_test_toggles()
+    CP = CarInterface.get_params(CAR.HYUNDAI_SANTA_FE_2022, gen_empty_fingerprint(), [], True, False, False, toggles)
+
+    assert CP.longitudinalActuatorDelay == pytest.approx(0.4)
+    assert list(CP.longitudinalTuning.kpBP) == pytest.approx([0.0, 8.0, 20.0, 35.0])
+    assert list(CP.longitudinalTuning.kpV) == pytest.approx([0.20, 0.17, 0.12, 0.08])
+    assert list(CP.longitudinalTuning.kiBP) == pytest.approx([0.0, 8.0, 20.0, 35.0])
+    assert list(CP.longitudinalTuning.kiV) == pytest.approx([0.02, 0.03, 0.05, 0.07])
+
  def test_kia_niro_phev_2022_longitudinal_params_soften_final_stop_hold(self):
    toggles = get_test_toggles()
    CP = CarInterface.get_params(CAR.KIA_NIRO_PHEV_2022, gen_empty_fingerprint(), [], True, False, False, toggles)
@@ -574,6 +574,7 @@ FW_VERSIONS = {
    (Ecu.abs, 0x7b0, None): [
      b'\xa1 #\x14\x00',
      b'\xa1 #\x17\x00',
+      b'\xa1 $\x17\x00',
    ],
    (Ecu.eps, 0x746, None): [
      b'+\xc0\x10\x11\x00',
@@ -585,10 +586,12 @@ FW_VERSIONS = {
    (Ecu.engine, 0x7a2, None): [
      b'\xed,\xa0q\x07',
      b'\xed,\xa2q\x07',
+      b'\xfb,\xa2p\x07',
    ],
    (Ecu.transmission, 0x7a3, None): [
      b'\xa8\x8e\xf41\x00',
      b'\xa8\xfe\xf41\x00',
+      b'\xa9\x17w!r',
    ],
  },
 }
@@ -4,6 +4,7 @@ import pytest

 from opendbc.car.subaru.carcontroller import CarController
 from opendbc.car.subaru.fingerprints import FW_VERSIONS
+from opendbc.car.fw_versions import match_fw_to_car
 from opendbc.car.subaru.interface import CarInterface
 from opendbc.car.subaru.values import CAR, SubaruFlags, SubaruSafetyFlags
 from opendbc.car.structs import CarParams
@@ -66,6 +67,23 @@ class TestSubaruFingerprint:
        for fw in fws:
          assert len(fw) == fw_size, f"{platform} {ecu}: {len(fw)} {fw_size}"

+  def test_outback_2024_firmware(self):
+    outback_fw = FW_VERSIONS[CAR.SUBARU_OUTBACK_2023]
+    assert b'\xa1 $\x17\x00' in outback_fw[(CarParams.Ecu.abs, 0x7b0, None)]
+    assert b'\xfb,\xa2p\x07' in outback_fw[(CarParams.Ecu.engine, 0x7a2, None)]
+    assert b'\xa9\x17w!r' in outback_fw[(CarParams.Ecu.transmission, 0x7a3, None)]
+
+    car_fw = [
+      CarParams.CarFw(ecu=CarParams.Ecu.abs, fwVersion=b'\xa1 $\x17\x00', address=0x7b0, brand="subaru"),
+      CarParams.CarFw(ecu=CarParams.Ecu.eps, fwVersion=b'+\xc0\x12\x11\x00', address=0x746, brand="subaru"),
+      CarParams.CarFw(ecu=CarParams.Ecu.fwdCamera, fwVersion=b'\t!\x08\x046\x05!\x08\x01/', address=0x787, brand="subaru"),
+      CarParams.CarFw(ecu=CarParams.Ecu.engine, fwVersion=b'\xfb,\xa2p\x07', address=0x7a2, brand="subaru"),
+      CarParams.CarFw(ecu=CarParams.Ecu.transmission, fwVersion=b'\xa9\x17w!r', address=0x7a3, brand="subaru"),
+    ]
+    exact, matches = match_fw_to_car(car_fw, "4S4BTGUD6R3155987", allow_fuzzy=False, log=False)
+    assert exact
+    assert matches == {CAR.SUBARU_OUTBACK_2023}
+

 ANGLE_PLATFORMS = (
  CAR.SUBARU_FORESTER_2022,
@@ -219,7 +219,7 @@ class CAR(Platforms):
    flags=SubaruFlags.LKAS_ANGLE,
  )
  SUBARU_OUTBACK_2023 = SubaruGen2PlatformConfig(
-    [SubaruCarDocs("Subaru Outback 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d]))],
+    [SubaruCarDocs("Subaru Outback 2023-24", "All", car_parts=CarParts.common([CarHarness.subaru_d]))],
    SUBARU_OUTBACK.specs,
    flags=SubaruFlags.LKAS_ANGLE,
  )
@@ -433,7 +433,8 @@ class Controls:
                                                self.starpilot_toggles, has_lead=long_plan.hasLead,
                                                traffic_mode_enabled=self.sm['starpilotCarState'].trafficModeEnabled,
                                                profile_max_accel=self.sm['starpilotPlan'].maxAcceleration,
-                                                pedal_override=tesla_pedal_override),
+                                                pedal_override=tesla_pedal_override,
+                                                leads=(self.sm['radarState'].leadOne, self.sm['radarState'].leadTwo)),
                                self.starpilot_toggles.max_desired_acceleration))

    # Steering PID loop and lateral MPC
@@ -233,7 +233,7 @@ class LongControl:
    return min(output_accel, float(positive_cap))

  def update(self, active, CS, a_target, should_stop, accel_limits, starpilot_toggles, has_lead=False,
-             traffic_mode_enabled=False, profile_max_accel=0.0, pedal_override=False):
+             traffic_mode_enabled=False, profile_max_accel=0.0, pedal_override=False, leads=None):
    """Update longitudinal control. This updates the state machine and runs a PID loop"""
    self.pid.neg_limit = accel_limits[0]
    self.pid.pos_limit = accel_limits[1]
@@ -286,7 +286,9 @@ class LongControl:

    else:  # LongCtrlState.pid
      a_target = self.vehicle_tuning.shape_gm_truck_accel_target(a_target, CS.vEgo, should_stop)
-      a_target = self.vehicle_tuning.shape_toyota_sienna_accel_target(a_target, CS.vEgo, should_stop)
+      a_target = self.vehicle_tuning.shape_toyota_sienna_accel_target(
+        a_target, CS.vEgo, should_stop, leads=leads,
+      )
      error = a_target - CS.aEgo
      self.update_mpc_mode(self.experimental_mode)
      self.vehicle_tuning.shape_volt_test_tune_integrator(self.pid, error, CS.vEgo)
@@ -25,9 +25,15 @@ GM_TRUCK_TARGET_FILTER_BRAKE_BYPASS = -0.65
 GM_TRUCK_TARGET_FILTER_DROP_BYPASS = 0.45
 TOYOTA_SIENNA_TARGET_FILTER_MIN_SPEED = 12.0
 TOYOTA_SIENNA_TARGET_FILTER_UP_TAU = 0.18
-TOYOTA_SIENNA_TARGET_FILTER_DOWN_TAU = 0.12
+TOYOTA_SIENNA_TARGET_FILTER_DOWN_TAU = 0.24
 TOYOTA_SIENNA_TARGET_FILTER_BRAKE_BYPASS = -0.75
 TOYOTA_SIENNA_TARGET_FILTER_DROP_BYPASS = 0.65
+TOYOTA_SIENNA_COMFORT_FILTER_MIN_SPEED = 5.0
+TOYOTA_SIENNA_COMFORT_FILTER_MIN_DISTANCE = 10.0
+TOYOTA_SIENNA_COMFORT_FILTER_MIN_TTC = 4.5
+TOYOTA_SIENNA_COMFORT_FILTER_MAX_CLOSING_SPEED = 4.0
+TOYOTA_SIENNA_COMFORT_FILTER_MAX_LEAD_BRAKE = 2.5
+TOYOTA_SIENNA_COMFORT_FILTER_BRAKE_BYPASS = -2.5


 def get_bolt_acc_pedal_friction_bias(output_accel, a_target, v_ego):
@@ -159,17 +165,51 @@ class LongControlVehicleTuning:
    self.gm_truck_filtered_a_target += alpha * (float(a_target) - self.gm_truck_filtered_a_target)
    return self.gm_truck_filtered_a_target

-  def shape_toyota_sienna_accel_target(self, a_target, v_ego, should_stop):
-    """Dampen ordinary high-speed lead handoffs without delaying safety braking."""
-    if not self.is_toyota_sienna_4g or v_ego < TOYOTA_SIENNA_TARGET_FILTER_MIN_SPEED or should_stop:
+  def shape_toyota_sienna_accel_target(self, a_target, v_ego, should_stop, leads=None):
+    """Smooth Sienna lead braking only while there is still comfortable stopping room."""
+    if not self.is_toyota_sienna_4g or should_stop:
      self.toyota_sienna_target_filter_initialized = False
      return a_target

-    bypass_filter = (
-      a_target <= TOYOTA_SIENNA_TARGET_FILTER_BRAKE_BYPASS or
-      (self.toyota_sienna_target_filter_initialized and
-       a_target < self.toyota_sienna_filtered_a_target - TOYOTA_SIENNA_TARGET_FILTER_DROP_BYPASS)
-    )
+    comfort_lead = None
+    if leads:
+      active_leads = [
+        lead for lead in leads
+        if bool(getattr(lead, "status", False)) and
+        abs(float(getattr(lead, "yRel", 0.0))) <= 1.75 and
+        float(getattr(lead, "dRel", 0.0)) > 0.0
+      ]
+      if active_leads:
+        comfort_lead = min(active_leads, key=lambda lead: float(getattr(lead, "dRel", 0.0)))
+
+    comfort_filter_active = False
+    if comfort_lead is not None and v_ego >= TOYOTA_SIENNA_COMFORT_FILTER_MIN_SPEED:
+      lead_distance = float(getattr(comfort_lead, "dRel", 0.0))
+      lead_speed = max(float(getattr(comfort_lead, "vLead", 0.0)), 0.0)
+      closing_speed = max(0.0, float(v_ego) - lead_speed)
+      ttc = lead_distance / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf")
+      lead_brake = max(0.0, -float(getattr(comfort_lead, "aLeadK", 0.0)))
+      comfort_filter_active = (
+        lead_distance >= TOYOTA_SIENNA_COMFORT_FILTER_MIN_DISTANCE and
+        ttc >= TOYOTA_SIENNA_COMFORT_FILTER_MIN_TTC and
+        closing_speed <= TOYOTA_SIENNA_COMFORT_FILTER_MAX_CLOSING_SPEED and
+        lead_brake <= TOYOTA_SIENNA_COMFORT_FILTER_MAX_LEAD_BRAKE
+      )
+
+    # Keep the legacy high-speed filter unchanged. The lower-speed entry is only
+    # for a centered lead with enough room to soften a comfort response.
+    if v_ego < TOYOTA_SIENNA_TARGET_FILTER_MIN_SPEED and not comfort_filter_active:
+      self.toyota_sienna_target_filter_initialized = False
+      return a_target
+
+    if comfort_filter_active:
+      bypass_filter = a_target <= TOYOTA_SIENNA_COMFORT_FILTER_BRAKE_BYPASS
+    else:
+      bypass_filter = (
+        a_target <= TOYOTA_SIENNA_TARGET_FILTER_BRAKE_BYPASS or
+        (self.toyota_sienna_target_filter_initialized and
+         a_target < self.toyota_sienna_filtered_a_target - TOYOTA_SIENNA_TARGET_FILTER_DROP_BYPASS)
+      )
    if not self.toyota_sienna_target_filter_initialized or bypass_filter:
      self.toyota_sienna_filtered_a_target = float(a_target)
      self.toyota_sienna_target_filter_initialized = True
@@ -92,7 +92,7 @@ STABLE_FOLLOW_CRUISE_PULLAWAY_MIN_HEADWAY_MARGIN = -0.10
 STABLE_FOLLOW_CRUISE_PULLAWAY_HYSTERESIS_MAX = 1.75
 VISION_FOLLOW_CRUISE_HOLD_MIN_MODEL_PROB = 0.95
 VISION_FOLLOW_CRUISE_HOLD_MAX_CRUISE_ADVANTAGE = 2.0
-NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED = 8.0
+NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED = 4.0
 NEAR_DUPLICATE_IDENTICAL_RADAR_SOURCE_MIN_SPEED = 10.0
 NEAR_DUPLICATE_LEAD_SOURCE_MIN_MODEL_PROB = 0.9
 NEAR_DUPLICATE_LEAD_SOURCE_MAX_LEAD_BRAKE = 0.35
@@ -1042,6 +1042,28 @@ def test_toyota_sienna_target_filter_bypasses_stop_and_urgent_braking():
  assert tuning.shape_toyota_sienna_accel_target(-0.20, 20.0, True) == pytest.approx(-0.20)


+def test_toyota_sienna_target_filter_smooths_comfortable_lead_braking():
+  CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
+  tuning = vehicle_tunes.LongControlVehicleTuning(CP)
+  lead = SimpleNamespace(status=True, yRel=0.0, dRel=24.0, vLead=9.0, aLeadK=-1.2)
+
+  tuning.shape_toyota_sienna_accel_target(0.50, 10.0, False, leads=(lead,))
+  filtered = tuning.shape_toyota_sienna_accel_target(-1.5, 10.0, False, leads=(lead,))
+
+  assert -1.5 < filtered < 0.50
+
+
+def test_toyota_sienna_target_filter_keeps_authority_when_lead_is_urgent():
+  CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
+  tuning = vehicle_tunes.LongControlVehicleTuning(CP)
+  lead = SimpleNamespace(status=True, yRel=0.0, dRel=12.0, vLead=2.0, aLeadK=-2.0)
+
+  tuning.shape_toyota_sienna_accel_target(0.50, 12.0, False, leads=(lead,))
+  urgent = tuning.shape_toyota_sienna_accel_target(-1.5, 12.0, False, leads=(lead,))
+
+  assert urgent == pytest.approx(-1.5)
+
+
 def test_toyota_sienna_target_filter_does_not_change_other_vehicles():
  CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_CAMRY)
  tuning = vehicle_tunes.LongControlVehicleTuning(CP)
@@ -4178,6 +4178,19 @@ def test_near_duplicate_vision_source_hysteresis_applies_at_tesla_city_speed():
  assert lead_1_bias > 0.0


+def test_near_duplicate_vision_source_hysteresis_holds_through_low_speed_stop_approach():
+  v_ego = 5.0
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead_one = make_lead(status=True, d_rel=9.8, v_lead=3.0, a_lead=-0.02, radar=False, model_prob=1.0)
+  lead_two = make_lead(status=True, d_rel=9.9, v_lead=3.02, a_lead=-0.01, radar=False, model_prob=1.0)
+
+  lead_0_bias, lead_1_bias = planner.mpc.get_near_duplicate_lead_source_hysteresis("lead0", lead_one, lead_two, v_ego)
+
+  assert lead_0_bias == 0.0
+  assert lead_1_bias > 0.0
+
+
 def test_stable_follow_cruise_hysteresis_applies_for_radar_lead():
  v_ego = 27.0
  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
@@ -33,7 +33,7 @@ def make_toggles(**overrides):
  return SimpleNamespace(**defaults)


-def make_sm(planner, *, frame: int, v_ego: float, left_blinker: bool, right_blinker: bool = False):
+def make_sm(planner, *, frame: int, v_ego: float, left_blinker: bool, right_blinker: bool = False, standstill: bool = False):
  return FakeSM(frame, {
    "radarState": SimpleNamespace(
      leadOne=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0, modelProb=0.0, radar=False),
@@ -42,7 +42,7 @@ def make_sm(planner, *, frame: int, v_ego: float, left_blinker: bool, right_blin
    "carState": SimpleNamespace(
      vCruise=50.0,
      vEgo=v_ego,
-      standstill=False,
+      standstill=standstill,
      leftBlinker=left_blinker,
      rightBlinker=right_blinker,
    ),
@@ -82,6 +82,32 @@ def test_lateral_resume_delay_zero_keeps_immediate_resume(monkeypatch):
    planner.shutdown()


+def test_turn_signal_keeps_lateral_paused_at_standstill(monkeypatch):
+  planner = make_planner(monkeypatch)
+
+  try:
+    toggles = make_toggles(pause_lateral_below_speed=99.0)
+
+    planner.update(0.0, False, make_sm(planner, frame=1, v_ego=0.0, left_blinker=True, standstill=True), toggles)
+
+    assert planner.lateral_check is False
+  finally:
+    planner.shutdown()
+
+
+def test_standstill_without_turn_signal_keeps_lateral_allowed(monkeypatch):
+  planner = make_planner(monkeypatch)
+
+  try:
+    toggles = make_toggles(pause_lateral_below_speed=99.0)
+
+    planner.update(0.0, False, make_sm(planner, frame=1, v_ego=0.0, left_blinker=False, standstill=True), toggles)
+
+    assert planner.lateral_check is True
+  finally:
+    planner.shutdown()
+
+
 def test_lateral_resume_delay_holds_resume_after_low_speed_turn(monkeypatch):
  planner = make_planner(monkeypatch)

@@ -98,8 +98,6 @@ FRAME_FRAGMENT_SHADER_EXTERNAL_MICI = """
  out vec4 fragColor;
  void main() {
    vec4 color = texture(texture0, fragTexCoord);
-    float gray = dot(color.rgb, vec3(0.299, 0.587, 0.114));
-    color.rgb = mix(vec3(gray), color.rgb, 0.2);
    color.rgb = clamp((color.rgb - 0.5) * 1.2 + 0.5, 0.0, 1.0);
    color.rgb = pow(color.rgb, vec3(1.0/1.28));
    if (enhance_driver == 1) {
@@ -123,8 +121,6 @@ FRAME_FRAGMENT_SHADER_YUV_MICI = VERSION + """
    float y = texture(texture0, fragTexCoord).r;
    vec2 uv = texture(texture1, fragTexCoord).ra - 0.5;
    vec3 rgb = vec3(y + 1.402*uv.y, y - 0.344*uv.x - 0.714*uv.y, y + 1.772*uv.x);
-    float gray = dot(rgb, vec3(0.299, 0.587, 0.114));
-    rgb = mix(vec3(gray), rgb, 0.2);
    rgb = clamp((rgb - 0.5) * 1.2 + 0.5, 0.0, 1.0);
    if (enhance_driver == 1) {
      float brightness = 1.1;
@@ -153,17 +153,19 @@ class StarPilotPlanner:
    self.lateral_acceleration = v_ego**2 * sm["controlsState"].curvature
    self.driving_in_curve = abs(self.lateral_acceleration) >= MINIMUM_LATERAL_ACCELERATION

+    CS = sm["carState"]
+    blinker_on = CS.leftBlinker or CS.rightBlinker
+    signal_pause = blinker_on and starpilot_toggles.pause_lateral_below_signal
+
    self.lateral_check = v_ego >= starpilot_toggles.pause_lateral_below_speed
-    self.lateral_check |= not (sm["carState"].leftBlinker or sm["carState"].rightBlinker) and starpilot_toggles.pause_lateral_below_signal
-    self.lateral_check |= sm["carState"].standstill
+    self.lateral_check |= not blinker_on and starpilot_toggles.pause_lateral_below_signal
+    self.lateral_check |= CS.standstill and not signal_pause
    self.lateral_check &= not sm["starpilotCarState"].pauseLateral

    # Blinker-based lateral resume delay: after blinker turns off, delay lateral
    # resumption if the vehicle went below half the pause speed during the blinker.
    # This lets the driver manually straighten the wheel after a turn without
    # openpilot fighting them.
-    CS = sm["carState"]
-    blinker_on = CS.leftBlinker or CS.rightBlinker
    prev_blinker_on = self.CS_prev_left_blinker or self.CS_prev_right_blinker

    if blinker_on: