Volkswagen safety updates: Phase 1 (#444)

* Checkpoint Panda refactoring updates * Rename MQB safety tests in preparation for PQ * Refactor MQB tests, add missing torque test * Bring in MQB init without CRC LUT setup * Fix to ACC_06 test case * Fix to ACC_06 test case * Tweak comment for clarity * Drop superfluous return
commaai · Feb 19, 2020 · ccf75c4 · ccf75c4
1 parent 769ade0
commit ccf75c4
Show file tree

Hide file tree

Showing 7 changed files with 222 additions and 166 deletions.
diff --git a/board/safety.h b/board/safety.h
@@ -31,7 +31,7 @@
 #define SAFETY_TESLA 10U
 #define SAFETY_SUBARU 11U
 #define SAFETY_MAZDA 13U
-#define SAFETY_VOLKSWAGEN 15U
+#define SAFETY_VOLKSWAGEN_MQB 15U
 #define SAFETY_TOYOTA_IPAS 16U
 #define SAFETY_ALLOUTPUT 17U
 #define SAFETY_GM_ASCM 18U
@@ -185,7 +185,7 @@ const safety_hook_config safety_hook_registry[] = {
   {SAFETY_CHRYSLER, &chrysler_hooks},
   {SAFETY_SUBARU, &subaru_hooks},
   {SAFETY_MAZDA, &mazda_hooks},
-  {SAFETY_VOLKSWAGEN, &volkswagen_hooks},
+  {SAFETY_VOLKSWAGEN_MQB, &volkswagen_mqb_hooks},
   {SAFETY_NOOUTPUT, &nooutput_hooks},
 #ifdef ALLOW_DEBUG
   {SAFETY_CADILLAC, &cadillac_hooks},

diff --git a/board/safety/safety_volkswagen.h b/board/safety/safety_volkswagen.h
@@ -1,142 +1,156 @@
-// Safety-relevant CAN messages for the Volkswagen MQB platform.
-#define MSG_EPS_01              0x09F
-#define MSG_MOTOR_20            0x121
-#define MSG_ACC_06              0x122
-#define MSG_HCA_01              0x126
-#define MSG_GRA_ACC_01          0x12B
-#define MSG_LDW_02              0x397
-
+// Safety-relevant steering constants for Volkswagen
 const int VOLKSWAGEN_MAX_STEER = 250;               // 2.5 Nm (EPS side max of 3.0Nm with fault if violated)
 const int VOLKSWAGEN_MAX_RT_DELTA = 75;             // 4 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 50 ; 50 * 1.5 for safety pad = 75
 const uint32_t VOLKSWAGEN_RT_INTERVAL = 250000;     // 250ms between real time checks
-const int VOLKSWAGEN_MAX_RATE_UP = 4;               // 2.0 Nm/s available rate of change from the steering rack (EPS side delta-limit of 5.0 Nm/s)
-const int VOLKSWAGEN_MAX_RATE_DOWN = 10;            // 5.0 Nm/s available rate of change from the steering rack (EPS side delta-limit of 5.0 Nm/s)
+const int VOLKSWAGEN_MAX_RATE_UP = 4;               // 2.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
+const int VOLKSWAGEN_MAX_RATE_DOWN = 10;            // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
 const int VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE = 80;
 const int VOLKSWAGEN_DRIVER_TORQUE_FACTOR = 3;
 
-// MSG_GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
-const AddrBus VOLKSWAGEN_TX_MSGS[] = {{MSG_HCA_01, 0}, {MSG_GRA_ACC_01, 0}, {MSG_GRA_ACC_01, 2}, {MSG_LDW_02, 0}};
+// Safety-relevant CAN messages for the Volkswagen MQB platform
+#define MSG_EPS_01      0x09F   // RX from EPS, for driver steering torque
+#define MSG_MOTOR_20    0x121   // RX from ECU, for driver throttle input
+#define MSG_ACC_06      0x122   // RX from ACC radar, for status and engagement
+#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_LDW_02      0x397   // TX by OP, Lane line recognition and text alerts
+
+// Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
+const AddrBus VOLKSWAGEN_MQB_TX_MSGS[] = {{MSG_HCA_01, 0}, {MSG_GRA_ACC_01, 0}, {MSG_GRA_ACC_01, 2}, {MSG_LDW_02, 0}};
+const int VOLKSWAGEN_MQB_TX_MSGS_LEN = sizeof(VOLKSWAGEN_MQB_TX_MSGS) / sizeof(VOLKSWAGEN_MQB_TX_MSGS[0]);
 
 // TODO: do checksum and counter checks
-AddrCheckStruct volkswagen_rx_checks[] = {
+AddrCheckStruct volkswagen_mqb_rx_checks[] = {
   {.addr = {MSG_EPS_01}, .bus = 0, .expected_timestep = 10000U},
   {.addr = {MSG_ACC_06}, .bus = 0, .expected_timestep = 20000U},
   {.addr = {MSG_MOTOR_20}, .bus = 0, .expected_timestep = 20000U},
 };
+const int VOLKSWAGEN_MQB_RX_CHECKS_LEN = sizeof(volkswagen_mqb_rx_checks) / sizeof(volkswagen_mqb_rx_checks[0]);
 
-const int VOLKSWAGEN_RX_CHECK_LEN = sizeof(volkswagen_rx_checks) / sizeof(volkswagen_rx_checks[0]);
 
-struct sample_t volkswagen_torque_driver;  // last few driver torques measured
+struct sample_t volkswagen_torque_driver; // Last few driver torques measured
 int volkswagen_rt_torque_last = 0;
 int volkswagen_desired_torque_last = 0;
 uint32_t volkswagen_ts_last = 0;
 int volkswagen_gas_prev = 0;
+int volkswagen_torque_msg = 0;
+int volkswagen_lane_msg = 0;
+
+static void volkswagen_mqb_init(int16_t param) {
+  UNUSED(param);
 
-static int volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+  controls_allowed = false;
+  relay_malfunction = false;
+  volkswagen_torque_msg = MSG_HCA_01;
+  volkswagen_lane_msg = MSG_LDW_02;
+}
+
+static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 
-  bool valid = addr_safety_check(to_push, volkswagen_rx_checks, VOLKSWAGEN_RX_CHECK_LEN,
+  bool valid = addr_safety_check(to_push, volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_RX_CHECKS_LEN,
                                  NULL, NULL, NULL);
 
   if (valid) {
-   int bus = GET_BUS(to_push);
-   int addr = GET_ADDR(to_push);
-
-   // Update driver input torque samples from EPS_01.Driver_Strain for absolute torque, and EPS_01.Driver_Strain_VZ
-   // for the direction.
-   if ((bus == 0) && (addr == MSG_EPS_01)) {
-     int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1F) << 8);
-     int sign = (GET_BYTE(to_push, 6) & 0x80) >> 7;
-     if (sign == 1) {
-       torque_driver_new *= -1;
-     }
-
-     update_sample(&volkswagen_torque_driver, torque_driver_new);
-   }
-
-   // Monitor ACC_06.ACC_Status_ACC for stock ACC status. Because the current MQB port is lateral-only, OP's control
-   // allowed state is directly driven by stock ACC engagement. Permit the ACC message to come from either bus, in
-   // order to accommodate future camera-side integrations if needed.
-   if (addr == MSG_ACC_06) {
-     int acc_status = (GET_BYTE(to_push, 7) & 0x70) >> 4;
-     controls_allowed = ((acc_status == 3) || (acc_status == 4) || (acc_status == 5)) ? 1 : 0;
-   }
-
-   // exit controls on rising edge of gas press. Bits [12-20)
-   if (addr == MSG_MOTOR_20) {
-     int gas = (GET_BYTES_04(to_push) >> 12) & 0xFF;
-     if ((gas > 0) && (volkswagen_gas_prev == 0)) {
-       controls_allowed = 0;
-     }
-     volkswagen_gas_prev = gas;
-   }
-
-   if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) {
-     relay_malfunction = true;
-   }
+    int bus = GET_BUS(to_push);
+    int addr = GET_ADDR(to_push);
+
+    // Update driver input torque samples
+    // Signal: EPS_01.Driver_Strain (absolute torque)
+    // Signal: EPS_01.Driver_Strain_VZ (direction)
+    if ((bus == 0) && (addr == MSG_EPS_01)) {
+      int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1F) << 8);
+      int sign = (GET_BYTE(to_push, 6) & 0x80) >> 7;
+      if (sign == 1) {
+        torque_driver_new *= -1;
+      }
+      update_sample(&volkswagen_torque_driver, torque_driver_new);
+    }
+
+    // Update ACC status from radar for controls-allowed state
+    // Signal: ACC_06.ACC_Status_ACC
+    if ((bus == 0) && (addr == MSG_ACC_06)) {
+      int acc_status = (GET_BYTE(to_push, 7) & 0x70) >> 4;
+      controls_allowed = ((acc_status == 3) || (acc_status == 4) || (acc_status == 5)) ? 1 : 0;
+    }
+
+    // Exit controls on rising edge of gas press
+    // Signal: Motor_20.MO_Fahrpedalrohwert_01
+    if ((bus == 0) && (addr == MSG_MOTOR_20)) {
+      int gas = (GET_BYTES_04(to_push) >> 12) & 0xFF;
+      if ((gas > 0) && (volkswagen_gas_prev == 0)) {
+        controls_allowed = 0;
+      }
+      volkswagen_gas_prev = gas;
+    }
+
+    // If there are HCA messages on bus 0 not sent by OP, there's a relay problem
+    if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) {
+      relay_malfunction = true;
+    }
   }
   return valid;
 }
 
