mawei
/
panda
mirror of https://github.com/commaai/panda.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Toyota gas cancellation (#200) 

* cancel on pedal press for toyota.
This commit is contained in:
rbiasini 2019-06-05 13:38:59 -07:00 committed by GitHub
parent 436b203e0b
commit 09714e3a44
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 157 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
board/safety.h
View File

@ -46,6 +46,7 @@ typedef struct {
// This can be set by the safety hooks.
int controls_allowed = 0;
int gas_interceptor_detected = 0;
int gas_interceptor_prev = 0;
// Include the actual safety policies.
#include "safety/safety_defaults.h"
@ -104,7 +105,6 @@ typedef struct {
#define SAFETY_SUBARU 10
#define SAFETY_GM_ASCM 0x1334
#define SAFETY_TOYOTA_IPAS 0x1335
#define SAFETY_TOYOTA_NOLIMITS 0x1336
#define SAFETY_ALLOUTPUT 0x1337
#define SAFETY_ELM327 0xE327
@ -119,7 +119,6 @@ const safety_hook_config safety_hook_registry[] = {
{SAFETY_HYUNDAI, &hyundai_hooks},
{SAFETY_CHRYSLER, &chrysler_hooks},
{SAFETY_SUBARU, &subaru_hooks},
{SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks},
{SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
{SAFETY_GM_ASCM, &gm_ascm_hooks},
{SAFETY_TESLA, &tesla_hooks},

10
board/safety/safety_honda.h
View File

@ -7,12 +7,10 @@
// brake rising edge
// brake > 0mph
const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 328;
const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 328; // ratio between offset and gain from dbc file
int honda_brake_prev = 0;
int honda_gas_prev = 0;
int honda_gas_interceptor_prev = 0;
int honda_ego_speed = 0;
// TODO: auto-detect bosch hardware based on CAN messages?
bool honda_bosch_hardware = false;
bool honda_alt_brake_msg = false;
@ -59,10 +57,10 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
gas_interceptor_detected = 1;
int gas_interceptor = ((to_push->RDLR & 0xFF) << 8) | ((to_push->RDLR & 0xFF00) >> 8);
if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) &&
(honda_gas_interceptor_prev <= HONDA_GAS_INTERCEPTOR_THRESHOLD)) {
(gas_interceptor_prev <= HONDA_GAS_INTERCEPTOR_THRESHOLD)) {
controls_allowed = 0;
}
honda_gas_interceptor_prev = gas_interceptor;
gas_interceptor_prev = gas_interceptor;
}
// exit controls on rising edge of gas press if no interceptor
@ -87,7 +85,7 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
// and the the latching controls_allowed flag is True
int pedal_pressed = honda_gas_prev || (honda_gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) ||
int pedal_pressed = honda_gas_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) ||
(honda_brake_prev && honda_ego_speed);
int current_controls_allowed = controls_allowed && !(pedal_pressed);

59
board/safety/safety_toyota.h
View File

