diff --git a/board/safety/safety_volkswagen_mqb.h b/board/safety/safety_volkswagen_mqb.h index d880a69a6e..6852317934 100644 --- a/board/safety/safety_volkswagen_mqb.h +++ b/board/safety/safety_volkswagen_mqb.h @@ -23,12 +23,14 @@ const LongitudinalLimits VOLKSWAGEN_MQB_LONG_LIMITS = { #define MSG_ESP_19 0x0B2 // RX from ABS, for wheel speeds #define MSG_LH_EPS_03 0x09F // RX from EPS, for driver steering torque #define MSG_ESP_05 0x106 // RX from ABS, for brake switch state +#define MSG_ACC_10 0x117 // TX by OP, FCW/AEB control to the stopping coordinator #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_ACC_15 0x2A9 // TX by OP, FCW/AEB HUD to the instrument cluster #define MSG_ACC_02 0x30C // TX by OP, ACC HUD data to the instrument cluster #define MSG_MOTOR_14 0x3BE // RX from ECU, for brake switch status #define MSG_LDW_02 0x397 // TX by OP, Lane line recognition and text alerts @@ -37,7 +39,8 @@ const LongitudinalLimits VOLKSWAGEN_MQB_LONG_LIMITS = { const CanMsg VOLKSWAGEN_MQB_STOCK_TX_MSGS[] = {{MSG_HCA_01, 0, 8}, {MSG_GRA_ACC_01, 0, 8}, {MSG_GRA_ACC_01, 2, 8}, {MSG_LDW_02, 0, 8}, {MSG_LH_EPS_03, 2, 8}}; const CanMsg VOLKSWAGEN_MQB_LONG_TX_MSGS[] = {{MSG_HCA_01, 0, 8}, {MSG_LDW_02, 0, 8}, {MSG_LH_EPS_03, 2, 8}, - {MSG_ACC_02, 0, 8}, {MSG_ACC_06, 0, 8}, {MSG_ACC_07, 0, 8}}; + {MSG_ACC_02, 0, 8}, {MSG_ACC_06, 0, 8}, {MSG_ACC_07, 0, 8}, + {MSG_ACC_10, 0, 8}, {MSG_ACC_15, 0, 8}}; RxCheck volkswagen_mqb_rx_checks[] = { {.msg = {{MSG_ESP_19, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}}, @@ -241,6 +244,26 @@ static bool volkswagen_mqb_tx_hook(const CANPacket_t *to_send) { } } + // Safety check for FCW/AEB control + if (addr == MSG_ACC_10) { + bool violation = false; + + bool partial_braking = GET_BIT(to_send, 28U); // Signal: ACC_10.ANB_Teilbremsung_Freigabe + bool target_braking = GET_BIT(to_send, 39U); // Signal: ACC_10.ANB_Zielbremsung_Freigabe + // Signal: ACC_10.ANB_Zielbrems_Teilbrems_Verz_Anf (acceleration in m/s2, scale 0.024, offset -20.016) + // aeb_accel = true aeb_accel * 1000 to avoid floating point math + int aeb_accel = ((((GET_BYTE(to_send, 3) & 0xE0U) >> 5) | ((GET_BYTE(to_send, 4) & 0x7FU) << 3)) * 24U) - 20016U; + + // Until openpilot AEB is supported, enforce no actuation + violation |= partial_braking; + violation |= target_braking; + violation |= aeb_accel != 0; // Inactive accel value + + 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 ((addr == MSG_GRA_ACC_01) && !controls_allowed) { @@ -270,7 +293,8 @@ static int volkswagen_mqb_fwd_hook(int bus_num, int addr) { if ((addr == MSG_HCA_01) || (addr == MSG_LDW_02)) { // openpilot takes over LKAS steering control and related HUD messages from the camera bus_fwd = -1; - } else if (volkswagen_longitudinal && ((addr == MSG_ACC_02) || (addr == MSG_ACC_06) || (addr == MSG_ACC_07))) { + } else if (volkswagen_longitudinal && ((addr == MSG_ACC_02) || (addr == MSG_ACC_06) || (addr == MSG_ACC_07) || \ + (addr == MSG_ACC_10) || (addr == MSG_ACC_15))) { // openpilot takes over acceleration/braking control and related HUD messages from the stock ACC radar bus_fwd = -1; } else { diff --git a/tests/safety/test_volkswagen_mqb.py b/tests/safety/test_volkswagen_mqb.py index 276ee6c27d..4823893b67 100755 --- a/tests/safety/test_volkswagen_mqb.py +++ b/tests/safety/test_volkswagen_mqb.py @@ -8,16 +8,20 @@ MAX_ACCEL = 2.0 MIN_ACCEL = -3.5 +AEB_MIN_ACCEL = -8.0 +INACTIVE_AEB_ACCEL = 0.0 MSG_ESP_19 = 0xB2 # RX from ABS, for wheel speeds MSG_LH_EPS_03 = 0x9F # RX from EPS, for driver steering torque MSG_ESP_05 = 0x106 # RX from ABS, for brake light state +MSG_ACC_10 = 0x117 # TX by OP, FCW/AEB control to the stopping coordinator MSG_TSK_06 = 0x120 # RX from ECU, for ACC status from drivetrain coordinator MSG_MOTOR_20 = 0x121 # RX from ECU, for driver throttle input MSG_ACC_06 = 0x122 # TX by OP, ACC control instructions to the drivetrain coordinator MSG_HCA_01 = 0x126 # TX by OP, Heading Control Assist steering torque MSG_GRA_ACC_01 = 0x12B # TX by OP, ACC control buttons for cancel/resume MSG_ACC_07 = 0x12E # TX by OP, ACC control instructions to the drivetrain coordinator +MSG_ACC_15 = 0x2A9 # TX by OP, FCW/AEB HUD to the instrument cluster MSG_ACC_02 = 0x30C # TX by OP, ACC HUD data to the instrument cluster MSG_LDW_02 = 0x397 # TX by OP, Lane line recognition and text alerts @@ -102,6 +106,15 @@ def _acc_07_msg(self, accel, secondary_accel=3.02): values = {"ACC_Sollbeschleunigung_02": accel, "ACC_Folgebeschl": secondary_accel} return self.packer.make_can_msg_panda("ACC_07", 0, values) + # FCW/AEB control message + def _acc_10_msg(self, accel=0.0, partial_braking=False, target_braking=False): + values = { + "ANB_Teilbremsung_Freigabe": partial_braking, + "ANB_Zielbremsung_Freigabe": target_braking, + "ANB_Zielbrems_Teilbrems_Verz_Anf": accel, + } + return self.packer.make_can_msg_panda("ACC_10", 0, values) + # Verify brake_pressed is true if either the switch or pressure threshold signals are true def test_redundant_brake_signals(self): test_combinations = [(True, True, True), (True, True, False), (True, False, True), (False, False, False)] @@ -157,8 +170,10 @@ def test_spam_cancel_safety_check(self): class TestVolkswagenMqbLongSafety(TestVolkswagenMqbSafety): - TX_MSGS = [[MSG_HCA_01, 0], [MSG_LDW_02, 0], [MSG_LH_EPS_03, 2], [MSG_ACC_02, 0], [MSG_ACC_06, 0], [MSG_ACC_07, 0]] - FWD_BLACKLISTED_ADDRS = {0: [MSG_LH_EPS_03], 2: [MSG_HCA_01, MSG_LDW_02, MSG_ACC_02, MSG_ACC_06, MSG_ACC_07]} + TX_MSGS = [[MSG_HCA_01, 0], [MSG_LDW_02, 0], [MSG_LH_EPS_03, 2], [MSG_ACC_02, 0], + [MSG_ACC_06, 0], [MSG_ACC_07, 0], [MSG_ACC_10, 0], [MSG_ACC_15, 0]] + FWD_BLACKLISTED_ADDRS = {0: [MSG_LH_EPS_03], 2: [MSG_HCA_01, MSG_LDW_02, MSG_ACC_02, MSG_ACC_06, + MSG_ACC_07, MSG_ACC_10, MSG_ACC_15]} FWD_BUS_LOOKUP = {0: 2, 2: 0} INACTIVE_ACCEL = 3.01 @@ -220,6 +235,18 @@ def test_accel_safety_check(self): # ensure the optional secondary accel field remains inactive for now self.assertEqual(is_inactive_accel, self._tx(self._acc_07_msg(accel, secondary_accel=accel)), (controls_allowed, accel)) + def test_aeb_actuation(self): + for partial_braking, target_braking in [[False, False], [True, False], [False, True]]: + for accel in np.concatenate((np.arange(AEB_MIN_ACCEL - 2, 0.0, 0.1), [INACTIVE_AEB_ACCEL])): + accel = round(accel, 2) # floats might not hit exact boundary conditions without rounding + aeb_valid_inactive = accel == INACTIVE_AEB_ACCEL and not any([partial_braking, target_braking]) + # TODO: When real AEB is implemented + # aeb_valid_active = AEB_MIN_ACCEL <= accel <= 0.0 and any([partial_braking, target_braking]) + # send = aeb_valid_inactive or aeb_valid_active + send = aeb_valid_inactive + self.assertEqual(send, self._tx(self._acc_10_msg(accel, partial_braking, target_braking)), + f"{send=} {accel=} {partial_braking=} {target_braking=}") + if __name__ == "__main__": unittest.main()