-static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static bool volkswagen_steering_check(int desired_torque) {
+  bool violation = false;
+  uint32_t ts = TIM2->CNT;
+
+  if (controls_allowed) {
+    // *** global torque limit check ***
+    violation |= max_limit_check(desired_torque, VOLKSWAGEN_MAX_STEER, -VOLKSWAGEN_MAX_STEER);
+
+    // *** torque rate limit check ***
+    violation |= driver_limit_check(desired_torque, volkswagen_desired_torque_last, &volkswagen_torque_driver,
+      VOLKSWAGEN_MAX_STEER, VOLKSWAGEN_MAX_RATE_UP, VOLKSWAGEN_MAX_RATE_DOWN,
+      VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE, VOLKSWAGEN_DRIVER_TORQUE_FACTOR);
+    volkswagen_desired_torque_last = desired_torque;
+
+    // *** torque real time rate limit check ***
+    violation |= rt_rate_limit_check(desired_torque, volkswagen_rt_torque_last, VOLKSWAGEN_MAX_RT_DELTA);
+
+    // every RT_INTERVAL set the new limits
+    uint32_t ts_elapsed = get_ts_elapsed(ts, volkswagen_ts_last);
+    if (ts_elapsed > VOLKSWAGEN_RT_INTERVAL) {
+      volkswagen_rt_torque_last = desired_torque;
+      volkswagen_ts_last = ts;
+    }
+  }
+
+  // no torque if controls is not allowed
+  if (!controls_allowed && (desired_torque != 0)) {
+    violation = true;
+  }
+
+  // reset to 0 if either controls is not allowed or there's a violation
+  if (violation || !controls_allowed) {
+    volkswagen_desired_torque_last = 0;
+    volkswagen_rt_torque_last = 0;
+    volkswagen_ts_last = ts;
+  }
+
+  return violation;
+}
+
+static int volkswagen_mqb_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   int addr = GET_ADDR(to_send);
   int bus = GET_BUS(to_send);
   int tx = 1;
 
