Skip to content

Commit

Permalink
Nissan leaf (#473)
Browse files Browse the repository at this point in the history 
* use rear wheels for speed

* Add leaf gas message

* Add brake press check to safety

* Fix typo in comparison

* threshold on gas and brake pressed and add cruise enabled check

* Fix brake pressed result

* Use same message for cruise enabled detection

* Fix typo

* Fix tests

* Add RX checks

* Allow sending cancel from panda

* Add cancel command to TX messages in tests

* Remove angle limit

* Change speed factor

* Change speed factor in tests

* Remove max angle from tests

* Fix tests

* try cancel using seatbelt

* Try different cancel message

* Fix TX_MSGS in test

* Fix fwd test
  • Loading branch information
@pd0wm
pd0wm authored Mar 28, 2020
1 parent 0696730 commit de89fcd
Show file tree
Hide file tree
Showing 2 changed files with 78 additions and 69 deletions.
88 changes: 47 additions & 41 deletions board/safety/safety_nissan.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,18 +9,16 @@ 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}, {0x2b1, 0}, {0x4cc, 0}, {0x20b, 2}};
const AddrBus NISSAN_TX_MSGS[] = {{0x169, 0}, {0x2b1, 0}, {0x4cc, 0}, {0x20b, 2}, {0x280, 2}};

AddrCheckStruct nissan_rx_checks[] = {
{.addr = {0x2}, .bus = 0, .expected_timestep = 10000U},
{.addr = {0x29a}, .bus = 0, .expected_timestep = 20000U},
{.addr = {0x1b6}, .bus = 1, .expected_timestep = 10000U},
{.addr = {0x2}, .bus = 0, .expected_timestep = 10000U}, // STEER_ANGLE_SENSOR (100Hz)
{.addr = {0x285}, .bus = 0, .expected_timestep = 20000U}, // WHEEL_SPEEDS_REAR (50Hz)
{.addr = {0x30f}, .bus = 2, .expected_timestep = 100000U}, // CRUISE_STATE (10Hz)
{.addr = {0x15c, 0x239}, .bus = 0, .expected_timestep = 20000U}, // GAS_PEDAL (100Hz / 50Hz)
{.addr = {0x454, 0x1cc}, .bus = 0, .expected_timestep = 100000U}, // DOORS_LIGHTS (10Hz) / BRAKE (100Hz)
};
const int NISSAN_RX_CHECK_LEN = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]);

Expand Down Expand Up @@ -54,15 +52,22 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
update_sample(&nissan_angle_meas, angle_meas_new);
}

if (addr == 0x29a) {
if (addr == 0x285) {
// Get current speed
// Factor 0.00555
nissan_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.00555 / 3.6;
// Factor 0.005
nissan_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.005 / 3.6;
}

// exit controls on rising edge of gas press
if (addr == 0x15c) {
bool gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3));
// X-Trail 0x15c, Leaf 0x239
if ((addr == 0x15c) || (addr == 0x239)) {
bool gas_pressed = true;
if (addr == 0x15c){
gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3)) > 1;
} else {
gas_pressed = GET_BYTE(to_push, 0) > 3;
}

if (gas_pressed && !gas_pressed_prev) {
controls_allowed = 0;
}
Expand All @@ -75,26 +80,34 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
}

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
// X-trail 0x454, Leaf 0x1cc
if ((addr == 0x454) || (addr == 0x1cc)) {
bool brake_pressed = true;
if (addr == 0x454){
brake_pressed = (GET_BYTE(to_push, 2) & 0x80) != 0;
} else {
brake_pressed = GET_BYTE(to_push, 0) > 3;
}

// exit controls on rising edge of brake press, or if speed > 0 and brake
if (addr == 0x454) {
bool brake_pressed = (GET_BYTE(to_push, 2) & 0x80) != 0;
if (brake_pressed && (!brake_pressed_prev || (nissan_speed > 0.))) {
controls_allowed = 0;
}
brake_pressed_prev = brake_pressed;
if (brake_pressed && (!brake_pressed_prev || (nissan_speed > 0.))) {
controls_allowed = 0;
}
brake_pressed_prev = brake_pressed;
}


// Handle cruise enabled
if ((bus == 2) && (addr == 0x30f)) {
bool cruise_engaged = (GET_BYTE(to_push, 0) >> 3) & 1;

if (cruise_engaged && !nissan_cruise_engaged_last) {
controls_allowed = 1;
}
if (!cruise_engaged) {
controls_allowed = 0;
}
nissan_cruise_engaged_last = cruise_engaged;
}
}
return valid;
Expand Down Expand Up @@ -133,16 +146,6 @@ static int nissan_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
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);

Expand Down Expand Up @@ -183,7 +186,10 @@ static int nissan_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int addr = GET_ADDR(to_fwd);

if (bus_num == 0) {
bus_fwd = 2; // ADAS
int block_msg = (addr == 0x280); // CANCEL_MSG
if (!block_msg) {
bus_fwd = 2; // ADAS
}
}

if (bus_num == 2) {
Expand Down
59 changes: 31 additions & 28 deletions tests/safety/test_nissan.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,13 +5,11 @@
from panda.tests.safety import libpandasafety_py
from panda.tests.safety.common import StdTest, make_msg

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

TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2]]
TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2], [0x280, 2]]

def twos_comp(val, bits):
if val >= 0:
Expand Down Expand Up @@ -50,8 +48,8 @@ def _angle_meas_msg_array(self, angle):
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
to_send = make_msg(1, 0x30f)
to_send[0].RDHR = (state & 0x1) << 3

return to_send

Expand All @@ -64,8 +62,8 @@ def _lkas_control_msg(self, angle, state):
return to_send

def _speed_msg(self, speed):
to_send = make_msg(0, 0x29a)
speed = int(speed / 0.00555 * 3.6)
to_send = make_msg(0, 0x285)
speed = int(speed / 0.005 * 3.6)
to_send[0].RDLR = ((speed & 0xFF) << 24) | ((speed & 0xFF00) << 8)

return to_send
Expand All @@ -92,46 +90,48 @@ def test_spam_can_buses(self):
StdTest.test_spam_can_buses(self, TX_MSGS)

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.]
speeds = [0., 1., 5., 10., 15., 50.]
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._set_prev_angle(a)
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)))
# Stay within limits
# Up
self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a + sign(a) * max_delta_up, 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)))

# Don't change
self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a, 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)))

# Down
self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a - sign(a) * max_delta_down, 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)))
# Inject too high rates
# Up
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())

# Don't change
self.safety.set_controls_allowed(1)
self._set_prev_angle(np.clip(a, -angle_lim, angle_lim))
self._set_prev_angle(a)
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.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a, 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)))

# Down
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
Expand Down Expand Up @@ -181,15 +181,18 @@ def test_fwd_hook(self):
buss = list(range(0x0, 0x3))
msgs = list(range(0x1, 0x800))

blocked_msgs = [0x169,0x2b1,0x4cc]
blocked_msgs = [(2, 0x169), (2, 0x2b1), (2, 0x4cc), (0, 0x280)]
for b in buss:
for m in msgs:
if b == 0:
fwd_bus = 2
elif b == 1:
fwd_bus = -1
elif b == 2:
fwd_bus = -1 if m in blocked_msgs else 0
fwd_bus = 0

if (b, m) in blocked_msgs:
fwd_bus = -1

# assume len 8
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
Expand Down

0 comments on commit de89fcd

Please sign in to comment.