Skip to content

Commit

Permalink
WIP: Toyota brake check. (#459)
Browse files Browse the repository at this point in the history
* Toyota brake check with safety tests
  • Loading branch information
rbiasini authored Mar 5, 2020
1 parent 2ef996f commit 4368748
Show file tree
Hide file tree
Showing 5 changed files with 79 additions and 17 deletions.
2 changes: 1 addition & 1 deletion board/drivers/llcan.h
Original file line number Diff line number Diff line change
Expand Up @@ -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)) {}

Expand Down
51 changes: 39 additions & 12 deletions board/safety/safety_toyota.h
Original file line number Diff line number Diff line change
Expand Up @@ -16,16 +16,19 @@ 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
{0x2E4, 0}, {0x411, 0}, {0x412, 0}, {0x343, 0}, {0x1D2, 0}, // LKAS + ACC
{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]);

Expand All @@ -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


Expand Down Expand Up @@ -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);

Expand All @@ -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) {
Expand All @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion tests/safety/libpandasafety_py.py
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@
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);
Expand Down
2 changes: 1 addition & 1 deletion tests/safety/test.c
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
39 changes: 37 additions & 2 deletions tests/safety/test_toyota.py
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
MAX_RT_DELTA = 375
RT_INTERVAL = 250000

STANDSTILL_THRESHOLD = 100 # 1kph

MAX_TORQUE_ERROR = 350
INTERCEPTOR_THRESHOLD = 475

Expand Down Expand Up @@ -62,7 +64,7 @@ def _torque_meas_msg(self, torque):
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):
Expand All @@ -77,6 +79,20 @@ def _accel_msg(self, accel):
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
Expand Down Expand Up @@ -121,7 +137,7 @@ def test_disable_control_allowed_from_cruise(self):
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))
Expand Down Expand Up @@ -155,6 +171,25 @@ def test_disengage_on_gas_interceptor(self):
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)
Expand Down

0 comments on commit 4368748

Please sign in to comment.