-  if (!msg_allowed(addr, bus, VOLKSWAGEN_TX_MSGS, sizeof(VOLKSWAGEN_TX_MSGS)/sizeof(VOLKSWAGEN_TX_MSGS[0]))) {
-    tx = 0;
-  }
-
-  if (relay_malfunction) {
+  if (!msg_allowed(addr, bus, VOLKSWAGEN_MQB_TX_MSGS, VOLKSWAGEN_MQB_TX_MSGS_LEN) || relay_malfunction) {
     tx = 0;
   }
 
-  // Safety check for HCA_01 Heading Control Assist torque.
+  // Safety check for HCA_01 Heading Control Assist torque
+  // Signal: HCA_01.Assist_Torque (absolute torque)
+  // Signal: HCA_01.Assist_VZ (direction)
   if (addr == MSG_HCA_01) {
-    bool violation = false;
-
     int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x3F) << 8);
     int sign = (GET_BYTE(to_send, 3) & 0x80) >> 7;
     if (sign == 1) {
       desired_torque *= -1;
     }
 
-    uint32_t ts = TIM2->CNT;
-
-    if (controls_allowed) {
-
-      // *** global torque limit check ***
-      violation |= max_limit_check(desired_torque, VOLKSWAGEN_MAX_STEER, -VOLKSWAGEN_MAX_STEER);
-
-      // *** torque rate limit check ***
-      violation |= driver_limit_check(desired_torque, volkswagen_desired_torque_last, &volkswagen_torque_driver,
-        VOLKSWAGEN_MAX_STEER, VOLKSWAGEN_MAX_RATE_UP, VOLKSWAGEN_MAX_RATE_DOWN,
-        VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE, VOLKSWAGEN_DRIVER_TORQUE_FACTOR);
-      volkswagen_desired_torque_last = desired_torque;
-
-      // *** torque real time rate limit check ***
-      violation |= rt_rate_limit_check(desired_torque, volkswagen_rt_torque_last, VOLKSWAGEN_MAX_RT_DELTA);
-
-      // every RT_INTERVAL set the new limits
-      uint32_t ts_elapsed = get_ts_elapsed(ts, volkswagen_ts_last);
-      if (ts_elapsed > VOLKSWAGEN_RT_INTERVAL) {
-        volkswagen_rt_torque_last = desired_torque;
-        volkswagen_ts_last = ts;
-      }
-    }
-
-    // no torque if controls is not allowed
-    if (!controls_allowed && (desired_torque != 0)) {
-      violation = true;
-    }
-
-    // reset to 0 if either controls is not allowed or there's a violation
-    if (violation || !controls_allowed) {
-      volkswagen_desired_torque_last = 0;
-      volkswagen_rt_torque_last = 0;
-      volkswagen_ts_last = ts;
-    }
-
-    if (violation) {
+    if (volkswagen_steering_check(desired_torque)) {
       tx = 0;
     }
   }
