Skip to content

Commit

Permalink
Pedal: use avg between tracks (#253)
Browse files Browse the repository at this point in the history
  • Loading branch information
rbiasini authored Jul 15, 2019
1 parent c597dcc commit e49d0db
Show file tree
Hide file tree
Showing 5 changed files with 33 additions and 24 deletions.
2 changes: 1 addition & 1 deletion board/safety/safety_honda.h
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// length check because bosch hardware also uses this id (0x201 w/ len = 8)
if ((addr == 0x201) && (len == 6)) {
gas_interceptor_detected = 1;
int gas_interceptor = (GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1);
int gas_interceptor = GET_INTERCEPTOR(to_push);
if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) &&
(gas_interceptor_prev <= HONDA_GAS_INTERCEPTOR_THRESHOLD) &&
long_controls_allowed) {
Expand Down
2 changes: 1 addition & 1 deletion board/safety/safety_toyota.h
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of interceptor gas press
if (addr == 0x201) {
gas_interceptor_detected = 1;
int gas_interceptor = (GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1);
int gas_interceptor = GET_INTERCEPTOR(to_push);
if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) &&
(gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRESHOLD) &&
long_controls_allowed) {
Expand Down
3 changes: 3 additions & 0 deletions board/safety_declarations.h
Original file line number Diff line number Diff line change
Expand Up @@ -50,3 +50,6 @@ int gas_interceptor_prev = 0;

// This is set by USB command 0xdf
bool long_controls_allowed = 1;

// avg between 2 tracks
#define GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + ((GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2 ) / 2)
25 changes: 14 additions & 11 deletions tests/safety/test_honda.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@

MAX_BRAKE = 255

INTERCEPTOR_THRESHOLD = 328

class TestHondaSafety(unittest.TestCase):
@classmethod
def setUp(cls):
Expand Down Expand Up @@ -66,7 +68,9 @@ def _send_interceptor_msg(self, gas, addr):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = addr << 21
to_send[0].RDTR = 6
to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8)
gas2 = gas * 2
to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \
((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8)

return to_send

Expand Down Expand Up @@ -176,16 +180,15 @@ def test_allow_engage_with_gas_pressed(self):

def test_disengage_on_gas_interceptor(self):
for long_controls_allowed in [0, 1]:
self.safety.set_long_controls_allowed(long_controls_allowed)
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_controls_allowed(1)
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
if long_controls_allowed:
self.assertFalse(self.safety.get_controls_allowed())
else:
self.assertTrue(self.safety.get_controls_allowed())
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_gas_interceptor_detected(False)
for g in range(0, 0x1000):
self.safety.set_long_controls_allowed(long_controls_allowed)
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_controls_allowed(True)
self.safety.safety_rx_hook(self._send_interceptor_msg(g, 0x201))
remain_enabled = (not long_controls_allowed or g <= INTERCEPTOR_THRESHOLD)
self.assertEqual(remain_enabled, self.safety.get_controls_allowed())
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_gas_interceptor_detected(False)
self.safety.set_long_controls_allowed(True)

def test_allow_engage_with_gas_interceptor_pressed(self):
Expand Down
25 changes: 14 additions & 11 deletions tests/safety/test_toyota.py
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@

MAX_TORQUE_ERROR = 350

INTERCEPTOR_THRESHOLD = 475

def twos_comp(val, bits):
if val >= 0:
return val
Expand Down Expand Up @@ -81,7 +83,9 @@ def _send_interceptor_msg(self, gas, addr):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = addr << 21
to_send[0].RDTR = 6
to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8)
gas2 = gas * 2
to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \
((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8)

return to_send

Expand Down Expand Up @@ -145,16 +149,15 @@ def test_allow_engage_with_gas_pressed(self):

def test_disengage_on_gas_interceptor(self):
for long_controls_allowed in [0, 1]:
self.safety.set_long_controls_allowed(long_controls_allowed)
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_controls_allowed(True)
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
if long_controls_allowed:
self.assertFalse(self.safety.get_controls_allowed())
else:
self.assertTrue(self.safety.get_controls_allowed())
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_gas_interceptor_detected(False)
for g in range(0, 0x1000):
self.safety.set_long_controls_allowed(long_controls_allowed)
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_controls_allowed(True)
self.safety.safety_rx_hook(self._send_interceptor_msg(g, 0x201))
remain_enabled = (not long_controls_allowed or g <= INTERCEPTOR_THRESHOLD)
self.assertEqual(remain_enabled, self.safety.get_controls_allowed())
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_gas_interceptor_detected(False)
self.safety.set_long_controls_allowed(True)

def test_allow_engage_with_gas_interceptor_pressed(self):
Expand Down

0 comments on commit e49d0db

Please sign in to comment.