From 0c3355462483cf3150b7d481c76cbf0659c16c7a Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Sun, 16 Jul 2023 15:09:37 +0100 Subject: [PATCH] Ford CAN FD safety (#1455) --- board/safety/safety_ford.h | 69 ++++++++++++++++++++-- python/__init__.py | 1 + tests/safety/common.py | 2 +- tests/safety/test_ford.py | 117 ++++++++++++++++++++++++++++--------- 4 files changed, 153 insertions(+), 36 deletions(-) diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 5fcdcdc4..4f71650c 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -9,7 +9,8 @@ #define FORD_ACCDATA 0x186 // TX by OP, ACC controls #define FORD_ACCDATA_3 0x18A // TX by OP, ACC/TJA user interface #define FORD_Lane_Assist_Data1 0x3CA // TX by OP, Lane Keep Assist -#define FORD_LateralMotionControl 0x3D3 // TX by OP, Traffic Jam Assist +#define FORD_LateralMotionControl 0x3D3 // TX by OP, Lateral Control message +#define FORD_LateralMotionControl2 0x3D6 // TX by OP, alternate Lateral Control message #define FORD_IPMA_Data 0x3D8 // TX by OP, IPMA and LKAS user interface // CAN bus numbers. @@ -37,6 +38,27 @@ const CanMsg FORD_LONG_TX_MSGS[] = { }; #define FORD_LONG_TX_LEN (sizeof(FORD_LONG_TX_MSGS) / sizeof(FORD_LONG_TX_MSGS[0])) +const CanMsg FORD_CANFD_STOCK_TX_MSGS[] = { + {FORD_Steering_Data_FD1, 0, 8}, + {FORD_Steering_Data_FD1, 2, 8}, + {FORD_ACCDATA_3, 0, 8}, + {FORD_Lane_Assist_Data1, 0, 8}, + {FORD_LateralMotionControl2, 0, 8}, + {FORD_IPMA_Data, 0, 8}, +}; +#define FORD_CANFD_STOCK_TX_LEN (sizeof(FORD_CANFD_STOCK_TX_MSGS) / sizeof(FORD_CANFD_STOCK_TX_MSGS[0])) + +const CanMsg FORD_CANFD_LONG_TX_MSGS[] = { + {FORD_Steering_Data_FD1, 0, 8}, + {FORD_Steering_Data_FD1, 2, 8}, + {FORD_ACCDATA, 0, 8}, + {FORD_ACCDATA_3, 0, 8}, + {FORD_Lane_Assist_Data1, 0, 8}, + {FORD_LateralMotionControl2, 0, 8}, + {FORD_IPMA_Data, 0, 8}, +}; +#define FORD_CANFD_LONG_TX_LEN (sizeof(FORD_CANFD_LONG_TX_MSGS) / sizeof(FORD_CANFD_LONG_TX_MSGS[0])) + // warning: quality flags are not yet checked in openpilot's CAN parser, // this may be the cause of blocked messages AddrCheckStruct ford_addr_checks[] = { @@ -133,8 +155,10 @@ static bool ford_get_quality_flag_valid(CANPacket_t *to_push) { } const uint16_t FORD_PARAM_LONGITUDINAL = 1; +const uint16_t FORD_PARAM_CANFD = 2; bool ford_longitudinal = false; +bool ford_canfd = false; const LongitudinalLimits FORD_LONG_LIMITS = { // acceleration cmd limits (used for brakes) @@ -154,12 +178,16 @@ const LongitudinalLimits FORD_LONG_LIMITS = { #define FORD_INACTIVE_CURVATURE_RATE 4096U #define FORD_INACTIVE_PATH_OFFSET 512U #define FORD_INACTIVE_PATH_ANGLE 1000U + +#define FORD_CANFD_INACTIVE_CURVATURE_RATE 1024U + #define FORD_MAX_SPEED_DELTA 2.0 // m/s static bool ford_lkas_msg_check(int addr) { return (addr == FORD_ACCDATA_3) || (addr == FORD_Lane_Assist_Data1) || (addr == FORD_LateralMotionControl) + || (addr == FORD_LateralMotionControl2) || (addr == FORD_IPMA_Data); } @@ -249,13 +277,20 @@ static int ford_rx_hook(CANPacket_t *to_push) { } static int ford_tx_hook(CANPacket_t *to_send) { - int tx = 1; int addr = GET_ADDR(to_send); - - if (ford_longitudinal) { - tx = msg_allowed(to_send, FORD_LONG_TX_MSGS, FORD_LONG_TX_LEN); + int tx; + if (ford_canfd) { + if (ford_longitudinal) { + tx = msg_allowed(to_send, FORD_CANFD_LONG_TX_MSGS, FORD_CANFD_LONG_TX_LEN); + } else { + tx = msg_allowed(to_send, FORD_CANFD_STOCK_TX_MSGS, FORD_CANFD_STOCK_TX_LEN); + } } else { - tx = msg_allowed(to_send, FORD_STOCK_TX_MSGS, FORD_STOCK_TX_LEN); + if (ford_longitudinal) { + tx = msg_allowed(to_send, FORD_LONG_TX_MSGS, FORD_LONG_TX_LEN); + } else { + tx = msg_allowed(to_send, FORD_STOCK_TX_MSGS, FORD_STOCK_TX_LEN); + } } // Safety check for ACCDATA accel and brake requests @@ -327,6 +362,27 @@ static int ford_tx_hook(CANPacket_t *to_send) { } } + // Safety check for LateralMotionControl2 action + if (addr == FORD_LateralMotionControl2) { + // Signal: LatCtl_D2_Rq + bool steer_control_enabled = ((GET_BYTE(to_send, 0) >> 4) & 0x7U) != 0U; + unsigned int raw_curvature = (GET_BYTE(to_send, 2) << 3) | (GET_BYTE(to_send, 3) >> 5); + unsigned int raw_curvature_rate = (GET_BYTE(to_send, 6) << 3) | (GET_BYTE(to_send, 7) >> 5); + unsigned int raw_path_angle = ((GET_BYTE(to_send, 3) & 0x1FU) << 6) | (GET_BYTE(to_send, 4) >> 2); + unsigned int raw_path_offset = ((GET_BYTE(to_send, 4) & 0x3U) << 8) | GET_BYTE(to_send, 5); + + // These signals are not yet tested with the current safety limits + bool violation = (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET); + + // Check angle error and steer_control_enabled + int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature + violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS); + + if (violation) { + tx = 0; + } + } + // 1 allows the message through return tx; } @@ -367,6 +423,7 @@ static const addr_checks* ford_init(uint16_t param) { UNUSED(param); #ifdef ALLOW_DEBUG ford_longitudinal = GET_FLAG(param, FORD_PARAM_LONGITUDINAL); + ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD); #endif return &ford_rx_checks; } diff --git a/python/__init__.py b/python/__init__.py index ffa0b1be..26a70ff2 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -254,6 +254,7 @@ class Panda: FLAG_GM_HW_CAM_LONG = 2 FLAG_FORD_LONG_CONTROL = 1 + FLAG_FORD_CANFD = 2 def __init__(self, serial: Optional[str] = None, claim: bool = True, disable_checks: bool = True): self._connect_serial = serial diff --git a/tests/safety/common.py b/tests/safety/common.py index 24b3f9b7..1b141d85 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -901,7 +901,7 @@ class PandaSafetyTest(PandaSafetyTestBase): continue if {attr, current_test}.issubset({'TestGmCameraSafety', 'TestGmCameraLongitudinalSafety'}): continue - if {attr, current_test}.issubset({'TestFordStockSafety', 'TestFordLongitudinalSafety'}): + if attr.startswith('TestFord') and current_test.startswith('TestFord'): continue if attr.startswith('TestHyundaiCanfd') and current_test.startswith('TestHyundaiCanfd'): continue diff --git a/tests/safety/test_ford.py b/tests/safety/test_ford.py index dfc317fa..7d2a128f 100755 --- a/tests/safety/test_ford.py +++ b/tests/safety/test_ford.py @@ -17,7 +17,8 @@ MSG_Steering_Data_FD1 = 0x083 # TX by OP, various driver switches and LKAS/ MSG_ACCDATA = 0x186 # TX by OP, ACC controls MSG_ACCDATA_3 = 0x18A # TX by OP, ACC/TJA user interface MSG_Lane_Assist_Data1 = 0x3CA # TX by OP, Lane Keep Assist -MSG_LateralMotionControl = 0x3D3 # TX by OP, Traffic Jam Assist +MSG_LateralMotionControl = 0x3D3 # TX by OP, Lateral Control message +MSG_LateralMotionControl2 = 0x3D6 # TX by OP, alternate Lateral Control message MSG_IPMA_Data = 0x3D8 # TX by OP, IPMA and LKAS user interface @@ -57,18 +58,26 @@ class Buttons: TJA_TOGGLE = 2 -# Ford safety has two different configurations tested here: -# * stock longitudinal -# * openpilot longitudinal +# Ford safety has four different configurations tested here: +# * CAN with stock longitudinal +# * CAN with openpilot longitudinal +# * CAN FD with stock longitudinal +# * CAN FD with openpilot longitudinal class TestFordSafetyBase(common.PandaSafetyTest): STANDSTILL_THRESHOLD = 1 RELAY_MALFUNCTION_ADDR = MSG_IPMA_Data RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl, + MSG_LateralMotionControl2, MSG_IPMA_Data]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + # Max allowed delta between car speeds MAX_SPEED_DELTA = 2.0 # m/s + STEER_MESSAGE = 0 + # Curvature control limits DEG_TO_CAN = 50000 # 1 / (2e-5) rad to can MAX_CURVATURE = 0.02 @@ -158,16 +167,26 @@ class TestFordSafetyBase(common.PandaSafetyTest): } return self.packer.make_can_msg_panda("Lane_Assist_Data1", 0, values) - # TJA command - def _tja_command_msg(self, enabled: bool, path_offset: float, path_angle: float, curvature: float, curvature_rate: float): - values = { - "LatCtl_D_Rq": 1 if enabled else 0, - "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter - "LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians - "LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2 - "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter - } - return self.packer.make_can_msg_panda("LateralMotionControl", 0, values) + # LCA command + def _lat_ctl_msg(self, enabled: bool, path_offset: float, path_angle: float, curvature: float, curvature_rate: float): + if self.STEER_MESSAGE == MSG_LateralMotionControl: + values = { + "LatCtl_D_Rq": 1 if enabled else 0, + "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians + "LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2 + "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter + } + return self.packer.make_can_msg_panda("LateralMotionControl", 0, values) + elif self.STEER_MESSAGE == MSG_LateralMotionControl2: + values = { + "LatCtl_D2_Rq": 1 if enabled else 0, + "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians + "LatCtlCrv_NoRate2_Actl": curvature_rate, # Curvature rate [-0.001024|0.001023] 1/meter^2 + "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter + } + return self.packer.make_can_msg_panda("LateralMotionControl2", 0, values) # Cruise control buttons def _acc_button_msg(self, button: int, bus: int): @@ -258,7 +277,7 @@ class TestFordSafetyBase(common.PandaSafetyTest): with self.subTest(controls_allowed=controls_allowed, steer_control_enabled=steer_control_enabled, path_offset=path_offset, path_angle=path_angle, curvature_rate=curvature_rate, curvature=curvature): - self.assertEqual(should_tx, self._tx(self._tja_command_msg(steer_control_enabled, path_offset, path_angle, curvature, curvature_rate))) + self.assertEqual(should_tx, self._tx(self._lat_ctl_msg(steer_control_enabled, path_offset, path_angle, curvature, curvature_rate))) def test_curvature_rate_limit_up(self): """ @@ -285,7 +304,7 @@ class TestFordSafetyBase(common.PandaSafetyTest): self._reset_curvature_measurement(sign * (self.MAX_CURVATURE_ERROR + 1e-3), speed) for should_tx, curvature in cases: self._set_prev_desired_angle(sign * small_curvature) - self.assertEqual(should_tx, self._tx(self._tja_command_msg(True, 0, 0, sign * (small_curvature + curvature), 0))) + self.assertEqual(should_tx, self._tx(self._lat_ctl_msg(True, 0, 0, sign * (small_curvature + curvature), 0))) def test_curvature_rate_limit_down(self): self.safety.set_controls_allowed(True) @@ -308,7 +327,7 @@ class TestFordSafetyBase(common.PandaSafetyTest): self._reset_curvature_measurement(sign * (self.MAX_CURVATURE - self.MAX_CURVATURE_ERROR - 1e-3), speed) for should_tx, curvature in cases: self._set_prev_desired_angle(sign * self.MAX_CURVATURE) - self.assertEqual(should_tx, self._tx(self._tja_command_msg(True, 0, 0, sign * curvature, 0))) + self.assertEqual(should_tx, self._tx(self._lat_ctl_msg(True, 0, 0, sign * curvature, 0))) def test_prevent_lkas_action(self): self.safety.set_controls_allowed(1) @@ -336,12 +355,12 @@ class TestFordSafetyBase(common.PandaSafetyTest): class TestFordStockSafety(TestFordSafetyBase): + STEER_MESSAGE = MSG_LateralMotionControl + TX_MSGS = [ [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0], [MSG_LateralMotionControl, 0], [MSG_IPMA_Data, 0], ] - FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl, MSG_IPMA_Data]} - FWD_BUS_LOOKUP = {0: 2, 2: 0} def setUp(self): self.packer = CANPackerPanda("ford_lincoln_base_pt") @@ -350,13 +369,24 @@ class TestFordStockSafety(TestFordSafetyBase): self.safety.init_tests() -class TestFordLongitudinalSafety(TestFordSafetyBase): +class TestFordCANFDStockSafety(TestFordSafetyBase): + STEER_MESSAGE = MSG_LateralMotionControl2 + TX_MSGS = [ - [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA, 0], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0], - [MSG_LateralMotionControl, 0], [MSG_IPMA_Data, 0], + [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0], + [MSG_LateralMotionControl2, 0], [MSG_IPMA_Data, 0], ] - FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA, MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl, MSG_IPMA_Data]} - FWD_BUS_LOOKUP = {0: 2, 2: 0} + + def setUp(self): + self.packer = CANPackerPanda("ford_lincoln_base_pt") + self.safety = libpanda_py.libpanda + self.safety.set_safety_hooks(Panda.SAFETY_FORD, Panda.FLAG_FORD_CANFD) + self.safety.init_tests() + + +class TestFordLongitudinalSafetyBase(TestFordSafetyBase): + FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA, MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl, + MSG_LateralMotionControl2, MSG_IPMA_Data]} MAX_ACCEL = 2.0 # accel is used for brakes, but openpilot can set positive values MIN_ACCEL = -3.5 @@ -366,11 +396,10 @@ class TestFordLongitudinalSafety(TestFordSafetyBase): MIN_GAS = -0.5 INACTIVE_GAS = -5.0 - def setUp(self): - self.packer = CANPackerPanda("ford_lincoln_base_pt") - self.safety = libpanda_py.libpanda - self.safety.set_safety_hooks(Panda.SAFETY_FORD, Panda.FLAG_FORD_LONG_CONTROL) - self.safety.init_tests() + @classmethod + def setUpClass(cls): + if cls.__name__ == "TestFordLongitudinalSafetyBase": + raise unittest.SkipTest # ACC command def _acc_command_msg(self, gas: float, brake: float, cmbb_deny: bool = False): @@ -408,5 +437,35 @@ class TestFordLongitudinalSafety(TestFordSafetyBase): self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.INACTIVE_GAS, brake))) +class TestFordLongitudinalSafety(TestFordLongitudinalSafetyBase): + STEER_MESSAGE = MSG_LateralMotionControl + + TX_MSGS = [ + [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA, 0], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0], + [MSG_LateralMotionControl, 0], [MSG_IPMA_Data, 0], + ] + + def setUp(self): + self.packer = CANPackerPanda("ford_lincoln_base_pt") + self.safety = libpanda_py.libpanda + self.safety.set_safety_hooks(Panda.SAFETY_FORD, Panda.FLAG_FORD_LONG_CONTROL) + self.safety.init_tests() + + +class TestFordCANFDLongitudinalSafety(TestFordLongitudinalSafetyBase): + STEER_MESSAGE = MSG_LateralMotionControl2 + + TX_MSGS = [ + [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA, 0], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0], + [MSG_LateralMotionControl2, 0], [MSG_IPMA_Data, 0], + ] + + def setUp(self): + self.packer = CANPackerPanda("ford_lincoln_base_pt") + self.safety = libpanda_py.libpanda + self.safety.set_safety_hooks(Panda.SAFETY_FORD, Panda.FLAG_FORD_LONG_CONTROL | Panda.FLAG_FORD_CANFD) + self.safety.init_tests() + + if __name__ == "__main__": unittest.main()