Skip to content

Commit

Permalink
VW button spam: fix safety and add tests (#306)
Browse files Browse the repository at this point in the history 
* VW button spam: fix safety and add tests

* button spam is actually sent on bus 2

* Fix safety test. Still need to add gas safety test

* added gas safety test to VW and using consistent nomenclature

* style fixes
  • Loading branch information
@rbiasini
rbiasini authored Oct 29, 2019
1 parent 6cccf96 commit 48d0d0c
Show file tree
Hide file tree
Showing 4 changed files with 121 additions and 46 deletions.
89 changes: 53 additions & 36 deletions board/safety/safety_volkswagen.h
View file
Original file line number Diff line number Diff line change
@@ -1,18 +1,20 @@
const int VW_MAX_STEER = 250; // 2.5 nm
const int VW_MAX_RT_DELTA = 75; // 4 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 125 ; 125 * 1.5 for safety pad = 75
const uint32_t VW_RT_INTERVAL = 250000; // 250ms between real time checks
const int VW_MAX_RATE_UP = 4; // 5.0 nm/s available rate of change from the steering rack
const int VW_MAX_RATE_DOWN = 10; // 5.0 nm/s available rate of change from the steering rack
const int VW_DRIVER_TORQUE_ALLOWANCE = 80;
const int VW_DRIVER_TORQUE_FACTOR = 3;

struct sample_t vw_torque_driver; // last few driver torques measured
int vw_rt_torque_last = 0;
int vw_desired_torque_last = 0;
uint32_t vw_ts_last = 0;
const int VOLKSWAGEN_MAX_STEER = 250; // 2.5 Nm (EPS side max of 3.0Nm with fault if violated)
const int VOLKSWAGEN_MAX_RT_DELTA = 75; // 4 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 50 ; 50 * 1.5 for safety pad = 75
const uint32_t VOLKSWAGEN_RT_INTERVAL = 250000; // 250ms between real time checks
const int VOLKSWAGEN_MAX_RATE_UP = 4; // 2.0 Nm/s available rate of change from the steering rack (EPS side delta-limit of 5.0 Nm/s)
const int VOLKSWAGEN_MAX_RATE_DOWN = 10; // 5.0 Nm/s available rate of change from the steering rack (EPS side delta-limit of 5.0 Nm/s)
const int VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE = 80;
const int VOLKSWAGEN_DRIVER_TORQUE_FACTOR = 3;

struct sample_t volkswagen_torque_driver; // last few driver torques measured
int volkswagen_rt_torque_last = 0;
int volkswagen_desired_torque_last = 0;
uint32_t volkswagen_ts_last = 0;
int volkswagen_gas_prev = 0;

// Safety-relevant CAN messages for the Volkswagen MQB platform.
#define MSG_EPS_01 0x09F
#define MSG_MOTOR_20 0x121
#define MSG_ACC_06 0x122
#define MSG_HCA_01 0x126
#define MSG_GRA_ACC_01 0x12B
Expand All @@ -23,6 +25,7 @@ static void volkswagen_init(int16_t param) {
UNUSED(param); // May use param in the future to indicate MQB vs PQ35/PQ46/NMS vs MLB, or wiring configuration.
controls_allowed = 0;
}

static void volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
Expand All @@ -36,20 +39,30 @@ static void volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
torque_driver_new *= -1;
}

update_sample(&vw_torque_driver, torque_driver_new);
update_sample(&volkswagen_torque_driver, torque_driver_new);
}

// Monitor ACC_06.ACC_Status_ACC for stock ACC status. Because the current MQB port is lateral-only, OP's control
// allowed state is directly driven by stock ACC engagement. Permit the ACC message to come from either bus, in
// order to accommodate future camera-side integrations if needed.
if (addr == MSG_ACC_06) {
int acc_status = (GET_BYTE(to_push,7) & 0x70) >> 4;
int acc_status = (GET_BYTE(to_push, 7) & 0x70) >> 4;
controls_allowed = ((acc_status == 3) || (acc_status == 4) || (acc_status == 5)) ? 1 : 0;
}

// exit controls on rising edge of gas press. Bits [12-20)
if (addr == MSG_MOTOR_20) {
int gas = (GET_BYTES_04(to_push) >> 12) & 0xFF;
if ((gas > 0) && (volkswagen_gas_prev == 0) && long_controls_allowed) {
controls_allowed = 0;
}
volkswagen_gas_prev = gas;
}
}

static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
int tx = 1;

