Skip to content

Commit

Permalink
Volkswagen safety updates: Phase 1 (#444)
Browse files Browse the repository at this point in the history 
* Checkpoint Panda refactoring updates

* Rename MQB safety tests in preparation for PQ

* Refactor MQB tests, add missing torque test

* Bring in MQB init without CRC LUT setup

* Fix to ACC_06 test case

* Fix to ACC_06 test case

* Tweak comment for clarity

* Drop superfluous return
  • Loading branch information
@jyoung8607
jyoung8607 committed Feb 19, 2020
1 parent 769ade0 commit ccf75c4
Show file tree
Hide file tree
Showing 7 changed files with 222 additions and 166 deletions.
4 changes: 2 additions & 2 deletions board/safety.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@
#define SAFETY_TESLA 10U
#define SAFETY_SUBARU 11U
#define SAFETY_MAZDA 13U
#define SAFETY_VOLKSWAGEN 15U
#define SAFETY_VOLKSWAGEN_MQB 15U
#define SAFETY_TOYOTA_IPAS 16U
#define SAFETY_ALLOUTPUT 17U
#define SAFETY_GM_ASCM 18U
Expand Down Expand Up @@ -185,7 +185,7 @@ const safety_hook_config safety_hook_registry[] = {
{SAFETY_CHRYSLER, &chrysler_hooks},
{SAFETY_SUBARU, &subaru_hooks},
{SAFETY_MAZDA, &mazda_hooks},
{SAFETY_VOLKSWAGEN, &volkswagen_hooks},
{SAFETY_VOLKSWAGEN_MQB, &volkswagen_mqb_hooks},
{SAFETY_NOOUTPUT, &nooutput_hooks},
#ifdef ALLOW_DEBUG
{SAFETY_CADILLAC, &cadillac_hooks},
Expand Down
235 changes: 124 additions & 111 deletions board/safety/safety_volkswagen.h
View file
Original file line number Diff line number Diff line change
@@ -1,142 +1,156 @@
// 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
#define MSG_LDW_02 0x397

// Safety-relevant steering constants for Volkswagen
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_MAX_RATE_UP = 4; // 2.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
const int VOLKSWAGEN_MAX_RATE_DOWN = 10; // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
const int VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE = 80;
const int VOLKSWAGEN_DRIVER_TORQUE_FACTOR = 3;

// MSG_GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
const AddrBus VOLKSWAGEN_TX_MSGS[] = {{MSG_HCA_01, 0}, {MSG_GRA_ACC_01, 0}, {MSG_GRA_ACC_01, 2}, {MSG_LDW_02, 0}};
// Safety-relevant CAN messages for the Volkswagen MQB platform
#define MSG_EPS_01 0x09F // RX from EPS, for driver steering torque
#define MSG_MOTOR_20 0x121 // RX from ECU, for driver throttle input
#define MSG_ACC_06 0x122 // RX from ACC radar, for status and engagement
#define MSG_HCA_01 0x126 // TX by OP, Heading Control Assist steering torque
#define MSG_GRA_ACC_01 0x12B // TX by OP, ACC control buttons for cancel/resume
#define MSG_LDW_02 0x397 // TX by OP, Lane line recognition and text alerts

// Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
const AddrBus VOLKSWAGEN_MQB_TX_MSGS[] = {{MSG_HCA_01, 0}, {MSG_GRA_ACC_01, 0}, {MSG_GRA_ACC_01, 2}, {MSG_LDW_02, 0}};
const int VOLKSWAGEN_MQB_TX_MSGS_LEN = sizeof(VOLKSWAGEN_MQB_TX_MSGS) / sizeof(VOLKSWAGEN_MQB_TX_MSGS[0]);

// TODO: do checksum and counter checks
AddrCheckStruct volkswagen_rx_checks[] = {
AddrCheckStruct volkswagen_mqb_rx_checks[] = {
{.addr = {MSG_EPS_01}, .bus = 0, .expected_timestep = 10000U},
{.addr = {MSG_ACC_06}, .bus = 0, .expected_timestep = 20000U},
{.addr = {MSG_MOTOR_20}, .bus = 0, .expected_timestep = 20000U},
};
const int VOLKSWAGEN_MQB_RX_CHECKS_LEN = sizeof(volkswagen_mqb_rx_checks) / sizeof(volkswagen_mqb_rx_checks[0]);

const int VOLKSWAGEN_RX_CHECK_LEN = sizeof(volkswagen_rx_checks) / sizeof(volkswagen_rx_checks[0]);

struct sample_t volkswagen_torque_driver; // last few driver torques measured
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;
int volkswagen_torque_msg = 0;
int volkswagen_lane_msg = 0;

static void volkswagen_mqb_init(int16_t param) {
UNUSED(param);

static int volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
controls_allowed = false;
relay_malfunction = false;
volkswagen_torque_msg = MSG_HCA_01;
volkswagen_lane_msg = MSG_LDW_02;
}

static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

bool valid = addr_safety_check(to_push, volkswagen_rx_checks, VOLKSWAGEN_RX_CHECK_LEN,
bool valid = addr_safety_check(to_push, volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_RX_CHECKS_LEN,
NULL, NULL, NULL);

if (valid) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);

// Update driver input torque samples from EPS_01.Driver_Strain for absolute torque, and EPS_01.Driver_Strain_VZ
// for the direction.
if ((bus == 0) && (addr == MSG_EPS_01)) {
int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1F) << 8);
int sign = (GET_BYTE(to_push, 6) & 0x80) >> 7;
if (sign == 1) {
torque_driver_new *= -1;
}

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;
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)) {
controls_allowed = 0;
}
volkswagen_gas_prev = gas;
}

