diff --git a/opendbc/car/volkswagen/carcontroller.py b/opendbc/car/volkswagen/carcontroller.py index 4d4f394fdeb..40b23f50e49 100644 --- a/opendbc/car/volkswagen/carcontroller.py +++ b/opendbc/car/volkswagen/carcontroller.py @@ -60,6 +60,11 @@ def __init__(self, dbc_names, CP): self.gra_acc_counter_last = None self.hca_mitigation = HCAMitigation(self.CCP) + self.last_set_speed = 0 + self.last_lead_distance_bars = 0 + self.mlb_hud_text = 0 + self.texte_timer = 0 + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl @@ -190,8 +195,22 @@ def update(self, CC, CS, now_nanos): # FIXME: PQ may need to use the on-the-wire mph/kmh toggle to fix rounding errors # FIXME: Detect clusters with vEgoCluster offsets and apply an identical vCruiseCluster offset set_speed = hud_control.setSpeed * CV.MS_TO_KPH + + # MLB:Logic for hud text, bottom acc text display + if self.CP.flags & VolkswagenFlags.MLB: + if set_speed != self.last_set_speed: + self.texte_timer = self.frame + int(2.0 / DT_CTRL) + self.mlb_hud_text = 21 + self.last_set_speed = set_speed + elif hud_control.leadDistanceBars != self.last_lead_distance_bars: + self.texte_timer = self.frame + int(2.0 / DT_CTRL) + self.mlb_hud_text = {1: 2, 2: 3, 3: 4, 4: 5}.get(hud_control.leadDistanceBars, 0) + self.last_lead_distance_bars = hud_control.leadDistanceBars + elif self.frame > self.texte_timer: + self.mlb_hud_text = 0 + can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, self.CAN.pt, acc_hud_status, set_speed, - lead_distance, hud_control.leadDistanceBars)) + lead_distance, hud_control, self.mlb_hud_text)) # **** Stock ACC Button Controls **************************************** # diff --git a/opendbc/car/volkswagen/carstate.py b/opendbc/car/volkswagen/carstate.py index 3844a39a8d6..97f37a6f98f 100644 --- a/opendbc/car/volkswagen/carstate.py +++ b/opendbc/car/volkswagen/carstate.py @@ -342,7 +342,7 @@ def update_mlb(self, pt_cp, cam_cp, ext_cp, alt_cp) -> structs.CarState: self.parse_mlb_mqb_steering_state(ret, pt_cp) - brake_pedal_pressed = bool(pt_cp.vl["Motor_03"]["MO_Fahrer_bremst"]) + brake_pedal_pressed = bool(pt_cp.vl["Motor_03"]["MO_BLS"]) brake_pressure_detected = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"]) ret.brakePressed = brake_pedal_pressed or brake_pressure_detected ret.parkingBrake = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"]) diff --git a/opendbc/car/volkswagen/interface.py b/opendbc/car/volkswagen/interface.py index 74712061c4a..ae4cd6df379 100644 --- a/opendbc/car/volkswagen/interface.py +++ b/opendbc/car/volkswagen/interface.py @@ -3,7 +3,7 @@ from opendbc.car.volkswagen.carcontroller import CarController from opendbc.car.volkswagen.carstate import CarState from opendbc.car.volkswagen.radar_interface import RadarInterface -from opendbc.car.volkswagen.values import CanBus, CAR, DBC, NetworkLocation, TransmissionType, VolkswagenFlags, VolkswagenSafetyFlags +from opendbc.car.volkswagen.values import CanBus, CAR, CarControllerParams, DBC, NetworkLocation, TransmissionType, VolkswagenFlags, VolkswagenSafetyFlags class CarInterface(CarInterfaceBase): CarState = CarState @@ -13,6 +13,11 @@ class CarInterface(CarInterfaceBase): DRIVABLE_GEARS = (structs.CarState.GearShifter.eco, structs.CarState.GearShifter.sport, structs.CarState.GearShifter.manumatic) + @staticmethod + def get_pid_accel_limits(CP, current_speed, cruise_speed): + accel_min = CarControllerParams.MLB_ACCEL_MIN if CP.flags & VolkswagenFlags.MLB else CarControllerParams.ACCEL_MIN + return accel_min, CarControllerParams.ACCEL_MAX + @staticmethod def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams: ret.brand = "volkswagen" diff --git a/opendbc/car/volkswagen/mlbcan.py b/opendbc/car/volkswagen/mlbcan.py index dc3b717e9cd..008374af79e 100644 --- a/opendbc/car/volkswagen/mlbcan.py +++ b/opendbc/car/volkswagen/mlbcan.py @@ -36,22 +36,63 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resu def acc_control_value(main_switch_on, acc_faulted, long_active): - return 0 + if acc_faulted: + acc_control = 6 + elif long_active: + acc_control = 3 + elif main_switch_on: + acc_control = 2 + else: + acc_control = 0 + + return acc_control + + +def create_acc_accel_control(packer, bus, acc_type, acc_enabled, accel, acc_control, stopping, starting, esp_hold): + commands = [] + + acc_01_values = { + "ACC_Status_ACC": acc_control, + "ACC_Sollbeschleunigung": accel if acc_enabled else 0, + "ACC_zul_Regelabw_unten": 0.2, + "ACC_zul_Regelabw_oben": 0.2, + "ACC_neg_Sollbeschl_Grad": 4.0 if acc_enabled else 0, + "ACC_pos_Sollbeschl_Grad": 4.0 if acc_enabled else 0, + "ACC_Anfahren": starting, + "ACC_Anhalten": stopping, + "ACC_Dynamik": 2, + "ACC_Minimale_Bremsung": stopping, + } + commands.append(packer.make_can_msg("ACC_01", bus, acc_01_values)) + + return commands def acc_hud_status_value(main_switch_on, acc_faulted, long_active): - return 0 + # TODO: happens to resemble the ACC control value for now, but extend this for init/gas override later + return acc_control_value(main_switch_on, acc_faulted, long_active) -def create_acc_accel_control(packer, bus, acc_type, acc_enabled, accel, acc_control, stopping, starting, esp_hold): - values = {} - return packer.make_can_msg("ACC_05", bus, values) +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, hud_control, mlb_hud_text): + acc_active = acc_hud_status in (3, 4) + values = { + "ACC_Status_Anzeige": acc_hud_status, + "ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.04, + "ACC_Display_Prio": 0, + "ACC_Anzeige_Zeitluecke": 1 if acc_active else 0, + "ACC_Gesetzte_Zeitluecke": hud_control.leadDistanceBars, # TODO: Update openpilot charisma using stock rocker switch + "ACC_Tachokranz": 1 if acc_active else 0, + "ACC_Relevantes_Objekt": 2 if hud_control.visualAlert > 0 else (1 if acc_active and hud_control.leadVisible else 0), + "ACC_Status_Prim_Anz": 2 if hud_control.visualAlert > 0 else (1 if acc_active else 0), + "ACC_Akustik": 1 if hud_control.audibleAlert == 5 else 0, # Audible alert on OP warningImmediate + "ACC_Abstandsindex": 1023 if acc_active else 1022, + "ACC_Texte_Primaeranz": mlb_hud_text, + } -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, distance): - values = {} return packer.make_can_msg("ACC_02", bus, values) + def volkswagen_mlb_checksum(address: int, sig, d: bytearray) -> int: xor_starting_value = { 0x109: 0x08, # ACC_01 diff --git a/opendbc/car/volkswagen/mqbcan.py b/opendbc/car/volkswagen/mqbcan.py index 67dd9da71aa..6f61e4a02dc 100644 --- a/opendbc/car/volkswagen/mqbcan.py +++ b/opendbc/car/volkswagen/mqbcan.py @@ -128,11 +128,11 @@ def create_acc_accel_control(packer, bus, acc_type, acc_enabled, accel, acc_cont return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, distance): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, hud_control, mlb_hud_text): values = { "ACC_Status_Anzeige": acc_hud_status, "ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36, - "ACC_Gesetzte_Zeitluecke": distance + 2, + "ACC_Gesetzte_Zeitluecke": hud_control.leadDistanceBars + 2, "ACC_Display_Prio": 3, "ACC_Abstandsindex": lead_distance, } diff --git a/opendbc/car/volkswagen/pqcan.py b/opendbc/car/volkswagen/pqcan.py index 897f45324fb..b0cfc590343 100644 --- a/opendbc/car/volkswagen/pqcan.py +++ b/opendbc/car/volkswagen/pqcan.py @@ -92,10 +92,10 @@ def create_acc_accel_control(packer, bus, acc_type, acc_enabled, accel, acc_cont return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, distance): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, hud_control, mlb_hud_text): values = { "ACA_StaACC": acc_hud_status, - "ACA_Zeitluecke": distance + 2, + "ACA_Zeitluecke": hud_control.leadDistanceBars + 2, "ACA_V_Wunsch": set_speed, "ACA_gemZeitl": lead_distance, "ACA_PrioDisp": 3, diff --git a/opendbc/car/volkswagen/values.py b/opendbc/car/volkswagen/values.py index 9a7851b662d..6d2b3a90c53 100644 --- a/opendbc/car/volkswagen/values.py +++ b/opendbc/car/volkswagen/values.py @@ -67,6 +67,7 @@ class CarControllerParams: ACCEL_MAX = 2.0 # 2.0 m/s^2 max acceleration ACCEL_MIN = -3.5 # 3.5 m/s^2 max deceleration + MLB_ACCEL_MIN = -2.95 # -3.0 trips the MLB ACC ECU fault (2014 Audi Q5 3.0T) def __init__(self, CP): can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) @@ -142,6 +143,7 @@ def __init__(self, CP): self.hca_status_values = can_define.dv["LH_EPS_03"]["EPS_HCA_Status"] if CP.flags & VolkswagenFlags.MLB: + self.ACCEL_MIN = self.MLB_ACCEL_MIN self.STEER_DRIVER_ALLOWANCE = 60 # Driver intervention threshold 0.6 Nm self.STEER_DELTA_UP = 9 # Max HCA reached in 0.66s (STEER_MAX / (50Hz * 0.66)) self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60)) @@ -322,7 +324,7 @@ def init_make(self, CP: structs.CarParams): # FW_VERSIONS for that existing CAR. class CAR(Platforms): - config: VolkswagenMQBPlatformConfig | VolkswagenPQPlatformConfig | VolkswagenMEBPlatformConfig + config: VolkswagenMQBPlatformConfig | VolkswagenPQPlatformConfig | VolkswagenMEBPlatformConfig | VolkswagenMLBPlatformConfig VOLKSWAGEN_ARTEON_MK1 = VolkswagenMQBPlatformConfig( [ @@ -515,7 +517,7 @@ class CAR(Platforms): ) AUDI_Q5_MK1 = VolkswagenMLBPlatformConfig( [VWCarDocs("Audi Q5 2013-17")], - VolkswagenCarSpecs(mass=1895, wheelbase=2.81), + VolkswagenCarSpecs(mass=1895, wheelbase=2.81, minEnableSpeed=15 * CV.KPH_TO_MS), chassis_codes={"8R"}, wmis={WMI.AUDI_EUROPE_MPV, WMI.AUDI_GERMANY_CAR}, ) diff --git a/opendbc/dbc/vw_mlb.dbc b/opendbc/dbc/vw_mlb.dbc index 4f7bb05a0b5..760326fdba1 100644 --- a/opendbc/dbc/vw_mlb.dbc +++ b/opendbc/dbc/vw_mlb.dbc @@ -43,7 +43,8 @@ BO_ 265 ACC_01: 8 Gateway_B8 SG_ ACC_neg_Sollbeschl_Grad : 40|8@1+ (0.05,0) [0|12.7] "Unit_MeterPerCubicSecon" Motor_MLB_AU48_AU416_Diesel,Motor_MLB_B8_Q5_Otto,Motor_MLB_Q5_Hybrid SG_ ACC_pos_Sollbeschl_Grad : 48|8@1+ (0.05,0) [0|12.7] "Unit_MeterPerCubicSecon" Motor_MLB_AU48_AU416_Diesel,Motor_MLB_B8_Q5_Otto,Motor_MLB_Q5_Hybrid SG_ ACC_Dynamik : 58|2@1+ (1,0) [0|3] "" Motor_MLB_AU48_AU416_Diesel,Motor_MLB_B8_Q5_Otto,Motor_MLB_Q5_Hybrid - SG_ ACC_Anhalten : 56|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Anfahren : 56|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Anhalten : 57|1@1+ (1,0) [0|1] "" XXX SG_ ACC_Status_ACC : 60|3@1+ (1,0) [0|7] "" XXX SG_ ACC_Minimale_Bremsung : 63|1@1+ (1,0) [0|1] "" Motor_MLB_AU48_AU416_Diesel,Motor_MLB_B8_Q5_Otto,Motor_MLB_Q5_Hybrid diff --git a/opendbc/safety/modes/volkswagen_common.h b/opendbc/safety/modes/volkswagen_common.h index fc5516b3c95..ec135f16442 100644 --- a/opendbc/safety/modes/volkswagen_common.h +++ b/opendbc/safety/modes/volkswagen_common.h @@ -22,6 +22,7 @@ bool volkswagen_brake_pressure_detected = false; #define MSG_LH_EPS_03 0x09FU // RX from EPS, for driver steering torque #define MSG_ESP_19 0x0B2U // RX from ABS, for wheel speeds #define MSG_ESP_05 0x106U // RX from ABS, for brake switch state +#define MSG_ACC_01 0x109U // TX by OP, ACC control instructions to the drivetrain coordinator #define MSG_TSK_06 0x120U // RX from ECU, for ACC status from drivetrain coordinator #define MSG_MOTOR_20 0x121U // RX from ECU, for driver throttle input #define MSG_ACC_06 0x122U // TX by OP, ACC control instructions to the drivetrain coordinator diff --git a/opendbc/safety/modes/volkswagen_mlb.h b/opendbc/safety/modes/volkswagen_mlb.h index 1390ea7e529..532489caae1 100644 --- a/opendbc/safety/modes/volkswagen_mlb.h +++ b/opendbc/safety/modes/volkswagen_mlb.h @@ -3,12 +3,25 @@ #include "opendbc/safety/declarations.h" #include "opendbc/safety/modes/volkswagen_common.h" +// ACC_01.ACC_Status_ACC, the states that tell the drivetrain ACC is regulating +#define VOLKSWAGEN_MLB_ACC_AKTIV_REGELT 3U +#define VOLKSWAGEN_MLB_ACC_OVERRIDE 4U + static safety_config volkswagen_mlb_init(uint16_t param) { // Transmit of LS_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration static const CanMsg VOLKSWAGEN_MLB_STOCK_TX_MSGS[] = {{MSG_HCA_01, 0, 8, .check_relay = true}, {MSG_LDW_02, 0, 8, .check_relay = true}, {MSG_LS_01, 0, 4, .check_relay = false}, {MSG_LS_01, 2, 4, .check_relay = false}}; + static const CanMsg VOLKSWAGEN_MLB_LONG_TX_MSGS[] = { + {MSG_HCA_01, 0, 8, .check_relay = true}, + {MSG_LS_01, 0, 4, .check_relay = false}, + {MSG_LS_01, 2, 4, .check_relay = false}, + {MSG_LDW_02, 0, 8, .check_relay = true}, + {MSG_ACC_02, 0, 8, .check_relay = true}, + {MSG_ACC_01, 0, 8, .check_relay = true}, + }; + static RxCheck volkswagen_mlb_rx_checks[] = { // TODO: implement checksum validation {.msg = {{MSG_ESP_03, 0, 8, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, @@ -19,10 +32,16 @@ static safety_config volkswagen_mlb_init(uint16_t param) { {.msg = {{MSG_LS_01, 0, 4, 10U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, }; - SAFETY_UNUSED(param); volkswagen_common_init(); - return BUILD_SAFETY_CFG(volkswagen_mlb_rx_checks, VOLKSWAGEN_MLB_STOCK_TX_MSGS); +#ifdef ALLOW_DEBUG + volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL); +#else + SAFETY_UNUSED(param); +#endif + + return volkswagen_longitudinal ? BUILD_SAFETY_CFG(volkswagen_mlb_rx_checks, VOLKSWAGEN_MLB_LONG_TX_MSGS) : \ + BUILD_SAFETY_CFG(volkswagen_mlb_rx_checks, VOLKSWAGEN_MLB_STOCK_TX_MSGS); } static void volkswagen_mlb_rx_hook(const CANPacket_t *msg) { @@ -44,6 +63,19 @@ static void volkswagen_mlb_rx_hook(const CANPacket_t *msg) { } if (msg->addr == MSG_LS_01) { + // If using openpilot longitudinal, enter controls on falling edge of Set or Resume with main switch on + // Signal: LS_01.LS_Tip_Setzen + // Signal: LS_01.LS_Tip_Wiederaufnahme + if (volkswagen_longitudinal) { + bool set_button = GET_BIT(msg, 16U); + bool resume_button = GET_BIT(msg, 19U); + if ((volkswagen_set_button_prev && !set_button) || + (volkswagen_resume_button_prev && !resume_button)) { + controls_allowed = GET_BIT(msg, 12U); // LS_Hauptschalter + } + volkswagen_set_button_prev = set_button; + volkswagen_resume_button_prev = resume_button; + } // Always exit controls on rising edge of Cancel // Signal: LS_01.LS_Abbrechen if (GET_BIT(msg, 13U)) { @@ -52,10 +84,10 @@ static void volkswagen_mlb_rx_hook(const CANPacket_t *msg) { } // Signal: Motor_03.MO_Fahrpedalrohwert_01 - // Signal: Motor_03.MO_Fahrer_bremst + // Signal: Motor_03.MO_BLS (bit 34) if (msg->addr == MSG_MOTOR_03) { gas_pressed = msg->data[6] != 0U; - volkswagen_brake_pedal_switch = GET_BIT(msg, 35U); + volkswagen_brake_pedal_switch = GET_BIT(msg, 34U); } if (msg->addr == MSG_ESP_05) { @@ -73,7 +105,9 @@ static void volkswagen_mlb_rx_hook(const CANPacket_t *msg) { int acc_status = (msg->data[7] & 0xC0U) >> 6; bool cruise_engaged = (acc_status == 1) || (acc_status == 2); - pcm_cruise_check(cruise_engaged); + if (!volkswagen_longitudinal) { + pcm_cruise_check(cruise_engaged); + } } } } @@ -90,6 +124,15 @@ static bool volkswagen_mlb_tx_hook(const CANPacket_t *msg) { .type = TorqueDriverLimited, }; + // longitudinal limits + // acceleration in m/s2 * 1000 to avoid floating point math + // Braking limited to -2.95m/s^2: -3.0 faults the 2014 Audi Q5 ACC ECU (requires ignition cycle to clear) + const LongitudinalLimits VOLKSWAGEN_MLB_LONG_LIMITS = { + .max_accel = 2000, + .min_accel = -2950, + .inactive_accel = 0, + }; + bool tx = true; // Safety check for HCA_01 Heading Control Assist torque @@ -104,6 +147,28 @@ static bool volkswagen_mlb_tx_hook(const CANPacket_t *msg) { } } + // Safety check for ACC_01 acceleration request + // To avoid floating point math, scale upward and compare to pre-scaled safety m/s^2 boundaries + if (msg->addr == MSG_ACC_01) { + bool violation = false; + int desired_accel = 0; + + // Signal: ACC_01.ACC_Sollbeschleunigung (acceleration in m/s^2, scale 0.005, offset -7.22) + desired_accel = ((((msg->data[4] & 0x07U) << 8) | msg->data[3]) * 5U) - 7220U; + + violation |= longitudinal_accel_checks(desired_accel, VOLKSWAGEN_MLB_LONG_LIMITS); + + // Signal: ACC_01.ACC_Status_ACC + uint8_t acc_status = (msg->data[7] >> 4) & 0x07U; + bool acc_status_active = (acc_status == VOLKSWAGEN_MLB_ACC_AKTIV_REGELT) || + (acc_status == VOLKSWAGEN_MLB_ACC_OVERRIDE); + violation |= acc_status_active && !controls_allowed; + + if (violation) { + tx = false; + } + } + // FORCE CANCEL: ensuring that only the cancel button press is sent when controls are off. // This avoids unintended engagements while still allowing resume spam if ((msg->addr == MSG_LS_01) && !controls_allowed) { diff --git a/opendbc/safety/tests/common.py b/opendbc/safety/tests/common.py index 090a7fb325c..295281d0e45 100644 --- a/opendbc/safety/tests/common.py +++ b/opendbc/safety/tests/common.py @@ -1030,12 +1030,12 @@ def test_tx_hook_on_wrong_safety_mode(self): if attr == 'TestVolkswagenMqbLongSafety' and current_test.startswith('TestSubaru'): tx = list(filter(lambda m: m[0] not in [0x122, ], tx)) - # Volkswagen MQB and Honda Nidec ACC HUD messages overlap - if attr == 'TestVolkswagenMqbLongSafety' and current_test.startswith('TestHondaNidec'): + # Volkswagen MQB/MLB longitudinal and Honda Nidec ACC HUD messages overlap at 0x30C + if attr in ('TestVolkswagenMqbLongSafety', 'TestVolkswagenMlbLongSafety') and current_test.startswith('TestHondaNidec'): tx = list(filter(lambda m: m[0] not in [0x30c, ], tx)) - # Volkswagen MQB and Honda Bosch Radarless ACC HUD messages overlap - if attr == 'TestVolkswagenMqbLongSafety' and current_test.startswith('TestHondaBoschRadarless'): + # Volkswagen MQB/MLB longitudinal and Honda Bosch Radarless ACC HUD messages overlap at 0x30C + if attr in ('TestVolkswagenMqbLongSafety', 'TestVolkswagenMlbLongSafety') and current_test.startswith('TestHondaBoschRadarless'): tx = list(filter(lambda m: m[0] not in [0x30c, ], tx)) # TODO: Temporary, should be fixed in panda firmware, safety_honda.h diff --git a/opendbc/safety/tests/test_volkswagen_mlb.py b/opendbc/safety/tests/test_volkswagen_mlb.py index 57bb93a77de..dc564db6845 100755 --- a/opendbc/safety/tests/test_volkswagen_mlb.py +++ b/opendbc/safety/tests/test_volkswagen_mlb.py @@ -1,14 +1,25 @@ #!/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 CANPackerSafety +from opendbc.car.volkswagen.values import VolkswagenSafetyFlags +MAX_ACCEL = 2.0 +MIN_ACCEL = -2.95 + +MSG_ACC_01 = 0x109 # TX by OP, longitudinal drivetrain control +MSG_ACC_02 = 0x30C # TX by OP, ACC HUD data to the instrument cluster MSG_LS_01 = 0x10B # TX by OP, ACC control buttons for cancel/resume MSG_HCA_01 = 0x126 # TX by OP, Heading Control Assist steering torque MSG_LDW_02 = 0x397 # TX by OP, Lane line recognition and text alerts +# ACC_01.ACC_Status_ACC +ACC_AKTIV_REGELT = 3 +ACC_OVERRIDE = 4 + class TestVolkswagenMlbSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest): RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_01, MSG_LDW_02)} @@ -34,7 +45,7 @@ def _esp_05_msg(self, brake): # Brake pedal switch def _motor_03_msg(self, brake_signal=False, gas_signal=0): values = { - "MO_Fahrer_bremst": brake_signal, + "MO_BLS": brake_signal, "MO_Fahrpedalrohwert_01": gas_signal, } return self.packer.make_can_msg_safety("Motor_03", 0, values) @@ -67,8 +78,9 @@ def _torque_cmd_msg(self, torque, steer_req=1): return self.packer.make_can_msg_safety("HCA_01", 0, values) # Cruise control buttons - def _ls_01_msg(self, cancel=0, resume=0, _set=0, bus=2): - values = {"LS_Abbrechen": cancel, "LS_Tip_Setzen": _set, "LS_Tip_Wiederaufnahme": resume} + def _ls_01_msg(self, cancel=0, resume=0, _set=0, main_switch=1, bus=2): + values = {"LS_Abbrechen": cancel, "LS_Tip_Setzen": _set, "LS_Tip_Wiederaufnahme": resume, + "LS_Hauptschalter": main_switch} return self.packer.make_can_msg_safety("LS_01", bus, values) # Verify brake_pressed is true if either the switch or pressure threshold signals are true @@ -132,5 +144,69 @@ def test_cancel_button(self): self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after cancel") +class TestVolkswagenMlbLongSafety(TestVolkswagenMlbSafetyBase): + TX_MSGS = [[MSG_HCA_01, 0], [MSG_LS_01, 0], [MSG_LS_01, 2], [MSG_LDW_02, 0], + [MSG_ACC_02, 0], [MSG_ACC_01, 0]] + FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_01, MSG_LDW_02, MSG_ACC_02, MSG_ACC_01]} + RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_01, MSG_LDW_02, MSG_ACC_02, MSG_ACC_01)} + + def setUp(self): + self.packer = CANPackerSafety("vw_mlb") + self.safety = libsafety_py.libsafety + self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenMlb, VolkswagenSafetyFlags.LONG_CONTROL) + self.safety.init_tests() + + # Acceleration request to drivetrain coordinator + def _acc_01_msg(self, accel, acc_status=0): + values = {"ACC_Sollbeschleunigung": accel, "ACC_Status_ACC": acc_status} + return self.packer.make_can_msg_safety("ACC_01", 0, values) + + # stock cruise controls are entirely bypassed under openpilot longitudinal control + def test_disable_control_allowed_from_cruise(self): + pass + + def test_enable_control_allowed_from_cruise(self): + pass + + def test_cruise_engaged_prev(self): + pass + + def test_set_and_resume_buttons(self): + for button in ["set", "resume"]: + # ACC main switch must be on, engage on falling edge of button + self.safety.set_controls_allowed(0) + self._rx(self._ls_01_msg(_set=(button == "set"), resume=(button == "resume"), main_switch=0, bus=0)) + self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} with main switch off") + self._rx(self._ls_01_msg(_set=(button == "set"), resume=(button == "resume"), main_switch=1, bus=0)) + self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} rising edge") + self._rx(self._ls_01_msg(main_switch=1, bus=0)) + self.assertTrue(self.safety.get_controls_allowed(), f"controls not allowed on {button} falling edge") + + def test_cancel_button(self): + # Disable on rising edge of cancel button + self.safety.set_controls_allowed(1) + self._rx(self._ls_01_msg(cancel=True, bus=0)) + self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after cancel") + + def test_accel_safety_check(self): + for controls_allowed in [True, False]: + # enforce we don't skip over 0 (inactive accel) or exact boundary conditions + for accel in np.concatenate((np.arange(MIN_ACCEL - 2, MAX_ACCEL + 2, 0.03), [0])): + accel = round(accel, 2) + is_inactive_accel = accel == 0 + send = (controls_allowed and MIN_ACCEL <= accel <= MAX_ACCEL) or is_inactive_accel + self.safety.set_controls_allowed(controls_allowed) + self.assertEqual(send, self._tx(self._acc_01_msg(accel)), (controls_allowed, accel)) + + def test_acc_status_safety_check(self): + # claiming ACC_AKTIV_REGELT or ACC_OVERRIDE is what makes the drivetrain act on our requests. + # off, init, standby and the fault states stay available for the HUD with controls disallowed + for controls_allowed in (True, False): + for acc_status in range(8): + self.safety.set_controls_allowed(controls_allowed) + send = controls_allowed or acc_status not in (ACC_AKTIV_REGELT, ACC_OVERRIDE) + self.assertEqual(send, self._tx(self._acc_01_msg(0, acc_status=acc_status)), (controls_allowed, acc_status)) + + if __name__ == "__main__": unittest.main()