Ford: AEB safety

board/safety/safety_ford.h

@@ -5,6 +5,7 @@
 #define MSG_BrakeSysFeatures      0x415   // RX from ABS, for vehicle speed
 #define MSG_EngVehicleSpThrottle2 0x202   // RX from PCM, for second vehicle speed
 #define MSG_Yaw_Data_FD1          0x91    // RX from RCM, for yaw rate
+#define MSG_ACCDATA_2             0x187   // RX from IPMA, for AEB status
 #define MSG_Steering_Data_FD1     0x083   // TX by OP, various driver switches and LKAS/CC buttons
 #define MSG_ACCDATA               0x186   // TX by OP, ACC controls
 #define MSG_ACCDATA_3             0x18A   // TX by OP, ACC/TJA user interface
@@ -44,6 +45,7 @@ AddrCheckStruct ford_addr_checks[] = {
   // TODO: MSG_EngVehicleSpThrottle2 has a counter that skips by 2, understand and enable counter check
   {.msg = {{MSG_EngVehicleSpThrottle2, 0, 8, .check_checksum = true, .quality_flag=true, .expected_timestep = 20000U}, { 0 }, { 0 }}},
   {.msg = {{MSG_Yaw_Data_FD1, 0, 8, .check_checksum = true, .max_counter = 255U, .quality_flag=true, .expected_timestep = 10000U}, { 0 }, { 0 }}},
+  {.msg = {{MSG_ACCDATA_2, 2, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}},  // TODO: counter/checksum
   // These messages have no counter or checksum
   {.msg = {{MSG_EngBrakeData, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}},
   {.msg = {{MSG_EngVehicleSpThrottle, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}},
@@ -135,6 +137,7 @@ static bool ford_get_quality_flag_valid(CANPacket_t *to_push) {
 const uint16_t FORD_PARAM_LONGITUDINAL = 1;
 
 bool ford_longitudinal = false;
+bool ford_stock_aeb = false;
 
 const LongitudinalLimits FORD_LONG_LIMITS = {
   // acceleration cmd limits (used for brakes)
@@ -222,6 +225,12 @@ static int ford_rx_hook(CANPacket_t *to_push) {
       update_sample(&angle_meas, ROUND(current_curvature * (float)FORD_STEERING_LIMITS.angle_deg_to_can));
     }
 
+    // Update AEB status
+    if (addr == MSG_ACCDATA_2) {
+      // Signal: CmbbBrkDecel_B_Rq
+      ford_stock_aeb = GET_BIT(to_push, 15) == 1U;
+    }
+
     // Update gas pedal
     if (addr == MSG_EngVehicleSpThrottle) {
       // Pedal position: (0.1 * val) in percent
@@ -267,12 +276,22 @@ static int ford_tx_hook(CANPacket_t *to_send) {
     // Signal: CmbbDeny_B_Actl
     int cmbb_deny = GET_BIT(to_send, 37U);
 
+    // Signal: CmbbDeny_B_Actl
+    int cmbb_deny = GET_BIT(to_send, 37);
+    // Signal: CmbbEngTqMn_B_Rq
+    int cmbb_engine_torque_min = GET_BIT(to_send, 52);
+
     bool violation = false;
     violation |= longitudinal_accel_checks(accel, FORD_LONG_LIMITS);
     violation |= longitudinal_gas_checks(gas, FORD_LONG_LIMITS);
 
-    // Safety check for stock AEB
+    // Safety checks for stock AEB
     violation |= cmbb_deny != 0; // do not prevent stock AEB actuation
+    if (ford_stock_aeb) {
+      violation |= accel != FORD_LONG_LIMITS.inactive_accel;
+      violation |= gas != FORD_LONG_LIMITS.inactive_gas;
+      violation |= cmbb_engine_torque_min != 1U;
+    }
 
     if (violation) {
       tx = 0;
@@ -365,6 +384,7 @@ static int ford_fwd_hook(int bus_num, int addr) {
 
 static const addr_checks* ford_init(uint16_t param) {
   UNUSED(param);
+  ford_stock_aeb = false;
 #ifdef ALLOW_DEBUG
   ford_longitudinal = GET_FLAG(param, FORD_PARAM_LONGITUDINAL);
 #endif

tests/safety/test_ford.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+import itertools
 import numpy as np
 import unittest
 
@@ -13,6 +14,7 @@
 MSG_BrakeSysFeatures = 0x415       # RX from ABS, for vehicle speed
 MSG_EngVehicleSpThrottle2 = 0x202  # RX from PCM, for second vehicle speed
 MSG_Yaw_Data_FD1 = 0x91            # RX from RCM, for yaw rate
+MSG_ACCDATA_2 = 0x187              # RX from IPMA, for AEB status
 MSG_Steering_Data_FD1 = 0x083      # TX by OP, various driver switches and LKAS/CC buttons
 MSG_ACCDATA = 0x186                # TX by OP, ACC controls
 MSG_ACCDATA_3 = 0x18A              # TX by OP, ACC/TJA user interface
@@ -135,6 +137,10 @@ def _yaw_rate_msg(self, curvature: float, speed: float, quality_flag=True):
     self.__class__.cnt_yaw_rate += 1
     return self.packer.make_can_msg_panda("Yaw_Data_FD1", 0, values, fix_checksum=checksum)
 
+  def _stock_aeb_msg(self, aeb: bool):
+    values = {"CmbbBrkDecel_B_Rq": 1 if aeb else 0}
+    return self.packer.make_can_msg_panda("ACCDATA_2", 0, values)
+
   # Drive throttle input
   def _user_gas_msg(self, gas: float):
     values = {"ApedPos_Pc_ActlArb": gas}
@@ -373,23 +379,27 @@ def setUp(self):
     self.safety.init_tests()
 
   # ACC command
-  def _acc_command_msg(self, gas: float, brake: float, cmbb_deny: bool = False):
+  def _acc_command_msg(self, gas: float, brake: float, cmbb_deny: bool = False, min_engine_torque: bool = False):
     values = {
-      "AccPrpl_A_Rq": gas,                       # [-5|5.23] m/s^2
-      "AccBrkTot_A_Rq": brake,                   # [-20|11.9449] m/s^2
-      "CmbbDeny_B_Actl": 1 if cmbb_deny else 0,  # [0|1] deny AEB actuation
+      "AccPrpl_A_Rq": gas,      # [-5|5.23] m/s^2
+      "AccBrkTot_A_Rq": brake,  # [-20|11.9449] m/s^2
+      "CmbbDeny_B_Actl": 1 if cmbb_deny else 0,
+      "CmbbEngTqMn_B_Rq": 1 if min_engine_torque else 0,
     }
     return self.packer.make_can_msg_panda("ACCDATA", 0, values)
 
   def test_stock_aeb(self):
-    # Test that CmbbDeny_B_Actl is never 1, it prevents the ABS module from actuating AEB requests from ACCDATA_2
     for controls_allowed in (True, False):
       self.safety.set_controls_allowed(controls_allowed)
-      for cmbb_deny in (True, False):
-        should_tx = not cmbb_deny
-        self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.INACTIVE_GAS, self.INACTIVE_ACCEL, cmbb_deny)))
-        should_tx = controls_allowed and not cmbb_deny
-        self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.MAX_GAS, self.MAX_ACCEL, cmbb_deny)))
+      for aeb in (True, False):
+        self.assertTrue(self._rx(self._stock_aeb_msg(aeb)))
+
+        for cmbb_deny in (True, False):
+          for min_engine_torque in (True, False):
+            # cmbb_deny should always be false, and min_engine_torque should be true only when AEB
+            should_tx = not cmbb_deny and (not aeb or min_engine_torque)
+            self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.INACTIVE_GAS, self.INACTIVE_ACCEL,
+                                                                       cmbb_deny, min_engine_torque)))
 
   def test_gas_safety_check(self):
     for controls_allowed in (True, False):