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
Willem Melching
GitHub
parent
0696730c14
commit
de89fcdc4f
|
|
@ -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]);
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
|
@ -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);
|
||||
|
||||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -5,13 +5,11 @@ from panda import Panda
|
|||
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:
|
||||
|
|
@ -50,8 +48,8 @@ class TestNissanSafety(unittest.TestCase):
|
|||
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
|
||||
|
||||
|
|
@ -64,8 +62,8 @@ class TestNissanSafety(unittest.TestCase):
|
|||
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
|
||||
|
|
@ -92,46 +90,48 @@ class TestNissanSafety(unittest.TestCase):
|
|||
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)))
|
||||
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)))
|
||||
# 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())
|
||||
|
||||
# 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)))
|
||||
# Don't change
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a, 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())
|
||||
|
||||
# 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
|
||||
|
|
@ -181,7 +181,7 @@ class TestNissanSafety(unittest.TestCase):
|
|||
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:
|
||||
|
|
@ -189,7 +189,10 @@ class TestNissanSafety(unittest.TestCase):
|
|||
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)))
|
||||
|
|
|
|||
Loading…
Reference in New Issue