From 7df0d854202fa188bab46514a8ce99e2dd6b72c7 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 1 Mar 2025 02:53:54 -0600 Subject: [PATCH] Fix Ford CAN FD safety replay (#1889) --- opendbc/safety/tests/safety_replay/helpers.py | 8 ++++++-- opendbc/safety/tests/safety_replay/replay_drive.py | 9 ++++----- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/opendbc/safety/tests/safety_replay/helpers.py b/opendbc/safety/tests/safety_replay/helpers.py index e8dac688..f116f90a 100644 --- a/opendbc/safety/tests/safety_replay/helpers.py +++ b/opendbc/safety/tests/safety_replay/helpers.py @@ -1,3 +1,4 @@ +from opendbc.car.ford.values import FordSafetyFlags from opendbc.car.toyota.values import ToyotaSafetyFlags from opendbc.car.structs import CarParams from opendbc.safety.tests.libsafety import libsafety_py @@ -23,7 +24,7 @@ def is_steering_msg(mode, param, addr): elif mode == CarParams.SafetyModel.subaru: ret = addr == 0x122 elif mode == CarParams.SafetyModel.ford: - ret = addr == 0x3d3 + ret = addr == 0x3d6 if param & FordSafetyFlags.CANFD else addr == 0x3d3 elif mode == CarParams.SafetyModel.nissan: ret = addr == 0x169 elif mode == CarParams.SafetyModel.rivian: @@ -53,7 +54,10 @@ def get_steer_value(mode, param, to_send): torque = ((to_send.data[3] & 0x1F) << 8) | to_send.data[2] torque = -to_signed(torque, 13) elif mode == CarParams.SafetyModel.ford: - angle = ((to_send.data[0] << 3) | (to_send.data[1] >> 5)) - 1000 + if param & FordSafetyFlags.CANFD: + angle = ((to_send.data[2] << 3) | (to_send.data[3] >> 5)) - 1000 + else: + angle = ((to_send.data[0] << 3) | (to_send.data[1] >> 5)) - 1000 elif mode == CarParams.SafetyModel.nissan: angle = (to_send.data[0] << 10) | (to_send.data[1] << 2) | (to_send.data[2] >> 6) angle = -angle + (1310 * 100) diff --git a/opendbc/safety/tests/safety_replay/replay_drive.py b/opendbc/safety/tests/safety_replay/replay_drive.py index eb004dc7..fb17a369 100755 --- a/opendbc/safety/tests/safety_replay/replay_drive.py +++ b/opendbc/safety/tests/safety_replay/replay_drive.py @@ -7,16 +7,15 @@ from opendbc.safety.tests.libsafety import libsafety_py from opendbc.safety.tests.safety_replay.helpers import package_can_msg, init_segment # replay a drive to check for safety violations -def replay_drive(lr, safety_mode, param, alternative_experience, segment=False): +def replay_drive(lr, safety_mode, param, alternative_experience): safety = libsafety_py.libsafety err = safety.set_safety_hooks(safety_mode, param) assert err == 0, "invalid safety mode: %d" % safety_mode safety.set_alternative_experience(alternative_experience) - if segment: - init_segment(safety, lr, safety_mode, param) - lr.reset() + init_segment(safety, lr, safety_mode, param) + lr.reset() rx_tot, rx_invalid, tx_tot, tx_blocked, tx_controls, tx_controls_blocked = 0, 0, 0, 0, 0, 0 safety_tick_rx_invalid = False @@ -93,4 +92,4 @@ if __name__ == "__main__": args.alternative_experience = CP.alternativeExperience print(f"replaying {args.route_or_segment_name[0]} with safety mode {args.mode}, param {args.param}, alternative experience {args.alternative_experience}") - replay_drive(lr, args.mode, args.param, args.alternative_experience, segment=len(lr.logreader_identifiers) == 1) + replay_drive(lr, args.mode, args.param, args.alternative_experience)