Nissan leaf (#473)
* 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 testmaster
parent
0696730c14
commit
de89fcdc4f
|
@ -9,18 +9,16 @@ const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_DOWN = {
|
||||||
{2., 7., 17.},
|
{2., 7., 17.},
|
||||||
{5., 3.5, .5}};
|
{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 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[] = {
|
AddrCheckStruct nissan_rx_checks[] = {
|
||||||
{.addr = {0x2}, .bus = 0, .expected_timestep = 10000U},
|
{.addr = {0x2}, .bus = 0, .expected_timestep = 10000U}, // STEER_ANGLE_SENSOR (100Hz)
|
||||||
{.addr = {0x29a}, .bus = 0, .expected_timestep = 20000U},
|
{.addr = {0x285}, .bus = 0, .expected_timestep = 20000U}, // WHEEL_SPEEDS_REAR (50Hz)
|
||||||
{.addr = {0x1b6}, .bus = 1, .expected_timestep = 10000U},
|
{.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]);
|
const int NISSAN_RX_CHECK_LEN = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]);
|
||||||
|
|
||||||
|
@ -54,15 +52,22 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||||
update_sample(&nissan_angle_meas, angle_meas_new);
|
update_sample(&nissan_angle_meas, angle_meas_new);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (addr == 0x29a) {
|
if (addr == 0x285) {
|
||||||
// Get current speed
|
// Get current speed
|
||||||
// Factor 0.00555
|
// Factor 0.005
|
||||||
nissan_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.00555 / 3.6;
|
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
|
// exit controls on rising edge of gas press
|
||||||
if (addr == 0x15c) {
|
// X-Trail 0x15c, Leaf 0x239
|
||||||
bool gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3));
|
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) {
|
if (gas_pressed && !gas_pressed_prev) {
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
|
@ -75,26 +80,34 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (bus == 1) {
|
// exit controls on rising edge of brake press, or if speed > 0 and brake
|
||||||
if (addr == 0x1b6) {
|
// X-trail 0x454, Leaf 0x1cc
|
||||||
int cruise_engaged = (GET_BYTE(to_push, 4) >> 6) & 1;
|
if ((addr == 0x454) || (addr == 0x1cc)) {
|
||||||
if (cruise_engaged && !nissan_cruise_engaged_last) {
|
bool brake_pressed = true;
|
||||||
controls_allowed = 1;
|
if (addr == 0x454){
|
||||||
}
|
brake_pressed = (GET_BYTE(to_push, 2) & 0x80) != 0;
|
||||||
if (!cruise_engaged) {
|
} else {
|
||||||
controls_allowed = 0;
|
brake_pressed = GET_BYTE(to_push, 0) > 3;
|
||||||
}
|
|
||||||
nissan_cruise_engaged_last = cruise_engaged;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// exit controls on rising edge of brake press, or if speed > 0 and brake
|
if (brake_pressed && (!brake_pressed_prev || (nissan_speed > 0.))) {
|
||||||
if (addr == 0x454) {
|
controls_allowed = 0;
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
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;
|
return valid;
|
||||||
|
@ -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 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);
|
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;
|
// check for violation;
|
||||||
violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
|
violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
|
||||||
|
|
||||||
|
@ -183,7 +186,10 @@ static int nissan_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||||
int addr = GET_ADDR(to_fwd);
|
int addr = GET_ADDR(to_fwd);
|
||||||
|
|
||||||
if (bus_num == 0) {
|
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) {
|
if (bus_num == 2) {
|
||||||
|
|
|
@ -5,13 +5,11 @@ from panda import Panda
|
||||||
from panda.tests.safety import libpandasafety_py
|
from panda.tests.safety import libpandasafety_py
|
||||||
from panda.tests.safety.common import StdTest, make_msg
|
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_BP = [0., 5., 15.]
|
||||||
ANGLE_DELTA_V = [5., .8, .15] # windup limit
|
ANGLE_DELTA_V = [5., .8, .15] # windup limit
|
||||||
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind 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):
|
def twos_comp(val, bits):
|
||||||
if val >= 0:
|
if val >= 0:
|
||||||
|
@ -50,8 +48,8 @@ class TestNissanSafety(unittest.TestCase):
|
||||||
self.safety.safety_rx_hook(self._angle_meas_msg(angle))
|
self.safety.safety_rx_hook(self._angle_meas_msg(angle))
|
||||||
|
|
||||||
def _lkas_state_msg(self, state):
|
def _lkas_state_msg(self, state):
|
||||||
to_send = make_msg(0, 0x1b6)
|
to_send = make_msg(1, 0x30f)
|
||||||
to_send[0].RDHR = (state & 0x1) << 6
|
to_send[0].RDHR = (state & 0x1) << 3
|
||||||
|
|
||||||
return to_send
|
return to_send
|
||||||
|
|
||||||
|
@ -64,8 +62,8 @@ class TestNissanSafety(unittest.TestCase):
|
||||||
return to_send
|
return to_send
|
||||||
|
|
||||||
def _speed_msg(self, speed):
|
def _speed_msg(self, speed):
|
||||||
to_send = make_msg(0, 0x29a)
|
to_send = make_msg(0, 0x285)
|
||||||
speed = int(speed / 0.00555 * 3.6)
|
speed = int(speed / 0.005 * 3.6)
|
||||||
to_send[0].RDLR = ((speed & 0xFF) << 24) | ((speed & 0xFF00) << 8)
|
to_send[0].RDLR = ((speed & 0xFF) << 24) | ((speed & 0xFF00) << 8)
|
||||||
|
|
||||||
return to_send
|
return to_send
|
||||||
|
@ -92,46 +90,48 @@ class TestNissanSafety(unittest.TestCase):
|
||||||
StdTest.test_spam_can_buses(self, TX_MSGS)
|
StdTest.test_spam_can_buses(self, TX_MSGS)
|
||||||
|
|
||||||
def test_angle_cmd_when_enabled(self):
|
def test_angle_cmd_when_enabled(self):
|
||||||
|
|
||||||
# when controls are allowed, angle cmd rate limit is enforced
|
# 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., 50.]
|
||||||
speeds = [0., 1., 5., 10., 15., 100.]
|
|
||||||
angles = [-300, -100, -10, 0, 10, 100, 300]
|
angles = [-300, -100, -10, 0, 10, 100, 300]
|
||||||
for a in angles:
|
for a in angles:
|
||||||
for s in speeds:
|
for s in speeds:
|
||||||
max_delta_up = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V)
|
max_delta_up = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V)
|
||||||
max_delta_down = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
|
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
|
# first test against false positives
|
||||||
self._angle_meas_msg_array(a)
|
self._angle_meas_msg_array(a)
|
||||||
self.safety.safety_rx_hook(self._speed_msg(s))
|
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.safety.set_controls_allowed(1)
|
||||||
|
|
||||||
self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(
|
# Stay within limits
|
||||||
np.clip(a + sign(a) * max_delta_up, -angle_lim, angle_lim), 1)))
|
# Up
|
||||||
self.assertTrue(self.safety.get_controls_allowed())
|
self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a + sign(a) * max_delta_up, 1)))
|
||||||
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())
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
# now inject too high rates
|
# Don't change
|
||||||
self.assertEqual(False, self.safety.safety_tx_hook(self._lkas_control_msg(a + sign(a) *
|
self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a, 1)))
|
||||||
(max_delta_up + 1), 1)))
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
# 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())
|
||||||
|
|
||||||
|
# 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())
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
# Don't change
|
||||||
self.safety.set_controls_allowed(1)
|
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.assertTrue(self.safety.get_controls_allowed())
|
||||||
self.assertEqual(True, self.safety.safety_tx_hook(
|
self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a, 1)))
|
||||||
self._lkas_control_msg(np.clip(a, -angle_lim, angle_lim), 1)))
|
|
||||||
self.assertTrue(self.safety.get_controls_allowed())
|
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())
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
# Check desired steer should be the same as steer angle when controls are off
|
# Check desired steer should be the same as steer angle when controls are off
|
||||||
|
@ -181,7 +181,7 @@ class TestNissanSafety(unittest.TestCase):
|
||||||
buss = list(range(0x0, 0x3))
|
buss = list(range(0x0, 0x3))
|
||||||
msgs = list(range(0x1, 0x800))
|
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 b in buss:
|
||||||
for m in msgs:
|
for m in msgs:
|
||||||
if b == 0:
|
if b == 0:
|
||||||
|
@ -189,7 +189,10 @@ class TestNissanSafety(unittest.TestCase):
|
||||||
elif b == 1:
|
elif b == 1:
|
||||||
fwd_bus = -1
|
fwd_bus = -1
|
||||||
elif b == 2:
|
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
|
# assume len 8
|
||||||
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
|
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
|
||||||
|
|
Loading…
Reference in New Issue