Added Nissan safety (#244)

* Added Nissan safety

board/safety.h

@@ -14,6 +14,7 @@
 #include "safety/safety_chrysler.h"
 #include "safety/safety_subaru.h"
 #include "safety/safety_mazda.h"
+#include "safety/safety_nissan.h"
 #include "safety/safety_volkswagen.h"
 #include "safety/safety_elm327.h"
 
@@ -31,6 +32,7 @@
 #define SAFETY_TESLA 10U
 #define SAFETY_SUBARU 11U
 #define SAFETY_MAZDA 13U
+#define SAFETY_NISSAN 14U
 #define SAFETY_VOLKSWAGEN_MQB 15U
 #define SAFETY_TOYOTA_IPAS 16U
 #define SAFETY_ALLOUTPUT 17U
@@ -206,6 +208,7 @@ const safety_hook_config safety_hook_registry[] = {
   {SAFETY_CADILLAC, &cadillac_hooks},
   {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
   {SAFETY_TESLA, &tesla_hooks},
+  {SAFETY_NISSAN, &nissan_hooks},
   {SAFETY_ALLOUTPUT, &alloutput_hooks},
   {SAFETY_GM_ASCM, &gm_ascm_hooks},
   {SAFETY_FORD, &ford_hooks},

board/safety/safety_nissan.h

@@ -0,0 +1,215 @@
+
+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 struct lookup_t NISSAN_LOOKUP_MAX_ANGLE = {
+  {3.3, 12, 32},
+  {540., 120., 23.}};
+
+const int NISSAN_DEG_TO_CAN = 100; 
+
+const AddrBus NISSAN_TX_MSGS[] = {{0x169, 0}, {0x20b, 2}};
+
+AddrCheckStruct nissan_rx_checks[] = {
+  {.addr = {0x2}, .bus = 0, .expected_timestep = 100000U},
+  {.addr = {0x29a}, .bus = 0, .expected_timestep = 50000U},
+  {.addr = {0x1b6}, .bus = 1, .expected_timestep = 100000U},
+};
+const int NISSAN_RX_CHECK_LEN = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]);
+
+float nissan_speed = 0;
+//int nissan_controls_allowed_last = 0;
+uint32_t nissan_ts_angle_last = 0;
+int nissan_cruise_engaged_last = 0;
+int nissan_desired_angle_last = 0;
+int nissan_gas_prev = 0;
+int nissan_brake_prev = 0;
+
+struct sample_t nissan_angle_meas;            // last 3 steer angles
+
+
+static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+
+  bool valid = addr_safety_check(to_push, nissan_rx_checks, NISSAN_RX_CHECK_LEN,
+                                 NULL, NULL, NULL);
+
+  if (valid) {
+    int bus = GET_BUS(to_push);
+    int addr = GET_ADDR(to_push);
+
+    if (bus == 0) {
+      if (addr == 0x2) {
+        // Current steering angle
+        // Factor -0.1, little endian
+        int angle_meas_new = (GET_BYTES_04(to_push) & 0xFFFF);
+        // Need to multiply by 10 here as LKAS and Steering wheel are different base unit
+        angle_meas_new = to_signed(angle_meas_new, 16) * 10;
+
+        // update array of samples
+        update_sample(&nissan_angle_meas, angle_meas_new);
+      }
+
+      if (addr == 0x29a) {
+        // Get current speed
+        // Factor 0.00555
+        nissan_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.00555 / 3.6;
+      }
+
+      // exit controls on rising edge of gas press
+      if (addr == 0x15c) {
+        int gas = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3));
+        if ((gas > 0) && (nissan_gas_prev == 0)) {
+          controls_allowed = 0;
+        }
+        nissan_gas_prev = gas;
+      }
+
+      // 0x169 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
+      if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x169)) {
+        relay_malfunction = true;
+      }
+    }
+
+    if (bus == 1) {
+      if (addr == 0x1b6) {
+        int cruise_engaged = (GET_BYTE(to_push, 4) >> 6) & 1;
+        if (cruise_engaged && !nissan_cruise_engaged_last) {
+          controls_allowed = 1;
+        }
+        if (!cruise_engaged) {
+          controls_allowed = 0;
+        }
+        nissan_cruise_engaged_last = cruise_engaged;
+      }
+
+      // exit controls on rising edge of brake press, or if speed > 0 and brake
+      if (addr == 0x454) {
+        int brake = (GET_BYTE(to_push, 2) & 0x80);
+        if ((brake > 0) && ((nissan_brake_prev == 0) || (nissan_speed > 0.))) {
+          controls_allowed = 0;
+        }
+        nissan_brake_prev = brake;
+      }
+    }
+  }
+  return valid;
+}
+
+
+static int nissan_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+  int tx = 1;
+  int addr = GET_ADDR(to_send);
+  int bus = GET_BUS(to_send);
+  bool violation = 0;
+
+  if (!msg_allowed(addr, bus, NISSAN_TX_MSGS, sizeof(NISSAN_TX_MSGS) / sizeof(NISSAN_TX_MSGS[0]))) {
+    tx = 0;
+  }
+
+  if (relay_malfunction) {
+    tx = 0;
+  }
+
+  // steer cmd checks
+  if (addr == 0x169) {
+    int desired_angle = ((GET_BYTE(to_send, 0) << 10) | (GET_BYTE(to_send, 1) << 2) | ((GET_BYTE(to_send, 2) >> 6) & 0x3));
+    bool lka_active = (GET_BYTE(to_send, 6) >> 4) & 1;
+
+    // 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, nissan_speed) * NISSAN_DEG_TO_CAN) + 1.;
+      int delta_angle_up = (int)(delta_angle_float);
+      delta_angle_float =  (interpolate(NISSAN_LOOKUP_ANGLE_RATE_DOWN, nissan_speed) * NISSAN_DEG_TO_CAN) + 1.;
+      int delta_angle_down = (int)(delta_angle_float);
+      int highest_desired_angle = nissan_desired_angle_last + ((nissan_desired_angle_last > 0) ? delta_angle_up : delta_angle_down);
+      int lowest_desired_angle = nissan_desired_angle_last - ((nissan_desired_angle_last >= 0) ? delta_angle_down : delta_angle_up);
+
+      // Limit maximum steering angle at current speed 
+      int maximum_angle = ((int)interpolate(NISSAN_LOOKUP_MAX_ANGLE, nissan_speed));
+
+      if (highest_desired_angle > (maximum_angle * NISSAN_DEG_TO_CAN)) {
+        highest_desired_angle = (maximum_angle * NISSAN_DEG_TO_CAN);
+      }
+      if (lowest_desired_angle < (-maximum_angle * NISSAN_DEG_TO_CAN)) {
+        lowest_desired_angle = (-maximum_angle * NISSAN_DEG_TO_CAN);
+      }
+
+      // check for violation;
+      violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
+
+      //nissan_controls_allowed_last = controls_allowed;
+    }
+    nissan_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 < (nissan_angle_meas.min - 1)) ||
+          (desired_angle > (nissan_angle_meas.max + 1)))) {
+      violation = 1;
+    }
+
+    // no lka_enabled bit if controls not allowed
+    if (!controls_allowed && lka_active) {
+      violation = 1;
+    }
+  }
+
+  // acc button check, only allow cancel button to be sent
+  if (addr == 0x20b) {
+    // Violation of any button other than cancel is pressed
+    violation |= ((GET_BYTE(to_send, 1) & 0x3d) > 0);
+  }
+
+  if (violation) {
+    controls_allowed = 0;
+    tx = 0;
+  }
+
+  return tx;
+}
+
+
+static int nissan_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  int bus_fwd = -1;
+  int addr = GET_ADDR(to_fwd);
+
+  if (bus_num == 0) {
+    bus_fwd = 2;  // ADAS
+  }
+
+  if (bus_num == 2) {
+    // 0x169 is LKAS
+    int block_msg = (addr == 0x169);
+    if (!block_msg) {
+      bus_fwd = 0;  // V-CAN
+    }
+  }
+
+  if (relay_malfunction) {
+    bus_fwd = -1;
+  }
+
+  // fallback to do not forward
+  return bus_fwd;
+}
+
+const safety_hooks nissan_hooks = {
+  .init = nooutput_init,
+  .rx = nissan_rx_hook,
+  .tx = nissan_tx_hook,
+  .tx_lin = nooutput_tx_lin_hook,
+  .fwd = nissan_fwd_hook,
+  .addr_check = nissan_rx_checks,
+  .addr_check_len = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]),
+};