@@ -158,38 +172,37 @@ static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
   int addr = GET_ADDR(to_fwd);
   int bus_fwd = -1;
 
-  // NOTE: Will need refactoring for other bus layouts, such as no-forwarding at camera or J533 running-gear CAN
-
   if (!relay_malfunction) {
     switch (bus_num) {
       case 0:
-        // Forward all traffic from J533 gateway to Extended CAN devices.
+        // Forward all traffic from the Extended CAN onward
         bus_fwd = 2;
         break;
       case 2:
-        if ((addr == MSG_HCA_01) || (addr == MSG_LDW_02)) {
-          // OP takes control of the Heading Control Assist and Lane Departure Warning messages from the camera.
+        if ((addr == volkswagen_torque_msg) || (addr == volkswagen_lane_msg)) {
+          // OP takes control of the Heading Control Assist and Lane Departure Warning messages from the camera
           bus_fwd = -1;
         } else {
-          // Forward all remaining traffic from Extended CAN devices to J533 gateway.
+          // Forward all remaining traffic from Extended CAN devices to J533 gateway
           bus_fwd = 0;
         }
         break;
       default:
-        // No other buses should be in use; fallback to do-not-forward.
+        // No other buses should be in use; fallback to do-not-forward
         bus_fwd = -1;
         break;
     }
   }
   return bus_fwd;
 }
 
-const safety_hooks volkswagen_hooks = {
-  .init = nooutput_init,
-  .rx = volkswagen_rx_hook,
-  .tx = volkswagen_tx_hook,
+// Volkswagen MQB platform
+const safety_hooks volkswagen_mqb_hooks = {
+  .init = volkswagen_mqb_init,
+  .rx = volkswagen_mqb_rx_hook,
+  .tx = volkswagen_mqb_tx_hook,
   .tx_lin = nooutput_tx_lin_hook,
   .fwd = volkswagen_fwd_hook,
-  .addr_check = volkswagen_rx_checks,
-  .addr_check_len = sizeof(volkswagen_rx_checks) / sizeof(volkswagen_rx_checks[0]),
+  .addr_check = volkswagen_mqb_rx_checks,
+  .addr_check_len = sizeof(volkswagen_mqb_rx_checks) / sizeof(volkswagen_mqb_rx_checks[0]),
 };
diff --git a/python/__init__.py b/python/__init__.py
@@ -123,7 +123,7 @@ class Panda(object):
   SAFETY_TESLA = 10
   SAFETY_SUBARU = 11
   SAFETY_MAZDA = 13
-  SAFETY_VOLKSWAGEN = 15
+  SAFETY_VOLKSWAGEN_MQB = 15
   SAFETY_TOYOTA_IPAS = 16
   SAFETY_ALLOUTPUT = 17
   SAFETY_GM_ASCM = 18

diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py
@@ -91,10 +91,12 @@
 void set_subaru_torque_driver(int min, int max);
 
 void init_tests_volkswagen(void);
+int get_volkswagen_gas_prev(void);
+int get_volkswagen_torque_driver_min(void);
+int get_volkswagen_torque_driver_max(void);
 void set_volkswagen_desired_torque_last(int t);
 void set_volkswagen_rt_torque_last(int t);
 void set_volkswagen_torque_driver(int min, int max);
-int get_volkswagen_gas_prev(void);
 
 """)
 

diff --git a/tests/safety/test.c b/tests/safety/test.c
@@ -148,6 +148,14 @@ void set_volkswagen_torque_driver(int min, int max){
   volkswagen_torque_driver.max = max;
 }
 
+int get_volkswagen_torque_driver_min(void){
+  return volkswagen_torque_driver.min;
+}
+
+int get_volkswagen_torque_driver_max(void){
+  return volkswagen_torque_driver.max;
+}
+
 int get_chrysler_torque_meas_min(void){
   return chrysler_torque_meas.min;
 }