diff --git a/opendbc_repo/opendbc/car/car.capnp b/opendbc_repo/opendbc/car/car.capnp index 6ea415bb9..a0d3fad8a 100644 --- a/opendbc_repo/opendbc/car/car.capnp +++ b/opendbc_repo/opendbc/car/car.capnp @@ -644,6 +644,8 @@ struct CarParams { fcaGiorgio @32; rivian @33; volkswagenMeb @34; + teslaPreAP @35; + volvo @36; } enum SteerControlType { diff --git a/opendbc_repo/opendbc/car/honda/tests/test_bosch_a_radar.py b/opendbc_repo/opendbc/car/honda/tests/test_bosch_a_radar.py index a4f58d03e..0d3fcaa72 100644 --- a/opendbc_repo/opendbc/car/honda/tests/test_bosch_a_radar.py +++ b/opendbc_repo/opendbc/car/honda/tests/test_bosch_a_radar.py @@ -1107,13 +1107,12 @@ def test_crv_5g_bosch_a_radar_dbc_wired_for_parser_unit_tests(): assert ri.rcp.bus == CanBus(cp).camera -def test_accord_bosch_a_radar_dbc_wired_for_parser_unit_tests(): +def test_accord_bosch_a_radar_stays_disabled_until_validated(): cp = CarInterface.get_non_essential_params(CAR.HONDA_ACCORD) - assert cp.radarUnavailable is False + assert cp.radarUnavailable is True ri = CarInterface.RadarInterface(cp) - assert ri.bosch_a_radar is True - assert ri.rcp is not None - assert ri.rcp.bus == CanBus(cp).camera + assert ri.bosch_a_radar is False + assert ri.rcp is None def test_civic_bosch_object_feed_uses_camera_side_acc_can(): @@ -1136,7 +1135,7 @@ _EXPECTED_BOSCH_A_CARS = frozenset({ CAR.HONDA_E, CAR.HONDA_E_ADVANCE, }) -_VERIFIED_BOSCH_A_CARS = frozenset({CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G}) +_VERIFIED_BOSCH_A_CARS = frozenset({CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G}) _EXCLUDED_BOSCH_CARS = [ CAR.HONDA_CIVIC_2022, @@ -1168,7 +1167,7 @@ def test_bosch_a_gate_stays_closed_for_non_bosch_a_platforms(car): def test_bosch_a_verified_platform_gate_can_open(): - for car in (CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G): + for car in (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G): cp = CarInterface.get_non_essential_params(car) assert cp.radarUnavailable is False @@ -1188,7 +1187,7 @@ def test_bosch_a_toggle_defaults_on_but_allowlist_still_gates_platforms(): try: Params().remove("HondaBoschARadar") assert CarInterface.get_non_essential_params(CAR.HONDA_CIVIC_BOSCH).radarUnavailable is False - assert CarInterface.get_non_essential_params(CAR.HONDA_ACCORD).radarUnavailable is False + assert CarInterface.get_non_essential_params(CAR.HONDA_ACCORD).radarUnavailable is True assert CarInterface.get_non_essential_params(CAR.HONDA_ACCORD_11G).radarUnavailable is True finally: Params().put_bool("HondaBoschARadar", original) diff --git a/opendbc_repo/opendbc/car/honda/values.py b/opendbc_repo/opendbc/car/honda/values.py index ea23969be..86953ae44 100644 --- a/opendbc_repo/opendbc/car/honda/values.py +++ b/opendbc_repo/opendbc/car/honda/values.py @@ -533,10 +533,7 @@ HONDA_BOSCH_ALT_RADAR = CAR.with_flags(HondaFlags.BOSCH_ALT_RADAR) # HondaBoschARadar. This describes hardware compatibility only; it is deliberately separate from the # verified set below so a newly supported model cannot start using unvalidated radar data by accident. HONDA_BOSCH_A = HONDA_BOSCH - HONDA_BOSCH_RADARLESS - HONDA_BOSCH_CANFD - HONDA_BOSCH_ALT_RADAR -# Add individual CAR entries only after the exact platform has a real capture and decoder replay -# validation. Civic, CR-V 5G, and 2018-22 Accord captures exercise the plain Bosch-A object bank; -# every other Bosch-A variant remains disabled until it gets the same verification. -HONDA_BOSCH_A_RADAR_VERIFIED = frozenset({CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G}) +HONDA_BOSCH_A_RADAR_VERIFIED = frozenset({CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G}) HONDA_BOSCH_TJA_CONTROL = CAR.with_flags(HondaFlags.BOSCH_TJA_CONTROL) HONDA_CAMERA_MESSAGE_CARS = { CAR.HONDA_ACCORD, diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 9fcaf74e7..eab034279 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -180,13 +180,6 @@ def should_use_ev6_gt_line_stop_direct_tracking(ev6_gt_line: bool, stopping: boo return bool(ev6_gt_line and stopping and v_ego > EV6_GT_LINE_STOP_BRAKE_CAP_MAX_SPEED and accel_cmd < actual_accel) -def apply_carnival_steering_override(car_fingerprint, steering_pressed: bool, - apply_steer_req: bool, apply_torque: int) -> tuple[bool, int]: - if car_fingerprint == CAR.KIA_CARNIVAL_2025 and steering_pressed: - return False, 0 - return apply_steer_req, apply_torque - - def update_ev9_longitudinal_tuning(state: EV9LongitudinalTuningState, enabled: bool, stopping: bool, v_ego: float) -> EV9LongitudinalTuningState: if not enabled: @@ -620,10 +613,6 @@ class CarController(CarControllerBase): if not CC.latActive: apply_torque = 0 - apply_steer_req, apply_torque = apply_carnival_steering_override( - self.CP.carFingerprint, CS.out.steeringPressed, apply_steer_req, apply_torque, - ) - # Hold torque with induced temporary fault when cutting the actuation bit # FIXME: we don't use this with CAN FD? torque_fault = CC.latActive and not apply_steer_req diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index d7386d03e..824876b5b 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -20,7 +20,6 @@ from opendbc.car.hyundai.carcontroller import CarController, Ioniq6LongitudinalT should_track_stop_accel_directly_for_car, \ preserve_stock_canfd_lfa_status, \ preserve_stock_canfd_lkas_status, \ - apply_carnival_steering_override, \ suppress_redundant_gv70_brake_cancel from opendbc.car.hyundai.carstate import CarState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state, \ get_canfd_cruise_available @@ -209,12 +208,6 @@ class TestHyundaiFingerprint: assert parser.vl["LKAS_ALT"]["TORQUE_REQUEST"] == 123 assert parser.vl["LKAS_ALT"]["STEER_REQ"] == 1 - def test_carnival_steering_override_is_scoped_to_2025_platform(self): - assert apply_carnival_steering_override(CAR.KIA_CARNIVAL_2025, True, True, 123) == (False, 0) - assert apply_carnival_steering_override(CAR.KIA_CARNIVAL_2025, False, True, 123) == (True, 123) - assert apply_carnival_steering_override(CAR.KIA_CARNIVAL_HEV_4TH_GEN, True, True, 123) == (True, 123) - assert apply_carnival_steering_override(CAR.HYUNDAI_IONIQ_6, True, True, 123) == (True, 123) - def test_canfd_torque_bsm_parser_registers_rear_blindspots(self): CP = CarParams.new_message() CP.carFingerprint = CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index 2f76b5085..c8f1cc264 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -4,7 +4,7 @@ from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, structs from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_std_steer_angle_limits, apply_steer_angle_limits_vm, common_fault_avoidance from opendbc.car.interfaces import CarControllerBase from opendbc.car.subaru import subarucan -from opendbc.car.subaru.values import CAR, DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags +from opendbc.car.subaru.values import CAR, DBC, GLOBAL_ES_ADDR, SUBARU_STOP_START_CARS, CanBus, CarControllerParams, SubaruFlags from opendbc.car.vehicle_model import VehicleModel # FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and @@ -87,12 +87,12 @@ class CarController(CarControllerBase): self.stop_start_acknowledged = False def _stop_start_off_request(self, CC, CS, starpilot_toggles): - """Send one bounded Outback Stop/Start OFF request after ignition. + """Send one bounded Subaru Stop/Start OFF request after ignition. This is intentionally opt-in and limited to a stationary vehicle in Park/Neutral. A single ignition session gets at most one attempt. """ - if self.CP.carFingerprint != CAR.SUBARU_OUTBACK_2023 or \ + if self.CP.carFingerprint not in SUBARU_STOP_START_CARS or \ not getattr(starpilot_toggles, "subaru_stop_start_off", False) or self.stop_start_attempted: return None diff --git a/opendbc_repo/opendbc/car/subaru/interface.py b/opendbc_repo/opendbc/car/subaru/interface.py index 01d52fead..741551a0a 100644 --- a/opendbc_repo/opendbc/car/subaru/interface.py +++ b/opendbc_repo/opendbc/car/subaru/interface.py @@ -3,7 +3,7 @@ from opendbc.car.disable_ecu import disable_ecu from opendbc.car.interfaces import CarInterfaceBase from opendbc.car.subaru.carcontroller import CarController from opendbc.car.subaru.carstate import CarState -from opendbc.car.subaru.values import CAR, CanBus, GLOBAL_ES_ADDR, SubaruFlags, SubaruSafetyFlags +from opendbc.car.subaru.values import CAR, CanBus, GLOBAL_ES_ADDR, SUBARU_STOP_START_CARS, SubaruFlags, SubaruSafetyFlags class CarInterface(CarInterfaceBase): @@ -40,7 +40,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM.value if ret.flags & SubaruFlags.D_PLATFORM_CAMERA: ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM_CAMERA.value - if candidate == CAR.SUBARU_OUTBACK_2023: + if candidate in SUBARU_STOP_START_CARS: ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.STOP_START_BUTTON.value if candidate in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023): ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.FIXED_ANGLE_LIMITS.value diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index ee68dd0a4..bc612530d 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py @@ -206,8 +206,12 @@ def test_outback_2023_uses_d_platform_bus_layout(): assert CP.lateralSmoothSeconds == pytest.approx(0.4) -def test_stop_start_request_is_bounded_and_uses_live_dashlights(): - CP = CarInterface.get_non_essential_params(CAR.SUBARU_OUTBACK_2023) +@pytest.mark.parametrize("platform, expected_bus", [ + (CAR.SUBARU_OUTBACK_2023, CanBus.alt), + (CAR.SUBARU_LEGACY_2025, CanBus.main), +]) +def test_stop_start_request_is_bounded_and_uses_live_dashlights(platform, expected_bus): + CP = CarInterface.get_non_essential_params(platform) controller = CarController({}, CP) controller.frame = 101 @@ -239,9 +243,9 @@ def test_stop_start_request_is_bounded_and_uses_live_dashlights(): _, can_sends = controller.update(CC, CS, 0, toggles) stop_start_msgs = [msg for msg in can_sends if msg[0] == 0x390] assert len(stop_start_msgs) == 1 - assert stop_start_msgs[0][2] == CanBus.alt - parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("Dashlights", 0)], CanBus.alt) - parser.update([(1, [stop_start_msgs[0]])]) + assert stop_start_msgs[0][2] == expected_bus + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("Dashlights", 0)], expected_bus) + parser.update([(expected_bus, [stop_start_msgs[0]])]) assert parser.vl["Dashlights"]["STOP_START"] == 1 assert parser.vl["Dashlights"]["COUNTER"] == 7 @@ -262,6 +266,7 @@ def test_legacy_2025_uses_gen2_angle_bus_layout(): assert not (CP.flags & SubaruFlags.D_PLATFORM_CAMERA) assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM_CAMERA) assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.FIXED_ANGLE_LIMITS + assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.STOP_START_BUTTON assert CanBus.main_for_cp(CP) == CanBus.main assert CanBus.angle_for_cp(CP) == CanBus.main assert parsers[Bus.pt].bus == CanBus.main diff --git a/opendbc_repo/opendbc/car/subaru/values.py b/opendbc_repo/opendbc/car/subaru/values.py index e1b101506..b766791a9 100644 --- a/opendbc_repo/opendbc/car/subaru/values.py +++ b/opendbc_repo/opendbc/car/subaru/values.py @@ -270,6 +270,12 @@ class CAR(Platforms): ) +SUBARU_STOP_START_CARS = ( + CAR.SUBARU_OUTBACK_2023, + CAR.SUBARU_LEGACY_2025, +) + + SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ diff --git a/opendbc_repo/opendbc/car/tests/routes.py b/opendbc_repo/opendbc/car/tests/routes.py index ef90b9c96..a6edb488c 100644 --- a/opendbc_repo/opendbc/car/tests/routes.py +++ b/opendbc_repo/opendbc/car/tests/routes.py @@ -14,6 +14,7 @@ from opendbc.car.tesla.values import CAR as TESLA from opendbc.car.toyota.values import CAR as TOYOTA from opendbc.car.values import Platform from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN +from opendbc.car.volvo.values import CAR as VOLVO from opendbc.car.body.values import CAR as COMMA from opendbc.car.psa.values import CAR as PSA @@ -105,6 +106,11 @@ non_tested_cars = [ TOYOTA.TOYOTA_COROLLA, TOYOTA.TOYOTA_RAV4H, + # No recorded routes yet + VOLVO.VOLVO_XC40_RECHARGE, + VOLVO.VOLVO_S60_RECHARGE, + VOLVO.POLESTAR_2, + ] non_tested_cars.extend(CC_ONLY_CAR) diff --git a/opendbc_repo/opendbc/car/tests/test_fw_fingerprint.py b/opendbc_repo/opendbc/car/tests/test_fw_fingerprint.py index 6851b10ba..473c3ef1c 100644 --- a/opendbc_repo/opendbc/car/tests/test_fw_fingerprint.py +++ b/opendbc_repo/opendbc/car/tests/test_fw_fingerprint.py @@ -283,6 +283,7 @@ class TestFwFingerprintTiming: 'tesla': 0.1, 'toyota': 0.7, 'volkswagen': 0.65, + 'volvo': 0.0, 'rivian': 0.3, 'psa': 0.1, }, diff --git a/opendbc_repo/opendbc/car/torque_data/override.toml b/opendbc_repo/opendbc/car/torque_data/override.toml index a5876f5e0..599ada1ef 100644 --- a/opendbc_repo/opendbc/car/torque_data/override.toml +++ b/opendbc_repo/opendbc/car/torque_data/override.toml @@ -142,6 +142,8 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "HONDA_NBOX_2G" = [1.2, 1.2, 0.2] "ACURA_TLX_2G" = [1.2, 1.2, 0.15] "PORSCHE_MACAN_MK1" = [2.0, 2.0, 0.2] +"VOLVO_XC40_RECHARGE" = [1.5, 1.5, 0.1] +"VOLVO_S60_RECHARGE" = [1.5, 1.5, 0.1] # Dashcam or fallback configured as ideal car "MOCK" = [10.0, 10, 0.0] diff --git a/opendbc_repo/opendbc/car/torque_data/substitute.toml b/opendbc_repo/opendbc/car/torque_data/substitute.toml index b389afc76..5c4135813 100644 --- a/opendbc_repo/opendbc/car/torque_data/substitute.toml +++ b/opendbc_repo/opendbc/car/torque_data/substitute.toml @@ -136,3 +136,5 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "CHEVROLET_SILVERADO_CC" = "CHEVROLET_SILVERADO" "CADILLAC_XT4_CC" = "CADILLAC_XT4" "CADILLAC_XT6" = "GMC_ACADIA" + +"POLESTAR_2" = "VOLVO_XC40_RECHARGE" diff --git a/opendbc_repo/opendbc/car/values.py b/opendbc_repo/opendbc/car/values.py index f606249a4..d1fc2a541 100644 --- a/opendbc_repo/opendbc/car/values.py +++ b/opendbc_repo/opendbc/car/values.py @@ -14,8 +14,9 @@ from opendbc.car.subaru.values import CAR as SUBARU from opendbc.car.tesla.values import CAR as TESLA from opendbc.car.toyota.values import CAR as TOYOTA from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN +from opendbc.car.volvo.values import CAR as VOLVO -Platform = BODY | CHRYSLER | FORD | GM | HONDA | HYUNDAI | MAZDA | MOCK | NISSAN | PSA | RIVIAN | SUBARU | TESLA | TOYOTA | VOLKSWAGEN +Platform = BODY | CHRYSLER | FORD | GM | HONDA | HYUNDAI | MAZDA | MOCK | NISSAN | PSA | RIVIAN | SUBARU | TESLA | TOYOTA | VOLKSWAGEN | VOLVO BRANDS = get_args(Platform) PLATFORMS: dict[str, Platform] = {str(platform): platform for brand in BRANDS for platform in brand} diff --git a/opendbc_repo/opendbc/car/volvo/__init__.py b/opendbc_repo/opendbc/car/volvo/__init__.py new file mode 100644 index 000000000..4cb6eb394 --- /dev/null +++ b/opendbc_repo/opendbc/car/volvo/__init__.py @@ -0,0 +1 @@ +# Volvo CMA platform support for openpilot diff --git a/opendbc_repo/opendbc/car/volvo/carcontroller.py b/opendbc_repo/opendbc/car/volvo/carcontroller.py new file mode 100644 index 000000000..350479eee --- /dev/null +++ b/opendbc_repo/opendbc/car/volvo/carcontroller.py @@ -0,0 +1,287 @@ +import numpy as np + +from opendbc.can.packer import CANPacker +from opendbc.car import Bus +from opendbc.car.interfaces import CarControllerBase +from opendbc.car.lateral import apply_std_steer_angle_limits +from opendbc.car.volvo.helpers import LCA3CounterSync +from opendbc.car.volvo.volvocan import (create_lca_message, create_pscm_message, create_lca_3_message, create_lca_2_message, create_lca_4_message, + create_lca_5_message, create_lca_6_message, create_lca_7_message, create_pscm_related_message) +from opendbc.car.volvo.values import CarControllerParams + + +class CarController(CarControllerBase): + def __init__(self, dbc_names, CP): + super().__init__(dbc_names, CP) + self.packer = CANPacker(dbc_names[Bus.party]) + self.apply_angle_last = 0.0 # Track last applied steering angle + + self.gear_acc = 60 + self.lca_4_acc = 0 # Bresenham accumulator for 29 Hz + + # Counter management for LCA_2 + self.lca_2_counter_1 = None # Will grab initial value from CarState + self.lca_2_counter_2 = None + + # Counter management for PSCM_RELATED + self.pscm_related_counter = None # Will grab initial value from CarState + + # Counter management for LCA_3 (pattern-based) + self.lca_3_counter_sync = LCA3CounterSync() + + # Counter management for LCA_5 (formerly SPEED_1) + self.lca_5_counter = None # Will grab initial value from CarState + + self.last_lat_active = False # Track state + + self.lca_7_acc = 0 # Bresenham accumulator for 29 Hz + self.lca_7_last_steer = 0 # used to calculate change in steer from last update + + # LCA torque-authority envelope state. Both arms are persistent across frames. + # See CarControllerParams.LCA_AUTH_* and route_analysis/lca_override_mechanism.md. + # When lat_active goes True they ramp up from 0 to ±MAX at REBUILD_RATE; on + # driver override they collapse at COLLAPSE_RATE (symmetric until SPLIT, then + # asymmetric: yielding arm → 0, counter arm holds at ±PLATEAU). + self.lca_auth_pos = 0.0 + self.lca_auth_neg = 0.0 + # "Light contact" rising-edge detector for haptic-ack on resting hands. + # Per-frame |drv| derivative; LIGHT_HOLD_FRAMES counter ticks down while + # the brief-yield window is active and does not re-arm during that window. + self.lca_auth_drv_prev = 0.0 + self.lca_auth_light_frames = 0 + # Frames since real_override was last active — used to gate light_collapse + # re-arming during active co-steering (must be > LIGHT_COOLDOWN_FRAMES). + self.lca_auth_real_off_frames = 1000 # large initial → light can fire immediately + # Override-mode latch with hysteresis (enter at ENTER, exit at EXIT). + # Prevents threshold flapping when driver torque hovers near the boundary, + # which caused ~10 Hz EPS-torque ripple felt during lane-change overrides. + self.lca_auth_override_active = False + # LP-filtered |drv| for yield-arm magnitude calculation. Suppresses 1-2 + # unit driver-torque jitter that would otherwise propagate (~10x amplified + # via YIELD_SLOPE) into envelope ripple felt at the wheel. + self.lca_auth_drv_mag_filt = 0.0 + + def update(self, CC, CS, now_nanos, starpilot_toggles): + can_sends = [] + actuators = CC.actuators + + # Detect disengagement + if not CC.latActive and self.last_lat_active: + #self.lca_commands.reset() # Clear state ← IMPORTANT! + pass + + lat_active = CC.latActive + + # lateral control - angle-based steering + # NOTE: LCA message is sent every frame (even when inactive) to replace stock LCA + # Stock LCA is permanently blocked by panda safety, so we must always send + if self.frame % CarControllerParams.STEER_STEP == 0: # 100 Hz + # Get desired steering angle from controlsd (LatControlAngle) + apply_angle = actuators.steeringAngleDeg # degrees + + # Clamp commanded angle to actual ± ANGLE_ERROR. Without this, a driver override + # lets the model's plan drift far from the wheel's actual position; on release, + # the EPS slams back toward that stale command and overshoots. Stock Volvo Pilot + # Assist keeps this gap inside ~2° even under sustained override. + apply_angle = float(np.clip( + apply_angle, + CS.out.steeringAngleDeg - CarControllerParams.ANGLE_ERROR, + CS.out.steeringAngleDeg + CarControllerParams.ANGLE_ERROR, + )) + + # Rate limit + inactive passthrough (apply_angle = steering angle when not lat_active) + apply_angle = apply_std_steer_angle_limits(apply_angle, self.apply_angle_last, CS.out.vEgoRaw, + CS.out.steeringAngleDeg, lat_active, CarControllerParams.ANGLE_LIMITS) + + # Update LCA torque-authority envelope (replicates stock Pilot Assist's + # easy-override and bounce-free release). Stock holds both arms at ±614 + # in steady state; on override the arms collapse to a shifted plateau + # (counter arm deeper than yielding arm); rebuilds at +230 c/s. + P = CarControllerParams + DT = 0.01 # 100 Hz + # Override trigger uses an explicit |steeringTorque| threshold rather than + # CS.steeringPressed, which is a very-sensitive DM-fallback floor (raw>2) + # — fires from resting hands alone and is not an override-intent signal. + drv_mag = abs(CS.out.steeringTorque) + drv_rate = drv_mag - self.lca_auth_drv_prev + self.lca_auth_drv_prev = drv_mag + # LP filter on |drv| used for yield-arm magnitude — absorbs 1-2 unit + # driver-torque jitter that would otherwise propagate into ~10 unit + # envelope ripple via the YIELD_SLOPE multiplier. + self.lca_auth_drv_mag_filt = ((1.0 - P.LCA_AUTH_YIELD_LP_ALPHA) * self.lca_auth_drv_mag_filt + + P.LCA_AUTH_YIELD_LP_ALPHA * drv_mag) + # Hysteretic override latch — enter at ENTER, hold until drv drops below + # EXIT. Eliminates ~10 Hz envelope flapping when |drv| hovers near a + # single threshold during sustained co-steering. + if not self.lca_auth_override_active and drv_mag > P.LCA_AUTH_OVERRIDE_ENTER: + self.lca_auth_override_active = True + elif self.lca_auth_override_active and drv_mag < P.LCA_AUTH_OVERRIDE_EXIT: + self.lca_auth_override_active = False + real_override = self.lca_auth_override_active + # Track frames since real_override was last active. Used to gate light + # contact re-firing — light_collapse must NOT trigger while the driver + # is actively co-steering (real_override repeatedly entering/exiting). + if real_override: + self.lca_auth_real_off_frames = 0 + else: + self.lca_auth_real_off_frames += 1 + # Per-frame rising edge into the "light contact" zone arms a brief-yield + # window for haptic acknowledgment of hand-on-wheel. Suppressed while + # the window is already active OR real_override has been off less than + # LIGHT_COOLDOWN_FRAMES (i.e., user is actively co-steering). + if (drv_mag > P.LCA_AUTH_LIGHT_THRESH and + drv_rate > P.LCA_AUTH_LIGHT_RISE_DELTA and + self.lca_auth_light_frames == 0 and + self.lca_auth_real_off_frames > P.LCA_AUTH_LIGHT_COOLDOWN_FRAMES): + self.lca_auth_light_frames = P.LCA_AUTH_LIGHT_HOLD_FRAMES + else: + self.lca_auth_light_frames = max(0, self.lca_auth_light_frames - 1) + light_collapse = self.lca_auth_light_frames > 0 + overriding = real_override or light_collapse + # Collapse rate scales with driver torque so a sharp pothole jolt drops the + # envelope faster than a soft sustained press. Floor at base rate so light + # contact still produces a perceptible (but small) dip. + collapse_rate = P.LCA_AUTH_COLLAPSE_RATE * max(1.0, drv_mag / float(P.LCA_AUTH_OVERRIDE_ENTER)) + step = collapse_rate * DT + if not lat_active: + self.lca_auth_pos = 0.0 + self.lca_auth_neg = 0.0 + self.lca_auth_drv_prev = 0.0 + self.lca_auth_light_frames = 0 + self.lca_auth_override_active = False + self.lca_auth_drv_mag_filt = 0.0 + self.lca_auth_real_off_frames = 1000 + elif overriding: + if self.lca_auth_pos > P.LCA_AUTH_SPLIT or -self.lca_auth_neg > P.LCA_AUTH_SPLIT: + # Symmetric collapse phase: both arms shrink toward ±SPLIT + self.lca_auth_pos = max(float(P.LCA_AUTH_SPLIT), self.lca_auth_pos - step) + self.lca_auth_neg = min(-float(P.LCA_AUTH_SPLIT), self.lca_auth_neg + step) + else: + # Asymmetric plateau phase. CS.out.steeringTorque > 0 in openpilot + # convention = driver pushing right → yields right authority + # (LOOSELY/+ arm), retains left (INV/- arm). + # Yield arm scales with |drv| above OVERRIDE_THRESH — strong presses + # (potholes, hard corrections) cross past zero so EPS hands the wheel + # to the driver in their direction. + excess = max(0.0, self.lca_auth_drv_mag_filt - float(P.LCA_AUTH_OVERRIDE_ENTER)) + yield_signed = float(P.LCA_AUTH_YIELD_BASE) - P.LCA_AUTH_YIELD_SLOPE * excess + yield_signed = max(float(P.LCA_AUTH_YIELD_MIN), min(yield_signed, float(P.LCA_AUTH_YIELD_BASE))) + if CS.out.steeringTorque > 0: # driver pushing right + target_pos = yield_signed # yield arm (+ side) + target_neg = -float(P.LCA_AUTH_PLATEAU_COUNTER) # counter arm + else: # driver pushing left (or zero — default to symmetric collapse direction) + target_pos = float(P.LCA_AUTH_PLATEAU_COUNTER) # counter arm + target_neg = -yield_signed # yield arm (− side) + # Drive each arm toward its plateau target at COLLAPSE_RATE + self.lca_auth_pos = max(target_pos, self.lca_auth_pos - step) \ + if self.lca_auth_pos > target_pos \ + else min(target_pos, self.lca_auth_pos + step) + self.lca_auth_neg = min(target_neg, self.lca_auth_neg + step) \ + if self.lca_auth_neg < target_neg \ + else max(target_neg, self.lca_auth_neg - step) + else: + # No override → rebuild both arms toward saturation + rebuild_step = P.LCA_AUTH_REBUILD_RATE * DT + self.lca_auth_pos = min(float(P.LCA_AUTH_MAX), self.lca_auth_pos + rebuild_step) + self.lca_auth_neg = max(-float(P.LCA_AUTH_MAX), self.lca_auth_neg - rebuild_step) + + # LCA - 0x58 - 100 Hz (angle-based) + can_sends.append(create_lca_message(self.packer, lat_active, apply_angle, CS.msg_lca, + authority_pos=int(round(self.lca_auth_pos)), + authority_neg=int(round(self.lca_auth_neg)))) + self.apply_angle_last = apply_angle + + # PSCM (bus 2 -> 0) - 0x16 - 100 Hz + can_sends.append(create_pscm_message(self.packer, lat_active, CS.msg_pscm, self.frame)) + # EGSM - 0x45 - 100 Hz + #can_sends.append(create_egsm_message(self.packer, CS.msg_egsm)) + + # PSCM_RELATED (bus 2 -> 0) - 0x17 - 100 Hz + # Initialize counter from CarState on first run + if self.pscm_related_counter is None: + self.pscm_related_counter = CS.msg_pscm_related['SIG1_BYTE_1_HI_NIBBLE'] + + # Increment counter by +1, wrap from 14 → 0 (modulo 15) + self.pscm_related_counter = (self.pscm_related_counter + 1) % 15 + + can_sends.append(create_pscm_related_message(self.packer, lat_active, CS.pilot_assist_engaged, + CS.msg_pscm_related, self.pscm_related_counter)) + + # LCA_3 - 0x57 - avg 66.66 Hz + #if (self.frame * 67) % 100 < 67: # if (self.frame % 3) < 2: + # 0x57 at ~66.67 Hz: send on 2 out of every 3 frames + # Pattern: send on frame % 3 == 0 or 2, skip when frame % 3 == 1 + if self.frame % 3 != 1: # → 2/3 * 100 Hz = 66.67 Hz + # Update counter with observed value, get counter to send + counter, is_synced = self.lca_3_counter_sync.update(CS.msg_lca_3['COUNTER_1']) + can_sends.append(create_lca_3_message(self.packer, lat_active, apply_angle, CS.msg_lca_3, counter)) + #can_sends.append(create_0x1a_message(self.packer, CS.msg_0x1a)) + + # SPEED messages - 0x60, 0x68 - 50 Hz + if self.frame % 2 == 0: # 50 Hz + #can_sends.append(create_speed_message(self.packer, CS.msg_speed)) + #can_sends.append(create_speed_2_message(self.packer, CS.msg_speed_2)) + pass + + # LCA_2 - 0x69 - 50 Hz + # Spoof PILOT_ASSIST_ENGAGED to keep PSCM accepting LCA commands + if self.frame % 2 == 0: # 50 Hz + # Initialize counters from CarState on first run + if self.lca_2_counter_1 is None: + self.lca_2_counter_1 = CS.msg_lca_2['COUNTER_1'] + self.lca_2_counter_2 = CS.msg_lca_2['COUNTER_2'] + + # Increment counters (COUNTER_1 by +2, COUNTER_2 by +4, both modulo 16) + self.lca_2_counter_1 = (self.lca_2_counter_1 + 2) % 16 + self.lca_2_counter_2 = (self.lca_2_counter_2 + 4) % 16 + + can_sends.append(create_lca_2_message(self.packer, lat_active, CS.msg_lca_2, + self.lca_2_counter_1, self.lca_2_counter_2)) + + # LCA_5 (formerly SPEED_1) - 0x67 - 50 Hz + # Contains wheel speeds + LCA signals (LCA_TURN_BITS, LCA_5_STEER) + if self.frame % 2 == 0: # 50 Hz + # Initialize counter from CarState on first run + if self.lca_5_counter is None: + self.lca_5_counter = CS.msg_lca_5['COUNTER'] + + # Increment counter by +4, wrap at 15 (0xF never used) + self.lca_5_counter = (self.lca_5_counter + 4) % 15 + + can_sends.append(create_lca_5_message(self.packer, lat_active, apply_angle, + CS.msg_lca_5, self.lca_5_counter)) + + # LCA_4 - 0x90 - 29 Hz + # Spoof LCA_ENABLE bits to maintain PA ON state when openpilot is active + # Using Bresenham-style accumulator for precise 29 Hz + self.lca_4_acc += 29 + if self.lca_4_acc >= 100: + self.lca_4_acc -= 100 + can_sends.append(create_lca_4_message(self.packer, lat_active, CS.msg_lca_4, apply_angle)) + + # LCA_6 - 0X97 - 25 Hz + if self.frame % 4 == 0: # 25 Hz + can_sends.append(create_lca_6_message(self.packer, lat_active, CS.msg_lca_6, apply_angle)) + + # LCA_7 - 0x92 - 29 Hz + # Using Bresenham-style accumulator for precise 29 Hz + self.lca_7_acc += 29 + if self.lca_7_acc >= 100: + self.lca_7_acc -= 100 + delta_steer = apply_angle - self.lca_7_last_steer + can_sends.append(create_lca_7_message(self.packer, lat_active, CS.msg_lca_7, apply_angle, delta_steer)) + self.lca_7_last_steer = apply_angle + + # GEAR_POSITION - 0x80 - 40 Hz + #self.gear_acc += 40 # Bresenham-style approach + #if self.gear_acc >= 100: + # self.gear_acc -= 100 + if self.frame % 5 == 0 or self.frame % 5 == 2: # 2/5 * 100 Hz = 40 Hz # openpilot forward delay causes DTC in EGSM, but fixes DTC in PSCM + #can_sends.append(create_gear_position_message(self.packer, CS.msg_gear_position)) + pass + + new_actuators = actuators.as_builder() + new_actuators.steeringAngleDeg = self.apply_angle_last + self.frame += 1 + self.last_lat_active = CC.latActive + return new_actuators, can_sends diff --git a/opendbc_repo/opendbc/car/volvo/carstate.py b/opendbc_repo/opendbc/car/volvo/carstate.py new file mode 100644 index 000000000..f1423e43b --- /dev/null +++ b/opendbc_repo/opendbc/car/volvo/carstate.py @@ -0,0 +1,145 @@ +from cereal import custom +from opendbc.car import structs, Bus +from opendbc.can.parser import CANParser +from opendbc.car.volvo.values import DBC, VolvoSPAPlatformConfig, CAR +from opendbc.car.interfaces import CarStateBase + +GearShifter = structs.CarState.GearShifter +TransmissionType = structs.CarParams.TransmissionType + +# main-bus SPEED (0x60) is raw counts in the DBC; measured against GPS ground speed. +# Must match VOLVO_SPEED_TO_MS in opendbc/safety/modes/volvo.h. +SPEED_TO_MS = 0.003977 + + +class CarState(CarStateBase): + def __init__(self, CP, FPCP): + super().__init__(CP, FPCP) + self.is_spa = isinstance(CAR(CP.carFingerprint).config, VolvoSPAPlatformConfig) + self.gas_pressed_prev = False + self.dispatch_lca_2_msg = False + self.msg_pscm = {} + self.msg_lca = {} + self.msg_lca_2 = {} + self.msg_lca_3 = {} + self.msg_gear_position = {} + self.pilot_assist_engaged = False + self.msg_lca_5 = {} # Formerly msg_speed_1 + self.msg_speed = {} + self.msg_speed_2 = {} + self.msg_0x1a = {} + self.msg_egsm = {} + self.msg_pscm_related = {} + self.msg_lca_4 = {} + self.msg_lca_6 = {} + self.msg_lca_7 = {} + + def update(self, can_parsers, starpilot_toggles) -> structs.CarState: + cp_main = can_parsers[Bus.main] + cp_pt = can_parsers[Bus.pt] + cp_party = can_parsers[Bus.party] + ret = structs.CarState() + + # car speed + # SPEED on the main bus, not BUS1_SPEED on the PT bus: the main bus is identical + # across harnesses, while which car bus lands on PT (bus 1) is not, and the PT DBC + # in use depends on the fingerprint. Regressed against GPS ground speed over two + # routes on different harnesses: r=0.99989 both, residual sd 0.35-0.40 km/h. + ret.vEgoRaw = cp_main.vl["SPEED"]["SPEED"] * SPEED_TO_MS + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgoRaw <= 0.1 # 0.1 m/s + + # gas + # CMA ECM_1.ACCELERATOR_PEDAL_POS is raw 0-255 (DBC factor 1, idle ~20). + # SPA ECM_1.ACCELERATOR_PEDAL_POS is DBC-scaled to percent (factor 0.00390625, idle ~0). + # Thresholds must match volvo.h (see opendbc/safety/modes/volvo.h GAS_PRESSED_THRESHOLD_*) + # and opendbc/safety/tests/test_volvo.py::test_gas_threshold_self_consistent. + if self.is_spa: + ret.gasPressed = cp_pt.vl["ECM_1"]["ACCELERATOR_PEDAL_POS"] > 1.0 # percent + else: + ret.gasPressed = cp_pt.vl["ECM_1"]["ACCELERATOR_PEDAL_POS"] > 20+1 # raw counts, 20 baseline + 1 tolerance + + # brake + #ret.brakePressed = bool(cp_main.vl["LCA_2"]["BRAKE_PEDAL_PRESSED_A"] or cp_main.vl["LCA_2"]["BRAKE_PEDAL_PRESSED_B"]) + # BRAKE_PEDAL_PRESSED_A goes active when user starts pressing brake pedal, but no brake light is on yet due to tolerance + # BRAKE_PEDAL_PRESSED_B goes active when when the brake pedal is pressed above minimum threshold, brake light is on + ret.brakePressed = cp_main.vl["LCA_2"]["BRAKE_PEDAL_PRESSED_B"] == 1 + ret.parkingBrake = False # TODO: add parking brake + + # stability control - becomes true when ESC intervenes (e.g., aquaplaning) + ret.espActive = cp_main.vl["LCA_2"]["ESC_ACTUATING"] == 1 and cp_main.vl["LCA_2"]["ESC_ELIGIBLE"] == 1 + + # steering wheel + ret.steeringAngleDeg = cp_party.vl['PSCM']['PSCM_ANGLE_SENSOR'] # openpilot expects a negative value for a right turn + #ret.steeringAngleDeg = cp_party.vl['SAS']['SAS_ANGLE_SENSOR'] + + # Driver steering torque feedback (used for driver override detection) + ret.steeringTorque = -cp_party.vl['DRIVER_INPUT']['STEERING_DRIVER_INPUT'] # Car right turn is negative, openpilot right turn is positive + ret.steeringPressed = abs(cp_party.vl['DRIVER_INPUT']['STEERING_DRIVER_INPUT']) > 2 + + # EPS status - placeholder until actual signal is found + self.eps_active = True # Assume EPS is active for now + + if self.is_spa: + # SPA: byte 0 bit 1, inverted (0 = cruise on, 1 = cruise off) + cruise_raw = cp_pt.vl["BUS1_CRUISE_CONTROL"]["CRUISE_CONTROL_SPA_ENABLED"] == 1 + else: + # CMA: two separate boolean signals + cruise_raw = cp_pt.vl["BUS1_CRUISE_CONTROL"]["CRUISE_CONTROL_ENABLED"] == 1 or cp_pt.vl["BUS1_CRUISE_CONTROL"]["CRUISE_CONTROL_ENABLED_IDLE_TRAFFIC"] == 1 + + ret.cruiseState.enabled = cruise_raw + + self.gas_pressed_prev = ret.gasPressed + ret.cruiseState.available = True # TODO: Determine actual availability + ret.cruiseState.speed = 0 # TODO: Find cruise set speed (not required for lateral control) + ret.cruiseState.nonAdaptive = False + ret.cruiseState.standstill = ret.standstill # False # Todo: Find cruise control standstill signal + + # gear + gearPosition = cp_main.vl['GEAR_POSITION']['GEAR_POSITION'] # 0: P; 1: R; 2: N; 3: D; 4: B; + if gearPosition == 0: + ret.gearShifter = GearShifter.park + elif gearPosition == 1: + ret.gearShifter = GearShifter.reverse + elif gearPosition == 2: + ret.gearShifter = GearShifter.neutral + elif gearPosition == 3: + ret.gearShifter = GearShifter.drive + elif gearPosition == 4: + ret.gearShifter = GearShifter.drive + + # blinkers TODO FlexRay + ret.leftBlinker = False + ret.rightBlinker = False + + # lock info TODO FlexRay + ret.doorOpen = False # TODO: add door open + ret.seatbeltUnlatched = False # TODO: add seatbelt unlatched + + # Store entire message dictionaries + self.msg_pscm = cp_party.vl['PSCM'] + self.msg_lca = cp_main.vl['LCA'] + self.msg_lca_2 = cp_main.vl['LCA_2'] + self.msg_lca_3 = cp_main.vl['LCA_3'] + self.msg_lca_4 = cp_main.vl['LCA_4'] + self.msg_lca_5 = cp_main.vl['LCA_5'] + self.msg_lca_6 = cp_main.vl['LCA_6'] + self.msg_lca_7 = cp_main.vl['LCA_7'] + self.msg_speed = cp_main.vl['SPEED'] + self.msg_speed_2 = cp_main.vl['SPEED_2'] + self.msg_gear_position = cp_main.vl['GEAR_POSITION'] + self.msg_egsm = cp_party.vl['EGSM'] + self.msg_pscm_related = cp_party.vl['PSCM_RELATED'] + + self.pilot_assist_engaged = cp_main.vl['LCA_2']['PILOT_ASSIST_ENGAGED'] == 1 + + fp_ret = custom.StarPilotCarState.new_message() + return ret, fp_ret + + @staticmethod + def get_can_parsers(CP): + return { + Bus.main: CANParser(DBC[CP.carFingerprint][Bus.main], [], 0), + Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 1), + Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], 2), + } diff --git a/opendbc_repo/opendbc/car/volvo/fingerprints.py b/opendbc_repo/opendbc/car/volvo/fingerprints.py new file mode 100644 index 000000000..df9d6e418 --- /dev/null +++ b/opendbc_repo/opendbc/car/volvo/fingerprints.py @@ -0,0 +1,18 @@ +# ruff: noqa: E501 +""" AUTO-FORMATTED USING opendbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE.""" +from opendbc.car.volvo.values import CAR + +FINGERPRINTS = { + CAR.VOLVO_XC40_RECHARGE: [{ + 21: 8, 22: 8, 23: 8, 26: 8, 69: 8, 85: 8, 87: 8, 88: 8, 96: 8, 103: 8, 104: 8, 105: 8, 128: 8, 144: 8, 146: 8, 147: 8, 151: 8, 304: 8, 336: 8, 339: 8, 341: 8, 394: 8, 395: 8, 640: 8, 656: 8, 666: 8, 773: 8, 778: 8, 1072: 8, 1120: 8, 1302: 8, 1336: 8, 1407: 8, 1422: 8, 1423: 8, 1424: 8, 1425: 8, 1426: 8 + }], + CAR.VOLVO_S60_RECHARGE: [{ + 21: 8, 22: 8, 23: 8, 26: 8, 69: 8, 85: 8, 87: 8, 88: 8, 96: 8, 103: 8, 104: 8, 105: 8, 128: 8, 144: 8, 146: 8, 147: 8, 151: 8, 336: 8, 339: 8, 341: 8, 395: 8, 587: 8, 640: 8, 656: 8, 666: 8, 773: 8, 778: 8, 1072: 8, 1120: 8, 1298: 8, 1302: 8, 1319: 8, 1336: 8, 1407: 8, 1422: 8, 1423: 8, 1424: 8, 1425: 8, 1426: 8, 1554: 8, 1587: 8, 1718: 8, 1792: 8, 1793: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1843: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1871: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1920: 8, 1927: 8, 1937: 8, 1943: 8, 1953: 8, 1954: 8, 1955: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1988: 8, 1990: 8, 2000: 8, 2001: 8, 2002: 8, 2004: 8, 2017: 8, 2018: 8, 2020: 8 + }], + CAR.POLESTAR_2: [{ + 7: 4, 21: 8, 22: 8, 23: 8, 26: 8, 35: 8, 37: 8, 53: 8, 58: 8, 67: 8, 69: 8, 70: 8, 85: 8, 87: 8, 88: 8, 96: 8, 103: 8, 104: 8, 105: 8, 112: 8, 117: 8, 128: 8, 133: 8, 138: 8, 144: 8, 146: 8, 147: 8, 151: 8, 256: 8, 277: 8, 278: 8, 284: 8, 293: 8, 309: 8, 320: 8, 325: 8, 336: 8, 339: 8, 341: 8, 348: 8, 349: 8, 352: 8, 368: 8, 370: 8, 373: 8, 375: 8, 376: 8, 389: 8, 395: 8, 400: 8, 408: 8, 411: 8, 417: 8, 420: 8, 426: 8, 429: 8, 435: 8, 440: 8, 464: 8, 556: 8, 640: 8, 656: 8, 666: 8, 773: 8, 778: 8, 789: 8, 791: 8, 800: 8, 805: 8, 807: 8, 816: 8, 821: 8, 832: 8, 837: 8, 841: 8, 848: 8, 853: 8, 854: 8, 858: 8, 860: 8, 873: 8, 882: 8, 889: 8, 890: 8, 891: 8, 892: 8, 893: 8, 896: 8, 899: 8, 901: 8, 917: 8, 919: 8, 1043: 8, 1045: 8, 1061: 8, 1072: 8, 1077: 8, 1088: 8, 1093: 8, 1120: 8, 1127: 8, 1160: 8, 1168: 8, 1171: 8, 1174: 8, 1175: 8, 1177: 8, 1296: 8, 1302: 8, 1320: 8, 1334: 8, 1336: 8, 1407: 8, 1422: 8, 1423: 8, 1424: 8, 1425: 8, 1426: 8, 1428: 8, 1429: 8, 1430: 8, 1431: 8, 1432: 8, 1554: 8, 1584: 8, 1587: 8, 2022: 8 + }], +} + +FW_VERSIONS: dict[str, dict[tuple, list[bytes]]] = { +} diff --git a/opendbc_repo/opendbc/car/volvo/helpers.py b/opendbc_repo/opendbc/car/volvo/helpers.py new file mode 100644 index 000000000..3e43403db --- /dev/null +++ b/opendbc_repo/opendbc/car/volvo/helpers.py @@ -0,0 +1,397 @@ +def checksum_lca_2_message(b0: int, b5: int) -> int: + """ + Compute checksum for VCU1 CAN ID 0x69 from bytes b0 and b5. + + b0: first data byte (MSB) of the frame (usually 0x18 in your logs) + b5: sixth data byte of the frame (what you called Byte5) + + Returns: checksum byte (0..255) that goes into byte index 6. + """ + if b0 == 0 and b5 == 128: # Hotfix openpilot test (don't know where this alleged test message comes from) + return 0 + + # Masks per checksum bit (bit 0..7) for b0 and b5 + M0 = [0x08, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00] + M5 = [0x83, 0x86, 0xCF, 0xCD, 0x09, 0x02, 0x44, 0x89] + + def parity8(x: int) -> int: + # 1 if x has an odd number of bits set, else 0 + x ^= x >> 4 + x ^= x >> 2 + x ^= x >> 1 + return x & 1 + + b0 &= 0xFF + b5 &= 0xFF + + c = 0 + for bit in range(8): + p = 0 + if M0[bit]: + p ^= parity8(b0 & M0[bit]) + if M5[bit]: + p ^= parity8(b5 & M5[bit]) + c |= (p << bit) + + return c & 0xFF + +def checksum_2_0x69_message(b0: int, b1: int, b3: int = 0, b4: int = 0) -> int: + """ + Compute checksum byte (b2) for CAN ID 0x69 (LCA_2 message). + + The checksum depends on bytes 0, 1, 3, and 4. During normal driving (BYTE_1_MSBS_3=0), + bytes 3-4 (NEW_SIGNAL_2) are always 0, so only b0 and b1 matter. During stability + control events (aquaplaning, etc.), BYTE_1_MSBS_3 becomes non-zero and bytes 3-4 + contain non-zero values that affect the checksum. + + Args: + b0: Byte 0 (usually 0x18) + b1: Byte 1 ([7:5] BYTE_1_MSBS_3 | [4] PILOT_ASSIST_ENGAGED | [3:0] COUNTER_1) + b3: Byte 3 (NEW_SIGNAL_2 high byte, default 0) + b4: Byte 4 (NEW_SIGNAL_2 low byte, default 0) + + Returns: + Checksum byte (0-255) for position 2 + """ + b0 &= 0xFF + b1 &= 0xFF + b3 &= 0xFF + b4 &= 0xFF + + def bit(byte, pos): + return (byte >> pos) & 1 + + c = 0 + # Bit 0 + c |= (bit(b0, 0) ^ bit(b1, 2) ^ bit(b1, 3) ^ bit(b1, 4) ^ bit(b1, 5) ^ bit(b4, 0) ^ bit(b4, 1)) << 0 + # Bit 1 + c |= (bit(b0, 1) ^ bit(b0, 3) ^ bit(b1, 0) ^ bit(b1, 3) ^ bit(b1, 5) ^ bit(b1, 6) ^ + bit(b3, 1) ^ bit(b4, 0) ^ bit(b4, 1) ^ bit(b4, 2)) << 1 + # Bit 2 + c |= (bit(b0, 0) ^ bit(b0, 3) ^ bit(b1, 1) ^ bit(b1, 2) ^ bit(b1, 3) ^ bit(b1, 4) ^ bit(b1, 5) ^ bit(b1, 6) ^ + bit(b3, 0) ^ bit(b3, 1) ^ bit(b4, 0) ^ bit(b4, 2) ^ bit(b4, 3)) << 2 + # Bit 3 + c |= (bit(b0, 0) ^ bit(b0, 1) ^ bit(b1, 0) ^ bit(b1, 4) ^ bit(b1, 6) ^ + bit(b3, 0) ^ bit(b3, 1) ^ bit(b4, 0) ^ bit(b4, 3) ^ bit(b4, 4)) << 3 + # Bit 4 + c |= (bit(b0, 0) ^ bit(b0, 1) ^ bit(b0, 3) ^ bit(b1, 1) ^ bit(b1, 2) ^ bit(b1, 3) ^ bit(b1, 4) ^ + bit(b4, 4) ^ bit(b4, 5)) << 4 + # Bit 5 + c |= (bit(b0, 1) ^ bit(b1, 0) ^ bit(b1, 2) ^ bit(b1, 3) ^ bit(b1, 5) ^ + bit(b3, 1) ^ bit(b4, 5) ^ bit(b4, 6)) << 5 + # Bit 6 + c |= (bit(b0, 3) ^ bit(b1, 0) ^ bit(b1, 1) ^ bit(b1, 3) ^ bit(b1, 6) ^ + bit(b3, 0) ^ bit(b4, 6) ^ bit(b4, 7)) << 6 + # Bit 7 + c |= (bit(b0, 3) ^ bit(b1, 1) ^ bit(b1, 2) ^ bit(b1, 4) ^ bit(b4, 0) ^ bit(b4, 7)) << 7 + return c & 0xFF + +def checksum_1_pscm_related_message(b1, b2): + """ + Computes checksum #1 (goes in byte[0]) for PSCM-related 0x17 message. + Depends only on (byte[1], byte[2]). + + b1 = SIG1 counter (high nibble) | LCA_ENABLED_ECHO (low nibble) + b2 = 0x80 | SIG1 counter replica (low nibble) + + Linear over GF(2), same shape as checksum_lca_2_message: each output bit is + the parity of a fixed mask over b1 and b2. Solved from 137,965 logged PSCM + frames (60 distinct (b1,b2) keys, leave-one-out cross-validated 60/60). + + This replaces a 45-entry lookup table that covered only LCA_ENABLED_ECHO in + {0, 1, 4} and returned 0 on a miss. During an ESC intervention the rack + reports ECHO=6, so openpilot transmitted 128 consecutive frames with an + invalid checksum (0x00) before this fix. + + Note: bit 3 of b1 (LCA_ENABLED_ECHO >= 8) has never been observed on the bus, + so its contribution is unconstrained by the data and is taken to be zero. + """ + # Masks per checksum bit (bit 0..7) for b1 and b2 + M1 = [0x41, 0x82, 0x55, 0xF3, 0xA7, 0x46, 0x94, 0x20] + M2 = [0x00, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x80] + + def parity8(x: int) -> int: + # 1 if x has an odd number of bits set, else 0 + x ^= x >> 4 + x ^= x >> 2 + x ^= x >> 1 + return x & 1 + + b1 &= 0xFF + b2 &= 0xFF + + c = 0 + for bit in range(8): + p = 0 + if M1[bit]: + p ^= parity8(b1 & M1[bit]) + if M2[bit]: + p ^= parity8(b2 & M2[bit]) + c |= (p << bit) + + return c & 0xFF + + +def checksum_2_pscm_related_message(b2): + """ + Computes checksum #2 (goes in byte[3]) for PSCM-related 0x17 message. + Depends only on byte[2]. + """ + + lut = { + 0x80: 0xBF, + 0x81: 0xF3, + 0x82: 0x27, + 0x83: 0x6B, + 0x84: 0x92, + 0x85: 0xDE, + 0x86: 0x0A, + 0x87: 0x46, + 0x88: 0xE5, + 0x89: 0xA9, + 0x8A: 0x7D, + 0x8B: 0x31, + 0x8C: 0xC8, + 0x8D: 0x84, + 0x8E: 0x50, + } + + return lut.get(b2, 0) + + +def checksum_lca_4_message(*args) -> int: + """ + Placeholder for LCA_4 (0x90) checksum calculation. + + TODO: Implementation will be provided after checksum analysis is complete. + For now, returns 0 as a placeholder. + + Args: + *args: Byte values needed for checksum calculation (TBD) + + Returns: + Checksum byte (0-255) + """ + # Placeholder - will be replaced with actual checksum algorithm + return 0 + + +class LCA3CounterSync: + """ + Best-effort pattern synchronization for LCA_3 COUNTER_1. + + The counter follows a 20-element pattern that cycles based on transmission count. + We track recent observed counter values and match them against the pattern to + determine the current index. While not synchronized, we pass through stock values. + Once synchronized, we permanently use the pattern. + """ + + PATTERN = [2, 2, 1, 2, 2, 2, 1, 2, 2, 3, 0, 2, 3, 2, 0, 2, 3, 2, 0, 3] + PATTERN_LEN = 20 + WINDOW_SIZE = 5 # Track last 5 values for matching + MIN_CONFIDENCE = 4 # Need 4 consecutive matches to sync + + def __init__(self): + self.pattern_index = None # Current index in pattern (None = not synced) + self.observed_window = [] # Circular buffer of last N observed values + self.confidence = 0 # Number of consecutive successful matches + + def update(self, observed_counter: int) -> tuple: + """ + Update with newly observed counter value from stock message. + + Args: + observed_counter: Counter value from CS.msg_lca_3['COUNTER_1'] + + Returns: + Tuple of (counter_to_send, is_synchronized) + """ + # If already synchronized, ignore stock and use our pattern permanently + if self.pattern_index is not None: + counter_to_send = self.PATTERN[self.pattern_index] + self.pattern_index = (self.pattern_index + 1) % self.PATTERN_LEN + return counter_to_send, True + + # Not synchronized yet - try to find pattern index + self.observed_window.append(observed_counter) + if len(self.observed_window) > self.WINDOW_SIZE: + self.observed_window.pop(0) + + # Attempt to sync if we have enough samples + if len(self.observed_window) >= 3: + self._attempt_sync() + + # While not synced, pass through stock counter + return observed_counter, False + + def _attempt_sync(self): + """Try to find current pattern index based on observed window.""" + # Try to match observation window against all positions in pattern + best_match_idx = None + best_match_len = 0 + + for start_idx in range(self.PATTERN_LEN): + match_len = self._count_match(start_idx) + if match_len > best_match_len: + best_match_len = match_len + best_match_idx = start_idx + + # Require MIN_CONFIDENCE matching values to declare sync + if best_match_len >= self.MIN_CONFIDENCE: + # The match tells us where we WERE in the pattern + # We need to set index to NEXT position for next transmission + self.pattern_index = (best_match_idx + len(self.observed_window)) % self.PATTERN_LEN + self.confidence = best_match_len + + def _count_match(self, pattern_start_idx: int) -> int: + """ + Count how many values in observed_window match pattern starting at pattern_start_idx. + + Returns: + Number of consecutive matching values from start + """ + match_count = 0 + for i, observed in enumerate(self.observed_window): + pattern_idx = (pattern_start_idx + i) % self.PATTERN_LEN + if observed == self.PATTERN[pattern_idx]: + match_count += 1 + else: + break # Stop at first mismatch + return match_count + + def is_synchronized(self) -> bool: + """Returns True if we have synchronized to the pattern.""" + return self.pattern_index is not None + +def checksum_lca_5_message(byte0: int, byte1: int, byte3: int, byte4: int, byte5: int) -> int: + """ + Calculate checksum for LCA_5 message 0x67 (byte 2) + + Args: + byte0: Byte 0 (0-255) + byte1: Byte 1 (0-255) + byte3: Byte 3 (0-255) + byte4: Byte 4 (0-255) + byte5: Byte 5 (0-255) + + Returns: + int: Checksum value (0-255) for byte 2 + + Example: + >>> checksum = checksum_lca_5_message(0x80, 0x00, 0x4F, 0x00, 0x00) + >>> print(f"0x{checksum:02X}") + 0x32 + """ + + # Helper function to extract a bit (LSB = bit 0) + def bit(byte_val, pos): + return (byte_val >> pos) & 1 + + checksum = 0 + + # Bit 0: XOR of 13 bits + checksum |= ( + bit(byte0, 2) ^ bit(byte0, 3) ^ bit(byte0, 5) ^ bit(byte0, 7) ^ + bit(byte1, 2) ^ + bit(byte3, 4) ^ bit(byte3, 6) ^ + bit(byte4, 0) ^ bit(byte4, 1) ^ bit(byte4, 5) ^ bit(byte4, 6) ^ + bit(byte5, 3) ^ bit(byte5, 6) + ) << 0 + + # Bit 1: XOR of 12 bits + checksum |= ( + bit(byte0, 0) ^ bit(byte0, 3) ^ bit(byte0, 4) ^ bit(byte0, 7) ^ + bit(byte1, 0) ^ bit(byte1, 3) ^ + bit(byte3, 5) ^ bit(byte3, 7) ^ + bit(byte4, 2) ^ bit(byte4, 6) ^ + bit(byte5, 4) ^ bit(byte5, 7) + ) << 1 + + # Bit 2: XOR of 17 bits + checksum |= ( + bit(byte0, 1) ^ bit(byte0, 2) ^ bit(byte0, 3) ^ bit(byte0, 4) ^ + bit(byte1, 0) ^ bit(byte1, 1) ^ bit(byte1, 2) ^ bit(byte1, 4) ^ + bit(byte3, 4) ^ + bit(byte4, 1) ^ bit(byte4, 3) ^ bit(byte4, 5) ^ bit(byte4, 6) ^ + bit(byte5, 1) ^ bit(byte5, 3) ^ bit(byte5, 5) ^ bit(byte5, 6) + ) << 2 + + # Bit 3: XOR of 19 bits + checksum |= ( + bit(byte0, 0) ^ bit(byte0, 4) ^ bit(byte0, 7) ^ + bit(byte1, 1) ^ bit(byte1, 3) ^ bit(byte1, 5) ^ + bit(byte3, 4) ^ bit(byte3, 5) ^ bit(byte3, 6) ^ + bit(byte4, 0) ^ bit(byte4, 1) ^ bit(byte4, 2) ^ bit(byte4, 4) ^ bit(byte4, 5) ^ + bit(byte5, 1) ^ bit(byte5, 2) ^ bit(byte5, 3) ^ bit(byte5, 4) ^ bit(byte5, 7) + ) << 3 + + # Bit 4: XOR of 16 bits + checksum |= ( + bit(byte0, 1) ^ bit(byte0, 2) ^ bit(byte0, 3) ^ bit(byte0, 7) ^ + bit(byte1, 4) ^ bit(byte1, 6) ^ + bit(byte3, 4) ^ bit(byte3, 5) ^ bit(byte3, 7) ^ + bit(byte4, 1) ^ bit(byte4, 2) ^ bit(byte4, 3) ^ + bit(byte5, 2) ^ bit(byte5, 4) ^ bit(byte5, 5) ^ bit(byte5, 6) + ) << 4 + + # Bit 5: XOR of 15 bits + checksum |= ( + bit(byte0, 0) ^ bit(byte0, 2) ^ bit(byte0, 3) ^ bit(byte0, 4) ^ + bit(byte1, 5) ^ bit(byte1, 7) ^ + bit(byte3, 5) ^ bit(byte3, 6) ^ + bit(byte4, 2) ^ bit(byte4, 3) ^ bit(byte4, 4) ^ + bit(byte5, 3) ^ bit(byte5, 5) ^ bit(byte5, 6) ^ bit(byte5, 7) + ) << 5 + + # Bit 6: XOR of 19 bits + checksum |= ( + bit(byte0, 0) ^ bit(byte0, 1) ^ bit(byte0, 3) ^ bit(byte0, 4) ^ bit(byte0, 5) ^ bit(byte0, 7) ^ + bit(byte1, 0) ^ bit(byte1, 6) ^ + bit(byte3, 4) ^ bit(byte3, 6) ^ bit(byte3, 7) ^ + bit(byte4, 0) ^ bit(byte4, 3) ^ bit(byte4, 4) ^ bit(byte4, 5) ^ + bit(byte5, 1) ^ bit(byte5, 4) ^ bit(byte5, 6) ^ bit(byte5, 7) + ) << 6 + + # Bit 7: XOR of 15 bits + checksum |= ( + bit(byte0, 1) ^ bit(byte0, 2) ^ bit(byte0, 4) ^ bit(byte0, 5) ^ + bit(byte1, 1) ^ bit(byte1, 7) ^ + bit(byte3, 5) ^ bit(byte3, 7) ^ + bit(byte4, 0) ^ bit(byte4, 4) ^ bit(byte4, 5) ^ bit(byte4, 6) ^ + bit(byte5, 2) ^ bit(byte5, 5) ^ bit(byte5, 7) + ) << 7 + + return checksum + + +# Test examples +if __name__ == "__main__": + print("CAN 0x67 Checksum Calculator") + print("=" * 60) + + # Test cases + tests = [ + ([0x80, 0x00, 0x4F, 0x00, 0x00, 0xBA, 0x00], 0x32), + ([0x80, 0x00, 0x8F, 0x00, 0x00, 0xBA, 0x00], 0x89), + ([0x80, 0x00, 0xCF, 0x00, 0x00, 0xBA, 0x00], 0xE0), + ([0x80, 0x00, 0x1F, 0x00, 0x00, 0xBA, 0x00], 0x06), + ] + + all_passed = True + for i, (bytes_list, expected) in enumerate(tests, 1): + calculated = checksum_lca_5_message(*bytes_list[:5]) + status = "✓" if calculated == expected else "✗" + + print(f"\nTest {i}: {status}") + print(f" Bytes: {' '.join(f'{b:02X}' for b in bytes_list)}") + print(f" Expected: 0x{expected:02X}") + print(f" Calculated: 0x{calculated:02X}") + + if calculated != expected: + all_passed = False + + print("\n" + "=" * 60) + if all_passed: + print("All tests passed! ✓") + else: + print("Some tests failed! ✗") diff --git a/opendbc_repo/opendbc/car/volvo/interface.py b/opendbc_repo/opendbc/car/volvo/interface.py new file mode 100644 index 000000000..3c1176009 --- /dev/null +++ b/opendbc_repo/opendbc/car/volvo/interface.py @@ -0,0 +1,42 @@ +from opendbc.car import structs, get_safety_config +from opendbc.car.interfaces import CarInterfaceBase +from opendbc.car.volvo.carcontroller import CarController +from opendbc.car.volvo.carstate import CarState +from opendbc.car.volvo.values import VolvoSPAPlatformConfig, CAR + +TransmissionType = structs.CarParams.TransmissionType + +VOLVO_FLAG_SPA = 1 +SAFETY_VOLVO = structs.CarParams.SafetyModel.volvo + + +class CarInterface(CarInterfaceBase): + CarState = CarState + CarController = CarController + + @staticmethod + def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams: + ret.brand = 'volvo' + + safety_param = 0 + if isinstance(CAR(candidate).config, VolvoSPAPlatformConfig): + safety_param = VOLVO_FLAG_SPA + ret.safetyConfigs = [get_safety_config(SAFETY_VOLVO, safety_param)] + #ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.noOutput)] + + ret.dashcamOnly = False + + ret.steerActuatorDelay = 0.3 + ret.steerLimitTimer = 0.1 + ret.steerAtStandstill = True + + # Use angle-based steering control for Volvo CMA platform + ret.steerControlType = structs.CarParams.SteerControlType.angle + # Note: No lateral tuning configuration needed for basic angle control + ret.radarUnavailable = True + + ret.alphaLongitudinalAvailable = False + + ret.pcmCruise = True + + return ret diff --git a/opendbc_repo/opendbc/car/volvo/tests/__init__.py b/opendbc_repo/opendbc/car/volvo/tests/__init__.py new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/opendbc_repo/opendbc/car/volvo/tests/__init__.py @@ -0,0 +1 @@ + diff --git a/opendbc_repo/opendbc/car/volvo/tests/test_checksums.py b/opendbc_repo/opendbc/car/volvo/tests/test_checksums.py new file mode 100644 index 000000000..7051292de --- /dev/null +++ b/opendbc_repo/opendbc/car/volvo/tests/test_checksums.py @@ -0,0 +1,59 @@ +import unittest + +from opendbc.car.volvo.helpers import ( + checksum_1_pscm_related_message, + checksum_2_pscm_related_message, +) + +# (b1, b2) -> byte[0], observed on the bus. +# b1 = SIG1 counter (high nibble) | LCA_ENABLED_ECHO (low nibble), b2 = 0x80 | counter. +# ECHO 0/1/4 are normal driving; ECHO 6 only appears while ESC is intervening and was +# the case that used to fall through to a 0x00 checksum. +PSCM_RELATED_CHECKSUM_1 = { + (0x00, 0x80): 0xD4, (0x01, 0x80): 0xC9, (0x04, 0x80): 0xA0, (0x06, 0x80): 0x9A, + (0x10, 0x81): 0x98, (0x11, 0x81): 0x85, (0x14, 0x81): 0xEC, (0x16, 0x81): 0xD6, + (0x20, 0x82): 0x4C, (0x21, 0x82): 0x51, (0x24, 0x82): 0x38, (0x26, 0x82): 0x02, + (0x30, 0x83): 0x00, (0x31, 0x83): 0x1D, (0x34, 0x83): 0x74, (0x36, 0x83): 0x4E, + (0x40, 0x84): 0xF9, (0x41, 0x84): 0xE4, (0x44, 0x84): 0x8D, (0x46, 0x84): 0xB7, + (0x50, 0x85): 0xB5, (0x51, 0x85): 0xA8, (0x54, 0x85): 0xC1, (0x56, 0x85): 0xFB, + (0x60, 0x86): 0x61, (0x61, 0x86): 0x7C, (0x64, 0x86): 0x15, (0x66, 0x86): 0x2F, + (0x70, 0x87): 0x2D, (0x71, 0x87): 0x30, (0x74, 0x87): 0x59, (0x76, 0x87): 0x63, + (0x80, 0x88): 0x8E, (0x81, 0x88): 0x93, (0x84, 0x88): 0xFA, (0x86, 0x88): 0xC0, + (0x90, 0x89): 0xC2, (0x91, 0x89): 0xDF, (0x94, 0x89): 0xB6, (0x96, 0x89): 0x8C, + (0xA0, 0x8A): 0x16, (0xA1, 0x8A): 0x0B, (0xA4, 0x8A): 0x62, (0xA6, 0x8A): 0x58, + (0xB0, 0x8B): 0x5A, (0xB1, 0x8B): 0x47, (0xB4, 0x8B): 0x2E, (0xB6, 0x8B): 0x14, + (0xC0, 0x8C): 0xA3, (0xC1, 0x8C): 0xBE, (0xC4, 0x8C): 0xD7, (0xC6, 0x8C): 0xED, + (0xD0, 0x8D): 0xEF, (0xD1, 0x8D): 0xF2, (0xD4, 0x8D): 0x9B, (0xD6, 0x8D): 0xA1, + (0xE0, 0x8E): 0x3B, (0xE1, 0x8E): 0x26, (0xE4, 0x8E): 0x4F, (0xE6, 0x8E): 0x75, +} + +# b2 -> byte[3], same source +PSCM_RELATED_CHECKSUM_2 = { + 0x80: 0xBF, 0x81: 0xF3, 0x82: 0x27, 0x83: 0x6B, 0x84: 0x92, + 0x85: 0xDE, 0x86: 0x0A, 0x87: 0x46, 0x88: 0xE5, 0x89: 0xA9, + 0x8A: 0x7D, 0x8B: 0x31, 0x8C: 0xC8, 0x8D: 0x84, 0x8E: 0x50, +} + + +class TestPscmRelatedChecksums(unittest.TestCase): + def test_checksum_1_matches_the_car(self): + for (b1, b2), expected in PSCM_RELATED_CHECKSUM_1.items(): + with self.subTest(b1=hex(b1), b2=hex(b2)): + assert checksum_1_pscm_related_message(b1, b2) == expected + + def test_checksum_1_covers_esc_echo(self): + # regression: ECHO=6 used to miss the lookup table and return 0x00, which the + # receiving ECU logged as a checksum fault for as long as ESC was active + for counter in range(15): + b1, b2 = (counter << 4) | 6, 0x80 | counter + with self.subTest(counter=counter): + assert checksum_1_pscm_related_message(b1, b2) == PSCM_RELATED_CHECKSUM_1[(b1, b2)] + + def test_checksum_2_matches_the_car(self): + for b2, expected in PSCM_RELATED_CHECKSUM_2.items(): + with self.subTest(b2=hex(b2)): + assert checksum_2_pscm_related_message(b2) == expected + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/car/volvo/tests/test_controller.py b/opendbc_repo/opendbc/car/volvo/tests/test_controller.py new file mode 100644 index 000000000..5a73796ff --- /dev/null +++ b/opendbc_repo/opendbc/car/volvo/tests/test_controller.py @@ -0,0 +1,69 @@ +from collections import defaultdict +from types import SimpleNamespace + +from opendbc.car.volvo.carcontroller import CarController +from opendbc.car.volvo.helpers import checksum_lca_5_message +from opendbc.car.volvo.interface import CarInterface +from opendbc.car.volvo.values import DBC + + +def _zero_message(): + return defaultdict(int) + + +def _state(): + return SimpleNamespace( + out=SimpleNamespace(steeringAngleDeg=0.0, vEgoRaw=12.0, steeringTorque=0.0), + msg_lca=_zero_message(), + msg_pscm=_zero_message(), + msg_pscm_related=_zero_message(), + msg_lca_3=_zero_message(), + msg_lca_2=_zero_message(), + msg_lca_5=_zero_message(), + msg_lca_4=_zero_message(), + msg_lca_6=_zero_message(), + msg_lca_7=_zero_message(), + pilot_assist_engaged=False, + ) + + +class _Actuators: + steeringAngleDeg = 30.0 + + def as_builder(self): + return SimpleNamespace(steeringAngleDeg=self.steeringAngleDeg) + + +def test_controller_emits_valid_eight_byte_messages_and_lca5_checksum(): + cp = CarInterface.get_non_essential_params("POLESTAR_2") + controller = CarController(DBC[cp.carFingerprint], cp) + cs = _state() + cc = SimpleNamespace(latActive=True, actuators=_Actuators()) + + actuators, can_sends = controller.update(cc, cs, 0, None) + + assert can_sends + assert {msg[2] for msg in can_sends} == {0, 2} + assert all(len(msg[1]) == 8 for msg in can_sends) + assert 0.0 < actuators.steeringAngleDeg < 540.0 + + lca5 = next(msg for msg in can_sends if msg[0] == 0x67) + data = lca5[1] + assert data[2] == checksum_lca_5_message(data[0], data[1], data[3], data[4], data[5]) + + +def test_controller_relays_stock_lca5_angle_when_inactive(): + cp = CarInterface.get_non_essential_params("VOLVO_XC40_RECHARGE") + controller = CarController(DBC[cp.carFingerprint], cp) + cs = _state() + cs.msg_lca_5["LCA_5_STEER"] = 12.0 + cc = SimpleNamespace(latActive=False, actuators=_Actuators()) + + _, can_sends = controller.update(cc, cs, 0, None) + lca5 = next(msg for msg in can_sends if msg[0] == 0x67) + + # The inactive path must not manufacture a new angle command. + raw = ((lca5[1][6] & 0x7F) << 8) | lca5[1][7] + if raw & (1 << 14): + raw -= 1 << 15 + assert abs(raw * 0.05596 - 12.0) < 0.1 diff --git a/opendbc_repo/opendbc/car/volvo/values.py b/opendbc_repo/opendbc/car/volvo/values.py new file mode 100644 index 000000000..5b697bef0 --- /dev/null +++ b/opendbc_repo/opendbc/car/volvo/values.py @@ -0,0 +1,154 @@ +from dataclasses import dataclass, field + +from opendbc.car.structs import CarParams +from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms +from opendbc.car.lateral import AngleSteeringLimits +from opendbc.car.docs_definitions import CarDocs, CarHarness, CarParts +from opendbc.car.fw_query_definitions import FwQueryConfig + +Ecu = CarParams.Ecu + + +class CarControllerParams: + STEER_STEP = 1 # 100 Hz LCA command frequency (controlsd runs at 100 Hz) + + # Max commanded-vs-actual steering angle error (deg). Stock Volvo Pilot Assist holds + # commanded within ~1.7° of actual even under sustained driver override; bounding the + # command to actual ± this error prevents the stale-command snap-back that causes + # aggressive post-release overcorrection. + ANGLE_ERROR = 3.0 + + # LCA torque-authority envelope, modeled after stock Pilot Assist behavior. + # LCA_STEER_LOOSELY (positive arm) and LCA_STEER_LOOSELY_INV (negative arm) + # form a directional envelope that PSCM applies to its EPS torque. Stock PA: + # - holds both arms at saturation (±LCA_AUTH_MAX) when no driver torque + # - on driver override, collapses both arms symmetrically at COLLAPSE_RATE + # until envelope reaches ~±LCA_AUTH_SPLIT, then splits asymmetrically: + # the arm matching driver direction (yielding) settles at ±PLATEAU_YIELD, + # the counter arm holds at ±PLATEAU_COUNTER (yield is shallower than counter) + # - rebuilds at REBUILD_RATE after release (~3 s back to saturation) + # See route_analysis/lca_override_mechanism.md for the data behind these. + LCA_AUTH_MAX = 614 # signal saturation + LCA_AUTH_PLATEAU_COUNTER = 130 # counter-arm magnitude during sustained override + # Override trigger thresholds on |CS.out.steeringTorque| (op-convention raw + # units, mirror of DRIVER_INPUT). Must be ABOVE the resting-hand noise floor + # (CS.steeringPressed uses |raw|>2 as a sensitive DM-fallback floor and does + # NOT indicate override intent — don't use it for envelope triggering). + # Hysteresis: enter override at ENTER, exit at EXIT (< ENTER) to prevent the + # envelope flapping between collapse and rebuild when driver torque hovers + # near a single threshold (was causing ~10 Hz EPS-torque ripple in lane + # changes when driver applied 6-8 raw to "ride along" with op). + LCA_AUTH_OVERRIDE_ENTER = 5 + LCA_AUTH_OVERRIDE_EXIT = 3 + # "Light contact" / haptic-acknowledgment region. When |drv| crosses into + # [LIGHT_THRESH, OVERRIDE_THRESH] from below, briefly collapse the envelope + # for LIGHT_HOLD_FRAMES (a haptic confirmation of hand-on-wheel detection), + # then rebuild even while the contact persists. Prevents the driver from + # needing to sustain force just to feel that the system noticed them — helps + # with hand-fatigue / RSI. + # Rising edge detected via per-frame derivative; the brief-yield window does + # NOT re-arm while still active, so a steady elevated torque only triggers + # one yield and then the envelope rebuilds. + # Cooldown: light_collapse only fires when real_override has been off for + # LIGHT_COOLDOWN_FRAMES — suppresses repeated firings during active + # co-steering (lane changes), where |drv| oscillates and would otherwise + # re-arm the haptic-ack window each time, causing felt ripple. + LCA_AUTH_LIGHT_THRESH = 3 # min |drv| to consider as contact + LCA_AUTH_LIGHT_RISE_DELTA = 1.0 # min per-frame increase in |drv| to count as rising contact + LCA_AUTH_LIGHT_HOLD_FRAMES = 15 # ~150 ms of yield on fresh light contact + LCA_AUTH_LIGHT_COOLDOWN_FRAMES = 30 # ~300 ms quiet-time on real_override before light contact re-arms + # Yield-arm plateau scales with driver-torque magnitude so brief strong presses + # (potholes, lane corrections) get full yield while light sustained pressure + # only gets a soft yield. yield_signed = YIELD_BASE − YIELD_SLOPE * + # max(0, drv_mag_filt − OVERRIDE_ENTER), clamped to [YIELD_MIN, YIELD_BASE]. + # At |drv|=7 (just over threshold): yield = +60 (light resistance). + # At |drv|=14: yield ≈ -4 (crosses past zero — EPS hands wheel to driver). + # drv_mag_filt is a low-pass of |drv| (alpha=0.04, ~250 ms time constant) — + # without it, 1-2 unit driver-torque jitter became ~10 unit yield-arm jitter + # which PSCM converted to felt ripple at sustained co-steering pressure. + LCA_AUTH_YIELD_BASE = 60 # yield-arm magnitude at the override threshold + LCA_AUTH_YIELD_SLOPE = 8 # counts of yield reduction per unit |drv torque| above threshold + LCA_AUTH_YIELD_MIN = -30 # cap how far past zero the yield arm can go (full hand-over) + LCA_AUTH_YIELD_LP_ALPHA = 0.04 # LP-filter coefficient on |drv| for yield calc (~250 ms tau) + LCA_AUTH_SPLIT = 200 # symmetric → asymmetric handover + LCA_AUTH_REBUILD_RATE = 230 # counts/s (≈ 2.7 s rebuild from 0 to 614) + LCA_AUTH_COLLAPSE_RATE = 2500 # counts/s base (scales with |drv|/THRESH for sharper pothole jolts) + + # Angle limits for rate limiting + ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits( + 540, # deg - 1.5 turns to lock + ([0., 5., 25.], [2.5, 1.5, .2]), # rate up limits at different speeds + ([0., 5., 25.], [5., 2., .3]), # rate down limits at different speeds + ) + + +@dataclass +class VolvoCarDocs(CarDocs): + package: str = "Pilot Assist & Adaptive Cruise Control" + car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.custom])) + + +@dataclass +class VolvoCMAPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: { + Bus.main: 'volvo_mid_1', + Bus.party: 'volvo_mid_1', + Bus.pt: 'volvo_front_1_cma', + }) + +@dataclass +class VolvoSPAPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: { + Bus.main: 'volvo_mid_1', + Bus.party: 'volvo_mid_1', + Bus.pt: 'volvo_front_1_spa', + }) + + +class CAR(Platforms): + VOLVO_XC40_RECHARGE = VolvoCMAPlatformConfig( + [VolvoCarDocs("Volvo XC40 Recharge 2021-23")], + CarSpecs( + mass=2170, + wheelbase=2.702, + steerRatio=15.8, + centerToFrontRatio=0.52, + ), + ) + + VOLVO_S60_RECHARGE = VolvoSPAPlatformConfig( + [VolvoCarDocs("Volvo S60 Recharge 2024")], + CarSpecs( + mass=2020, + wheelbase=2.872, + steerRatio=16.2, + centerToFrontRatio=0.516, + ), + ) + + # Polestar 2 is technically CMA, but appears to use SPA DBC for CAN 1 bus + POLESTAR_2 = VolvoSPAPlatformConfig( + [VolvoCarDocs("Polestar 2 2020-25")], + CarSpecs( + mass=2123, + wheelbase=2.735, + steerRatio=15.8, + centerToFrontRatio=0.52, + ), + ) + +# FW Query configuration for Volvo CMA platform +# FW_QUERY_CONFIG = FwQueryConfig( +# requests=[ +# Request( +# [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST], +# [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], +# bus=0, +# ), +# ], +# ) +FW_QUERY_CONFIG = FwQueryConfig( + requests=[] +) + +DBC = CAR.create_dbc_map() diff --git a/opendbc_repo/opendbc/car/volvo/volvocan.py b/opendbc_repo/opendbc/car/volvo/volvocan.py new file mode 100644 index 000000000..9f7f0753a --- /dev/null +++ b/opendbc_repo/opendbc/car/volvo/volvocan.py @@ -0,0 +1,464 @@ +from opendbc.car.volvo.helpers import (checksum_lca_2_message, checksum_2_0x69_message, checksum_1_pscm_related_message, + checksum_2_pscm_related_message, checksum_lca_5_message) +from opendbc.car.carlog import carlog + +def create_lca_message(packer, lat_active: bool, apply_angle: float, msg_lca: dict, + authority_pos: int = 614, authority_neg: int = -614, + overrides: dict | None = None): + """ + Create LCA (Lane Centering Assist) steering command for Volvo CMA platform. + Uses angle-based control via the LCA_STEER signal. + + NOTE: This message must be sent continuously (even when inactive) because + stock LCA is permanently blocked by panda safety. When lat_active=False, + we send a safe/inactive LCA message to maintain PSCM communication. + + Args: + packer: CAN packer instance + lat_active: Whether lateral control is active + apply_angle: Steering angle in degrees (positive = left, negative = right) + msg_lca: Dictionary containing LCA message values + authority_pos: LCA_STEER_LOOSELY value [0..614] — right-pull torque-authority + envelope. Saturated (614) for stock-equivalent stiff feel; the + carcontroller envelope tracker collapses this on driver override + and rebuilds slowly to reproduce stock PA's easy-override feel. + authority_neg: LCA_STEER_LOOSELY_INV value [-614..0] — left-pull authority. + overrides: Optional dict of signal overrides (keys are UPPERCASE DBC signal names) + """ + if not lat_active: + return packer.make_can_msg('LCA', 2, msg_lca) + + # In openpilot, a positive angle corresponds to a LEFT turn. + # In Volvo, a positive LCA_STEER value corresponds to a LEFT turn. + + values = { + 'NEW_SIGNAL_1': 3, + 'LCA_ENABLE_INV': 0 if lat_active else 1, + 'LANE_KEEP_ACTIVE_INV': 3, + 'LCA_STEER_LOOSELY': int(authority_pos) if lat_active else 0, + 'NEW_SIGNAL_7': 7, + 'LCA_STEER_LOOSELY_INV': int(authority_neg) if lat_active else 0, + # Steering rate - Stock LCA increased from 35 to 39 steppedly when steering request was overridden by openpilot that couldn't steer enough + 'LCA_RATE_OF_CHANGE': 80 if lat_active else 251, + 'LCA_STEER': msg_lca['LCA_STEER'], + 'NEW_SIGNAL_6': 15, + } + + # Apply any overrides from live testing config + if overrides: + for key, val in overrides.items(): + values[key] = val + + return packer.make_can_msg('LCA', 2, values) + +def create_pscm_message(packer, lat_active: bool, msg_pscm: dict, frame: int): + values = { + 'PSCM_ANGLE_SENSOR': msg_pscm['PSCM_ANGLE_SENSOR'], + 'BIT_0': msg_pscm['BIT_0'], + 'HANDS_ON_STEERING_WHEEL_A': msg_pscm['HANDS_ON_STEERING_WHEEL_A'], + 'HANDS_ON_STEERING_WHEEL_B': msg_pscm['HANDS_ON_STEERING_WHEEL_B'], + 'BYTE_4': msg_pscm['BYTE_4'], + 'DRIVER_INPUT_DEVIATION': msg_pscm['DRIVER_INPUT_DEVIATION'], + 'BYTE_6': msg_pscm['BYTE_6'], + 'BYTE_7': msg_pscm['BYTE_7'], + } + + # Spoof hands on wheel while openpilot is actively steering, so the stock EPS + # doesn't fault/nag on torque that didn't come from a human. + if lat_active: + values['HANDS_ON_STEERING_WHEEL_B'] = 186 if frame % 2 == 0 else 154 # msg_pscm['HANDS_ON_STEERING_WHEEL_B'] + values['HANDS_ON_STEERING_WHEEL_A'] = 195 if frame % 2 == 0 else 249 # msg_pscm['HANDS_ON_STEERING_WHEEL_A'] + + return packer.make_can_msg('PSCM', 0, values) + +def create_lca_3_message(packer, lat_active: bool, apply_angle: float, msg_lca_3: dict, counter_value: int): + """ + Create LCA_3 message for Volvo CMA platform. + This message enables PSCM to accept LCA commands. + + Args: + packer: CAN packer instance + lat_active: Whether lateral control is active + apply_angle: Steering angle in degrees (used for direction indicator) + msg_lca_3: Dictionary containing LCA_3 message values + counter_value: Counter value to use (from pattern or stock) + """ + values = { + 'NEW_SIGNAL_3': 0 if lat_active else msg_lca_3['NEW_SIGNAL_3'], + 'LCA_ACCEPT_COMMANDS_RELATED': 15 if lat_active else msg_lca_3['LCA_ACCEPT_COMMANDS_RELATED'], + 'NEW_SIGNAL_2': 0 if lat_active else msg_lca_3['NEW_SIGNAL_2'], + 'NEW_SIGNAL_5': 30 if lat_active else msg_lca_3['NEW_SIGNAL_5'], + 'LCA_ACCEPT_COMMANDS_INV': 0 if lat_active else msg_lca_3['LCA_ACCEPT_COMMANDS_INV'], + 'NEW_SIGNAL_4': 3 if lat_active else msg_lca_3['NEW_SIGNAL_4'], + 'SPEED_A': msg_lca_3['SPEED_A'], + 'SPEED_B': msg_lca_3['SPEED_B'], + 'NEW_SIGNAL_8': 1 if lat_active else msg_lca_3['NEW_SIGNAL_8'], + 'NEW_SIGNAL_7': 3 if lat_active else msg_lca_3['NEW_SIGNAL_7'], + 'NEW_SIGNAL_9': msg_lca_3['NEW_SIGNAL_9'], + 'COUNTER_1': counter_value, + } + + return packer.make_can_msg('LCA_3', 2, values) + +def diff_dicts(a, b): + only_in_a = a.keys() - b.keys() + only_in_b = b.keys() - a.keys() + in_both = a.keys() & b.keys() + + changed = {k: (a[k], b[k]) for k in in_both if a[k] != b[k]} + + return { + "only_in_a": {k: a[k] for k in only_in_a}, + "only_in_b": {k: b[k] for k in only_in_b}, + "changed": changed, + } + +def create_lca_2_message(packer, lat_active: bool, msg_lca_2: dict, counter_1: int, counter_2: int): + """ + Create LCA_2 message to spoof PILOT_ASSIST_ENGAGED when openpilot is active. + + When lat_active=True, we set PILOT_ASSIST_ENGAGED=1 to make PSCM accept LCA commands, + even if the driver has disabled stock Pilot Assist. + + Args: + packer: CAN packer instance + lat_active: Whether lateral control is active + msg_lca_2: Dictionary containing LCA_2 message values from car + counter_1: Managed COUNTER_1 value (increments by +2 mod 16) + counter_2: Managed COUNTER_2 value (increments by +4 mod 16) + """ + #return packer.make_can_msg('LCA_2', 2, msg_lca_2) + #if not lat_active: + # return packer.make_can_msg('LCA_2', 2, msg_lca_2) + + #values = dict(msg_lca_2) + + values = { + 'BYTE_0': 24 if lat_active else msg_lca_2['BYTE_0'], # 24 always + 'COUNTER_1': msg_lca_2['COUNTER_1'], # Byte 1 Low Nibble [5:8] - 4-bit counter that increments by +2 (modulo 16) + 'PILOT_ASSIST_ENGAGED': 1 if lat_active else msg_lca_2['PILOT_ASSIST_ENGAGED'], # Byte 1 [4] + 'BYTE_1_BITFIELD_0': msg_lca_2['BYTE_1_BITFIELD_0'], + 'ESC_ACTUATING': msg_lca_2['ESC_ACTUATING'], + 'ESC_ELIGIBLE': msg_lca_2['ESC_ELIGIBLE'], + 'CHECKSUM_2': msg_lca_2['CHECKSUM_2'], # Checksum on bytes 0 and 1 + 'NEW_SIGNAL_2': 0 if lat_active else msg_lca_2['NEW_SIGNAL_2'], + 'COUNTER_2': msg_lca_2['COUNTER_2'], # Byte 5 Low Nibble - 4-bit counter that increments by +4 (modulo 16) + 'NEW_SIGNAL_3': 3 if lat_active else msg_lca_2['NEW_SIGNAL_3'], + 'BRAKE_PEDAL_PRESSED_B': msg_lca_2['BRAKE_PEDAL_PRESSED_B'], + 'BRAKE_PEDAL_PRESSED_A': msg_lca_2['BRAKE_PEDAL_PRESSED_A'], + 'CHECKSUM_1': msg_lca_2['CHECKSUM_1'], # Byte 6 is a checksum based on Bytes 1, 2, and 5 only + 'BYTE_7': 0 if lat_active else msg_lca_2['BYTE_7'], + } + + dat = packer.make_can_msg('LCA_2', 2, values) + + built_bytes = dat[1] # dat is (addr, bytes, bus) tuple - extract bytes + b0 = built_bytes[0] + b1 = built_bytes[1] + b2 = built_bytes[2] + b3 = built_bytes[3] + b4 = built_bytes[4] + b5 = built_bytes[5] + + values['CHECKSUM_1'] = checksum_lca_2_message(b0, b5) + + # Only validate when not active and message is valid (BYTE_0 should be 24, not 0) + if not lat_active: + #assert values['CHECKSUM_1'] == msg_lca_2['CHECKSUM_1'] + if values['CHECKSUM_1'] != msg_lca_2['CHECKSUM_1']: + carlog.warning("[volvocan.py] LCA_2 CHECKSUM mismatch") + print(f"b0={b0}, b1={b1}, b2={b2}, b5={b5}, calculated={values['CHECKSUM_1']}, expected={msg_lca_2['CHECKSUM_1']}") + #assert False + + # Checksum 2 - depends on bytes 0, 1, 3, and 4 + checksum_2 = checksum_2_0x69_message(b0, b1, b3, b4) + values['CHECKSUM_2'] = checksum_2 + if not lat_active: + if values['CHECKSUM_2'] != msg_lca_2['CHECKSUM_2']: + carlog.warning("[volvocan.py] LCA_2 CHECKSUM_2 mismatch") + print(f"b0={b0}, b1={b1}, b3={b3}, b4={b4}, calculated={values['CHECKSUM_2']}, expected={msg_lca_2['CHECKSUM_2']}") + #assert False + values['COUNTER_1'] = counter_1 + values['COUNTER_2'] = counter_2 + # Re-pack with updated counters to get correct bytes for checksum calculation + dat = packer.make_can_msg('LCA_2', 2, values) + built_bytes = dat[1] # dat is (addr, bytes, bus) tuple - extract bytes + b0 = built_bytes[0] + b1 = built_bytes[1] + b3 = built_bytes[3] + b4 = built_bytes[4] + b5 = built_bytes[5] + values['CHECKSUM_1'] = checksum_lca_2_message(b0, b5) + values['CHECKSUM_2'] = checksum_2_0x69_message(b0, b1, b3, b4) + return packer.make_can_msg('LCA_2', 2, values) + +def create_lca_5_message(packer, lat_active: bool, target_angle_deg: float, msg_lca_5: dict, counter: int, + overrides: dict | None = None): + """ + Create LCA_5 message (0x67) with angle-based steering control. + + Args: + packer: CAN packer instance + lat_active: Whether lateral control is active + target_angle_deg: Target steering angle in degrees (positive = left, negative = right) + msg_lca_5: Stock LCA_5 values from car + counter: Counter value (0-15, increments by 4) + overrides: Optional dict of signal overrides (keys are UPPERCASE DBC signal names) + + Returns: + CAN message for LCA_5 on bus 2 + """ + + # DBC defines LCA_5_STEER as 15-bit signed with scale 0.05596 deg/count + # Packer handles the encoding automatically - just pass the angle in degrees + + # Build values dictionary (wheel speeds and counter unchanged) + values = { + 'WHEEL_SPEED_1': msg_lca_5['WHEEL_SPEED_1'], + 'NEW_SIGNAL_4': msg_lca_5['NEW_SIGNAL_4'], + 'NEW_SIGNAL_1': msg_lca_5['NEW_SIGNAL_1'], + 'WHEEL_SPEED_2': msg_lca_5['WHEEL_SPEED_2'], + 'NEW_SIGNAL_5': msg_lca_5['NEW_SIGNAL_5'], + 'NEW_SIGNAL_2': msg_lca_5['NEW_SIGNAL_2'], + 'LCA_5_STEER': target_angle_deg if lat_active else msg_lca_5['LCA_5_STEER'], + 'COUNTER': counter, + } + + # Apply any overrides from live testing config + if overrides: + for key, val in overrides.items(): + values[key] = val + + dat = packer.make_can_msg('LCA_5', 2, values) + built_bytes = dat[1] # dat is (addr, bytes, bus) tuple - extract bytes + values['CHECKSUM'] = checksum_lca_5_message(built_bytes[0], built_bytes[1], built_bytes[3], built_bytes[4], built_bytes[5]) + return packer.make_can_msg('LCA_5', 2, values) + +def create_speed_message(packer, msg_speed: dict): + """ + Forward SPEED message (0x60) by copying all bytes. + + Args: + packer: CAN packer instance + msg_speed: Dictionary containing SPEED message values from car + """ + values = { + 'SPEED': msg_speed['SPEED'], + 'NEW_SIGNAL_1': msg_speed['NEW_SIGNAL_1'], + 'NEW_SIGNAL_2': msg_speed['NEW_SIGNAL_2'], + 'NEW_SIGNAL_3': msg_speed['NEW_SIGNAL_3'], + 'NEW_SIGNAL_4': msg_speed['NEW_SIGNAL_4'], + 'NEW_SIGNAL_5': msg_speed['NEW_SIGNAL_5'], + 'NEW_SIGNAL_6': msg_speed['NEW_SIGNAL_6'], + } + + return packer.make_can_msg('SPEED', 2, values) + +def create_speed_2_message(packer, msg_speed_2: dict): + """ + Forward SPEED_2 message (0x68) by copying all bytes. + + Args: + packer: CAN packer instance + msg_speed_2: Dictionary containing SPEED_2 message values from car + """ + values = { + 'WHEEL_SPEED_LEFT': msg_speed_2['WHEEL_SPEED_LEFT'], + 'WHEEL_SPEED_RIGHT': msg_speed_2['WHEEL_SPEED_RIGHT'], + 'NEW_SIGNAL_1': msg_speed_2['NEW_SIGNAL_1'], + 'NEW_SIGNAL_2': msg_speed_2['NEW_SIGNAL_2'], + 'NEW_SIGNAL_3': msg_speed_2['NEW_SIGNAL_3'], + 'NEW_SIGNAL_4': msg_speed_2['NEW_SIGNAL_4'], + 'COUNTER_1': msg_speed_2['COUNTER_1'], + 'COUNTER_2': msg_speed_2['COUNTER_2'], + } + + return packer.make_can_msg('SPEED_2', 2, values) + +def create_speed_3_message(packer, msg_speed_3: dict): + """ + Forward SPEED_3 message (0x60) by copying all bytes. + + Args: + packer: CAN packer instance + msg_speed_3: Dictionary containing SPEED_3 message values from car + """ + values = { + 'ALL_BYTES': msg_speed_3['ALL_BYTES'], + } + + return packer.make_can_msg('SPEED_3', 2, values) + +def create_0x1a_message(packer, msg_0x1a: dict): + """ + Forward 0x1A message by copying all bytes. + + Args: + packer: CAN packer instance + msg_0x1a: Dictionary containing 0x1A message values from car + """ + values = { + 'ALL_BYTES': msg_0x1a['ALL_BYTES'], + } + return packer.make_can_msg('NEW_MSG_1A', 2, values) + +def create_gear_position_message(packer, msg_gear_position: dict): + """ + Forward GEAR_POSITION message by copying all bytes. + + Args: + packer: CAN packer instance + msg_gear_position: Dictionary containing GEAR_POSITION message values from car + """ + values = dict(msg_gear_position) + values['GEAR_POSITION'] = msg_gear_position['GEAR_POSITION'] # 3 + return packer.make_can_msg('GEAR_POSITION', 2, values) + +def create_egsm_message(packer, msg_egsm: dict): + """ + Forward EGSM message by copying all bytes. + + Args: + packer: CAN packer instance + msg_egsm: Dictionary containing EGSM message values from car + """ + values = { + 'ALL_BYTES': msg_egsm['ALL_BYTES'], + } + return packer.make_can_msg('EGSM', 0, values) + +def create_pscm_related_message(packer, lat_active: bool, stock_lca_engaged: bool, msg_pscm_related: dict, sig1_counter: int): + # BO_ 23 PSCM_RELATED: 8 XXX + # SG_ CHECKSUM : 7|8@0+ (1,0) [0|255] "" XXX + # SG_ LCA_ENABLED_ECHO : 11|4@0+ (1,0) [0|15] "" XXX + # SG_ SIG1_BYTE_1_HI_NIBBLE : 15|4@0+ (1,0) [0|15] "" XXX + # SG_ SIG1_REPLICA_BYTE_2_LO_NIBLE : 19|4@0+ (1,0) [0|15] "" XXX + # SG_ NEW_SIGNAL_2 : 23|4@0+ (1,0) [0|15] "" XXX + # SG_ BYTE_3 : 31|8@0+ (1,0) [0|255] "" XXX + # SG_ BYTE_4_5 : 39|16@0+ (1,0) [0|65535] "" XXX + # SG_ BYTE_6 : 55|8@0+ (1,0) [0|255] "" XXX + # SG_ BYTE_7 : 63|8@0+ (1,0) [0|255] "" XXX + values = dict(msg_pscm_related) + + # Update SIG1 counter (same value in both locations for redundancy) + values['SIG1_BYTE_1_HI_NIBBLE'] = sig1_counter + values['SIG1_REPLICA_BYTE_2_LO_NIBLE'] = sig1_counter + + dat = packer.make_can_msg('PSCM_RELATED', 0, values) + built_bytes = dat[1] # dat is (addr, bytes, bus) tuple - extract bytes + b1 = built_bytes[1] + b2 = built_bytes[2] + values['CHECKSUM_1'] = checksum_1_pscm_related_message(b1, b2) + values['CHECKSUM_2'] = checksum_2_pscm_related_message(b2) + #assert values['CHECKSUM_1'] == msg_pscm_related['CHECKSUM_1'] + #assert values['CHECKSUM_2'] == msg_pscm_related['CHECKSUM_2'] + if lat_active and not stock_lca_engaged: + values['LCA_ENABLED_ECHO'] = 0 + b1 = packer.make_can_msg('PSCM_RELATED', 0, values)[1][1] + values['CHECKSUM_1'] = checksum_1_pscm_related_message(b1, b2) + return packer.make_can_msg('PSCM_RELATED', 0, values) + +def create_lca_4_message(packer, lat_active: bool, msg_lca_4: dict, lca_4_steer: int, + overrides: dict | None = None): + """ + Create LCA_4 (0x90) message to maintain Pilot Assist state when openpilot is active. + + Critical: LCA_ENABLE (byte 1 bits 0-1) must be held at 3 (both bits=1) when lat_active. + When PA turns off, these bits start varying (become counters). We need to keep them + stable at 3 to fool PSCM into thinking PA is still on, allowing LCA commands to be accepted. + + Based on analysis from route_analysis/pilot_assist_off/BASELINE_FILTERED_FINDINGS.md: + - Message 0x090 byte 1 bits 0-1 are PA state signals + - During PA ON: bits are stable at 3 (binary 11) + - During PA OFF: bits start varying (counters) + - PSCM uses this to determine whether to accept LCA steering commands + + Args: + packer: CAN packer instance + lat_active: Whether lateral control is active + msg_lca_4: Dictionary containing LCA_4 message values from car + lca_4_steer: Pre-computed signed angle with hysteresis applied + overrides: Optional dict of signal overrides (keys are UPPERCASE DBC signal names) + + Returns: + CAN message for LCA_4 on bus 2 + """ + if not lat_active: + # When not active, just relay stock message unchanged + return packer.make_can_msg('LCA_4', 2, msg_lca_4) + + # When lat_active, force LCA_ENABLE to 3 (PA ON state) + values = { + 'BYTE_0': msg_lca_4['BYTE_0'], + 'LCA_ENABLE': 3, # Force bits 0-1 to 1 (value=3 means both bits set) + 'BYTE_1_FLAGS': msg_lca_4['BYTE_1_FLAGS'], + 'BYTE_1_NIBBLE_HI': msg_lca_4['BYTE_1_NIBBLE_HI'], + 'BYTE_2_3': msg_lca_4['BYTE_2_3'], + 'YAW_RATE': msg_lca_4['YAW_RATE'], + 'BYTE_6': msg_lca_4['BYTE_6'], + 'BYTE_7_NIBBLE_LO': msg_lca_4['BYTE_7_NIBBLE_LO'], + 'BYTE_7_NIBBLE_HI': msg_lca_4['BYTE_7_NIBBLE_HI'], + } + + # Apply any overrides from live testing config + if overrides: + for key, val in overrides.items(): + values[key] = val + + # TODO: Add checksum calculation when checksum function is implemented + # If message has a checksum signal, it would be calculated here like: + # values['CHECKSUM'] = checksum_lca_4_message(...) + + # TODO: Add checksum validation when not active (once checksum is known) + # if not lat_active and 'CHECKSUM' in msg_lca_4: + # if values['CHECKSUM'] != msg_lca_4['CHECKSUM']: + # carlog.warning("[volvocan.py] LCA_4 CHECKSUM mismatch") + + return packer.make_can_msg('LCA_4', 2, values) + +def create_lca_6_message(packer, lat_active: bool, msg_lca_6: dict, lca_6_steer: int, + overrides: dict | None = None): + + if not lat_active: + # When not active, just relay stock message unchanged + return packer.make_can_msg('LCA_6', 2, msg_lca_6) + + values = { + 'LCA_6_STEER': msg_lca_6['LCA_6_STEER'], + 'LCA_6_STEER_2': msg_lca_6['LCA_6_STEER_2'], + 'NEW_SIGNAL_1': msg_lca_6['NEW_SIGNAL_1'], + 'NEW_SIGNAL_2': msg_lca_6['NEW_SIGNAL_2'], + 'NEW_SIGNAL_3': msg_lca_6['NEW_SIGNAL_3'], + 'NEW_SIGNAL_4': msg_lca_6['NEW_SIGNAL_4'], + 'NEW_SIGNAL_5': msg_lca_6['NEW_SIGNAL_5'], + } + + # Apply any overrides from live testing config + if overrides: + for key, val in overrides.items(): + values[key] = val + + return packer.make_can_msg('LCA_6', 2, values) + +def create_lca_7_message(packer, lat_active: bool, msg_lca_7: dict, lca_7_steer: int, lca_7_delta_steer: int, + overrides: dict | None = None, steer_active: bool = False): + + if not lat_active: + # When not active, just relay stock message unchanged + return packer.make_can_msg('LCA_7', 2, msg_lca_7) + + values = { + 'LCA_7_STEER': msg_lca_7['LCA_7_STEER'], + 'LCA_7_DELTA_STEER': msg_lca_7['LCA_7_DELTA_STEER'], + 'NEW_SIGNAL_1': msg_lca_7['NEW_SIGNAL_1'], + 'NEW_SIGNAL_2': msg_lca_7['NEW_SIGNAL_2'], + 'NEW_SIGNAL_3': msg_lca_7['NEW_SIGNAL_3'], + 'NEW_SIGNAL_4': msg_lca_7['NEW_SIGNAL_4'], + } + + # Apply any overrides from live testing config + if overrides: + for key, val in overrides.items(): + values[key] = val + + return packer.make_can_msg('LCA_7', 2, values) diff --git a/opendbc_repo/opendbc/dbc/volvo_cma.dbc b/opendbc_repo/opendbc/dbc/volvo_cma.dbc new file mode 100644 index 000000000..662930de7 --- /dev/null +++ b/opendbc_repo/opendbc/dbc/volvo_cma.dbc @@ -0,0 +1,245 @@ +BO_ 21 DRIVER_INPUT: 8 XXX + SG_ BYTE_0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_1 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_2 : 16|8@1+ (1,0) [0|255] "" XXX + SG_ STEERING_DRIVER_RATE_OF_CHANGE : 31|8@0- (1,0) [0|255] "" XXX + SG_ BYTE_4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_5 : 40|8@1+ (1,0) [0|255] "" XXX + SG_ STEERING_DRIVER_INPUT : 55|8@0- (1,1) [0|255] "" XXX + SG_ BYTE_7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 22 PSCM: 8 XXX + SG_ PSCM_ANGLE_SENSOR : 6|15@0- (0.05596,0) [-916|916] "º" XXX + SG_ BIT_0 : 7|1@0+ (1,0) [0|1] "" XXX + SG_ HANDS_ON_STEERING_WHEEL_A : 23|8@0+ (1,0) [0|255] "" XXX + SG_ HANDS_ON_STEERING_WHEEL_B : 31|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ DRIVER_INPUT_DEVIATION : 47|8@0- (1,0) [-128|127] "" XXX + SG_ BYTE_6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 23 PSCM_RELATED: 8 XXX + SG_ CHECKSUM_1 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ LCA_ENABLED_ECHO : 11|4@0+ (1,0) [0|15] "" XXX + SG_ SIG1_BYTE_1_HI_NIBBLE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ SIG1_REPLICA_BYTE_2_LO_NIBLE : 19|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_2 : 23|4@0+ (1,0) [0|15] "" XXX + SG_ CHECKSUM_2 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_5 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 26 NEW_MSG_1A: 8 XXX + SG_ NEW_SIGNAL_2 : 5|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_1 : 7|1@0+ (1,0) [0|1] "" XXX + +BO_ 38 NEW_MSG_26: 8 XXX + SG_ NEW_SIGNAL_1 : 5|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_2 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_4 : 35|12@0+ (1,0) [0|4095] "" XXX + SG_ NEW_SIGNAL_3 : 39|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_6 : 51|12@0+ (1,0) [0|4095] "" XXX + SG_ NEW_SIGNAL_5 : 55|1@0+ (1,0) [0|1] "" XXX + +BO_ 55 NEW_MSG_37: 8 XXX + SG_ ACCELERATOR_PEDAL_RATE_OF_CHANGE : 6|15@0+ (1,0) [0|32767] "" XXX + +BO_ 69 EGSM: 8 XXX + SG_ COUNTER_1 : 3|12@0+ (1,0) [0|4095] "" XXX + SG_ GEAR_LEVER_DBC_INCOHERENT : 5|2@0+ (1,0) [0|3] "" XXX + SG_ COUNTER_2 : 19|12@0+ (1,0) [0|4095] "" XXX + SG_ GEAR_1ST_RATCH : 20|1@0+ (1,0) [0|1] "" XXX + SG_ GEAR_RATCHED_UP : 21|1@0+ (1,0) [0|1] "" XXX + SG_ GEAR_RATCHED_DOWN : 22|1@0+ (1,0) [0|1] "" XXX + SG_ GEAR_PRESSED : 36|2@0+ (1,0) [0|3] "" XXX + SG_ PARKING_BRAKE_BUTTON : 38|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER_3 : 51|12@0+ (1,0) [0|4095] "" XXX + +BO_ 85 SAS: 8 XXX + SG_ SAS_ANGLE_SENSOR : 6|15@0- (0.05596,0) [0|32767] "º" XXX + SG_ NEW_SIGNAL_1 : 7|1@0+ (1,0) [0|1] "" XXX + SG_ SAS_INPUT_ACTIVITY : 21|6@0+ (1,0) [0|16383] "" XXX + SG_ NEW_SIGNAL_2 : 23|2@0+ (1,0) [0|3] "" XXX + SG_ SAS_RATE_OF_CHANGE : 31|8@0- (1,0) [0|255] "" XXX + SG_ SAS_CHECKSUM : 39|8@0+ (1,0) [0|4095] "" XXX + SG_ NEW_SIGNAL_4 : 43|20@0+ (1,0) [0|1048575] "" XXX + SG_ SAS_COUNTER : 44|4@1+ (1,0) [0|15] "" XXX + +BO_ 87 LCA_3: 8 XXX + SG_ NEW_SIGNAL_3 : 0|1@0+ (1,0) [0|1] "" XXX + SG_ LCA_ACCEPT_COMMANDS_RELATED : 4|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_2 : 7|3@0+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_5 : 12|5@0+ (1,0) [0|31] "" XXX + SG_ LCA_ACCEPT_COMMANDS_INV : 13|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_4 : 15|2@0+ (1,0) [0|3] "" XXX + SG_ SPEED_A : 23|16@0+ (1,0) [0|65535] "" XXX + SG_ SPEED_B : 39|16@0+ (1,0) [0|65535] "" XXX + SG_ NEW_SIGNAL_8 : 51|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER_1 : 53|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_7 : 55|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_9 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 88 LCA: 8 XXX + SG_ NEW_SIGNAL_3 : 1|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_8 : 2|1@0+ (1,0) [0|1] "" XXX + SG_ LCA_ENABLE_INV : 3|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_2 : 5|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_1 : 7|2@0+ (1,0) [0|3] "" XXX + SG_ LCA_STEER_LOOSELY_1 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ LCA_STEER_ACTIVE_INCOHERENT : 16|1@0+ (1,0) [0|1] "" XXX + SG_ LCA_STEER_ACTIVE : 18|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_7 : 21|3@0+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_9 : 23|2@0+ (1,0) [0|3] "" XXX + SG_ LCA_STEER_LOOSELY_2 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ CURVE_RIGHT : 45|6@0+ (1,0) [0|63] "" XXX + SG_ NEW_SIGNAL_5 : 46|2@1+ (1,0) [0|3] "" XXX + SG_ LCA_STEER : 55|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_10 : 59|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_6 : 60|4@1+ (1,0) [0|15] "" XXX + +BO_ 96 SPEED_3: 8 XXX + SG_ NEW_SIGNAL_1 : 0|8@1+ (1,0) [0|255] "" XXX + SG_ SPEED_COUNTER : 15|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_3 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_5 : 24|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_2 : 27|3@0+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_4 : 31|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_8 : 34|3@0+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_6 : 35|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_7 : 39|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_9 : 40|8@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_10 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_11 : 56|8@1+ (1,0) [0|255] "" XXX + +BO_ 103 LCA_5: 8 XXX + SG_ WHEEL_SPEED_1 : 6|15@0+ (0.1,0) [0|255] "rpm" XXX + SG_ NEW_SIGNAL_4 : 7|1@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 23|8@0+ (1,0) [0|4095] "" XXX + SG_ NEW_SIGNAL_1 : 24|4@1+ (1,0) [0|15] "" XXX + SG_ COUNTER : 28|4@1+ (1,0) [0|15] "" XXX + SG_ WHEEL_SPEED_2 : 39|15@0+ (0.1,0) [0|32767] "rpm" XXX + SG_ NEW_SIGNAL_5 : 40|1@0+ (1,0) [0|1] "" XXX + SG_ LCA_TURN_BITS : 55|8@0+ (1,0) [0|255] "" XXX + SG_ LCA_5_STEER : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 104 SPEED_2: 8 XXX + SG_ WHEEL_SPEED_3 : 6|15@0+ (0.1,0) [0|32767] "rpm" XXX + SG_ WHEEL_SPEED_4 : 39|15@0+ (0.1,0) [0|32767] "rpm" XXX + SG_ NEW_SIGNAL_2 : 51|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_3 : 54|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_1 : 56|8@1+ (1,0) [0|255] "" XXX + +BO_ 105 LCA_2: 8 XXX + SG_ BYTE_0 : 7|8@0+ (1,0) [0|2047] "" XXX + SG_ COUNTER_1 : 11|4@0+ (1,0) [0|4095] "" XXX + SG_ PILOT_ASSIST_ENGAGED : 12|1@0+ (1,0) [0|1] "" XXX + SG_ BYTE_1_MSBS_3 : 15|3@0+ (1,0) [0|7] "" XXX + SG_ CHECKSUM_2 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_2 : 31|16@0+ (1,0) [0|65535] "" XXX + SG_ COUNTER_2 : 43|4@0+ (1,0) [0|4095] "" XXX + SG_ NEW_SIGNAL_3 : 45|2@0+ (1,0) [0|3] "" XXX + SG_ BRAKE_PEDAL_PRESSED_B : 46|1@0+ (1,0) [0|1] "" XXX + SG_ BRAKE_PEDAL_PRESSED_A : 47|1@0+ (-1,1) [0|1] "" XXX + SG_ CHECKSUM_1 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 112 BUS1_SPEED: 8 XXX + SG_ BUS1_SPEED : 23|16@0+ (0.01886,0) [0|65535] "m/s" XXX + +BO_ 128 GEAR_POSITION: 8 XXX + SG_ NEW_SIGNAL_7 : 3|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_6 : 7|3@0+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_8 : 12|4@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_1 : 43|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_3 : 47|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_2 : 48|4@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_4 : 55|1@0+ (1,0) [0|1] "" XXX + SG_ GEAR_POSITION : 57|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_5 : 63|1@0+ (1,0) [0|1] "" XXX + +BO_ 144 LCA_4: 8 XXX + SG_ BYTE_0 : 7|8@0+ (1,0) [0|16383] "" XXX + SG_ LCA_ENABLE : 9|2@0+ (1,0) [0|3] "" XXX + SG_ BYTE_1_FLAGS : 11|2@0+ (1,0) [0|3] "" XXX + SG_ BYTE_1_NIBBLE_HI : 15|4@0+ (1,0) [0|63] "" XXX + SG_ BYTE_2_3 : 23|16@0- (1,0) [0|65535] "" XXX + SG_ YAW_RATE : 39|16@0- (1,0) [0|65535] "" XXX + SG_ BYTE_6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_7_NIBBLE_LO : 59|4@0+ (1,0) [0|15] "" XXX + SG_ BYTE_7_NIBBLE_HI : 60|4@1+ (1,0) [0|15] "" XXX + +BO_ 146 NEW_MSG_92: 8 XXX + SG_ NEW_SIGNAL_1 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_2 : 12|4@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_3 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_4 : 25|7@1+ (1,0) [0|127] "" XXX + SG_ NEW_SIGNAL_5 : 38|7@0+ (1,0) [0|127] "" XXX + SG_ NEW_SIGNAL_6 : 40|7@1+ (1,0) [0|127] "" XXX + SG_ NEW_SIGNAL_7 : 48|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_8 : 49|3@1+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_9 : 57|7@1+ (1,0) [0|127] "" XXX + +BO_ 147 NEW_MSG_93: 8 XXX + SG_ NEW_SIGNAL_1 : 38|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_2 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_4 : 52|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_3 : 55|1@0+ (1,0) [0|1] "" XXX + +BO_ 151 LCA_SUSPECT: 8 XXX + SG_ NEW_SIGNAL_1 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_2 : 8|8@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_3 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_6 : 25|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_5 : 26|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_4 : 27|5@1+ (1,0) [0|31] "" XXX + SG_ NEW_SIGNAL_7 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_9 : 43|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_8 : 44|4@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_10 : 48|8@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_12 : 56|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_11 : 63|7@0+ (1,0) [0|127] "" XXX + +BO_ 336 NEW_MSG_150: 8 XXX + SG_ IGN_DRAFT : 5|2@0+ (1,0) [0|3] "" XXX + +BO_ 341 NEW_MSG_155: 8 XXX + SG_ NEW_SIGNAL_1 : 23|8@0+ (1,0) [0|255] "" XXX + +BO_ 592 ECM_1: 8 XXX + SG_ ACCELERATOR_PEDAL_POS : 31|8@0+ (1,0) [0|255] "" XXX + +BO_ 773 NEW_MSG_305: 8 XXX + SG_ NEW_SIGNAL_1 : 23|8@0+ (1,0) [0|255] "" XXX + +BO_ 778 NEW_MSG_30A: 8 XXX + SG_ NEW_SIGNAL_1 : 15|8@0+ (1,0) [0|255] "" XXX + +BO_ 832 BUS1_CRUISE_CONTROL: 8 XXX + SG_ CRUISE_CONTROL_ENABLED : 56|1@0+ (1,0) [0|1] "" XXX + SG_ CRUISE_CONTROL_ENABLED_IDLE_TRAFFIC : 57|1@0+ (1,0) [0|1] "" XXX + +BO_ 896 NEW_MSG_380: 8 XXX + SG_ NEW_SIGNAL_1 : 23|8@0+ (1,0) [0|255] "" XXX + +BO_ 1336 NEW_MSG_538: 8 XXX + SG_ SWM_MULTIMEDIA_ACTIVITY : 22|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_1 : 23|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_3 : 24|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_2 : 29|1@0+ (1,0) [0|1] "" XXX + +CM_ BO_ 23 "Might be related to PSCM 0x16"; +CM_ SG_ 87 LCA_ACCEPT_COMMANDS_RELATED "Goes to 15 when actively operating LCA"; +CM_ SG_ 87 LCA_ACCEPT_COMMANDS_INV "Goes to 0 when PSCM allowed to receive LCA commands"; +CM_ SG_ 87 NEW_SIGNAL_9 "NEW_SIGNAL_9 appears to be similar to LCA_5_STEER, but different scale, and zero-point is at 128. I haven't seen what happens once LCA_TURN_BITS wrap"; +CM_ SG_ 88 LCA_ENABLE_INV "Master enable, active-low"; +CM_ SG_ 88 CURVE_RIGHT "Only appears to be HIGH on curve right, LOW curve left"; +CM_ SG_ 88 LCA_STEER "Seems torque-based, signed, follows the road curvature"; +CM_ SG_ 103 WHEEL_SPEED_1 "Possible Front Left (FL)"; +CM_ SG_ 103 WHEEL_SPEED_2 "Possible Front Right (FR)"; +CM_ SG_ 103 LCA_TURN_BITS "Two-byte torque encoding (high byte). Left: 128->134 (increments), Right: 255->249 (decrements), Neutral: 186"; +CM_ SG_ 103 LCA_5_STEER "Two-byte torque encoding (low byte). Left: 0->255 (wraps at boundary), Right: 255->0 (wraps at boundary)"; +CM_ SG_ 104 WHEEL_SPEED_3 "Possible Rear Left (RR)"; +CM_ SG_ 104 WHEEL_SPEED_4 "Possibe Rear Right (RR)"; +CM_ SG_ 592 ACCELERATOR_PEDAL_POS "Full gas ~200; Idle ~20"; diff --git a/opendbc_repo/opendbc/dbc/volvo_front_1_cma.dbc b/opendbc_repo/opendbc/dbc/volvo_front_1_cma.dbc new file mode 100644 index 000000000..1a0891e8f --- /dev/null +++ b/opendbc_repo/opendbc/dbc/volvo_front_1_cma.dbc @@ -0,0 +1,14 @@ +BO_ 55 NEW_MSG_37: 8 XXX + SG_ ACCELERATOR_PEDAL_RATE_OF_CHANGE : 6|15@0+ (1,0) [0|32767] "" XXX + +BO_ 112 BUS1_SPEED: 8 XXX + SG_ BUS1_SPEED : 23|16@0+ (0.01886,0) [0|65535] "m/s" XXX + +BO_ 592 ECM_1: 8 XXX + SG_ ACCELERATOR_PEDAL_POS : 31|8@0+ (1,0) [0|255] "" XXX + +BO_ 832 BUS1_CRUISE_CONTROL: 8 XXX + SG_ CRUISE_CONTROL_ENABLED : 56|1@0+ (1,0) [0|1] "" XXX + SG_ CRUISE_CONTROL_ENABLED_IDLE_TRAFFIC : 57|1@0+ (1,0) [0|1] "" XXX + +CM_ SG_ 592 ACCELERATOR_PEDAL_POS "Full gas ~200; Idle ~20"; diff --git a/opendbc_repo/opendbc/dbc/volvo_front_1_spa.dbc b/opendbc_repo/opendbc/dbc/volvo_front_1_spa.dbc new file mode 100644 index 000000000..523c1edcc --- /dev/null +++ b/opendbc_repo/opendbc/dbc/volvo_front_1_spa.dbc @@ -0,0 +1,10 @@ +BO_ 37 ECM_1: 8 XXX + SG_ ACCELERATOR_PEDAL_POS : 6|15@0+ (0.00390625,0) [0|32767] "%" XXX + +BO_ 117 BUS1_SPEED: 8 XXX + SG_ BUS1_SPEED : 6|15@0+ (0.0044704,0) [0|32767] "" XXX + +BO_ 841 BUS1_CRUISE_CONTROL: 8 XXX + SG_ CRUISE_CONTROL_SPA_ENABLED : 1|1@0+ (-1,1) [0|1] "" XXX + +CM_ SG_ 117 BUS1_SPEED "m/s"; diff --git a/opendbc_repo/opendbc/dbc/volvo_mid_1.dbc b/opendbc_repo/opendbc/dbc/volvo_mid_1.dbc new file mode 100644 index 000000000..1fb5c0116 --- /dev/null +++ b/opendbc_repo/opendbc/dbc/volvo_mid_1.dbc @@ -0,0 +1,188 @@ +BO_ 21 DRIVER_INPUT: 8 XXX + SG_ BYTE_0 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_1 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_2 : 16|8@1+ (1,0) [0|255] "" XXX + SG_ STEERING_DRIVER_RATE_OF_CHANGE : 31|8@0- (1,0) [0|255] "" XXX + SG_ BYTE_4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_5 : 40|8@1+ (1,0) [0|255] "" XXX + SG_ STEERING_DRIVER_INPUT : 55|8@0- (1,1) [0|255] "" XXX + SG_ BYTE_7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 22 PSCM: 8 XXX + SG_ PSCM_ANGLE_SENSOR : 6|15@0- (0.05596,0) [-916|916] "º" XXX + SG_ BIT_0 : 7|1@0+ (1,0) [0|1] "" XXX + SG_ HANDS_ON_STEERING_WHEEL_A : 23|8@0+ (1,0) [0|255] "" XXX + SG_ HANDS_ON_STEERING_WHEEL_B : 31|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_4 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ DRIVER_INPUT_DEVIATION : 47|8@0- (1,0) [-128|127] "" XXX + SG_ BYTE_6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 23 PSCM_RELATED: 8 XXX + SG_ CHECKSUM_1 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ LCA_ENABLED_ECHO : 11|4@0+ (1,0) [0|15] "" XXX + SG_ SIG1_BYTE_1_HI_NIBBLE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ SIG1_REPLICA_BYTE_2_LO_NIBLE : 19|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_2 : 23|4@0+ (1,0) [0|15] "" XXX + SG_ CHECKSUM_2 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_4_5 : 39|16@0+ (1,0) [0|65535] "" XXX + SG_ BYTE_6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 69 EGSM: 8 XXX + SG_ COUNTER_1 : 3|12@0+ (1,0) [0|4095] "" XXX + SG_ GEAR_LEVER_DBC_INCOHERENT : 5|2@0+ (1,0) [0|3] "" XXX + SG_ COUNTER_2 : 19|12@0+ (1,0) [0|4095] "" XXX + SG_ GEAR_1ST_RATCH : 20|1@0+ (1,0) [0|1] "" XXX + SG_ GEAR_RATCHED_UP : 21|1@0+ (1,0) [0|1] "" XXX + SG_ GEAR_RATCHED_DOWN : 22|1@0+ (1,0) [0|1] "" XXX + SG_ GEAR_PRESSED : 36|2@0+ (1,0) [0|3] "" XXX + SG_ PARKING_BRAKE_BUTTON : 38|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER_3 : 51|12@0+ (1,0) [0|4095] "" XXX + +BO_ 85 SAS: 8 XXX + SG_ SAS_ANGLE_SENSOR : 6|15@0- (0.05596,0) [0|32767] "º" XXX + SG_ SAS_RATE_OF_CHANGE : 21|14@0- (1,0) [0|16383] "" XXX + SG_ SAS_CHECKSUM : 39|8@0+ (1,0) [0|4095] "" XXX + SG_ SAS_COUNTER : 44|4@1+ (1,0) [0|15] "" XXX + +BO_ 87 LCA_3: 8 XXX + SG_ NEW_SIGNAL_3 : 0|1@0+ (1,0) [0|1] "" XXX + SG_ LCA_ACCEPT_COMMANDS_RELATED : 4|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_2 : 7|3@0+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_5 : 12|5@0+ (1,0) [0|31] "" XXX + SG_ LCA_ACCEPT_COMMANDS_INV : 13|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_4 : 15|2@0+ (1,0) [0|3] "" XXX + SG_ SPEED_A : 23|16@0+ (1,0) [0|65535] "" XXX + SG_ SPEED_B : 39|16@0+ (1,0) [0|65535] "" XXX + SG_ NEW_SIGNAL_8 : 51|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER_1 : 53|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_7 : 55|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_9 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 88 LCA: 8 XXX + SG_ LCA_STEER_LOOSELY : 2|11@0- (1,0) [0|2047] "" XXX + SG_ LCA_ENABLE_INV : 3|1@0+ (1,0) [0|1] "" XXX + SG_ LANE_KEEP_ACTIVE_INV : 7|2@0+ (1,0) [0|3] "" XXX + SG_ LCA_STEER_LOOSELY_INV : 18|11@0- (1,0) [0|2047] "" XXX + SG_ NEW_SIGNAL_7 : 21|3@0+ (1,0) [0|7] "" XXX + SG_ LCA_RATE_OF_CHANGE : 39|8@0+ (1,0) [0|255] "" XXX + SG_ LCA_STEER : 45|14@0- (1,0) [0|16383] "" XXX + SG_ NEW_SIGNAL_1 : 47|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_6 : 60|4@1+ (1,0) [0|15] "" XXX + +BO_ 96 SPEED: 8 XXX + SG_ SPEED : 6|15@0+ (1,0) [0|32767] "" XXX + SG_ NEW_SIGNAL_1 : 7|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_2 : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ NEW_SIGNAL_4 : 24|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_3 : 27|3@0+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_6 : 35|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_5 : 36|1@0+ (1,0) [0|1] "" XXX + +BO_ 103 LCA_5: 8 XXX + SG_ WHEEL_SPEED_1 : 6|15@0+ (0.1,0) [0|255] "rpm" XXX + SG_ NEW_SIGNAL_4 : 7|1@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 23|8@0+ (1,0) [0|4095] "" XXX + SG_ NEW_SIGNAL_1 : 24|4@1+ (1,0) [0|15] "" XXX + SG_ COUNTER : 28|4@1+ (1,0) [0|15] "" XXX + SG_ WHEEL_SPEED_2 : 39|15@0+ (0.1,0) [0|32767] "rpm" XXX + SG_ NEW_SIGNAL_5 : 40|1@0+ (1,0) [0|1] "" XXX + SG_ LCA_5_STEER : 54|15@0- (0.05596,0) [0|32767] "deg" XXX + SG_ NEW_SIGNAL_2 : 55|1@0+ (1,0) [0|1] "" XXX + +BO_ 104 SPEED_2: 8 XXX + SG_ WHEEL_SPEED_LEFT : 6|15@0+ (1,0) [0|32767] "" XXX + SG_ NEW_SIGNAL_1 : 7|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_2 : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ NEW_SIGNAL_3 : 27|4@0+ (1,0) [0|15] "" XXX + SG_ WHEEL_SPEED_RIGHT : 39|15@0+ (1,0) [0|32767] "" XXX + SG_ COUNTER_1 : 51|12@0+ (1,0) [0|4095] "" XXX + SG_ NEW_SIGNAL_4 : 53|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER_2 : 54|1@0+ (1,0) [0|1] "" XXX + +BO_ 105 LCA_2: 8 XXX + SG_ BYTE_0 : 7|8@0+ (1,0) [0|2047] "" XXX + SG_ COUNTER_1 : 11|4@0+ (1,0) [0|4095] "" XXX + SG_ PILOT_ASSIST_ENGAGED : 12|1@0+ (1,0) [0|1] "" XXX + SG_ ESC_ACTUATING : 13|1@0+ (1,0) [0|1] "" XXX + SG_ ESC_ELIGIBLE : 14|1@0+ (1,0) [0|1] "" XXX + SG_ BYTE_1_BITFIELD_0 : 15|1@0+ (1,0) [0|7] "" XXX + SG_ CHECKSUM_2 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_2 : 31|16@0+ (1,0) [0|65535] "" XXX + SG_ COUNTER_2 : 43|4@0+ (1,0) [0|4095] "" XXX + SG_ NEW_SIGNAL_3 : 45|2@0+ (1,0) [0|3] "" XXX + SG_ BRAKE_PEDAL_PRESSED_B : 46|1@0+ (1,0) [0|1] "" XXX + SG_ BRAKE_PEDAL_PRESSED_A : 47|1@0+ (-1,1) [0|1] "" XXX + SG_ CHECKSUM_1 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_7 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 128 GEAR_POSITION: 8 XXX + SG_ NEW_SIGNAL_3 : 3|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_2 : 4|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_1 : 7|3@0+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_5 : 11|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_4 : 15|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_17 : 17|2@0+ (1,0) [0|3] "" XXX + SG_ AEB_A : 18|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_18 : 19|1@0+ (1,0) [0|1] "" XXX + SG_ AEB_B : 20|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_6 : 23|3@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_7 : 24|8@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_8 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_13 : 43|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_10 : 46|3@0+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_9 : 47|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_14 : 51|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_12 : 54|3@0+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_11 : 55|1@0+ (1,0) [0|1] "" XXX + SG_ GEAR_POSITION : 58|3@0+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_16 : 62|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_15 : 63|1@0+ (1,0) [0|1] "" XXX + +BO_ 144 LCA_4: 8 XXX + SG_ BYTE_0 : 7|8@0+ (1,0) [0|16383] "" XXX + SG_ LCA_ENABLE : 9|2@0+ (1,0) [0|3] "" XXX + SG_ BYTE_1_FLAGS : 11|2@0+ (1,0) [0|3] "" XXX + SG_ BYTE_1_NIBBLE_HI : 15|4@0+ (1,0) [0|63] "" XXX + SG_ BYTE_2_3 : 23|16@0- (1,0) [0|65535] "" XXX + SG_ YAW_RATE : 39|16@0- (1,0) [0|65535] "" XXX + SG_ BYTE_6 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_7_NIBBLE_LO : 59|4@0+ (1,0) [0|15] "" XXX + SG_ BYTE_7_NIBBLE_HI : 60|4@1+ (1,0) [0|15] "" XXX + +BO_ 146 LCA_7: 8 XXX + SG_ NEW_SIGNAL_1 : 7|12@0+ (1,0) [0|4095] "" XXX + SG_ NEW_SIGNAL_2 : 11|4@0+ (1,0) [0|15] "" XXX + SG_ LCA_7_STEER : 23|15@0- (1,0) [0|32767] "" XXX + SG_ NEW_SIGNAL_3 : 24|2@0+ (1,0) [0|3] "" XXX + SG_ LCA_7_DELTA_STEER : 38|15@0- (1,0) [0|32767] "" XXX + SG_ NEW_SIGNAL_4 : 55|16@0+ (1,0) [0|65535] "" XXX + +BO_ 151 LCA_6: 8 XXX + SG_ LCA_6_STEER : 7|16@0- (1,0) [0|65535] "" XXX + SG_ NEW_SIGNAL_1 : 23|14@0- (1,0) [0|16383] "" XXX + SG_ NEW_SIGNAL_3 : 25|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_2 : 39|12@0+ (1,0) [0|4095] "" XXX + SG_ NEW_SIGNAL_4 : 43|4@0+ (1,0) [0|15] "" XXX + SG_ LCA_6_STEER_2 : 55|15@0- (1,0) [0|32767] "" XXX + SG_ NEW_SIGNAL_5 : 56|1@0+ (1,0) [0|1] "" XXX + +BO_ 336 NEW_MSG_150: 8 XXX + SG_ IGN_DRAFT : 5|2@0+ (1,0) [0|3] "" XXX + +BO_ 1336 NEW_MSG_538: 8 XXX + SG_ SWM_MULTIMEDIA_ACTIVITY : 22|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_1 : 23|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_3 : 24|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_2 : 29|1@0+ (1,0) [0|1] "" XXX + +CM_ BO_ 23 "Might be related to PSCM 0x16"; +CM_ SG_ 87 LCA_ACCEPT_COMMANDS_RELATED "Goes to 15 when actively operating LCA"; +CM_ SG_ 87 LCA_ACCEPT_COMMANDS_INV "Goes to 0 when PSCM allowed to receive LCA commands"; +CM_ SG_ 88 LCA_ENABLE_INV "Master enable, active-low"; +CM_ SG_ 103 WHEEL_SPEED_1 "Possible Front Left (FL)"; +CM_ SG_ 103 WHEEL_SPEED_2 "Possible Front Right (FR)"; +CM_ SG_ 105 ESC_ACTUATING "Goes to 1 in combination with ESC_ELIGIBLE when ESC is actively actuating"; +CM_ SG_ 105 ESC_ELIGIBLE "Goes to 1 on e.g. speed bump, but doesn't trigger any intervention"; +VAL_ 128 GEAR_POSITION 0 "Park" 1 "Reverse" 2 "Neutral" 3 "Drive" 4 "B-Mode"; diff --git a/opendbc_repo/opendbc/safety/declarations.h b/opendbc_repo/opendbc/safety/declarations.h index d9a9f2461..6da076324 100644 --- a/opendbc_repo/opendbc/safety/declarations.h +++ b/opendbc_repo/opendbc/safety/declarations.h @@ -34,6 +34,7 @@ #define SAFETY_RIVIAN 33U #define SAFETY_VOLKSWAGEN_MEB 34U #define SAFETY_TESLA_PREAP 35U +#define SAFETY_VOLVO 36U #define GET_BIT(msg, b) ((bool)!!(((msg)->data[((b) / 8U)] >> ((b) % 8U)) & 0x1U)) #define GET_FLAG(value, mask) (((value) & (mask)) == (mask)) @@ -380,4 +381,5 @@ extern const safety_hooks volkswagen_mqb_hooks; extern const safety_hooks volkswagen_pq_hooks; extern const safety_hooks rivian_hooks; extern const safety_hooks psa_hooks; +extern const safety_hooks volvo_hooks; extern const safety_hooks tesla_preap_hooks; diff --git a/opendbc_repo/opendbc/safety/modes/subaru.h b/opendbc_repo/opendbc/safety/modes/subaru.h index 4e3e1037a..20fdddab3 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru.h +++ b/opendbc_repo/opendbc/safety/modes/subaru.h @@ -292,7 +292,7 @@ static bool subaru_tx_hook(const CANPacket_t *msg) { if (msg->addr == MSG_SUBARU_Dashlights) { violation |= !subaru_stop_start_button; - violation |= msg->bus != SUBARU_ALT_BUS; + violation |= msg->bus != (subaru_d_platform ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS); violation |= !GET_BIT(msg, 54U); violation |= subaru_get_checksum(msg) != subaru_compute_checksum(msg); } @@ -341,6 +341,12 @@ static safety_config subaru_init(uint16_t param) { SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS) }; + static const CanMsg SUBARU_GEN2_LKAS_ANGLE_STOP_START_TX_MSGS[] = { + SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE) + SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS) + SUBARU_STOP_START_TX_MSGS(SUBARU_MAIN_BUS) + }; + static const CanMsg SUBARU_D_PLATFORM_ANGLE_MAIN_TX_MSGS[] = { SUBARU_D_PLATFORM_ANGLE_TX_MSGS(SUBARU_MAIN_BUS) SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS) @@ -409,7 +415,8 @@ static safety_config subaru_init(uint16_t param) { ret = subaru_d_platform ? (subaru_stop_start_button ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_STOP_START_MAIN_TX_MSGS) : \ (subaru_d_platform_camera ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_CAMERA_TX_MSGS) : \ BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_MAIN_TX_MSGS))) : \ - subaru_gen2 ? BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_TX_MSGS) : \ + subaru_gen2 ? (subaru_stop_start_button ? BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_STOP_START_TX_MSGS) : \ + BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_TX_MSGS)) : \ BUILD_SAFETY_CFG(subaru_lkas_angle_rx_checks, SUBARU_LKAS_ANGLE_TX_MSGS); } else if (subaru_gen2) { ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_LONG_TX_MSGS) : \ diff --git a/opendbc_repo/opendbc/safety/modes/volvo.h b/opendbc_repo/opendbc/safety/modes/volvo.h new file mode 100644 index 000000000..6abc593a6 --- /dev/null +++ b/opendbc_repo/opendbc/safety/modes/volvo.h @@ -0,0 +1,337 @@ +#pragma once + +#include "opendbc/safety/declarations.h" + +// safetyParam: 0 = CMA (XC40 Recharge), 1 = SPA (S60 Recharge, Polestar 2) +// Polestar 2 is technically CMA, but appears to use SPA DBC for CAN 1 bus +#define VOLVO_FLAG_SPA 1U + +// Volvo CAN message addresses shared between CMA and SPA +#define VOLVO_LCA_STEER 0x58U // TX from VCU1 to PSCM, LCA steering command (0x58) +#define VOLVO_LCA_2 0x69U // RX from BCM, brake pedal, cruise state +#define VOLVO_SAS 0x55U // RX from SAS, steering angle sensor +#define VOLVO_PSCM 0x16U // RX from PSCM, driver steering input +#define VOLVO_GEAR_POSITION 0x80U // RX from transmission, gear position +#define VOLVO_DRIVER_INPUT 0x15U +#define VOLVO_LCA_3 0x57U // TX from VCU1 to PSCM +#define VOLVO_LCA_5 0x67U // TX LCA_5 message (formerly SPEED_1, contains wheel speeds and LCA signals) +#define VOLVO_SPEED 0x60U // RX/TX SPEED message +#define VOLVO_SPEED_2 0x68U // RX +#define VOLVO_0x1a 0x1aU // RX +#define VOLVO_EGSM 0x45U // RX from EGSM +#define VOLVO_PSCM_RELATED 0x17U // RX from PSCM, related messages +#define VOLVO_LCA_4 0x90U // TX LCA_4 message (PA status spoofing) +#define VOLVO_LCA_6 0x97U // TX LCA_6 message +#define VOLVO_LCA_7 0x92U // TX LCA_7 message + +// CMA-specific PT bus addresses +#define VOLVO_CMA_BUS1_SPEED 0x70U // RX vehicle speed +#define VOLVO_CMA_ECM_1 0x250U // RX accelerator pedal position +#define VOLVO_CMA_BUS1_CRUISE_CONTROL 0x340U // RX cruise control state + +// SPA-specific PT bus addresses +#define VOLVO_SPA_BUS1_SPEED 0x75U // RX vehicle speed +#define VOLVO_SPA_ECM_1 0x25U // RX accelerator pedal position +#define VOLVO_SPA_BUS1_CRUISE_CONTROL 0x349U // RX cruise control state + +// SPEED (0x60) is raw counts in the DBC. Measured against GPS ground speed on two +// harnesses: implied LSB 0.0039736 and 0.0039792 m/s. +#define VOLVO_SPEED_TO_MS 0.003977f + +// LCA_5_STEER is a signed 15-bit steering-wheel-angle command in 0.05596 deg/count. +// Keep the absolute envelope aligned with the software controller's 540 deg limit. +#define VOLVO_ANGLE_DEG_TO_CAN 17.869907f +#define VOLVO_MAX_ANGLE_CAN 9650 +#define VOLVO_DRIVER_OVERRIDE 2 + + +// CAN bus definitions for Volvo +// Using same naming as carstate.py for consistency: main, pt, party +#define VOLVO_MAIN_BUS 0U // Bus.main - VCU1 car side +#define VOLVO_PT_BUS 1U // Bus.pt - VCU1 ECM side (where ECM is) +#define VOLVO_PARTY_BUS 2U // Bus.party - VCU PSCM/BCM2 side (BCM2, SAS, EGSM, PSCM, where LCA is sent to) + +// Runtime addresses set by volvo_init based on safetyParam +static uint16_t volvo_ecm_1_addr; +static uint16_t volvo_bus1_cruise_control_addr; + +static int volvo_be_15(const CANPacket_t *msg, uint8_t byte) { + return (int)(((uint16_t)(msg->data[byte] & 0x7FU) << 8U) | msg->data[byte + 1U]); +} + +static int volvo_pscm_angle(const CANPacket_t *msg) { + return to_signed(volvo_be_15(msg, 0U), 15); +} + +static int volvo_lca_5_angle(const CANPacket_t *msg) { + return to_signed(volvo_be_15(msg, 6U), 15); +} + +static const AngleSteeringLimits VOLVO_ANGLE_STEERING_LIMITS = { + .max_angle = VOLVO_MAX_ANGLE_CAN, + .angle_deg_to_can = VOLVO_ANGLE_DEG_TO_CAN, + .angle_rate_up_lookup = { + {0.0f, 5.0f, 25.0f}, + {2.5f, 1.5f, 0.2f}, + }, + .angle_rate_down_lookup = { + {0.0f, 5.0f, 25.0f}, + {5.0f, 2.0f, 0.3f}, + }, + .frequency = 50U, +}; + +static void volvo_rx_hook(const CANPacket_t *msg) { + // Monitor the vehicle state required for cruise, disengagement, and angle + // safety. All steering TX frames are separately constrained in volvo_tx_hook. + + // Main bus (bus 0) messages + if (msg->bus == VOLVO_MAIN_BUS) { + // Update brake pedal and cruise state from BCM2 + if (msg->addr == VOLVO_LCA_2) { + // DBC: SG_ BRAKE_PEDAL_PRESSED_A : 47|1@0+ (-1,1) - inverted in DBC, so we invert raw bit + // DBC: SG_ BRAKE_PEDAL_PRESSED_B : 46|1@0+ (1,0) - not inverted + //bool brake_a = !((msg->data[5] >> 7) & 1U); // Raw bit, active low (DBC inverts it) + bool brake_b = (msg->data[5] >> 6) & 1U; // Raw bit, active high + //brake_pressed = brake_a || brake_b; + brake_pressed = brake_b; + } + + // Vehicle speed from the main bus, matching carstate.py. The PT bus carries a + // speed message too, but which car bus lands on PT is harness-dependent and its + // scaling differs per PT DBC, so both sides read the main bus instead. + // DBC: SG_ SPEED : 6|15@0+ (1,0) - raw counts, scaled here + if (msg->addr == VOLVO_SPEED) { + uint16_t speed_raw = ((msg->data[0] & 0x7FU) << 8) | msg->data[1]; + float speed = (float)speed_raw * VOLVO_SPEED_TO_MS; + vehicle_moving = speed > 0.1; + UPDATE_VEHICLE_SPEED(speed); + } + } + + // PT bus (bus 1) messages + if (msg->bus == VOLVO_PT_BUS) { + if (msg->addr == volvo_ecm_1_addr) { + if (volvo_ecm_1_addr == VOLVO_CMA_ECM_1) { + // CMA: SG_ ACCELERATOR_PEDAL_POS : 31|8@0+ (1,0) [0|255] + uint8_t gas_pedal_position = msg->data[3]; + gas_pressed = gas_pedal_position > 21U; // 20 baseline + 1 tolerance + } else { + // SPA: SG_ ACCELERATOR_PEDAL_POS : 6|15@0+ (0.00390625,0) [0|32767] "%" + uint16_t gas_raw = ((msg->data[0] & 0x7FU) << 8) | msg->data[1]; + gas_pressed = (gas_raw * 0.00390625) > 1.0; // > 1% + } + } + + if (msg->addr == volvo_bus1_cruise_control_addr) { + bool cruise_enabled; + if (volvo_bus1_cruise_control_addr == VOLVO_CMA_BUS1_CRUISE_CONTROL) { + // CMA: SG_ CRUISE_CONTROL_ENABLED : 56|1@0+ and CRUISE_CONTROL_ENABLED_IDLE_TRAFFIC : 57|1@0+ + cruise_enabled = ((msg->data[7] & 1U) || (msg->data[7] & 2U)); + } else { + // SPA: SG_ CRUISE_CONTROL_SPA_ENABLED : 1|1@0+ (-1,1) — byte 0 bit 1, active low + cruise_enabled = !((msg->data[0] >> 1) & 1U); + } + pcm_cruise_check(cruise_enabled); + } + } + + // Party bus (bus 2) messages - BCM2, SAS, PSCM, EGSM + if (msg->bus == VOLVO_PARTY_BUS) { + + if (msg->addr == VOLVO_PSCM) { + // PSCM_ANGLE_SENSOR is the measurement consumed by carstate.py. It uses + // the same signed 0.05596 deg/count representation as LCA_5_STEER. + update_sample(&angle_meas, volvo_pscm_angle(msg)); + } + + // DRIVER_INPUT is the signal consumed by carstate.py for driver torque. + // The PSCM frame's DRIVER_INPUT_DEVIATION is a different signal and must + // not be substituted here: doing so leaves the hardware disengage path blind. + if (msg->addr == VOLVO_DRIVER_INPUT) { + // STEERING_DRIVER_INPUT is a Motorola signal starting at bit 55. The + // DBC also carries a +1 offset, so its raw byte is data[6]. + const int driver_input = to_signed(msg->data[6], 8) + 1; + update_sample(&torque_driver, driver_input); + steering_disengage = SAFETY_ABS(driver_input) > VOLVO_DRIVER_OVERRIDE; + } + + } +} + +static bool volvo_tx_hook(const CANPacket_t *msg) { + bool tx = true; + + // LCA_5 carries the actual angle command used by the controller. The stock + // LCA frame also contains an angle-shaped field, but the imported controller + // deliberately leaves that field at the observed vehicle value. + if (msg->addr == VOLVO_LCA_5) { + const int desired_angle = volvo_lca_5_angle(msg); + tx &= SAFETY_ABS(desired_angle) <= VOLVO_MAX_ANGLE_CAN; + tx &= !steer_angle_cmd_checks(desired_angle, controls_allowed, VOLVO_ANGLE_STEERING_LIMITS); + } + + // Keep the two torque-authority arms and the companion LCA angle bounded even + // though these fields are not the primary steering command. + if (msg->addr == VOLVO_LCA_STEER) { + const int authority_pos = to_signed((int)(((uint16_t)(msg->data[0] & 0x07U) << 8U) | msg->data[1]), 11); + const int authority_neg = to_signed((int)(((uint16_t)(msg->data[2] & 0x07U) << 8U) | msg->data[3]), 11); + const int lca_angle = to_signed((int)(((uint16_t)(msg->data[5] & 0x3FU) << 8U) | msg->data[6]), 14); + tx &= authority_pos >= 0 && authority_pos <= 614; + tx &= authority_neg >= -614 && authority_neg <= 0; + tx &= SAFETY_ABS(lca_angle) <= VOLVO_MAX_ANGLE_CAN; + } + + // PSCM is relayed back onto the main bus to preserve the stock hands-on-wheel + // path. Do not allow that relay to invent a steering-angle measurement. + if (msg->addr == VOLVO_PSCM) { + const int relayed_angle = volvo_pscm_angle(msg); + const int measured_max = SAFETY_CLAMP(angle_meas.max, -VOLVO_MAX_ANGLE_CAN, VOLVO_MAX_ANGLE_CAN) + 1; + const int measured_min = SAFETY_CLAMP(angle_meas.min, -VOLVO_MAX_ANGLE_CAN, VOLVO_MAX_ANGLE_CAN) - 1; + tx &= !safety_max_limit_check(relayed_angle, measured_max, measured_min); + } + + // NOTE: the wrong-bus rejections below are unreachable defense-in-depth: + // safety_tx_hook() only calls this hook after the message passes the + // VOLVO_TX_MSGS allowlist, which already pins each TX address to a single + // bus (and VOLVO_DRIVER_INPUT/VOLVO_SAS are not TX'able on any bus). + // Hence the GCOV_EXCL markers, following the defaults.h convention. + // test_volvo.py's test_tx_hook_wrong_bus_blocked pins down the blocked + // wrong-bus TX behavior at the safety_tx_hook() level. + if (msg->addr == VOLVO_LCA_STEER) { + // LCA message flows: VCU1 (main bus) -> PSCM (party bus) + // We're acting as VCU1, so we send LCA message to party bus (bus 2) + // GCOV_EXCL_START + // Unreachable by design (allowlist pins VOLVO_LCA_STEER to the party bus) + if (msg->bus != VOLVO_PARTY_BUS) { + tx = false; // Wrong bus + } + // GCOV_EXCL_STOP + } + + if (msg->addr == VOLVO_PSCM) { + // PSCM message: we relay from party bus (bus 2) to main bus (bus 0) + // So we TX on main bus (bus 0) + // GCOV_EXCL_START + // Unreachable by design (allowlist pins VOLVO_PSCM to the main bus) + if (msg->bus != VOLVO_MAIN_BUS) { + tx = false; // Wrong bus + } + // GCOV_EXCL_STOP + } + + if (msg->addr == VOLVO_DRIVER_INPUT) { + // Driver input message: we relay from party bus (bus 2) to main bus (bus 0) + // So we TX on main bus (bus 0) + // GCOV_EXCL_START + // Unreachable by design (VOLVO_DRIVER_INPUT is not in VOLVO_TX_MSGS) + if (msg->bus != VOLVO_MAIN_BUS) { + tx = false; // Wrong bus + } + } + // GCOV_EXCL_STOP + + if (msg->addr == VOLVO_SAS) { + // SAS message: we relay from party bus (bus 2) to main bus (bus 0) + // So we TX on main bus (bus 0) + // GCOV_EXCL_START + // Unreachable by design (VOLVO_SAS is not in VOLVO_TX_MSGS) + if (msg->bus != VOLVO_MAIN_BUS) { + tx = false; // Wrong bus + } + } + // GCOV_EXCL_STOP + + if (msg->addr == VOLVO_LCA_2) { + // LCA_2 -> PSCM + // GCOV_EXCL_START + // Unreachable by design (allowlist pins VOLVO_LCA_2 to the party bus) + if (msg->bus != VOLVO_PARTY_BUS) { + tx = false; // Wrong bus + } + // GCOV_EXCL_STOP + } + + return tx; +} + +static safety_config volvo_init(uint16_t param) { + bool spa = GET_FLAG(param, VOLVO_FLAG_SPA); + + // Set PT bus addresses based on platform + volvo_ecm_1_addr = spa ? VOLVO_SPA_ECM_1 : VOLVO_CMA_ECM_1; + volvo_bus1_cruise_control_addr = spa ? VOLVO_SPA_BUS1_CRUISE_CONTROL : VOLVO_CMA_BUS1_CRUISE_CONTROL; + + // Define allowed TX messages - very permissive + static const CanMsg VOLVO_TX_MSGS[] = { + {VOLVO_LCA_STEER, VOLVO_PARTY_BUS, 8, .check_relay = true}, // LCA steering command to party bus + {VOLVO_PSCM, VOLVO_MAIN_BUS, 8, .check_relay = true}, // PSCM message sent to main bus (relay from party bus) + {VOLVO_LCA_3, VOLVO_PARTY_BUS, 8, .check_relay = true}, // LCA_3 message sent to party bus + {VOLVO_LCA_2, VOLVO_PARTY_BUS, 8, .check_relay = true}, // LCA_2 message sent to party bus (spoof PILOT_ASSIST_ENGAGED for PSCM) + {VOLVO_LCA_4, VOLVO_PARTY_BUS, 8, .check_relay = true}, // LCA_4 message sent to party bus (spoof LCA_ENABLE for PA state) + {VOLVO_LCA_5, VOLVO_PARTY_BUS, 8, .check_relay = true}, // LCA_5 message sent to party bus (wheel speeds + LCA signals) + {VOLVO_LCA_6, VOLVO_PARTY_BUS, 8, .check_relay = true}, // LCA_6 message sent to party bus + {VOLVO_LCA_7, VOLVO_PARTY_BUS, 8, .check_relay = true}, // LCA_7 message sent to party bus + //{VOLVO_SPEED, VOLVO_PARTY_BUS, 8, .check_relay = true}, // SPEED message sent to main bus + //{VOLVO_SPEED_2, VOLVO_PARTY_BUS, 8, .check_relay = true}, // SPEED_2 message sent to main bus + //{VOLVO_0x1a, VOLVO_PARTY_BUS, 8, .check_relay = true}, // 0x1a message sent to main bus + //{VOLVO_GEAR_POSITION, VOLVO_PARTY_BUS, 8, .check_relay = true}, // GEAR_POSITION message sent from main to party bus + //{VOLVO_EGSM, VOLVO_MAIN_BUS, 8, .check_relay = true}, // EGSM message sent from party to main bus + {VOLVO_PSCM_RELATED, VOLVO_MAIN_BUS, 8, .check_relay = true}, // PSCM_RELATED message sent to party bus + }; + + // Define RX checks - PT bus addresses depend on CMA vs SPA + safety_config ret; + if (!spa) { + static RxCheck volvo_rx_checks_cma[] = { + {.msg = {{VOLVO_GEAR_POSITION, VOLVO_MAIN_BUS, 8, 40U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_LCA_2, VOLVO_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_LCA_4, VOLVO_MAIN_BUS, 8, 29U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_LCA_6, VOLVO_MAIN_BUS, 8, 25U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_LCA_7, VOLVO_MAIN_BUS, 8, 29U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_SAS, VOLVO_PARTY_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_PSCM, VOLVO_PARTY_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_DRIVER_INPUT, VOLVO_PARTY_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_CMA_ECM_1, VOLVO_PT_BUS, 8, 17U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_LCA_STEER, VOLVO_MAIN_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_CMA_BUS1_CRUISE_CONTROL, VOLVO_PT_BUS, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_LCA_3, VOLVO_MAIN_BUS, 8, 67U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_LCA_5, VOLVO_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_SPEED, VOLVO_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_SPEED_2, VOLVO_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_EGSM, VOLVO_PARTY_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_PSCM_RELATED, VOLVO_PARTY_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + }; + ret = BUILD_SAFETY_CFG(volvo_rx_checks_cma, VOLVO_TX_MSGS); + } else { + static RxCheck volvo_rx_checks_spa[] = { + {.msg = {{VOLVO_GEAR_POSITION, VOLVO_MAIN_BUS, 8, 40U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_LCA_2, VOLVO_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_LCA_4, VOLVO_MAIN_BUS, 8, 29U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_LCA_6, VOLVO_MAIN_BUS, 8, 25U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_LCA_7, VOLVO_MAIN_BUS, 8, 29U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_SAS, VOLVO_PARTY_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_PSCM, VOLVO_PARTY_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_DRIVER_INPUT, VOLVO_PARTY_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_SPA_ECM_1, VOLVO_PT_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_LCA_STEER, VOLVO_MAIN_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + // true rate is 5Hz, but safety_tick invalidates checks declared <10Hz; lagging floor is 1s either way + {.msg = {{VOLVO_SPA_BUS1_CRUISE_CONTROL, VOLVO_PT_BUS, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_LCA_3, VOLVO_MAIN_BUS, 8, 67U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_LCA_5, VOLVO_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_SPEED, VOLVO_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_SPEED_2, VOLVO_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_EGSM, VOLVO_PARTY_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{VOLVO_PSCM_RELATED, VOLVO_PARTY_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + }; + ret = BUILD_SAFETY_CFG(volvo_rx_checks_spa, VOLVO_TX_MSGS); + } + return ret; +} + +const safety_hooks volvo_hooks = { + .init = volvo_init, + .rx = volvo_rx_hook, + .tx = volvo_tx_hook, + // No custom fwd hook - stock LCA always blocked by .check_relay = true +}; diff --git a/opendbc_repo/opendbc/safety/safety.h b/opendbc_repo/opendbc/safety/safety.h index f9beaf52d..0aa280fbe 100644 --- a/opendbc_repo/opendbc/safety/safety.h +++ b/opendbc_repo/opendbc/safety/safety.h @@ -27,6 +27,7 @@ #include "opendbc/safety/modes/elm327.h" #include "opendbc/safety/modes/body.h" #include "opendbc/safety/modes/psa.h" +#include "opendbc/safety/modes/volvo.h" #ifdef CANFD #include "opendbc/safety/modes/hyundai_canfd.h" @@ -427,6 +428,7 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { {SAFETY_RIVIAN, &rivian_hooks}, {SAFETY_TESLA, &tesla_hooks}, {SAFETY_TESLA_PREAP, &tesla_preap_hooks}, + {SAFETY_VOLVO, &volvo_hooks}, #ifdef CANFD {SAFETY_HYUNDAI_CANFD, &hyundai_canfd_hooks}, {SAFETY_VOLKSWAGEN_MEB, &volkswagen_meb_hooks}, diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru.py b/opendbc_repo/opendbc/safety/tests/test_subaru.py index 28d5a9d8b..91effbe43 100755 --- a/opendbc_repo/opendbc/safety/tests/test_subaru.py +++ b/opendbc_repo/opendbc/safety/tests/test_subaru.py @@ -370,6 +370,21 @@ class TestSubaruGen2FixedAngleSafety(TestSubaruGen2AngleStockLongitudinalSafety) raise unittest.SkipTest("Breakpoint angle limits do not enforce a real-time message frequency") +class TestSubaruGen2FixedAngleStopStartSafety(TestSubaruGen2FixedAngleSafety): + FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.FIXED_ANGLE_LIMITS | \ + SubaruSafetyFlags.STOP_START_BUTTON + TX_MSGS = TestSubaruGen2FixedAngleSafety.TX_MSGS + [[SubaruMsg.Dashlights, SUBARU_MAIN_BUS]] + + def _stop_start_msg(self, pressed): + return self.packer.make_can_msg_safety( + "Dashlights", SUBARU_MAIN_BUS, {"COUNTER": 0, "STOP_START": pressed}, + ) + + def test_stop_start_tx_requires_pressed_bit(self): + self.assertTrue(self._tx(self._stop_start_msg(True))) + self.assertFalse(self._tx(self._stop_start_msg(False))) + + class TestSubaruDPlatformAngleSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase): FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM ALT_MAIN_BUS = SUBARU_ALT_BUS diff --git a/opendbc_repo/opendbc/safety/tests/test_volvo.py b/opendbc_repo/opendbc/safety/tests/test_volvo.py new file mode 100644 index 000000000..3716d63be --- /dev/null +++ b/opendbc_repo/opendbc/safety/tests/test_volvo.py @@ -0,0 +1,320 @@ +#!/usr/bin/env python3 +""" +Safety tests for Volvo CMA/SPA. + +The safety mode lives at ``opendbc/safety/modes/volvo.h`` and is parameterized +by ``safetyParam``: + + - ``safetyParam == 0`` → CMA platform (Volvo XC40 Recharge) + - ``safetyParam == VOLVO_FLAG_SPA`` → SPA platform (Volvo S60 Recharge, + Polestar 2) + +The two platforms share LCA/PSCM/etc. addresses on the main and party buses +but use *different* PT-bus addresses and signal scales for ECM_1 and +BUS1_CRUISE_CONTROL. Vehicle speed is read from main-bus SPEED on both, so it +is not platform-dependent. This test file exercises both platforms through the +same generic ``CarSafetyTest`` harness so that any future divergence between +``carstate.py`` and ``volvo.h`` — e.g. a threshold drifting out of sync — is +caught on a laptop instead of in the car. + +Companion to: ``opendbc/car/volvo/carstate.py`` (must agree on thresholds). +""" + +import pathlib +import re +import unittest + +from opendbc.car.volvo.interface import SAFETY_VOLVO +from opendbc.safety.tests.libsafety import libsafety_py +import opendbc.safety.tests.common as common +from opendbc.safety.tests.common import CANPackerSafety + + +# Must match VOLVO_FLAG_SPA in opendbc/safety/modes/volvo.h +VOLVO_FLAG_SPA = 1 + +# Must match VOLVO_SPEED_TO_MS in volvo.h and SPEED_TO_MS in carstate.py +VOLVO_SPEED_TO_MS = 0.003977 + +# Bus layout (must match volvo.h) +VOLVO_MAIN_BUS = 0 +VOLVO_PT_BUS = 1 +VOLVO_PARTY_BUS = 2 + +# Shared platform-independent addresses +VOLVO_SPEED = 0x60 +VOLVO_LCA_STEER = 0x58 +VOLVO_LCA_2 = 0x69 +VOLVO_LCA_3 = 0x57 +VOLVO_LCA_4 = 0x90 +VOLVO_LCA_5 = 0x67 +VOLVO_LCA_6 = 0x97 +VOLVO_LCA_7 = 0x92 +VOLVO_PSCM = 0x16 +VOLVO_PSCM_RELATED = 0x17 +VOLVO_SAS = 0x55 +VOLVO_DRIVER_INPUT = 0x15 + +# Addresses panda TX'd with .check_relay = true, grouped by bus +_TX_PARTY_BUS_ADDRS = ( + VOLVO_LCA_STEER, VOLVO_LCA_2, VOLVO_LCA_3, VOLVO_LCA_4, + VOLVO_LCA_5, VOLVO_LCA_6, VOLVO_LCA_7, +) +_TX_MAIN_BUS_ADDRS = (VOLVO_PSCM, VOLVO_PSCM_RELATED) + + +class TestVolvoSafetyBase(common.CarSafetyTest): + """Shared RX/TX/relay tests for both CMA and SPA. + + Subclasses pin down the PT-bus DBC and per-platform gas/cruise encoding. + """ + + TX_MSGS = ( + [[a, VOLVO_PARTY_BUS] for a in _TX_PARTY_BUS_ADDRS] + + [[a, VOLVO_MAIN_BUS] for a in _TX_MAIN_BUS_ADDRS] + ) + RELAY_MALFUNCTION_ADDRS = { + VOLVO_PARTY_BUS: _TX_PARTY_BUS_ADDRS, + VOLVO_MAIN_BUS: _TX_MAIN_BUS_ADDRS, + } + # volvo.h installs no custom fwd hook, so the panda uses the default main<->party + # forwarding (bus 0 <-> bus 2). For each TX addr with .check_relay=true, the panda + # automatically refuses to forward from the bus where stock traffic lives TO the + # bus where openpilot is TX'ing — that's what keeps the stock ECU from fighting us. + FWD_BLACKLISTED_ADDRS = { + VOLVO_MAIN_BUS: list(_TX_PARTY_BUS_ADDRS), # stock on 0 must not forward to 2 + VOLVO_PARTY_BUS: list(_TX_MAIN_BUS_ADDRS), # stock on 2 must not forward to 0 + } + STANDSTILL_THRESHOLD = 0.1 # m/s, matches volvo.h + + # Subclasses set these + SAFETY_PARAM: int = 0 + PT_DBC: str = "" + + MID_DBC = "volvo_mid_1" + + @classmethod + def setUpClass(cls): + if cls.__name__ == "TestVolvoSafetyBase": + cls.safety = None + raise unittest.SkipTest + super().setUpClass() + + def setUp(self): + self.pt_packer = CANPackerSafety(self.PT_DBC) + self.mid_packer = CANPackerSafety(self.MID_DBC) + # CarSafetyTest.packer is referenced by a few generic tests; point it at + # the mid-bus DBC since most shared messages live there. + self.packer = self.mid_packer + self.safety = libsafety_py.libsafety + self.safety.set_safety_hooks(SAFETY_VOLVO, self.SAFETY_PARAM) + self.safety.init_tests() + + def test_tx_hook_on_wrong_safety_mode(self): + # CMA and SPA intentionally share the party-bus TX allowlist. safetyParam + # selects the PT-bus RX encodings, not a different set of steering frames. + self.skipTest("CMA and SPA intentionally share Volvo TX IDs") + + # ---- Abstract methods from CarSafetyTest ---- + + def _user_brake_msg(self, brake): + # volvo.h reads BRAKE_PEDAL_PRESSED_B (bit 46, factor 1, active high) + values = {"BRAKE_PEDAL_PRESSED_B": 1 if brake else 0} + return self.mid_packer.make_can_msg_safety("LCA_2", VOLVO_MAIN_BUS, values) + + def _speed_msg(self, speed): + # Main-bus SPEED, the same message on both platforms. The DBC carries it as + # raw counts (factor 1) and volvo.h applies VOLVO_SPEED_TO_MS, so convert here. + values = {"SPEED": round(speed / VOLVO_SPEED_TO_MS)} + return self.mid_packer.make_can_msg_safety("SPEED", VOLVO_MAIN_BUS, values) + + def _speed_msg_2(self, speed): + # Volvo's safety mode only consumes a single vehicle-speed source, so the + # generic rx_hook_speed_mismatch test doesn't apply. Returning None asks + # the harness to skip it (see common.CarSafetyTest.test_rx_hook_speed_mismatch). + return None + + def _angle_meas_msg(self, angle): + return self.mid_packer.make_can_msg_safety( + "PSCM", VOLVO_PARTY_BUS, {"PSCM_ANGLE_SENSOR": angle}) + + def _angle_cmd_msg(self, angle): + return self.mid_packer.make_can_msg_safety( + "LCA_5", VOLVO_PARTY_BUS, {"LCA_5_STEER": angle}) + + def _reset_angle_measurement(self, angle): + for _ in range(common.MAX_SAMPLE_VALS): + self._rx(self._angle_meas_msg(angle)) + + def _reset_speed_measurement(self, speed): + for _ in range(common.MAX_SAMPLE_VALS): + self._rx(self._speed_msg(speed)) + + def test_angle_tx_is_bounded(self): + """The LCA_5 angle command must obey the measured-angle safety envelope.""" + self._reset_angle_measurement(0) + self._reset_speed_measurement(10) + + self.safety.set_controls_allowed(True) + self.safety.set_desired_angle_last(0) + self.assertTrue(self._tx(self._angle_cmd_msg(0))) + + # The software controller's ±540 degree envelope is also enforced by the + # panda. Set the previous command explicitly so this test isolates the + # absolute bound from the rate limiter. + angle_can = round(540 / 0.05596) + self.safety.set_desired_angle_last(angle_can) + self.assertTrue(self._tx(self._angle_cmd_msg(540))) + self.safety.set_desired_angle_last(0) + self.assertFalse(self._tx(self._angle_cmd_msg(541))) + + def test_angle_tx_follows_measurement_when_controls_disabled(self): + self._reset_angle_measurement(10) + self.safety.set_controls_allowed(False) + + self.assertTrue(self._tx(self._angle_cmd_msg(10))) + self.assertFalse(self._tx(self._angle_cmd_msg(20))) + + def test_lca_authority_is_bounded(self): + self.safety.set_controls_allowed(True) + valid = { + "LCA_STEER_LOOSELY": 614, + "LCA_STEER_LOOSELY_INV": -614, + "LCA_STEER": 0, + } + self.assertTrue(self._tx(self.mid_packer.make_can_msg_safety( + "LCA", VOLVO_PARTY_BUS, valid))) + + for key, value in (("LCA_STEER_LOOSELY", 615), + ("LCA_STEER_LOOSELY_INV", -615)): + invalid = valid | {key: value} + self.assertFalse(self._tx(self.mid_packer.make_can_msg_safety( + "LCA", VOLVO_PARTY_BUS, invalid))) + + def test_pscm_relay_cannot_invent_angle(self): + self._reset_angle_measurement(10) + valid = self.mid_packer.make_can_msg_safety( + "PSCM", VOLVO_MAIN_BUS, {"PSCM_ANGLE_SENSOR": 10}) + invalid = self.mid_packer.make_can_msg_safety( + "PSCM", VOLVO_MAIN_BUS, {"PSCM_ANGLE_SENSOR": 20}) + self.assertTrue(self._tx(valid)) + self.assertFalse(self._tx(invalid)) + + def test_driver_override_disengages_controls(self): + self.safety.set_controls_allowed(True) + msg = self.mid_packer.make_can_msg_safety( + "DRIVER_INPUT", VOLVO_PARTY_BUS, {"STEERING_DRIVER_INPUT": 6}) + self._rx(msg) + self.assertFalse(self.safety.get_controls_allowed()) + + # ---- Volvo-specific consistency tests ---- + + def test_tx_hook_wrong_bus_blocked(self): + """ + Every TX address is pinned to exactly one bus by the VOLVO_TX_MSGS + allowlist, which safety_tx_hook() enforces before volvo_tx_hook() runs. + TX attempts anywhere off the allowlisted bus must be rejected, even with + controls allowed. Also covers the relayed-RX addresses (SAS, DRIVER_INPUT) + that volvo_tx_hook() used to re-check: they are not TX'able on any bus. + """ + tx_msgs = {tuple(m) for m in self.TX_MSGS} + addrs = {addr for addr, _ in tx_msgs} | {VOLVO_SAS, VOLVO_DRIVER_INPUT} + self.safety.set_controls_allowed(True) + for addr in sorted(addrs): + for bus in range(4): + if (addr, bus) in tx_msgs: + self.assertTrue(self._tx(common.make_msg(bus, addr)), + f"blocked TX addr={hex(addr)} on allowlisted bus={bus}") + else: + self.assertFalse(self._tx(common.make_msg(bus, addr)), + f"allowed TX addr={hex(addr)} on wrong bus={bus}") + + def test_gas_threshold_self_consistent(self): + """ + Sanity: the panda's gas_pressed edge is exactly where GAS_PRESSED_THRESHOLD + is defined. Regression guard for the CMA-vs-SPA scale mismatch bug where + carstate.py applied CMA thresholds (raw 0-255) to SPA's DBC-scaled percent. + If you change the threshold in volvo.h, update carstate.py AND this test. + """ + # Just below threshold → not pressed. Use a value strictly less than threshold. + just_below = max(self.GAS_PRESSED_THRESHOLD - 1, 0) + self._rx(self._user_gas_msg(just_below)) + self.assertFalse(self.safety.get_gas_pressed_prev(), + f"gas flagged at {just_below} (threshold={self.GAS_PRESSED_THRESHOLD})") + + # Just above threshold → pressed. + self._rx(self._user_gas_msg(self.GAS_PRESSED_THRESHOLD + 1)) + self.assertTrue(self.safety.get_gas_pressed_prev(), + f"gas not flagged above {self.GAS_PRESSED_THRESHOLD}") + + def test_speed_scale_self_consistent(self): + """ + The main-bus SPEED LSB is written out in three places: volvo.h, carstate.py + and this file. They must agree, or the panda and openpilot disagree about + vehicle speed. Parsed as text so this holds without importing either. + """ + root = pathlib.Path(__file__).parents[3] + sources = { + "volvo.h": (root / "opendbc/safety/modes/volvo.h", + r"#define\s+VOLVO_SPEED_TO_MS\s+([0-9.]+)f?"), + "carstate.py": (root / "opendbc/car/volvo/carstate.py", + r"^SPEED_TO_MS\s*=\s*([0-9.]+)"), + } + for name, (path, pattern) in sources.items(): + m = re.search(pattern, path.read_text(), re.MULTILINE) + self.assertIsNotNone(m, f"could not find the speed LSB in {name}") + self.assertEqual(float(m.group(1)), VOLVO_SPEED_TO_MS, + f"{name} speed LSB disagrees with test_volvo.py") + + +class TestVolvoCMA(TestVolvoSafetyBase): + """ + Volvo CMA: XC40 Recharge. + + ECM_1.ACCELERATOR_PEDAL_POS is raw 0-255 (DBC factor 1). Idle is ~20; panda + fires gas_pressed above 21. carstate.py uses the same raw threshold. + """ + SAFETY_PARAM = 0 + PT_DBC = "volvo_front_1_cma" + GAS_PRESSED_THRESHOLD = 21 # raw counts + + def _user_gas_msg(self, gas): + values = {"ACCELERATOR_PEDAL_POS": gas} + return self.pt_packer.make_can_msg_safety("ECM_1", VOLVO_PT_BUS, values) + + def _pcm_status_msg(self, enable): + # CMA cruise is a pair of bools; either one being high enables cruise. + values = {"CRUISE_CONTROL_ENABLED": 1 if enable else 0, + "CRUISE_CONTROL_ENABLED_IDLE_TRAFFIC": 0} + return self.pt_packer.make_can_msg_safety( + "BUS1_CRUISE_CONTROL", VOLVO_PT_BUS, values) + + +class TestVolvoSPA(TestVolvoSafetyBase): + """ + Volvo SPA: S60 Recharge, Polestar 2. + + ECM_1.ACCELERATOR_PEDAL_POS is DBC-scaled to percent (factor 0.00390625, + range 0-100%). Idle is ~0; panda fires gas_pressed above 1.0%. + carstate.py MUST use the same percent-based threshold — see + opendbc/car/volvo/carstate.py. A mismatch here manifests in test_models.py + as persistent ``panda safety doesn't agree with openpilot: {'gasPressed': N}``. + """ + SAFETY_PARAM = VOLVO_FLAG_SPA + PT_DBC = "volvo_front_1_spa" + GAS_PRESSED_THRESHOLD = 1.0 # percent + + def _user_gas_msg(self, gas): + values = {"ACCELERATOR_PEDAL_POS": gas} + return self.pt_packer.make_can_msg_safety("ECM_1", VOLVO_PT_BUS, values) + + def _pcm_status_msg(self, enable): + # SPA cruise signal has DBC factor -1 offset 1, so the "enabled" DBC value + # is 1 (which encodes as raw bit 0 — panda inverts it). + values = {"CRUISE_CONTROL_SPA_ENABLED": 1 if enable else 0} + return self.pt_packer.make_can_msg_safety( + "BUS1_CRUISE_CONTROL", VOLVO_PT_BUS, values) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 654ad3ab4..9dbb007b2 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -637,6 +637,7 @@ class LatControlTorque(LatControl): output_torque *= get_genesis_g70_high_speed_error_scale( setpoint, measurement, desired_lateral_jerk, CS.vEgo, ) + output_torque *= get_genesis_g70_angle_output_scale(CS.steeringAngleDeg, output_torque) low_speed_output_limit = get_genesis_g70_low_speed_output_limit(setpoint, CS.vEgo) output_torque = float(np.clip(output_torque, -low_speed_output_limit, low_speed_output_limit)) elif self.is_genesis_gv70: diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 8cde01a2a..13231f5a3 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -301,6 +301,9 @@ GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR = 0.18 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH = 0.15 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK = 0.15 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH = 0.10 +GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN = 0.45 +GENESIS_G70_ANGLE_OUTPUT_TAPER_START = 70.0 +GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH = 6.0 BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3 BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045 @@ -3179,6 +3182,16 @@ def get_genesis_g70_low_speed_output_limit(desired_lateral_accel: float, v_ego: return max(0.05, 1.0 - GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_REDUCTION * speed_weight * center_weight) +def get_genesis_g70_angle_output_scale(steering_angle_deg: float, output_torque: float) -> float: + """Ease G70 torque as it approaches the EPS high-angle protection threshold.""" + if steering_angle_deg == 0.0 or output_torque * steering_angle_deg <= 0.0: + return 1.0 + + angle_weight = _sigmoid((abs(steering_angle_deg) - GENESIS_G70_ANGLE_OUTPUT_TAPER_START) / + GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH) + return 1.0 - ((1.0 - GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN) * angle_weight) + + def get_genesis_g70_curve_unwind_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: if desired_lateral_accel * desired_lateral_jerk >= 0.0: diff --git a/selfdrive/controls/lib/lead_behavior.py b/selfdrive/controls/lib/lead_behavior.py index 600b855ea..46c8ff978 100644 --- a/selfdrive/controls/lib/lead_behavior.py +++ b/selfdrive/controls/lib/lead_behavior.py @@ -116,7 +116,11 @@ def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_ga v_cruise: float | None = None, y_rel: float | None = None, min_headway_margin: float = TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN, full_headway_margin: float = TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN, - bias_gain: float = TRACKED_LEAD_CATCHUP_BIAS_GAIN) -> float: + bias_gain: float = TRACKED_LEAD_CATCHUP_BIAS_GAIN, + bias_cap: float | None = None, + speed_range: tuple[float, float] | None = None, + fade_margins: tuple[float, float] | None = None, + cruise_error_full: float = TRACKED_LEAD_CATCHUP_BIAS_CRUISE_ERROR_FULL) -> float: gap_error = lead_distance - desired_gap actual_hw = lead_distance / max(v_ego, 1e-3) desired_hw = desired_gap / max(v_ego, 1e-3) @@ -125,20 +129,24 @@ def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_ga if gap_error <= 0.0: return 0.0 - speed_factor = _smoothstep(v_ego, HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED, TRACKED_LEAD_CATCHUP_BIAS_FULL_SPEED) + speed_min, speed_full = speed_range or (HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED, TRACKED_LEAD_CATCHUP_BIAS_FULL_SPEED) + speed_factor = _smoothstep(v_ego, speed_min, speed_full) cruise_factor = 1.0 if v_cruise is not None: - cruise_factor = _smoothstep(v_cruise - v_ego, 0.0, TRACKED_LEAD_CATCHUP_BIAS_CRUISE_ERROR_FULL) + cruise_factor = _smoothstep(v_cruise - v_ego, 0.0, cruise_error_full) if speed_factor == 0.0 or cruise_factor == 0.0: return 0.0 # Encourage ACC to treat a tracked lead as the active constraint when we're # hanging far above the requested time gap, but don't override cruise for a # truly distant lead or one we're already closing on decisively. - fade_start_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN, - TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_START - desired_hw) - fade_end_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN, - TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END - desired_hw) + if fade_margins is None: + fade_start_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN, + TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_START - desired_hw) + fade_end_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN, + TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END - desired_hw) + else: + fade_start_margin, fade_end_margin = fade_margins entry_factor = _smoothstep(headway_margin, min_headway_margin, full_headway_margin) @@ -155,8 +163,9 @@ def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_ga TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET, TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET) - bias_cap = max(10.0, TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR * v_ego) - return (min(gap_error * max(0.0, float(bias_gain)), bias_cap) * speed_factor * cruise_factor * + if bias_cap is None: + bias_cap = max(10.0, TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR * v_ego) + return (min(gap_error * max(0.0, float(bias_gain)), float(bias_cap)) * speed_factor * cruise_factor * entry_factor * exit_factor * closing_factor * lateral_factor) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 6d1d737e5..8db19f206 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -925,7 +925,9 @@ class LongitudinalMpc: optional_far_lead_comfort=True, smooth_duplicate_vision=False, stop_x=None, silverado_early_follow=False, modelV2=None, lead_obstacle_bias=(0.0, 0.0), tracked_lead_catchup_headway_margins=None, - tracked_lead_catchup_bias_gain=None): + tracked_lead_catchup_bias_gain=None, tracked_lead_catchup_bias_cap=None, + tracked_lead_catchup_speed_range=None, tracked_lead_catchup_fade_margins=None, + tracked_lead_catchup_cruise_error_full=None): v_ego = self.x0[1] lead_one = radarstate.leadOne lead_two = radarstate.leadTwo @@ -970,7 +972,9 @@ class LongitudinalMpc: cruise_obstacle += self.get_stable_follow_cruise_hysteresis(lead_one, v_ego, t_follow) elif prev_source == 'lead1': cruise_obstacle += self.get_stable_follow_cruise_hysteresis(lead_two, v_ego, t_follow) - if optional_far_lead_comfort and tracking_lead and lead_one.status: + crv_tracked_lead_tune = tracked_lead_catchup_bias_cap is not None + if (optional_far_lead_comfort and tracking_lead and lead_one.status and + (not crv_tracked_lead_tune or bool(getattr(lead_one, "radar", False)))): desired_gap = desired_follow_distance(v_ego, lead_one.vLead, t_follow) closing_speed = max(0.0, v_ego - lead_one.vLead) catchup_kwargs = {} @@ -981,6 +985,14 @@ class LongitudinalMpc: } if tracked_lead_catchup_bias_gain is not None: catchup_kwargs["bias_gain"] = tracked_lead_catchup_bias_gain + if tracked_lead_catchup_bias_cap is not None: + catchup_kwargs["bias_cap"] = tracked_lead_catchup_bias_cap + if tracked_lead_catchup_speed_range is not None: + catchup_kwargs["speed_range"] = tracked_lead_catchup_speed_range + if tracked_lead_catchup_fade_margins is not None: + catchup_kwargs["fade_margins"] = tracked_lead_catchup_fade_margins + if tracked_lead_catchup_cruise_error_full is not None: + catchup_kwargs["cruise_error_full"] = tracked_lead_catchup_cruise_error_full cruise_obstacle += get_tracked_lead_catchup_bias( v_ego, lead_one.dRel, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 10b56514a..98a202dbb 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -28,6 +28,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_toyota_rav4_tss2_lead_departure_tune, get_force_stop_distance_bias, get_force_stop_handoff_distance, + get_stop_sign_low_speed_hold, allow_radar_standstill_gap_settle, is_gm_silverado_early_follow_lead, is_toyota_rav4_tss2_post_departure_tune, @@ -44,6 +45,10 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_standstill_stopped_lead_guard_distance_margin, get_standstill_stopped_lead_guard_max_lead_speed, get_tracked_lead_catchup_bias_gain, + get_tracked_lead_catchup_bias_cap, + get_tracked_lead_catchup_speed_range, + get_tracked_lead_catchup_fade_margins, + get_tracked_lead_catchup_cruise_error_full, get_tracked_lead_catchup_headway_margins, ) from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N @@ -578,6 +583,10 @@ class LongitudinalPlanner: self.radar_standstill_gap_settle_elapsed = 0.0 self.radar_standstill_gap_settle_active = False self.tracked_lead_catchup_bias_gain = get_tracked_lead_catchup_bias_gain(CP) + self.tracked_lead_catchup_bias_cap = get_tracked_lead_catchup_bias_cap(CP) + self.tracked_lead_catchup_speed_range = get_tracked_lead_catchup_speed_range(CP) + self.tracked_lead_catchup_fade_margins = get_tracked_lead_catchup_fade_margins(CP) + self.tracked_lead_catchup_cruise_error_full = get_tracked_lead_catchup_cruise_error_full(CP) self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) @@ -2293,7 +2302,11 @@ class LongitudinalPlanner: modelV2=sm['modelV2'], lead_obstacle_bias=stopped_lead_obstacle_bias, tracked_lead_catchup_headway_margins=self.tracked_lead_catchup_headway_margins, - tracked_lead_catchup_bias_gain=self.tracked_lead_catchup_bias_gain) + tracked_lead_catchup_bias_gain=self.tracked_lead_catchup_bias_gain, + tracked_lead_catchup_bias_cap=self.tracked_lead_catchup_bias_cap, + tracked_lead_catchup_speed_range=self.tracked_lead_catchup_speed_range, + tracked_lead_catchup_fade_margins=self.tracked_lead_catchup_fade_margins, + tracked_lead_catchup_cruise_error_full=self.tracked_lead_catchup_cruise_error_full) self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) @@ -3060,7 +3073,14 @@ class LongitudinalPlanner: sm['starpilotPlan'].vCruise <= FORCE_STOP_HANDOFF_MAX_VCRUISE ) ) - longitudinalPlan.shouldStop = bool(self.output_should_stop) or force_stop_handoff + stop_sign_hold_speed = get_stop_sign_low_speed_hold(self.CP) + stop_sign_low_speed_hold = bool( + stop_sign_hold_speed is not None and + bool(getattr(sm['starpilotPlan'], 'stopSignConfirmed', False)) and + float(getattr(sm['carState'], 'vEgo', 0.0)) <= stop_sign_hold_speed and + not bool(getattr(sm['carState'], 'gasPressed', False)) + ) + longitudinalPlan.shouldStop = bool(self.output_should_stop) or force_stop_handoff or stop_sign_low_speed_hold longitudinalPlan.allowBrake = True longitudinalPlan.allowThrottle = bool(self.allow_throttle) diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index e626faa40..2d3fd418f 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -22,6 +22,7 @@ HONDA_ACCORD_STOP_GO_ACCEL_RISE_RATE = 4.0 HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE = 1.25 GENESIS_GV70_ELECTRIFIED_LEAD_FOLLOW_JERK_SCALE = 1.35 FORD_LIGHTNING_LEAD_FOLLOW_JERK_SCALE = 1.20 +HONDA_CRV_5G_LEAD_FOLLOW_JERK_SCALE = 1.20 GM_SILVERADO_EARLY_FOLLOW_MIN_EGO_SPEED = 18.0 GM_SILVERADO_EARLY_FOLLOW_MAX_DISTANCE = 130.0 GM_SILVERADO_EARLY_FOLLOW_MIN_MODEL_PROB = 0.85 @@ -32,6 +33,13 @@ FORD_LIGHTNING_FOLLOW_PREBRAKE_MIN_HEADWAY = 1.0 FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_MIN_HEADWAY_MARGIN = 0.10 FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_FULL_HEADWAY_MARGIN = 0.25 FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_BIAS_GAIN = 0.65 +HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_MIN_HEADWAY_MARGIN = 0.10 +HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_FULL_HEADWAY_MARGIN = 0.35 +HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_BIAS_GAIN = 1.25 +HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_BIAS_CAP = 65.0 +HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_SPEED_RANGE = (10.0, 18.0) +HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_FADE_MARGINS = (0.75, 6.5) +HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_CRUISE_ERROR_FULL = 0.75 FORD_LIGHTNING_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.5 FORD_LIGHTNING_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.75 FORD_LIGHTNING_STANDSTILL_GUARD_DISTANCE_MARGIN = 5.0 @@ -106,6 +114,7 @@ TOYOTA_CAMRY_TSS2_FORCE_STOP_DISTANCE_BIAS_M = 6.0 DEFAULT_FORCE_STOP_HANDOFF_M = 6.0 HYUNDAI_SANTA_FE_2022_FORCE_STOP_REANCHOR_SPEED_TOLERANCE = 0.25 HYUNDAI_SANTA_FE_2022_FORCE_STOP_LOW_SPEED_HOLD = 2.5 +KIA_CARNIVAL_2025_STOP_SIGN_LOW_SPEED_HOLD = 0.75 def get_toyota_prius_stopped_lead_obstacle_bias(CP, lead, v_ego): @@ -278,6 +287,11 @@ def get_standstill_stopped_lead_guard_max_lead_speed(CP, default): def get_tracked_lead_catchup_headway_margins(CP): + if is_honda_crv_5g(CP): + return ( + HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_MIN_HEADWAY_MARGIN, + HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_FULL_HEADWAY_MARGIN, + ) if is_ford_f150_lightning(CP): return ( FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_MIN_HEADWAY_MARGIN, @@ -287,11 +301,37 @@ def get_tracked_lead_catchup_headway_margins(CP): def get_tracked_lead_catchup_bias_gain(CP): + if is_honda_crv_5g(CP): + return HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_BIAS_GAIN if is_ford_f150_lightning(CP): return FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_BIAS_GAIN return None +def get_tracked_lead_catchup_bias_cap(CP): + if is_honda_crv_5g(CP): + return HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_BIAS_CAP + return None + + +def get_tracked_lead_catchup_speed_range(CP): + if is_honda_crv_5g(CP): + return HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_SPEED_RANGE + return None + + +def get_tracked_lead_catchup_fade_margins(CP): + if is_honda_crv_5g(CP): + return HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_FADE_MARGINS + return None + + +def get_tracked_lead_catchup_cruise_error_full(CP): + if is_honda_crv_5g(CP): + return HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_CRUISE_ERROR_FULL + return None + + def is_ford_f150_lightning(CP): return ( getattr(CP, "brand", "") == "ford" and @@ -436,6 +476,8 @@ def get_lead_follow_jerk_scale(CP): str(getattr(CP, "carFingerprint", "")) == "GENESIS_GV70_ELECTRIFIED_1ST_GEN" ): return GENESIS_GV70_ELECTRIFIED_LEAD_FOLLOW_JERK_SCALE + if is_honda_crv_5g(CP): + return HONDA_CRV_5G_LEAD_FOLLOW_JERK_SCALE if is_ford_f150_lightning(CP): return FORD_LIGHTNING_LEAD_FOLLOW_JERK_SCALE return 1.0 @@ -577,3 +619,10 @@ def get_force_stop_low_speed_hold(car_params): if str(getattr(car_params, "carFingerprint", car_params)) == "HYUNDAI_SANTA_FE_2022": return HYUNDAI_SANTA_FE_2022_FORCE_STOP_LOW_SPEED_HOLD return None + + +def get_stop_sign_low_speed_hold(car_params): + """Keep a confirmed Carnival stop latched through the final low-speed handoff.""" + if str(getattr(car_params, "carFingerprint", car_params)) == "KIA_CARNIVAL_2025": + return KIA_CARNIVAL_2025_STOP_SIGN_LOW_SPEED_HOLD + return None diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 52a9afc4b..a0d7f70fb 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -86,6 +86,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_genesis_g90_friction_threshold, get_genesis_g70_center_output_scale, get_genesis_g70_curve_unwind_output_scale, + get_genesis_g70_angle_output_scale, get_genesis_g70_friction_jerk_deadzone, get_genesis_g70_friction_threshold, get_genesis_g70_high_speed_error_scale, @@ -967,6 +968,8 @@ class TestLatControl: assert get_genesis_g70_low_speed_angle_damping(0.0, 20.0, 0.0, 2.0) > 0.0 assert get_genesis_g70_curve_unwind_output_scale(0.7, -0.5, 25.0) == pytest.approx(1.0) assert get_genesis_g70_curve_unwind_output_scale(0.7, 0.5, 25.0) == 1.0 + assert get_genesis_g70_angle_output_scale(55.0, 1.0) > get_genesis_g70_angle_output_scale(85.0, 1.0) + assert get_genesis_g70_angle_output_scale(85.0, -1.0) == pytest.approx(1.0) assert get_genesis_g70_friction_jerk_deadzone(25.0, 0.0) > 0.25 assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, 0.5, 25.0) < 0.90 assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, -0.5, 25.0) == 1.0 @@ -1147,6 +1150,30 @@ class TestLatControl: assert lac_log.active assert tapered_output == pytest.approx(base_output * 0.5) + def test_genesis_g70_angle_output_taper_update_path(self, monkeypatch): + monkeypatch.setattr(latcontrol_torque, "get_genesis_g70_angle_output_scale", lambda *_args: 1.0) + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.GENESIS_G70_2020) + CS.vEgo = 15.0 + CS.steeringAngleDeg = 85.0 + base_output, _, lac_log = controller.update( + True, CS, VM, params, False, 0.004, False, 0.2, None, None, starpilot_toggles, + ) + + monkeypatch.setattr(latcontrol_torque, "get_genesis_g70_angle_output_scale", lambda *_args: 0.5) + tapered_controller, tapered_VM, tapered_CS, tapered_params, tapered_toggles = self._build_torque_controller( + HYUNDAI.GENESIS_G70_2020, + ) + tapered_CS.vEgo = 15.0 + tapered_CS.steeringAngleDeg = 85.0 + tapered_output, _, _ = tapered_controller.update( + True, tapered_CS, tapered_VM, tapered_params, False, 0.004, False, 0.2, None, None, tapered_toggles, + ) + + assert controller.is_genesis_g70 + assert lac_log.active + assert base_output != 0.0 + assert tapered_output == pytest.approx(base_output * 0.5) + def test_ram_1500_transition_taper_curve(self): assert get_ram_1500_transition_output_scale(0.4, 0.2, 17.0) == pytest.approx(1.0) assert get_ram_1500_transition_output_scale(0.4, 1.1, 8.0) == pytest.approx(1.0) diff --git a/selfdrive/controls/tests/test_lead_behavior.py b/selfdrive/controls/tests/test_lead_behavior.py index 4070fbb2d..4c6796400 100644 --- a/selfdrive/controls/tests/test_lead_behavior.py +++ b/selfdrive/controls/tests/test_lead_behavior.py @@ -41,6 +41,26 @@ def test_tracked_lead_catchup_bias_corrects_lightning_aggressive_follow_bookmark assert bias > 2.0 +def test_tracked_lead_catchup_bias_handles_crv_hanging_gap(): + tuned_bias = get_tracked_lead_catchup_bias( + 19.8, + 85.0, + 43.636, + 1.7, + v_cruise=20.44, + y_rel=-0.54, + min_headway_margin=0.10, + full_headway_margin=0.35, + bias_gain=1.25, + bias_cap=65.0, + speed_range=(10.0, 18.0), + fade_margins=(0.75, 6.5), + cruise_error_full=0.75, + ) + + assert tuned_bias > 40.0 + + def test_tracked_lead_catchup_bias_ignores_very_far_gap(): bias = get_tracked_lead_catchup_bias(31.4, 110.0, 38.0, 0.1) assert bias == 0.0 diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 0a6241078..c78077062 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -39,8 +39,13 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_standstill_gap_settle_max_extra_gap, get_standstill_stopped_lead_guard_distance_margin, get_standstill_stopped_lead_guard_max_lead_speed, + get_stop_sign_low_speed_hold, + get_tracked_lead_catchup_bias_cap, get_tracked_lead_catchup_bias_gain, + get_tracked_lead_catchup_cruise_error_full, + get_tracked_lead_catchup_fade_margins, get_tracked_lead_catchup_headway_margins, + get_tracked_lead_catchup_speed_range, get_toyota_prius_stopped_lead_obstacle_bias, get_toyota_rav4_tss2_lead_departure_tune, get_toyota_rav4_tss2_early_lead_cap, @@ -801,6 +806,20 @@ def test_lightning_stopped_lead_guard_tune_is_vehicle_specific(): assert get_tracked_lead_catchup_bias_gain(civic) is None +def test_crv_tracked_lead_catchup_tune_is_vehicle_specific(): + crv = CarInterface.get_non_essential_params(CAR.HONDA_CRV_5G) + civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + + assert get_tracked_lead_catchup_headway_margins(crv) == pytest.approx((0.10, 0.35)) + assert get_tracked_lead_catchup_bias_gain(crv) == pytest.approx(1.25) + assert get_tracked_lead_catchup_bias_cap(crv) == pytest.approx(65.0) + assert get_tracked_lead_catchup_speed_range(crv) == pytest.approx((10.0, 18.0)) + assert get_tracked_lead_catchup_fade_margins(crv) == pytest.approx((0.75, 6.5)) + assert get_tracked_lead_catchup_cruise_error_full(crv) == pytest.approx(0.75) + assert get_tracked_lead_catchup_bias_cap(civic) is None + assert get_tracked_lead_catchup_speed_range(civic) is None + + def test_lightning_far_follow_output_slew_damps_nonurgent_lead_braking(): v_ego = 24.0 CP = FordCarInterface.get_non_essential_params(FORD_CAR.FORD_F_150_LIGHTNING_MK1) @@ -3408,6 +3427,42 @@ def test_publish_force_stop_handoff_sets_should_stop_when_vcruise_low(): assert pm.sent["longitudinalPlan"].longitudinalPlan.shouldStop +def test_carnival_confirmed_stop_sign_stays_latched_at_low_speed(): + 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)} + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + CP.carFingerprint = "KIA_CARNIVAL_2025" + planner = LongitudinalPlanner(CP, init_v=0.4) + planner.output_a_target = -0.2 + 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 + + sm = FakeSM(make_sm(0.4, desired_accel=0.0, min_accel=-1.0, experimental_mode=False)) + sm["starpilotPlan"].stopSignConfirmed = True + pm = FakePM() + + assert get_stop_sign_low_speed_hold(CP) == pytest.approx(0.75) + planner.publish(sm, pm) + + assert pm.sent["longitudinalPlan"].longitudinalPlan.shouldStop + + def test_allow_throttle_hysteresis_filters_gas_prob_chatter(): v_ego = 10.0 diff --git a/selfdrive/controls/tests/test_starpilot_planner.py b/selfdrive/controls/tests/test_starpilot_planner.py index f27727c57..8ebe49422 100644 --- a/selfdrive/controls/tests/test_starpilot_planner.py +++ b/selfdrive/controls/tests/test_starpilot_planner.py @@ -44,6 +44,7 @@ def test_lead_follow_jerk_scale_is_platform_specific(): assert get_lead_follow_jerk_scale(SimpleNamespace(brand="hyundai", carFingerprint="HYUNDAI_ELANTRA_2021")) == 1.25 assert get_lead_follow_jerk_scale(SimpleNamespace(brand="hyundai", carFingerprint="GENESIS_GV70_ELECTRIFIED_1ST_GEN")) == 1.35 assert get_lead_follow_jerk_scale(SimpleNamespace(brand="ford", carFingerprint="FORD_F_150_LIGHTNING_MK1")) == 1.20 + assert get_lead_follow_jerk_scale(SimpleNamespace(brand="honda", carFingerprint="HONDA_CRV_5G")) == 1.20 assert get_lead_follow_jerk_scale(SimpleNamespace(brand="other", carFingerprint="OTHER_CAR")) == 1.0 diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index b97571850..f8a737bbc 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -5,7 +5,7 @@ import pytest from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME -from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController +from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController, MIN_TRAINING_TIME from openpilot.starpilot.controls.lib.starpilot_vcruise import ( FORCE_STOP_CAP_SLACK_M, FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME, @@ -194,6 +194,22 @@ def test_curve_speed_controller_releases_immediately_when_disabled(): assert not vcruise.csc_controlling_speed +def test_curve_speed_controller_does_not_compete_with_force_stop(): + planner, vcruise = make_vcruise(red_light=True, road_curvature=0.02) + sm = make_sm(standstill=False) + toggles = make_toggles() + toggles.curve_speed_controller = True + planner.road_curvature_detected = True + vcruise.csc.target_set = True + vcruise.csc.target = 12.0 + + update_vcruise(vcruise, sm, toggles, now=25.0, v_ego=20.0) + + assert not vcruise.csc_controlling_speed + assert not vcruise.csc.target_set + + + def test_curve_speed_controller_can_be_limited_to_driving_without_a_lead(): planner, vcruise = make_vcruise() sm = make_sm(standstill=False) @@ -251,7 +267,7 @@ def test_curve_speed_controller_learns_when_speed_is_manually_controlled(long_ac planner.driving_in_curve = True planner.road_curvature_detected = True planner.lateral_acceleration = 2.4 - vcruise.csc.training_timer = PLANNER_TIME + vcruise.csc.training_timer = MIN_TRAINING_TIME update_vcruise(vcruise, sm, toggles, now=50.0, v_ego=20.0) @@ -269,7 +285,7 @@ def test_curve_speed_controller_learns_when_longitudinal_override_event_is_activ planner.driving_in_curve = True planner.road_curvature_detected = True planner.lateral_acceleration = 2.4 - vcruise.csc.training_timer = PLANNER_TIME + vcruise.csc.training_timer = MIN_TRAINING_TIME update_vcruise(vcruise, sm, toggles, now=50.0, v_ego=20.0) @@ -283,7 +299,7 @@ def test_curve_speed_controller_persists_data_after_leaving_curve(): sm["carControl"].longActive = False planner.driving_in_curve = True planner.lateral_acceleration = 2.4 - vcruise.csc.training_timer = PLANNER_TIME + vcruise.csc.training_timer = MIN_TRAINING_TIME vcruise.csc.log_data(20.0, sm) assert not any(key == "CurvatureData" for key, _ in planner.params.writes) @@ -300,7 +316,7 @@ def test_curve_speed_controller_publishes_live_values_to_memory_params(): sm["carControl"].longActive = False planner.driving_in_curve = True planner.lateral_acceleration = 2.4 - vcruise.csc.training_timer = PLANNER_TIME + vcruise.csc.training_timer = MIN_TRAINING_TIME vcruise.csc.log_data(20.0, sm) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 7a5dc1a6b..ca4c4e797 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -71,7 +71,7 @@ def _model_smooth_seconds(params, key, default): def _should_publish_model_output(model_output, vipc_dropped_frames: int, external_gpu_active: bool = False) -> bool: - return model_output is not None and (external_gpu_active or vipc_dropped_frames == 0) + return model_output is not None and vipc_dropped_frames == 0 MIN_LAT_CONTROL_SPEED = 0.3 @@ -1011,7 +1011,7 @@ def main(demo=False): mt2 = time.perf_counter() model_execution_time = mt2 - mt1 - if model_output is not None and vipc_dropped_frames > 0 and not external_gpu_active: + if model_output is not None and vipc_dropped_frames > 0: cloudlog.error(f"suppressing model output after dropping {vipc_dropped_frames} frames") if _should_publish_model_output(model_output, vipc_dropped_frames, external_gpu_active): diff --git a/selfdrive/modeld/tests/test_model_fallback.py b/selfdrive/modeld/tests/test_model_fallback.py index a1b2e8280..7cf1e8726 100644 --- a/selfdrive/modeld/tests/test_model_fallback.py +++ b/selfdrive/modeld/tests/test_model_fallback.py @@ -15,12 +15,13 @@ class FakeParams: (object(), 0, False, True), (object(), 1, False, False), (object(), 2, False, False), - (object(), 1, True, True), - (object(), 2, True, True), + (object(), 0, True, True), + (object(), 1, True, False), + (object(), 2, True, False), (None, 0, False, False), (None, 1, True, False), ]) -def test_model_output_is_suppressed_after_vipc_drop(model_output, dropped_frames, external_gpu_active, expected): +def test_model_output_is_suppressed_after_vipc_drop_for_both_runtimes(model_output, dropped_frames, external_gpu_active, expected): assert modeld._should_publish_model_output(model_output, dropped_frames, external_gpu_active) is expected diff --git a/starpilot/common/assets/device_settings_layout.json b/starpilot/common/assets/device_settings_layout.json index 8aa5cd07d..10a73b6ee 100644 --- a/starpilot/common/assets/device_settings_layout.json +++ b/starpilot/common/assets/device_settings_layout.json @@ -3406,8 +3406,8 @@ { "key": "SubaruStopStartOff", "label": "Stop/Start Off at Startup", - "description": "For Subaru Outback 2023-24, send one momentary Stop/Start request after ignition while the vehicle is stationary and in Park or Neutral.", - "picker_description": "Requests Stop/Start OFF once after ignition on supported Outbacks.", + "description": "For supported Subaru Outback 2023-24 and Legacy 2025 vehicles, send one momentary Stop/Start request after ignition while stationary and in Park or Neutral.", + "picker_description": "Requests Stop/Start OFF once after ignition on supported Subarus.", "data_type": "bool", "ui_type": "toggle", "galaxy_only": true, diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index eea603ee6..da62b845c 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -20,7 +20,7 @@ from opendbc.car.gm.values import CAR as GM_CAR, EV_CAR as GM_EV_CAR, GMFlags from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, EV_CAR as HYUNDAI_EV_CAR, HyundaiFlags, HyundaiStarPilotSafetyFlags from opendbc.car.interfaces import TORQUE_SUBSTITUTE_PATH, CarInterfaceBase, GearShifter from opendbc.car.mock.values import CAR as MOCK -from opendbc.car.subaru.values import CAR as SUBARU_CAR, SubaruFlags +from opendbc.car.subaru.values import SUBARU_STOP_START_CARS, SubaruFlags from opendbc.car.tesla.values import CAR as TESLA_CAR from opendbc.car.toyota.values import CAR as TOYOTA_CAR, ToyotaStarPilotFlags from openpilot.common.basedir import BASEDIR @@ -1488,7 +1488,7 @@ class StarPilotVariables: not (CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID | SubaruFlags.LKAS_ANGLE))) toggle.subaru_sng_manual_parking_brake = self.get_value("SubaruSNGManualParkingBrake", condition=toggle.subaru_sng) toggle.subaru_stop_start_off = self.get_value( - "SubaruStopStartOff", condition=toggle.car_model == SUBARU_CAR.SUBARU_OUTBACK_2023, + "SubaruStopStartOff", condition=toggle.car_model in SUBARU_STOP_START_CARS, ) toggle.jeep_brake_hold = self.get_value( diff --git a/starpilot/controls/lib/curve_speed_controller.py b/starpilot/controls/lib/curve_speed_controller.py index ad8bb2e1a..e1bce48ae 100644 --- a/starpilot/controls/lib/curve_speed_controller.py +++ b/starpilot/controls/lib/curve_speed_controller.py @@ -7,6 +7,7 @@ from openpilot.common.realtime import DT_MDL from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, DEFAULT_LATERAL_ACCELERATION, PLANNER_TIME CALIBRATION_PROGRESS_THRESHOLD = 10 / DT_MDL +MIN_TRAINING_TIME = 5.0 CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS CSC_MAX_DECEL_RATE = 1.5 MAX_CURVATURE = 0.1 @@ -150,9 +151,8 @@ class CurveSpeedController: self.persistence_timer += DT_MDL in_curve = ( - self.training_timer >= PLANNER_TIME and - self.starpilot_planner.driving_in_curve and - not (sm["carState"].leftBlinker or sm["carState"].rightBlinker) + self.training_timer >= MIN_TRAINING_TIME and + self.starpilot_planner.driving_in_curve ) if in_curve: lateral_acceleration = abs(self.starpilot_planner.lateral_acceleration) diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 283830b84..d77180a1c 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -563,6 +563,8 @@ class StarPilotVCruise: following_lead = bool(getattr(self.starpilot_planner.starpilot_following, "following_lead", False)) manual_speed_control = is_manual_speed_control(sm) csc_available = ( + not force_stop_active and + not self.forcing_stop and long_control_active and not manual_speed_control and v_ego > CRUISING_SPEED and diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index d647f24f9..8d865cca3 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -304,7 +304,6 @@ class StarPilotCard: elif self.gap_counter == self.long_press_threshold: self.handle_button_event("distance_long", sm, starpilot_toggles) elif self.gap_counter == self.very_long_press_threshold: - self.handle_button_event("distance_long", sm, starpilot_toggles) self.handle_button_event("distance_very_long", sm, starpilot_toggles) if cancel_pressed: @@ -322,8 +321,7 @@ class StarPilotCard: elif self.cancel_counter == self.long_press_threshold: pulse_glide_cancel_consumed = self.handle_button_event("cancel_long", sm, starpilot_toggles) or False elif self.cancel_counter == self.very_long_press_threshold: - pulse_glide_cancel_consumed = self.handle_button_event("cancel_long", sm, starpilot_toggles) or False - pulse_glide_cancel_consumed |= self.handle_button_event("cancel_very_long", sm, starpilot_toggles) or False + pulse_glide_cancel_consumed = self.handle_button_event("cancel_very_long", sm, starpilot_toggles) or False if pulse_glide_cancel_consumed: self.cancel_pulse_glide_suppressed = True @@ -353,7 +351,6 @@ class StarPilotCard: elif self.mode_counter == self.long_press_threshold: self.handle_button_event("mode_long", sm, starpilot_toggles) elif self.mode_counter == self.very_long_press_threshold: - self.handle_button_event("mode_long", sm, starpilot_toggles) self.handle_button_event("mode_very_long", sm, starpilot_toggles) if starpilotCarState.customPressed: @@ -367,7 +364,6 @@ class StarPilotCard: elif self.custom_counter == self.long_press_threshold: self.handle_button_event("star_long", sm, starpilot_toggles) elif self.custom_counter == self.very_long_press_threshold: - self.handle_button_event("star_long", sm, starpilot_toggles) self.handle_button_event("star_very_long", sm, starpilot_toggles) if not getattr(starpilot_toggles, "pulse_and_glide_available", False): diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index 2beda9663..022a8f39b 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -231,6 +231,38 @@ def test_pulse_and_glide_long_cancel_consumes_release_after_threshold(monkeypatc assert release.buttonEvents == [] +@pytest.mark.parametrize( + ("pressed_field", "long_key", "very_long_key"), + ( + ("distancePressed", "distance_long", "distance_very_long"), + ("cancelPressed", "cancel_long", "cancel_very_long"), + ("modePressed", "mode_long", "mode_very_long"), + ("customPressed", "star_long", "star_very_long"), + ), +) +def test_very_long_press_does_not_repeat_long_press_action(monkeypatch, tmp_path, pressed_field, long_key, very_long_key): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard(SimpleNamespace(brand="gm"), SimpleNamespace(alternativeExperience=0)) + sm = make_sm() + toggles = make_toggles(has_canfd_media_buttons=pressed_field in ("modePressed", "customPressed")) + starpilot_car_state = SimpleNamespace( + distancePressed=False, + cancelPressed=False, + modePressed=False, + customPressed=False, + ) + setattr(starpilot_car_state, pressed_field, True) + handled = [] + monkeypatch.setattr(card, "handle_button_event", lambda key, _sm, _toggles: handled.append(key) or False) + + for _ in range(card.very_long_press_threshold): + card.update(make_car_state(), starpilot_car_state, sm, toggles) + + assert handled == [long_key, very_long_key] + + def make_car_state(available=False, enabled=False, button_events=None, brake_pressed=False, gas_pressed=False): return SimpleNamespace( buttonEvents=button_events or [],