python/__init__.py

@@ -123,6 +123,7 @@ class Panda(object):
   SAFETY_TESLA = 10
   SAFETY_SUBARU = 11
   SAFETY_MAZDA = 13
+  SAFETY_NISSAN = 14
   SAFETY_VOLKSWAGEN_MQB = 15
   SAFETY_TOYOTA_IPAS = 16
   SAFETY_ALLOUTPUT = 17

tests/safety/libpandasafety_py.py

@@ -100,6 +100,10 @@
 void set_volkswagen_rt_torque_last(int t);
 void set_volkswagen_torque_driver(int min, int max);
 
+void init_tests_nissan(void);
+void set_nissan_desired_angle_last(int t);
+void set_nissan_brake_prev(int t);
+
 """)
 
 libpandasafety = ffi.dlopen(libpandasafety_fn)

tests/safety/test.c

@@ -272,6 +272,14 @@ void set_honda_fwd_brake(bool c){
   honda_fwd_brake = c;
 }
 
+void set_nissan_brake_prev(bool c){
+  nissan_brake_prev = c;
+}
+
+void set_nissan_desired_angle_last(int t){
+  nissan_desired_angle_last = t;
+}
+
 void init_tests(void){
   // get HW_TYPE from env variable set in test.sh
   hw_type = atoi(getenv("HW_TYPE"));
@@ -361,6 +369,16 @@ void init_tests_honda(void){
   honda_fwd_brake = false;
 }
 
+void init_tests_nissan(void){
+  init_tests();
+  nissan_angle_meas.min = 0;
+  nissan_angle_meas.max = 0;
+  nissan_desired_angle_last = 0;
+  nissan_gas_prev = 0;
+  nissan_brake_prev = 0;
+  set_timer(0);
+}
+
 void set_gmlan_digital_output(int to_set){
 }