diff --git a/opendbc_repo/opendbc/can/dbc.cc b/opendbc_repo/opendbc/can/dbc.cc index 09c93b89..9b81a6ca 100644 --- a/opendbc_repo/opendbc/can/dbc.cc +++ b/opendbc_repo/opendbc/can/dbc.cc @@ -57,9 +57,9 @@ ChecksumState* get_checksum(const std::string& dbc_name) { s = new ChecksumState({8, -1, 7, -1, false, TOYOTA_CHECKSUM, &toyota_checksum}); } else if (startswith(dbc_name, "hyundai_canfd_generated")) { s = new ChecksumState({16, -1, 0, -1, true, HKG_CAN_FD_CHECKSUM, &hkg_can_fd_checksum}); - } else if (startswith(dbc_name, {"vw_mqb_2010", "vw_mqbevo", "vw_meb"})) { + } else if (startswith(dbc_name, {"vw_mqb", "vw_mqbevo", "vw_meb"})) { s = new ChecksumState({8, 4, 0, 0, true, VOLKSWAGEN_MQB_MEB_CHECKSUM, &volkswagen_mqb_meb_checksum}); - } else if (startswith(dbc_name, "vw_golf_mk4")) { + } else if (startswith(dbc_name, "vw_pq")) { s = new ChecksumState({8, 4, 0, -1, true, XOR_CHECKSUM, &xor_checksum}); } else if (startswith(dbc_name, "subaru_global_")) { s = new ChecksumState({8, -1, 0, -1, true, SUBARU_CHECKSUM, &subaru_checksum}); diff --git a/opendbc_repo/opendbc/can/libdbc.so b/opendbc_repo/opendbc/can/libdbc.so index 64c1be5c..6bb7f2d7 100755 Binary files a/opendbc_repo/opendbc/can/libdbc.so and b/opendbc_repo/opendbc/can/libdbc.so differ diff --git a/opendbc_repo/opendbc/can/tests/test_checksums.py b/opendbc_repo/opendbc/can/tests/test_checksums.py index b9a0a9ad..006bd12e 100644 --- a/opendbc_repo/opendbc/can/tests/test_checksums.py +++ b/opendbc_repo/opendbc/can/tests/test_checksums.py @@ -93,7 +93,7 @@ class TestCanChecksums: def verify_volkswagen_mqb_crc(self, subtests, msg_name: str, msg_addr: int, test_messages: list[bytes], counter_field: str = 'COUNTER'): """Test AUTOSAR E2E Profile 2 CRCs""" assert len(test_messages) == 16 # All counter values must be tested - self.verify_checksum(subtests, "vw_mqb_2010", msg_name, msg_addr, test_messages, counter_field=counter_field) + self.verify_checksum(subtests, "vw_mqb", msg_name, msg_addr, test_messages, counter_field=counter_field) def test_volkswagen_mqb_crc_lwi_01(self, subtests): self.verify_volkswagen_mqb_crc(subtests, "LWI_01", 0x86, [ diff --git a/opendbc_repo/opendbc/car/__init__.py b/opendbc_repo/opendbc/car/__init__.py index dde58dd8..b8308532 100644 --- a/opendbc_repo/opendbc/car/__init__.py +++ b/opendbc_repo/opendbc/car/__init__.py @@ -98,13 +98,16 @@ class Bus(StrEnum): ap_party = auto() -def apply_driver_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS): +def apply_driver_steer_torque_limits(apply_torque: int, apply_torque_last: int, driver_torque: float, LIMITS, steer_max: int = None): + # some safety modes utilize a dynamic max steer + if steer_max is None: + steer_max = LIMITS.STEER_MAX # limits due to driver torque - driver_max_torque = LIMITS.STEER_MAX + (LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER - driver_min_torque = -LIMITS.STEER_MAX + (-LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER - max_steer_allowed = max(min(LIMITS.STEER_MAX, driver_max_torque), 0) - min_steer_allowed = min(max(-LIMITS.STEER_MAX, driver_min_torque), 0) + driver_max_torque = steer_max + (LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER + driver_min_torque = -steer_max + (-LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER + max_steer_allowed = max(min(steer_max, driver_max_torque), 0) + min_steer_allowed = min(max(-steer_max, driver_min_torque), 0) apply_torque = np.clip(apply_torque, min_steer_allowed, max_steer_allowed) # slow rate if steer torque increases in magnitude diff --git a/opendbc_repo/opendbc/car/car.capnp b/opendbc_repo/opendbc/car/car.capnp index 28d1dcea..5c5a7d68 100644 --- a/opendbc_repo/opendbc/car/car.capnp +++ b/opendbc_repo/opendbc/car/car.capnp @@ -470,10 +470,11 @@ struct CarParams { minEnableSpeed @7 :Float32; minSteerSpeed @8 :Float32; + steerAtStandstill @77 :Bool; # is steering available at standstill? just check if it faults safetyConfigs @62 :List(SafetyConfig); alternativeExperience @65 :Int16; # panda flag for features like no disengage on gas - # Car docs fields + # Car docs fields, not used for control maxLateralAccel @68 :Float32; autoResumeSng @69 :Bool; # describes whether car can resume from a stop automatically diff --git a/opendbc_repo/opendbc/car/docs_definitions.py b/opendbc_repo/opendbc/car/docs_definitions.py index bf44cd3d..846c2c3d 100644 --- a/opendbc_repo/opendbc/car/docs_definitions.py +++ b/opendbc_repo/opendbc/car/docs_definitions.py @@ -75,7 +75,7 @@ class Mount(EnumBase): class Cable(EnumBase): - long_obdc_cable = BasePart("long OBD-C cable") + long_obdc_cable = BasePart("long OBD-C cable (9.5 ft)") usb_a_2_a_cable = BasePart("USB A-A cable") usbc_otg_cable = BasePart("USB C OTG cable") usbc_coupler = BasePart("USB-C coupler") @@ -85,12 +85,12 @@ class Cable(EnumBase): class Accessory(EnumBase): harness_box = BasePart("harness box") - comma_power_v2 = BasePart("comma power v2") + comma_power = BasePart("comma power v3") @dataclass class BaseCarHarness(BasePart): - parts: list[Enum] = field(default_factory=lambda: [Accessory.harness_box, Accessory.comma_power_v2]) + parts: list[Enum] = field(default_factory=lambda: [Accessory.harness_box, Accessory.comma_power]) has_connector: bool = True # without are hidden on the harness connector page @@ -108,7 +108,8 @@ class CarHarness(EnumBase): fca = BaseCarHarness("FCA connector") ram = BaseCarHarness("Ram connector") vw_a = BaseCarHarness("VW A connector") - vw_j533 = BaseCarHarness("VW J533 connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler, Accessory.comma_power_v2]) + vw_c = BaseCarHarness("VW C connector") + vw_j533 = BaseCarHarness("VW J533 connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler]) hyundai_a = BaseCarHarness("Hyundai A connector") hyundai_b = BaseCarHarness("Hyundai B connector") hyundai_c = BaseCarHarness("Hyundai C connector") @@ -128,17 +129,17 @@ class CarHarness(EnumBase): hyundai_q = BaseCarHarness("Hyundai Q connector") hyundai_r = BaseCarHarness("Hyundai R connector") custom = BaseCarHarness("Developer connector") - obd_ii = BaseCarHarness("OBD-II connector", parts=[Cable.long_obdc_cable], has_connector=False) + obd_ii = BaseCarHarness("OBD-II connector", parts=[Cable.long_obdc_cable, Cable.usbc_coupler], has_connector=False) gm = BaseCarHarness("GM connector", parts=[Accessory.harness_box]) - gmsdgm = BaseCarHarness("GM SDGM connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler, Accessory.comma_power_v2]) - nissan_a = BaseCarHarness("Nissan A connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler]) - nissan_b = BaseCarHarness("Nissan B connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler]) + gmsdgm = BaseCarHarness("GM SDGM connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler]) + nissan_a = BaseCarHarness("Nissan A connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler]) + nissan_b = BaseCarHarness("Nissan B connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler]) mazda = BaseCarHarness("Mazda connector") ford_q3 = BaseCarHarness("Ford Q3 connector") - ford_q4 = BaseCarHarness("Ford Q4 connector", parts=[Accessory.harness_box, Accessory.comma_power_v2, Cable.long_obdc_cable, Cable.usbc_coupler]) - rivian = BaseCarHarness("Rivian A connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler]) - tesla_a = BaseCarHarness("Tesla A connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler]) - tesla_b = BaseCarHarness("Tesla B connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler]) + ford_q4 = BaseCarHarness("Ford Q4 connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler]) + rivian = BaseCarHarness("Rivian A connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler]) + tesla_a = BaseCarHarness("Tesla A connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler]) + tesla_b = BaseCarHarness("Tesla B connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler]) class Device(EnumBase): diff --git a/opendbc_repo/opendbc/car/ford/interface.py b/opendbc_repo/opendbc/car/ford/interface.py index f1e1bfe2..6f08c10a 100644 --- a/opendbc_repo/opendbc/car/ford/interface.py +++ b/opendbc_repo/opendbc/car/ford/interface.py @@ -33,6 +33,7 @@ class CarInterface(CarInterfaceBase): ret.steerControlType = structs.CarParams.SteerControlType.angle ret.steerActuatorDelay = 0.2 ret.steerLimitTimer = 1.0 + ret.steerAtStandstill = True ret.longitudinalTuning.kiBP = [0.] ret.longitudinalTuning.kiV = [0.5] diff --git a/opendbc_repo/opendbc/car/fw_query_definitions.py b/opendbc_repo/opendbc/car/fw_query_definitions.py index 16e569b0..5c48fed6 100644 --- a/opendbc_repo/opendbc/car/fw_query_definitions.py +++ b/opendbc_repo/opendbc/car/fw_query_definitions.py @@ -56,6 +56,11 @@ class StdQueries: SUPPLIER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_VERSION_NUMBER) + MANUFACTURER_ECU_HARDWARE_NUMBER_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_HARDWARE_NUMBER) + MANUFACTURER_ECU_HARDWARE_NUMBER_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_HARDWARE_NUMBER) + UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index bbfd7056..ce5655c9 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -89,7 +89,7 @@ class CarController(CarControllerBase): self.apply_gas = self.params.INACTIVE_REGEN self.apply_brake = 0 else: - self.apply_gas = int(round(np.interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) + self.apply_gas = float(np.interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)) self.apply_brake = int(round(np.interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) # Don't allow any gas above inactive regen while stopping # FIXME: brakes aren't applied immediately when enabling at a stop diff --git a/opendbc_repo/opendbc/car/gm/gmcan.py b/opendbc_repo/opendbc/car/gm/gmcan.py index c9d1effc..cb74b609 100644 --- a/opendbc_repo/opendbc/car/gm/gmcan.py +++ b/opendbc_repo/opendbc/car/gm/gmcan.py @@ -56,16 +56,14 @@ def create_gas_regen_command(packer, bus, throttle, idx, enabled, at_full_stop): values = { "GasRegenCmdActive": enabled, "RollingCounter": idx, - "GasRegenCmdActiveInv": 1 - enabled, "GasRegenCmd": throttle, "GasRegenFullStopActive": at_full_stop, - "GasRegenAlwaysOne": 1, - "GasRegenAlwaysOne2": 1, - "GasRegenAlwaysOne3": 1, + "GasRegenAccType": 1, } dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[1] - values["GasRegenChecksum"] = (((0xff - dat[1]) & 0xff) << 16) | \ + values["GasRegenChecksum"] = ((1 - enabled) << 24) | \ + (((0xff - dat[1]) & 0xff) << 16) | \ (((0xff - dat[2]) & 0xff) << 8) | \ ((0x100 - dat[3] - idx) & 0xff) diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index 8059ab12..2f8c0979 100755 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -7,7 +7,7 @@ from opendbc.car.common.basedir import BASEDIR from opendbc.car.common.conversions import Conversions as CV from opendbc.car.gm.carcontroller import CarController from opendbc.car.gm.carstate import CarState -from opendbc.car.gm.radar_interface import RadarInterface, RADAR_HEADER_MSG +from opendbc.car.gm.radar_interface import RadarInterface, RADAR_HEADER_MSG, CAMERA_DATA_HEADER_MSG from opendbc.car.gm.values import CAR, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, SDGM_CAR, ALT_ACCS, CanBus, GMSafetyFlags from opendbc.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel @@ -127,7 +127,8 @@ class CarInterface(CarInterfaceBase): else: # ASCM, OBD-II harness ret.openpilotLongitudinalControl = True ret.networkLocation = NetworkLocation.gateway - ret.radarUnavailable = RADAR_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and not docs + # LRR messages can take up to a few seconds to start sending after ignition, check camera data as well which starts earlier + ret.radarUnavailable = RADAR_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and CAMERA_DATA_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and not docs ret.pcmCruise = False # stock non-adaptive cruise control is kept off # supports stop and go, but initial engage must (conservatively) be above 18mph ret.minEnableSpeed = 18 * CV.MPH_TO_MS diff --git a/opendbc_repo/opendbc/car/gm/radar_interface.py b/opendbc_repo/opendbc/car/gm/radar_interface.py index 2daffb85..3bbfc9ca 100755 --- a/opendbc_repo/opendbc/car/gm/radar_interface.py +++ b/opendbc_repo/opendbc/car/gm/radar_interface.py @@ -6,7 +6,8 @@ from opendbc.car.common.conversions import Conversions as CV from opendbc.car.gm.values import DBC, CanBus from opendbc.car.interfaces import RadarInterfaceBase -RADAR_HEADER_MSG = 1120 +RADAR_HEADER_MSG = 1120 # F_LRR_Obj_Header +CAMERA_DATA_HEADER_MSG = 1056 # F_Vision_Obj_Header SLOT_1_MSG = RADAR_HEADER_MSG + 1 NUM_SLOTS = 20 diff --git a/opendbc_repo/opendbc/car/gm/values.py b/opendbc_repo/opendbc/car/gm/values.py index b2b4e538..a32c3aa4 100644 --- a/opendbc_repo/opendbc/car/gm/values.py +++ b/opendbc_repo/opendbc/car/gm/values.py @@ -34,27 +34,26 @@ class CarControllerParams: def __init__(self, CP): # Gas/brake lookups - self.ZERO_GAS = 2048 # Coasting self.MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen if CP.carFingerprint in (CAMERA_ACC_CAR | SDGM_CAR): - self.MAX_GAS = 3400 - self.MAX_ACC_REGEN = 1514 - self.INACTIVE_REGEN = 1554 + self.MAX_GAS = 1346.0 + self.MAX_ACC_REGEN = -540.0 + self.INACTIVE_REGEN = -500.0 # Camera ACC vehicles have no regen while enabled. - # Camera transitions to MAX_ACC_REGEN from ZERO_GAS and uses friction brakes instantly + # Camera transitions to MAX_ACC_REGEN from zero gas and uses friction brakes instantly max_regen_acceleration = 0. else: - self.MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill. - self.MAX_ACC_REGEN = 1404 # Max ACC regen is slightly less than max paddle regen - self.INACTIVE_REGEN = 1404 + self.MAX_GAS = 1018.0 # Safety limit, not ACC max. Stock ACC >2042 from standstill. + self.MAX_ACC_REGEN = -650.0 # Max ACC regen is slightly less than max paddle regen + self.INACTIVE_REGEN = -650.0 # ICE has much less engine braking force compared to regen in EVs, # lower threshold removes some braking deadzone max_regen_acceleration = -1. if CP.carFingerprint in EV_CAR else -0.1 self.GAS_LOOKUP_BP = [max_regen_acceleration, 0., self.ACCEL_MAX] - self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS] + self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, 0., self.MAX_GAS] self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, max_regen_acceleration] self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.] diff --git a/opendbc_repo/opendbc/car/honda/values.py b/opendbc_repo/opendbc/car/honda/values.py index 9233f750..04ffb3f8 100644 --- a/opendbc_repo/opendbc/car/honda/values.py +++ b/opendbc_repo/opendbc/car/honda/values.py @@ -255,7 +255,7 @@ class CAR(Platforms): HONDA_PILOT = HondaNidecPlatformConfig( [ HondaCarDocs("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS), - HondaCarDocs("Honda Passport 2019-23", "All", min_steer_speed=12. * CV.MPH_TO_MS), + HondaCarDocs("Honda Passport 2019-25", "All", min_steer_speed=12. * CV.MPH_TO_MS), ], CarSpecs(mass=4278 * CV.LB_TO_KG, wheelbase=2.86, centerToFrontRatio=0.428, steerRatio=16.0, tireStiffnessFactor=0.444), # as spec radar_dbc_dict('acura_ilx_2016_can_generated'), diff --git a/opendbc_repo/opendbc/car/hyundai/fingerprints.py b/opendbc_repo/opendbc/car/hyundai/fingerprints.py index 9d7c7831..9812bdde 100644 --- a/opendbc_repo/opendbc/car/hyundai/fingerprints.py +++ b/opendbc_repo/opendbc/car/hyundai/fingerprints.py @@ -53,11 +53,13 @@ FW_VERSIONS = { b'\xf1\x00AE MDPS C 1.00 1.05 56310/G2501 4AEHC105', b'\xf1\x00AE MDPS C 1.00 1.07 56310/G2301 4AEHC107', b'\xf1\x00AE MDPS C 1.00 1.07 56310/G2501 4AEHC107', + b'\xf1\x00AE MDPS C 1.00 1.07 56310/G2551 4AEHC107', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00AEH MFC AT EUR LHD 1.00 1.00 95740-G2400 180222', b'\xf1\x00AEH MFC AT EUR LHD 1.00 1.00 95740-G7200 160418', b'\xf1\x00AEH MFC AT USA LHD 1.00 1.00 95740-G2400 180222', + b'\xf1\x00AEH MFC AT EUR RHD 1.00 1.00 95740-G2400 180222', ], }, CAR.HYUNDAI_IONIQ_PHEV_2019: { @@ -728,9 +730,11 @@ FW_VERSIONS = { }, CAR.KIA_NIRO_EV_2ND_GEN: { (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00SG__ RDR ----- 1.00 1.00 99110-AT200 ', b'\xf1\x00SG2_ RDR ----- 1.00 1.01 99110-AT000 ', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00SG2EMFC AT EUR LHD 1.00 1.00 99211-AT200 240315', b'\xf1\x00SG2EMFC AT EUR LHD 1.01 1.09 99211-AT000 220801', b'\xf1\x00SG2EMFC AT USA LHD 1.01 1.09 99211-AT000 220801', ], @@ -1043,6 +1047,7 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00NX4 FR_CMR AT CAN LHD 1.00 1.00 99211-N9260 14Y', b'\xf1\x00NX4 FR_CMR AT CAN LHD 1.00 1.01 99211-N9100 14A', + b'\xf1\x00NX4 FR_CMR AT CAN LHD 1.00 1.00 99211-N9220 14K', b'\xf1\x00NX4 FR_CMR AT EUR LHD 1.00 1.00 99211-N9220 14K', b'\xf1\x00NX4 FR_CMR AT EUR LHD 1.00 2.02 99211-N9000 14E', b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9210 14G', diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 8772fb4a..f72abaf1 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -413,7 +413,7 @@ class CAR(Platforms): flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.EV, ) KIA_NIRO_EV_2ND_GEN = HyundaiCanFDPlatformConfig( - [HyundaiCarDocs("Kia Niro EV 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a]))], + [HyundaiCarDocs("Kia Niro EV 2023-24", "All", car_parts=CarParts.common([CarHarness.hyundai_a]))], KIA_NIRO_EV.specs, flags=HyundaiFlags.EV, ) diff --git a/opendbc_repo/opendbc/car/rivian/carcontroller.py b/opendbc_repo/opendbc/car/rivian/carcontroller.py index 88289582..c1d0d308 100644 --- a/opendbc_repo/opendbc/car/rivian/carcontroller.py +++ b/opendbc_repo/opendbc/car/rivian/carcontroller.py @@ -1,3 +1,4 @@ +import numpy as np from opendbc.can.packer import CANPacker from opendbc.car import Bus, apply_driver_steer_torque_limits from opendbc.car.interfaces import CarControllerBase @@ -18,21 +19,24 @@ class CarController(CarControllerBase): can_sends = [] apply_torque = 0 + steer_max = round(float(np.interp(CS.out.vEgoRaw, CarControllerParams.STEER_MAX_LOOKUP[0], + CarControllerParams.STEER_MAX_LOOKUP[1]))) if CC.latActive: - new_torque = int(round(CC.actuators.torque * CarControllerParams.STEER_MAX)) + new_torque = int(round(CC.actuators.torque * steer_max)) apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, - CS.out.steeringTorque, CarControllerParams) + CS.out.steeringTorque, CarControllerParams, steer_max) # send steering command self.apply_torque_last = apply_torque - can_sends.append(create_lka_steering(self.packer, CS.acm_lka_hba_cmd, apply_torque, CC.latActive)) + can_sends.append(create_lka_steering(self.packer, self.frame, CS.acm_lka_hba_cmd, apply_torque, CC.enabled, CC.latActive)) if self.frame % 5 == 0: can_sends.append(create_wheel_touch(self.packer, CS.sccm_wheel_touch, CC.enabled)) # Longitudinal control if self.CP.openpilotLongitudinalControl: - can_sends.append(create_longitudinal(self.packer, self.frame % 15, actuators.accel, CC.enabled)) + accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)) + can_sends.append(create_longitudinal(self.packer, self.frame, accel, CC.enabled)) else: interface_status = None if CC.cruiseControl.cancel: @@ -46,7 +50,7 @@ class CarController(CarControllerBase): can_sends.append(create_adas_status(self.packer, CS.vdm_adas_status, interface_status)) new_actuators = actuators.as_builder() - new_actuators.torque = apply_torque / CarControllerParams.STEER_MAX + new_actuators.torque = apply_torque / steer_max new_actuators.torqueOutputCan = apply_torque self.frame += 1 diff --git a/opendbc_repo/opendbc/car/rivian/interface.py b/opendbc_repo/opendbc/car/rivian/interface.py index bc6f66e5..509889b5 100644 --- a/opendbc_repo/opendbc/car/rivian/interface.py +++ b/opendbc_repo/opendbc/car/rivian/interface.py @@ -3,6 +3,7 @@ from opendbc.car.interfaces import CarInterfaceBase from opendbc.car.rivian.carcontroller import CarController from opendbc.car.rivian.carstate import CarState from opendbc.car.rivian.radar_interface import RadarInterface +from opendbc.car.rivian.values import RivianSafetyFlags class CarInterface(CarInterfaceBase): @@ -16,7 +17,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.rivian)] - ret.steerActuatorDelay = 0.25 + ret.steerActuatorDelay = 0.15 ret.steerLimitTimer = 0.4 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) @@ -27,7 +28,7 @@ class CarInterface(CarInterfaceBase): ret.experimentalLongitudinalAvailable = False if experimental_long: ret.openpilotLongitudinalControl = True - #ret.safetyConfigs[0].safetyParam |= Panda.FLAG_RIVIAN_LONG_CONTROL + ret.safetyConfigs[0].safetyParam |= RivianSafetyFlags.LONG_CONTROL.value ret.longitudinalActuatorDelay = 0.35 ret.vEgoStopping = 0.25 diff --git a/opendbc_repo/opendbc/car/rivian/riviancan.py b/opendbc_repo/opendbc/car/rivian/riviancan.py index 9961306c..f22b5917 100644 --- a/opendbc_repo/opendbc/car/rivian/riviancan.py +++ b/opendbc_repo/opendbc/car/rivian/riviancan.py @@ -11,40 +11,32 @@ def checksum(data, poly, xor_output): return crc ^ xor_output -def create_lka_steering(packer, acm_lka_hba_cmd, apply_torque, enabled): - values = {s: acm_lka_hba_cmd[s] for s in [ - "ACM_lkaHbaCmd_Counter", - "ACM_lkaHbaCmd_Checksum", - "ACM_HapticRequest", - "ACM_lkaStrToqReq", - "ACM_lkaSymbolState", - "ACM_lkaToiFlt", - "ACM_lkaActToi", +def create_lka_steering(packer, frame, acm_lka_hba_cmd, apply_torque, enabled, active): + # forward auto high beam and speed limit status and nothing else + values = {s: acm_lka_hba_cmd[s] for s in ( "ACM_hbaSysState", - "ACM_FailinfoAeb", - "ACM_lkaRHWarning", - "ACM_lkaLHWarning", - "ACM_lkaLaneRecogState", - "ACM_hbaOpt", "ACM_hbaLamp", - "ACM_lkaHandsoffSoundWarning", - "ACM_lkaHandsoffDisplayWarning", - "ACM_unkown1", - "ACM_unkown2", - "ACM_unkown3", - "ACM_unkown4", - "ACM_unkown6", - ]} + "ACM_hbaOnOffState", + "ACM_slifOnOffState", + )} - if enabled: - values["ACM_lkaActToi"] = 1 - values["ACM_lkaSymbolState"] = 3 - values["ACM_lkaLaneRecogState"] = 3 - values["ACM_lkaStrToqReq"] = apply_torque - values["ACM_unkown2"] = 1 - values["ACM_unkown3"] = 4 - values["ACM_unkown4"] = 160 - values["ACM_unkown6"] = 1 + values |= { + "ACM_lkaHbaCmd_Counter": frame % 15, + "ACM_lkaStrToqReq": apply_torque, + "ACM_lkaActToi": active, + + "ACM_lkaLaneRecogState": 3 if enabled else 0, + "ACM_lkaSymbolState": 3 if enabled else 0, + + # static values + "ACM_lkaElkRequest": 0, + "ACM_ldwlkaOnOffState": 2, # 2=LKAS+LDW on + "ACM_elkOnOffState": 1, # 1=LKAS on + # TODO: what are these used for? + "ACM_ldwWarnTypeState": 2, # always 2 + "ACM_ldwWarnTimingState": 1, # always 1 + #"ACM_lkaHandsoffDisplayWarning": 1, # TODO: we can send this when openpilot wants you to pay attention + } data = packer.make_can_msg("ACM_lkaHbaCmd", 0, values)[1] values["ACM_lkaHbaCmd_Checksum"] = checksum(data[1:], 0x1D, 0x63) diff --git a/opendbc_repo/opendbc/car/rivian/values.py b/opendbc_repo/opendbc/car/rivian/values.py index c0fdb8bb..a488f324 100644 --- a/opendbc_repo/opendbc/car/rivian/values.py +++ b/opendbc_repo/opendbc/car/rivian/values.py @@ -1,9 +1,9 @@ from dataclasses import dataclass, field from enum import StrEnum, IntFlag -from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, structs +from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, structs, uds from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts, Device -from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries +from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 from opendbc.car.vin import Vin @@ -66,6 +66,10 @@ def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str return {str(c) for c in candidates} +RIVIAN_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(0xf1a0) +RIVIAN_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + FW_QUERY_CONFIG = FwQueryConfig( requests=[ Request( @@ -73,7 +77,21 @@ FW_QUERY_CONFIG = FwQueryConfig( [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.SUPPLIER_SOFTWARE_VERSION_RESPONSE], rx_offset=0x40, bus=0, - ) + ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_ECU_HARDWARE_NUMBER_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_ECU_HARDWARE_NUMBER_RESPONSE], + rx_offset=0x40, + bus=0, + logging=True, + ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, RIVIAN_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, RIVIAN_VERSION_RESPONSE], + rx_offset=0x40, + bus=0, + logging=True, + ), ], match_fw_to_car_fuzzy=match_fw_to_car_fuzzy, ) @@ -88,10 +106,16 @@ GEAR_MAP = { class CarControllerParams: - # The Rivian R1T we tested on achieves slightly more lateral acceleration going left vs. right - # and lateral acceleration rises as speed increases. This value is set conservatively to - # reach a maximum of 2.5-3.0 m/s^2 turning left at 80 mph, but is less at lower speeds - STEER_MAX = 250 # ~2.5 m/s^2 + # The R1T 2023 and R1S 2023 we tested on achieves slightly more lateral acceleration going left vs. right + # and lateral acceleration falls linearly as speed decreases from 38 mph to 20 mph. These values are set + # conservatively to reach a maximum of 3.0 m/s^2 turning left at 80 mph + + # These refer to turning left: + # 250 is ~2.8 m/s^2 above 17 m/s, then linearly ramps to ~1.6 m/s^2 from 17 m/s to 9 m/s + # TODO: it is theorized older models have different steering racks and achieve down to half the + # lateral acceleration referenced here at all speeds. detect this and ship a torque increase for those models + STEER_MAX = 250 # 350 is intended to maintain lateral accel, not increase it + STEER_MAX_LOOKUP = [9, 17], [350, 250] STEER_STEP = 1 STEER_DELTA_UP = 3 # torque increase per refresh STEER_DELTA_DOWN = 5 # torque decrease per refresh diff --git a/opendbc_repo/opendbc/car/subaru/fingerprints.py b/opendbc_repo/opendbc/car/subaru/fingerprints.py index 53f1e6f0..51c01e10 100644 --- a/opendbc_repo/opendbc/car/subaru/fingerprints.py +++ b/opendbc_repo/opendbc/car/subaru/fingerprints.py @@ -54,6 +54,7 @@ FW_VERSIONS = { (Ecu.abs, 0x7b0, None): [ b'\xa1 \x02\x01', b'\xa1 \x02\x02', + b'\xa1 \x03\x02', b'\xa1 \x03\x03', b'\xa1 \x04\x01', ], @@ -67,6 +68,7 @@ FW_VERSIONS = { ], (Ecu.engine, 0x7e0, None): [ b'\xde"a0\x07', + b'\xe2"a0\x07', b'\xde,\xa0@\x07', b'\xe2"aq\x07', b'\xe2,\xa0@\x07', @@ -463,6 +465,7 @@ FW_VERSIONS = { b'\xa1 \x06\x00', b'\xa1 \x06\x01', b'\xa1 \x06\x02', + b'\xa1 \x06\x03', b'\xa1 \x07\x00', b'\xa1 \x07\x02', b'\xa1 \x07\x03', @@ -496,6 +499,7 @@ FW_VERSIONS = { b'\xe2"`p\x07', b'\xe2"`q\x07', b'\xe3,\xa0@\x07', + b'\xe2,\xa0p\x07', ], (Ecu.transmission, 0x7e1, None): [ b'\xa5\xf6D@\x00', @@ -505,6 +509,7 @@ FW_VERSIONS = { b'\xa7\x8e\xf40\x00', b'\xa7\xf6D@\x00', b'\xa7\xfe\xf4@\x00', + b'\xa7\xfe\xf6@\x00', ], }, CAR.SUBARU_FORESTER_2022: { diff --git a/opendbc_repo/opendbc/car/tesla/carstate.py b/opendbc_repo/opendbc/car/tesla/carstate.py index a374090d..d476484a 100644 --- a/opendbc_repo/opendbc/car/tesla/carstate.py +++ b/opendbc_repo/opendbc/car/tesla/carstate.py @@ -71,8 +71,8 @@ class CarState(CarStateBase): ret.doorOpen = cp_party.vl["UI_warning"]["anyDoorOpen"] == 1 # Blinkers - ret.leftBlinker = cp_party.vl["UI_warning"]["leftBlinkerOn"] != 0 - ret.rightBlinker = cp_party.vl["UI_warning"]["rightBlinkerOn"] != 0 + ret.leftBlinker = cp_party.vl["UI_warning"]["leftBlinkerBlinking"] in (1, 2) + ret.rightBlinker = cp_party.vl["UI_warning"]["rightBlinkerBlinking"] in (1, 2) # Seatbelt ret.seatbeltUnlatched = cp_party.vl["UI_warning"]["buckleStatus"] != 1 @@ -84,6 +84,9 @@ class CarState(CarStateBase): # AEB ret.stockAeb = cp_ap_party.vl["DAS_control"]["DAS_aebEvent"] == 1 + # Stock Autosteer should be off (includes FSD) + ret.invalidLkasSetting = cp_ap_party.vl["DAS_settings"]["DAS_autosteerEnabled"] != 0 + # Buttons # ToDo: add Gap adjust button # Messages needed by carcontroller @@ -106,6 +109,7 @@ class CarState(CarStateBase): ap_party_messages = [ ("DAS_control", 25), ("DAS_status", 2), + ("DAS_settings", 2), ("SCCM_steeringAngleSensor", 100), ] diff --git a/opendbc_repo/opendbc/car/tesla/fingerprints.py b/opendbc_repo/opendbc/car/tesla/fingerprints.py index b39c3f63..ea48be52 100644 --- a/opendbc_repo/opendbc/car/tesla/fingerprints.py +++ b/opendbc_repo/opendbc/car/tesla/fingerprints.py @@ -8,6 +8,7 @@ FW_VERSIONS = { (Ecu.eps, 0x730, None): [ b'TeM3_E014p10_0.0.0 (16),E014.17.00', b'TeM3_E014p10_0.0.0 (16),EL014.17.00', + b'TeM3_ES014p11_0.0.0 (25),ES014.19.0', b'TeMYG4_DCS_Update_0.0.0 (13),E4014.28.1', b'TeMYG4_DCS_Update_0.0.0 (9),E4014.26.0', b'TeMYG4_Main_0.0.0 (59),E4H014.29.0', @@ -25,6 +26,7 @@ FW_VERSIONS = { b'TeMYG4_DCS_Update_0.0.0 (13),Y4P002.27.1', b'TeMYG4_DCS_Update_0.0.0 (9),Y4P002.25.0', b'TeMYG4_Legacy3Y_0.0.0 (2),Y4003.02.0', + b'TeMYG4_Legacy3Y_0.0.0 (5),Y4003.03.2', b'TeMYG4_Legacy3Y_0.0.0 (2),Y4P003.02.0', b'TeMYG4_SingleECU_0.0.0 (33),Y4S002.26', ], diff --git a/opendbc_repo/opendbc/car/tesla/interface.py b/opendbc_repo/opendbc/car/tesla/interface.py index 6b882806..1796bdba 100644 --- a/opendbc_repo/opendbc/car/tesla/interface.py +++ b/opendbc_repo/opendbc/car/tesla/interface.py @@ -12,12 +12,12 @@ class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experimental_long, docs) -> structs.CarParams: ret.brand = "tesla" - ret.dashcamOnly = True ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.tesla)] ret.steerLimitTimer = 1.0 ret.steerActuatorDelay = 0.1 + ret.steerAtStandstill = True ret.steerControlType = structs.CarParams.SteerControlType.angle ret.radarUnavailable = True diff --git a/opendbc_repo/opendbc/car/tests/routes.py b/opendbc_repo/opendbc/car/tests/routes.py index d582ec6e..85cea325 100644 --- a/opendbc_repo/opendbc/car/tests/routes.py +++ b/opendbc_repo/opendbc/car/tests/routes.py @@ -214,6 +214,7 @@ routes = [ CarTestRoute("2475fb3eb2ffcc2e|2022-04-29--12-46-23", TOYOTA.TOYOTA_RAV4_TSS2_2022), # hybrid CarTestRoute("20ba9ade056a8c7b|2021-02-08--21-57-35", TOYOTA.TOYOTA_RAV4_PRIME), # SecOC CarTestRoute("8bfb000e03b2a257/00000004--f9eee5f52e", TOYOTA.TOYOTA_SIENNA_4TH_GEN), # SecOC + CarTestRoute("0b54d0594d924cd9/00000057--b6206a3205", TOYOTA.TOYOTA_YARIS), # SecOC CarTestRoute("7a31f030957b9c85|2023-04-01--14-12-51", TOYOTA.LEXUS_ES), CarTestRoute("37041c500fd30100|2020-12-30--12-17-24", TOYOTA.LEXUS_ES), # hybrid CarTestRoute("e6a24be49a6cd46e|2019-10-29--10-52-42", TOYOTA.LEXUS_ES_TSS2), @@ -251,6 +252,7 @@ routes = [ CarTestRoute("ffcd23abbbd02219|2024-02-28--14-59-38", VOLKSWAGEN.VOLKSWAGEN_CADDY_MK3), CarTestRoute("cae14e88932eb364|2021-03-26--14-43-28", VOLKSWAGEN.VOLKSWAGEN_GOLF_MK7), # Stock ACC CarTestRoute("3cfdec54aa035f3f|2022-10-13--14-58-58", VOLKSWAGEN.VOLKSWAGEN_GOLF_MK7), # openpilot longitudinal + CarTestRoute("17fbdc1cd49dc9af/00000027--ee57555e5b", VOLKSWAGEN.VOLKSWAGEN_ID4_MK1), # FIXME: temporary, replace later CarTestRoute("578742b26807f756|00000010--41ee3e5bec", VOLKSWAGEN.VOLKSWAGEN_JETTA_MK6), CarTestRoute("58a7d3b707987d65|2021-03-25--17-26-37", VOLKSWAGEN.VOLKSWAGEN_JETTA_MK7), CarTestRoute("4d134e099430fba2|2021-03-26--00-26-06", VOLKSWAGEN.VOLKSWAGEN_PASSAT_MK8), diff --git a/opendbc_repo/opendbc/car/tests/test_fw_fingerprint.py b/opendbc_repo/opendbc/car/tests/test_fw_fingerprint.py index 417ce3d2..58d2ba65 100644 --- a/opendbc_repo/opendbc/car/tests/test_fw_fingerprint.py +++ b/opendbc_repo/opendbc/car/tests/test_fw_fingerprint.py @@ -260,7 +260,7 @@ class TestFwFingerprintTiming: print(f'get_vin {name} case, query time={self.total_time / self.N} seconds') def test_fw_query_timing(self, subtests, mocker): - total_ref_time = {1: 7.1, 2: 7.7} + total_ref_time = {1: 7.3, 2: 7.9} brand_ref_times = { 1: { 'gm': 1.0, @@ -275,7 +275,7 @@ class TestFwFingerprintTiming: 'tesla': 0.1, 'toyota': 0.7, 'volkswagen': 0.65, - 'rivian': 0.1, + 'rivian': 0.3, }, 2: { 'ford': 1.6, diff --git a/opendbc_repo/opendbc/car/torque_data/override.toml b/opendbc_repo/opendbc/car/torque_data/override.toml index 16e07c2a..c04e6c0a 100644 --- a/opendbc_repo/opendbc/car/torque_data/override.toml +++ b/opendbc_repo/opendbc/car/torque_data/override.toml @@ -48,6 +48,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "CHEVROLET_EQUINOX" = [2.5, 2.5, 0.05] "CHEVROLET_VOLT_2019" = [1.4, 1.4, 0.16] "VOLKSWAGEN_CADDY_MK3" = [1.2, 1.2, 0.1] +"VOLKSWAGEN_ID4_MK1" = [nan, 2.5, nan] "VOLKSWAGEN_PASSAT_NMS" = [2.5, 2.5, 0.1] "VOLKSWAGEN_SHARAN_MK2" = [2.5, 2.5, 0.1] "HYUNDAI_SANTA_CRUZ_1ST_GEN" = [2.7, 2.7, 0.1] @@ -73,6 +74,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "HYUNDAI_STARIA_4TH_GEN" = [1.8, 2.0, 0.15] "GENESIS_GV70_ELECTRIFIED_1ST_GEN" = [1.9, 1.9, 0.09] "GENESIS_G80_2ND_GEN_FL" = [2.5819356441497803, 2.5, 0.11244568973779678] +# Note that some Rivians achieve significantly less lateral acceleration than this "RIVIAN_R1_GEN1" = [2.8, 2.5, 0.07] "HYUNDAI_NEXO_1ST_GEN" = [2.5, 2.5, 0.1] diff --git a/opendbc_repo/opendbc/car/torque_data/params.toml b/opendbc_repo/opendbc/car/torque_data/params.toml index 4bb8d45c..fde2ca22 100644 --- a/opendbc_repo/opendbc/car/torque_data/params.toml +++ b/opendbc_repo/opendbc/car/torque_data/params.toml @@ -73,6 +73,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "TOYOTA_RAV4H" = [1.9796257271652042, 1.7503987331707576, 0.14628860048885406] "TOYOTA_RAV4_TSS2_2022" = [2.241883248393209, 1.9304407208090029, 0.112174] "TOYOTA_SIENNA" = [1.689726, 1.3208264576110418, 0.140456] +"TOYOTA_YARIS" = [2.22984, 1.86145, 0.168189] "VOLKSWAGEN_ARTEON_MK1" = [1.45136518053819, 1.3639364049316804, 0.23806361745695032] "VOLKSWAGEN_ATLAS_MK1" = [1.4677006726964945, 1.6733266634075656, 0.12959584092073367] "VOLKSWAGEN_GOLF_MK7" = [1.3750394140491293, 1.5814743077200641, 0.2018321939386586] diff --git a/opendbc_repo/opendbc/car/torque_data/substitute.toml b/opendbc_repo/opendbc/car/torque_data/substitute.toml index a91ac358..911dd4bb 100644 --- a/opendbc_repo/opendbc/car/torque_data/substitute.toml +++ b/opendbc_repo/opendbc/car/torque_data/substitute.toml @@ -59,6 +59,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "CHEVROLET_MALIBU" = "CHEVROLET_VOLT" "HOLDEN_ASTRA" = "CHEVROLET_VOLT" +"SKODA_ENYAQ_MK1" = "VOLKSWAGEN_ID4_MK1" "SKODA_FABIA_MK4" = "VOLKSWAGEN_GOLF_MK7" "SKODA_OCTAVIA_MK3" = "SKODA_SUPERB_MK3" "SKODA_KODIAQ_MK1" = "SKODA_SUPERB_MK3" diff --git a/opendbc_repo/opendbc/car/toyota/fingerprints.py b/opendbc_repo/opendbc/car/toyota/fingerprints.py index 3452c4cb..2b6e1b47 100644 --- a/opendbc_repo/opendbc/car/toyota/fingerprints.py +++ b/opendbc_repo/opendbc/car/toyota/fingerprints.py @@ -1783,4 +1783,21 @@ FW_VERSIONS = { b'\x028646FV201000\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', ], }, + CAR.TOYOTA_YARIS: { + (Ecu.engine, 0x700, None): [ + b'\x0189663K015300\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'\x018965BK003200\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'\x01F1526K007500\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F0D05300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F5205200\x00\x00\x00\x008646G5202200\x00\x00\x00\x00', + ], + }, } diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 4fa1eeea..1515ebd9 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -31,7 +31,6 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.ALT_BRAKE.value if ret.flags & ToyotaFlags.SECOC.value: - ret.dashcamOnly = True ret.secOcRequired = True ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.SECOC.value diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index 889b9d0c..5d43cf4e 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -271,7 +271,7 @@ class CAR(Platforms): ) TOYOTA_RAV4_TSS2_2023 = ToyotaTSS2PlatformConfig( [ - ToyotaCarDocs("Toyota RAV4 2023-24"), + ToyotaCarDocs("Toyota RAV4 2023-25"), ToyotaCarDocs("Toyota RAV4 Hybrid 2023-25", video_link="https://youtu.be/4eIsEq4L4Ng"), ], TOYOTA_RAV4_TSS2.specs, @@ -281,6 +281,11 @@ class CAR(Platforms): [ToyotaCarDocs("Toyota RAV4 Prime 2021-23", min_enable_speed=MIN_ACC_SPEED)], CarSpecs(mass=4372. * CV.LB_TO_KG, wheelbase=2.68, steerRatio=16.88, tireStiffnessFactor=0.5533), ) + TOYOTA_YARIS = ToyotaSecOCPlatformConfig( + [ToyotaCarDocs("Toyota Yaris 2023 (Non-US only)", min_enable_speed=MIN_ACC_SPEED)], + CarSpecs(mass=1170, wheelbase=2.55, steerRatio=14.80, tireStiffnessFactor=0.5533), + flags=ToyotaFlags.RADAR_ACC, + ) TOYOTA_MIRAI = ToyotaTSS2PlatformConfig( # TSS 2.5 [ToyotaCarDocs("Toyota Mirai 2021")], CarSpecs(mass=4300. * CV.LB_TO_KG, wheelbase=2.91, steerRatio=14.8, tireStiffnessFactor=0.8), diff --git a/opendbc_repo/opendbc/car/volkswagen/carcontroller.py b/opendbc_repo/opendbc/car/volkswagen/carcontroller.py index e2a4fe86..048b283c 100644 --- a/opendbc_repo/opendbc/car/volkswagen/carcontroller.py +++ b/opendbc_repo/opendbc/car/volkswagen/carcontroller.py @@ -3,7 +3,7 @@ from opendbc.can.packer import CANPacker from opendbc.car import Bus, DT_CTRL, apply_driver_steer_torque_limits, structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.interfaces import CarControllerBase -from opendbc.car.volkswagen import mqbcan, pqcan +from opendbc.car.volkswagen import mqbcan, pqcan, mebcan from opendbc.car.volkswagen.values import CANBUS, CarControllerParams, VolkswagenFlags VisualAlert = structs.CarControl.HUDControl.VisualAlert @@ -14,12 +14,20 @@ class CarController(CarControllerBase): def __init__(self, dbc_names, CP): super().__init__(dbc_names, CP) self.CCP = CarControllerParams(CP) - self.CCS = pqcan if CP.flags & VolkswagenFlags.PQ else mqbcan + if CP.flags & VolkswagenFlags.PQ: + self.CCS = pqcan + elif CP.flags & VolkswagenFlags.MEB: + self.CCS = mebcan + else: + self.CCS = mqbcan self.packer_pt = CANPacker(dbc_names[Bus.pt]) self.ext_bus = CANBUS.pt if CP.networkLocation == structs.CarParams.NetworkLocation.fwdCamera else CANBUS.cam self.aeb_available = not CP.flags & VolkswagenFlags.PQ + self.openpilot_longitudinal = self.CP.openpilotLongitudinalControl and not self.CP.flags & VolkswagenFlags.MEB self.apply_torque_last = 0 + self.apply_curvature_last = 0 + self.apply_steer_power_last = 0 self.gra_acc_counter_last = None self.eps_timer_soft_disable_alert = False self.hca_frame_timer_running = 0 @@ -33,50 +41,83 @@ class CarController(CarControllerBase): # **** Steering Controls ************************************************ # if self.frame % self.CCP.STEER_STEP == 0: - # Logic to avoid HCA state 4 "refused": - # * Don't steer unless HCA is in state 3 "ready" or 5 "active" - # * Don't steer at standstill - # * Don't send > 3.00 Newton-meters torque - # * Don't send the same torque for > 6 seconds - # * Don't send uninterrupted steering for > 360 seconds - # MQB racks reset the uninterrupted steering timer after a single frame - # of HCA disabled; this is done whenever output happens to be zero. + if self.CP.flags & VolkswagenFlags.MEB: + # The QFK (lateral control coordinator) control loop compares actuation curvature to its own current curvature + # Calibrate our actuator command by the offset between openpilot's vehicle model and QFK perceived curvatures + apply_curvature = actuators.curvature + (CS.qfk_curvature - CC.currentCurvature) - if CC.latActive: - new_torque = int(round(actuators.torque * self.CCP.STEER_MAX)) - apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.CCP) - self.hca_frame_timer_running += self.CCP.STEER_STEP - if self.apply_torque_last == apply_torque: - self.hca_frame_same_torque += self.CCP.STEER_STEP - if self.hca_frame_same_torque > self.CCP.STEER_TIME_STUCK_TORQUE / DT_CTRL: - apply_torque -= (1, -1)[apply_torque < 0] - self.hca_frame_same_torque = 0 + # Progressive QFK power control: smooth engage and disengage, reduce power when the driver is overriding + qfk_enable = True + if CC.latActive: + min_power = max(self.apply_steer_power_last - self.CCP.STEERING_POWER_STEP, self.CCP.STEERING_POWER_MIN) + max_power = max(self.apply_steer_power_last + self.CCP.STEERING_POWER_STEP, self.CCP.STEERING_POWER_MAX) + target_power = int(np.interp(CS.out.steeringTorque, [self.CCP.STEER_DRIVER_ALLOWANCE, self.CCP.STEER_DRIVER_MAX], + [self.CCP.STEERING_POWER_MAX, self.CCP.STEERING_POWER_MIN])) + apply_steer_power = min(max(target_power, min_power), max_power) + elif self.apply_steer_power_last > 0: + apply_steer_power = max(self.apply_steer_power_last - self.CCP.STEERING_POWER_STEP, 0) else: - self.hca_frame_same_torque = 0 - hca_enabled = abs(apply_torque) > 0 + qfk_enable = False + apply_curvature = 0 + apply_steer_power = 0 + + can_sends.append(mebcan.create_steering_control(self.packer_pt, CANBUS.pt, apply_curvature, qfk_enable, apply_steer_power)) + + self.apply_curvature_last = apply_curvature + self.apply_steer_power_last = apply_steer_power + else: - hca_enabled = False - apply_torque = 0 + # Logic to avoid HCA state 4 "refused": + # * Don't steer unless HCA is in state 3 "ready" or 5 "active" + # * Don't steer at standstill + # * Don't send > 3.00 Newton-meters torque + # * Don't send the same torque for > 6 seconds + # * Don't send uninterrupted steering for > 360 seconds + # MQB racks reset the uninterrupted steering timer after a single frame + # of HCA disabled; this is done whenever output happens to be zero. - if not hca_enabled: - self.hca_frame_timer_running = 0 + if CC.latActive: + new_torque = int(round(actuators.torque * self.CCP.STEER_MAX)) + apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.CCP) + self.hca_frame_timer_running += self.CCP.STEER_STEP + if self.apply_torque_last == apply_torque: + self.hca_frame_same_torque += self.CCP.STEER_STEP + if self.hca_frame_same_torque > self.CCP.STEER_TIME_STUCK_TORQUE / DT_CTRL: + apply_torque -= (1, -1)[apply_torque < 0] + self.hca_frame_same_torque = 0 + else: + self.hca_frame_same_torque = 0 + hca_enabled = abs(apply_torque) > 0 + else: + hca_enabled = False + apply_torque = 0 - self.eps_timer_soft_disable_alert = self.hca_frame_timer_running > self.CCP.STEER_TIME_ALERT / DT_CTRL - self.apply_torque_last = apply_torque - can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, apply_torque, hca_enabled)) + if not hca_enabled: + self.hca_frame_timer_running = 0 - if self.CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT: - # Pacify VW Emergency Assist driver inactivity detection by changing its view of driver steering input torque - # to the greatest of actual driver input or 2x openpilot's output (1x openpilot output is not enough to - # consistently reset inactivity detection on straight level roads). See commaai/openpilot#23274 for background. - ea_simulated_torque = float(np.clip(apply_torque * 2, -self.CCP.STEER_MAX, self.CCP.STEER_MAX)) - if abs(CS.out.steeringTorque) > abs(ea_simulated_torque): - ea_simulated_torque = CS.out.steeringTorque - can_sends.append(self.CCS.create_eps_update(self.packer_pt, CANBUS.cam, CS.eps_stock_values, ea_simulated_torque)) + self.eps_timer_soft_disable_alert = self.hca_frame_timer_running > self.CCP.STEER_TIME_ALERT / DT_CTRL + self.apply_torque_last = apply_torque + can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, apply_torque, hca_enabled)) + + if self.CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT: + # Pacify VW Emergency Assist driver inactivity detection by changing its view of driver steering input torque + # to the greatest of actual driver input or 2x openpilot's output (1x openpilot output is not enough to + # consistently reset inactivity detection on straight level roads). See commaai/openpilot#23274 for background. + ea_simulated_torque = float(np.clip(apply_torque * 2, -self.CCP.STEER_MAX, self.CCP.STEER_MAX)) + if abs(CS.out.steeringTorque) > abs(ea_simulated_torque): + ea_simulated_torque = CS.out.steeringTorque + can_sends.append(self.CCS.create_eps_update(self.packer_pt, CANBUS.cam, CS.eps_stock_values, ea_simulated_torque)) + + # TODO: refactor a bit + if self.CP.flags & VolkswagenFlags.MEB: + if self.frame % 2 == 0: + can_sends.append(mebcan.create_ea_control(self.packer_pt, CANBUS.pt)) + if self.frame % 50 == 0: + can_sends.append(mebcan.create_ea_hud(self.packer_pt, CANBUS.pt)) # **** Acceleration Controls ******************************************** # - if self.CP.openpilotLongitudinalControl: + if self.openpilot_longitudinal: if self.frame % self.CCP.ACC_CONTROL_STEP == 0: acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) accel = float(np.clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0) @@ -100,7 +141,7 @@ class CarController(CarControllerBase): can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.latActive, CS.out.steeringPressed, hud_alert, hud_control)) - if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl: + if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.openpilot_longitudinal: lead_distance = 0 if hud_control.leadVisible and self.frame * DT_CTRL > 1.0: # Don't display lead until we know the scaling factor lead_distance = 512 if CS.upscale_lead_car_signal else 8 @@ -120,6 +161,7 @@ class CarController(CarControllerBase): new_actuators = actuators.as_builder() new_actuators.torque = self.apply_torque_last / self.CCP.STEER_MAX new_actuators.torqueOutputCan = self.apply_torque_last + new_actuators.curvature = float(self.apply_curvature_last) self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"] self.frame += 1 diff --git a/opendbc_repo/opendbc/car/volkswagen/carstate.py b/opendbc_repo/opendbc/car/volkswagen/carstate.py index 2e346222..b51965b0 100644 --- a/opendbc_repo/opendbc/car/volkswagen/carstate.py +++ b/opendbc_repo/opendbc/car/volkswagen/carstate.py @@ -19,6 +19,7 @@ class CarState(CarStateBase): self.esp_hold_confirmation = False self.upscale_lead_car_signal = False self.eps_stock_values = False + self.qfk_curvature = 0. def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]): if not self.CP.pcmCruise: @@ -63,7 +64,55 @@ class CarState(CarStateBase): else: ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Gateway_73"]["GE_Fahrstufe"], None)) - if True: + if self.CP.flags & VolkswagenFlags.MEB: + # MEB-specific + self.qfk_curvature = -pt_cp.vl["QFK_01"]["Curvature"] * (1, -1)[int(pt_cp.vl["QFK_01"]["Curvature_VZ"])] + ret.fuelGauge = pt_cp.vl["Motor_16"]["MO_Energieinhalt_BMS"] # TODO: is this available on MQB as well? + + ret.wheelSpeeds = self.get_wheel_speeds( + pt_cp.vl["ESC_51"]["VL_Radgeschw"], + pt_cp.vl["ESC_51"]["VR_Radgeschw"], + pt_cp.vl["ESC_51"]["HL_Radgeschw"], + pt_cp.vl["ESC_51"]["HR_Radgeschw"], + ) + + ret.yawRate = pt_cp.vl["ESC_50"]["Yaw_Rate"] * (1, -1)[int(pt_cp.vl["ESC_50"]["Yaw_Rate_Sign"])] * CV.DEG_TO_RAD + hca_status = self.CCP.hca_status_values.get(pt_cp.vl["QFK_01"]["LatCon_HCA_Status"]) + + drive_mode = ret.gearShifter == GearShifter.drive + ret.gas = pt_cp.vl["Motor_54"]["Accelerator_Pressure"] + ret.brake = pt_cp.vl["ESC_51"]["Brake_Pressure"] + ret.brakePressed = bool(pt_cp.vl["Motor_14"]["MO_Fahrer_bremst"]) # includes regen braking by user + ret.parkingBrake = pt_cp.vl["Gateway_73"]["EPB_Status"] in (1, 4) # EPB closing or closed + + ret.doorOpen = any([pt_cp.vl["ZV_02"]["ZV_FT_offen"], + pt_cp.vl["ZV_02"]["ZV_BT_offen"], + pt_cp.vl["ZV_02"]["ZV_HFS_offen"], + pt_cp.vl["ZV_02"]["ZV_HBFS_offen"], + pt_cp.vl["ZV_02"]["ZV_HD_offen"]]) + + if self.CP.enableBsm: + ret.leftBlindspot = bool(ext_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Info_Left"]) or bool(ext_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Warn_Left"]) + ret.rightBlindspot = bool(ext_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Info_Right"]) or bool(ext_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Warn_Right"]) + + ret.stockFcw = bool(pt_cp.vl["VMM_02"]["FCW_Active"]) or bool(ext_cp.vl["AWV_03"]["FCW_Active"]) + ret.stockAeb = bool(pt_cp.vl["VMM_02"]["AEB_Active"]) + + self.travel_assist_available = bool(cam_cp.vl["TA_01"]["Travel_Assist_Available"]) + self.acc_type = ext_cp.vl["ACC_18"]["ACC_Typ"] + self.esp_hold_confirmation = bool(pt_cp.vl["VMM_02"]["ESP_Hold"]) + acc_limiter_mode = bool(ext_cp.vl["MEB_ACC_01"]["ACC_Limiter_Mode"]) + speed_limiter_mode = bool(pt_cp.vl["Motor_51"]["TSK_Limiter_ausgewaehlt"]) + + ret.cruiseState.available = pt_cp.vl["Motor_51"]["TSK_Status"] in (2, 3, 4, 5) + ret.cruiseState.enabled = pt_cp.vl["Motor_51"]["TSK_Status"] in (3, 4, 5) + ret.cruiseState.speed = int(round(ext_cp.vl["MEB_ACC_01"]["ACC_Wunschgeschw_02"])) * CV.KPH_TO_MS if self.CP.pcmCruise else 0 + ret.accFaulted = drive_mode and pt_cp.vl["Motor_51"]["TSK_Status"] in (6, 7) + + ret.leftBlinker = bool(pt_cp.vl["Blinkmodi_02"]["BM_links"]) + ret.rightBlinker = bool(pt_cp.vl["Blinkmodi_02"]["BM_rechts"]) + + else: # MQB-specific self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) # Analog vs digital instrument cluster @@ -256,6 +305,8 @@ class CarState(CarStateBase): def get_can_parsers(CP): if CP.flags & VolkswagenFlags.PQ: return CarState.get_can_parsers_pq(CP) + elif CP.flags & VolkswagenFlags.MEB: + return CarState.get_can_parsers_meb(CP) pt_messages = [ # sig_address, frequency @@ -356,6 +407,51 @@ class CarState(CarStateBase): Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CANBUS.cam), } + @staticmethod + def get_can_parsers_meb(CP): + pt_messages = [ + # sig_address, frequency + ("LWI_01", 100), # From J500 Steering Assist with integrated sensors + ("GRA_ACC_01", 33), # From J533 CAN gateway (via LIN from steering wheel controls) + ("Airbag_02", 5), # From J234 Airbag control module + ("Motor_14", 10), # From J623 Engine control module + ("Motor_16", 2), # From J623 Engine control module + ("Blinkmodi_02", 1), # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active) + ("LH_EPS_03", 100), # From J500 Steering Assist with integrated sensors + ("ZV_02", 5), # From ZV + ("QFK_01", 100), # From Steering + ("ESP_21", 50), # + ("ESC_51", 100), # + ("Motor_54", 10), # + ("ESC_50", 50), # + ("VMM_02", 50), # + ("Gateway_73", 20), # + ("Motor_51", 50), # + ] + + if CP.networkLocation == NetworkLocation.fwdCamera: + # Radars are here on CANBUS.pt + pt_messages += MebExtraSignals.fwd_radar_messages + if CP.enableBsm: + pt_messages += MebExtraSignals.bsm_radar_messages + + cam_messages = [ + # sig_address, frequency + ("LDW_02", 10), # From R242 Driver assistance camera + ("TA_01", 10), # From R242 Driver assistance camera (Travel Assist) + ] + + if CP.networkLocation == NetworkLocation.gateway: + # Radars are here on CANBUS.cam + cam_messages += MebExtraSignals.fwd_radar_messages + if CP.enableBsm: + cam_messages += MebExtraSignals.bsm_radar_messages + + return { + Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, CANBUS.pt), + Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CANBUS.cam), + } + class MqbExtraSignals: # Additional signal and message lists for optional or bus-portable controllers @@ -378,3 +474,15 @@ class PqExtraSignals: bsm_radar_messages = [ ("SWA_1", 20), # From J1086 Lane Change Assist ] + + +class MebExtraSignals: + # Additional signal and message lists for optional or bus-portable controllers + fwd_radar_messages = [ + ("MEB_ACC_01", 17), # + ("ACC_18", 50), # + ("AWV_03", 1), # Front Collision Detection (1 Hz when inactive, 50 Hz when active) + ] + bsm_radar_messages = [ + ("MEB_Side_Assist_01", 20), + ] diff --git a/opendbc_repo/opendbc/car/volkswagen/fingerprints.py b/opendbc_repo/opendbc/car/volkswagen/fingerprints.py index 1162e3f9..f9ae9bba 100644 --- a/opendbc_repo/opendbc/car/volkswagen/fingerprints.py +++ b/opendbc_repo/opendbc/car/volkswagen/fingerprints.py @@ -977,6 +977,19 @@ FW_VERSIONS = { b'\xf1\x875Q0907572R \xf1\x890771', ], }, + CAR.VOLKSWAGEN_ID4_MK1: { + (Ecu.srs, 0x715, None): [ + b'\xf1\x875WA959655R \xf1\x890717', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x871EA907572H \xf1\x890234', + ], + }, + CAR.SKODA_ENYAQ_MK1: { + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x871EA907567B \xf1\x890232', + ], + }, CAR.SKODA_FABIA_MK4: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8705E906018CF\xf1\x891905', diff --git a/opendbc_repo/opendbc/car/volkswagen/interface.py b/opendbc_repo/opendbc/car/volkswagen/interface.py index 70a67857..780fa61f 100644 --- a/opendbc_repo/opendbc/car/volkswagen/interface.py +++ b/opendbc_repo/opendbc/car/volkswagen/interface.py @@ -37,6 +37,18 @@ class CarInterface(CarInterfaceBase): # Panda ALLOW_DEBUG firmware required. ret.dashcamOnly = True + elif ret.flags & VolkswagenFlags.MEB: + # Set global MEB parameters + ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.noOutput),get_safety_config(structs.CarParams.SafetyModel.volkswagenMeb)] + ret.enableBsm = 0x24C in fingerprint[0] # MEB_Side_Assist_01 + ret.steerControlType = structs.CarParams.SteerControlType.angle + ret.transmissionType = TransmissionType.automatic + + if any(msg in fingerprint[1] for msg in (0x520, 0x86, 0xFD, 0x13D)): # Airbag_02, LWI_01, ESP_21, QFK_01 + ret.networkLocation = NetworkLocation.gateway + else: + ret.networkLocation = NetworkLocation.fwdCamera + else: # Set global MQB parameters ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.volkswagen)] @@ -63,6 +75,8 @@ class CarInterface(CarInterfaceBase): if ret.flags & VolkswagenFlags.PQ: ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif ret.flags & VolkswagenFlags.MEB: + ret.steerActuatorDelay = 0.2 else: ret.steerActuatorDelay = 0.1 ret.lateralTuning.pid.kpBP = [0.] @@ -73,7 +87,7 @@ class CarInterface(CarInterfaceBase): # Global longitudinal tuning defaults, can be overridden per-vehicle - ret.experimentalLongitudinalAvailable = ret.networkLocation == NetworkLocation.gateway or docs + ret.experimentalLongitudinalAvailable = not ret.flags & VolkswagenFlags.MEB and (ret.networkLocation == NetworkLocation.gateway or docs) if experimental_long: # Proof-of-concept, prep for E2E only. No radar points available. Panda ALLOW_DEBUG firmware required. ret.openpilotLongitudinalControl = True diff --git a/opendbc_repo/opendbc/car/volkswagen/values.py b/opendbc_repo/opendbc/car/volkswagen/values.py index 750a99e1..f6e6b4f1 100644 --- a/opendbc_repo/opendbc/car/volkswagen/values.py +++ b/opendbc_repo/opendbc/car/volkswagen/values.py @@ -72,6 +72,38 @@ class CarControllerParams: "laneAssistDeactivTrailer": 5, # "Lane Assist: no function with trailer" } + elif CP.flags & VolkswagenFlags.MEB: + self.LDW_STEP = 10 # LDW_02 message frequency 10Hz + self.ACC_HUD_STEP = 6 # MEB_ACC_01 message frequency 16Hz + self.STEER_DRIVER_ALLOWANCE = 60 # Driver torque 0.6 Nm, begin steering reduction from MAX + self.STEER_DRIVER_MAX = 300 # Driver torque 3.0 Nm, stop steering reduction at MIN + self.STEERING_POWER_MAX = 50 # HCA_03 maximum steering power, percentage + self.STEERING_POWER_MIN = 20 # HCA_03 minimum steering power, percentage + self.STEERING_POWER_STEP = 2 # HCA_03 steering power step increments + + self.shifter_values = can_define.dv["Gateway_73"]["GE_Fahrstufe"] + self.hca_status_values = can_define.dv["QFK_01"]["LatCon_HCA_Status"] + + self.BUTTONS = [ + Button(structs.CarState.ButtonEvent.Type.setCruise, "GRA_ACC_01", "GRA_Tip_Setzen", [1]), + Button(structs.CarState.ButtonEvent.Type.resumeCruise, "GRA_ACC_01", "GRA_Tip_Wiederaufnahme", [1]), + Button(structs.CarState.ButtonEvent.Type.accelCruise, "GRA_ACC_01", "GRA_Tip_Hoch", [1]), + Button(structs.CarState.ButtonEvent.Type.decelCruise, "GRA_ACC_01", "GRA_Tip_Runter", [1]), + Button(structs.CarState.ButtonEvent.Type.cancel, "GRA_ACC_01", "GRA_Hauptschalter", [1]), + Button(structs.CarState.ButtonEvent.Type.gapAdjustCruise, "GRA_ACC_01", "GRA_Verstellung_Zeitluecke", [3]), + ] + + self.LDW_MESSAGES = { + "none": 0, # Nothing to display + "laneAssistTakeOverUrgent": 4, # "Lane Assist: Please Take Over Steering" (red) + "laneAssistTakeOver": 8, # "Lane Assist: Please Take Over Steering" (white) + } + self.LDW_SOUNDS = { + "None": 0, # No sound + "Chime": 1, # Play a chime + "Beep": 2, # Play a loud beep + } + else: self.LDW_STEP = 10 # LDW_02 message frequency 10Hz self.ACC_HUD_STEP = 6 # ACC_02 message frequency 16Hz @@ -108,8 +140,8 @@ class CarControllerParams: class CANBUS: - pt = 0 - cam = 2 + pt = 4 + cam = 6 class WMI(StrEnum): @@ -143,20 +175,33 @@ class VolkswagenFlags(IntFlag): # Static flags PQ = 2 + MEB = 4 @dataclass class VolkswagenMQBPlatformConfig(PlatformConfig): - dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'vw_mqb_2010'}) + dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'vw_mqb'}) # Volkswagen uses the VIN WMI and chassis code to match in the absence of the comma power # on camera-integrated cars, as we lose too many ECUs to reliably identify the vehicle chassis_codes: set[str] = field(default_factory=set) wmis: set[WMI] = field(default_factory=set) +@dataclass +class VolkswagenMEBPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'vw_meb', Bus.radar: 'vw_meb'}) + # Volkswagen uses the VIN WMI and chassis code to match in the absence of the comma power + # on camera-integrated cars, as we lose too many ECUs to reliably identify the vehicle + chassis_codes: set[str] = field(default_factory=set) + wmis: set[WMI] = field(default_factory=set) + + def init(self): + self.flags |= VolkswagenFlags.MEB + + @dataclass class VolkswagenPQPlatformConfig(VolkswagenMQBPlatformConfig): - dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'vw_golf_mk4'}) + dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'vw_pq'}) def init(self): self.flags |= VolkswagenFlags.PQ @@ -188,6 +233,9 @@ class Footnote(Enum): "Model-years 2022 and beyond may have a combined CAN gateway and BCM, which is supported by openpilot " + "in software, but doesn't yet have a harness available from the comma store.", Column.HARDWARE) + VW_MEB = CarFootnote( + "Volkswagen MEB plattform is using CAN-FD, which is supported by comma 3x or red panda.", + Column.HARDWARE) @dataclass @@ -200,7 +248,9 @@ class VWCarDocs(CarDocs): if "SKODA" in CP.carFingerprint: self.footnotes.append(Footnote.SKODA_HEATED_WINDSHIELD) - if CP.carFingerprint in (CAR.VOLKSWAGEN_CRAFTER_MK2, CAR.VOLKSWAGEN_TRANSPORTER_T61): + if CP.flags & VolkswagenFlags.MEB: + self.car_parts = CarParts.common([CarHarness.vw_c]) + elif CP.carFingerprint in (CAR.VOLKSWAGEN_CRAFTER_MK2, CAR.VOLKSWAGEN_TRANSPORTER_T61): self.car_parts = CarParts([Device.threex_angled_mount, CarHarness.vw_j533]) if abs(CP.minSteerSpeed - CarControllerParams.DEFAULT_MIN_STEER_SPEED) < 1e-3: @@ -212,7 +262,7 @@ class VWCarDocs(CarDocs): # FW_VERSIONS for that existing CAR. class CAR(Platforms): - config: VolkswagenMQBPlatformConfig | VolkswagenPQPlatformConfig + config: VolkswagenMQBPlatformConfig | VolkswagenPQPlatformConfig | VolkswagenMEBPlatformConfig VOLKSWAGEN_ARTEON_MK1 = VolkswagenMQBPlatformConfig( [ @@ -274,6 +324,12 @@ class CAR(Platforms): chassis_codes={"5G", "AU", "BA", "BE"}, wmis={WMI.VOLKSWAGEN_MEXICO_CAR, WMI.VOLKSWAGEN_EUROPE_CAR}, ) + VOLKSWAGEN_ID4_MK1 = VolkswagenMEBPlatformConfig( + [VWCarDocs("Volkswagen ID.4 2021-25", footnotes=[Footnote.VW_MEB])], + VolkswagenCarSpecs(mass=2099, wheelbase=2.77), + chassis_codes={"E2", "E8"}, + wmis={WMI.VOLKSWAGEN_EUROPE_SUV, WMI.VOLKSWAGEN_USA_SUV}, + ) VOLKSWAGEN_JETTA_MK6 = VolkswagenPQPlatformConfig( [VWCarDocs("Volkswagen Jetta 2015-18")], VolkswagenCarSpecs(mass=1518, wheelbase=2.65, minSteerSpeed=50 * CV.KPH_TO_MS, minEnableSpeed=20 * CV.KPH_TO_MS), @@ -398,6 +454,15 @@ class CAR(Platforms): chassis_codes={"5F"}, wmis={WMI.SEAT}, ) + SKODA_ENYAQ_MK1 = VolkswagenMEBPlatformConfig( + [ + VWCarDocs("Å koda Enyaq 2021-25"), + VWCarDocs("Å koda Enyaq Coupe 2023-25"), + ], + VolkswagenCarSpecs(mass=2137, wheelbase=2.77), + chassis_codes={"NY"}, + wmis={WMI.SKODA}, + ) SKODA_FABIA_MK4 = VolkswagenMQBPlatformConfig( [VWCarDocs("Å koda Fabia 2022-23", footnotes=[Footnote.VW_MQB_A0])], VolkswagenCarSpecs(mass=1266, wheelbase=2.56), diff --git a/opendbc_repo/opendbc/dbc/generator/gm/gm_global_a_powertrain.dbc b/opendbc_repo/opendbc/dbc/generator/gm/gm_global_a_powertrain.dbc index 2dd72d36..adfb96b7 100644 --- a/opendbc_repo/opendbc/dbc/generator/gm/gm_global_a_powertrain.dbc +++ b/opendbc_repo/opendbc/dbc/generator/gm/gm_global_a_powertrain.dbc @@ -194,15 +194,12 @@ BO_ 711 BECMBatteryVoltageCurrent: 6 K17_EBCM SG_ HVBatteryCurrent : 12|13@0- (0.15,0) [-614.4|614.25] "A" NEO BO_ 715 ASCMGasRegenCmd: 8 K124_ASCM - SG_ GasRegenAlwaysOne2 : 9|1@0+ (1,0) [0|1] "" NEO - SG_ GasRegenAlwaysOne : 14|1@0+ (1,0) [0|1] "" NEO - SG_ GasRegenChecksum : 47|24@0+ (1,0) [0|0] "" NEO - SG_ GasRegenCmdActiveInv : 32|1@0+ (1,0) [0|0] "" NEO + SG_ GasRegenAccType : 15|2@0+ (1,0) [0|3] "" NEO + SG_ GasRegenChecksum : 32|25@0+ (1,0) [0|0] "" NEO SG_ GasRegenFullStopActive : 13|1@0+ (1,0) [0|0] "" NEO SG_ GasRegenCmdActive : 0|1@0+ (1,0) [0|0] "" NEO SG_ RollingCounter : 7|2@0+ (1,0) [0|0] "" NEO - SG_ GasRegenAlwaysOne3 : 23|1@0+ (1,0) [0|1] "" NEO - SG_ GasRegenCmd : 22|12@0+ (1,0) [0|0] "" NEO + SG_ GasRegenCmd : 10|19@0+ (0.125,-22534) [-22534|43001.875] "Nm" NEO BO_ 717 ASCM_2CD: 5 K124_ASCM diff --git a/opendbc_repo/opendbc/dbc/generator/nissan/_nissan_common.dbc b/opendbc_repo/opendbc/dbc/generator/nissan/_nissan_common.dbc index 6300ea94..13da6361 100644 --- a/opendbc_repo/opendbc/dbc/generator/nissan/_nissan_common.dbc +++ b/opendbc_repo/opendbc/dbc/generator/nissan/_nissan_common.dbc @@ -107,7 +107,7 @@ BO_ 1227 LKAS_SETTINGS: 8 XXX VAL_ 1228 PROPILOT_NA_MSGS 0 "NO_MSG" 1 "NA_FRONT_CAMERA_IMPARED" 2 "STEERING_ASSIST_ON_STANDBY" 3 "NA_PARKING_ASSIST_ENABLED" 4 "STEER_ASSIST_CURRENTLY_NA" 5 "NA_BAD_WEATHER" 6 "NA_PARK_BRAKE_ON" 7 "NA_SEATBELT_NOT_FASTENED" ; VAL_ 1228 BOTTOM_MSG 0 "OK_STEER_ASSIST_SETTINGS" 1 "NO_MSG" 2 "PRESS_SET_TO_SET_SPEED" 3 "PRESS_RES_SET_TO_CHANGE_SPEED" 4 "PRESS_RES_TO_RESTART" 5 "NO_MSG" 6 "CRUISE_NOT_AVAIL" 7 "NO_MSG" ; -VAL_ 689 FOLLOW_DISTANCE 0 "NO_FOLLOW_DISTANCE" 1 "FOLLOW_DISTANCE_1" 2 "FOLLOW_DISTANCE_2" 3 "FOLLOW_DISANCE_3" ; +VAL_ 689 FOLLOW_DISTANCE 0 "NO_FOLLOW_DISTANCE" 1 "FOLLOW_DISTANCE_1" 2 "FOLLOW_DISTANCE_2" 3 "FOLLOW_DISTANCE_3" ; VAL_ 689 AUDIBLE_TONE 0 "NO_TONE" 1 "CONT" 2 "FAST_BEEP_CONT" 3 "TRIPLE_FAST_BEEP_CONT" 4 "SLOW_BEEP_CONT" 5 "QUAD_SLOW_BEEP_CONT" 6 "SINGLE_BEEP_ONCE" 7 "DOUBLE_BEEP_ONCE" ; VAL_ 689 SMALL_STEERING_WHEEL_ICON 0 "NO_ICON" 1 "GRAY_ICON" 2 "GRAY_ICON_FLASHING" 3 "GREEN_ICON" 4 "GREEN_ICON_FLASHING" 5 "RED_ICON" 6 "RED_ICON_FLASHING" 7 "YELLOW_ICON" ; VAL_ 689 LARGE_STEERING_WHEEL_ICON 0 "NO_STEERINGWHEEL" 1 "GRAY_STEERINGWHEEL" 2 "GREEN_STEERINGWHEEL" 3 "GREEN_STEERINGWHEEL_FLASHING" ; diff --git a/opendbc_repo/opendbc/dbc/gm_global_a_powertrain_generated.dbc b/opendbc_repo/opendbc/dbc/gm_global_a_powertrain_generated.dbc index 429665cc..507de8c7 100644 --- a/opendbc_repo/opendbc/dbc/gm_global_a_powertrain_generated.dbc +++ b/opendbc_repo/opendbc/dbc/gm_global_a_powertrain_generated.dbc @@ -197,15 +197,12 @@ BO_ 711 BECMBatteryVoltageCurrent: 6 K17_EBCM SG_ HVBatteryCurrent : 12|13@0- (0.15,0) [-614.4|614.25] "A" NEO BO_ 715 ASCMGasRegenCmd: 8 K124_ASCM - SG_ GasRegenAlwaysOne2 : 9|1@0+ (1,0) [0|1] "" NEO - SG_ GasRegenAlwaysOne : 14|1@0+ (1,0) [0|1] "" NEO - SG_ GasRegenChecksum : 47|24@0+ (1,0) [0|0] "" NEO - SG_ GasRegenCmdActiveInv : 32|1@0+ (1,0) [0|0] "" NEO + SG_ GasRegenAccType : 15|2@0+ (1,0) [0|3] "" NEO + SG_ GasRegenChecksum : 32|25@0+ (1,0) [0|0] "" NEO SG_ GasRegenFullStopActive : 13|1@0+ (1,0) [0|0] "" NEO SG_ GasRegenCmdActive : 0|1@0+ (1,0) [0|0] "" NEO SG_ RollingCounter : 7|2@0+ (1,0) [0|0] "" NEO - SG_ GasRegenAlwaysOne3 : 23|1@0+ (1,0) [0|1] "" NEO - SG_ GasRegenCmd : 22|12@0+ (1,0) [0|0] "" NEO + SG_ GasRegenCmd : 10|19@0+ (0.125,-22534) [-22534|43001.875] "Nm" NEO BO_ 717 ASCM_2CD: 5 K124_ASCM diff --git a/opendbc_repo/opendbc/dbc/nissan_leaf_2018_generated.dbc b/opendbc_repo/opendbc/dbc/nissan_leaf_2018_generated.dbc index bfd30e48..7c638d08 100644 --- a/opendbc_repo/opendbc/dbc/nissan_leaf_2018_generated.dbc +++ b/opendbc_repo/opendbc/dbc/nissan_leaf_2018_generated.dbc @@ -111,7 +111,7 @@ BO_ 1227 LKAS_SETTINGS: 8 XXX VAL_ 1228 PROPILOT_NA_MSGS 0 "NO_MSG" 1 "NA_FRONT_CAMERA_IMPARED" 2 "STEERING_ASSIST_ON_STANDBY" 3 "NA_PARKING_ASSIST_ENABLED" 4 "STEER_ASSIST_CURRENTLY_NA" 5 "NA_BAD_WEATHER" 6 "NA_PARK_BRAKE_ON" 7 "NA_SEATBELT_NOT_FASTENED" ; VAL_ 1228 BOTTOM_MSG 0 "OK_STEER_ASSIST_SETTINGS" 1 "NO_MSG" 2 "PRESS_SET_TO_SET_SPEED" 3 "PRESS_RES_SET_TO_CHANGE_SPEED" 4 "PRESS_RES_TO_RESTART" 5 "NO_MSG" 6 "CRUISE_NOT_AVAIL" 7 "NO_MSG" ; -VAL_ 689 FOLLOW_DISTANCE 0 "NO_FOLLOW_DISTANCE" 1 "FOLLOW_DISTANCE_1" 2 "FOLLOW_DISTANCE_2" 3 "FOLLOW_DISANCE_3" ; +VAL_ 689 FOLLOW_DISTANCE 0 "NO_FOLLOW_DISTANCE" 1 "FOLLOW_DISTANCE_1" 2 "FOLLOW_DISTANCE_2" 3 "FOLLOW_DISTANCE_3" ; VAL_ 689 AUDIBLE_TONE 0 "NO_TONE" 1 "CONT" 2 "FAST_BEEP_CONT" 3 "TRIPLE_FAST_BEEP_CONT" 4 "SLOW_BEEP_CONT" 5 "QUAD_SLOW_BEEP_CONT" 6 "SINGLE_BEEP_ONCE" 7 "DOUBLE_BEEP_ONCE" ; VAL_ 689 SMALL_STEERING_WHEEL_ICON 0 "NO_ICON" 1 "GRAY_ICON" 2 "GRAY_ICON_FLASHING" 3 "GREEN_ICON" 4 "GREEN_ICON_FLASHING" 5 "RED_ICON" 6 "RED_ICON_FLASHING" 7 "YELLOW_ICON" ; VAL_ 689 LARGE_STEERING_WHEEL_ICON 0 "NO_STEERINGWHEEL" 1 "GRAY_STEERINGWHEEL" 2 "GREEN_STEERINGWHEEL" 3 "GREEN_STEERINGWHEEL_FLASHING" ; diff --git a/opendbc_repo/opendbc/dbc/nissan_x_trail_2017_generated.dbc b/opendbc_repo/opendbc/dbc/nissan_x_trail_2017_generated.dbc index c6a3ef7a..66d6bf07 100644 --- a/opendbc_repo/opendbc/dbc/nissan_x_trail_2017_generated.dbc +++ b/opendbc_repo/opendbc/dbc/nissan_x_trail_2017_generated.dbc @@ -111,7 +111,7 @@ BO_ 1227 LKAS_SETTINGS: 8 XXX VAL_ 1228 PROPILOT_NA_MSGS 0 "NO_MSG" 1 "NA_FRONT_CAMERA_IMPARED" 2 "STEERING_ASSIST_ON_STANDBY" 3 "NA_PARKING_ASSIST_ENABLED" 4 "STEER_ASSIST_CURRENTLY_NA" 5 "NA_BAD_WEATHER" 6 "NA_PARK_BRAKE_ON" 7 "NA_SEATBELT_NOT_FASTENED" ; VAL_ 1228 BOTTOM_MSG 0 "OK_STEER_ASSIST_SETTINGS" 1 "NO_MSG" 2 "PRESS_SET_TO_SET_SPEED" 3 "PRESS_RES_SET_TO_CHANGE_SPEED" 4 "PRESS_RES_TO_RESTART" 5 "NO_MSG" 6 "CRUISE_NOT_AVAIL" 7 "NO_MSG" ; -VAL_ 689 FOLLOW_DISTANCE 0 "NO_FOLLOW_DISTANCE" 1 "FOLLOW_DISTANCE_1" 2 "FOLLOW_DISTANCE_2" 3 "FOLLOW_DISANCE_3" ; +VAL_ 689 FOLLOW_DISTANCE 0 "NO_FOLLOW_DISTANCE" 1 "FOLLOW_DISTANCE_1" 2 "FOLLOW_DISTANCE_2" 3 "FOLLOW_DISTANCE_3" ; VAL_ 689 AUDIBLE_TONE 0 "NO_TONE" 1 "CONT" 2 "FAST_BEEP_CONT" 3 "TRIPLE_FAST_BEEP_CONT" 4 "SLOW_BEEP_CONT" 5 "QUAD_SLOW_BEEP_CONT" 6 "SINGLE_BEEP_ONCE" 7 "DOUBLE_BEEP_ONCE" ; VAL_ 689 SMALL_STEERING_WHEEL_ICON 0 "NO_ICON" 1 "GRAY_ICON" 2 "GRAY_ICON_FLASHING" 3 "GREEN_ICON" 4 "GREEN_ICON_FLASHING" 5 "RED_ICON" 6 "RED_ICON_FLASHING" 7 "YELLOW_ICON" ; VAL_ 689 LARGE_STEERING_WHEEL_ICON 0 "NO_STEERINGWHEEL" 1 "GRAY_STEERINGWHEEL" 2 "GREEN_STEERINGWHEEL" 3 "GREEN_STEERINGWHEEL_FLASHING" ; diff --git a/opendbc_repo/opendbc/dbc/rivian_primary_actuator.dbc b/opendbc_repo/opendbc/dbc/rivian_primary_actuator.dbc index 686ec6fe..6422562d 100644 --- a/opendbc_repo/opendbc/dbc/rivian_primary_actuator.dbc +++ b/opendbc_repo/opendbc/dbc/rivian_primary_actuator.dbc @@ -85,28 +85,27 @@ BO_ 272 ACM_SteeringControl: 8 ACM SG_ ACM_SteeringAngleRequest : 23|15@0+ (0.1,-1638.4) [-1638.4|1638.3] "deg" EPAS_P BO_ 288 ACM_lkaHbaCmd: 8 ACM - SG_ ACM_lkaHbaCmd_Checksum : 7|8@0+ (1,0) [0|0] "" EPAS_P - SG_ ACM_lkaHbaCmd_Counter : 11|4@0+ (1,0) [0|0] "" EPAS_P - SG_ ACM_unkown1 : 12|1@0+ (1,0) [0|1] "" XXX - SG_ ACM_unkown6 : 13|1@0+ (1,0) [0|1] "" XXX - SG_ ACM_unkown5 : 14|1@0+ (1,0) [0|1] "" XXX + SG_ ACM_lkaHbaCmd_Checksum : 7|8@0+ (1,0) [0|255] "" EPAS_P + SG_ ACM_lkaHbaCmd_Counter : 11|4@0+ (1,0) [0|15] "" EPAS_P + SG_ ACM_lkaElkRequest : 14|3@0+ (1,0) [0|7] "" EPAS_P SG_ ACM_HapticRequest : 15|1@0+ (1,0) [0|1] "" EPAS_P SG_ ACM_lkaStrToqReq : 23|11@0+ (1,-1024) [-1024|1024] "" EPAS_P SG_ ACM_lkaSymbolState : 26|3@0+ (1,0) [0|7] "" EPAS_P SG_ ACM_lkaToiFlt : 27|1@0+ (1,0) [0|1] "" EPAS_P SG_ ACM_lkaActToi : 28|1@0+ (1,0) [0|1] "" EPAS_P SG_ ACM_hbaSysState : 34|3@0+ (1,0) [0|7] "" EPAS_P - SG_ ACM_FailinfoAeb : 37|3@0+ (1,0) [0|7] "" EPAS_P - SG_ ACM_unkown2 : 38|2@1+ (1,0) [0|3] "" XXX - SG_ ACM_lkaRHWarning : 41|2@0+ (1,0) [0|3] "" EPAS_P - SG_ ACM_lkaLHWarning : 43|2@0+ (1,0) [0|3] "" EPAS_P + SG_ ACM_hbaLamp : 35|1@0+ (1,0) [0|1] "" EPAS_P + SG_ ACM_slifOnOffState : 37|2@0+ (1,0) [0|3] "" EPAS_P + SG_ ACM_elkOnOffState : 39|2@0+ (1,0) [0|3] "" EPAS_P + SG_ ACM_ldwLHWarning : 43|3@0+ (1,0) [0|7] "" EPAS_P SG_ ACM_lkaLaneRecogState : 45|2@0+ (1,0) [0|3] "" EPAS_P - SG_ ACM_hbaOpt : 46|1@0+ (1,0) [0|1] "" EPAS_P - SG_ ACM_hbaLamp : 47|1@0+ (1,0) [0|1] "" EPAS_P - SG_ ACM_unkown3 : 51|4@0+ (1,0) [0|15] "" XXX + SG_ ACM_hbaOnOffState : 47|2@0+ (1,0) [0|3] "" EPAS_P + SG_ ACM_ldwlkaOnOffState : 48|3@0+ (1,0) [0|7] "" EPAS_P + SG_ ACM_ldwWarnTimingState : 51|3@0+ (1,0) [0|7] "" EPAS_P SG_ ACM_lkaHandsoffSoundWarning : 53|2@0+ (1,0) [0|3] "" EPAS_P SG_ ACM_lkaHandsoffDisplayWarning : 55|2@0+ (1,0) [0|3] "" EPAS_P - SG_ ACM_unkown4 : 56|8@1+ (1,0) [0|255] "" XXX + SG_ ACM_ldwRHWarning : 58|3@0+ (1,0) [0|7] "" EPAS_P + SG_ ACM_ldwWarnTypeState : 61|3@0+ (1,0) [0|7] "" EPAS_P BO_ 304 RCM_IMU_LatAccYaw: 8 RCM SG_ RCM_LateralAccelYaw_Checksum : 7|8@0+ (1,0) [0|25] "" ACM,ESP,VDM @@ -828,14 +827,15 @@ VAL_ 288 ACM_lkaSymbolState 0 "ACM_LKASYMBOLSTATE_OFF" 1 "ACM_LKASYMBOLSTATE_WHI VAL_ 288 ACM_lkaToiFlt 0 "ACM_LKATOIFLT_NO_FAULT" 1 "ACM_LKATOIFLT_FAULT_PRESENT"; VAL_ 288 ACM_lkaActToi 0 "ACM_LKAACTTOI_DE_ACTIVATE_TOI" 1 "ACM_LKAACTTOI_ACTIVATE_TOI"; VAL_ 288 ACM_hbaSysState 0 "ACM_HBASYSSTATE_DEFAULT_DISABLE" 1 "ACM_HBASYSSTATE_HBA_ENABLE_HIGH_BEAM_OFF" 2 "ACM_HBASYSSTATE_HBA_ENABLE_HIGH_BEAM_ON" 7 "ACM_HBASYSSTATE_SYSTEM_FAIL"; -VAL_ 288 ACM_FailinfoAeb 0 "ACM_FAILINFOAEB_NORMAL" 1 "ACM_FAILINFOAEB_CAMERA_FAILURE" 2 "ACM_FAILINFOAEB_FRONT_RADAR_COM_ERR" 3 "ACM_FAILINFOAEB_CAMERA_BLOCKAGE" 4 "ACM_FAILINFOAEB_RESERVED_4" 5 "ACM_FAILINFOAEB_RESERVED_5" 6 "ACM_FAILINFOAEB_RESERVED_6"; -VAL_ 288 ACM_lkaRHWarning 0 "ACM_LKARHWARNING_NO_WARNING" 1 "ACM_LKARHWARNING_HAPTIC_WARNING_AND_DISPLAY" 2 "ACM_LKARHWARNING_ACOUSTIC_WARNING_AND_DISPLAY" 3 "ACM_LKARHWARNING_HAPTIC_ACOUSTIC_AND_DISPLAY"; -VAL_ 288 ACM_lkaLHWarning 0 "ACM_LKALHWARNING_NO_WARNING" 1 "ACM_LKALHWARNING_HAPTIC_WARNING_AND_DISPLAY" 2 "ACM_LKALHWARNING_ACOUSTIC_WARNING_AND_DISPLAY" 3 "ACM_LKALHWARNING_HAPTIC_ACOUSTIC_AND_DISPLAY"; +VAL_ 288 ACM_ldwRHWarning 0 "ACM_LKARHWARNING_NO_WARNING" 1 "ACM_LKARHWARNING_HAPTIC_WARNING_AND_DISPLAY" 2 "ACM_LKARHWARNING_ACOUSTIC_WARNING_AND_DISPLAY" 3 "ACM_LKARHWARNING_HAPTIC_ACOUSTIC_AND_DISPLAY"; +VAL_ 288 ACM_ldwLHWarning 0 "ACM_LKALHWARNING_NO_WARNING" 1 "ACM_LKALHWARNING_HAPTIC_WARNING_AND_DISPLAY" 2 "ACM_LKALHWARNING_ACOUSTIC_WARNING_AND_DISPLAY" 3 "ACM_LKALHWARNING_HAPTIC_ACOUSTIC_AND_DISPLAY"; VAL_ 288 ACM_lkaLaneRecogState 0 "ACM_LKALANERECOGSTATE_NOT_RECOGNITION" 1 "ACM_LKALANERECOGSTATE_LEFT_LANE_RECOGNITION" 2 "ACM_LKALANERECOGSTATE_RIGHT_LANE_RECOGNITION" 3 "ACM_LKALANERECOGSTATE_FULL_LANE_RECOGNITION"; -VAL_ 288 ACM_hbaOpt 0 "ACM_HBAOPT_NONE_HBA_OPTION_DEFAULT" 1 "ACM_HBAOPT_HBA_SYSTEM_ENABLE"; VAL_ 288 ACM_hbaLamp 0 "ACM_HBALAMP_HBA_INDICATOR_LAMP_OFF" 1 "ACM_HBALAMP_HBA_INDICATOR_LAMP_ON"; VAL_ 288 ACM_lkaHandsoffSoundWarning 0 "ACM_LKAHANDSOFFSOUNDWARNING_NO_INFO" 1 "ACM_LKAHANDSOFFSOUNDWARNING_WARNING" 2 "ACM_LKAHANDSOFFSOUNDWARNING_RESERVED_2" 3 "ACM_LKAHANDSOFFSOUNDWARNING_RESERVED_3"; VAL_ 288 ACM_lkaHandsoffDisplayWarning 0 "ACM_LKAHANDSOFFDISPLAYWARNING_NO_INFO" 1 "ACM_LKAHANDSOFFDISPLAYWARNING_WARNING" 2 "ACM_LKAHANDSOFFDISPLAYWARNING_RESERVED_2" 3 "ACM_LKAHANDSOFFDISPLAYWARNING_RESERVED_3"; +VAL_ 288 ACM_lkaElkRequest 0 "off" 1 "applying torque right for left departure" 2 "applying torque left for right departure"; +VAL_ 288 ACM_ldwlkaOnOffState 1 "LDW on" 2 "LKAS+LDW on" 3 "all off"; +VAL_ 288 ACM_elkOnOffState 1 "LKAS toggled on" 2 "LKAS toggled off"; VAL_ 304 RCM_IMU_LatAcc_Stat_SensAvail 0 "RCM_IMU_LatAcc_Stat_SensAvail_InSpec" 1 "RCM_IMU_LatAcc_Stat_SensAvail_NotInSpec"; VAL_ 304 RCM_IMU_LatAcc_Stat_Fail 0 "RCM_IMU_LatAcc_Stat_Fail_NotFailed" 1 "RCM_IMU_LatAcc_Stat_Fail_Failed"; VAL_ 304 RCM_IMU_LatAcc_Stat_Init 0 "RCM_IMU_LatAcc_Stat_Init_Finished" 1 "RCM_IMU_LatAcc_Stat_Init_Running"; diff --git a/opendbc_repo/opendbc/dbc/tesla_model3_party.dbc b/opendbc_repo/opendbc/dbc/tesla_model3_party.dbc index b8dcf0ff..2f7b67a0 100644 --- a/opendbc_repo/opendbc/dbc/tesla_model3_party.dbc +++ b/opendbc_repo/opendbc/dbc/tesla_model3_party.dbc @@ -189,6 +189,8 @@ BO_ 599 DI_speed: 8 PARTY SG_ DI_speedCounter : 8|4@1+ (1,0) [0|15] "" park SG_ DI_speedChecksum : 0|8@1+ (1,0) [0|255] "" park +BO_ 605 XXX_longitudinalRelated: 6 XXX + BO_ 1160 DAS_steeringControl: 4 PARTY SG_ DAS_steeringControlChecksum : 31|8@0+ (1,0) [0|255] "" aps SG_ DAS_steeringControlCounter : 19|4@0+ (1,0) [0|15] "" aps @@ -224,15 +226,28 @@ BO_ 646 DI_state: 8 ETH SG_ DI_locStatusChecksum : 0|8@1+ (1,0) [0|0] "" X BO_ 659 DAS_settings: 8 XXX - SG_ DAS_autopilotEnabled : 38|1@0+ (1,0) [0|1] "" XXX + SG_ DAS_driverSteeringWeight : 1|2@0+ (1,0) [0|255] "" XXX + SG_ DAS_slipStart : 2|1@0+ (1,0) [0|1] "" XXX + SG_ DAS_offRoadAssist : 3|2@1+ (1,0) [0|63] "" XXX + SG_ DAS_distanceUnits : 13|1@1+ (1,0) [0|255] "" XXX + SG_ DAS_aebEnabled : 18|1@0+ (1,0) [0|255] "" XXX + SG_ DAS_adaptiveHeadlights : 22|1@1+ (1,0) [0|31] "" XXX + SG_ DAS_autosteerEnabled2 : 24|1@0+ (1,0) [0|1] "" XXX + SG_ DAS_fcwEnabled : 34|1@0+ (1,0) [0|1] "" XXX + SG_ DAS_fcwSensitivity : 37|2@0+ (1,0) [0|63] "" XXX + SG_ DAS_autosteerEnabled : 38|1@0+ (1,0) [0|1] "" XXX + SG_ DAS_obstacleAwareAcceleration : 42|1@0+ (1,0) [0|1] "" XXX + SG_ DAS_driverAccelerationMode : 44|1@1+ (1,0) [0|127] "" XXX SG_ DAS_settingCounter : 52|4@1+ (1,0) [0|15] "" XXX SG_ DAS_settingChecksum : 63|8@0+ (1,0) [0|255] "" XXX BO_ 785 UI_warning: 7 XXX SG_ buckleStatus : 13|1@0+ (1,0) [0|1] "" XXX - SG_ scrollWheelRightTilt : 21|1@0+ (1,0) [0|1] "" XXX + SG_ scrollWheelPressed : 21|1@0+ (1,0) [0|1] "" XXX SG_ leftBlinkerOn : 22|1@0+ (1,0) [0|1] "" XXX SG_ rightBlinkerOn : 23|1@0+ (1,0) [0|1] "" XXX + SG_ leftBlinkerBlinking : 25|2@0+ (1,0) [0|3] "" XXX + SG_ rightBlinkerBlinking : 26|2@1+ (1,0) [0|15] "" XXX SG_ anyDoorOpen : 28|1@0+ (1,0) [0|1] "" XXX SG_ wiperSettings : 39|8@0+ (1,0) [0|255] "" XXX SG_ highBeam : 50|1@0+ (1,0) [0|1] "" XXX @@ -266,11 +281,12 @@ BO_ 923 DAS_status: 8 PARTY SG_ DAS_autopilotState : 0|4@1+ (1,0) [0|15] "" aps +CM_ BO_ 605 "Bytes change when toggling between FSD and AP, as well as Traffic Light and Stop Sign Control in TACC"; - - - - +CM_ SG_ 659 DAS_autosteerEnabled "1 if Autosteer or FSD is enabled, 0 otherwise"; +CM_ SG_ 785 leftBlinkerOn "only describes stalk position if half pressed without auto-cancel blinkers. otherwise acts as expected"; +CM_ SG_ 785 rightBlinkerOn "only describes stalk position if half pressed without auto-cancel blinkers. otherwise acts as expected"; +CM_ SG_ 785 scrollWheelPressed "captures either scroll wheel left, right or down press"; VAL_ 545 VCFRONT_uiAudioLVState 1 "LV_ON" 2 "LV_GOING_DOWN" 3 "LV_FAULT" 0 "LV_OFF" ; VAL_ 545 VCFRONT_uiHiCurrentLVState 1 "LV_ON" 2 "LV_GOING_DOWN" 3 "LV_FAULT" 0 "LV_OFF" ; @@ -385,8 +401,15 @@ VAL_ 646 DI_parkBrakeState 0 "UNAVAILABLE" 1 "RELEASED" 2 "REQUESTED" 3 "APPLIED VAL_ 646 DI_autoparkState 0 "UNAVAILABLE" 1 "STANDBY" 2 "STARTED" 3 "ACTIVE" 4 "COMPLETE" 5 "PAUSED" 6 "ABORTED" 7 "RESUMED" 8 "UNPARK_COMPLETE" 9 "SELFPARK_STARTED" 15 "SNA" ; VAL_ 646 DI_speedUnits 0 "MPH" 1 "KPH" ; VAL_ 646 DI_cruiseState 0 "UNAVAILABLE" 1 "STANDBY" 2 "ENABLED" 3 "STANDSTILL" 4 "OVERRIDE" 5 "FAULT" 6 "PRE_FAULT" 7 "PRE_CANCEL" ; +VAL_ 659 DAS_driverSteeringWeight 0 "light" 1 "standard" 2 "heavy"; +VAL_ 659 DAS_offRoadAssist 0 "disabled" 3 "enabled"; +VAL_ 659 DAS_distanceUnits 1 "miles" 0 "kilometers"; +VAL_ 659 DAS_fcwSensitivity 0 "early" 1 "medium" 2 "late" 3 "off"; +VAL_ 659 DAS_driverAccelerationMode 0 "chill" 1 "standard"; VAL_ 785 buckleStatus 1 "LATCHED" 0 "UNLATCHED" ; VAL_ 785 anyDoorOpen 1 "OPEN" 0 "CLOSED" ; +VAL_ 785 leftBlinkerBlinking 0 "off" 1 "blinking, off" 2 "blinking, on"; +VAL_ 785 rightBlinkerBlinking 0 "off" 1 "blinking, off" 2 "blinking, on"; VAL_ 923 DAS_autoLaneChangeState 5 "ALC_UNAVAILABLE_VEHICLE_SPEED" 17 "ALC_ABORT_POOR_VIEW_RANGE" 23 "ALC_BLOCKED_VEH_TTC_AND_USS_L" 0 "ALC_UNAVAILABLE_DISABLED" 26 "ALC_BLOCKED_LANE_TYPE_L" 29 "ALC_ABORT_TIMEOUT" 9 "ALC_IN_PROGRESS_L" 4 "ALC_UNAVAILABLE_EXITING_HIGHWAY" 22 "ALC_BLOCKED_VEH_TTC_L" 12 "ALC_WAITING_FOR_SIDE_OBST_TO_PASS_R" 18 "ALC_ABORT_LC_HEALTH_BAD" 28 "ALC_WAITING_HANDS_ON" 8 "ALC_AVAILABLE_BOTH" 11 "ALC_WAITING_FOR_SIDE_OBST_TO_PASS_L" 3 "ALC_UNAVAILABLE_TP_FOLLOW" 2 "ALC_UNAVAILABLE_SONICS_INVALID" 21 "ALC_UNAVAILABLE_SOLID_LANE_LINE" 24 "ALC_BLOCKED_VEH_TTC_R" 1 "ALC_UNAVAILABLE_NO_LANES" 25 "ALC_BLOCKED_VEH_TTC_AND_USS_R" 30 "ALC_ABORT_MISSION_PLAN_INVALID" 27 "ALC_BLOCKED_LANE_TYPE_R" 19 "ALC_ABORT_BLINKER_TURNED_OFF" 31 "ALC_SNA" 13 "ALC_WAITING_FOR_FWD_OBST_TO_PASS_L" 16 "ALC_ABORT_SIDE_OBSTACLE_PRESENT_R" 6 "ALC_AVAILABLE_ONLY_L" 20 "ALC_ABORT_OTHER_REASON" 15 "ALC_ABORT_SIDE_OBSTACLE_PRESENT_L" 7 "ALC_AVAILABLE_ONLY_R" 14 "ALC_WAITING_FOR_FWD_OBST_TO_PASS_R" 10 "ALC_IN_PROGRESS_R" ; VAL_ 923 DAS_autopilotHandsOnState 8 "LC_HANDS_ON_SUSPENDED" 15 "LC_HANDS_ON_SNA" 7 "LC_HANDS_ON_REQD_STRUCK_OUT" 3 "LC_HANDS_ON_REQD_VISUAL" 4 "LC_HANDS_ON_REQD_CHIME_1" 6 "LC_HANDS_ON_REQD_SLOWING" 1 "LC_HANDS_ON_REQD_DETECTED" 2 "LC_HANDS_ON_REQD_NOT_DETECTED" 5 "LC_HANDS_ON_REQD_CHIME_2" 0 "LC_HANDS_ON_NOT_REQD" ; VAL_ 923 DAS_fleetSpeedState 0 "FLEETSPEED_UNAVAILABLE" 1 "FLEETSPEED_AVAILABLE" 2 "FLEETSPEED_ACTIVE" 3 "FLEETSPEED_HOLD" ; @@ -404,6 +427,3 @@ VAL_ 923 DAS_fusedSpeedLimit 31 "NONE" 0 "UNKNOWN_SNA" ; VAL_ 923 DAS_blindSpotRearRight 3 "SNA" 0 "NO_WARNING" 1 "WARNING_LEVEL_1" 2 "WARNING_LEVEL_2" ; VAL_ 923 DAS_blindSpotRearLeft 3 "SNA" 0 "NO_WARNING" 1 "WARNING_LEVEL_1" 2 "WARNING_LEVEL_2" ; VAL_ 923 DAS_autopilotState 15 "SNA" 8 "ABORTING" 3 "ACTIVE_NOMINAL" 0 "DISABLED" 4 "ACTIVE_RESTRICTED" 5 "ACTIVE_NAV" 14 "FAULT" 1 "UNAVAILABLE" 9 "ABORTED" 2 "AVAILABLE" ; - - - diff --git a/opendbc_repo/opendbc/dbc/vw_meb.dbc b/opendbc_repo/opendbc/dbc/vw_meb.dbc index 98125aaa..1833d7c5 100644 --- a/opendbc_repo/opendbc/dbc/vw_meb.dbc +++ b/opendbc_repo/opendbc/dbc/vw_meb.dbc @@ -169,7 +169,7 @@ BO_ 192 EM1_01: 32 XXX SG_ Schubbetrieb : 79|1@0+ (1,0) [0|1] "" XXX BO_ 219 AWV_03: 48 XXX - SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX + SG_ XCHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX SG_ FCW_Active : 64|1@0+ (1,0) [0|1] "" XXX SG_ Pre_Brake_Fill : 76|1@0+ (1,0) [0|1] "" XXX @@ -180,8 +180,8 @@ BO_ 247 MEB_HVEM_02: 8 XXX SG_ NEW_SIGNAL_3 : 44|10@1+ (1,0) [0|7] "" XXX SG_ NEW_SIGNAL_2 : 54|7@1+ (1,0) [0|3] "" XXX -BO_ 252 ESC_51: 48 XXX - SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX +BO_ 252 ESC_51: 64 XXX + SG_ XCHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX SG_ AEB_Breaking_01 : 24|8@1+ (1,0) [0|255] "" XXX SG_ AEB_Breaking_02 : 32|8@1+ (1,0) [0|255] "" XXX @@ -223,7 +223,7 @@ BO_ 253 ESP_21: 8 Gateway SG_ ESC_Neutralschaltung : 63|1@1+ (1,0) [0|1] "" Vector__XXX BO_ 258 ESC_50: 48 XXX - SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX + SG_ XCHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX SG_ Lateral_Accel : 16|8@1+ (0.15,-18.9) [0|255] "Unit_MeterPerSquareSecond" XXX SG_ Longitudinal_Accel : 24|10@1+ (0.03125,-16) [0|255] "Unit_MeterPerSquareSecond" XXX @@ -242,8 +242,8 @@ BO_ 261 VMM_01: 8 XXX SG_ NEW_SIGNAL_1 : 40|2@1+ (1,0) [0|3] "" XXX SG_ Brake : 53|7@1+ (1,0) [0|3] "" XXX -BO_ 267 Motor_51: 32 XXX - SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX +BO_ 267 Motor_51: 48 XXX + SG_ XCHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX SG_ Accel_Pedal_Pressure : 12|9@1+ (0.4,0) [0|255] "" XXX SG_ Accel_Low_Pressed_Support : 21|1@1+ (1,0) [0|7] "" XXX @@ -319,7 +319,7 @@ BO_ 299 GRA_ACC_01: 8 Gateway BO_ 312 IPA_01: 32 XXX BO_ 313 VMM_02: 32 XXX - SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX + SG_ XCHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX SG_ Brake_Pressed_1 : 16|1@0+ (1,0) [0|1] "" XXX SG_ Brake_Pressed_2 : 27|1@0+ (1,0) [0|1] "" XXX @@ -330,7 +330,7 @@ BO_ 313 VMM_02: 32 XXX SG_ Brake_Pressure : 76|11@1+ (1,0) [0|100] "" XXX BO_ 317 QFK_01: 32 XXX - SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX + SG_ XCHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX SG_ NEW_SIGNAL_5 : 12|1@0+ (1,0) [0|1] "" XXX SG_ NEW_SIGNAL_9 : 14|1@0+ (1,0) [0|1] "" XXX diff --git a/opendbc_repo/opendbc/safety/safety.h b/opendbc_repo/opendbc/safety/safety.h index ced9f3b8..99859796 100644 --- a/opendbc_repo/opendbc/safety/safety.h +++ b/opendbc_repo/opendbc/safety/safety.h @@ -25,6 +25,7 @@ // CAN-FD only safety modes #ifdef CANFD #include "safety/safety_hyundai_canfd.h" +#include "safety/safety_volkswagen_meb.h" #endif // from cereal.car.CarParams.SafetyModel @@ -417,6 +418,9 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { {SAFETY_RIVIAN, &rivian_hooks}, #ifdef CANFD {SAFETY_HYUNDAI_CANFD, &hyundai_canfd_hooks}, +#ifdef ALLOW_DEBUG + {SAFETY_VOLKSWAGEN_MEB, &volkswagen_meb_hooks}, +#endif #endif #ifdef ALLOW_DEBUG {SAFETY_TESLA, &tesla_hooks}, @@ -643,13 +647,21 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee uint32_t ts = microsecond_timer_get(); if (controls_allowed) { + // Some safety models support variable torque limit based on vehicle speed + int max_torque = limits.max_torque; + if (limits.dynamic_max_torque) { + const float fudged_speed = (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.; + max_torque = interpolate(limits.max_torque_lookup, fudged_speed) + 1; + max_torque = CLAMP(max_torque, -limits.max_torque, limits.max_torque); + } + // *** global torque limit check *** - violation |= max_limit_check(desired_torque, limits.max_steer, -limits.max_steer); + violation |= max_limit_check(desired_torque, max_torque, -max_torque); // *** torque rate limit check *** if (limits.type == TorqueDriverLimited) { violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver, - limits.max_steer, limits.max_rate_up, limits.max_rate_down, + max_torque, limits.max_rate_up, limits.max_rate_down, limits.driver_torque_allowance, limits.driver_torque_multiplier); } else { violation |= dist_to_meas_check(desired_torque, desired_torque_last, &torque_meas, @@ -662,7 +674,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee // every RT_INTERVAL set the new limits uint32_t ts_elapsed = get_ts_elapsed(ts, ts_torque_check_last); - if (ts_elapsed > limits.max_rt_interval) { + if (ts_elapsed > MAX_TORQUE_RT_INTERVAL) { rt_torque_last = desired_torque; ts_torque_check_last = ts; } diff --git a/opendbc_repo/opendbc/safety/safety/safety_chrysler.h b/opendbc_repo/opendbc/safety/safety/safety_chrysler.h index c581fce8..67bda562 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_chrysler.h +++ b/opendbc_repo/opendbc/safety/safety/safety_chrysler.h @@ -104,9 +104,8 @@ static void chrysler_rx_hook(const CANPacket_t *to_push) { static bool chrysler_tx_hook(const CANPacket_t *to_send) { const TorqueSteeringLimits CHRYSLER_STEERING_LIMITS = { - .max_steer = 261, + .max_torque = 261, .max_rt_delta = 112, - .max_rt_interval = 250000, .max_rate_up = 3, .max_rate_down = 3, .max_torque_error = 80, @@ -114,9 +113,8 @@ static bool chrysler_tx_hook(const CANPacket_t *to_send) { }; const TorqueSteeringLimits CHRYSLER_RAM_DT_STEERING_LIMITS = { - .max_steer = 350, + .max_torque = 350, .max_rt_delta = 112, - .max_rt_interval = 250000, .max_rate_up = 6, .max_rate_down = 6, .max_torque_error = 80, @@ -124,9 +122,8 @@ static bool chrysler_tx_hook(const CANPacket_t *to_send) { }; const TorqueSteeringLimits CHRYSLER_RAM_HD_STEERING_LIMITS = { - .max_steer = 361, + .max_torque = 361, .max_rt_delta = 182, - .max_rt_interval = 250000, .max_rate_up = 14, .max_rate_down = 14, .max_torque_error = 80, diff --git a/opendbc_repo/opendbc/safety/safety/safety_ford.h b/opendbc_repo/opendbc/safety/safety/safety_ford.h index b4a4808d..f7943562 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_ford.h +++ b/opendbc_repo/opendbc/safety/safety/safety_ford.h @@ -69,7 +69,6 @@ static uint32_t ford_compute_checksum(const CANPacket_t *to_push) { chksum = 0xFFU - chksum; } else { } - return chksum; } @@ -154,6 +153,7 @@ static void ford_rx_hook(const CANPacket_t *to_push) { // Signal: Veh_V_ActlEng float filtered_pcm_speed = ((GET_BYTE(to_push, 6) << 8) | GET_BYTE(to_push, 7)) * 0.01 / 3.6; bool is_invalid_speed = ABS(filtered_pcm_speed - ((float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR)) > FORD_MAX_SPEED_DELTA; + // TODO: this should generically cause rx valid to fall until re-enable if (is_invalid_speed) { controls_allowed = false; } diff --git a/opendbc_repo/opendbc/safety/safety/safety_gm.h b/opendbc_repo/opendbc/safety/safety/safety_gm.h index fb12c49b..17e08995 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_gm.h +++ b/opendbc_repo/opendbc/safety/safety/safety_gm.h @@ -98,13 +98,12 @@ static void gm_rx_hook(const CANPacket_t *to_push) { static bool gm_tx_hook(const CANPacket_t *to_send) { const TorqueSteeringLimits GM_STEERING_LIMITS = { - .max_steer = 300, + .max_torque = 300, .max_rate_up = 10, .max_rate_down = 15, .driver_torque_allowance = 65, .driver_torque_multiplier = 4, .max_rt_delta = 128, - .max_rt_interval = 250000, .type = TorqueDriverLimited, }; @@ -135,7 +134,8 @@ static bool gm_tx_hook(const CANPacket_t *to_send) { // GAS/REGEN: safety check if (addr == 0x2CB) { bool apply = GET_BIT(to_send, 0U); - int gas_regen = ((GET_BYTE(to_send, 2) & 0x7FU) << 5) + ((GET_BYTE(to_send, 3) & 0xF8U) >> 3); + // convert float CAN signal to an int for gas checks: 22534 / 0.125 = 180272 + int gas_regen = (((GET_BYTE(to_send, 1) & 0x7U) << 16) | (GET_BYTE(to_send, 2) << 8) | GET_BYTE(to_send, 3)) - 180272U; bool violation = false; // Allow apply bit in pre-enabled and overriding states @@ -189,10 +189,13 @@ static safety_config gm_init(uint16_t param) { const uint16_t GM_PARAM_HW_CAM = 1; const uint16_t GM_PARAM_EV = 4; + // common safety checks assume unscaled integer values + static const int GM_GAS_TO_CAN = 8; // 1 / 0.125 + static const LongitudinalLimits GM_ASCM_LONG_LIMITS = { - .max_gas = 3072, - .min_gas = 1404, - .inactive_gas = 1404, + .max_gas = 1018 * GM_GAS_TO_CAN, + .min_gas = -650 * GM_GAS_TO_CAN, + .inactive_gas = -650 * GM_GAS_TO_CAN, .max_brake = 400, }; @@ -202,9 +205,9 @@ static safety_config gm_init(uint16_t param) { static const LongitudinalLimits GM_CAM_LONG_LIMITS = { - .max_gas = 3400, - .min_gas = 1514, - .inactive_gas = 1554, + .max_gas = 1346 * GM_GAS_TO_CAN, + .min_gas = -540 * GM_GAS_TO_CAN, + .inactive_gas = -500 * GM_GAS_TO_CAN, .max_brake = 400, }; diff --git a/opendbc_repo/opendbc/safety/safety/safety_hyundai.h b/opendbc_repo/opendbc/safety/safety/safety_hyundai.h index e56c123f..3b656488 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_hyundai.h +++ b/opendbc_repo/opendbc/safety/safety/safety_hyundai.h @@ -4,11 +4,10 @@ #include "safety_hyundai_common.h" #define HYUNDAI_LIMITS(steer, rate_up, rate_down) { \ - .max_steer = (steer), \ + .max_torque = (steer), \ .max_rate_up = (rate_up), \ .max_rate_down = (rate_down), \ .max_rt_delta = 112, \ - .max_rt_interval = 250000, \ .driver_torque_allowance = 50, \ .driver_torque_multiplier = 2, \ .type = TorqueDriverLimited, \ diff --git a/opendbc_repo/opendbc/safety/safety/safety_hyundai_canfd.h b/opendbc_repo/opendbc/safety/safety/safety_hyundai_canfd.h index 7e0cdead..73192bd6 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_hyundai_canfd.h +++ b/opendbc_repo/opendbc/safety/safety/safety_hyundai_canfd.h @@ -139,9 +139,8 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *to_push) { static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send) { const TorqueSteeringLimits HYUNDAI_CANFD_STEERING_LIMITS = { - .max_steer = 270, + .max_torque = 270, .max_rt_delta = 112, - .max_rt_interval = 250000, .max_rate_up = 2, .max_rate_down = 3, .driver_torque_allowance = 250, diff --git a/opendbc_repo/opendbc/safety/safety/safety_mazda.h b/opendbc_repo/opendbc/safety/safety/safety_mazda.h index 7159729a..440b3948 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_mazda.h +++ b/opendbc_repo/opendbc/safety/safety/safety_mazda.h @@ -50,11 +50,10 @@ static void mazda_rx_hook(const CANPacket_t *to_push) { static bool mazda_tx_hook(const CANPacket_t *to_send) { const TorqueSteeringLimits MAZDA_STEERING_LIMITS = { - .max_steer = 800, + .max_torque = 800, .max_rate_up = 10, .max_rate_down = 25, .max_rt_delta = 300, - .max_rt_interval = 250000, .driver_torque_multiplier = 1, .driver_torque_allowance = 15, .type = TorqueDriverLimited, diff --git a/opendbc_repo/opendbc/safety/safety/safety_rivian.h b/opendbc_repo/opendbc/safety/safety/safety_rivian.h index 0afb3090..3a125904 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_rivian.h +++ b/opendbc_repo/opendbc/safety/safety/safety_rivian.h @@ -2,8 +2,78 @@ #include "safety_declarations.h" +#define RIVIAN_MAX_SPEED_DELTA 2.0 // m/s + static bool rivian_longitudinal = false; +static uint8_t rivian_get_counter(const CANPacket_t *to_push) { + int addr = GET_ADDR(to_push); + + uint8_t cnt = 0; + if ((addr == 0x208) || (addr == 0x150)) { + // Signal: ESP_Status_Counter, VDM_PropStatus_Counter + cnt = GET_BYTE(to_push, 1) & 0xFU; + } else { + } + return cnt; +} + +static uint32_t rivian_get_checksum(const CANPacket_t *to_push) { + int addr = GET_ADDR(to_push); + + uint8_t chksum = 0; + if ((addr == 0x208) || (addr == 0x150)) { + // Signal: ESP_Status_Checksum, VDM_PropStatus_Checksum + chksum = GET_BYTE(to_push, 0); + } else { + } + return chksum; +} + +static uint8_t _rivian_compute_checksum(const CANPacket_t *to_push, uint8_t poly, uint8_t xor_output) { + int len = GET_LEN(to_push); + + uint8_t crc = 0; + // Skip the checksum byte + for (int i = 1; i < len; i++) { + crc ^= GET_BYTE(to_push, i); + for (int j = 0; j < 8; j++) { + if ((crc & 0x80U) != 0U) { + crc = (crc << 1) ^ poly; + } else { + crc <<= 1; + } + } + } + return crc ^ xor_output; +} + +static uint32_t rivian_compute_checksum(const CANPacket_t *to_push) { + int addr = GET_ADDR(to_push); + + uint8_t chksum = 0; + if (addr == 0x208) { + chksum = _rivian_compute_checksum(to_push, 0x1D, 0xB1); + } else if (addr == 0x150) { + chksum = _rivian_compute_checksum(to_push, 0x1D, 0x9A); + } else { + } + return chksum; +} + +static bool rivian_get_quality_flag_valid(const CANPacket_t *to_push) { + int addr = GET_ADDR(to_push); + + bool valid = false; + if (addr == 0x208) { + valid = ((GET_BYTE(to_push, 3) >> 3) & 0x3U) == 0x1U; // ESP_Vehicle_Speed_Q + } else if (addr == 0x150) { + valid = (GET_BYTE(to_push, 1) >> 6) == 0x1U; // VDM_VehicleSpeedQ + } else { + } + return valid; +} + static void rivian_rx_hook(const CANPacket_t *to_push) { int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); @@ -11,7 +81,22 @@ static void rivian_rx_hook(const CANPacket_t *to_push) { if (bus == 0) { // Vehicle speed if (addr == 0x208) { - vehicle_moving = GET_BYTE(to_push, 6) | GET_BYTE(to_push, 7); + float speed = ((GET_BYTE(to_push, 6) << 8) | GET_BYTE(to_push, 7)) * 0.01; + vehicle_moving = speed > 0.0; + UPDATE_VEHICLE_SPEED(speed / 3.6); + } + + // Gas pressed and second speed source for variable torque limit + if (addr == 0x150) { + gas_pressed = GET_BYTE(to_push, 3) | (GET_BYTE(to_push, 4) & 0xC0U); + + // Disable controls if speeds from VDM and ESP ECUs are too far apart. + float vdm_speed = ((GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6)) * 0.01 / 3.6; + bool is_invalid_speed = ABS(vdm_speed - ((float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR)) > RIVIAN_MAX_SPEED_DELTA; + // TODO: this should generically cause rx valid to fall until re-enable + if (is_invalid_speed) { + controls_allowed = false; + } } // Driver torque @@ -20,11 +105,6 @@ static void rivian_rx_hook(const CANPacket_t *to_push) { update_sample(&torque_driver, torque_driver_new); } - // Gas pressed - if (addr == 0x150) { - gas_pressed = GET_BYTE(to_push, 3) | (GET_BYTE(to_push, 4) & 0xC0U); - } - // Brake pressed if (addr == 0x38f) { brake_pressed = GET_BIT(to_push, 23U); @@ -41,12 +121,17 @@ static void rivian_rx_hook(const CANPacket_t *to_push) { } static bool rivian_tx_hook(const CANPacket_t *to_send) { + // Rivian utilizes more torque at low speed to maintain the same lateral accel const TorqueSteeringLimits RIVIAN_STEERING_LIMITS = { - .max_steer = 250, + .max_torque = 350, + .dynamic_max_torque = true, + .max_torque_lookup = { + {9., 17., 17.}, + {350, 250, 250}, + }, .max_rate_up = 3, .max_rate_down = 5, .max_rt_delta = 125, - .max_rt_interval = 250000, .driver_torque_multiplier = 2, .driver_torque_allowance = 100, .type = TorqueDriverLimited, @@ -125,9 +210,9 @@ static safety_config rivian_init(uint16_t param) { static const CanMsg RIVIAN_LONG_TX_MSGS[] = {{0x120, 0, 8, true}, {0x321, 2, 7, false}, {0x160, 0, 5, true}}; static RxCheck rivian_rx_checks[] = { - {.msg = {{0x208, 0, 8, .frequency = 50U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, // ESP_Status (speed) + {.msg = {{0x208, 0, 8, .frequency = 50U, .max_counter = 14U, .quality_flag = true}, { 0 }, { 0 }}}, // ESP_Status (speed) + {.msg = {{0x150, 0, 7, .frequency = 50U, .max_counter = 14U, .quality_flag = true}, { 0 }, { 0 }}}, // VDM_PropStatus (gas pedal & 2nd speed) {.msg = {{0x380, 0, 5, .frequency = 100U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, // EPAS_SystemStatus (driver torque) - {.msg = {{0x150, 0, 7, .frequency = 50U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, // VDM_PropStatus (gas pedal) {.msg = {{0x38f, 0, 6, .frequency = 50U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, // iBESP2 (brakes) {.msg = {{0x100, 2, 8, .frequency = 100U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, // ACM_Status (cruise state) }; @@ -150,4 +235,8 @@ const safety_hooks rivian_hooks = { .rx = rivian_rx_hook, .tx = rivian_tx_hook, .fwd = rivian_fwd_hook, + .get_counter = rivian_get_counter, + .get_checksum = rivian_get_checksum, + .compute_checksum = rivian_compute_checksum, + .get_quality_flag_valid = rivian_get_quality_flag_valid, }; diff --git a/opendbc_repo/opendbc/safety/safety/safety_subaru.h b/opendbc_repo/opendbc/safety/safety/safety_subaru.h index 8c597760..3b43d392 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_subaru.h +++ b/opendbc_repo/opendbc/safety/safety/safety_subaru.h @@ -4,9 +4,8 @@ #define SUBARU_STEERING_LIMITS_GENERATOR(steer_max, rate_up, rate_down) \ { \ - .max_steer = (steer_max), \ + .max_torque = (steer_max), \ .max_rt_delta = 940, \ - .max_rt_interval = 250000, \ .max_rate_up = (rate_up), \ .max_rate_down = (rate_down), \ .driver_torque_multiplier = 50, \ diff --git a/opendbc_repo/opendbc/safety/safety/safety_subaru_preglobal.h b/opendbc_repo/opendbc/safety/safety/safety_subaru_preglobal.h index 8c229a04..b985c7b7 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_subaru_preglobal.h +++ b/opendbc_repo/opendbc/safety/safety/safety_subaru_preglobal.h @@ -55,9 +55,8 @@ static void subaru_preglobal_rx_hook(const CANPacket_t *to_push) { static bool subaru_preglobal_tx_hook(const CANPacket_t *to_send) { const TorqueSteeringLimits SUBARU_PG_STEERING_LIMITS = { - .max_steer = 2047, + .max_torque = 2047, .max_rt_delta = 940, - .max_rt_interval = 250000, .max_rate_up = 50, .max_rate_down = 70, .driver_torque_multiplier = 10, diff --git a/opendbc_repo/opendbc/safety/safety/safety_toyota.h b/opendbc_repo/opendbc/safety/safety/safety_toyota.h index 7185001d..d9210ec8 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_toyota.h +++ b/opendbc_repo/opendbc/safety/safety/safety_toyota.h @@ -155,19 +155,18 @@ static void toyota_rx_hook(const CANPacket_t *to_push) { static bool toyota_tx_hook(const CANPacket_t *to_send) { const TorqueSteeringLimits TOYOTA_TORQUE_STEERING_LIMITS = { - .max_steer = 1500, + .max_torque = 1500, .max_rate_up = 15, // ramp up slow .max_rate_down = 25, // ramp down fast .max_torque_error = 350, // max torque cmd in excess of motor torque .max_rt_delta = 450, // the real time limit is 1800/sec, a 20% buffer - .max_rt_interval = 250000, .type = TorqueMotorLimited, // the EPS faults when the steering angle rate is above a certain threshold for too long. to prevent this, // we allow setting STEER_REQUEST bit to 0 while maintaining the requested torque value for a single frame .min_valid_request_frames = 18, .max_invalid_request_frames = 1, - .min_valid_request_rt_interval = 170000, // 170ms; a ~10% buffer on cutting every 19 frames + .min_valid_request_rt_interval = 171000, // 171ms; a ~10% buffer on cutting every 19 frames .has_steer_req_tolerance = true, }; diff --git a/opendbc_repo/opendbc/safety/safety/safety_volkswagen_common.h b/opendbc_repo/opendbc/safety/safety/safety_volkswagen_common.h index 1285bb88..d75a5861 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_volkswagen_common.h +++ b/opendbc_repo/opendbc/safety/safety/safety_volkswagen_common.h @@ -17,13 +17,26 @@ bool volkswagen_resume_button_prev = false; #define MSG_LH_EPS_03 0x09F // RX from EPS, for driver steering torque #define MSG_ESP_19 0x0B2 // RX from ABS, for wheel speeds +#define MSG_ESC_51 0x0FC // RX, for wheel speeds +#define MSG_ESC_50 0x102 // RX, for yaw rate #define MSG_ESP_05 0x106 // RX from ABS, for brake switch state +#define MSG_Motor_51 0x10B // RX for TSK state #define MSG_TSK_06 0x120 // RX from ECU, for ACC status from drivetrain coordinator #define MSG_MOTOR_20 0x121 // RX from ECU, for driver throttle input #define MSG_ACC_06 0x122 // TX by OP, ACC control instructions to the drivetrain coordinator #define MSG_HCA_01 0x126 // TX by OP, Heading Control Assist steering torque #define MSG_GRA_ACC_01 0x12B // TX by OP, ACC control buttons for cancel/resume #define MSG_ACC_07 0x12E // TX by OP, ACC control instructions to the drivetrain coordinator +#define MSG_VMM_02 0x139 // RX, for ESP hold management +#define MSG_QFK_01 0x13D // RX, for steering angle +#define MSG_Motor_54 0x14C // RX, for accel pedal +#define MSG_ACC_18 0x14D // RX from ECU, for ACC status +#define MSG_EA_01 0x1A4 // TX, for EA mitigation +#define MSG_EA_02 0x1F0 // TX, for EA mitigation +#define MSG_EML_06 0x20A // RX, for yaw rate +#define MSG_TA_01 0x26B // TX for Travel Assist status +#define MSG_MEB_ACC_01 0x300 // RX from ECU, for ACC status +#define MSG_HCA_03 0x303 // TX by OP, Heading Control Assist steering torque #define MSG_ACC_02 0x30C // TX by OP, ACC HUD data to the instrument cluster #define MSG_LDW_02 0x397 // TX by OP, Lane line recognition and text alerts #define MSG_MOTOR_14 0x3BE // RX from ECU, for brake switch status @@ -56,12 +69,22 @@ static uint32_t volkswagen_mqb_meb_compute_crc(const CANPacket_t *to_push) { crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter]; } else if (addr == MSG_ESP_05) { crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter]; + } else if (addr == MSG_QFK_01) { + crc ^= (uint8_t[]){0x20,0xCA,0x68,0xD5,0x1B,0x31,0xE2,0xDA,0x08,0x0A,0xD4,0xDE,0x9C,0xE4,0x35,0x5B}[counter]; } else if (addr == MSG_TSK_06) { crc ^= (uint8_t[]){0xC4,0xE2,0x4F,0xE4,0xF8,0x2F,0x56,0x81,0x9F,0xE5,0x83,0x44,0x05,0x3F,0x97,0xDF}[counter]; } else if (addr == MSG_MOTOR_20) { crc ^= (uint8_t[]){0xE9,0x65,0xAE,0x6B,0x7B,0x35,0xE5,0x5F,0x4E,0xC7,0x86,0xA2,0xBB,0xDD,0xEB,0xB4}[counter]; } else if (addr == MSG_GRA_ACC_01) { crc ^= (uint8_t[]){0x6A,0x38,0xB4,0x27,0x22,0xEF,0xE1,0xBB,0xF8,0x80,0x84,0x49,0xC7,0x9E,0x1E,0x2B}[counter]; + } else if (addr == MSG_ESC_51) { + crc ^= (uint8_t[]){0x77,0x5C,0xA0,0x89,0x4B,0x7C,0xBB,0xD6,0x1F,0x6C,0x4F,0xF6,0x20,0x2B,0x43,0xDD}[counter]; + } else if (addr == MSG_Motor_54) { + crc ^= (uint8_t[]){0x16,0x35,0x59,0x15,0x9A,0x2A,0x97,0xB8,0x0E,0x4E,0x30,0xCC,0xB3,0x07,0x01,0xAD}[counter]; + } else if (addr == MSG_Motor_51) { + crc ^= (uint8_t[]){0x77,0x5C,0xA0,0x89,0x4B,0x7C,0xBB,0xD6,0x1F,0x6C,0x4F,0xF6,0x20,0x2B,0x43,0xDD}[counter]; + } else if (addr == MSG_MOTOR_14) { + crc ^= (uint8_t[]){0x1F,0x28,0xC6,0x85,0xE6,0xF8,0xB0,0x19,0x5B,0x64,0x35,0x21,0xE4,0xF7,0x9C,0x24}[counter]; } else { // Undefined CAN message, CRC check expected to fail } diff --git a/opendbc_repo/opendbc/safety/safety/safety_volkswagen_mqb.h b/opendbc_repo/opendbc/safety/safety/safety_volkswagen_mqb.h index f3a18516..ebbff496 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_volkswagen_mqb.h +++ b/opendbc_repo/opendbc/safety/safety/safety_volkswagen_mqb.h @@ -127,9 +127,8 @@ static void volkswagen_mqb_rx_hook(const CANPacket_t *to_push) { static bool volkswagen_mqb_tx_hook(const CANPacket_t *to_send) { // lateral limits const TorqueSteeringLimits VOLKSWAGEN_MQB_STEERING_LIMITS = { - .max_steer = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated) + .max_torque = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated) .max_rt_delta = 75, // 4 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 50 ; 50 * 1.5 for safety pad = 75 - .max_rt_interval = 250000, // 250ms between real time checks .max_rate_up = 4, // 2.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s) .max_rate_down = 10, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s) .driver_torque_allowance = 80, diff --git a/opendbc_repo/opendbc/safety/safety/safety_volkswagen_pq.h b/opendbc_repo/opendbc/safety/safety/safety_volkswagen_pq.h index 3f0d8269..c4cf014f 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_volkswagen_pq.h +++ b/opendbc_repo/opendbc/safety/safety/safety_volkswagen_pq.h @@ -154,9 +154,8 @@ static void volkswagen_pq_rx_hook(const CANPacket_t *to_push) { static bool volkswagen_pq_tx_hook(const CANPacket_t *to_send) { // lateral limits const TorqueSteeringLimits VOLKSWAGEN_PQ_STEERING_LIMITS = { - .max_steer = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated) + .max_torque = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated) .max_rt_delta = 113, // 6 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 75 ; 125 * 1.5 for safety pad = 113 - .max_rt_interval = 250000, // 250ms between real time checks .max_rate_up = 6, // 3.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s) .max_rate_down = 10, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s) .driver_torque_multiplier = 3, diff --git a/opendbc_repo/opendbc/safety/safety_declarations.h b/opendbc_repo/opendbc/safety/safety_declarations.h index c9fe9131..85002f3f 100644 --- a/opendbc_repo/opendbc/safety/safety_declarations.h +++ b/opendbc_repo/opendbc/safety/safety_declarations.h @@ -33,6 +33,7 @@ extern const int MAX_WRONG_COUNTERS; #define MAX_SAMPLE_VALS 6 // used to represent floating point vehicle speed in a sample_t #define VEHICLE_SPEED_FACTOR 1000.0 +#define MAX_TORQUE_RT_INTERVAL 250000U // sample struct that keeps 6 samples in memory @@ -62,11 +63,13 @@ typedef enum { typedef struct { // torque cmd limits - const int max_steer; + const int max_torque; // this upper limit is always enforced + const bool dynamic_max_torque; // use max_torque_lookup to apply torque limit based on speed + const struct lookup_t max_torque_lookup; + const int max_rate_up; const int max_rate_down; - const int max_rt_delta; - const uint32_t max_rt_interval; + const int max_rt_delta; // max change in torque per 250ms interval (MAX_TORQUE_RT_INTERVAL) const SteeringControlType type; @@ -293,3 +296,4 @@ extern const safety_hooks toyota_hooks; extern const safety_hooks volkswagen_mqb_hooks; extern const safety_hooks volkswagen_pq_hooks; extern const safety_hooks rivian_hooks; +extern const safety_hooks volkswagen_meb_hooks; diff --git a/opendbc_repo/opendbc/safety/tests/common.py b/opendbc_repo/opendbc/safety/tests/common.py index f087fd52..6ca8d585 100644 --- a/opendbc_repo/opendbc/safety/tests/common.py +++ b/opendbc_repo/opendbc/safety/tests/common.py @@ -166,6 +166,7 @@ class LongitudinalGasBrakeSafetyTest(PandaSafetyTestBase, abc.ABC): MIN_GAS: int = 0 MAX_GAS: int | None = None INACTIVE_GAS = 0 + MIN_POSSIBLE_GAS: int = 0. MAX_POSSIBLE_GAS: int | None = None def test_gas_brake_limits_correct(self): @@ -187,16 +188,17 @@ class LongitudinalGasBrakeSafetyTest(PandaSafetyTestBase, abc.ABC): self._generic_limit_safety_check(self._send_brake_msg, self.MIN_BRAKE, self.MAX_BRAKE, 0, self.MAX_POSSIBLE_BRAKE, 1) def test_gas_safety_check(self): - self._generic_limit_safety_check(self._send_gas_msg, self.MIN_GAS, self.MAX_GAS, 0, self.MAX_POSSIBLE_GAS, 1, self.INACTIVE_GAS) + self._generic_limit_safety_check(self._send_gas_msg, self.MIN_GAS, self.MAX_GAS, self.MIN_POSSIBLE_GAS, self.MAX_POSSIBLE_GAS, 1, self.INACTIVE_GAS) class TorqueSteeringSafetyTestBase(PandaSafetyTestBase, abc.ABC): MAX_RATE_UP = 0 MAX_RATE_DOWN = 0 - MAX_TORQUE = 0 + MAX_TORQUE_LOOKUP: tuple[list[float], list[int]] = ([0], [0]) + DYNAMIC_MAX_TORQUE = False MAX_RT_DELTA = 0 - RT_INTERVAL = 0 + RT_INTERVAL = 250000 NO_STEER_REQ_BIT = False @@ -206,23 +208,53 @@ class TorqueSteeringSafetyTestBase(PandaSafetyTestBase, abc.ABC): cls.safety = None raise unittest.SkipTest + @property + def MAX_TORQUE(self): + return max(self.MAX_TORQUE_LOOKUP[1]) + + @property + def _torque_speed_range(self): + if not self.DYNAMIC_MAX_TORQUE: + return [0] + else: + # test with more precision inside breakpoint range + min_speed = min(self.MAX_TORQUE_LOOKUP[0]) + max_speed = max(self.MAX_TORQUE_LOOKUP[0]) + return np.concatenate([np.arange(0, min_speed, 5), np.arange(min_speed, max_speed, 0.5), np.arange(max_speed, 40, 5)]) + + def _get_max_torque(self, speed): + # matches safety fudge + torque = int(np.interp(speed - 1, self.MAX_TORQUE_LOOKUP[0], self.MAX_TORQUE_LOOKUP[1]) + 1) + return min(torque, self.MAX_TORQUE) + @abc.abstractmethod def _torque_cmd_msg(self, torque, steer_req=1): pass + @abc.abstractmethod + def _speed_msg(self, speed): + pass + + def _reset_speed_measurement(self, speed): + for _ in range(MAX_SAMPLE_VALS): + self._rx(self._speed_msg(speed)) + def _set_prev_torque(self, t): self.safety.set_desired_torque_last(t) self.safety.set_rt_torque_last(t) def test_steer_safety_check(self): - for enabled in [0, 1]: - for t in range(int(-self.MAX_TORQUE * 1.5), int(self.MAX_TORQUE * 1.5)): - self.safety.set_controls_allowed(enabled) - self._set_prev_torque(t) - if abs(t) > self.MAX_TORQUE or (not enabled and abs(t) > 0): - self.assertFalse(self._tx(self._torque_cmd_msg(t))) - else: - self.assertTrue(self._tx(self._torque_cmd_msg(t))) + for speed in self._torque_speed_range: + self._reset_speed_measurement(speed) + max_torque = self._get_max_torque(speed) + for enabled in [0, 1]: + for t in range(int(-max_torque * 1.5), int(max_torque * 1.5)): + self.safety.set_controls_allowed(enabled) + self._set_prev_torque(t) + if abs(t) > max_torque or (not enabled and abs(t) > 0): + self.assertFalse(self._tx(self._torque_cmd_msg(t))) + else: + self.assertTrue(self._tx(self._torque_cmd_msg(t))) def test_non_realtime_limit_up(self): self.safety.set_controls_allowed(True) @@ -266,7 +298,12 @@ class SteerRequestCutSafetyTest(TorqueSteeringSafetyTestBase, abc.ABC): # Safety around steering request bit mismatch tolerance MIN_VALID_STEERING_FRAMES: int MAX_INVALID_STEERING_FRAMES: int - MIN_VALID_STEERING_RT_INTERVAL: int + STEER_STEP: int = 1 + + @property + def MIN_VALID_STEERING_RT_INTERVAL(self): + # a ~10% buffer + return int((self.MIN_VALID_STEERING_FRAMES + 1) * self.STEER_STEP * 10000 * 0.9) def test_steer_req_bit_frames(self): """ @@ -389,40 +426,44 @@ class DriverTorqueSteeringSafetyTest(TorqueSteeringSafetyTestBase, abc.ABC): # Tests down limits and driver torque blending self.safety.set_controls_allowed(True) - # Cannot stay at MAX_TORQUE if above DRIVER_TORQUE_ALLOWANCE - for sign in [-1, 1]: - for driver_torque in np.arange(0, self.DRIVER_TORQUE_ALLOWANCE * 2, 1): - self._reset_torque_driver_measurement(-driver_torque * sign) - self._set_prev_torque(self.MAX_TORQUE * sign) - should_tx = abs(driver_torque) <= self.DRIVER_TORQUE_ALLOWANCE - self.assertEqual(should_tx, self._tx(self._torque_cmd_msg(self.MAX_TORQUE * sign))) + for speed in self._torque_speed_range: + self._reset_speed_measurement(speed) + max_torque = self._get_max_torque(speed) - # arbitrary high driver torque to ensure max steer torque is allowed - max_driver_torque = int(self.MAX_TORQUE / self.DRIVER_TORQUE_FACTOR + self.DRIVER_TORQUE_ALLOWANCE + 1) + # Cannot stay at MAX_TORQUE if above DRIVER_TORQUE_ALLOWANCE + for sign in [-1, 1]: + for driver_torque in np.arange(0, self.DRIVER_TORQUE_ALLOWANCE * 2, 1): + self._reset_torque_driver_measurement(-driver_torque * sign) + self._set_prev_torque(max_torque * sign) + should_tx = abs(driver_torque) <= self.DRIVER_TORQUE_ALLOWANCE + self.assertEqual(should_tx, self._tx(self._torque_cmd_msg(max_torque * sign))) - # spot check some individual cases - for sign in [-1, 1]: - # Ensure we wind down factor units for every unit above allowance - driver_torque = (self.DRIVER_TORQUE_ALLOWANCE + 10) * sign - torque_desired = (self.MAX_TORQUE - 10 * self.DRIVER_TORQUE_FACTOR) * sign - delta = 1 * sign - self._set_prev_torque(torque_desired) - self._reset_torque_driver_measurement(-driver_torque) - self.assertTrue(self._tx(self._torque_cmd_msg(torque_desired))) - self._set_prev_torque(torque_desired + delta) - self._reset_torque_driver_measurement(-driver_torque) - self.assertFalse(self._tx(self._torque_cmd_msg(torque_desired + delta))) + # arbitrary high driver torque to ensure max steer torque is allowed + max_driver_torque = int(max_torque / self.DRIVER_TORQUE_FACTOR + self.DRIVER_TORQUE_ALLOWANCE + 1) - # If we're well past the allowance, minimum wind down is MAX_RATE_DOWN - self._set_prev_torque(self.MAX_TORQUE * sign) - self._reset_torque_driver_measurement(-max_driver_torque * sign) - self.assertTrue(self._tx(self._torque_cmd_msg((self.MAX_TORQUE - self.MAX_RATE_DOWN) * sign))) - self._set_prev_torque(self.MAX_TORQUE * sign) - self._reset_torque_driver_measurement(-max_driver_torque * sign) - self.assertTrue(self._tx(self._torque_cmd_msg(0))) - self._set_prev_torque(self.MAX_TORQUE * sign) - self._reset_torque_driver_measurement(-max_driver_torque * sign) - self.assertFalse(self._tx(self._torque_cmd_msg((self.MAX_TORQUE - self.MAX_RATE_DOWN + 1) * sign))) + # spot check some individual cases + for sign in [-1, 1]: + # Ensure we wind down factor units for every unit above allowance + driver_torque = (self.DRIVER_TORQUE_ALLOWANCE + 10) * sign + torque_desired = (max_torque - 10 * self.DRIVER_TORQUE_FACTOR) * sign + delta = 1 * sign + self._set_prev_torque(torque_desired) + self._reset_torque_driver_measurement(-driver_torque) + self.assertTrue(self._tx(self._torque_cmd_msg(torque_desired))) + self._set_prev_torque(torque_desired + delta) + self._reset_torque_driver_measurement(-driver_torque) + self.assertFalse(self._tx(self._torque_cmd_msg(torque_desired + delta))) + + # If we're well past the allowance, minimum wind down is MAX_RATE_DOWN + self._set_prev_torque(max_torque * sign) + self._reset_torque_driver_measurement(-max_driver_torque * sign) + self.assertTrue(self._tx(self._torque_cmd_msg((max_torque - self.MAX_RATE_DOWN) * sign))) + self._set_prev_torque(max_torque * sign) + self._reset_torque_driver_measurement(-max_driver_torque * sign) + self.assertTrue(self._tx(self._torque_cmd_msg(0))) + self._set_prev_torque(max_torque * sign) + self._reset_torque_driver_measurement(-max_driver_torque * sign) + self.assertFalse(self._tx(self._torque_cmd_msg((max_torque - self.MAX_RATE_DOWN + 1) * sign))) def test_realtime_limits(self): self.safety.set_controls_allowed(True) @@ -479,34 +520,41 @@ class MotorTorqueSteeringSafetyTest(TorqueSteeringSafetyTestBase, abc.ABC): self.safety.set_torque_meas(t, t) def test_torque_absolute_limits(self): - for controls_allowed in [True, False]: - for torque in np.arange(-self.MAX_TORQUE - 1000, self.MAX_TORQUE + 1000, self.MAX_RATE_UP): - self.safety.set_controls_allowed(controls_allowed) - self.safety.set_rt_torque_last(torque) - self.safety.set_torque_meas(torque, torque) - self.safety.set_desired_torque_last(torque - self.MAX_RATE_UP) + for speed in self._torque_speed_range: + self._reset_speed_measurement(speed) + max_torque = self._get_max_torque(speed) + for controls_allowed in [True, False]: + for torque in np.arange(-max_torque - 1000, max_torque + 1000, self.MAX_RATE_UP): + self.safety.set_controls_allowed(controls_allowed) + self.safety.set_rt_torque_last(torque) + self.safety.set_torque_meas(torque, torque) + self.safety.set_desired_torque_last(torque - self.MAX_RATE_UP) - if controls_allowed: - send = (-self.MAX_TORQUE <= torque <= self.MAX_TORQUE) - else: - send = torque == 0 + if controls_allowed: + send = (-max_torque <= torque <= max_torque) + else: + send = torque == 0 - self.assertEqual(send, self._tx(self._torque_cmd_msg(torque))) + self.assertEqual(send, self._tx(self._torque_cmd_msg(torque))) def test_non_realtime_limit_down(self): self.safety.set_controls_allowed(True) - torque_meas = self.MAX_TORQUE - self.MAX_TORQUE_ERROR - 50 + for speed in self._torque_speed_range: + self._reset_speed_measurement(speed) + max_torque = self._get_max_torque(speed) - self.safety.set_rt_torque_last(self.MAX_TORQUE) - self.safety.set_torque_meas(torque_meas, torque_meas) - self.safety.set_desired_torque_last(self.MAX_TORQUE) - self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_TORQUE - self.MAX_RATE_DOWN))) + torque_meas = max_torque - self.MAX_TORQUE_ERROR - 50 - self.safety.set_rt_torque_last(self.MAX_TORQUE) - self.safety.set_torque_meas(torque_meas, torque_meas) - self.safety.set_desired_torque_last(self.MAX_TORQUE) - self.assertFalse(self._tx(self._torque_cmd_msg(self.MAX_TORQUE - self.MAX_RATE_DOWN + 1))) + self.safety.set_rt_torque_last(max_torque) + self.safety.set_torque_meas(torque_meas, torque_meas) + self.safety.set_desired_torque_last(max_torque) + self.assertTrue(self._tx(self._torque_cmd_msg(max_torque - self.MAX_RATE_DOWN))) + + self.safety.set_rt_torque_last(max_torque) + self.safety.set_torque_meas(torque_meas, torque_meas) + self.safety.set_desired_torque_last(max_torque) + self.assertFalse(self._tx(self._torque_cmd_msg(max_torque - self.MAX_RATE_DOWN + 1))) def test_exceed_torque_sensor(self): self.safety.set_controls_allowed(True) @@ -577,7 +625,23 @@ class MotorTorqueSteeringSafetyTest(TorqueSteeringSafetyTestBase, abc.ABC): self.assertEqual(self.safety.get_torque_meas_max(), 0) -class AngleSteeringSafetyTest(PandaSafetyTestBase): +class VehicleSpeedSafetyTest(PandaSafetyTestBase): + @classmethod + def setUpClass(cls): + if cls.__name__ == "VehicleSpeedSafetyTest": + cls.safety = None + raise unittest.SkipTest + + @abc.abstractmethod + def _speed_msg(self, speed): + pass + + def test_vehicle_speed_measurements(self): + # TODO: lower tolerance on these tests + self._common_measurement_test(self._speed_msg, 0, 80, 1, self.safety.get_vehicle_speed_min, self.safety.get_vehicle_speed_max) + + +class AngleSteeringSafetyTest(VehicleSpeedSafetyTest): STEER_ANGLE_MAX: float = 300 DEG_TO_CAN: float @@ -591,10 +655,6 @@ class AngleSteeringSafetyTest(PandaSafetyTestBase): cls.safety = None raise unittest.SkipTest - @abc.abstractmethod - def _speed_msg(self, speed): - pass - @abc.abstractmethod def _angle_cmd_msg(self, angle: float, enabled: bool): pass @@ -615,10 +675,6 @@ class AngleSteeringSafetyTest(PandaSafetyTestBase): for _ in range(MAX_SAMPLE_VALS): self._rx(self._speed_msg(speed)) - def test_vehicle_speed_measurements(self): - # TODO: lower tolerance on these tests - self._common_measurement_test(self._speed_msg, 0, 80, 1, self.safety.get_vehicle_speed_min, self.safety.get_vehicle_speed_max) - def test_steering_angle_measurements(self): self._common_measurement_test(self._angle_meas_msg, -self.STEER_ANGLE_MAX, self.STEER_ANGLE_MAX, self.DEG_TO_CAN, self.safety.get_angle_meas_min, self.safety.get_angle_meas_max) @@ -776,7 +832,8 @@ class PandaSafetyTest(PandaSafetyTestBase): continue if {attr, current_test}.issubset({'TestHyundaiLongitudinalSafety', 'TestHyundaiLongitudinalSafetyCameraSCC', 'TestHyundaiSafetyFCEVLong'}): continue - if {attr, current_test}.issubset({'TestVolkswagenMqbSafety', 'TestVolkswagenMqbStockSafety', 'TestVolkswagenMqbLongSafety'}): + if {attr, current_test}.issubset({'TestVolkswagenMqbSafety', 'TestVolkswagenMqbStockSafety', 'TestVolkswagenMqbLongSafety', + 'TestVolkswagenMebSafety', 'TestVolkswagenMebStockSafety',}): continue # overlapping TX addrs, but they're not actuating messages for either car diff --git a/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py b/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py index d4d31de9..f0e09fcf 100644 --- a/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py +++ b/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py @@ -18,7 +18,7 @@ def is_steering_msg(mode, param, addr): ret = addr == (0x191 if param & ToyotaSafetyFlags.LTA else 0x2E4) elif mode == CarParams.SafetyModel.gm: ret = addr == 384 - elif mode == CarParams.SafetyModel.hyundai: + elif mode in (CarParams.SafetyModel.hyundai, CarParams.SafetyModel.hyundaiLegacy): ret = addr == 832 elif mode == CarParams.SafetyModel.hyundaiCanfd: ret = addr == (0x110 if param & HyundaiSafetyFlags.CANFD_LKA_STEERING_ALT else @@ -52,7 +52,7 @@ def get_steer_value(mode, param, to_send): elif mode == CarParams.SafetyModel.gm: torque = ((to_send.data[0] & 0x7) << 8) | to_send.data[1] torque = to_signed(torque, 11) - elif mode == CarParams.SafetyModel.hyundai: + elif mode in (CarParams.SafetyModel.hyundai, CarParams.SafetyModel.hyundaiLegacy): torque = (((to_send.data[3] & 0x7) << 8) | to_send.data[2]) - 1024 elif mode == CarParams.SafetyModel.hyundaiCanfd: torque = ((to_send.data[5] >> 1) | (to_send.data[6] & 0xF) << 7) - 1024 diff --git a/opendbc_repo/opendbc/safety/tests/test_chrysler.py b/opendbc_repo/opendbc/safety/tests/test_chrysler.py index 854c2f5f..5510e137 100755 --- a/opendbc_repo/opendbc/safety/tests/test_chrysler.py +++ b/opendbc_repo/opendbc/safety/tests/test_chrysler.py @@ -15,9 +15,8 @@ class TestChryslerSafety(common.PandaCarSafetyTest, common.MotorTorqueSteeringSa MAX_RATE_UP = 3 MAX_RATE_DOWN = 3 - MAX_TORQUE = 261 + MAX_TORQUE_LOOKUP = [0], [261] MAX_RT_DELTA = 112 - RT_INTERVAL = 250000 MAX_TORQUE_ERROR = 80 LKAS_ACTIVE_VALUE = 1 @@ -80,7 +79,7 @@ class TestChryslerRamDTSafety(TestChryslerSafety): MAX_RATE_UP = 6 MAX_RATE_DOWN = 6 - MAX_TORQUE = 350 + MAX_TORQUE_LOOKUP = [0], [350] DAS_BUS = 2 @@ -101,7 +100,7 @@ class TestChryslerRamHDSafety(TestChryslerSafety): RELAY_MALFUNCTION_ADDRS = {0: (0x276,)} FWD_BLACKLISTED_ADDRS = {2: [0x275, 0x276]} - MAX_TORQUE = 361 + MAX_TORQUE_LOOKUP = [0], [361] MAX_RATE_UP = 14 MAX_RATE_DOWN = 14 MAX_RT_DELTA = 182 diff --git a/opendbc_repo/opendbc/safety/tests/test_gm.py b/opendbc_repo/opendbc/safety/tests/test_gm.py index ab652472..a92e1173 100755 --- a/opendbc_repo/opendbc/safety/tests/test_gm.py +++ b/opendbc_repo/opendbc/safety/tests/test_gm.py @@ -23,7 +23,8 @@ class GmLongitudinalBase(common.PandaCarSafetyTest, common.LongitudinalGasBrakeS MAX_POSSIBLE_BRAKE = 2 ** 12 MAX_BRAKE = 400 - MAX_POSSIBLE_GAS = 2 ** 12 + MAX_POSSIBLE_GAS = 4000 # reasonably excessive limits, not signal max + MIN_POSSIBLE_GAS = -4000 PCM_CRUISE = False # openpilot can control the PCM state if longitudinal @@ -80,9 +81,8 @@ class TestGmSafetyBase(common.PandaCarSafetyTest, common.DriverTorqueSteeringSaf MAX_RATE_UP = 10 MAX_RATE_DOWN = 15 - MAX_TORQUE = 300 + MAX_TORQUE_LOOKUP = [0], [300] MAX_RT_DELTA = 128 - RT_INTERVAL = 250000 DRIVER_TORQUE_ALLOWANCE = 65 DRIVER_TORQUE_FACTOR = 4 @@ -151,9 +151,9 @@ class TestGmAscmSafety(GmLongitudinalBase, TestGmSafetyBase): FWD_BUS_LOOKUP: dict[int, int] = {} BRAKE_BUS = 2 - MAX_GAS = 3072 - MIN_GAS = 1404 # maximum regen - INACTIVE_GAS = 1404 + MAX_GAS = 1018 + MIN_GAS = -650 # maximum regen + INACTIVE_GAS = -650 def setUp(self): self.packer = CANPackerPanda("gm_global_a_powertrain_generated") @@ -211,9 +211,9 @@ class TestGmCameraLongitudinalSafety(GmLongitudinalBase, TestGmCameraSafetyBase) FWD_BLACKLISTED_ADDRS = {2: [0x180, 0x2CB, 0x370, 0x315], 0: [0x184]} # block LKAS, ACC messages and PSCMStatus BUTTONS_BUS = 0 # rx only - MAX_GAS = 3400 - MIN_GAS = 1514 # maximum regen - INACTIVE_GAS = 1554 + MAX_GAS = 1346 + MIN_GAS = -540 # maximum regen + INACTIVE_GAS = -500 def setUp(self): self.packer = CANPackerPanda("gm_global_a_powertrain_generated") diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai.py b/opendbc_repo/opendbc/safety/tests/test_hyundai.py index fa95fdca..80ec273a 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai.py @@ -53,16 +53,14 @@ class TestHyundaiSafety(HyundaiButtonBase, common.PandaCarSafetyTest, common.Dri MAX_RATE_UP = 3 MAX_RATE_DOWN = 7 - MAX_TORQUE = 384 + MAX_TORQUE_LOOKUP = [0], [384] MAX_RT_DELTA = 112 - RT_INTERVAL = 250000 DRIVER_TORQUE_ALLOWANCE = 50 DRIVER_TORQUE_FACTOR = 2 # Safety around steering req bit MIN_VALID_STEERING_FRAMES = 89 MAX_INVALID_STEERING_FRAMES = 2 - MIN_VALID_STEERING_RT_INTERVAL = 810000 # a ~10% buffer, can send steer up to 110Hz cnt_gas = 0 cnt_speed = 0 @@ -117,7 +115,7 @@ class TestHyundaiSafety(HyundaiButtonBase, common.PandaCarSafetyTest, common.Dri class TestHyundaiSafetyAltLimits(TestHyundaiSafety): MAX_RATE_UP = 2 MAX_RATE_DOWN = 3 - MAX_TORQUE = 270 + MAX_TORQUE_LOOKUP = [0], [270] def setUp(self): self.packer = CANPackerPanda("hyundai_kia_generic") @@ -129,7 +127,7 @@ class TestHyundaiSafetyAltLimits(TestHyundaiSafety): class TestHyundaiSafetyAltLimits2(TestHyundaiSafety): MAX_RATE_UP = 2 MAX_RATE_DOWN = 3 - MAX_TORQUE = 170 + MAX_TORQUE_LOOKUP = [0], [170] def setUp(self): self.packer = CANPackerPanda("hyundai_kia_generic") diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py index b31c8c3e..2fdea0d6 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py @@ -30,10 +30,9 @@ class TestHyundaiCanfdBase(HyundaiButtonBase, common.PandaCarSafetyTest, common. MAX_RATE_UP = 2 MAX_RATE_DOWN = 3 - MAX_TORQUE = 270 + MAX_TORQUE_LOOKUP = [0], [270] MAX_RT_DELTA = 112 - RT_INTERVAL = 250000 DRIVER_TORQUE_ALLOWANCE = 250 DRIVER_TORQUE_FACTOR = 2 @@ -41,7 +40,6 @@ class TestHyundaiCanfdBase(HyundaiButtonBase, common.PandaCarSafetyTest, common. # Safety around steering req bit MIN_VALID_STEERING_FRAMES = 89 MAX_INVALID_STEERING_FRAMES = 2 - MIN_VALID_STEERING_RT_INTERVAL = 810000 # a ~10% buffer, can send steer up to 110Hz PT_BUS = 0 SCC_BUS = 2 diff --git a/opendbc_repo/opendbc/safety/tests/test_mazda.py b/opendbc_repo/opendbc/safety/tests/test_mazda.py index e60248d2..67a22d49 100755 --- a/opendbc_repo/opendbc/safety/tests/test_mazda.py +++ b/opendbc_repo/opendbc/safety/tests/test_mazda.py @@ -16,10 +16,9 @@ class TestMazdaSafety(common.PandaCarSafetyTest, common.DriverTorqueSteeringSafe MAX_RATE_UP = 10 MAX_RATE_DOWN = 25 - MAX_TORQUE = 800 + MAX_TORQUE_LOOKUP = [0], [800] MAX_RT_DELTA = 300 - RT_INTERVAL = 250000 DRIVER_TORQUE_ALLOWANCE = 15 DRIVER_TORQUE_FACTOR = 1 diff --git a/opendbc_repo/opendbc/safety/tests/test_rivian.py b/opendbc_repo/opendbc/safety/tests/test_rivian.py index a1b6b3e8..e3f88856 100755 --- a/opendbc_repo/opendbc/safety/tests/test_rivian.py +++ b/opendbc_repo/opendbc/safety/tests/test_rivian.py @@ -1,29 +1,51 @@ #!/usr/bin/env python3 import unittest +import numpy as np from opendbc.car.structs import CarParams from opendbc.safety.tests.libsafety import libsafety_py import opendbc.safety.tests.common as common from opendbc.safety.tests.common import CANPackerPanda from opendbc.car.rivian.values import RivianSafetyFlags +from opendbc.car.rivian.riviancan import checksum as _checksum -class TestRivianSafetyBase(common.PandaCarSafetyTest, common.DriverTorqueSteeringSafetyTest, common.LongitudinalAccelSafetyTest): +def checksum(msg): + addr, dat, bus = msg + ret = bytearray(dat) + + # ESP_Status + if addr == 0x208: + ret[0] = _checksum(ret[1:], 0x1D, 0xB1) + elif addr == 0x150: + ret[0] = _checksum(ret[1:], 0x1D, 0x9A) + + return addr, ret, bus + + +class TestRivianSafetyBase(common.PandaCarSafetyTest, common.DriverTorqueSteeringSafetyTest, common.LongitudinalAccelSafetyTest, + common.VehicleSpeedSafetyTest): TX_MSGS = [[0x120, 0], [0x321, 2], [0x162, 2]] RELAY_MALFUNCTION_ADDRS = {0: (0x120,)} FWD_BLACKLISTED_ADDRS = {0: [0x321, 0x162], 2: [0x120]} - MAX_TORQUE = 250 + MAX_TORQUE_LOOKUP = [9, 17], [350, 250] + DYNAMIC_MAX_TORQUE = True MAX_RATE_UP = 3 MAX_RATE_DOWN = 5 MAX_RT_DELTA = 125 - RT_INTERVAL = 250000 DRIVER_TORQUE_ALLOWANCE = 100 DRIVER_TORQUE_FACTOR = 2 + # Max allowed delta between car speeds + MAX_SPEED_DELTA = 2.0 # m/s + + cnt_speed = 0 + cnt_speed_2 = 0 + def _torque_driver_msg(self, torque): values = {"EPAS_TorsionBarTorque": torque / 100.0} return self.packer.make_can_msg_panda("EPAS_SystemStatus", 0, values) @@ -32,26 +54,29 @@ class TestRivianSafetyBase(common.PandaCarSafetyTest, common.DriverTorqueSteerin values = {"ACM_lkaStrToqReq": torque, "ACM_lkaActToi": steer_req} return self.packer.make_can_msg_panda("ACM_lkaHbaCmd", 0, values) - def _speed_msg(self, speed): - values = {"ESP_Status": speed * 3.6} - return self.packer.make_can_msg_panda("ESP_Vehicle_Speed", 0, values) + def _speed_msg(self, speed, quality_flag=True): + values = {"ESP_Vehicle_Speed": speed * 3.6, "ESP_Status_Counter": self.cnt_speed % 15, + "ESP_Vehicle_Speed_Q": 1 if quality_flag else 0} + self.__class__.cnt_speed += 1 + return self.packer.make_can_msg_panda("ESP_Status", 0, values, fix_checksum=checksum) + + def _speed_msg_2(self, speed, quality_flag=True): + return self._user_gas_msg(0, speed, quality_flag) def _user_brake_msg(self, brake): values = {"iBESP2_BrakePedalApplied": brake} return self.packer.make_can_msg_panda("iBESP2", 0, values) - def _user_gas_msg(self, gas): - values = {"VDM_AcceleratorPedalPosition": gas} - return self.packer.make_can_msg_panda("VDM_PropStatus", 0, values) + def _user_gas_msg(self, gas, speed=0, quality_flag=True): + values = {"VDM_AcceleratorPedalPosition": gas, "VDM_VehicleSpeed": speed * 3.6, + "VDM_PropStatus_Counter": self.cnt_speed_2 % 15, "VDM_VehicleSpeedQ": 1 if quality_flag else 0} + self.__class__.cnt_speed_2 += 1 + return self.packer.make_can_msg_panda("VDM_PropStatus", 0, values, fix_checksum=checksum) def _pcm_status_msg(self, enable): values = {"ACM_FeatureStatus": enable, "ACM_Unkown1": 1} return self.packer.make_can_msg_panda("ACM_Status", 2, values) - def _vehicle_moving_msg(self, speed: float): - values = {"ESP_Vehicle_Speed": speed} - return self.packer.make_can_msg_panda("ESP_Status", 0, values) - def _accel_msg(self, accel: float): values = {"ACM_AccelerationRequest": accel} return self.packer.make_can_msg_panda("ACM_longitudinalRequest", 0, values) @@ -67,6 +92,40 @@ class TestRivianSafetyBase(common.PandaCarSafetyTest, common.DriverTorqueSteerin } self.assertTrue(self._tx(self.packer.make_can_msg_panda("SCCM_WheelTouch", 2, values))) + def test_rx_hook(self): + # checksum, counter, and quality flag checks + for quality_flag in (True, False): + for msg in ("speed", "speed_2"): + self.safety.set_controls_allowed(True) + # send multiple times to verify counter checks + for _ in range(10): + if msg == "speed": + to_push = self._speed_msg(0, quality_flag=quality_flag) + elif msg == "speed_2": + to_push = self._speed_msg_2(0, quality_flag=quality_flag) + + self.assertEqual(quality_flag, self._rx(to_push)) + self.assertEqual(quality_flag, self.safety.get_controls_allowed()) + + # Mess with checksum to make it fail + to_push[0].data[0] = 0xff + self.assertFalse(self._rx(to_push)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_rx_hook_speed_mismatch(self): + # TODO: this can be a common test w/ Ford + # Rivian has a dynamic max torque limit based on speed, so it checks two sources + for speed in np.arange(0, 40, 0.5): + for speed_delta in np.arange(-5, 5, 0.1): + speed_2 = round(max(speed + speed_delta, 0), 1) + # Set controls allowed in between rx since first message can reset it + self._rx(self._speed_msg(speed)) + self.safety.set_controls_allowed(True) + self._rx(self._speed_msg_2(speed_2)) + + within_delta = abs(speed - speed_2) <= self.MAX_SPEED_DELTA + self.assertEqual(self.safety.get_controls_allowed(), within_delta) + class TestRivianStockSafety(TestRivianSafetyBase): diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru.py b/opendbc_repo/opendbc/safety/tests/test_subaru.py index 87277324..4936eae1 100755 --- a/opendbc_repo/opendbc/safety/tests/test_subaru.py +++ b/opendbc_repo/opendbc/safety/tests/test_subaru.py @@ -60,7 +60,6 @@ class TestSubaruSafetyBase(common.PandaCarSafetyTest): FWD_BLACKLISTED_ADDRS = fwd_blacklisted_addr() MAX_RT_DELTA = 940 - RT_INTERVAL = 250000 DRIVER_TORQUE_ALLOWANCE = 60 DRIVER_TORQUE_FACTOR = 50 @@ -155,12 +154,12 @@ class TestSubaruLongitudinalSafetyBase(TestSubaruSafetyBase, common.Longitudinal class TestSubaruTorqueSafetyBase(TestSubaruSafetyBase, common.DriverTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest): MAX_RATE_UP = 50 MAX_RATE_DOWN = 70 - MAX_TORQUE = 2047 + MAX_TORQUE_LOOKUP = [0], [2047] # Safety around steering req bit MIN_VALID_STEERING_FRAMES = 7 MAX_INVALID_STEERING_FRAMES = 1 - MIN_VALID_STEERING_RT_INTERVAL = 144000 + STEER_STEP = 2 def _torque_cmd_msg(self, torque, steer_req=1): values = {"LKAS_Output": torque, "LKAS_Request": steer_req} @@ -178,7 +177,7 @@ class TestSubaruGen2TorqueSafetyBase(TestSubaruTorqueSafetyBase): MAX_RATE_UP = 40 MAX_RATE_DOWN = 40 - MAX_TORQUE = 1000 + MAX_TORQUE_LOOKUP = [0], [1000] class TestSubaruGen2TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruGen2TorqueSafetyBase): diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru_preglobal.py b/opendbc_repo/opendbc/safety/tests/test_subaru_preglobal.py index b2f482bf..af2c6cec 100755 --- a/opendbc_repo/opendbc/safety/tests/test_subaru_preglobal.py +++ b/opendbc_repo/opendbc/safety/tests/test_subaru_preglobal.py @@ -17,10 +17,9 @@ class TestSubaruPreglobalSafety(common.PandaCarSafetyTest, common.DriverTorqueSt MAX_RATE_UP = 50 MAX_RATE_DOWN = 70 - MAX_TORQUE = 2047 + MAX_TORQUE_LOOKUP = [0], [2047] MAX_RT_DELTA = 940 - RT_INTERVAL = 250000 DRIVER_TORQUE_ALLOWANCE = 75 DRIVER_TORQUE_FACTOR = 10 diff --git a/opendbc_repo/opendbc/safety/tests/test_toyota.py b/opendbc_repo/opendbc/safety/tests/test_toyota.py index dcca1eb8..a119615c 100755 --- a/opendbc_repo/opendbc/safety/tests/test_toyota.py +++ b/opendbc_repo/opendbc/safety/tests/test_toyota.py @@ -124,16 +124,14 @@ class TestToyotaSafetyTorque(TestToyotaSafetyBase, common.MotorTorqueSteeringSaf MAX_RATE_UP = 15 MAX_RATE_DOWN = 25 - MAX_TORQUE = 1500 + MAX_TORQUE_LOOKUP = [0], [1500] MAX_RT_DELTA = 450 - RT_INTERVAL = 250000 MAX_TORQUE_ERROR = 350 TORQUE_MEAS_TOLERANCE = 1 # toyota safety adds one to be conservative for rounding # Safety around steering req bit MIN_VALID_STEERING_FRAMES = 18 MAX_INVALID_STEERING_FRAMES = 1 - MIN_VALID_STEERING_RT_INTERVAL = 170000 # a ~10% buffer, can send steer up to 110Hz def setUp(self): self.packer = CANPackerPanda("toyota_nodsu_pt_generated") diff --git a/opendbc_repo/opendbc/safety/tests/test_volkswagen_mqb.py b/opendbc_repo/opendbc/safety/tests/test_volkswagen_mqb.py index de47b5ea..83e45f1f 100755 --- a/opendbc_repo/opendbc/safety/tests/test_volkswagen_mqb.py +++ b/opendbc_repo/opendbc/safety/tests/test_volkswagen_mqb.py @@ -28,9 +28,8 @@ class TestVolkswagenMqbSafetyBase(common.PandaCarSafetyTest, common.DriverTorque MAX_RATE_UP = 4 MAX_RATE_DOWN = 10 - MAX_TORQUE = 300 + MAX_TORQUE_LOOKUP = [0], [300] MAX_RT_DELTA = 75 - RT_INTERVAL = 250000 DRIVER_TORQUE_ALLOWANCE = 80 DRIVER_TORQUE_FACTOR = 3 @@ -133,7 +132,7 @@ class TestVolkswagenMqbStockSafety(TestVolkswagenMqbSafetyBase): FWD_BLACKLISTED_ADDRS = {0: [MSG_LH_EPS_03], 2: [MSG_HCA_01, MSG_LDW_02]} def setUp(self): - self.packer = CANPackerPanda("vw_mqb_2010") + self.packer = CANPackerPanda("vw_mqb") self.safety = libsafety_py.libsafety self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagen, 0) self.safety.init_tests() @@ -154,7 +153,7 @@ class TestVolkswagenMqbLongSafety(TestVolkswagenMqbSafetyBase): INACTIVE_ACCEL = 3.01 def setUp(self): - self.packer = CANPackerPanda("vw_mqb_2010") + self.packer = CANPackerPanda("vw_mqb") self.safety = libsafety_py.libsafety self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagen, VolkswagenSafetyFlags.LONG_CONTROL) self.safety.init_tests() diff --git a/opendbc_repo/opendbc/safety/tests/test_volkswagen_pq.py b/opendbc_repo/opendbc/safety/tests/test_volkswagen_pq.py index cc9e87df..8f6de057 100755 --- a/opendbc_repo/opendbc/safety/tests/test_volkswagen_pq.py +++ b/opendbc_repo/opendbc/safety/tests/test_volkswagen_pq.py @@ -26,9 +26,8 @@ class TestVolkswagenPqSafetyBase(common.PandaCarSafetyTest, common.DriverTorqueS MAX_RATE_UP = 6 MAX_RATE_DOWN = 10 - MAX_TORQUE = 300 + MAX_TORQUE_LOOKUP = [0], [300] MAX_RT_DELTA = 113 - RT_INTERVAL = 250000 DRIVER_TORQUE_ALLOWANCE = 80 DRIVER_TORQUE_FACTOR = 3 @@ -116,7 +115,7 @@ class TestVolkswagenPqStockSafety(TestVolkswagenPqSafetyBase): FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_1, MSG_LDW_1]} def setUp(self): - self.packer = CANPackerPanda("vw_golf_mk4") + self.packer = CANPackerPanda("vw_pq") self.safety = libsafety_py.libsafety self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenPq, 0) self.safety.init_tests() @@ -137,7 +136,7 @@ class TestVolkswagenPqLongSafety(TestVolkswagenPqSafetyBase, common.Longitudinal INACTIVE_ACCEL = 3.01 def setUp(self): - self.packer = CANPackerPanda("vw_golf_mk4") + self.packer = CANPackerPanda("vw_pq") self.safety = libsafety_py.libsafety self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenPq, VolkswagenSafetyFlags.LONG_CONTROL) self.safety.init_tests()