VW MLB: longitudinal support for cars on ACC_01

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 **************************************** #
 

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

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"

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

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,
   }

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,

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

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
 

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

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

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