Tůne

2026-08-05 16:26:14 +08:00 · 2026-05-21 22:35:02 -05:00
parent bfcd43452e
commit 103c44bfd8
7 changed files with 114 additions and 17 deletions
@@ -77,6 +77,21 @@ def should_send_acc_dashboard_status(CP, dash_speed_spoof_active):
  return status_car and (dash_speed_spoof_active or volt_camera_no_camera)


+def get_acc_dashboard_fcw_alert(hud_alert, CS):
+  if hud_alert == VisualAlert.fcw:
+    return 0x3
+
+  stock_fcw_alert = int(getattr(CS, "stock_fcw_alert", 0)) & 0x3
+  if stock_fcw_alert != 0:
+    return stock_fcw_alert
+
+  cs_out = getattr(CS, "out", None)
+  if cs_out is not None and (getattr(cs_out, "stockAeb", False) or getattr(cs_out, "stockFcw", False)):
+    return 0x3
+
+  return 0
+
+
 ECM_CRUISE_SPOOF_CARS = {
  CAR.CHEVROLET_BOLT_CC_2017,
  CAR.CHEVROLET_BOLT_CC_2018_2021,
@@ -688,9 +703,9 @@ class CarController(CarControllerBase):
            CS.auto_hold_engaged = False

        if should_send_acc_dashboard_status(self.CP, dash_speed_spoof_active):
-          send_fcw = hud_alert == VisualAlert.fcw
+          fcw_alert = get_acc_dashboard_fcw_alert(hud_alert, CS)
          can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled,
-                                                              hud_v_cruise * CV.MS_TO_KPH, hud_control, send_fcw))
+                                                              hud_v_cruise * CV.MS_TO_KPH, hud_control, fcw_alert))

      # Radar needs to know current speed and yaw rate (50hz),
      # and that ADAS is alive (10hz)
@@ -80,6 +80,7 @@ class CarState(CarStateBase):
    self.lkas_previously_enabled = 0
    self.lkas_enabled = 0
    self.pcm_acc_status = AccState.OFF
+    self.stock_fcw_alert = 0

  def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]):
    if not self.CP.pcmCruise:
@@ -265,9 +266,16 @@ class CarState(CarStateBase):

    ret.cruiseState.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF
    ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL
+    self.stock_fcw_alert = 0
    if self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.flags & GMFlags.NO_CAMERA.value:
-      if self.CP.carFingerprint not in CC_ONLY_CAR:
-        ret.cruiseState.speed = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCSpeedSetpoint"] * CV.KPH_TO_MS
+      has_acc_dashboard_status = self.CP.carFingerprint not in CC_ONLY_CAR or self.CP.carFingerprint == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL
+      if has_acc_dashboard_status:
+        acc_dashboard_status = cam_cp.vl["ASCMActiveCruiseControlStatus"]
+        if self.CP.carFingerprint not in CC_ONLY_CAR:
+          ret.cruiseState.speed = acc_dashboard_status["ACCSpeedSetpoint"] * CV.KPH_TO_MS
+        # Preserve the stock camera FCW level from 0x370 so the controller can
+        # replay it when that message is blocked and spoofed by openpilot long.
+        self.stock_fcw_alert = int(acc_dashboard_status["FCWAlert"])

      if self.CP.carFingerprint not in (SDGM_CAR | ASCM_INT):
        ret.stockAeb = cam_cp.vl["AEBCmd"]["AEBCmdActive"] != 0
@@ -209,7 +209,7 @@ def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_s
  return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values)


-def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, hud_control, fcw):
+def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, hud_control, fcw_alert):
  target_speed = min(target_speed_kph, 255)

  values = {
@@ -220,7 +220,7 @@ def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, hud_con
    "ACCCmdActive": enabled,
    "ACCAlwaysOne2": 1,
    "ACCLeadCar": hud_control.leadVisible,
-    "FCWAlert": 0x3 if fcw else 0
+    "FCWAlert": int(fcw_alert) & 0x3,
  }

  return packer.make_can_msg("ASCMActiveCruiseControlStatus", bus, values)
@@ -2,11 +2,17 @@ import pytest
 from types import SimpleNamespace
 from parameterized import parameterized

-from opendbc.can import CANPacker
+from opendbc.can import CANPacker, CANParser
 from opendbc.car import Bus, DT_CTRL
 from opendbc.car.car_helpers import interfaces
 from opendbc.car.gm import gmcan
-from opendbc.car.gm.carcontroller import should_send_acc_dashboard_status, should_send_cc_button_spam, should_spoof_dash_speed
+from opendbc.car.gm.carcontroller import (
+  VisualAlert,
+  get_acc_dashboard_fcw_alert,
+  should_send_acc_dashboard_status,
+  should_send_cc_button_spam,
+  should_spoof_dash_speed,
+)
 import opendbc.car.gm.interface as gm_interface
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.gm.fingerprints import FINGERPRINTS
@@ -246,3 +252,43 @@ class TestGMCarController:
    msgs = gmcan.create_gm_cc_spam_command(packer, controller, cs, actuators, SimpleNamespace(is_metric=False))

    assert [msg[2] for msg in msgs] == [0]