// Safety check for HCA_01 Heading Control Assist torque.
Expand All @@ -67,22 +80,22 @@ static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
if (controls_allowed) {

// *** global torque limit check ***
violation |= max_limit_check(desired_torque, VW_MAX_STEER, -VW_MAX_STEER);
violation |= max_limit_check(desired_torque, VOLKSWAGEN_MAX_STEER, -VOLKSWAGEN_MAX_STEER);

// *** torque rate limit check ***
violation |= driver_limit_check(desired_torque, vw_desired_torque_last, &vw_torque_driver,
VW_MAX_STEER, VW_MAX_RATE_UP, VW_MAX_RATE_DOWN,
VW_DRIVER_TORQUE_ALLOWANCE, VW_DRIVER_TORQUE_FACTOR);
vw_desired_torque_last = desired_torque;
violation |= driver_limit_check(desired_torque, volkswagen_desired_torque_last, &volkswagen_torque_driver,
VOLKSWAGEN_MAX_STEER, VOLKSWAGEN_MAX_RATE_UP, VOLKSWAGEN_MAX_RATE_DOWN,
VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE, VOLKSWAGEN_DRIVER_TORQUE_FACTOR);
volkswagen_desired_torque_last = desired_torque;

// *** torque real time rate limit check ***
violation |= rt_rate_limit_check(desired_torque, vw_rt_torque_last, VW_MAX_RT_DELTA);
violation |= rt_rate_limit_check(desired_torque, volkswagen_rt_torque_last, VOLKSWAGEN_MAX_RT_DELTA);

// every RT_INTERVAL set the new limits
uint32_t ts_elapsed = get_ts_elapsed(ts, vw_ts_last);
if (ts_elapsed > VW_RT_INTERVAL) {
vw_rt_torque_last = desired_torque;
vw_ts_last = ts;
uint32_t ts_elapsed = get_ts_elapsed(ts, volkswagen_ts_last);
if (ts_elapsed > VOLKSWAGEN_RT_INTERVAL) {
volkswagen_rt_torque_last = desired_torque;
volkswagen_ts_last = ts;
}
}

Expand All @@ -93,16 +106,25 @@ static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {

// reset to 0 if either controls is not allowed or there's a violation
if (violation || !controls_allowed) {
vw_desired_torque_last = 0;
vw_rt_torque_last = 0;
vw_ts_last = ts;
volkswagen_desired_torque_last = 0;
volkswagen_rt_torque_last = 0;
volkswagen_ts_last = ts;
}

if (violation) {
tx = 0;
}
}

// FORCE CANCEL: ensuring that only the cancel button press is sent when controls are off.
// This avoids unintended engagements while still allowing resume spam
if ((bus == 2) && (addr == MSG_GRA_ACC_01) && !controls_allowed) {
// disallow resume and set: bits 16 and 19
if ((GET_BYTE(to_send, 2) & 0x9) != 0) {
tx = 0;
}
}

// 1 allows the message through
return tx;
}
Expand All @@ -113,18 +135,13 @@ static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {

// NOTE: Will need refactoring for other bus layouts, such as no-forwarding at camera or J533 running-gear CAN

switch(bus_num) {
switch (bus_num) {
case 0:
if(addr == MSG_GRA_ACC_01) {
// OP intercepts, filters, and updates the cruise-control button messages before they reach the ACC radar.
bus_fwd = -1;
} else {
// Forward all remaining traffic from J533 gateway to Extended CAN devices.
bus_fwd = 2;
}
// Forward all traffic from J533 gateway to Extended CAN devices.
bus_fwd = 2;
break;
case 2:
if((addr == MSG_HCA_01) || (addr == MSG_LDW_02)) {
if ((addr == MSG_HCA_01) || (addr == MSG_LDW_02)) {
// OP takes control of the Heading Control Assist and Lane Departure Warning messages from the camera.
bus_fwd = -1;
} else {
Expand Down
1 change: 1 addition & 0 deletions tests/safety/libpandasafety_py.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,7 @@
void set_volkswagen_desired_torque_last(int t);
void set_volkswagen_rt_torque_last(int t);
void set_volkswagen_torque_driver(int min, int max);
int get_volkswagen_gas_prev(void);
""")

Expand Down
22 changes: 13 additions & 9 deletions tests/safety/test.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -163,8 +163,8 @@ void set_subaru_torque_driver(int min, int max){
}

void set_volkswagen_torque_driver(int min, int max){
vw_torque_driver.min = min;
vw_torque_driver.max = max;
volkswagen_torque_driver.min = min;
volkswagen_torque_driver.max = max;
}

int get_chrysler_torque_meas_min(void){
Expand Down Expand Up @@ -212,7 +212,7 @@ void set_subaru_rt_torque_last(int t){
}

void set_volkswagen_rt_torque_last(int t){
vw_rt_torque_last = t;
volkswagen_rt_torque_last = t;
}

void set_toyota_desired_torque_last(int t){
Expand Down Expand Up @@ -240,7 +240,11 @@ void set_subaru_desired_torque_last(int t){
}

void set_volkswagen_desired_torque_last(int t){
vw_desired_torque_last = t;
volkswagen_desired_torque_last = t;
}

int get_volkswagen_gas_prev(void){
return volkswagen_gas_prev;
}

bool get_honda_moving(void){
Expand Down Expand Up @@ -338,11 +342,11 @@ void init_tests_subaru(void){

void init_tests_volkswagen(void){
init_tests();
vw_torque_driver.min = 0;
vw_torque_driver.max = 0;
vw_desired_torque_last = 0;
vw_rt_torque_last = 0;
vw_ts_last = 0;
volkswagen_torque_driver.min = 0;
volkswagen_torque_driver.max = 0;
volkswagen_desired_torque_last = 0;
volkswagen_rt_torque_last = 0;
volkswagen_ts_last = 0;
set_timer(0);
}

Expand Down
55 changes: 54 additions & 1 deletion tests/safety/test_volkswagen.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,26 @@ def _torque_msg(self, torque):
to_send[0].RDLR |= 0x1 << 31
return to_send

def _gas_msg(self, gas):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x121 << 21
to_send[0].RDLR = (gas & 0xFF) << 12

return to_send

def _button_msg(self, bit):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x12B << 21
to_send[0].RDLR = 1 << bit
to_send[0].RDTR = 2 << 4

return to_send

def test_prev_gas(self):
for g in range(0, 256):
self.safety.safety_rx_hook(self._gas_msg(g))
self.assertEqual(g, self.safety.get_volkswagen_gas_prev())

def test_default_controls_not_allowed(self):
self.assertFalse(self.safety.get_controls_allowed())

Expand All @@ -78,6 +98,27 @@ def test_disable_control_allowed_from_cruise(self):
self.safety.safety_rx_hook(to_push)
self.assertFalse(self.safety.get_controls_allowed())

def test_disengage_on_gas(self):
for long_controls_allowed in [0, 1]:
self.safety.set_long_controls_allowed(long_controls_allowed)
self.safety.safety_rx_hook(self._gas_msg(0))
self.safety.set_controls_allowed(True)
self.safety.safety_rx_hook(self._gas_msg(1))
if long_controls_allowed:
self.assertFalse(self.safety.get_controls_allowed())
else:
self.assertTrue(self.safety.get_controls_allowed())
self.safety.set_long_controls_allowed(True)

def test_allow_engage_with_gas_pressed(self):
self.safety.safety_rx_hook(self._gas_msg(1))
self.safety.set_controls_allowed(True)
self.safety.safety_rx_hook(self._gas_msg(1))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.safety_rx_hook(self._gas_msg(1))
self.assertTrue(self.safety.get_controls_allowed())


def test_steer_safety_check(self):
for enabled in [0, 1]:
for t in range(-500, 500):
Expand All @@ -94,6 +135,18 @@ def test_manually_enable_controls_allowed(self):
self.safety.set_controls_allowed(0)
self.assertFalse(self.safety.get_controls_allowed())

def test_spam_cancel_safety_check(self):
BIT_CANCEL = 13
BIT_RESUME = 19
BIT_SET = 16
self.safety.set_controls_allowed(0)
self.assertTrue(self.safety.safety_tx_hook(self._button_msg(BIT_CANCEL)))
self.assertFalse(self.safety.safety_tx_hook(self._button_msg(BIT_RESUME)))
self.assertFalse(self.safety.safety_tx_hook(self._button_msg(BIT_SET)))
# do not block resume if we are engaged already
self.safety.set_controls_allowed(1)
self.assertTrue(self.safety.safety_tx_hook(self._button_msg(BIT_RESUME)))

def test_non_realtime_limit_up(self):
self.safety.set_volkswagen_torque_driver(0, 0)
self.safety.set_controls_allowed(True)
Expand Down Expand Up @@ -175,7 +228,7 @@ def test_realtime_limits(self):
def test_fwd_hook(self):
buss = list(range(0x0, 0x2))
msgs = list(range(0x1, 0x800))
blocked_msgs_0to2 = [0x12B]
blocked_msgs_0to2 = []
blocked_msgs_2to0 = [0x122, 0x397]
for b in buss:
for m in msgs:
Expand Down

0 comments on commit 48d0d0c

Please sign in to comment.