parent
d7f1195d1e
commit
db94a5b813
|
@ -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},
|
||||
|
|
|
@ -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]),
|
||||
};
|
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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){
|
||||
}
|
||||
|
||||
|
|
|
@ -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()
|
Loading…
Reference in New Issue