From 103c44bfd8f903798abd471cba04f29547425c27 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 21 May 2026 22:35:02 -0500 Subject: [PATCH] =?UTF-8?q?T=C5=AFne?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- opendbc_repo/opendbc/car/gm/carcontroller.py | 19 ++++++- opendbc_repo/opendbc/car/gm/carstate.py | 12 ++++- opendbc_repo/opendbc/car/gm/gmcan.py | 4 +- opendbc_repo/opendbc/car/gm/tests/test_gm.py | 50 ++++++++++++++++++- .../opendbc/car/toyota/carcontroller.py | 12 +++++ .../opendbc/car/toyota/tests/test_toyota.py | 18 ++++++- selfdrive/controls/lib/latcontrol_torque.py | 16 +++--- 7 files changed, 114 insertions(+), 17 deletions(-) diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index 599d357dd..0fc58014d 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -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) diff --git a/opendbc_repo/opendbc/car/gm/carstate.py b/opendbc_repo/opendbc/car/gm/carstate.py index 42c0cc00b..78fc88623 100644 --- a/opendbc_repo/opendbc/car/gm/carstate.py +++ b/opendbc_repo/opendbc/car/gm/carstate.py @@ -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 diff --git a/opendbc_repo/opendbc/car/gm/gmcan.py b/opendbc_repo/opendbc/car/gm/gmcan.py index b60e0bcbb..d0b8e17a4 100644 --- a/opendbc_repo/opendbc/car/gm/gmcan.py +++ b/opendbc_repo/opendbc/car/gm/gmcan.py @@ -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) diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index 3e30af2db..33435f76c 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -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 diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 66b58fc87..f30ddb601 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -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)) diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index bf718113d..b249e01b8 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -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): diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index c1e02f382..4d0c97945 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -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