if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) {
relay_malfunction = true;
}
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);

// Update driver input torque samples
// Signal: EPS_01.Driver_Strain (absolute torque)
// Signal: EPS_01.Driver_Strain_VZ (direction)
if ((bus == 0) && (addr == MSG_EPS_01)) {
int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1F) << 8);
int sign = (GET_BYTE(to_push, 6) & 0x80) >> 7;
if (sign == 1) {
torque_driver_new *= -1;
}
update_sample(&volkswagen_torque_driver, torque_driver_new);
}

// Update ACC status from radar for controls-allowed state
// Signal: ACC_06.ACC_Status_ACC
if ((bus == 0) && (addr == MSG_ACC_06)) {
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
// Signal: Motor_20.MO_Fahrpedalrohwert_01
if ((bus == 0) && (addr == MSG_MOTOR_20)) {
int gas = (GET_BYTES_04(to_push) >> 12) & 0xFF;
if ((gas > 0) && (volkswagen_gas_prev == 0)) {
controls_allowed = 0;
}
volkswagen_gas_prev = gas;
}

// If there are HCA messages on bus 0 not sent by OP, there's a relay problem
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) {
relay_malfunction = true;
}
}
return valid;
}

static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
static bool volkswagen_steering_check(int desired_torque) {
bool violation = false;
uint32_t ts = TIM2->CNT;

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

// *** torque rate limit check ***
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, volkswagen_rt_torque_last, VOLKSWAGEN_MAX_RT_DELTA);

// every RT_INTERVAL set the new limits
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;
}
}

// no torque if controls is not allowed
if (!controls_allowed && (desired_torque != 0)) {
violation = true;
}

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

return violation;
}

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

if (!msg_allowed(addr, bus, VOLKSWAGEN_TX_MSGS, sizeof(VOLKSWAGEN_TX_MSGS)/sizeof(VOLKSWAGEN_TX_MSGS[0]))) {
tx = 0;
}

