diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index 0fc58014d..86f9a6562 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -92,6 +92,29 @@ def get_acc_dashboard_fcw_alert(hud_alert, CS): return 0 +def get_acc_dashboard_status_values(enabled, target_speed_kph, hud_control, CS): + if enabled: + return { + "ACCCruiseState": 0, + "ACCLeadCar": int(hud_control.leadVisible) & 0x1, + "ACCResumeButton": 0, + "ACCSpeedSetpoint": target_speed_kph, + "ACCGapLevel": int(hud_control.leadDistanceBars) & 0x3, + "ACCCmdActive": 1, + } + + # Replay the stock camera dashboard context when openpilot long is enabled + # but openpilot itself is not actively driving the ACC cluster state. + return { + "ACCCruiseState": int(getattr(CS, "stock_acc_cruise_state", 0)) & 0x7, + "ACCLeadCar": int(getattr(CS, "stock_acc_lead_car", 0)) & 0x1, + "ACCResumeButton": int(getattr(CS, "stock_acc_resume_button", 0)) & 0x1, + "ACCSpeedSetpoint": float(getattr(CS, "stock_acc_speed_setpoint_kph", 0.0)), + "ACCGapLevel": int(getattr(CS, "stock_acc_gap_level", 0)) & 0x3, + "ACCCmdActive": int(getattr(CS, "stock_acc_cmd_active", 0)) & 0x1, + } + + ECM_CRUISE_SPOOF_CARS = { CAR.CHEVROLET_BOLT_CC_2017, CAR.CHEVROLET_BOLT_CC_2018_2021, @@ -703,9 +726,10 @@ class CarController(CarControllerBase): CS.auto_hold_engaged = False if should_send_acc_dashboard_status(self.CP, dash_speed_spoof_active): + acc_dashboard_status = get_acc_dashboard_status_values(CC.enabled, hud_v_cruise * CV.MS_TO_KPH, hud_control, CS) 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, fcw_alert)) + can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, + acc_dashboard_status, 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 78fc88623..f5acfb57f 100644 --- a/opendbc_repo/opendbc/car/gm/carstate.py +++ b/opendbc_repo/opendbc/car/gm/carstate.py @@ -81,6 +81,12 @@ class CarState(CarStateBase): self.lkas_enabled = 0 self.pcm_acc_status = AccState.OFF self.stock_fcw_alert = 0 + self.stock_acc_cruise_state = 0 + self.stock_acc_lead_car = 0 + self.stock_acc_resume_button = 0 + self.stock_acc_speed_setpoint_kph = 0.0 + self.stock_acc_gap_level = 0 + self.stock_acc_cmd_active = 0 def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]): if not self.CP.pcmCruise: @@ -273,6 +279,12 @@ class CarState(CarStateBase): 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 + self.stock_acc_cruise_state = int(acc_dashboard_status["ACCCruiseState"]) + self.stock_acc_lead_car = int(acc_dashboard_status["ACCLeadCar"]) + self.stock_acc_resume_button = int(acc_dashboard_status["ACCResumeButton"]) + self.stock_acc_speed_setpoint_kph = float(acc_dashboard_status["ACCSpeedSetpoint"]) + self.stock_acc_gap_level = int(acc_dashboard_status["ACCGapLevel"]) + self.stock_acc_cmd_active = int(acc_dashboard_status["ACCCmdActive"]) # 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"]) diff --git a/opendbc_repo/opendbc/car/gm/gmcan.py b/opendbc_repo/opendbc/car/gm/gmcan.py index d0b8e17a4..50bc91d86 100644 --- a/opendbc_repo/opendbc/car/gm/gmcan.py +++ b/opendbc_repo/opendbc/car/gm/gmcan.py @@ -209,17 +209,18 @@ 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_alert): - target_speed = min(target_speed_kph, 255) +def create_acc_dashboard_command(packer, bus, status_values, fcw_alert): + target_speed = min(max(float(status_values.get("ACCSpeedSetpoint", 0.0)), 0.0), 255.0) values = { "ACCAlwaysOne": 1, - "ACCResumeButton": 0, + "ACCCruiseState": int(status_values.get("ACCCruiseState", 0)) & 0x7, + "ACCResumeButton": int(status_values.get("ACCResumeButton", 0)) & 0x1, "ACCSpeedSetpoint": target_speed, - "ACCGapLevel": hud_control.leadDistanceBars * enabled, # 3 "far", 0 "inactive" - "ACCCmdActive": enabled, + "ACCGapLevel": int(status_values.get("ACCGapLevel", 0)) & 0x3, + "ACCCmdActive": int(status_values.get("ACCCmdActive", 0)) & 0x1, "ACCAlwaysOne2": 1, - "ACCLeadCar": hud_control.leadVisible, + "ACCLeadCar": int(status_values.get("ACCLeadCar", 0)) & 0x1, "FCWAlert": int(fcw_alert) & 0x3, } diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index 33435f76c..8df3dcf27 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -9,6 +9,7 @@ from opendbc.car.gm import gmcan from opendbc.car.gm.carcontroller import ( VisualAlert, get_acc_dashboard_fcw_alert, + get_acc_dashboard_status_values, should_send_acc_dashboard_status, should_send_cc_button_spam, should_spoof_dash_speed, @@ -259,9 +260,14 @@ class TestGMCarController: msg = gmcan.create_acc_dashboard_command( packer, 0, - True, - 100, - SimpleNamespace(leadDistanceBars=3, leadVisible=True), + { + "ACCCruiseState": 0, + "ACCLeadCar": 1, + "ACCResumeButton": 0, + "ACCSpeedSetpoint": 100, + "ACCGapLevel": 3, + "ACCCmdActive": 1, + }, 0x2, ) @@ -269,6 +275,24 @@ class TestGMCarController: assert parser.vl["ASCMActiveCruiseControlStatus"]["FCWAlert"] == 2 + def test_acc_dashboard_command_can_replay_stock_status_payload(self): + packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_ASCM][Bus.pt]) + msg = gmcan.create_acc_dashboard_command( + packer, + 0, + { + "ACCCruiseState": 0, + "ACCLeadCar": 1, + "ACCResumeButton": 0, + "ACCSpeedSetpoint": 50, + "ACCGapLevel": 2, + "ACCCmdActive": 0, + }, + 0x3, + ) + + assert msg[1].hex() == "010023200113" + def test_acc_dashboard_fcw_alert_prefers_openpilot_alert(self): cs = SimpleNamespace( stock_fcw_alert=1, @@ -292,3 +316,45 @@ class TestGMCarController: ) assert get_acc_dashboard_fcw_alert(VisualAlert.none, cs) == 0x3 + + def test_acc_dashboard_status_values_use_openpilot_hud_when_enabled(self): + cs = SimpleNamespace( + stock_acc_cruise_state=5, + stock_acc_lead_car=0, + stock_acc_resume_button=1, + stock_acc_speed_setpoint_kph=42.0, + stock_acc_gap_level=1, + stock_acc_cmd_active=0, + ) + + values = get_acc_dashboard_status_values(True, 105.0, SimpleNamespace(leadDistanceBars=3, leadVisible=True), cs) + + assert values == { + "ACCCruiseState": 0, + "ACCLeadCar": 1, + "ACCResumeButton": 0, + "ACCSpeedSetpoint": 105.0, + "ACCGapLevel": 3, + "ACCCmdActive": 1, + } + + def test_acc_dashboard_status_values_reuse_stock_camera_status_when_disabled(self): + cs = SimpleNamespace( + stock_acc_cruise_state=0, + stock_acc_lead_car=1, + stock_acc_resume_button=0, + stock_acc_speed_setpoint_kph=50.0, + stock_acc_gap_level=2, + stock_acc_cmd_active=0, + ) + + values = get_acc_dashboard_status_values(False, 0.0, SimpleNamespace(leadDistanceBars=0, leadVisible=False), cs) + + assert values == { + "ACCCruiseState": 0, + "ACCLeadCar": 1, + "ACCResumeButton": 0, + "ACCSpeedSetpoint": 50.0, + "ACCGapLevel": 2, + "ACCCmdActive": 0, + }