retropilot
/
panda
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

toyota: use universal gas pressed bit (#488) 

* toyota: use universal gas pressed bit

* fix tests
master
Adeeb 2020-04-09 12:09:34 -07:00 committed by GitHub
parent 74d10ccd38
commit bc90b60f97
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 26 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
board/drivers/llcan.h
View File

@ -11,7 +11,7 @@
#define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF)
#define GET_LEN(msg) ((msg)->RDTR & 0xF)
#define GET_ADDR(msg) ((((msg)->RIR & 4) != 0) ? ((msg)->RIR >> 3) : ((msg)->RIR >> 21))
#define GET_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0XFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU))
#define GET_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0xFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU))
#define GET_BYTES_04(msg) ((msg)->RDLR)
#define GET_BYTES_48(msg) ((msg)->RDHR)

17
board/safety/safety_toyota.h
View File

@ -88,6 +88,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// enter controls on rising edge of ACC, exit controls on ACC off
// exit controls on rising edge of gas press
if (addr == 0x1D2) {
// 5th bit is CRUISE_ACTIVE
int cruise_engaged = GET_BYTE(to_push, 0) & 0x20;
@ -98,6 +99,13 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
controls_allowed = 1;
}
toyota_cruise_engaged_last = cruise_engaged;
// handle gas_pressed
bool gas_pressed = ((GET_BYTE(to_push, 0) >> 4) & 1) == 0;
if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) {
controls_allowed = 0;
}
gas_pressed_prev = gas_pressed;
}
// sample speed
@ -133,15 +141,6 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
gas_interceptor_prev = gas_interceptor;
}
// exit controls on rising edge of gas press
if (addr == 0x2C1) {
bool gas_pressed = GET_BYTE(to_push, 6) != 0;
if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) {
controls_allowed = 0;
}
gas_pressed_prev = gas_pressed;
}
// 0x2E4 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4)) {
relay_malfunction = true;

32
tests/safety/test_toyota.py
View File

@ -97,9 +97,10 @@ class TestToyotaSafety(unittest.TestCase):
to_send[0].RDHR |= toyota_checksum(to_send[0], 0x226, 8) << 24
return to_send
def _send_gas_msg(self, gas):
to_send = make_msg(0, 0x2C1)
to_send[0].RDHR = (gas & 0xFF) << 16
def _gas_pressed_msg(self, pressed, enable_cruise=False):
to_send = make_msg(0, 0x1D2)
to_send[0].RDLR = ((1*(not pressed)) << 4) | (1*enable_cruise << 5)
to_send[0].RDHR = (toyota_checksum(to_send[0], 0x1D2, 8) << 24)
return to_send
def _send_interceptor_msg(self, gas, addr):
@ -139,9 +140,9 @@ class TestToyotaSafety(unittest.TestCase):
self.assertFalse(self.safety.get_controls_allowed())
def test_prev_gas(self):
for g in range(0, 256):
self.safety.safety_rx_hook(self._send_gas_msg(g))
self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev())
for pressed in [True, False]:
self.safety.safety_rx_hook(self._gas_pressed_msg(pressed))
self.assertEqual(pressed, self.safety.get_gas_pressed_prev())
def test_prev_gas_interceptor(self):
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
@ -152,26 +153,27 @@ class TestToyotaSafety(unittest.TestCase):
self.safety.set_gas_interceptor_detected(False)
def test_disengage_on_gas(self):
self.safety.safety_rx_hook(self._send_gas_msg(0))
self.safety.safety_rx_hook(self._gas_pressed_msg(False))
self.safety.set_controls_allowed(True)
self.safety.safety_rx_hook(self._send_gas_msg(1))
self.safety.safety_rx_hook(self._gas_pressed_msg(True, enable_cruise=True))
self.assertFalse(self.safety.get_controls_allowed())
def test_unsafe_mode_no_disengage_on_gas(self):
self.safety.safety_rx_hook(self._send_gas_msg(0))
self.safety.safety_rx_hook(self._gas_pressed_msg(False))
self.safety.set_controls_allowed(True)
self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS)
self.safety.safety_rx_hook(self._send_gas_msg(1))
self.safety.safety_rx_hook(self._gas_pressed_msg(True, enable_cruise=True))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT)
def test_allow_engage_with_gas_pressed(self):
self.safety.safety_rx_hook(self._send_gas_msg(1))
self.safety.safety_rx_hook(self._gas_pressed_msg(True))
self.safety.set_controls_allowed(True)
self.safety.safety_rx_hook(self._send_gas_msg(1))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.safety_rx_hook(self._send_gas_msg(1))
self.assertTrue(self.safety.get_controls_allowed())
for _ in range(2):
# since cruise msg is used for gas pedal state, cruise bit must
# also be set for this test or else it will set controls_allowed
self.safety.safety_rx_hook(self._gas_pressed_msg(True, enable_cruise=True))
self.assertTrue(self.safety.get_controls_allowed())
def test_disengage_on_gas_interceptor(self):
for g in range(0, 0x1000):