if (relay_malfunction) {
if (!msg_allowed(addr, bus, VOLKSWAGEN_MQB_TX_MSGS, VOLKSWAGEN_MQB_TX_MSGS_LEN) || relay_malfunction) {
tx = 0;
}

// Safety check for HCA_01 Heading Control Assist torque.
// Safety check for HCA_01 Heading Control Assist torque
// Signal: HCA_01.Assist_Torque (absolute torque)
// Signal: HCA_01.Assist_VZ (direction)
if (addr == MSG_HCA_01) {
bool violation = false;

int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x3F) << 8);
int sign = (GET_BYTE(to_send, 3) & 0x80) >> 7;
if (sign == 1) {
desired_torque *= -1;
}

uint32_t ts = TIM2->CNT;

if (controls_allowed) {

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

// *** torque rate limit check ***
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, volkswagen_rt_torque_last, VOLKSWAGEN_MAX_RT_DELTA);

// every RT_INTERVAL set the new limits
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;
}
}

// no torque if controls is not allowed
if (!controls_allowed && (desired_torque != 0)) {
violation = true;
}

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

if (violation) {
if (volkswagen_steering_check(desired_torque)) {
tx = 0;
}
}
Expand All @@ -158,38 +172,37 @@ static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int addr = GET_ADDR(to_fwd);
int bus_fwd = -1;

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

if (!relay_malfunction) {
switch (bus_num) {
case 0:
// Forward all traffic from J533 gateway to Extended CAN devices.
// Forward all traffic from the Extended CAN onward
bus_fwd = 2;
break;
case 2:
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.
if ((addr == volkswagen_torque_msg) || (addr == volkswagen_lane_msg)) {
// OP takes control of the Heading Control Assist and Lane Departure Warning messages from the camera
bus_fwd = -1;
} else {
// Forward all remaining traffic from Extended CAN devices to J533 gateway.
// Forward all remaining traffic from Extended CAN devices to J533 gateway
bus_fwd = 0;
}
break;
default:
// No other buses should be in use; fallback to do-not-forward.
// No other buses should be in use; fallback to do-not-forward
bus_fwd = -1;
break;
}
}
return bus_fwd;
}

const safety_hooks volkswagen_hooks = {
.init = nooutput_init,
.rx = volkswagen_rx_hook,
.tx = volkswagen_tx_hook,
// Volkswagen MQB platform
const safety_hooks volkswagen_mqb_hooks = {
.init = volkswagen_mqb_init,
.rx = volkswagen_mqb_rx_hook,
.tx = volkswagen_mqb_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
.fwd = volkswagen_fwd_hook,
.addr_check = volkswagen_rx_checks,
.addr_check_len = sizeof(volkswagen_rx_checks) / sizeof(volkswagen_rx_checks[0]),
.addr_check = volkswagen_mqb_rx_checks,
.addr_check_len = sizeof(volkswagen_mqb_rx_checks) / sizeof(volkswagen_mqb_rx_checks[0]),
};
2 changes: 1 addition & 1 deletion python/__init__.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@ class Panda(object):
SAFETY_TESLA = 10
SAFETY_SUBARU = 11
SAFETY_MAZDA = 13
SAFETY_VOLKSWAGEN = 15
SAFETY_VOLKSWAGEN_MQB = 15
SAFETY_TOYOTA_IPAS = 16
SAFETY_ALLOUTPUT = 17
SAFETY_GM_ASCM = 18
Expand Down
4 changes: 3 additions & 1 deletion tests/safety/libpandasafety_py.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,10 +91,12 @@
void set_subaru_torque_driver(int min, int max);
void init_tests_volkswagen(void);
int get_volkswagen_gas_prev(void);
int get_volkswagen_torque_driver_min(void);
int get_volkswagen_torque_driver_max(void);
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
8 changes: 8 additions & 0 deletions tests/safety/test.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -148,6 +148,14 @@ void set_volkswagen_torque_driver(int min, int max){
volkswagen_torque_driver.max = max;
}

int get_volkswagen_torque_driver_min(void){
return volkswagen_torque_driver.min;
}

int get_volkswagen_torque_driver_max(void){
return volkswagen_torque_driver.max;
}

int get_chrysler_torque_meas_min(void){
return chrysler_torque_meas.min;
}
Expand Down
Loading

0 comments on commit ccf75c4

Please sign in to comment.