Pedal gas pressed safety limits (#507)
* Fixed toyota pedal gas disengage and increased pedal limits to OP limits +5% * Fix safety unit tests? * Fix imports * Fix imports #2master
parent
715b1a1695
commit
0336f625dc
|
@ -9,7 +9,15 @@
|
|||
const AddrBus HONDA_N_TX_MSGS[] = {{0xE4, 0}, {0x194, 0}, {0x1FA, 0}, {0x200, 0}, {0x30C, 0}, {0x33D, 0}};
|
||||
const AddrBus HONDA_BG_TX_MSGS[] = {{0xE4, 2}, {0x296, 0}, {0x33D, 2}}; // Bosch Giraffe
|
||||
const AddrBus HONDA_BH_TX_MSGS[] = {{0xE4, 0}, {0x296, 1}, {0x33D, 0}}; // Bosch Harness
|
||||
const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 328; // ratio between offset and gain from dbc file
|
||||
|
||||
// Roughly calculated using the offsets in openpilot +5%:
|
||||
// In openpilot: ((gas1_norm + gas2_norm)/2) > 15
|
||||
// gas_norm1 = ((gain_dbc1*gas1) + offset_dbc)
|
||||
// gas_norm2 = ((gain_dbc2*gas2) + offset_dbc)
|
||||
// assuming that 2*(gain_dbc1*gas1) == (gain_dbc2*gas2)
|
||||
// In this safety: ((gas1 + (gas2/2))/2) > THRESHOLD
|
||||
const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 344;
|
||||
#define HONDA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + ((GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2 ) / 2) // avg between 2 tracks
|
||||
|
||||
// Nidec and Bosch giraffe have pt on bus 0
|
||||
AddrCheckStruct honda_rx_checks[] = {
|
||||
|
@ -122,7 +130,7 @@ static int 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_INTERCEPTOR(to_push);
|
||||
int gas_interceptor = HONDA_GET_INTERCEPTOR(to_push);
|
||||
if (!unsafe_allow_gas && (gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) &&
|
||||
(gas_interceptor_prev <= HONDA_GAS_INTERCEPTOR_THRESHOLD)) {
|
||||
controls_allowed = 0;
|
||||
|
|
|
@ -20,7 +20,14 @@ const int TOYOTA_ISO_MAX_ACCEL = 2000; // 2.0 m/s2
|
|||
const int TOYOTA_ISO_MIN_ACCEL = -3500; // -3.5 m/s2
|
||||
|
||||
const int TOYOTA_STANDSTILL_THRSLD = 100; // 1kph
|
||||
const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 475; // ratio between offset and gain from dbc file
|
||||
|
||||
// Roughly calculated using the offsets in openpilot +5%:
|
||||
// In openpilot: ((gas1_norm + gas2_norm)/2) > 15
|
||||
// gas_norm1 = ((gain_dbc*gas1) + offset1_dbc)
|
||||
// gas_norm2 = ((gain_dbc*gas2) + offset2_dbc)
|
||||
// In this safety: ((gas1 + gas2)/2) > THRESHOLD
|
||||
const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 845;
|
||||
#define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2) // avg between 2 tracks
|
||||
|
||||
const AddrBus TOYOTA_TX_MSGS[] = {{0x283, 0}, {0x2E6, 0}, {0x2E7, 0}, {0x33E, 0}, {0x344, 0}, {0x365, 0}, {0x366, 0}, {0x4CB, 0}, // DSU bus 0
|
||||
{0x128, 1}, {0x141, 1}, {0x160, 1}, {0x161, 1}, {0x470, 1}, // DSU bus 1
|
||||
|
@ -133,7 +140,7 @@ static int 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_INTERCEPTOR(to_push);
|
||||
int gas_interceptor = TOYOTA_GET_INTERCEPTOR(to_push);
|
||||
if (!unsafe_allow_gas && (gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD) &&
|
||||
(gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRSLD)) {
|
||||
controls_allowed = 0;
|
||||
|
|
|
@ -112,6 +112,3 @@ int unsafe_mode = 0;
|
|||
uint32_t safety_mode_cnt = 0U;
|
||||
// allow 1s of transition timeout after relay changes state before assessing malfunctioning
|
||||
const uint32_t RELAY_TRNS_TIMEOUT = 1U;
|
||||
|
||||
// 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)
|
||||
|
|
|
@ -36,13 +36,6 @@ def package_can_msg(msg):
|
|||
def make_msg(bus, addr, length=8):
|
||||
return package_can_msg([addr, 0, b'\x00'*length, bus])
|
||||
|
||||
def interceptor_msg(gas, addr):
|
||||
to_send = make_msg(0, addr, 6)
|
||||
gas2 = gas * 2
|
||||
to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \
|
||||
((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8)
|
||||
return to_send
|
||||
|
||||
class CANPackerPanda(CANPacker):
|
||||
def make_can_msg_panda(self, name_or_addr, bus, values, counter=-1):
|
||||
msg = self.make_can_msg(name_or_addr, bus, values, counter=-1)
|
||||
|
|
|
@ -3,12 +3,11 @@ import unittest
|
|||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import StdTest, make_msg, interceptor_msg, \
|
||||
MAX_WRONG_COUNTERS, UNSAFE_MODE
|
||||
from panda.tests.safety.common import StdTest, make_msg, MAX_WRONG_COUNTERS, UNSAFE_MODE
|
||||
|
||||
MAX_BRAKE = 255
|
||||
|
||||
INTERCEPTOR_THRESHOLD = 328
|
||||
INTERCEPTOR_THRESHOLD = 344
|
||||
N_TX_MSGS = [[0xE4, 0], [0x194, 0], [0x1FA, 0], [0x200, 0], [0x30C, 0], [0x33D, 0]]
|
||||
BH_TX_MSGS = [[0xE4, 0], [0x296, 1], [0x33D, 0]] # Bosch Harness
|
||||
BG_TX_MSGS = [[0xE4, 2], [0x296, 0], [0x33D, 2]] # Bosch Giraffe
|
||||
|
@ -18,6 +17,14 @@ HONDA_N_HW = 0
|
|||
HONDA_BG_HW = 1
|
||||
HONDA_BH_HW = 2
|
||||
|
||||
# Honda gas gains are the different
|
||||
def honda_interceptor_msg(gas, addr):
|
||||
to_send = make_msg(0, addr, 6)
|
||||
gas2 = gas * 2
|
||||
to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \
|
||||
((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8)
|
||||
return to_send
|
||||
|
||||
def honda_checksum(msg, addr, len_msg):
|
||||
checksum = 0
|
||||
while addr > 0:
|
||||
|
@ -170,11 +177,11 @@ class TestHondaSafety(unittest.TestCase):
|
|||
self.assertTrue(self.safety.get_gas_pressed_prev())
|
||||
|
||||
def test_prev_gas_interceptor(self):
|
||||
self.safety.safety_rx_hook(interceptor_msg(0x0, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(0x0, 0x201))
|
||||
self.assertFalse(self.safety.get_gas_interceptor_prev())
|
||||
self.safety.safety_rx_hook(interceptor_msg(0x1000, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(0x1000, 0x201))
|
||||
self.assertTrue(self.safety.get_gas_interceptor_prev())
|
||||
self.safety.safety_rx_hook(interceptor_msg(0x0, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(0x0, 0x201))
|
||||
self.safety.set_gas_interceptor_detected(False)
|
||||
|
||||
def test_disengage_on_gas(self):
|
||||
|
@ -199,31 +206,31 @@ class TestHondaSafety(unittest.TestCase):
|
|||
|
||||
def test_disengage_on_gas_interceptor(self):
|
||||
for g in range(0, 0x1000):
|
||||
self.safety.safety_rx_hook(interceptor_msg(0, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(0, 0x201))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.safety_rx_hook(interceptor_msg(g, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(g, 0x201))
|
||||
remain_enabled = g <= INTERCEPTOR_THRESHOLD
|
||||
self.assertEqual(remain_enabled, self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(interceptor_msg(0, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(0, 0x201))
|
||||
self.safety.set_gas_interceptor_detected(False)
|
||||
|
||||
def test_unsafe_mode_no_disengage_on_gas_interceptor(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS)
|
||||
for g in range(0, 0x1000):
|
||||
self.safety.safety_rx_hook(interceptor_msg(g, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(g, 0x201))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(interceptor_msg(0, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(0, 0x201))
|
||||
self.safety.set_gas_interceptor_detected(False)
|
||||
self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT)
|
||||
self.safety.set_controls_allowed(False)
|
||||
|
||||
def test_allow_engage_with_gas_interceptor_pressed(self):
|
||||
self.safety.safety_rx_hook(interceptor_msg(0x1000, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(0x1000, 0x201))
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(interceptor_msg(0x1000, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(0x1000, 0x201))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(interceptor_msg(0, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(0, 0x201))
|
||||
self.safety.set_gas_interceptor_detected(False)
|
||||
|
||||
def test_brake_safety_check(self):
|
||||
|
@ -252,7 +259,7 @@ class TestHondaSafety(unittest.TestCase):
|
|||
send = True
|
||||
else:
|
||||
send = gas == 0
|
||||
self.assertEqual(send, self.safety.safety_tx_hook(interceptor_msg(gas, 0x200)))
|
||||
self.assertEqual(send, self.safety.safety_tx_hook(honda_interceptor_msg(gas, 0x200)))
|
||||
|
||||
def test_steer_safety_check(self):
|
||||
self.safety.set_controls_allowed(0)
|
||||
|
@ -359,22 +366,22 @@ class TestHondaSafety(unittest.TestCase):
|
|||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
elif pedal == 'interceptor':
|
||||
# gas_interceptor_prev > INTERCEPTOR_THRESHOLD
|
||||
self.safety.safety_rx_hook(interceptor_msg(INTERCEPTOR_THRESHOLD+1, 0x201))
|
||||
self.safety.safety_rx_hook(interceptor_msg(INTERCEPTOR_THRESHOLD+1, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(INTERCEPTOR_THRESHOLD+1, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(INTERCEPTOR_THRESHOLD+1, 0x201))
|
||||
|
||||
self.safety.set_controls_allowed(1)
|
||||
hw = self.safety.get_honda_hw()
|
||||
if hw == HONDA_N_HW:
|
||||
self.safety.set_honda_fwd_brake(False)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._send_brake_msg(MAX_BRAKE)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(interceptor_msg(INTERCEPTOR_THRESHOLD, 0x200)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(honda_interceptor_msg(INTERCEPTOR_THRESHOLD, 0x200)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._send_steer_msg(0x1000)))
|
||||
|
||||
# reset status
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.safety_tx_hook(self._send_brake_msg(0))
|
||||
self.safety.safety_tx_hook(self._send_steer_msg(0))
|
||||
self.safety.safety_tx_hook(interceptor_msg(0, 0x200))
|
||||
self.safety.safety_tx_hook(honda_interceptor_msg(0, 0x200))
|
||||
if pedal == 'brake':
|
||||
self.safety.safety_rx_hook(self._speed_msg(0))
|
||||
self.safety.safety_rx_hook(self._brake_msg(0))
|
||||
|
@ -397,8 +404,8 @@ class TestHondaSafety(unittest.TestCase):
|
|||
allow_ctrl = True
|
||||
elif pedal == 'interceptor':
|
||||
# gas_interceptor_prev > INTERCEPTOR_THRESHOLD
|
||||
self.safety.safety_rx_hook(interceptor_msg(INTERCEPTOR_THRESHOLD+1, 0x201))
|
||||
self.safety.safety_rx_hook(interceptor_msg(INTERCEPTOR_THRESHOLD+1, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(INTERCEPTOR_THRESHOLD+1, 0x201))
|
||||
self.safety.safety_rx_hook(honda_interceptor_msg(INTERCEPTOR_THRESHOLD+1, 0x201))
|
||||
allow_ctrl = True
|
||||
|
||||
self.safety.set_controls_allowed(1)
|
||||
|
@ -406,14 +413,14 @@ class TestHondaSafety(unittest.TestCase):
|
|||
if hw == HONDA_N_HW:
|
||||
self.safety.set_honda_fwd_brake(False)
|
||||
self.assertEqual(allow_ctrl, self.safety.safety_tx_hook(self._send_brake_msg(MAX_BRAKE)))
|
||||
self.assertEqual(allow_ctrl, self.safety.safety_tx_hook(interceptor_msg(INTERCEPTOR_THRESHOLD, 0x200)))
|
||||
self.assertEqual(allow_ctrl, self.safety.safety_tx_hook(honda_interceptor_msg(INTERCEPTOR_THRESHOLD, 0x200)))
|
||||
self.assertEqual(allow_ctrl, self.safety.safety_tx_hook(self._send_steer_msg(0x1000)))
|
||||
# reset status
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT)
|
||||
self.safety.safety_tx_hook(self._send_brake_msg(0))
|
||||
self.safety.safety_tx_hook(self._send_steer_msg(0))
|
||||
self.safety.safety_tx_hook(interceptor_msg(0, 0x200))
|
||||
self.safety.safety_tx_hook(honda_interceptor_msg(0, 0x200))
|
||||
if pedal == 'brake':
|
||||
self.safety.safety_rx_hook(self._speed_msg(0))
|
||||
self.safety.safety_rx_hook(self._brake_msg(0))
|
||||
|
|
|
@ -4,7 +4,7 @@ import numpy as np
|
|||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
import panda.tests.safety.common as common
|
||||
from panda.tests.safety.common import CANPackerPanda, interceptor_msg, UNSAFE_MODE
|
||||
from panda.tests.safety.common import CANPackerPanda, make_msg, UNSAFE_MODE
|
||||
|
||||
MAX_RATE_UP = 10
|
||||
MAX_RATE_DOWN = 25
|
||||
|
@ -20,7 +20,14 @@ MAX_RT_DELTA = 375
|
|||
RT_INTERVAL = 250000
|
||||
|
||||
MAX_TORQUE_ERROR = 350
|
||||
INTERCEPTOR_THRESHOLD = 475
|
||||
INTERCEPTOR_THRESHOLD = 845
|
||||
|
||||
# Toyota gas gains are the same
|
||||
def toyota_interceptor_msg(gas, addr):
|
||||
to_send = make_msg(0, addr, 6)
|
||||
to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \
|
||||
((gas & 0xff) << 24) | ((gas & 0xff00) << 8)
|
||||
return to_send
|
||||
|
||||
class TestToyotaSafety(common.PandaSafetyTest):
|
||||
|
||||
|
@ -77,38 +84,38 @@ class TestToyotaSafety(common.PandaSafetyTest):
|
|||
return self.packer.make_can_msg_panda("PCM_CRUISE", 0, values)
|
||||
|
||||
def test_prev_gas_interceptor(self):
|
||||
self._rx(interceptor_msg(0x0, 0x201))
|
||||
self._rx(toyota_interceptor_msg(0x0, 0x201))
|
||||
self.assertFalse(self.safety.get_gas_interceptor_prev())
|
||||
self._rx(interceptor_msg(0x1000, 0x201))
|
||||
self._rx(toyota_interceptor_msg(0x1000, 0x201))
|
||||
self.assertTrue(self.safety.get_gas_interceptor_prev())
|
||||
self._rx(interceptor_msg(0x0, 0x201))
|
||||
self._rx(toyota_interceptor_msg(0x0, 0x201))
|
||||
|
||||
def test_disengage_on_gas_interceptor(self):
|
||||
for g in range(0, 0x1000):
|
||||
self._rx(interceptor_msg(0, 0x201))
|
||||
self._rx(toyota_interceptor_msg(0, 0x201))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._rx(interceptor_msg(g, 0x201))
|
||||
self._rx(toyota_interceptor_msg(g, 0x201))
|
||||
remain_enabled = g <= INTERCEPTOR_THRESHOLD
|
||||
self.assertEqual(remain_enabled, self.safety.get_controls_allowed())
|
||||
self._rx(interceptor_msg(0, 0x201))
|
||||
self._rx(toyota_interceptor_msg(0, 0x201))
|
||||
self.safety.set_gas_interceptor_detected(False)
|
||||
|
||||
def test_unsafe_mode_no_disengage_on_gas_interceptor(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS)
|
||||
for g in range(0, 0x1000):
|
||||
self._rx(interceptor_msg(g, 0x201))
|
||||
self._rx(toyota_interceptor_msg(g, 0x201))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self._rx(interceptor_msg(0, 0x201))
|
||||
self._rx(toyota_interceptor_msg(0, 0x201))
|
||||
self.safety.set_gas_interceptor_detected(False)
|
||||
self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT)
|
||||
|
||||
def test_allow_engage_with_gas_interceptor_pressed(self):
|
||||
self._rx(interceptor_msg(0x1000, 0x201))
|
||||
self._rx(toyota_interceptor_msg(0x1000, 0x201))
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._rx(interceptor_msg(0x1000, 0x201))
|
||||
self._rx(toyota_interceptor_msg(0x1000, 0x201))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self._rx(interceptor_msg(0, 0x201))
|
||||
self._rx(toyota_interceptor_msg(0, 0x201))
|
||||
|
||||
def test_accel_actuation_limits(self):
|
||||
limits = ((MIN_ACCEL, MAX_ACCEL, UNSAFE_MODE.DEFAULT),
|
||||
|
@ -214,10 +221,10 @@ class TestToyotaSafety(common.PandaSafetyTest):
|
|||
|
||||
def test_gas_interceptor_safety_check(self):
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.assertTrue(self._tx(interceptor_msg(0, 0x200)))
|
||||
self.assertFalse(self._tx(interceptor_msg(0x1000, 0x200)))
|
||||
self.assertTrue(self._tx(toyota_interceptor_msg(0, 0x200)))
|
||||
self.assertFalse(self._tx(toyota_interceptor_msg(0x1000, 0x200)))
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.assertTrue(self._tx(interceptor_msg(0x1000, 0x200)))
|
||||
self.assertTrue(self._tx(toyota_interceptor_msg(0x1000, 0x200)))
|
||||
|
||||
def test_rx_hook(self):
|
||||
# checksum checks
|
||||
|
|
Loading…
Reference in New Issue