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 <adeebshihadeh@gmail.com>

* Simplify brake pressed

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* Ford LKAS msg helper

* Simplify button violation check

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* Simplify LKAS action check

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* Tidy up tests

* Use standstill signal for tests

* Misra fix

* Unused definition

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* 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 <adeebshihadeh@gmail.com>
This commit is contained in:
Cameron Clough
2022-07-24 00:54:58 +01:00
committed by GitHub
parent 4b1d7d0215
commit 3a17c2e07f
3 changed files with 321 additions and 86 deletions

View File

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

View File

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

114
tests/safety/test_ford.py Normal file
View File

@@ -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()