VW MLB: Long added

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:
+    if self.CP.openpilotLongitudinalControl and self.CCS in (mqbcan, mlbcan) and not self.acc_counter_seeded and CS.acc_stock_counters:
-      for name in ("ACC_02", "ACC_06", "ACC_07", "ACC_10"):
+      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)

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)
+    cruise_main_switch = bool(pt_cp.vl["LS_01"]["LS_Hauptschalter"])
-    # currently regulating speed (3), driver accel override (4), brake only (5)
+    if not self.CP.pcmCruise:
-    if self.CP.carFingerprint == CAR.PORSCHE_MACAN_MK1:
+      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
+    allow_lat_only = self._params.get_bool("AllowLateralWhenLongUnavailable") and self._params.get_bool("AolEnabled")
-    ret.lateralAvailable = ret.cruiseState.available
+    ret.cruiseFaultLateralMode = allow_lat_only and ret.accFaulted and cruise_main_switch
-    ret.blockPcmEnable = False
+    ret.lateralAvailable = ret.cruiseState.available or ret.cruiseFaultLateralMode
+    ret.blockPcmEnable = ret.cruiseFaultLateralMode
 
     self.cruise_faulted = ret.accFaulted
     self.frame += 1

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):

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):
+def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control,
-  return mqb_create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control, entering, special_mode, special_active)
+                           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):
+def acc_control_value(main_switch_on, long_active, cruiseOverride, accFaulted):
-  return 0
+  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):
+def create_acc_accel_control(packer, bus, accel, acc_control, stopping):
-  values = {}
+  acc_enabled = acc_control in (3, 4)
-  return [packer.make_can_msg("ACC_05", bus, values)]
+
+  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):
+def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, leadDistance, distanceBars, fcw_alert, leadVisible,
-  values = {}
+                           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

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))

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

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;

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) {

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
+            # Volkswagen MQB/MLB and Honda Nidec ACC HUD messages overlap
-            if attr == 'TestVolkswagenMqbLongSafety' and current_test.startswith('TestHondaNidec'):
+            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
+            # Volkswagen MQB/MLB and Honda Bosch Radarless ACC HUD messages overlap
-            if attr == 'TestVolkswagenMqbLongSafety' and current_test.startswith('TestHondaBoschRadarless'):
+            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

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):
+  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}
+    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()