Ford: add safety code for LCA vehicles

board/safety/safety_ford.h

@@ -1,99 +1,130 @@
-// board enforces
-//   in-state
-//      accel set/resume
-//   out-state
-//      cancel button
-//      accel rising edge
-//      brake rising edge
-//      brake > 0mph
+// Safety-relevant CAN messages for Ford vehicles.
+#define MSG_ENG_BRAKE_DATA            0x165  // RX from PCM, for driver brake pedal and cruise state
+#define MSG_ENG_VEHICLE_SP_THROTTLE   0x204  // RX from PCM, for driver throttle input
+#define MSG_DESIRED_TORQ_BRK          0x213  // RX from ABS, for standstill state
+#define MSG_STEERING_DATA_FD1         0x083  // TX by OP, ACC control buttons for cancel
+#define MSG_LANE_ASSIST_DATA1         0x3CA  // TX by OP, Lane Keeping Assist
+#define MSG_LATERAL_MOTION_CONTROL    0x3D3  // TX by OP, Lane Centering Assist
+#define MSG_IPMA_DATA                 0x3D8  // TX by OP, IPMA HUD user interface
+
+// CAN bus numbers.
+#define FORD_MAIN_BUS 0U
+#define FORD_CAM_BUS  2U
+
+const CanMsg FORD_TX_MSGS[] = {
+  {MSG_STEERING_DATA_FD1, 0, 8},
+  {MSG_LANE_ASSIST_DATA1, 0, 8},
+  {MSG_LATERAL_MOTION_CONTROL, 0, 8},
+  {MSG_IPMA_DATA, 0, 8},
+};
+#define FORD_TX_LEN (sizeof(FORD_TX_MSGS) / sizeof(FORD_TX_MSGS[0]))
+
+AddrCheckStruct ford_addr_checks[] = {
+  {.msg = {{MSG_ENG_BRAKE_DATA, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}},
+  {.msg = {{MSG_ENG_VEHICLE_SP_THROTTLE, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}},
+  {.msg = {{MSG_DESIRED_TORQ_BRK, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}},
+};
+#define FORD_ADDR_CHECK_LEN (sizeof(ford_addr_checks) / sizeof(ford_addr_checks[0]))
+addr_checks ford_rx_checks = {ford_addr_checks, FORD_ADDR_CHECK_LEN};
 
 
+static bool ford_lkas_msg_check(int addr) {
+  return (addr == MSG_LANE_ASSIST_DATA1)
+      || (addr == MSG_LATERAL_MOTION_CONTROL)
+      || (addr == MSG_IPMA_DATA);
+}
+
 static int ford_rx_hook(CANPacket_t *to_push) {
+  bool valid = addr_safety_check(to_push, &ford_rx_checks, NULL, NULL, NULL);
 
-  int addr = GET_ADDR(to_push);
-  int bus = GET_BUS(to_push);
-  bool alt_exp_allow_gas = alternative_experience & ALT_EXP_DISABLE_DISENGAGE_ON_GAS;
+  if (valid && (GET_BUS(to_push) == FORD_MAIN_BUS)) {
+    int addr = GET_ADDR(to_push);
 
-  if (addr == 0x217) {
-    // wheel speeds are 14 bits every 16
-    vehicle_moving = false;
-    for (int i = 0; i < 8; i += 2) {
-      vehicle_moving |= GET_BYTE(to_push, i) | (GET_BYTE(to_push, (int)(i + 1)) & 0xFCU);
+    // Update in motion state from standstill signal
+    if (addr == MSG_DESIRED_TORQ_BRK) {
+      // Signal: VehStop_D_Stat
+      vehicle_moving = ((GET_BYTE(to_push, 3) >> 3) & 0x3U) == 0U;
     }
-  }
 
-  // state machine to enter and exit controls
-  if (addr == 0x83) {
-    bool cancel = GET_BYTE(to_push, 1) & 0x1U;
-    bool set_or_resume = GET_BYTE(to_push, 3) & 0x30U;
-    if (cancel) {
-      controls_allowed = 0;
-    }
-    if (set_or_resume) {
-      controls_allowed = 1;
+    // Update gas pedal
+    if (addr == MSG_ENG_VEHICLE_SP_THROTTLE) {
+      // Pedal position: (0.1 * val) in percent
+      // Signal: ApedPos_Pc_ActlArb
+      gas_pressed = (((GET_BYTE(to_push, 0) & 0x03U) << 8) | GET_BYTE(to_push, 1)) > 0U;
     }
-  }
 
-  // exit controls on rising edge of brake press or on brake press when
-  // speed > 0
-  if (addr == 0x165) {
-    brake_pressed = GET_BYTE(to_push, 0) & 0x20U;
-    if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
-      controls_allowed = 0;
-    }
-    brake_pressed_prev = brake_pressed;
-  }
+    // Update brake pedal and cruise state
+    if (addr == MSG_ENG_BRAKE_DATA) {
+      // Signal: BpedDrvAppl_D_Actl
+      brake_pressed = ((GET_BYTE(to_push, 0) >> 4) & 0x3U) == 2U;
+
+      // Signal: CcStat_D_Actl
+      unsigned int cruise_state = GET_BYTE(to_push, 1) & 0x07U;
+      bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U);
 
-  // exit controls on rising edge of gas press
-  if (addr == 0x204) {
-    gas_pressed = ((GET_BYTE(to_push, 0) & 0x03U) | GET_BYTE(to_push, 1)) != 0U;
-    if (!alt_exp_allow_gas && gas_pressed && !gas_pressed_prev) {
-      controls_allowed = 0;
+      // Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages
+      if (cruise_engaged && !cruise_engaged_prev) {
+        controls_allowed = true;
+      }
+      if (!cruise_engaged) {
+        controls_allowed = false;
+      }
+      cruise_engaged_prev = cruise_engaged;
     }
-    gas_pressed_prev = gas_pressed;
-  }
 
-  if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x3CA)) {
-    relay_malfunction_set();
+    // If steering controls messages are received on the destination bus, it's an indication
+    // that the relay might be malfunctioning.
+    generic_rx_checks(ford_lkas_msg_check(addr));
   }
-  return 1;
-}
 
-// all commands: just steering
-// if controls_allowed and no pedals pressed
-//     allow all commands up to limit
-// else
-//     block all commands that produce actuation
+  return valid;
+}
 
 static int ford_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) {
   UNUSED(longitudinal_allowed);
 
   int tx = 1;
   int addr = GET_ADDR(to_send);
 
-  // disallow actuator commands if gas or brake (with vehicle moving) are pressed
-  // and the the latching controls_allowed flag is True
-  int pedal_pressed = brake_pressed_prev && vehicle_moving;
-  bool alt_exp_allow_gas = alternative_experience & ALT_EXP_DISABLE_DISENGAGE_ON_GAS;
-  if (!alt_exp_allow_gas) {
-    pedal_pressed = pedal_pressed || gas_pressed_prev;
+  if (!msg_allowed(to_send, FORD_TX_MSGS, FORD_TX_LEN)) {
+    tx = 0;
   }
-  bool current_controls_allowed = controls_allowed && !(pedal_pressed);
-
-  // STEER: safety check
-  if (addr == 0x3CA) {
-    if (!current_controls_allowed) {
-      // bits 7-4 need to be 0xF to disallow lkas commands
-      if ((GET_BYTE(to_send, 0) & 0xF0U) != 0xF0U) {
-        tx = 0;
-      }
+
+  // Cruise button check, only allow cancel button to be sent
+  if (addr == MSG_STEERING_DATA_FD1) {
+    // Violation if any button other than cancel is pressed
+    // Signal: CcAslButtnCnclPress
+    bool violation = (GET_BYTE(to_send, 0) |
+                      (GET_BYTE(to_send, 1) & 0xFEU) |
+                      GET_BYTE(to_send, 2) |
+                      GET_BYTE(to_send, 3) |
+                      GET_BYTE(to_send, 4) |
+                      GET_BYTE(to_send, 5)) != 0U;
+    if (violation) {
+      tx = 0;
     }
   }
 
-  // FORCE CANCEL: safety check only relevant when spamming the cancel button
-  // ensuring that set and resume aren't sent
-  if (addr == 0x83) {
-    if ((GET_BYTE(to_send, 3) & 0x30U) != 0U) {
+  // Safety check for Lane_Assist_Data1 action
+  if (addr == MSG_LANE_ASSIST_DATA1) {
+    // Do not allow steering using Lane_Assist_Data1 (Lane-Departure Aid).
+    // This message must be sent for Lane Centering to work, and can include
+    // values such as the steering angle or lane curvature for debugging,
+    // but the action (LkaActvStats_D2_Req) must be set to zero.
+    unsigned int action = GET_BYTE(to_send, 0) >> 5;
+    if (action != 0U) {
+      tx = 0;
+    }
+  }
+
+  // Safety check for LateralMotionControl action
+  if (addr == MSG_LATERAL_MOTION_CONTROL) {
+    // Signal: LatCtl_D_Rq
+    unsigned int steer_control_type = (GET_BYTE(to_send, 4) >> 2) & 0x7U;
+    bool steer_control_enabled = steer_control_type != 0U;
+
+    // No steer control allowed when controls are not allowed
+    if (!controls_allowed && steer_control_enabled) {
       tx = 0;
     }
   }
@@ -102,12 +133,43 @@ static int ford_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) {
   return tx;
 }
 
-// TODO: keep camera on bus 2 and make a fwd_hook
+static int ford_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
+  int addr = GET_ADDR(to_fwd);
+  int bus_fwd = -1;
+
+  switch (bus_num) {
+    case FORD_MAIN_BUS: {
+      // Forward all traffic from bus 0 onward
+      bus_fwd = FORD_CAM_BUS;
+      break;
+    }
+    case FORD_CAM_BUS: {
+      // Block stock LKAS messages
+      if (!ford_lkas_msg_check(addr)) {
+        bus_fwd = FORD_MAIN_BUS;
+      }
+      break;
+    }
+    default: {
+      // No other buses should be in use; fallback to do-not-forward
+      bus_fwd = -1;
+      break;
+    }
+  }
+
+  return bus_fwd;
+}
+
+static const addr_checks* ford_init(uint16_t param) {
+  UNUSED(param);
+
+  return &ford_rx_checks;
+}
 
 const safety_hooks ford_hooks = {
-  .init = nooutput_init,
+  .init = ford_init,
   .rx = ford_rx_hook,
   .tx = ford_tx_hook,
   .tx_lin = nooutput_tx_lin_hook,
-  .fwd = default_fwd_hook,
+  .fwd = ford_fwd_hook,
 };

tests/safety/libpandasafety_py.py

@@ -1,5 +1,6 @@
 import os
 
+from typing import List
 from cffi import FFI
 
 can_dir = os.path.dirname(os.path.abspath(__file__))
@@ -65,11 +66,69 @@
 void init_tests(void);
 
 void init_tests_honda(void);
-void set_honda_fwd_brake(bool);
-void set_honda_alt_brake_msg(bool);
+void set_honda_fwd_brake(bool c);
+void set_honda_alt_brake_msg(bool c);
 void set_honda_bosch_long(bool c);
 int get_honda_hw(void);
 
 """)
 
-libpandasafety = ffi.dlopen(libpandasafety_fn)
+
+class CANPacket:
+  reserved: int
+  bus: int
+  data_len_code: int
+  rejected: int
+  returned: int
+  extended: int
+  addr: int
+  data: List[int]
+
+
+class PandaSafety:
+  def set_controls_allowed(self, c: bool) -> None: ...
+  def get_controls_allowed(self) -> bool: ...
+  def get_longitudinal_allowed(self) -> bool: ...
+  def set_alternative_experience(self, mode: int) -> None: ...
+  def get_alternative_experience(self) -> int: ...
+  def set_relay_malfunction(self, c: bool) -> None: ...
+  def get_relay_malfunction(self) -> bool: ...
+  def set_gas_interceptor_detected(self, c: bool) -> None: ...
+  def get_gas_interceptor_detected(self) -> bool: ...
+  def get_gas_interceptor_prev(self) -> int: ...
+  def get_gas_pressed_prev(self) -> bool: ...
+  def get_brake_pressed_prev(self) -> bool: ...
+  def get_acc_main_on(self) -> bool: ...
+
+  def set_torque_meas(self, min: int, max: int) -> None: ...  # pylint: disable=redefined-builtin
+  def get_torque_meas_min(self) -> int: ...
+  def get_torque_meas_max(self) -> int: ...
+  def set_torque_driver(self, min: int, max: int) -> None: ...  # pylint: disable=redefined-builtin
+  def get_torque_driver_min(self) -> int: ...
+  def get_torque_driver_max(self) -> int: ...
+  def set_desired_torque_last(self, t: int) -> None: ...
+  def set_rt_torque_last(self, t: int) -> None: ...
+  def set_desired_angle_last(self, t: int) -> None: ...
+
+  def get_cruise_engaged_prev(self) -> bool: ...
+  def get_vehicle_moving(self) -> bool: ...
+  def get_hw_type(self) -> int: ...
+  def set_timer(self, t: int) -> None: ...
+
+  def safety_rx_hook(self, to_send: CANPacket) -> int: ...
+  def safety_tx_hook(self, to_push: CANPacket) -> int: ...
+  def safety_fwd_hook(self, bus_num: int, to_fwd: CANPacket) -> int: ...
+  def set_safety_hooks(self, mode: int, param: int) -> int: ...
+
+  def safety_tick_current_rx_checks(self) -> None: ...
+  def addr_checks_valid(self) -> bool: ...
+
+  def init_tests(self) -> None: ...
+
+  def init_tests_honda(self) -> None: ...
+  def set_honda_fwd_brake(self, c: bool) -> None: ...
+  def set_honda_alt_brake_msg(self, c: bool) -> None: ...
+  def set_honda_bosch_long(self, c: bool) -> None: ...
+  def get_honda_hw(self) -> int: ...
+
+libpandasafety: PandaSafety = ffi.dlopen(libpandasafety_fn)