diff --git a/board/safety.h b/board/safety.h index e13021f..22c9721 100644 --- a/board/safety.h +++ b/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}, diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h new file mode 100644 index 0000000..c5a2132 --- /dev/null +++ b/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]), +}; diff --git a/python/__init__.py b/python/__init__.py index ea2b2d3..25f7d62 100644 --- a/python/__init__.py +++ b/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 diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index 82ae5b1..bb371cd 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -100,6 +100,10 @@ void set_volkswagen_desired_torque_last(int t); 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) diff --git a/tests/safety/test.c b/tests/safety/test.c index 5c405d8..4cec596 100644 --- a/tests/safety/test.c +++ b/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){ } diff --git a/tests/safety/test_nissan.py b/tests/safety/test_nissan.py new file mode 100644 index 0000000..1568208 --- /dev/null +++ b/tests/safety/test_nissan.py @@ -0,0 +1,199 @@ +#!/usr/bin/env python3 +import unittest +import numpy as np +from panda import Panda +from panda.tests.safety import libpandasafety_py +from panda.tests.safety.common import make_msg, test_relay_malfunction + +ANGLE_MAX_BP = [1.3, 10., 30.] +ANGLE_MAX_V = [540., 120., 23.] +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 twos_comp(val, bits): + if val >= 0: + return val + else: + return (2**bits) + val + +def sign(a): + if a > 0: + return 1 + else: + return -1 + + +class TestNissanSafety(unittest.TestCase): + @classmethod + def setUp(cls): + cls.safety = libpandasafety_py.libpandasafety + cls.safety.set_safety_hooks(Panda.SAFETY_NISSAN, 0) + cls.safety.init_tests_nissan() + + def _angle_meas_msg(self, angle): + to_send = make_msg(0, 0x2) + angle = int(angle * -10) + t = twos_comp(angle, 16) + to_send[0].RDLR = t & 0xFFFF + + return to_send + + def _set_prev_angle(self, t): + t = int(t * -100) + self.safety.set_nissan_desired_angle_last(t) + + def _set_brake_prev(self, state): + state = bool(state) + self.safety.set_nissan_brake_prev(state) + + def _angle_meas_msg_array(self, angle): + for i in range(6): + self.safety.safety_rx_hook(self._angle_meas_msg(angle)) + + def _lkas_state_msg(self, state): + to_send = make_msg(0, 0x1b6) + to_send[0].RDHR = (state & 0x1) << 6 + + return to_send + + def _lkas_control_msg(self, angle, state): + to_send = make_msg(0, 0x169) + angle = int((angle - 1310) * -100) + to_send[0].RDLR = ((angle & 0x3FC00) >> 10) | ((angle & 0x3FC) << 6) | ((angle & 0x3) << 16) + to_send[0].RDHR = ((state & 0x1) << 20) + + return to_send + + def _speed_msg(self, speed): + to_send = make_msg(0, 0x29a) + speed = int(speed / 0.00555 * 3.6) + to_send[0].RDLR = ((speed & 0xFF) << 24) | ((speed & 0xFF00) << 8) + + return to_send + + def _send_brake_cmd(self, brake): + to_send = make_msg(1, 0x454) + to_send[0].RDLR = ((brake & 0x1) << 23) + + return to_send + + def _send_gas_cmd(self, gas): + to_send = make_msg(0, 0x15c) + to_send[0].RDHR = ((gas & 0x3fc) << 6) | ((gas & 0x3) << 22) + + return to_send + + def _acc_button_cmd(self, buttons): + to_send = make_msg(2, 0x20b) + to_send[0].RDLR = (buttons << 8) + + return to_send + + def test_angle_cmd_when_enabled(self): + + # when controls are allowed, angle cmd rate limit is enforced + # test 1: no limitations if we stay within limits + speeds = [0., 1., 5., 10., 15., 100.] + 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) + angle_lim = np.interp(s, ANGLE_MAX_BP, ANGLE_MAX_V) + + # first test against false positives + self._angle_meas_msg_array(a) + self.safety.safety_rx_hook(self._speed_msg(s)) + + self._set_prev_angle(np.clip(a, -angle_lim, angle_lim)) + self.safety.set_controls_allowed(1) + + self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg( + np.clip(a + sign(a) * max_delta_up, -angle_lim, angle_lim), 1))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(True, self.safety.safety_tx_hook( + self._lkas_control_msg(np.clip(a, -angle_lim, angle_lim), 1))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg( + np.clip(a - sign(a) * max_delta_down, -angle_lim, angle_lim), 1))) + self.assertTrue(self.safety.get_controls_allowed()) + + # now inject too high rates + self.assertEqual(False, self.safety.safety_tx_hook(self._lkas_control_msg(a + sign(a) * + (max_delta_up + 1), 1))) + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(1) + self._set_prev_angle(np.clip(a, -angle_lim, angle_lim)) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(True, self.safety.safety_tx_hook( + self._lkas_control_msg(np.clip(a, -angle_lim, angle_lim), 1))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(False, self.safety.safety_tx_hook(self._lkas_control_msg(a - sign(a) * + (max_delta_down + 1), 1))) + self.assertFalse(self.safety.get_controls_allowed()) + + # Check desired steer should be the same as steer angle when controls are off + self.safety.set_controls_allowed(0) + self.assertEqual(True, self.safety.safety_tx_hook(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.safety.safety_tx_hook(self._lkas_control_msg(0, 1))) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_brake_rising_edge(self): + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._speed_msg(0)) + self._set_brake_prev(True) + self.safety.safety_rx_hook(self._send_brake_cmd(True)) + self.assertTrue(self.safety.get_controls_allowed()) + + self.safety.set_controls_allowed(1) + self._set_brake_prev(False) + self.safety.safety_rx_hook(self._send_brake_cmd(True)) + self.assertFalse(self.safety.get_controls_allowed()) + + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._speed_msg(1)) + self._set_brake_prev(True) + self.safety.safety_rx_hook(self._send_brake_cmd(True)) + self.assertFalse(self.safety.get_controls_allowed()) + + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._speed_msg(1)) + self._set_brake_prev(False) + self.safety.safety_rx_hook(self._send_brake_cmd(True)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_gas_rising_edge(self): + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._send_gas_cmd(100)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_acc_buttons(self): + self.safety.set_controls_allowed(1) + self.safety.safety_tx_hook(self._acc_button_cmd(0x2)) # Cancel button + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.safety_tx_hook(self._acc_button_cmd(0x1)) # ProPilot button + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(1) + self.safety.safety_tx_hook(self._acc_button_cmd(0x4)) # Follow Distance button + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(1) + self.safety.safety_tx_hook(self._acc_button_cmd(0x8)) # Set button + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(1) + self.safety.safety_tx_hook(self._acc_button_cmd(0x10)) # Res button + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(1) + self.safety.safety_tx_hook(self._acc_button_cmd(0x20)) # No button pressed + self.assertFalse(self.safety.get_controls_allowed()) + + def test_relay_malfunction(self): + test_relay_malfunction(self, 0x169) + +if __name__ == "__main__": + unittest.main()