Skip to content

Commit bc90b60

Browse files
toyota: use universal gas pressed bit (#488)
* toyota: use universal gas pressed bit * fix tests
1 parent 74d10cc commit bc90b60

File tree

3 files changed

+26
-25
lines changed

3 files changed

+26
-25
lines changed

board/drivers/llcan.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111
#define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF)
1212
#define GET_LEN(msg) ((msg)->RDTR & 0xF)
1313
#define GET_ADDR(msg) ((((msg)->RIR & 4) != 0) ? ((msg)->RIR >> 3) : ((msg)->RIR >> 21))
14-
#define GET_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0XFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU))
14+
#define GET_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0xFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU))
1515
#define GET_BYTES_04(msg) ((msg)->RDLR)
1616
#define GET_BYTES_48(msg) ((msg)->RDHR)
1717

board/safety/safety_toyota.h

+8-9
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
8888
}
8989

9090
// enter controls on rising edge of ACC, exit controls on ACC off
91+
// exit controls on rising edge of gas press
9192
if (addr == 0x1D2) {
9293
// 5th bit is CRUISE_ACTIVE
9394
int cruise_engaged = GET_BYTE(to_push, 0) & 0x20;
@@ -98,6 +99,13 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
9899
controls_allowed = 1;
99100
}
100101
toyota_cruise_engaged_last = cruise_engaged;
102+
103+
// handle gas_pressed
104+
bool gas_pressed = ((GET_BYTE(to_push, 0) >> 4) & 1) == 0;
105+
if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) {
106+
controls_allowed = 0;
107+
}
108+
gas_pressed_prev = gas_pressed;
101109
}
102110

103111
// sample speed
@@ -133,15 +141,6 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
133141
gas_interceptor_prev = gas_interceptor;
134142
}
135143

136-
// exit controls on rising edge of gas press
137-
if (addr == 0x2C1) {
138-
bool gas_pressed = GET_BYTE(to_push, 6) != 0;
139-
if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) {
140-
controls_allowed = 0;
141-
}
142-
gas_pressed_prev = gas_pressed;
143-
}
144-
145144
// 0x2E4 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
146145
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4)) {
147146
relay_malfunction = true;

tests/safety/test_toyota.py

+17-15
Original file line numberDiff line numberDiff line change
@@ -97,9 +97,10 @@ def _brake_msg(self, brake):
9797
to_send[0].RDHR |= toyota_checksum(to_send[0], 0x226, 8) << 24
9898
return to_send
9999

100-
def _send_gas_msg(self, gas):
101-
to_send = make_msg(0, 0x2C1)
102-
to_send[0].RDHR = (gas & 0xFF) << 16
100+
def _gas_pressed_msg(self, pressed, enable_cruise=False):
101+
to_send = make_msg(0, 0x1D2)
102+
to_send[0].RDLR = ((1*(not pressed)) << 4) | (1*enable_cruise << 5)
103+
to_send[0].RDHR = (toyota_checksum(to_send[0], 0x1D2, 8) << 24)
103104
return to_send
104105

105106
def _send_interceptor_msg(self, gas, addr):
@@ -139,9 +140,9 @@ def test_disable_control_allowed_from_cruise(self):
139140
self.assertFalse(self.safety.get_controls_allowed())
140141

141142
def test_prev_gas(self):
142-
for g in range(0, 256):
143-
self.safety.safety_rx_hook(self._send_gas_msg(g))
144-
self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev())
143+
for pressed in [True, False]:
144+
self.safety.safety_rx_hook(self._gas_pressed_msg(pressed))
145+
self.assertEqual(pressed, self.safety.get_gas_pressed_prev())
145146

146147
def test_prev_gas_interceptor(self):
147148
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
@@ -152,26 +153,27 @@ def test_prev_gas_interceptor(self):
152153
self.safety.set_gas_interceptor_detected(False)
153154

154155
def test_disengage_on_gas(self):
155-
self.safety.safety_rx_hook(self._send_gas_msg(0))
156+
self.safety.safety_rx_hook(self._gas_pressed_msg(False))
156157
self.safety.set_controls_allowed(True)
157-
self.safety.safety_rx_hook(self._send_gas_msg(1))
158+
self.safety.safety_rx_hook(self._gas_pressed_msg(True, enable_cruise=True))
158159
self.assertFalse(self.safety.get_controls_allowed())
159160

160161
def test_unsafe_mode_no_disengage_on_gas(self):
161-
self.safety.safety_rx_hook(self._send_gas_msg(0))
162+
self.safety.safety_rx_hook(self._gas_pressed_msg(False))
162163
self.safety.set_controls_allowed(True)
163164
self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS)
164-
self.safety.safety_rx_hook(self._send_gas_msg(1))
165+
self.safety.safety_rx_hook(self._gas_pressed_msg(True, enable_cruise=True))
165166
self.assertTrue(self.safety.get_controls_allowed())
166167
self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT)
167168

168169
def test_allow_engage_with_gas_pressed(self):
169-
self.safety.safety_rx_hook(self._send_gas_msg(1))
170+
self.safety.safety_rx_hook(self._gas_pressed_msg(True))
170171
self.safety.set_controls_allowed(True)
171-
self.safety.safety_rx_hook(self._send_gas_msg(1))
172-
self.assertTrue(self.safety.get_controls_allowed())
173-
self.safety.safety_rx_hook(self._send_gas_msg(1))
174-
self.assertTrue(self.safety.get_controls_allowed())
172+
for _ in range(2):
173+
# since cruise msg is used for gas pedal state, cruise bit must
174+
# also be set for this test or else it will set controls_allowed
175+
self.safety.safety_rx_hook(self._gas_pressed_msg(True, enable_cruise=True))
176+
self.assertTrue(self.safety.get_controls_allowed())
175177

176178
def test_disengage_on_gas_interceptor(self):
177179
for g in range(0, 0x1000):

0 commit comments

Comments
 (0)