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_BUS(msg) (((msg)->RDTR >> 4) & 0xFF)
#define GET_LEN(msg) ((msg)->RDTR & 0xF) #define GET_LEN(msg) ((msg)->RDTR & 0xF)
#define GET_ADDR(msg) ((((msg)->RIR & 4) != 0) ? ((msg)->RIR >> 3) : ((msg)->RIR >> 21)) #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_04(msg) ((msg)->RDLR)
#define GET_BYTES_48(msg) ((msg)->RDHR) #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 // enter controls on rising edge of ACC, exit controls on ACC off
// exit controls on rising edge of gas press
if (addr == 0x1D2) { if (addr == 0x1D2) {
// 5th bit is CRUISE_ACTIVE // 5th bit is CRUISE_ACTIVE
int cruise_engaged = GET_BYTE(to_push, 0) & 0x20; 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; controls_allowed = 1;
} }
toyota_cruise_engaged_last = cruise_engaged; 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 // sample speed
@ -133,15 +141,6 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
gas_interceptor_prev = gas_interceptor; 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 // 0x2E4 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4)) { if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4)) {
relay_malfunction = true; 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 to_send[0].RDHR |= toyota_checksum(to_send[0], 0x226, 8) << 24
return to_send return to_send
def _send_gas_msg(self, gas): def _gas_pressed_msg(self, pressed, enable_cruise=False):
to_send = make_msg(0, 0x2C1) to_send = make_msg(0, 0x1D2)
to_send[0].RDHR = (gas & 0xFF) << 16 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 return to_send
def _send_interceptor_msg(self, gas, addr): def _send_interceptor_msg(self, gas, addr):
@ -139,9 +140,9 @@ class TestToyotaSafety(unittest.TestCase):
self.assertFalse(self.safety.get_controls_allowed()) self.assertFalse(self.safety.get_controls_allowed())
def test_prev_gas(self): def test_prev_gas(self):
for g in range(0, 256): for pressed in [True, False]:
self.safety.safety_rx_hook(self._send_gas_msg(g)) self.safety.safety_rx_hook(self._gas_pressed_msg(pressed))
self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev()) self.assertEqual(pressed, self.safety.get_gas_pressed_prev())
def test_prev_gas_interceptor(self): def test_prev_gas_interceptor(self):
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201)) 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) self.safety.set_gas_interceptor_detected(False)
def test_disengage_on_gas(self): 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.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()) self.assertFalse(self.safety.get_controls_allowed())
def test_unsafe_mode_no_disengage_on_gas(self): 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_controls_allowed(True)
self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) 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.assertTrue(self.safety.get_controls_allowed())
self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT)
def test_allow_engage_with_gas_pressed(self): 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.set_controls_allowed(True)
self.safety.safety_rx_hook(self._send_gas_msg(1)) for _ in range(2):
self.assertTrue(self.safety.get_controls_allowed()) # since cruise msg is used for gas pedal state, cruise bit must
self.safety.safety_rx_hook(self._send_gas_msg(1)) # also be set for this test or else it will set controls_allowed
self.assertTrue(self.safety.get_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): def test_disengage_on_gas_interceptor(self):
for g in range(0, 0x1000): for g in range(0, 0x1000):