safety: common angle checks

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

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

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

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

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)
 

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.