diff --git a/board/safety.h b/board/safety.h index d6bbbc81fb..474c3c663a 100644 --- a/board/safety.h +++ b/board/safety.h @@ -605,6 +605,35 @@ 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) { + // 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.; + + 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 + 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; + + 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..c977cf66fc 100644 --- a/board/safety/safety_nissan.h +++ b/board/safety/safety_nissan.h @@ -1,15 +1,14 @@ - -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 = { + {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 @@ -104,7 +103,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; @@ -115,34 +114,11 @@ 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 (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..a0a82f4bca 100644 --- a/board/safety/safety_tesla.h +++ b/board/safety/safety_tesla.h @@ -1,12 +1,14 @@ -const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_UP = { +const SteeringLimits TESLA_STEERING_LIMITS = { + .angle_deg_to_can = 10, + .angle_rate_up_lookup = { {2., 7., 17.}, - {5., .8, .25}}; - -const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_DOWN = { + {5., .8, .25} + }, + .angle_rate_down_lookup = { {2., 7., 17.}, - {5., 3.5, .8}}; - -const int TESLA_DEG_TO_CAN = 10; + {5., 3.5, .8} + }, +}; const LongitudinalLimits TESLA_LONG_LIMITS = { .max_accel = 425, // 2. m/s^2 @@ -136,29 +138,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 f9106b1afa..7bc1175150 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -34,6 +34,7 @@ typedef enum { } SteeringControlType; typedef struct { + // torque cmd limits const int max_steer; const int max_rate_up; const int max_rate_down; @@ -54,6 +55,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 { @@ -132,6 +138,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); diff --git a/tests/safety/test_nissan.py b/tests/safety/test_nissan.py index 24e266fd14..df3ff36d79 100755 --- a/tests/safety/test_nissan.py +++ b/tests/safety/test_nissan.py @@ -16,6 +16,7 @@ class TestNissanSafety(common.PandaSafetyTest, common.AngleSteeringSafetyTest): 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.] @@ -28,11 +29,11 @@ def setUp(self): self.safety.set_safety_hooks(Panda.SAFETY_NISSAN, 0) self.safety.init_tests() - def _angle_cmd_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 _angle_meas_msg(self, angle): + def _angle_meas_msg(self, angle: float): values = {"STEER_ANGLE": angle} return self.packer.make_can_msg_panda("STEER_ANGLE_SENSOR", 0, values) diff --git a/tests/safety/test_tesla.py b/tests/safety/test_tesla.py index cdca23bb19..bf2367a09a 100755 --- a/tests/safety/test_tesla.py +++ b/tests/safety/test_tesla.py @@ -68,6 +68,7 @@ class TestTeslaSteeringSafety(TestTeslaSafety, common.AngleSteeringSafetyTest): RELAY_MALFUNCTION_ADDR = 0x488 FWD_BLACKLISTED_ADDRS = {2: [0x488]} + # Angle control limits DEG_TO_CAN = 10 ANGLE_DELTA_BP = [0., 5., 15.] @@ -80,14 +81,14 @@ def setUp(self): self.safety.set_safety_hooks(Panda.SAFETY_TESLA, 0) self.safety.init_tests() - def _angle_meas_msg(self, angle): - values = {"EPAS_internalSAS": angle} - return self.packer.make_can_msg_panda("EPAS_sysStatus", 0, values) - - def _angle_cmd_msg(self, angle, enabled): + def _angle_cmd_msg(self, angle: float, enabled: bool): 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: float): + values = {"EPAS_internalSAS": angle} + return self.packer.make_can_msg_panda("EPAS_sysStatus", 0, values) + def test_acc_buttons(self): """ FWD (cancel) always allowed.