Skip to content

Commit db94a5b

Browse files
avolmenskyavolmensky
authored
Added Nissan safety (commaai#244)
* Added Nissan safety
1 parent d7f1195 commit db94a5b

File tree

6 files changed

+440
-0
lines changed

6 files changed

+440
-0
lines changed

‎board/safety.h

+3
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
#include "safety/safety_chrysler.h"
1515
#include "safety/safety_subaru.h"
1616
#include "safety/safety_mazda.h"
17+
#include "safety/safety_nissan.h"
1718
#include "safety/safety_volkswagen.h"
1819
#include "safety/safety_elm327.h"
1920

@@ -31,6 +32,7 @@
3132
#define SAFETY_TESLA 10U
3233
#define SAFETY_SUBARU 11U
3334
#define SAFETY_MAZDA 13U
35+
#define SAFETY_NISSAN 14U
3436
#define SAFETY_VOLKSWAGEN_MQB 15U
3537
#define SAFETY_TOYOTA_IPAS 16U
3638
#define SAFETY_ALLOUTPUT 17U
@@ -206,6 +208,7 @@ const safety_hook_config safety_hook_registry[] = {
206208
{SAFETY_CADILLAC, &cadillac_hooks},
207209
{SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
208210
{SAFETY_TESLA, &tesla_hooks},
211+
{SAFETY_NISSAN, &nissan_hooks},
209212
{SAFETY_ALLOUTPUT, &alloutput_hooks},
210213
{SAFETY_GM_ASCM, &gm_ascm_hooks},
211214
{SAFETY_FORD, &ford_hooks},

‎board/safety/safety_nissan.h

+215
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,215 @@
1+
2+
const uint32_t NISSAN_RT_INTERVAL = 250000; // 250ms between real time checks
3+
4+
const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_UP = {
5+
{2., 7., 17.},
6+
{5., .8, .15}};
7+
8+
const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_DOWN = {
9+
{2., 7., 17.},
10+
{5., 3.5, .5}};
11+
12+
const struct lookup_t NISSAN_LOOKUP_MAX_ANGLE = {
13+
{3.3, 12, 32},
14+
{540., 120., 23.}};
15+
16+
const int NISSAN_DEG_TO_CAN = 100;
17+
18+
const AddrBus NISSAN_TX_MSGS[] = {{0x169, 0}, {0x20b, 2}};
19+
20+
AddrCheckStruct nissan_rx_checks[] = {
21+
{.addr = {0x2}, .bus = 0, .expected_timestep = 100000U},
22+
{.addr = {0x29a}, .bus = 0, .expected_timestep = 50000U},
23+
{.addr = {0x1b6}, .bus = 1, .expected_timestep = 100000U},
24+
};
25+
const int NISSAN_RX_CHECK_LEN = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]);
26+
27+
float nissan_speed = 0;
28+
//int nissan_controls_allowed_last = 0;
29+
uint32_t nissan_ts_angle_last = 0;
30+
int nissan_cruise_engaged_last = 0;
31+
int nissan_desired_angle_last = 0;
32+
int nissan_gas_prev = 0;
33+
int nissan_brake_prev = 0;
34+
35+
struct sample_t nissan_angle_meas; // last 3 steer angles
36+
37+
38+
static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
39+
40+
bool valid = addr_safety_check(to_push, nissan_rx_checks, NISSAN_RX_CHECK_LEN,
41+
NULL, NULL, NULL);
42+
43+
if (valid) {
44+
int bus = GET_BUS(to_push);
45+
int addr = GET_ADDR(to_push);
46+
47+
if (bus == 0) {
48+
if (addr == 0x2) {
49+
// Current steering angle
50+
// Factor -0.1, little endian
51+
int angle_meas_new = (GET_BYTES_04(to_push) & 0xFFFF);
52+
// Need to multiply by 10 here as LKAS and Steering wheel are different base unit
53+
angle_meas_new = to_signed(angle_meas_new, 16) * 10;
54+
55+
// update array of samples
56+
update_sample(&nissan_angle_meas, angle_meas_new);
57+
}
58+
59+
if (addr == 0x29a) {
60+
// Get current speed
61+
// Factor 0.00555
62+
nissan_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.00555 / 3.6;
63+
}
64+
65+
// exit controls on rising edge of gas press
66+
if (addr == 0x15c) {
67+
int gas = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3));
68+
if ((gas > 0) && (nissan_gas_prev == 0)) {
69+
controls_allowed = 0;
70+
}
71+
nissan_gas_prev = gas;
72+
}
73+
74+
// 0x169 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
75+
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x169)) {
76+
relay_malfunction = true;
77+
}
78+
}
79+
80+
if (bus == 1) {
81+
if (addr == 0x1b6) {
82+
int cruise_engaged = (GET_BYTE(to_push, 4) >> 6) & 1;
83+
if (cruise_engaged && !nissan_cruise_engaged_last) {
84+
controls_allowed = 1;
85+
}
86+
if (!cruise_engaged) {
87+
controls_allowed = 0;
88+
}
89+
nissan_cruise_engaged_last = cruise_engaged;
90+
}
91+
92+
// exit controls on rising edge of brake press, or if speed > 0 and brake
93+
if (addr == 0x454) {
94+
int brake = (GET_BYTE(to_push, 2) & 0x80);
95+
if ((brake > 0) && ((nissan_brake_prev == 0) || (nissan_speed > 0.))) {
96+
controls_allowed = 0;
97+
}
98+
nissan_brake_prev = brake;
99+
}
100+
}
101+
}
102+
return valid;
103+
}
104+
105+
106+
static int nissan_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
107+
int tx = 1;
108+
int addr = GET_ADDR(to_send);
109+
int bus = GET_BUS(to_send);
110+
bool violation = 0;
111+
112+
if (!msg_allowed(addr, bus, NISSAN_TX_MSGS, sizeof(NISSAN_TX_MSGS) / sizeof(NISSAN_TX_MSGS[0]))) {
113+
tx = 0;
114+
}
115+
116+
if (relay_malfunction) {
117+
tx = 0;
118+
}
119+
120+
// steer cmd checks
121+
if (addr == 0x169) {
122+
int desired_angle = ((GET_BYTE(to_send, 0) << 10) | (GET_BYTE(to_send, 1) << 2) | ((GET_BYTE(to_send, 2) >> 6) & 0x3));
123+
bool lka_active = (GET_BYTE(to_send, 6) >> 4) & 1;
124+
125+
// offeset 1310 * NISSAN_DEG_TO_CAN
126+
desired_angle = desired_angle - 131000;
127+
128+
if (controls_allowed && lka_active) {
129+
// add 1 to not false trigger the violation
130+
float delta_angle_float;
131+
delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_UP, nissan_speed) * NISSAN_DEG_TO_CAN) + 1.;
132+
int delta_angle_up = (int)(delta_angle_float);
133+
delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_DOWN, nissan_speed) * NISSAN_DEG_TO_CAN) + 1.;
134+
int delta_angle_down = (int)(delta_angle_float);
135+
int highest_desired_angle = nissan_desired_angle_last + ((nissan_desired_angle_last > 0) ? delta_angle_up : delta_angle_down);
136+
int lowest_desired_angle = nissan_desired_angle_last - ((nissan_desired_angle_last >= 0) ? delta_angle_down : delta_angle_up);
137+
138+
// Limit maximum steering angle at current speed
139+
int maximum_angle = ((int)interpolate(NISSAN_LOOKUP_MAX_ANGLE, nissan_speed));
140+
141+
if (highest_desired_angle > (maximum_angle * NISSAN_DEG_TO_CAN)) {
142+
highest_desired_angle = (maximum_angle * NISSAN_DEG_TO_CAN);
143+
}
144+
if (lowest_desired_angle < (-maximum_angle * NISSAN_DEG_TO_CAN)) {
145+
lowest_desired_angle = (-maximum_angle * NISSAN_DEG_TO_CAN);
146+
}
147+
148+
// check for violation;
149+
violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
150+
151+
//nissan_controls_allowed_last = controls_allowed;
152+
}
153+
nissan_desired_angle_last = desired_angle;
154+
155+
// desired steer angle should be the same as steer angle measured when controls are off
156+
if ((!controls_allowed) &&
157+
((desired_angle < (nissan_angle_meas.min - 1)) ||
158+
(desired_angle > (nissan_angle_meas.max + 1)))) {
159+
violation = 1;
160+
}
161+
162+
// no lka_enabled bit if controls not allowed
163+
if (!controls_allowed && lka_active) {
164+
violation = 1;
165+
}
166+
}
167+
168+
// acc button check, only allow cancel button to be sent
169+
if (addr == 0x20b) {
170+
// Violation of any button other than cancel is pressed
171+
violation |= ((GET_BYTE(to_send, 1) & 0x3d) > 0);
172+
}
173+
174+
if (violation) {
175+
controls_allowed = 0;
176+
tx = 0;
177+
}
178+
179+
return tx;
180+
}
181+
182+
183+
static int nissan_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
184+
int bus_fwd = -1;
185+
int addr = GET_ADDR(to_fwd);
186+
187+
if (bus_num == 0) {
188+
bus_fwd = 2; // ADAS
189+
}
190+
191+
if (bus_num == 2) {
192+
// 0x169 is LKAS
193+
int block_msg = (addr == 0x169);
194+
if (!block_msg) {
195+
bus_fwd = 0; // V-CAN
196+
}
197+
}
198+
199+
if (relay_malfunction) {
200+
bus_fwd = -1;
201+
}
202+
203+
// fallback to do not forward
204+
return bus_fwd;
205+
}
206+
207+
const safety_hooks nissan_hooks = {
208+
.init = nooutput_init,
209+
.rx = nissan_rx_hook,
210+
.tx = nissan_tx_hook,
211+
.tx_lin = nooutput_tx_lin_hook,
212+
.fwd = nissan_fwd_hook,
213+
.addr_check = nissan_rx_checks,
214+
.addr_check_len = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]),
215+
};

