diff --git a/iqdbc_repo/iqdbc/car/volkswagen/carcontroller.py b/iqdbc_repo/iqdbc/car/volkswagen/carcontroller.py index ba5c3ab5d..8786290ed 100644 --- a/iqdbc_repo/iqdbc/car/volkswagen/carcontroller.py +++ b/iqdbc_repo/iqdbc/car/volkswagen/carcontroller.py @@ -245,6 +245,10 @@ class CarController(CarControllerBase): self.leadDistanceBars = 0 self.lead_distance_bars_last = None self.distance_bar_frame = 0 + self.mlb_hud_text = 0 + self.mlb_hud_text_frame = 0 + self.mlb_set_speed_last = 0 + self.mlb_lead_distance_bars_last = None self.speed_limit_last = 0 self.speed_limit_changed_timer = 0 self.blinkerActive = None @@ -280,6 +284,19 @@ class CarController(CarControllerBase): return float(np.interp(v_ego, [0.4, 3.5, 4.0], [0.8, 0.95, 1.0])) return 1.0 + def _mlb_acc_hud_text(self, hud_control, set_speed: float) -> int: + if hud_control.leadDistanceBars != self.mlb_lead_distance_bars_last: + self.mlb_hud_text_frame = self.frame + self.mlb_hud_text = self.CCP.ACC_HUD_TEXT_DISTANCE.get(hud_control.leadDistanceBars, self.CCP.ACC_HUD_TEXTS["none"]) + elif set_speed != self.mlb_set_speed_last and hud_control.speedVisible: + self.mlb_hud_text_frame = self.frame + self.mlb_hud_text = self.CCP.ACC_HUD_TEXTS["setSpeed"] + elif self.frame - self.mlb_hud_text_frame >= self.CCP.ACC_HUD_TEXT_STEP: + self.mlb_hud_text = self.CCP.ACC_HUD_TEXTS["none"] + self.mlb_lead_distance_bars_last = hud_control.leadDistanceBars + self.mlb_set_speed_last = set_speed + return self.mlb_hud_text + def _should_spam_mqb_a0_resume(self, CS, enabled: bool) -> bool: return bool( enabled and @@ -429,8 +446,9 @@ class CarController(CarControllerBase): can_sends.append(self.CCS.create_blinker_control(self.packer_pt, self.CAN.pt, CS.ea_hud_stock_values, CS.ea_control_stock_values, left_blinker, right_blinker, self.hide_ea_error)) - if self.CP.openpilotLongitudinalControl and self.CCS == mqbcan and not self.acc_counter_seeded and CS.acc_stock_counters: - for name in ("ACC_02", "ACC_06", "ACC_07", "ACC_10"): + if self.CP.openpilotLongitudinalControl and self.CCS in (mqbcan, mlbcan) and not self.acc_counter_seeded and CS.acc_stock_counters: + seed_msgs = ("ACC_01", "ACC_02") if self.CCS is mlbcan else ("ACC_02", "ACC_06", "ACC_07", "ACC_10") + for name in seed_msgs: addr = self.packer_pt.dbc.name_to_msg[name].address self.packer_pt.counters[addr] = (CS.acc_stock_counters[name] + 1) % 16 self.acc_counter_seeded = True @@ -499,6 +517,8 @@ class CarController(CarControllerBase): self.long_deviation, self.long_jerklimit, eBrakeActive, esp_starting_override=esp_starting_override, esp_stopping_override=esp_stopping_override, )) + elif self.CCS == mlbcan: + can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, self.CAN.pt, accel, acc_control, stopping)) else: accel = apply_pq_stopping_accel(self.CP.carFingerprint, accel, stopping) @@ -572,12 +592,17 @@ class CarController(CarControllerBase): hud_control.leadVisible, hud_control.leadDistanceBars + 1, show_distance_bars, CS.esp_hold_confirmation, distance, gap, fcw_alert, acc_hud_event, speed_limit)) else: - leadDistance = min(8, hud_control.leadDistance) if hud_control.leadDistance != 0 else 0 + leadDistance = hud_control.leadDistance if self.CCS is mlbcan else \ + (min(8, hud_control.leadDistance) if hud_control.leadDistance != 0 else 0) self.leadDistanceBars = min(3, hud_control.leadDistanceBars) acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive, CC.cruiseControl.override) set_speed = hud_control.setSpeed * CV.MS_TO_KPH decel = dVisual(self.CCS, CS) - can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, self.CAN.pt, acc_hud_status, set_speed, leadDistance, self.leadDistanceBars, fcw_alert, hud_control.leadVisible, self.unavailable, decel, d_unresponsive)) + hud_kwargs = {"hud_text": self._mlb_acc_hud_text(hud_control, set_speed), + "desired_distance": max(8.0, CS.out.vEgo * hud_control.leadFollowTime)} if self.CCS is mlbcan else {} + can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, self.CAN.pt, acc_hud_status, set_speed, leadDistance, + self.leadDistanceBars, fcw_alert, hud_control.leadVisible, self.unavailable, + decel, d_unresponsive, **hud_kwargs)) if self.CP.flags & VolkswagenFlags.PQ: iq_lvbs_commander.update_turn_signals(self, CC, CS, can_sends) diff --git a/iqdbc_repo/iqdbc/car/volkswagen/carstate.py b/iqdbc_repo/iqdbc/car/volkswagen/carstate.py index bb35a8cce..fee92105b 100644 --- a/iqdbc_repo/iqdbc/car/volkswagen/carstate.py +++ b/iqdbc_repo/iqdbc/car/volkswagen/carstate.py @@ -675,9 +675,12 @@ class CarState(CarStateBase): else: ret.gearShifter = GearShifter.drive - # ACC okay but disabled (1), ACC ready (2), a radar visibility or other fault/disruption (6 or 7) - # currently regulating speed (3), driver accel override (4), brake only (5) - if self.CP.carFingerprint == CAR.PORSCHE_MACAN_MK1: + cruise_main_switch = bool(pt_cp.vl["LS_01"]["LS_Hauptschalter"]) + if not self.CP.pcmCruise: + ret.cruiseState.available = cruise_main_switch + ret.cruiseState.enabled = False + ret.accFaulted = False + elif self.CP.carFingerprint == CAR.PORSCHE_MACAN_MK1: ret.cruiseState.available = ext_cp.vl["ACC_05"]["ACC_Status_ACC"] in (2, 3, 4, 5) ret.cruiseState.enabled = ext_cp.vl["ACC_05"]["ACC_Status_ACC"] in (3, 4, 5) ret.accFaulted = ext_cp.vl["ACC_05"]["ACC_Status_ACC"] in (6, 7) @@ -687,11 +690,17 @@ class CarState(CarStateBase): ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw_02"] * CV.KPH_TO_MS ret.accFaulted = pt_cp.vl["TSK_02"]["TSK_Status"] in (3,) + ret.cruiseState.nonAdaptive = bool(pt_cp.vl["LS_01"]["LS_Limiter"]) + if not self.CP.pcmCruise: + self.acc_stock_counters["ACC_01"] = int(ext_cp.vl["ACC_01"]["COUNTER"]) + self.acc_stock_counters["ACC_02"] = int(ext_cp.vl["ACC_02"]["COUNTER"]) + self.esp_hold_confirmation = bool(pt_cp.vl["ESP_02"]["ESP_Stillstandsflag"]) + self.parse_mlb_mqb_steering_state(ret, pt_cp) self._update_mlb_iq_alc_state(pt_cp) ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0 - 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"]) @@ -728,9 +737,10 @@ class CarState(CarStateBase): ret.cruiseState.standstill = self.CP.pcmCruise and self.esp_hold_confirmation ret.standstill = ret.vEgoRaw == 0 - ret.cruiseFaultLateralMode = False - ret.lateralAvailable = ret.cruiseState.available - ret.blockPcmEnable = False + allow_lat_only = self._params.get_bool("AllowLateralWhenLongUnavailable") and self._params.get_bool("AolEnabled") + ret.cruiseFaultLateralMode = allow_lat_only and ret.accFaulted and cruise_main_switch + ret.lateralAvailable = ret.cruiseState.available or ret.cruiseFaultLateralMode + ret.blockPcmEnable = ret.cruiseFaultLateralMode self.cruise_faulted = ret.accFaulted self.frame += 1 diff --git a/iqdbc_repo/iqdbc/car/volkswagen/interface.py b/iqdbc_repo/iqdbc/car/volkswagen/interface.py index 85a4a9b10..e86118cb9 100644 --- a/iqdbc_repo/iqdbc/car/volkswagen/interface.py +++ b/iqdbc_repo/iqdbc/car/volkswagen/interface.py @@ -91,6 +91,7 @@ class CarInterface(CarInterfaceBase): safety_configs = [get_safety_config(structs.CarParams.SafetyModel.volkswagenMlb)] ret.enableBsm = 0x30F in fingerprint[0] # SWA_01 ret.networkLocation = NetworkLocation.gateway + ret.transmissionType = TransmissionType.automatic ret.dashcamOnly = False elif ret.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO): diff --git a/iqdbc_repo/iqdbc/car/volkswagen/mlbcan.py b/iqdbc_repo/iqdbc/car/volkswagen/mlbcan.py index aee56e208..f238a6fcf 100644 --- a/iqdbc_repo/iqdbc/car/volkswagen/mlbcan.py +++ b/iqdbc_repo/iqdbc/car/volkswagen/mlbcan.py @@ -1,4 +1,8 @@ -from iqdbc.car.volkswagen.mqbcan import (volkswagen_mqb_meb_checksum, xor_checksum, create_lka_hud_control as mqb_create_lka_hud_control) +from iqdbc.car.volkswagen.mqbcan import (volkswagen_mqb_meb_checksum, xor_checksum, + acc_control_value as mqb_acc_control_value, + acc_hud_status_value as mqb_acc_hud_status_value, + create_lka_hud_control as mqb_create_lka_hud_control) + def create_hca_steering_control(packer, bus, apply_steer, HCA_Status): values = { @@ -11,8 +15,10 @@ def create_hca_steering_control(packer, bus, apply_steer, HCA_Status): return packer.make_can_msg("HCA_01", bus, values) -def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control, entering=False, special_mode=False, special_active=False): - return mqb_create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control, entering, special_mode, special_active) +def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control, + entering=False, special_mode=False, special_active=False): + return mqb_create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control, + entering, special_mode, special_active) def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resume=False, set_button=False): @@ -32,23 +38,73 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resu return packer.make_can_msg("LS_01", bus, values) -def acc_control_value(main_switch_on, long_active, cruiseOverride): - return 0 +def acc_control_value(main_switch_on, long_active, cruiseOverride, accFaulted): + if cruiseOverride: + acc_control = 4 + elif accFaulted: + acc_control = 6 + elif long_active: + acc_control = 3 + elif main_switch_on: + acc_control = 2 + else: + acc_control = 0 + + return acc_control def acc_hud_status_value(main_switch_on, acc_faulted, longActive, longOverride): - return 0 + return mqb_acc_hud_status_value(main_switch_on, acc_faulted, longActive, longOverride) -def create_acc_accel_control(packer, bus, acc_type, accel, acc_control, stopping, starting, esp_hold, comfortBand, jerkLimit): - values = {} - return [packer.make_can_msg("ACC_05", bus, values)] +def create_acc_accel_control(packer, bus, accel, acc_control, stopping): + acc_enabled = acc_control in (3, 4) + + acc_01_values = { + "ACC_Status_ACC": acc_control, + "ACC_Sollbeschleunigung": accel if acc_enabled else 0, + "ACC_zul_Regelabw_unten": 0.2 if acc_enabled else 0, + "ACC_zul_Regelabw_oben": 0.2 if acc_enabled else 0, + "ACC_neg_Sollbeschl_Grad": 4.0 if acc_enabled else 0, + "ACC_pos_Sollbeschl_Grad": 4.0 if acc_enabled else 0, + "ACC_Dynamik": 3, + "ACC_Anhalten": stopping if acc_enabled else False, + "ACC_Minimale_Bremsung": 0, + } + + return [packer.make_can_msg("ACC_01", bus, acc_01_values)] -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, leadDistance, distanceBars, fcw_alert, leadVisible, unavailable, decel, d_unresponsive): - values = {} +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, leadDistance, distanceBars, fcw_alert, leadVisible, + unavailable, decel, d_unresponsive, hud_text=0, desired_distance=8.0): + engaged = acc_hud_status in (3, 4) + priodisp = 0 if fcw_alert else 1 if (acc_hud_status == 4 or decel or leadVisible) else 2 if (acc_hud_status in (3, 2)) else 0 + if not engaged: + acc_distance_index = 1022 + elif not leadVisible: + acc_distance_index = 1023 + else: + distance_ratio = leadDistance / max(desired_distance, 1.0) + acc_distance_index = int(max(1, min(1021, round(490 * (3 - 2 * distance_ratio))))) + + values = { + "ACC_Status_Anzeige": acc_hud_status, # 0 off, 1 init, 2 standby, 3 active, 4 overridden, 5 shutdown reaction, 6/7 fault + "ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36, # 327.36 (raw 1023) = "no display" + "ACC_Gesetzte_Zeitluecke": distanceBars, # 1 aggressive, 2 standard, 3 relaxed + "ACC_Anzeige_Zeitluecke": 1 if engaged else 0, # 0 gap bars not requested, 1 requested + "ACC_Tachokranz": 1 if engaged else 0, # 0 speedo ring not lit, 1 lit + "ACC_Display_Prio": priodisp, # 0 highest prio, 1 medium, 2 low, 3 none + "ACC_Abstandsindex": acc_distance_index, # 1 - 1020 = Lead distance, 1021 = Emergency brake alert, 1022 = ACC off, 1023 = ACC on but no lead + "ACC_Relevantes_Objekt": 2 if fcw_alert else (1 if leadVisible else 0), # lead car: 1 green, 2 red, 0 off + "ACC_Status_Prim_Anz": 2 if fcw_alert else (1 if engaged else 0), # ACC symbol: 1 green, 2 red, 3 yellow, 0 off + "ACC_Optischer_Fahrerhinweis": 1 if fcw_alert else 0, # 0 = off, 1 = on + "ACC_Akustik": 1 if (fcw_alert or d_unresponsive) else 0, # 0 none, 1 high prio, 2 low prio, 3 high prio continuous + "ACC_Texte_Primaeranz": hud_text, # primary HUD message text code, e.g. 10 "ACC ready", 53 "ACC off" (see DBC VAL_ for full list) + } + 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/iqdbc_repo/iqdbc/car/volkswagen/values.py b/iqdbc_repo/iqdbc/car/volkswagen/values.py index d6aa9554b..cff8bce3e 100644 --- a/iqdbc_repo/iqdbc/car/volkswagen/values.py +++ b/iqdbc_repo/iqdbc/car/volkswagen/values.py @@ -2,7 +2,7 @@ from collections import defaultdict, namedtuple from dataclasses import dataclass, field from enum import Enum, IntFlag, StrEnum -from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CanBusBase, CarSpecs, DbcDict, PlatformConfig, Platforms, structs, uds +from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CanBusBase, CarSpecs, DbcDict, DT_CTRL, PlatformConfig, Platforms, structs, uds from iqdbc.car.lateral import CurvatureSteeringLimits from iqdbc.can import CANDefine from iqdbc.car.common.conversions import Conversions as CV @@ -204,6 +204,7 @@ class CarControllerParams: 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)) + self.ACC_HUD_TEXT_STEP = int(2.0 / DT_CTRL) # ACC_02 primary display text dwell time if CP.carFingerprint == CAR.PORSCHE_MACAN_MK1: self.shifter_values = can_define.dv["Getriebe_03"]["GE_Waehlhebel"] @@ -219,6 +220,13 @@ class CarControllerParams: Button(structs.CarState.ButtonEvent.Type.gapAdjustCruise, "LS_01", "LS_Verstellung_Zeitluecke", [1, 2, 3]), ] + # ACC_02.ACC_Texte_Primaeranz, primary ACC display text at the bottom of the cluster + self.ACC_HUD_TEXTS = { + "none": 0, + "setSpeed": 21, + } + self.ACC_HUD_TEXT_DISTANCE = {1: 2, 2: 3, 3: 4, 4: 5} # follow distance bars to display text + else: self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm self.STEER_DELTA_UP = 4 # Max HCA reached in 1.50s (STEER_MAX / (50Hz * 1.50)) diff --git a/iqdbc_repo/iqdbc/dbc/vw_mlb.dbc b/iqdbc_repo/iqdbc/dbc/vw_mlb.dbc index 2f5f4b76c..401a80f66 100644 --- a/iqdbc_repo/iqdbc/dbc/vw_mlb.dbc +++ b/iqdbc_repo/iqdbc/dbc/vw_mlb.dbc @@ -35,7 +35,7 @@ BS_: BU_: Airbag_D4 EPB_D4 ESP_D4 Gateway_D4C7 Getriebe_AL551_951_D4_C7 Getriebe_DL501_C7 Getriebe_VL381_C7 LWS_D4 Motor_EDC17_D4 Motor_ME17_BY Motor_MED17_SIMOS8_D4 Motor_Slave_D4 QSP_D4 SAK_C7 SCR_C7 SCU_D4 BO_ 265 ACC_01: 8 Gateway_B8 - SG_ ACC_01_CHK : 0|8@1+ (1,0) [0|255] "" Magnetic_Ride_LB,Motor_MLB_AU48_AU416_Diesel,Motor_MLB_B8_Q5_Otto,Motor_MLB_Q5_Hybrid + SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" Magnetic_Ride_LB,Motor_MLB_AU48_AU416_Diesel,Motor_MLB_B8_Q5_Otto,Motor_MLB_Q5_Hybrid SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" Magnetic_Ride_LB,Motor_MLB_AU48_AU416_Diesel,Motor_MLB_B8_Q5_Otto,Motor_MLB_Q5_Hybrid SG_ ACC_zul_Regelabw_unten : 16|6@1+ (0.024,0) [0|1.512] "Unit_MeterPerSeconSquar" Motor_MLB_AU48_AU416_Diesel,Motor_MLB_B8_Q5_Otto,Motor_MLB_Q5_Hybrid SG_ ACC_Sollbeschleunigung : 24|11@1+ (0.005,-7.22) [-7.22|3.01] "Unit_MeterPerSeconSquar" Magnetic_Ride_LB,Motor_MLB_AU48_AU416_Diesel,Motor_MLB_B8_Q5_Otto,Motor_MLB_Q5_Hybrid @@ -43,12 +43,13 @@ 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 BO_ 780 ACC_02: 8 Gateway_D4C7 - SG_ ACC_02_CHK : 0|8@1+ (1,0) [0|255] "" HUD_C7,Kombi_D4 + SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" HUD_C7,Kombi_D4 SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" HUD_C7,Kombi_D4 SG_ ACC_Wunschgeschw_02 : 12|10@1+ (0.32,0) [0.00|326.72] "Unit_KiloMeterPerHour" HUD_C7,Kombi_D4 SG_ ACC_Status_Prim_Anz : 22|2@1+ (1.0,0.0) [0.0|3] "" HUD_C7,Kombi_D4 diff --git a/iqdbc_repo/iqdbc/safety/modes/volkswagen_common.h b/iqdbc_repo/iqdbc/safety/modes/volkswagen_common.h index f2a5ac78e..cc92e65d7 100644 --- a/iqdbc_repo/iqdbc/safety/modes/volkswagen_common.h +++ b/iqdbc_repo/iqdbc/safety/modes/volkswagen_common.h @@ -100,7 +100,7 @@ void can_set_checksum(CANPacket_t *packet); #define MSG_MOTOR_03 0x105U // RX from ECU, for driver throttle input and brake switch status #define MSG_TSK_02 0x10CU // RX from ECU, for ACC status from drivetrain coordinator #define MSG_ACC_05 0x10DU // RX from radar, for ACC status -#define MSG_ACC_01 0x109U // RX from radar, for ACC status (Audi B8) +#define MSG_ACC_01 0x109U // TX by OP, ACC control instructions to the drivetrain coordinator (Audi B8) static void volkswagen_common_init(void) { volkswagen_set_button_prev = false; diff --git a/iqdbc_repo/iqdbc/safety/modes/volkswagen_mlb.h b/iqdbc_repo/iqdbc/safety/modes/volkswagen_mlb.h index 3460f0332..ebc44dcc8 100644 --- a/iqdbc_repo/iqdbc/safety/modes/volkswagen_mlb.h +++ b/iqdbc_repo/iqdbc/safety/modes/volkswagen_mlb.h @@ -3,12 +3,42 @@ #include "iqdbc/safety/declarations.h" #include "iqdbc/safety/modes/volkswagen_common.h" +// -3.0 m/s^2 (VW_IQ_MIN_LONG_ACCEL, shared with MQB) faults the Audi Q5 ACC ECU and requires an +// ignition cycle to clear; MLB needs its own, stricter floor instead of the shared MQB constant. +#define VOLKSWAGEN_MLB_MIN_LONG_ACCEL -2950 + +// The real ECU flags ACC_Sollbeschleunigung as a stuck/implausible sensor if it holds the exact +// same value for too long, so the Python side dithers slightly around the inactive sentinel while +// disengaged. Accept a small band around VW_IQ_INACTIVE_LONG_ACCEL (+/-15 raw = +/-0.075 m/s^2) +// as "inactive" instead of requiring an exact match. +#define VOLKSWAGEN_MLB_INACTIVE_ACCEL_TOLERANCE 15 + +static bool volkswagen_mlb_long_accel_check(int desired_accel) { + int inactive_delta = desired_accel - VW_IQ_INACTIVE_LONG_ACCEL; + if ((inactive_delta >= -VOLKSWAGEN_MLB_INACTIVE_ACCEL_TOLERANCE) && (inactive_delta <= VOLKSWAGEN_MLB_INACTIVE_ACCEL_TOLERANCE)) { + return false; + } + // 0 m/s^2 ("hold current speed") is also accepted as an inactive/no-request value for MLB + if (desired_accel == 0) { + return false; + } + if (!controls_allowed) { + return true; + } + if (gas_pressed_prev && !volkswagen_allow_long_accel_with_gas_pressed) { + return true; + } + return (desired_accel > VW_IQ_MAX_LONG_ACCEL) || (desired_accel < VOLKSWAGEN_MLB_MIN_LONG_ACCEL); +} 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_LDW_02, 0, 8, .check_relay = true}, + {MSG_ACC_01, 0, 8, .check_relay = true}, {MSG_ACC_02, 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 +49,17 @@ 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); + volkswagen_allow_long_accel_with_gas_pressed = GET_FLAG(param, FLAG_VOLKSWAGEN_ALLOW_LONG_ACCEL_WITH_GAS_PRESSED); +#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 +81,27 @@ static void volkswagen_mlb_rx_hook(const CANPacket_t *msg) { } if (msg->addr == MSG_LS_01) { + // If using openpilot longitudinal, the stock ACC coordinator is relayed out, so the stalk main + // switch is the only remaining source of truth. Enter controls on falling edge of Set or Resume. + // Signal: LS_01.LS_Hauptschalter + // Signal: LS_01.LS_Tip_Setzen + // Signal: LS_01.LS_Tip_Wiederaufnahme + if (volkswagen_longitudinal) { + acc_main_on = GET_BIT(msg, 12U); + + 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 = acc_main_on; + } + volkswagen_set_button_prev = set_button; + volkswagen_resume_button_prev = resume_button; + + if (!acc_main_on) { + controls_allowed = false; + } + } + // Always exit controls on rising edge of Cancel // Signal: LS_01.LS_Abbrechen if (GET_BIT(msg, 13U)) { @@ -52,10 +110,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) -- MO_Fahrer_bremst (bit 35) sticks/is unreliable on real MLB hardware 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) { @@ -64,7 +122,7 @@ static void volkswagen_mlb_rx_hook(const CANPacket_t *msg) { brake_pressed = volkswagen_brake_pedal_switch || volkswagen_brake_pressure_detected; - if (msg->addr == MSG_TSK_02) { + if ((msg->addr == MSG_TSK_02) && !volkswagen_longitudinal) { // When using stock ACC, enter controls on rising edge of stock ACC engage, exit on disengage // Always exit controls on main switch off // Signal: TSK_02.TSK_Status @@ -80,7 +138,7 @@ static void volkswagen_mlb_rx_hook(const CANPacket_t *msg) { if (msg->bus == 2U) { // TODO: See if there's a bus-agnostic TSK message we can use instead - if (msg->addr == MSG_ACC_05) { + if ((msg->addr == MSG_ACC_05) && !volkswagen_longitudinal) { // When using stock ACC, enter controls on rising edge of stock ACC engage, exit on disengage // Always exit controls on main switch off // Signal: ACC_05.ACC_Status_ACC @@ -123,6 +181,17 @@ static bool volkswagen_mlb_tx_hook(const CANPacket_t *msg) { } } + // Safety check for ACC_01 acceleration request + // Signal: ACC_01.ACC_Sollbeschleunigung (acceleration in m/s^2, scale 0.005, offset -7.22) + // To avoid floating point math, scale upward and compare to pre-scaled safety m/s^2 boundaries + if (msg->addr == MSG_ACC_01) { + int desired_accel = ((((msg->data[4] & 0x07U) << 8) | msg->data[3]) * 5U) - 7220U; + + if (volkswagen_mlb_long_accel_check(desired_accel)) { + 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) { @@ -143,4 +212,4 @@ const safety_hooks volkswagen_mlb_hooks = { .get_counter = volkswagen_mqb_meb_get_counter, .get_checksum = volkswagen_mqb_meb_get_checksum, .compute_checksum = volkswagen_mqb_meb_compute_crc, -}; +}; \ No newline at end of file diff --git a/iqdbc_repo/iqdbc/safety/tests/common.py b/iqdbc_repo/iqdbc/safety/tests/common.py index fdae10db6..935fcb7c5 100644 --- a/iqdbc_repo/iqdbc/safety/tests/common.py +++ b/iqdbc_repo/iqdbc/safety/tests/common.py @@ -926,12 +926,12 @@ class SafetyTest(SafetyTestBase): if attr.startswith('TestSubaru') and current_test == 'TestVolkswagenMqbLongSafety': 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 and Honda Nidec ACC HUD messages overlap + 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 and Honda Bosch Radarless ACC HUD messages overlap + 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/iqdbc_repo/iqdbc/safety/tests/test_volkswagen_mlb.py b/iqdbc_repo/iqdbc/safety/tests/test_volkswagen_mlb.py old mode 100755 new mode 100644 index de7baaa63..c4d828216 --- a/iqdbc_repo/iqdbc/safety/tests/test_volkswagen_mlb.py +++ b/iqdbc_repo/iqdbc/safety/tests/test_volkswagen_mlb.py @@ -1,17 +1,24 @@ #!/usr/bin/env python3 import unittest +import numpy as np from iqdbc.car.structs import CarParams from iqdbc.safety.tests.libsafety import libsafety_py import iqdbc.safety.tests.common as common from iqdbc.safety.tests.common import CANPackerSafety +from iqdbc.car.volkswagen.values import VolkswagenSafetyFlags + +MAX_ACCEL = 2.0 +MIN_ACCEL = -2.95 MSG_LH_EPS_03 = 0x9F # RX from EPS, for driver steering torque +MSG_ACC_01 = 0x109 # TX by OP, ACC acceleration request to the drivetrain coordinator MSG_ESP_03 = 0x103 # RX from ABS, for wheel speeds MSG_MOTOR_03 = 0x105 # RX from ECU, for driver throttle input and driver brake input MSG_ESP_05 = 0x106 # RX from ABS, for brake light state MSG_LS_01 = 0x10B # TX by OP, ACC control buttons for cancel/resume MSG_TSK_02 = 0x10C # RX from ECU, for ACC status from drivetrain coordinator MSG_HCA_01 = 0x126 # TX by OP, Heading Control Assist steering torque +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 @@ -72,10 +79,16 @@ class TestVolkswagenMlbSafetyBase(common.CarSafetyTest, common.DriverTorqueSteer 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) + # Acceleration request to drivetrain coordinator + def _acc_01_msg(self, accel): + values = {"ACC_Sollbeschleunigung": accel} + return self.packer.make_can_msg_safety("ACC_01", 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)] @@ -137,5 +150,63 @@ class TestVolkswagenMlbStockSafety(TestVolkswagenMlbSafetyBase): self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after cancel") +class TestVolkswagenMlbLongSafety(TestVolkswagenMlbSafetyBase): + TX_MSGS = [[MSG_HCA_01, 0], [MSG_LDW_02, 0], [MSG_ACC_01, 0], [MSG_ACC_02, 0]] + FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_01, MSG_LDW_02, MSG_ACC_01, MSG_ACC_02]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_01, MSG_LDW_02, MSG_ACC_01, MSG_ACC_02)} + INACTIVE_ACCEL = 3.01 + + def setUp(self): + self.packer = CANPackerSafety("vw_mlb") + self.safety = libsafety_py.libsafety + safety_param = VolkswagenSafetyFlags.LONG_CONTROL | VolkswagenSafetyFlags.ALLOW_LONG_ACCEL_WITH_GAS_PRESSED + self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenMlb, safety_param) + self.safety.init_tests() + + # 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 + 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(main_switch=0, bus=0)) + self._rx(self._ls_01_msg(_set=(button == "set"), resume=(button == "resume"), bus=0)) + self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} rising edge") + self._rx(self._ls_01_msg(bus=0)) + self.assertTrue(self.safety.get_controls_allowed(), f"controls not allowed on {button} falling edge") + + def test_main_switch(self): + # Disable as soon as the ACC main switch turns off + self._rx(self._ls_01_msg(bus=0)) + self.safety.set_controls_allowed(1) + self._rx(self._ls_01_msg(main_switch=0, bus=0)) + self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after ACC main switch off") + + def test_accel_safety_check(self): + for controls_allowed in [True, False]: + for accel in np.concatenate((np.arange(MIN_ACCEL - 2, MAX_ACCEL + 2, 0.03), [0, self.INACTIVE_ACCEL])): + accel = round(accel, 2) + is_inactive_accel = accel == self.INACTIVE_ACCEL + 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_accel_allowed_with_gas_pressed(self): + self._rx(self._user_gas_msg(1)) + self.safety.set_controls_allowed(True) + self.assertTrue(self._tx(self._acc_01_msg(0.5))) + + if __name__ == "__main__": unittest.main()