From 3a17c2e07f2452b1d894c778afbaf76b482569e4 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Sun, 24 Jul 2022 00:54:58 +0100 Subject: [PATCH] Ford: add safety code for LCA vehicles (#966) * Ford: add CAN messages and signal definitions * Ford: implement safety hooks * Ford: add safety unit tests * Use standstill signal * Rename bus Co-authored-by: Adeeb Shihadeh * Simplify brake pressed Co-authored-by: Adeeb Shihadeh * Ford LKAS msg helper * Simplify button violation check Co-authored-by: Adeeb Shihadeh * Simplify LKAS action check Co-authored-by: Adeeb Shihadeh * Tidy up tests * Use standstill signal for tests * Misra fix * Unused definition Co-authored-by: Adeeb Shihadeh * Simplify signal bit shifting * Remove type hints * Add panda safety type hints * Fix vehicle moving signal * Fix button checks * Fix steer allowed tests * Fix standstill threshold in tests * Maybe this works? * More button tests * Revert "Simplify button violation check" This reverts commit c6496269844328a007ea52b5766501d7c1ea40c2. Co-authored-by: Adeeb Shihadeh --- board/safety/safety_ford.h | 228 +++++++++++++++++++----------- tests/safety/libpandasafety_py.py | 65 ++++++++- tests/safety/test_ford.py | 114 +++++++++++++++ 3 files changed, 321 insertions(+), 86 deletions(-) create mode 100644 tests/safety/test_ford.py diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 3d9ac604..806d6981 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -1,69 +1,84 @@ -// 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 int ford_rx_hook(CANPacket_t *to_push) { - - 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 (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); - } - } - - // 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; - } - } - - // 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; - } - - // 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; - } - gas_pressed_prev = gas_pressed; - } - - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x3CA)) { - relay_malfunction_set(); - } - return 1; +static bool ford_lkas_msg_check(int addr) { + return (addr == MSG_LANE_ASSIST_DATA1) + || (addr == MSG_LATERAL_MOTION_CONTROL) + || (addr == MSG_IPMA_DATA); } -// all commands: just steering -// if controls_allowed and no pedals pressed -// allow all commands up to limit -// else -// block all commands that produce actuation +static int ford_rx_hook(CANPacket_t *to_push) { + bool valid = addr_safety_check(to_push, &ford_rx_checks, NULL, NULL, NULL); + + if (valid && (GET_BUS(to_push) == FORD_MAIN_BUS)) { + int addr = GET_ADDR(to_push); + + // 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; + } + + // 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; + } + + // 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); + + // 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; + } + + // 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 valid; +} static int ford_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { UNUSED(longitudinal_allowed); @@ -71,29 +86,45 @@ static int ford_tx_hook(CANPacket_t *to_send, bool 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, }; diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index 983c2557..73806a6d 100644 --- a/tests/safety/libpandasafety_py.py +++ b/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 @@ bool addr_checks_valid(); 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) diff --git a/tests/safety/test_ford.py b/tests/safety/test_ford.py new file mode 100644 index 00000000..3e4db91a --- /dev/null +++ b/tests/safety/test_ford.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python3 +import unittest + +import panda.tests.safety.common as common + +from panda import Panda +from panda.tests.safety import libpandasafety_py +from panda.tests.safety.common import CANPackerPanda + +MSG_ENG_BRAKE_DATA = 0x165 # RX from PCM, for driver brake pedal and cruise state +MSG_ENG_VEHICLE_SP_THROTTLE2 = 0x202 # RX from PCM, for vehicle speed +MSG_ENG_VEHICLE_SP_THROTTLE = 0x204 # RX from PCM, for driver throttle input +MSG_STEERING_DATA_FD1 = 0x083 # TX by OP, ACC control buttons for cancel +MSG_LANE_ASSIST_DATA1 = 0x3CA # TX by OP, Lane Keeping Assist +MSG_LATERAL_MOTION_CONTROL = 0x3D3 # TX by OP, Lane Centering Assist +MSG_IPMA_DATA = 0x3D8 # TX by OP, IPMA HUD user interface + + +class TestFordSafety(common.PandaSafetyTest): + STANDSTILL_THRESHOLD = 1 + RELAY_MALFUNCTION_ADDR = MSG_IPMA_DATA + RELAY_MALFUNCTION_BUS = 0 + + TX_MSGS = [[MSG_STEERING_DATA_FD1, 0], [MSG_LANE_ASSIST_DATA1, 0], [MSG_LATERAL_MOTION_CONTROL, 0], [MSG_IPMA_DATA, 0]] + FWD_BLACKLISTED_ADDRS = {2: [MSG_LANE_ASSIST_DATA1, MSG_LATERAL_MOTION_CONTROL, MSG_IPMA_DATA]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + + def setUp(self): + self.packer = CANPackerPanda("ford_lincoln_base_pt") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_FORD, 0) + self.safety.init_tests() + + # Driver brake pedal + def _user_brake_msg(self, brake: bool): + # brake pedal and cruise state share same message, so we have to send + # the other signal too + enable = self.safety.get_controls_allowed() + values = { + "BpedDrvAppl_D_Actl": 2 if brake else 1, + "CcStat_D_Actl": 5 if enable else 0, + } + return self.packer.make_can_msg_panda("EngBrakeData", 0, values) + + # Standstill state + def _speed_msg(self, speed: float): + values = {"VehStop_D_Stat": 1 if speed <= self.STANDSTILL_THRESHOLD else 0} + return self.packer.make_can_msg_panda("DesiredTorqBrk", 0, values) + + # Drive throttle input + def _user_gas_msg(self, gas: float): + values = {"ApedPos_Pc_ActlArb": gas} + return self.packer.make_can_msg_panda("EngVehicleSpThrottle", 0, values) + + # Cruise status + def _pcm_status_msg(self, enable: bool): + # brake pedal and cruise state share same message, so we have to send + # the other signal too + brake = self.safety.get_brake_pressed_prev() + values = { + "BpedDrvAppl_D_Actl": 2 if brake else 1, + "CcStat_D_Actl": 5 if enable else 0, + } + return self.packer.make_can_msg_panda("EngBrakeData", 0, values) + + # LKAS command + def _lkas_command_msg(self, action: int): + values = { + "LkaActvStats_D2_Req": action, + } + return self.packer.make_can_msg_panda("Lane_Assist_Data1", 0, values) + + # TJA command + def _tja_command_msg(self, enabled: bool): + values = { + "LatCtl_D_Rq": 1 if enabled else 0, + } + return self.packer.make_can_msg_panda("LateralMotionControl", 0, values) + + # Cruise control buttons + def _acc_button_msg(self, cancel=0, resume=0, _set=0): + values = { + "CcAslButtnCnclPress": cancel, + "CcAsllButtnResPress": resume, + "CcAslButtnSetPress": _set, + } + return self.packer.make_can_msg_panda("Steering_Data_FD1", 0, values) + + def test_steer_allowed(self): + self.safety.set_controls_allowed(1) + self.assertTrue(self._tx(self._tja_command_msg(1))) + self.assertTrue(self.safety.get_controls_allowed()) + + self.safety.set_controls_allowed(0) + self.assertFalse(self._tx(self._tja_command_msg(1))) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_prevent_lkas_action(self): + self.safety.set_controls_allowed(1) + self.assertFalse(self._tx(self._lkas_command_msg(1))) + + self.safety.set_controls_allowed(0) + self.assertFalse(self._tx(self._lkas_command_msg(1))) + + def test_spam_cancel_safety_check(self): + for allowed in (0, 1): + self.safety.set_controls_allowed(allowed) + self.assertTrue(self._tx(self._acc_button_msg(cancel=1))) + self.assertFalse(self._tx(self._acc_button_msg(resume=1))) + self.assertFalse(self._tx(self._acc_button_msg(_set=1))) + + +if __name__ == "__main__": + unittest.main()