@ -1,6 +1,3 @@
int toyota_giraffe_switch_1 = 0; // is giraffe switch 1 high?
int toyota_camera_forwarded = 0; // should we forward the camera bus?
// global torque limit
const int TOYOTA_MAX_TORQUE = 1500; // max torque cmd allowed ever
@ -19,15 +16,19 @@ const int 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
// global actuation limit state
int toyota_actuation_limits = 1; // by default steer limits are imposed
const int TOYOTA_GAS_INTERCEPTOR_THRESHOLD = 475; // ratio between offset and gain from dbc file
// global actuation limit states
int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
// state of torque limits
// states
int toyota_giraffe_switch_1 = 0; // is giraffe switch 1 high?
int toyota_camera_forwarded = 0; // should we forward the camera bus?
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;
struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps
@ -46,19 +47,32 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// increase torque_meas by 1 to be conservative on rounding
toyota_torque_meas.min--;
toyota_torque_meas.max++;
}
// enter controls on rising edge of ACC, exit controls on ACC off
if ((to_push->RIR>>21) == 0x1D2) {
// 5th bit is CRUISE_ACTIVE
int cruise_engaged = to_push->RDLR & 0x20;
if (cruise_engaged && !toyota_cruise_engaged_last) {
controls_allowed = 1;
} else if (!cruise_engaged) {
// 4th bit is GAS_RELEASED
int gas = !(to_push->RDLR & 0x10);
if (!cruise_engaged || (gas && !toyota_gas_prev && !gas_interceptor_detected)) {
controls_allowed = 0;
} else if (cruise_engaged && !toyota_cruise_engaged_last) {
controls_allowed = 1;
}
toyota_cruise_engaged_last = cruise_engaged;
toyota_gas_prev = gas;
}
// exit controls on rising edge of gas press if interceptor (0x201)
if ((to_push->RIR>>21) == 0x201) {
gas_interceptor_detected = 1;
int gas_interceptor = ((to_push->RDLR & 0xFF) << 8) | ((to_push->RDLR & 0xFF00) >> 8);
if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) &&
(gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRESHOLD)) {
controls_allowed = 0;
}
gas_interceptor_prev = gas_interceptor;
}
int bus = (to_push->RDTR >> 4) & 0xF;
@ -83,7 +97,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// GAS PEDAL: safety check
if ((to_send->RIR>>21) == 0x200) {
if (controls_allowed && toyota_actuation_limits) {
if (controls_allowed) {
// all messages are fine here
} else {
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0;
@ -94,7 +108,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
if ((to_send->RIR>>21) == 0x343) {
int desired_accel = ((to_send->RDLR & 0xFF) << 8) | ((to_send->RDLR >> 8) & 0xFF);
desired_accel = to_signed(desired_accel, 16);
if (controls_allowed && toyota_actuation_limits) {
if (controls_allowed) {
int violation = max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL);
if (violation) return 0;
} else if (!controls_allowed && (desired_accel != 0)) {
@ -110,8 +124,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
uint32_t ts = TIM2->CNT;
// only check if controls are allowed and actuation_limits are imposed
if (controls_allowed && toyota_actuation_limits) {
if (controls_allowed) {
// *** global torque limit check ***
violation |= max_limit_check(desired_torque, TOYOTA_MAX_TORQUE, -TOYOTA_MAX_TORQUE);
@ -158,7 +171,6 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
static void toyota_init(int16_t param) {
controls_allowed = 0;
toyota_actuation_limits = 1;
toyota_giraffe_switch_1 = 0;
toyota_camera_forwarded = 0;
toyota_dbc_eps_torque_factor = param;
@ -186,20 +198,3 @@ const safety_hooks toyota_hooks = {
.ignition = default_ign_hook,
.fwd = toyota_fwd_hook,
};
static void toyota_nolimits_init(int16_t param) {
controls_allowed = 0;
toyota_actuation_limits = 0;
toyota_giraffe_switch_1 = 0;
toyota_camera_forwarded = 0;
toyota_dbc_eps_torque_factor = param;
}
const safety_hooks toyota_nolimits_hooks = {
.init = toyota_nolimits_init,
.rx = toyota_rx_hook,
.tx = toyota_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
.ignition = default_ign_hook,
.fwd = toyota_fwd_hook,
};

4
tests/safety/libpandasafety_py.py
View File

@ -32,6 +32,9 @@ typedef struct
void set_controls_allowed(int c);
int get_controls_allowed(void);
void set_gas_interceptor_detected(int c);
int get_gas_interceptor_detetcted(void);
int get_gas_interceptor_prev(void);
void set_timer(int t);
void reset_angle_control(void);
@ -41,6 +44,7 @@ int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
void toyota_init(int16_t param);
int get_toyota_torque_meas_min(void);
int get_toyota_torque_meas_max(void);
int 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);

17
tests/safety/test.c
View File

@ -51,6 +51,10 @@ void set_controls_allowed(int c){
controls_allowed = c;
}
void set_gas_interceptor_detected(int c){
gas_interceptor_detected = c;
}
void reset_angle_control(void){
angle_control = 0;
}
@ -59,6 +63,14 @@ int get_controls_allowed(void){
return controls_allowed;
}
int get_gas_interceptor_detected(void){
return gas_interceptor_detected;
}
int get_gas_interceptor_prev(void){
return gas_interceptor_prev;
}
void set_timer(int t){
timer.CNT = t;
}
@ -101,6 +113,10 @@ int get_chrysler_torque_meas_max(void){
return chrysler_torque_meas.max;
}
int get_toyota_gas_prev(void){
return toyota_gas_prev;
}
int get_toyota_torque_meas_min(void){
return toyota_torque_meas.min;
}
@ -233,7 +249,6 @@ void init_tests_subaru(void){
void init_tests_honda(void){
honda_ego_speed = 0;
gas_interceptor_detected = 0;
honda_brake_prev = 0;
honda_gas_prev = 0;
}

45
tests/safety/test_honda.py
View File

@ -53,10 +53,11 @@ class TestHondaSafety(unittest.TestCase):
return to_send
def _send_gas_msg(self, gas):
def _send_interceptor_msg(self, gas, addr):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x200 << 21
to_send[0].RDLR = gas
to_send[0].RIR = addr << 21
to_send[0].RDTR = 6
to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8)
return to_send
@ -133,10 +134,19 @@ class TestHondaSafety(unittest.TestCase):
self.assertFalse(self.safety.get_controls_allowed())
def test_prev_gas(self):
self.safety.honda_rx_hook(self._gas_msg(False))
self.assertFalse(self.safety.get_honda_gas_prev())
self.safety.honda_rx_hook(self._gas_msg(True))
self.assertTrue(self.safety.get_honda_gas_prev())
def test_prev_gas_interceptor(self):
self.safety.honda_rx_hook(self._send_interceptor_msg(0x0, 0x201))
self.assertFalse(self.safety.get_gas_interceptor_prev())
self.safety.honda_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.assertTrue(self.safety.get_gas_interceptor_prev())
self.safety.honda_rx_hook(self._send_interceptor_msg(0x0, 0x201))
self.safety.set_gas_interceptor_detected(False)
def test_disengage_on_gas(self):
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._gas_msg(1))
@ -148,18 +158,35 @@ class TestHondaSafety(unittest.TestCase):
self.safety.honda_rx_hook(self._gas_msg(1))
self.assertTrue(self.safety.get_controls_allowed())
def test_brake_safety_check(self):
self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0x0000)))
self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x1000)))
def test_disengage_on_gas_interceptor(self):
self.safety.honda_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.assertFalse(self.safety.get_controls_allowed())
self.safety.honda_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_gas_interceptor_detected(False)
def test_allow_engage_with_gas_interceptor_pressed(self):
self.safety.honda_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.honda_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_gas_interceptor_detected(False)
def test_brake_safety_check(self):
self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0)))
self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x1000)))
self.safety.set_controls_allowed(1)
self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0x1000)))
self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x00F0)))
def test_gas_safety_check(self):
def test_gas_interceptor_safety_check(self):
self.safety.set_controls_allowed(0)
self.assertTrue(self.safety.honda_tx_hook(self._send_gas_msg(0x0000)))
self.assertFalse(self.safety.honda_tx_hook(self._send_gas_msg(0x1000)))
self.assertTrue(self.safety.honda_tx_hook(self._send_interceptor_msg(0, 0x200)))
self.assertFalse(self.safety.honda_tx_hook(self._send_interceptor_msg(0x1000, 0x200)))
self.safety.set_controls_allowed(1)
self.assertTrue(self.safety.honda_tx_hook(self._send_interceptor_msg(0x1000, 0x200)))
def test_steer_safety_check(self):
self.safety.set_controls_allowed(0)

