From fd8809f3beabec650126929853f7671d0c61b8f4 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 30 Nov 2022 23:18:20 -0800 Subject: [PATCH 01/11] common angle checks --- board/safety.h | 34 +++++++++++++++++++++++++++++ board/safety/safety_nissan.h | 42 +++++++++++++----------------------- board/safety/safety_tesla.h | 36 +++++++++++-------------------- board/safety_declarations.h | 7 ++++++ 4 files changed, 69 insertions(+), 50 deletions(-) diff --git a/board/safety.h b/board/safety.h index d6bbbc81fb..1e9cdbaf3e 100644 --- a/board/safety.h +++ b/board/safety.h @@ -605,6 +605,40 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi return violation; } +// Safety checks for angle-based steering commands +bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const SteeringLimits limits) { + bool violation = false; + + if (controls_allowed && steer_control_enabled) { + // add 1 to not false trigger the violation + float delta_angle_float; + delta_angle_float = (interpolate(limits.angle_rate_up_lookup, vehicle_speed) * limits.angle_deg_to_can) + 1.; + int delta_angle_up = (int)(delta_angle_float); + delta_angle_float = (interpolate(limits.angle_rate_down_lookup, vehicle_speed) * limits.angle_deg_to_can) + 1.; + int delta_angle_down = (int)(delta_angle_float); + int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down); + int lowest_desired_angle = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down : delta_angle_up); + + // check for violation; + violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle); + } + desired_angle_last = desired_angle; + + // Angle should be the same as current angle while not steering + if ((!controls_allowed) && + ((desired_angle < (angle_meas.min - 1)) || + (desired_angle > (angle_meas.max + 1)))) { + violation = true; + } + + // No angle control allowed when controls are not allowed + if (!controls_allowed && steer_control_enabled) { + violation = true; + } + + return violation; +} + void pcm_cruise_check(bool cruise_engaged) { // Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages if (!cruise_engaged) { diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h index 9a2e294610..8d8c20297b 100644 --- a/board/safety/safety_nissan.h +++ b/board/safety/safety_nissan.h @@ -1,4 +1,3 @@ - const uint32_t NISSAN_RT_INTERVAL = 250000; // 250ms between real time checks const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_UP = { @@ -11,6 +10,18 @@ const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_DOWN = { const int NISSAN_DEG_TO_CAN = 100; +const SteeringLimits NISSAN_STEERING_LIMITS = { + .angle_deg_to_can = 100, + .angle_rate_up_lookup = { + {2., 7., 17.}, + {5., .8, .15} + }, + .angle_rate_down_lookup = { + {2., 7., 17.}, + {5., 3.5, .5} + }, +}; + const CanMsg NISSAN_TX_MSGS[] = { {0x169, 0, 8}, // LKAS {0x2b1, 0, 8}, // PROPILOT_HUD @@ -104,7 +115,7 @@ static int nissan_tx_hook(CANPacket_t *to_send) { int tx = 1; int addr = GET_ADDR(to_send); - bool violation = 0; + bool violation = false; if (!msg_allowed(to_send, NISSAN_TX_MSGS, sizeof(NISSAN_TX_MSGS) / sizeof(NISSAN_TX_MSGS[0]))) { tx = 0; @@ -118,31 +129,8 @@ static int nissan_tx_hook(CANPacket_t *to_send) { // offeset 1310 * NISSAN_DEG_TO_CAN desired_angle = desired_angle - 131000; - if (controls_allowed && lka_active) { - // add 1 to not false trigger the violation - float delta_angle_float; - delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_UP, vehicle_speed) * NISSAN_DEG_TO_CAN) + 1.; - int delta_angle_up = (int)(delta_angle_float); - delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_DOWN, vehicle_speed) * NISSAN_DEG_TO_CAN) + 1.; - int delta_angle_down = (int)(delta_angle_float); - int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down); - int lowest_desired_angle = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down : delta_angle_up); - - // check for violation; - violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle); - } - desired_angle_last = desired_angle; - - // desired steer angle should be the same as steer angle measured when controls are off - if ((!controls_allowed) && - ((desired_angle < (angle_meas.min - 1)) || - (desired_angle > (angle_meas.max + 1)))) { - violation = 1; - } - - // no lka_enabled bit if controls not allowed - if (!controls_allowed && lka_active) { - violation = 1; + if (steer_angle_cmd_checks(desired_angle, lka_active, NISSAN_STEERING_LIMITS)) { + violation = true; } } diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h index c2fd9dca48..497c9da3a9 100644 --- a/board/safety/safety_tesla.h +++ b/board/safety/safety_tesla.h @@ -8,6 +8,18 @@ const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_DOWN = { const int TESLA_DEG_TO_CAN = 10; +const SteeringLimits TESLA_STEERING_LIMITS = { + .angle_deg_to_can = 10, + .angle_rate_up_lookup = { + {2., 7., 17.}, + {5., .8, .25} + }, + .angle_rate_down_lookup = { + {2., 7., 17.}, + {5., 3.5, .8} + }, +}; + const LongitudinalLimits TESLA_LONG_LIMITS = { .max_accel = 425, // 2. m/s^2 .min_accel = 287, // -3.52 m/s^2 // TODO: limit to -3.48 @@ -136,29 +148,7 @@ static int tesla_tx_hook(CANPacket_t *to_send) { bool steer_control_enabled = (steer_control_type != 0) && // NONE (steer_control_type != 3); // DISABLED - // Rate limit while steering - if(controls_allowed && steer_control_enabled) { - // Add 1 to not false trigger the violation - float delta_angle_float; - delta_angle_float = (interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, vehicle_speed) * TESLA_DEG_TO_CAN); - int delta_angle_up = (int)(delta_angle_float) + 1; - delta_angle_float = (interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, vehicle_speed) * TESLA_DEG_TO_CAN); - int delta_angle_down = (int)(delta_angle_float) + 1; - int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down); - int lowest_desired_angle = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down : delta_angle_up); - - // Check for violation; - violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle); - } - desired_angle_last = desired_angle; - - // Angle should be the same as current angle while not steering - if(!controls_allowed && ((desired_angle < (angle_meas.min - 1)) || (desired_angle > (angle_meas.max + 1)))) { - violation = true; - } - - // No angle control allowed when controls are not allowed - if(!controls_allowed && steer_control_enabled) { + if (steer_angle_cmd_checks(desired_angle, steer_control_enabled, TESLA_STEERING_LIMITS)) { violation = true; } } diff --git a/board/safety_declarations.h b/board/safety_declarations.h index 27771bd618..6bb80086df 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -32,6 +32,7 @@ typedef enum { } SteeringControlType; typedef struct { + // torque cmd limits const int max_steer; const int max_rate_up; const int max_rate_down; @@ -52,6 +53,11 @@ typedef struct { const int max_invalid_request_frames; const uint32_t min_valid_request_rt_interval; const bool has_steer_req_tolerance; + + // angle cmd limits + const int angle_deg_to_can; + const struct lookup_t angle_rate_up_lookup; + const struct lookup_t angle_rate_down_lookup; } SteeringLimits; typedef struct { @@ -130,6 +136,7 @@ void generic_rx_checks(bool stock_ecu_detected); void relay_malfunction_set(void); void relay_malfunction_reset(void); bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLimits limits); +bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const SteeringLimits limits); bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits); bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limits); bool longitudinal_gas_checks(int desired_gas, const LongitudinalLimits limits); From 67958edaf6d5d64eddfb7ef2f950a155cb1c7ced Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 30 Nov 2022 23:23:50 -0800 Subject: [PATCH 02/11] clean up --- board/safety/safety_nissan.h | 14 +------------- board/safety/safety_tesla.h | 10 ---------- 2 files changed, 1 insertion(+), 23 deletions(-) diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h index 8d8c20297b..c977cf66fc 100644 --- a/board/safety/safety_nissan.h +++ b/board/safety/safety_nissan.h @@ -1,15 +1,3 @@ -const uint32_t NISSAN_RT_INTERVAL = 250000; // 250ms between real time checks - -const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_UP = { - {2., 7., 17.}, - {5., .8, .15}}; - -const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_DOWN = { - {2., 7., 17.}, - {5., 3.5, .5}}; - -const int NISSAN_DEG_TO_CAN = 100; - const SteeringLimits NISSAN_STEERING_LIMITS = { .angle_deg_to_can = 100, .angle_rate_up_lookup = { @@ -126,7 +114,7 @@ static int nissan_tx_hook(CANPacket_t *to_send) { int desired_angle = ((GET_BYTE(to_send, 0) << 10) | (GET_BYTE(to_send, 1) << 2) | ((GET_BYTE(to_send, 2) >> 6) & 0x3U)); bool lka_active = (GET_BYTE(to_send, 6) >> 4) & 1U; - // offeset 1310 * NISSAN_DEG_TO_CAN + // offeset 1310 * NISSAN_STEERING_LIMITS.angle_deg_to_can desired_angle = desired_angle - 131000; if (steer_angle_cmd_checks(desired_angle, lka_active, NISSAN_STEERING_LIMITS)) { diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h index 497c9da3a9..a0a82f4bca 100644 --- a/board/safety/safety_tesla.h +++ b/board/safety/safety_tesla.h @@ -1,13 +1,3 @@ -const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_UP = { - {2., 7., 17.}, - {5., .8, .25}}; - -const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_DOWN = { - {2., 7., 17.}, - {5., 3.5, .8}}; - -const int TESLA_DEG_TO_CAN = 10; - const SteeringLimits TESLA_STEERING_LIMITS = { .angle_deg_to_can = 10, .angle_rate_up_lookup = { From e462f149ace93ac49424d607175e7a90f9f536d7 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 30 Nov 2022 23:47:31 -0800 Subject: [PATCH 03/11] clean up check and add comments --- board/safety.h | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/board/safety.h b/board/safety.h index 1e9cdbaf3e..60c82f37a4 100644 --- a/board/safety.h +++ b/board/safety.h @@ -610,12 +610,11 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const bool violation = false; if (controls_allowed && steer_control_enabled) { + // convert floating point angle rate limits to ints in the scale of the desired angle on CAN, // add 1 to not false trigger the violation - float delta_angle_float; - delta_angle_float = (interpolate(limits.angle_rate_up_lookup, vehicle_speed) * limits.angle_deg_to_can) + 1.; - int delta_angle_up = (int)(delta_angle_float); - delta_angle_float = (interpolate(limits.angle_rate_down_lookup, vehicle_speed) * limits.angle_deg_to_can) + 1.; - int delta_angle_down = (int)(delta_angle_float); + int delta_angle_up = (interpolate(limits.angle_rate_up_lookup, vehicle_speed) * limits.angle_deg_to_can) + 1.; + int delta_angle_down = (interpolate(limits.angle_rate_down_lookup, vehicle_speed) * limits.angle_deg_to_can) + 1.; + int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down); int lowest_desired_angle = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down : delta_angle_up); @@ -625,16 +624,12 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const desired_angle_last = desired_angle; // Angle should be the same as current angle while not steering - if ((!controls_allowed) && - ((desired_angle < (angle_meas.min - 1)) || - (desired_angle > (angle_meas.max + 1)))) { + if (!controls_allowed && ((desired_angle < (angle_meas.min - 1)) || (desired_angle > (angle_meas.max + 1)))) { violation = true; } // No angle control allowed when controls are not allowed - if (!controls_allowed && steer_control_enabled) { - violation = true; - } + violation |= !controls_allowed && steer_control_enabled; return violation; } From 08647341f4a9b6d49fcc60905d0231b26ddd64d3 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 30 Nov 2022 23:49:32 -0800 Subject: [PATCH 04/11] readable --- board/safety.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/board/safety.h b/board/safety.h index 60c82f37a4..dc2314fdef 100644 --- a/board/safety.h +++ b/board/safety.h @@ -624,9 +624,9 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const desired_angle_last = desired_angle; // Angle should be the same as current angle while not steering - if (!controls_allowed && ((desired_angle < (angle_meas.min - 1)) || (desired_angle > (angle_meas.max + 1)))) { - violation = true; - } + violation |= (!controls_allowed && + ((desired_angle < (angle_meas.min - 1)) || + (desired_angle > (angle_meas.max + 1)))); // No angle control allowed when controls are not allowed violation |= !controls_allowed && steer_control_enabled; From 27d8df82058708ee52c2d9c6dd0a10b6a86412fb Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 30 Nov 2022 23:50:20 -0800 Subject: [PATCH 05/11] ints --- board/safety.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/board/safety.h b/board/safety.h index dc2314fdef..474c3c663a 100644 --- a/board/safety.h +++ b/board/safety.h @@ -610,7 +610,7 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const bool violation = false; if (controls_allowed && steer_control_enabled) { - // convert floating point angle rate limits to ints in the scale of the desired angle on CAN, + // convert floating point angle rate limits to integers in the scale of the desired angle on CAN, // add 1 to not false trigger the violation int delta_angle_up = (interpolate(limits.angle_rate_up_lookup, vehicle_speed) * limits.angle_deg_to_can) + 1.; int delta_angle_down = (interpolate(limits.angle_rate_down_lookup, vehicle_speed) * limits.angle_deg_to_can) + 1.; From 141510e5a021544de8bd0a514a16813a28a12a25 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 1 Dec 2022 00:39:02 -0800 Subject: [PATCH 06/11] tests draft --- tests/safety/common.py | 92 ++++++++++++++++++++++++ tests/safety/test_nissan.py | 135 ++++++++++++++++++------------------ tests/safety/test_tesla.py | 85 +++++++++++++---------- 3 files changed, 209 insertions(+), 103 deletions(-) diff --git a/tests/safety/common.py b/tests/safety/common.py index 51745a8f38..9dc89b5354 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -16,6 +16,10 @@ def make_msg(bus, addr, length=8): return libpanda_py.make_CANPacket(addr, bus, b'\x00' * length) +def sign_of(a): + return 1 if a > 0 else -1 + + class CANPackerPanda(CANPacker): def make_can_msg_panda(self, name_or_addr, bus, values, fix_checksum=None): msg = self.make_can_msg(name_or_addr, bus, values) @@ -127,6 +131,94 @@ def test_gas_interceptor_safety_check(self): self.assertEqual(send, self._tx(self._interceptor_gas_cmd(gas))) +class AngleSteeringSafetyTest(PandaSafetyTestBase): + + DEG_TO_CAN: int + + ANGLE_DELTA_BP: List[float] + ANGLE_DELTA_V: List[float] # windup limit + ANGLE_DELTA_VU: List[float] # unwind limit + + @classmethod + def setUpClass(cls): + if cls.__name__ == "AngleSteeringSafetyTest": + cls.safety = None + raise unittest.SkipTest + + @abc.abstractmethod + def _angle_cmd_msg(self, angle: float, enabled: bool): + pass + + @abc.abstractmethod + def _angle_meas_msg(self, angle: float): + pass + + def _set_prev_desired_angle(self, t): + t = int(t * self.DEG_TO_CAN) + self.safety.set_desired_angle_last(t) + + def _angle_meas_msg_array(self, angle): + for _ in range(6): + self._rx(self._angle_meas_msg(angle)) + + def test_angle_cmd_when_enabled(self): + # when controls are allowed, angle cmd rate limit is enforced + speeds = [0., 1., 5., 10., 15., 50.] + angles = [-300, -100, -10, 0, 10, 100, 300] + for a in angles: + for s in speeds: + max_delta_up = np.interp(s, self.ANGLE_DELTA_BP, self.ANGLE_DELTA_V) + max_delta_down = np.interp(s, self.ANGLE_DELTA_BP, self.ANGLE_DELTA_VU) + + # first test against false positives + self._angle_meas_msg_array(a) + self._rx(self._speed_msg(s)) + + self._set_prev_desired_angle(a) + self.safety.set_controls_allowed(1) + + # Stay within limits + # Up + self.assertTrue(self._tx(self._angle_cmd_msg(a + sign_of(a) * max_delta_up, True))) + self.assertTrue(self.safety.get_controls_allowed()) + + # Don't change + self.assertTrue(self._tx(self._angle_cmd_msg(a, True))) + self.assertTrue(self.safety.get_controls_allowed()) + + # Down + self.assertTrue(self._tx(self._angle_cmd_msg(a - sign_of(a) * max_delta_down, True))) + self.assertTrue(self.safety.get_controls_allowed()) + + # Inject too high rates + # Up + self.assertFalse(self._tx(self._angle_cmd_msg(a + sign_of(a) * (max_delta_up + 1), True))) + + # Don't change + self.safety.set_controls_allowed(1) + self._set_prev_desired_angle(a) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertTrue(self._tx(self._angle_cmd_msg(a, True))) + self.assertTrue(self.safety.get_controls_allowed()) + + # Down + print(s, a) + print(max_delta_down) + self.assertFalse(self._tx(self._angle_cmd_msg(a - sign_of(a) * (max_delta_down + 1), True)), a - sign_of(a) * (max_delta_down + 1)) + + # Check desired steer should be the same as steer angle when controls are off + self.safety.set_controls_allowed(0) + self.assertTrue(self._tx(self._angle_cmd_msg(a, False))) + + def test_angle_cmd_when_disabled_2(self): + self.safety.set_controls_allowed(0) + # raise Exception + + self._set_prev_desired_angle(0) + self.assertFalse(self._tx(self._angle_cmd_msg(0., True))) + self.assertFalse(self.safety.get_controls_allowed()) + + class TorqueSteeringSafetyTestBase(PandaSafetyTestBase): MAX_RATE_UP = 0 diff --git a/tests/safety/test_nissan.py b/tests/safety/test_nissan.py index 74c34c6d39..ba1e04aa5c 100755 --- a/tests/safety/test_nissan.py +++ b/tests/safety/test_nissan.py @@ -6,16 +6,12 @@ import panda.tests.safety.common as common from panda.tests.safety.common import CANPackerPanda -ANGLE_DELTA_BP = [0., 5., 15.] -ANGLE_DELTA_V = [5., .8, .15] # windup limit -ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit - def sign(a): return 1 if a > 0 else -1 -class TestNissanSafety(common.PandaSafetyTest): +class TestNissanSafety(common.PandaSafetyTest, common.AngleSteeringSafetyTest): TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2], [0x280, 2]] STANDSTILL_THRESHOLD = 0 @@ -25,6 +21,13 @@ class TestNissanSafety(common.PandaSafetyTest): FWD_BLACKLISTED_ADDRS = {0: [0x280], 2: [0x169, 0x2b1, 0x4cc]} FWD_BUS_LOOKUP = {0: 2, 2: 0} + # Angle control limits + DEG_TO_CAN = -100 + + ANGLE_DELTA_BP = [0., 5., 15.] + ANGLE_DELTA_V = [5., .8, .15] # windup limit + ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit + def setUp(self): self.packer = CANPackerPanda("nissan_x_trail_2017") self.safety = libpanda_py.libpanda @@ -35,20 +38,20 @@ def _angle_meas_msg(self, angle): values = {"STEER_ANGLE": angle} return self.packer.make_can_msg_panda("STEER_ANGLE_SENSOR", 0, values) - def _set_prev_angle(self, t): - t = int(t * -100) - self.safety.set_desired_angle_last(t) + # def _set_prev_angle(self, t): + # t = int(t * -100) + # self.safety.set_desired_angle_last(t) - def _angle_meas_msg_array(self, angle): - for _ in range(6): - self._rx(self._angle_meas_msg(angle)) + # def _angle_meas_msg_array(self, angle): + # for _ in range(6): + # self._rx(self._angle_meas_msg(angle)) def _pcm_status_msg(self, enable): values = {"CRUISE_ENABLED": enable} return self.packer.make_can_msg_panda("CRUISE_STATE", 2, values) - def _lkas_control_msg(self, angle, state): - values = {"DESIRED_ANGLE": angle, "LKA_ACTIVE": state} + def _angle_cmd_msg(self, angle: float, enabled: bool): + values = {"DESIRED_ANGLE": angle, "LKA_ACTIVE": 1 if enabled else 0} return self.packer.make_can_msg_panda("LKAS", 0, values) def _speed_msg(self, speed): @@ -71,59 +74,59 @@ def _acc_button_cmd(self, cancel=0, propilot=0, flw_dist=0, _set=0, res=0): "RES_BUTTON": res, "NO_BUTTON_PRESSED": no_button} return self.packer.make_can_msg_panda("CRUISE_THROTTLE", 2, values) - def test_angle_cmd_when_enabled(self): - # when controls are allowed, angle cmd rate limit is enforced - speeds = [0., 1., 5., 10., 15., 50.] - angles = [-300, -100, -10, 0, 10, 100, 300] - for a in angles: - for s in speeds: - max_delta_up = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) - max_delta_down = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) - - # first test against false positives - self._angle_meas_msg_array(a) - self._rx(self._speed_msg(s)) - - self._set_prev_angle(a) - self.safety.set_controls_allowed(1) - - # Stay within limits - # Up - self.assertEqual(True, self._tx(self._lkas_control_msg(a + sign(a) * max_delta_up, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Don't change - self.assertEqual(True, self._tx(self._lkas_control_msg(a, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Down - self.assertEqual(True, self._tx(self._lkas_control_msg(a - sign(a) * max_delta_down, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Inject too high rates - # Up - self.assertEqual(False, self._tx(self._lkas_control_msg(a + sign(a) * (max_delta_up + 1), 1))) - - # Don't change - self.safety.set_controls_allowed(1) - self._set_prev_angle(a) - self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self._tx(self._lkas_control_msg(a, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Down - self.assertEqual(False, self._tx(self._lkas_control_msg(a - sign(a) * (max_delta_down + 1), 1))) - - # Check desired steer should be the same as steer angle when controls are off - self.safety.set_controls_allowed(0) - self.assertEqual(True, self._tx(self._lkas_control_msg(a, 0))) - - def test_angle_cmd_when_disabled(self): - self.safety.set_controls_allowed(0) - - self._set_prev_angle(0) - self.assertFalse(self._tx(self._lkas_control_msg(0, 1))) - self.assertFalse(self.safety.get_controls_allowed()) + # def test_angle_cmd_when_enabled_old(self): + # # when controls are allowed, angle cmd rate limit is enforced + # speeds = [0., 1., 5., 10., 15., 50.] + # angles = [-300, -100, -10, 0, 10, 100, 300] + # for a in angles: + # for s in speeds: + # max_delta_up = np.interp(s, self.ANGLE_DELTA_BP, self.ANGLE_DELTA_V) + # max_delta_down = np.interp(s, self.ANGLE_DELTA_BP, self.ANGLE_DELTA_VU) + # + # # first test against false positives + # self._angle_meas_msg_array(a) + # self._rx(self._speed_msg(s)) + # + # self._set_prev_desired_angle(a) + # self.safety.set_controls_allowed(1) + # + # # Stay within limits + # # Up + # self.assertEqual(True, self._tx(self._angle_cmd_msg(a + sign(a) * max_delta_up, 1))) + # self.assertTrue(self.safety.get_controls_allowed()) + # + # # Don't change + # self.assertEqual(True, self._tx(self._angle_cmd_msg(a, 1))) + # self.assertTrue(self.safety.get_controls_allowed()) + # + # # Down + # self.assertEqual(True, self._tx(self._angle_cmd_msg(a - sign(a) * max_delta_down, 1))) + # self.assertTrue(self.safety.get_controls_allowed()) + # + # # Inject too high rates + # # Up + # self.assertEqual(False, self._tx(self._angle_cmd_msg(a + sign(a) * (max_delta_up + 1), 1))) + # + # # Don't change + # self.safety.set_controls_allowed(1) + # self._set_prev_desired_angle(a) + # self.assertTrue(self.safety.get_controls_allowed()) + # self.assertEqual(True, self._tx(self._angle_cmd_msg(a, 1))) + # self.assertTrue(self.safety.get_controls_allowed()) + # + # # Down + # self.assertEqual(False, self._tx(self._angle_cmd_msg(a - sign(a) * (max_delta_down + 1), 1))) + # + # # Check desired steer should be the same as steer angle when controls are off + # self.safety.set_controls_allowed(0) + # self.assertEqual(True, self._tx(self._angle_cmd_msg(a, 0))) + + # def test_angle_cmd_when_disabled(self): + # self.safety.set_controls_allowed(0) + # + # self._set_prev_desired_angle(0) + # self.assertFalse(self._tx(self._angle_cmd_msg(0, 1))) + # self.assertFalse(self.safety.get_controls_allowed()) def test_acc_buttons(self): btns = [ diff --git a/tests/safety/test_tesla.py b/tests/safety/test_tesla.py index 370cfda5c5..ae2a1b2ae9 100755 --- a/tests/safety/test_tesla.py +++ b/tests/safety/test_tesla.py @@ -6,10 +6,6 @@ from panda.tests.libpanda import libpanda_py from panda.tests.safety.common import CANPackerPanda -ANGLE_DELTA_BP = [0., 5., 15.] -ANGLE_DELTA_V = [5., .8, .15] # windup limit -ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit - MAX_ACCEL = 2.0 MIN_ACCEL = -3.5 @@ -22,7 +18,7 @@ class CONTROL_LEVER_STATE: FWD = 1 IDLE = 0 -def sign(a): +def sign_of(a): return 1 if a > 0 else -1 class TestTeslaSafety(common.PandaSafetyTest): @@ -35,21 +31,21 @@ def setUp(self): self.packer = None raise unittest.SkipTest - def _angle_meas_msg(self, angle): - values = {"EPAS_internalSAS": angle} - return self.packer.make_can_msg_panda("EPAS_sysStatus", 0, values) + # def _angle_meas_msg(self, angle): + # values = {"EPAS_internalSAS": angle} + # return self.packer.make_can_msg_panda("EPAS_sysStatus", 0, values) - def _set_prev_angle(self, t): - t = int(t * 10) - self.safety.set_desired_angle_last(t) + # def _set_prev_desired_angle(self, t): + # t = int(t * 10) + # self.safety.set_desired_angle_last(t) - def _angle_meas_msg_array(self, angle): - for _ in range(6): - self._rx(self._angle_meas_msg(angle)) + # def _angle_meas_msg_array(self, angle): + # for _ in range(6): + # self._rx(self._angle_meas_msg(angle)) - def _lkas_control_msg(self, angle, enabled): - values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": 1 if enabled else 0} - return self.packer.make_can_msg_panda("DAS_steeringControl", 0, values) + # def _angle_cmd_msg(self, angle, enabled): + # values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": 1 if enabled else 0} + # return self.packer.make_can_msg_panda("DAS_steeringControl", 0, values) def _speed_msg(self, speed): values = {"DI_vehicleSpeed": speed / 0.447} @@ -83,70 +79,85 @@ def _long_control_msg(self, set_speed, acc_val=0, jerk_limits=(0, 0), accel_limi } return self.packer.make_can_msg_panda("DAS_control", 0, values) -class TestTeslaSteeringSafety(TestTeslaSafety): +class TestTeslaSteeringSafety(TestTeslaSafety, common.AngleSteeringSafetyTest): TX_MSGS = [[0x488, 0], [0x45, 0], [0x45, 2]] RELAY_MALFUNCTION_ADDR = 0x488 FWD_BLACKLISTED_ADDRS = {2: [0x488]} + # Angle control limits + DEG_TO_CAN = 10 + + ANGLE_DELTA_BP = [0., 5., 15.] + ANGLE_DELTA_V = [5., .8, .15] # windup limit + ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit + def setUp(self): self.packer = CANPackerPanda("tesla_can") self.safety = libpanda_py.libpanda self.safety.set_safety_hooks(Panda.SAFETY_TESLA, 0) self.safety.init_tests() - def test_angle_cmd_when_enabled(self): + def _angle_cmd_msg(self, angle, enabled): + values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": 1 if enabled else 0} + return self.packer.make_can_msg_panda("DAS_steeringControl", 0, values) + + def _angle_meas_msg(self, angle): + values = {"EPAS_internalSAS": angle} + return self.packer.make_can_msg_panda("EPAS_sysStatus", 0, values) + + def test_angle_cmd_when_enabled_old(self): # when controls are allowed, angle cmd rate limit is enforced speeds = [0., 1., 5., 10., 15., 50.] angles = [-300, -100, -10, 0, 10, 100, 300] for a in angles: for s in speeds: - max_delta_up = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) - max_delta_down = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) + max_delta_up = np.interp(s, self.ANGLE_DELTA_BP, self.ANGLE_DELTA_V) + max_delta_down = np.interp(s, self.ANGLE_DELTA_BP, self.ANGLE_DELTA_VU) # first test against false positives self._angle_meas_msg_array(a) self._rx(self._speed_msg(s)) - self._set_prev_angle(a) + self._set_prev_desired_angle(a) self.safety.set_controls_allowed(1) # Stay within limits # Up - self.assertEqual(True, self._tx(self._lkas_control_msg(a + sign(a) * max_delta_up, 1))) + self.assertEqual(True, self._tx(self._angle_cmd_msg(a + sign_of(a) * max_delta_up, True))) self.assertTrue(self.safety.get_controls_allowed()) # Don't change - self.assertEqual(True, self._tx(self._lkas_control_msg(a, 1))) + self.assertEqual(True, self._tx(self._angle_cmd_msg(a, True))) self.assertTrue(self.safety.get_controls_allowed()) # Down - self.assertEqual(True, self._tx(self._lkas_control_msg(a - sign(a) * max_delta_down, 1))) + self.assertEqual(True, self._tx(self._angle_cmd_msg(a - sign_of(a) * max_delta_down, True))) self.assertTrue(self.safety.get_controls_allowed()) # Inject too high rates # Up - self.assertEqual(False, self._tx(self._lkas_control_msg(a + sign(a) * (max_delta_up + 1.1), 1))) + self.assertEqual(False, self._tx(self._angle_cmd_msg(a + sign_of(a) * (max_delta_up + 1.1), True))) # Don't change self.safety.set_controls_allowed(1) - self._set_prev_angle(a) + self._set_prev_desired_angle(a) self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self._tx(self._lkas_control_msg(a, 1))) + self.assertEqual(True, self._tx(self._angle_cmd_msg(a, True))) self.assertTrue(self.safety.get_controls_allowed()) # Down - self.assertEqual(False, self._tx(self._lkas_control_msg(a - sign(a) * (max_delta_down + 1.1), 1))) + self.assertEqual(False, self._tx(self._angle_cmd_msg(a - sign_of(a) * (max_delta_down + 1.1), True))) # Check desired steer should be the same as steer angle when controls are off self.safety.set_controls_allowed(0) - self.assertEqual(True, self._tx(self._lkas_control_msg(a, 0))) - - def test_angle_cmd_when_disabled(self): - self.safety.set_controls_allowed(0) - - self._set_prev_angle(0) - self.assertFalse(self._tx(self._lkas_control_msg(0, 1))) - self.assertFalse(self.safety.get_controls_allowed()) + self.assertEqual(True, self._tx(self._angle_cmd_msg(a, False))) + + # def test_angle_cmd_when_disabled(self): + # self.safety.set_controls_allowed(0) + # + # self._set_prev_desired_angle(0) + # self.assertFalse(self._tx(self._angle_cmd_msg(0, 1))) + # self.assertFalse(self.safety.get_controls_allowed()) def test_acc_buttons(self): """ From d888c466972748635dfaf8132fe77f3c0d3d54fc Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 1 Dec 2022 00:50:55 -0800 Subject: [PATCH 07/11] clean up --- tests/safety/common.py | 4 +- tests/safety/test_nissan.py | 75 ++----------------------------------- tests/safety/test_tesla.py | 73 ------------------------------------ 3 files changed, 5 insertions(+), 147 deletions(-) diff --git a/tests/safety/common.py b/tests/safety/common.py index 9dc89b5354..c15763df6e 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -202,9 +202,7 @@ def test_angle_cmd_when_enabled(self): self.assertTrue(self.safety.get_controls_allowed()) # Down - print(s, a) - print(max_delta_down) - self.assertFalse(self._tx(self._angle_cmd_msg(a - sign_of(a) * (max_delta_down + 1), True)), a - sign_of(a) * (max_delta_down + 1)) + self.assertFalse(self._tx(self._angle_cmd_msg(a - sign_of(a) * (max_delta_down + 1.1), True))) # TODO: WHY 1.1? # Check desired steer should be the same as steer angle when controls are off self.safety.set_controls_allowed(0) diff --git a/tests/safety/test_nissan.py b/tests/safety/test_nissan.py index ba1e04aa5c..180f83206e 100755 --- a/tests/safety/test_nissan.py +++ b/tests/safety/test_nissan.py @@ -1,16 +1,11 @@ #!/usr/bin/env python3 import unittest -import numpy as np from panda import Panda from panda.tests.libpanda import libpanda_py import panda.tests.safety.common as common from panda.tests.safety.common import CANPackerPanda -def sign(a): - return 1 if a > 0 else -1 - - class TestNissanSafety(common.PandaSafetyTest, common.AngleSteeringSafetyTest): TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2], [0x280, 2]] @@ -34,26 +29,18 @@ def setUp(self): self.safety.set_safety_hooks(Panda.SAFETY_NISSAN, 0) self.safety.init_tests() + def _angle_cmd_msg(self, angle: float, enabled: bool): + values = {"DESIRED_ANGLE": angle, "LKA_ACTIVE": 1 if enabled else 0} + return self.packer.make_can_msg_panda("LKAS", 0, values) + def _angle_meas_msg(self, angle): values = {"STEER_ANGLE": angle} return self.packer.make_can_msg_panda("STEER_ANGLE_SENSOR", 0, values) - # def _set_prev_angle(self, t): - # t = int(t * -100) - # self.safety.set_desired_angle_last(t) - - # def _angle_meas_msg_array(self, angle): - # for _ in range(6): - # self._rx(self._angle_meas_msg(angle)) - def _pcm_status_msg(self, enable): values = {"CRUISE_ENABLED": enable} return self.packer.make_can_msg_panda("CRUISE_STATE", 2, values) - def _angle_cmd_msg(self, angle: float, enabled: bool): - values = {"DESIRED_ANGLE": angle, "LKA_ACTIVE": 1 if enabled else 0} - return self.packer.make_can_msg_panda("LKAS", 0, values) - def _speed_msg(self, speed): # TODO: why the 3.6? m/s to kph? not in dbc values = {"WHEEL_SPEED_%s" % s: speed * 3.6 for s in ["RR", "RL"]} @@ -74,60 +61,6 @@ def _acc_button_cmd(self, cancel=0, propilot=0, flw_dist=0, _set=0, res=0): "RES_BUTTON": res, "NO_BUTTON_PRESSED": no_button} return self.packer.make_can_msg_panda("CRUISE_THROTTLE", 2, values) - # def test_angle_cmd_when_enabled_old(self): - # # when controls are allowed, angle cmd rate limit is enforced - # speeds = [0., 1., 5., 10., 15., 50.] - # angles = [-300, -100, -10, 0, 10, 100, 300] - # for a in angles: - # for s in speeds: - # max_delta_up = np.interp(s, self.ANGLE_DELTA_BP, self.ANGLE_DELTA_V) - # max_delta_down = np.interp(s, self.ANGLE_DELTA_BP, self.ANGLE_DELTA_VU) - # - # # first test against false positives - # self._angle_meas_msg_array(a) - # self._rx(self._speed_msg(s)) - # - # self._set_prev_desired_angle(a) - # self.safety.set_controls_allowed(1) - # - # # Stay within limits - # # Up - # self.assertEqual(True, self._tx(self._angle_cmd_msg(a + sign(a) * max_delta_up, 1))) - # self.assertTrue(self.safety.get_controls_allowed()) - # - # # Don't change - # self.assertEqual(True, self._tx(self._angle_cmd_msg(a, 1))) - # self.assertTrue(self.safety.get_controls_allowed()) - # - # # Down - # self.assertEqual(True, self._tx(self._angle_cmd_msg(a - sign(a) * max_delta_down, 1))) - # self.assertTrue(self.safety.get_controls_allowed()) - # - # # Inject too high rates - # # Up - # self.assertEqual(False, self._tx(self._angle_cmd_msg(a + sign(a) * (max_delta_up + 1), 1))) - # - # # Don't change - # self.safety.set_controls_allowed(1) - # self._set_prev_desired_angle(a) - # self.assertTrue(self.safety.get_controls_allowed()) - # self.assertEqual(True, self._tx(self._angle_cmd_msg(a, 1))) - # self.assertTrue(self.safety.get_controls_allowed()) - # - # # Down - # self.assertEqual(False, self._tx(self._angle_cmd_msg(a - sign(a) * (max_delta_down + 1), 1))) - # - # # Check desired steer should be the same as steer angle when controls are off - # self.safety.set_controls_allowed(0) - # self.assertEqual(True, self._tx(self._angle_cmd_msg(a, 0))) - - # def test_angle_cmd_when_disabled(self): - # self.safety.set_controls_allowed(0) - # - # self._set_prev_desired_angle(0) - # self.assertFalse(self._tx(self._angle_cmd_msg(0, 1))) - # self.assertFalse(self.safety.get_controls_allowed()) - def test_acc_buttons(self): btns = [ ("cancel", True), diff --git a/tests/safety/test_tesla.py b/tests/safety/test_tesla.py index ae2a1b2ae9..ac3b003031 100755 --- a/tests/safety/test_tesla.py +++ b/tests/safety/test_tesla.py @@ -18,9 +18,6 @@ class CONTROL_LEVER_STATE: FWD = 1 IDLE = 0 -def sign_of(a): - return 1 if a > 0 else -1 - class TestTeslaSafety(common.PandaSafetyTest): STANDSTILL_THRESHOLD = 0 GAS_PRESSED_THRESHOLD = 3 @@ -31,22 +28,6 @@ def setUp(self): self.packer = None raise unittest.SkipTest - # def _angle_meas_msg(self, angle): - # values = {"EPAS_internalSAS": angle} - # return self.packer.make_can_msg_panda("EPAS_sysStatus", 0, values) - - # def _set_prev_desired_angle(self, t): - # t = int(t * 10) - # self.safety.set_desired_angle_last(t) - - # def _angle_meas_msg_array(self, angle): - # for _ in range(6): - # self._rx(self._angle_meas_msg(angle)) - - # def _angle_cmd_msg(self, angle, enabled): - # values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": 1 if enabled else 0} - # return self.packer.make_can_msg_panda("DAS_steeringControl", 0, values) - def _speed_msg(self, speed): values = {"DI_vehicleSpeed": speed / 0.447} return self.packer.make_can_msg_panda("DI_torque2", 0, values) @@ -105,60 +86,6 @@ def _angle_meas_msg(self, angle): values = {"EPAS_internalSAS": angle} return self.packer.make_can_msg_panda("EPAS_sysStatus", 0, values) - def test_angle_cmd_when_enabled_old(self): - # when controls are allowed, angle cmd rate limit is enforced - speeds = [0., 1., 5., 10., 15., 50.] - angles = [-300, -100, -10, 0, 10, 100, 300] - for a in angles: - for s in speeds: - max_delta_up = np.interp(s, self.ANGLE_DELTA_BP, self.ANGLE_DELTA_V) - max_delta_down = np.interp(s, self.ANGLE_DELTA_BP, self.ANGLE_DELTA_VU) - - # first test against false positives - self._angle_meas_msg_array(a) - self._rx(self._speed_msg(s)) - - self._set_prev_desired_angle(a) - self.safety.set_controls_allowed(1) - - # Stay within limits - # Up - self.assertEqual(True, self._tx(self._angle_cmd_msg(a + sign_of(a) * max_delta_up, True))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Don't change - self.assertEqual(True, self._tx(self._angle_cmd_msg(a, True))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Down - self.assertEqual(True, self._tx(self._angle_cmd_msg(a - sign_of(a) * max_delta_down, True))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Inject too high rates - # Up - self.assertEqual(False, self._tx(self._angle_cmd_msg(a + sign_of(a) * (max_delta_up + 1.1), True))) - - # Don't change - self.safety.set_controls_allowed(1) - self._set_prev_desired_angle(a) - self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self._tx(self._angle_cmd_msg(a, True))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Down - self.assertEqual(False, self._tx(self._angle_cmd_msg(a - sign_of(a) * (max_delta_down + 1.1), True))) - - # Check desired steer should be the same as steer angle when controls are off - self.safety.set_controls_allowed(0) - self.assertEqual(True, self._tx(self._angle_cmd_msg(a, False))) - - # def test_angle_cmd_when_disabled(self): - # self.safety.set_controls_allowed(0) - # - # self._set_prev_desired_angle(0) - # self.assertFalse(self._tx(self._angle_cmd_msg(0, 1))) - # self.assertFalse(self.safety.get_controls_allowed()) - def test_acc_buttons(self): """ FWD (cancel) always allowed. From 057f30d6f0270f40ead2e87031cce2e28fade66d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 1 Dec 2022 00:54:33 -0800 Subject: [PATCH 08/11] comment --- tests/safety/common.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/safety/common.py b/tests/safety/common.py index c15763df6e..d44ebaeafe 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -202,7 +202,7 @@ def test_angle_cmd_when_enabled(self): self.assertTrue(self.safety.get_controls_allowed()) # Down - self.assertFalse(self._tx(self._angle_cmd_msg(a - sign_of(a) * (max_delta_down + 1.1), True))) # TODO: WHY 1.1? + self.assertFalse(self._tx(self._angle_cmd_msg(a - sign_of(a) * (max_delta_down + 1.1), True))) # TODO: WHY 1.1?? # Check desired steer should be the same as steer angle when controls are off self.safety.set_controls_allowed(0) From f33be0453d7f18334ad5ae80dc959956d2f82208 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 1 Dec 2022 00:55:56 -0800 Subject: [PATCH 09/11] pylint exception --- tests/safety/common.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/safety/common.py b/tests/safety/common.py index d44ebaeafe..45a1f84b47 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -172,7 +172,7 @@ def test_angle_cmd_when_enabled(self): # first test against false positives self._angle_meas_msg_array(a) - self._rx(self._speed_msg(s)) + self._rx(self._speed_msg(s)) # pylint: disable=no-member self._set_prev_desired_angle(a) self.safety.set_controls_allowed(1) From 88e37254f833795edcbf40576aaa4d8b4b0e51c1 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 2 Dec 2022 16:39:09 -0800 Subject: [PATCH 10/11] clean up --- tests/safety/common.py | 86 ------------------------------------------ 1 file changed, 86 deletions(-) diff --git a/tests/safety/common.py b/tests/safety/common.py index c7e8fc5a9d..92b34e851b 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -135,92 +135,6 @@ def test_gas_interceptor_safety_check(self): self.assertEqual(send, self._tx(self._interceptor_gas_cmd(gas))) -class AngleSteeringSafetyTest(PandaSafetyTestBase): - - DEG_TO_CAN: int - - ANGLE_DELTA_BP: List[float] - ANGLE_DELTA_V: List[float] # windup limit - ANGLE_DELTA_VU: List[float] # unwind limit - - @classmethod - def setUpClass(cls): - if cls.__name__ == "AngleSteeringSafetyTest": - cls.safety = None - raise unittest.SkipTest - - @abc.abstractmethod - def _angle_cmd_msg(self, angle: float, enabled: bool): - pass - - @abc.abstractmethod - def _angle_meas_msg(self, angle: float): - pass - - def _set_prev_desired_angle(self, t): - t = int(t * self.DEG_TO_CAN) - self.safety.set_desired_angle_last(t) - - def _angle_meas_msg_array(self, angle): - for _ in range(6): - self._rx(self._angle_meas_msg(angle)) - - def test_angle_cmd_when_enabled(self): - # when controls are allowed, angle cmd rate limit is enforced - speeds = [0., 1., 5., 10., 15., 50.] - angles = [-300, -100, -10, 0, 10, 100, 300] - for a in angles: - for s in speeds: - max_delta_up = np.interp(s, self.ANGLE_DELTA_BP, self.ANGLE_DELTA_V) - max_delta_down = np.interp(s, self.ANGLE_DELTA_BP, self.ANGLE_DELTA_VU) - - # first test against false positives - self._angle_meas_msg_array(a) - self._rx(self._speed_msg(s)) # pylint: disable=no-member - - self._set_prev_desired_angle(a) - self.safety.set_controls_allowed(1) - - # Stay within limits - # Up - self.assertTrue(self._tx(self._angle_cmd_msg(a + sign_of(a) * max_delta_up, True))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Don't change - self.assertTrue(self._tx(self._angle_cmd_msg(a, True))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Down - self.assertTrue(self._tx(self._angle_cmd_msg(a - sign_of(a) * max_delta_down, True))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Inject too high rates - # Up - self.assertFalse(self._tx(self._angle_cmd_msg(a + sign_of(a) * (max_delta_up + 1), True))) - - # Don't change - self.safety.set_controls_allowed(1) - self._set_prev_desired_angle(a) - self.assertTrue(self.safety.get_controls_allowed()) - self.assertTrue(self._tx(self._angle_cmd_msg(a, True))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Down - self.assertFalse(self._tx(self._angle_cmd_msg(a - sign_of(a) * (max_delta_down + 1.1), True))) # TODO: WHY 1.1?? - - # Check desired steer should be the same as steer angle when controls are off - self.safety.set_controls_allowed(0) - self.assertTrue(self._tx(self._angle_cmd_msg(a, False))) - - def test_angle_cmd_when_disabled_2(self): - self.safety.set_controls_allowed(0) - # raise Exception - - self._set_prev_desired_angle(0) - self.assertFalse(self._tx(self._angle_cmd_msg(0., True))) - self.assertFalse(self.safety.get_controls_allowed()) - - class TorqueSteeringSafetyTestBase(PandaSafetyTestBase): MAX_RATE_UP = 0 From ff064a97bca4d86772a012d597932a82513005ee Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 2 Dec 2022 16:40:16 -0800 Subject: [PATCH 11/11] duplicate --- tests/safety/common.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/tests/safety/common.py b/tests/safety/common.py index 92b34e851b..c07f12a8a2 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -20,10 +20,6 @@ def make_msg(bus, addr, length=8): return libpanda_py.make_CANPacket(addr, bus, b'\x00' * length) -def sign_of(a): - return 1 if a > 0 else -1 - - class CANPackerPanda(CANPacker): def make_can_msg_panda(self, name_or_addr, bus, values, fix_checksum=None): msg = self.make_can_msg(name_or_addr, bus, values)