‎python/__init__.py

+1
Original file line numberDiff line numberDiff line change
@@ -123,6 +123,7 @@ class Panda(object):
123123
SAFETY_TESLA = 10
124124
SAFETY_SUBARU = 11
125125
SAFETY_MAZDA = 13
126+
SAFETY_NISSAN = 14
126127
SAFETY_VOLKSWAGEN_MQB = 15
127128
SAFETY_TOYOTA_IPAS = 16
128129
SAFETY_ALLOUTPUT = 17

‎tests/safety/libpandasafety_py.py

+4
Original file line numberDiff line numberDiff line change
@@ -100,6 +100,10 @@
100100
void set_volkswagen_rt_torque_last(int t);
101101
void set_volkswagen_torque_driver(int min, int max);
102102
103+
void init_tests_nissan(void);
104+
void set_nissan_desired_angle_last(int t);
105+
void set_nissan_brake_prev(int t);
106+
103107
""")
104108

105109
libpandasafety = ffi.dlopen(libpandasafety_fn)

‎tests/safety/test.c

+18
Original file line numberDiff line numberDiff line change
@@ -272,6 +272,14 @@ void set_honda_fwd_brake(bool c){
272272
honda_fwd_brake = c;
273273
}
274274

275+
void set_nissan_brake_prev(bool c){
276+
nissan_brake_prev = c;
277+
}
278+
279+
void set_nissan_desired_angle_last(int t){
280+
nissan_desired_angle_last = t;
281+
}
282+
275283
void init_tests(void){
276284
// get HW_TYPE from env variable set in test.sh
277285
hw_type = atoi(getenv("HW_TYPE"));
@@ -361,6 +369,16 @@ void init_tests_honda(void){
361369
honda_fwd_brake = false;
362370
}
363371

372+
void init_tests_nissan(void){
373+
init_tests();
374+
nissan_angle_meas.min = 0;
375+
nissan_angle_meas.max = 0;
376+
nissan_desired_angle_last = 0;
377+
nissan_gas_prev = 0;
378+
nissan_brake_prev = 0;
379+
set_timer(0);
380+
}
381+
364382
void set_gmlan_digital_output(int to_set){
365383
}
366384

0 commit comments

Comments
 (0)