84
tests/safety/test_toyota.py
View File

@ -119,13 +119,28 @@ class TestToyotaSafety(unittest.TestCase):
to_send[0].RDLR = (a & 0xFF) << 8 | (a >> 8)
return to_send
def _gas_msg(self, gas):
def _send_gas_msg(self, gas):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x200 << 21
to_send[0].RDLR = gas
return to_send
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)
return to_send
def _pcm_cruise_msg(self, cruise_on, gas_pressed):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x1D2 << 21
to_send[0].RDLR = (cruise_on << 5) | ((not gas_pressed) << 4 )
return to_send
def test_default_controls_not_allowed(self):
self.assertFalse(self.safety.get_controls_allowed())
@ -134,22 +149,60 @@ class TestToyotaSafety(unittest.TestCase):
self.assertTrue(self.safety.get_controls_allowed())
def test_enable_control_allowed_from_cruise(self):
to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_push[0].RIR = 0x1D2 << 21
to_push[0].RDLR = 0x20
self.safety.toyota_rx_hook(to_push)
self.safety.toyota_rx_hook(self._pcm_cruise_msg(False, False))
self.assertFalse(self.safety.get_controls_allowed())
self.safety.toyota_rx_hook(self._pcm_cruise_msg(True, False))
self.assertTrue(self.safety.get_controls_allowed())
def test_disable_control_allowed_from_cruise(self):
to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_push[0].RIR = 0x1D2 << 21
to_push[0].RDLR = 0
self.safety.set_controls_allowed(1)
self.safety.toyota_rx_hook(to_push)
self.safety.toyota_rx_hook(self._pcm_cruise_msg(False, False))
self.assertFalse(self.safety.get_controls_allowed())
def test_prev_gas(self):
self.safety.toyota_rx_hook(self._pcm_cruise_msg(False, False))
self.assertFalse(self.safety.get_toyota_gas_prev())
self.safety.toyota_rx_hook(self._pcm_cruise_msg(False, True))
self.assertTrue(self.safety.get_toyota_gas_prev())
def test_prev_gas_interceptor(self):
self.safety.toyota_rx_hook(self._send_interceptor_msg(0x0, 0x201))
self.assertFalse(self.safety.get_gas_interceptor_prev())
self.safety.toyota_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.assertTrue(self.safety.get_gas_interceptor_prev())
self.safety.toyota_rx_hook(self._send_interceptor_msg(0x0, 0x201))
self.safety.set_gas_interceptor_detected(False)
def test_disengage_on_gas(self):
self.safety.toyota_rx_hook(self._pcm_cruise_msg(False, False))
self.safety.toyota_rx_hook(self._pcm_cruise_msg(True, False))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.toyota_rx_hook(self._pcm_cruise_msg(True, True))
self.assertFalse(self.safety.get_controls_allowed())
def test_allow_engage_with_gas_pressed(self):
self.safety.toyota_rx_hook(self._pcm_cruise_msg(False, True))
self.safety.toyota_rx_hook(self._pcm_cruise_msg(True, True))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.toyota_rx_hook(self._pcm_cruise_msg(True, True))
self.assertTrue(self.safety.get_controls_allowed())
def test_disengage_on_gas_interceptor(self):
self.safety.toyota_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_controls_allowed(1)
self.safety.toyota_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.assertFalse(self.safety.get_controls_allowed())
self.safety.toyota_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_gas_interceptor_detected(False)
def test_allow_engage_with_gas_interceptor_pressed(self):
self.safety.toyota_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.safety.set_controls_allowed(1)
self.safety.toyota_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.toyota_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_gas_interceptor_detected(False)
def test_accel_actuation_limits(self):
for accel in np.arange(MIN_ACCEL - 1000, MAX_ACCEL + 1000, 100):
for controls_allowed in [True, False]:
@ -416,12 +469,13 @@ class TestToyotaSafety(unittest.TestCase):
# reset no angle control at the end of the test
self.safety.reset_angle_control()
def test_gas_safety_check(self):
def test_gas_interceptor_safety_check(self):
self.safety.set_controls_allowed(0)
self.assertTrue(self.safety.honda_tx_hook(self._gas_msg(0x0000)))
self.assertFalse(self.safety.honda_tx_hook(self._gas_msg(0x1000)))
self.assertTrue(self.safety.toyota_tx_hook(self._send_interceptor_msg(0, 0x200)))
self.assertFalse(self.safety.toyota_tx_hook(self._send_interceptor_msg(0x1000, 0x200)))
self.safety.set_controls_allowed(1)
self.assertTrue(self.safety.honda_tx_hook(self._gas_msg(0x1000)))
self.assertTrue(self.safety.toyota_tx_hook(self._send_interceptor_msg(0x1000, 0x200)))
if __name__ == "__main__":