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

WIP: Toyota brake check. (#459) 

* Toyota brake check with safety tests
master
rbiasini 2020-03-05 00:16:03 -08:00 committed by GitHub
parent 2ef996fd92
commit 4368748851
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 79 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
board/drivers/llcan.h
View File

@ -52,7 +52,7 @@ bool llcan_set_speed(CAN_TypeDef *CAN_obj, uint32_t speed, bool loopback, bool s
void llcan_init(CAN_TypeDef *CAN_obj) {
// Enter init mode
register_set_bits(&(CAN_obj->FMR), CAN_FMR_FINIT);
// Wait for INAK bit to be set
while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) {}

51
board/safety/safety_toyota.h
View File

@ -16,7 +16,8 @@ const uint32_t TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks
const int TOYOTA_MAX_ACCEL = 1500; // 1.5 m/s2
const int TOYOTA_MIN_ACCEL = -3000; // 3.0 m/s2
const int TOYOTA_GAS_INTERCEPTOR_THRESHOLD = 475; // ratio between offset and gain from dbc file
const int TOYOTA_STANDSTILL_THRSLD = 100; // 1kph
const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 475; // ratio between offset and gain from dbc file
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
@ -24,8 +25,10 @@ const AddrBus TOYOTA_TX_MSGS[] = {{0x283, 0}, {0x2E6, 0}, {0x2E7, 0}, {0x33E, 0}
{0x200, 0}, {0x750, 0}}; // interceptor + Blindspot monitor
AddrCheckStruct toyota_rx_checks[] = {
{.addr = {0x260}, .bus = 0, .check_checksum = true, .max_counter = 0U, .expected_timestep = 20000U},
{.addr = {0x1D2}, .bus = 0, .check_checksum = true, .max_counter = 0U, .expected_timestep = 30000U},
{.addr = { 0xaa}, .bus = 0, .check_checksum = false, .expected_timestep = 12000U},
{.addr = {0x260}, .bus = 0, .check_checksum = true, .expected_timestep = 20000U},
{.addr = {0x1D2}, .bus = 0, .check_checksum = true, .expected_timestep = 30000U},
{.addr = {0x224, 0x226}, .bus = 0, .check_checksum = false, .expected_timestep = 25000U},
};
const int TOYOTA_RX_CHECKS_LEN = sizeof(toyota_rx_checks) / sizeof(toyota_rx_checks[0]);
@ -37,7 +40,9 @@ int toyota_desired_torque_last = 0; // last desired steer torque
int toyota_rt_torque_last = 0; // last desired torque for real time check
uint32_t toyota_ts_last = 0;
int toyota_cruise_engaged_last = 0; // cruise state
int toyota_gas_prev = 0;
bool toyota_gas_prev = false;
bool toyota_brake_prev = false;
bool toyota_moving = false;
struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps
@ -65,7 +70,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int addr = GET_ADDR(to_push);
// get eps motor torque (0.66 factor in dbc)
if (addr == 0x260) {
if ((addr == 0x260) && (bus == 0)) {
int torque_meas_new = (GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6);
torque_meas_new = to_signed(torque_meas_new, 16);
@ -81,7 +86,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 0x1D2) {
if ((addr == 0x1D2) && (bus == 0)) {
// 5th bit is CRUISE_ACTIVE
int cruise_engaged = GET_BYTE(to_push, 0) & 0x20;
if (!cruise_engaged) {
@ -93,21 +98,43 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
toyota_cruise_engaged_last = cruise_engaged;
}
// sample speed
if ((addr == 0xaa) && (bus == 0)) {
int speed = 0;
// sum 4 wheel speeds
for (int i=0; i<8; i+=2) {
int next_byte = i + 1; // hack to deal with misra 10.8
speed += (GET_BYTE(to_push, i) << 8) + GET_BYTE(to_push, next_byte);
}
toyota_moving = (speed / 4) > TOYOTA_STANDSTILL_THRSLD;
}
// exit controls on rising edge of brake pedal
// most cars have brake_pressed on 0x226, corolla and rav4 on 0x224
if (((addr == 0x224) || (addr == 0x226)) && (bus == 0)) {
int byte = (addr == 0x224) ? 0 : 4;
bool brake = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0;
if (brake && (!toyota_brake_prev || toyota_moving)) {
controls_allowed = 0;
}
toyota_brake_prev = brake;
}
// exit controls on rising edge of interceptor gas press
if (addr == 0x201) {
if ((addr == 0x201) && (bus == 0)) {
gas_interceptor_detected = 1;
int gas_interceptor = GET_INTERCEPTOR(to_push);
if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) &&
(gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRESHOLD)) {
if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD) &&
(gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRSLD)) {
controls_allowed = 0;
}
gas_interceptor_prev = gas_interceptor;
}
// exit controls on rising edge of gas press
if (addr == 0x2C1) {
int gas = GET_BYTE(to_push, 6);
if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected) {
if ((addr == 0x2C1) && (bus == 0)) {
bool gas = GET_BYTE(to_push, 6) != 0;
if (gas && !toyota_gas_prev && !gas_interceptor_detected) {
controls_allowed = 0;
}
toyota_gas_prev = gas;

2
tests/safety/libpandasafety_py.py
View File

@ -48,7 +48,7 @@ int set_safety_hooks(uint16_t mode, int16_t param);
void init_tests_toyota(void);
int get_toyota_torque_meas_min(void);
int get_toyota_torque_meas_max(void);
int get_toyota_gas_prev(void);
bool get_toyota_gas_prev(void);
void set_toyota_torque_meas(int min, int max);
void set_toyota_desired_torque_last(int t);
void set_toyota_rt_torque_last(int t);

2
tests/safety/test.c
View File

@ -159,7 +159,7 @@ int get_chrysler_torque_meas_max(void){
return chrysler_torque_meas.max;
}
int get_toyota_gas_prev(void){
bool get_toyota_gas_prev(void){
return toyota_gas_prev;
}

39
tests/safety/test_toyota.py
View File

@ -15,6 +15,8 @@ MIN_ACCEL = -3000
MAX_RT_DELTA = 375
RT_INTERVAL = 250000
STANDSTILL_THRESHOLD = 100 # 1kph
MAX_TORQUE_ERROR = 350
INTERCEPTOR_THRESHOLD = 475
@ -62,7 +64,7 @@ class TestToyotaSafety(unittest.TestCase):
t = twos_comp(torque, 16)
to_send = make_msg(0, 0x260)
to_send[0].RDHR = (t & 0xff00) | ((t & 0xFF) << 16)
to_send[0].RDHR = to_send[0].RDHR | (toyota_checksum(to_send[0], 0x260, 8) << 24)
to_send[0].RDHR |= toyota_checksum(to_send[0], 0x260, 8) << 24
return to_send
def _torque_msg(self, torque):
@ -77,6 +79,20 @@ class TestToyotaSafety(unittest.TestCase):
to_send[0].RDLR = (a & 0xFF) << 8 | (a >> 8)
return to_send
def _speed_msg(self, s):
to_send = make_msg(0, 0xaa)
to_send[0].RDLR = (s & 0xFF) << 8 | (s >> 8)
to_send[0].RDLR += (s & 0xFF) << 24 | ((s >> 8) << 16)
to_send[0].RDHR = (s & 0xFF) << 8 | (s >> 8)
to_send[0].RDHR += (s & 0xFF) << 24 | ((s >> 8) << 16)
return to_send
def _brake_msg(self, brake):
to_send = make_msg(0, 0x226)
to_send[0].RDHR = brake << 5
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
@ -121,7 +137,7 @@ class TestToyotaSafety(unittest.TestCase):
def test_prev_gas(self):
for g in range(0, 256):
self.safety.safety_rx_hook(self._send_gas_msg(g))
self.assertEqual(g, self.safety.get_toyota_gas_prev())
self.assertEqual(True if g > 0 else False, self.safety.get_toyota_gas_prev())
def test_prev_gas_interceptor(self):
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
@ -155,6 +171,25 @@ class TestToyotaSafety(unittest.TestCase):
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_gas_interceptor_detected(False)
def test_allow_brake_at_zero_speed(self):
# Brake was already pressed
self.safety.safety_rx_hook(self._speed_msg(0))
self.safety.safety_rx_hook(self._brake_msg(True))
self.safety.set_controls_allowed(1)
self.safety.safety_rx_hook(self._brake_msg(True))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.safety_rx_hook(self._brake_msg(False)) # reset no brakes
def test_not_allow_brake_when_moving(self):
# Brake was already pressed
self.safety.safety_rx_hook(self._brake_msg(True))
self.safety.safety_rx_hook(self._speed_msg(STANDSTILL_THRESHOLD + 1))
self.safety.set_controls_allowed(1)
self.safety.safety_rx_hook(self._brake_msg(True))
self.assertFalse(self.safety.get_controls_allowed())
def test_allow_engage_with_gas_interceptor_pressed(self):
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.safety.set_controls_allowed(1)