+
+  def test_acc_dashboard_command_preserves_raw_fcw_alert_level(self):
+    packer = CANPacker(DBC[CAR.CHEVROLET_BOLT_ACC_2022_2023][Bus.pt])
+    parser = CANParser(DBC[CAR.CHEVROLET_BOLT_ACC_2022_2023][Bus.pt], [("ASCMActiveCruiseControlStatus", 0)], 0)
+    msg = gmcan.create_acc_dashboard_command(
+      packer,
+      0,
+      True,
+      100,
+      SimpleNamespace(leadDistanceBars=3, leadVisible=True),
+      0x2,
+    )
+
+    parser.update([0, [msg]])
+
+    assert parser.vl["ASCMActiveCruiseControlStatus"]["FCWAlert"] == 2
+
+  def test_acc_dashboard_fcw_alert_prefers_openpilot_alert(self):
+    cs = SimpleNamespace(
+      stock_fcw_alert=1,
+      out=SimpleNamespace(stockAeb=False, stockFcw=False),
+    )
+
+    assert get_acc_dashboard_fcw_alert(VisualAlert.fcw, cs) == 0x3
+
+  def test_acc_dashboard_fcw_alert_replays_stock_camera_alert_level(self):
+    cs = SimpleNamespace(
+      stock_fcw_alert=2,
+      out=SimpleNamespace(stockAeb=False, stockFcw=False),
+    )
+
+    assert get_acc_dashboard_fcw_alert(VisualAlert.none, cs) == 2
+
+  def test_acc_dashboard_fcw_alert_falls_back_to_stock_aeb_event(self):
+    cs = SimpleNamespace(
+      stock_fcw_alert=0,
+      out=SimpleNamespace(stockAeb=True, stockFcw=False),
+    )
+
+    assert get_acc_dashboard_fcw_alert(VisualAlert.none, cs) == 0x3
@@ -127,6 +127,17 @@ def limit_interceptor_pcm_accel(pcm_accel_cmd: float, target_accel: float, stopp
  return limited


+def limit_interceptor_stopping_accel(pcm_accel_cmd: float, stopping: bool, v_ego: float, lead_visible: bool) -> float:
+  if not stopping or lead_visible or pcm_accel_cmd >= 0.0 or v_ego >= 1.5:
+    return pcm_accel_cmd
+
+  # Pedal/SDSU Toyotas can feel abrupt in the last few feet of a no-lead stop
+  # because stopping state holds onto a relatively strong negative accel. Keep
+  # real lead stops untouched, but soften the final crawl into standstill.
+  stop_floor = float(np.interp(v_ego, [0.0, 0.2, 0.5, 0.9, 1.5], [-0.90, -0.95, -1.05, -1.15, -1.30]))
+  return max(pcm_accel_cmd, stop_floor)
+
+
 class CarController(CarControllerBase):
  def __init__(self, dbc_names, CP):
    super().__init__(dbc_names, CP)
@@ -394,6 +405,7 @@ class CarController(CarControllerBase):

        if self.CP.enableGasInterceptorDEPRECATED:
          pcm_accel_cmd = limit_interceptor_pcm_accel(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo)
+          pcm_accel_cmd = limit_interceptor_stopping_accel(pcm_accel_cmd, stopping, CS.out.vEgo, bool(hud_control.leadVisible))

        pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))

@@ -7,7 +7,7 @@ from opendbc.can import CANPacker, CANParser
 from opendbc.car.structs import CarParams
 from opendbc.car.fw_versions import build_fw_dict
 from opendbc.car.toyota import toyotacan
-from opendbc.car.toyota.carcontroller import CarController, limit_interceptor_pcm_accel, update_permit_braking
+from opendbc.car.toyota.carcontroller import CarController, limit_interceptor_pcm_accel, limit_interceptor_stopping_accel, update_permit_braking
 from opendbc.car.toyota.carstate import calculate_interceptor_gas_pressed
 from opendbc.car.toyota.fingerprints import FW_VERSIONS
 from opendbc.car.toyota.values import CAR, DBC, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \
@@ -352,6 +352,22 @@ class TestToyotaCarController:

    assert limited <= 0.0

+  def test_interceptor_stopping_limit_softens_no_lead_final_crawl(self):
+    limited = limit_interceptor_stopping_accel(-1.48, True, 0.5, False)
+
+    assert limited > -1.48
+    assert limited == -1.05
+
+  def test_interceptor_stopping_limit_keeps_visible_lead_stop_untouched(self):
+    limited = limit_interceptor_stopping_accel(-1.48, True, 0.5, True)
+
+    assert limited == -1.48
+
+  def test_interceptor_stopping_limit_keeps_higher_speed_stop_untouched(self):
+    limited = limit_interceptor_stopping_accel(-1.48, True, 2.0, False)
+
+    assert limited == -1.48
+

 class TestToyotaCarState:
  def test_interceptor_gas_pressed_threshold(self):
@@ -347,16 +347,16 @@ 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.12
-IONIQ_EV_OLD_FF_REDUCTION_LEFT = 0.12
-IONIQ_EV_OLD_FF_REDUCTION_RIGHT = 0.24
+IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT = 1.16
+IONIQ_EV_OLD_FF_REDUCTION_LEFT = 0.16
+IONIQ_EV_OLD_FF_REDUCTION_RIGHT = 0.30
 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.03
+IONIQ_EV_OLD_TURN_IN_BOOST_LEFT = 0.01
 IONIQ_EV_OLD_TURN_IN_BOOST_RIGHT = 0.00
 IONIQ_EV_OLD_UNWIND_TAPER_LEFT = 0.26
 IONIQ_EV_OLD_UNWIND_TAPER_RIGHT = 0.06
@@ -420,11 +420,11 @@ IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.10
 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.04
 IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 0.60
 IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.14
-IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.48
-IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 15.5
+IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.62
+IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 17.0
 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 2.0
-IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.55
-IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.12
+IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.45
+IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.14
 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.82
 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.12
 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.10