Skip to content

Commit

Permalink
Volkswagen safety updates: Phase 2 (commaai#445)
Browse files Browse the repository at this point in the history
* CRC and counter checks, standstill/brake checks

* Clean up a tsk_06 that snuck through

* Be consistent about how we call _msg_esp_05

* Reduce scope: haunted by the ghost of MISRA future

* Improved check/test for in-motion braking

* MISRA styling fix
  • Loading branch information
jyoung8607 authored Feb 20, 2020
1 parent b2ffaae commit 598074c
Show file tree
Hide file tree
Showing 7 changed files with 266 additions and 7 deletions.
15 changes: 15 additions & 0 deletions board/safety.h
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,21 @@ int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
return current_hooks->fwd(bus_num, to_fwd);
}

// Given a CRC-8 poly, generate a static lookup table to use with a fast CRC-8
// algorithm. Called at init time for safety modes using CRC-8.
void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]) {
for (int i = 0; i < 256; i++) {
uint8_t crc = i;
for (int j = 0; j < 8; j++) {
if ((crc & 0x80U) != 0U)
crc = (uint8_t)((crc << 1) ^ poly);
else
crc <<= 1;
}
crc_lut[i] = crc;
}
}

bool msg_allowed(int addr, int bus, const AddrBus addr_list[], int len) {
bool allowed = false;
for (int i = 0; i < len; i++) {
Expand Down
82 changes: 77 additions & 5 deletions board/safety/safety_volkswagen.h
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,9 @@ const int VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE = 80;
const int VOLKSWAGEN_DRIVER_TORQUE_FACTOR = 3;

// Safety-relevant CAN messages for the Volkswagen MQB platform
#define MSG_ESP_19 0x0B2 // RX from ABS, for wheel speeds
#define MSG_EPS_01 0x09F // RX from EPS, for driver steering torque
#define MSG_ESP_05 0x106 // RX from ABS, for brake switch state
#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
Expand All @@ -19,11 +21,12 @@ const int VOLKSWAGEN_DRIVER_TORQUE_FACTOR = 3;
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_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},
{.addr = {MSG_ESP_19}, .bus = 0, .check_checksum = false, .max_counter = 0U, .expected_timestep = 10000U},
{.addr = {MSG_EPS_01}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U},
{.addr = {MSG_ESP_05}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
{.addr = {MSG_MOTOR_20}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
{.addr = {MSG_ACC_06}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
};
const int VOLKSWAGEN_MQB_RX_CHECKS_LEN = sizeof(volkswagen_mqb_rx_checks) / sizeof(volkswagen_mqb_rx_checks[0]);

Expand All @@ -32,9 +35,56 @@ 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;
bool volkswagen_moving = false;
bool volkswagen_brake_pressed_prev = false;
int volkswagen_gas_prev = 0;
int volkswagen_torque_msg = 0;
int volkswagen_lane_msg = 0;
uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR


static uint8_t volkswagen_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
return (uint8_t)GET_BYTE(to_push, 0);
}

static uint8_t volkswagen_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
return (uint8_t)GET_BYTE(to_push, 1) & 0xFU;
}

static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) {
int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);

// This is CRC-8H2F/AUTOSAR with a twist. See the OpenDBC implementation
// of this algorithm for a version with explanatory comments.

uint8_t crc = 0xFFU;
for (int i = 1; i < len; i++) {
crc ^= (uint8_t)GET_BYTE(to_push, i);
crc = volkswagen_crc8_lut_8h2f[crc];
}

uint8_t counter = volkswagen_get_counter(to_push);
switch(addr) {
case MSG_EPS_01:
crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter];
break;
case MSG_ESP_05:
crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter];
break;
case MSG_MOTOR_20:
crc ^= (uint8_t[]){0xE9,0x65,0xAE,0x6B,0x7B,0x35,0xE5,0x5F,0x4E,0xC7,0x86,0xA2,0xBB,0xDD,0xEB,0xB4}[counter];
break;
case MSG_ACC_06:
crc ^= (uint8_t[]){0x37,0x7D,0xF3,0xA9,0x18,0x46,0x6D,0x4D,0x3D,0x71,0x92,0x9C,0xE5,0x32,0x10,0xB9}[counter];
break;
default: // Undefined CAN message, CRC check expected to fail
break;
}
crc = volkswagen_crc8_lut_8h2f[crc];

return crc ^ 0xFFU;
}

static void volkswagen_mqb_init(int16_t param) {
UNUSED(param);
Expand All @@ -43,17 +93,29 @@ static void volkswagen_mqb_init(int16_t param) {
relay_malfunction = false;
volkswagen_torque_msg = MSG_HCA_01;
volkswagen_lane_msg = MSG_LDW_02;
gen_crc_lookup_table(0x2F, volkswagen_crc8_lut_8h2f);
}

static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

bool valid = addr_safety_check(to_push, volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_RX_CHECKS_LEN,
NULL, NULL, NULL);
volkswagen_get_checksum, volkswagen_mqb_compute_crc, volkswagen_get_counter);

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

// Update in-motion state by sampling front wheel speeds
// Signal: ESP_19.ESP_VL_Radgeschw_02 (front left) in scaled km/h
// Signal: ESP_19.ESP_VR_Radgeschw_02 (front right) in scaled km/h
if ((bus == 0) && (addr == MSG_ESP_19)) {
int wheel_speed_fl = GET_BYTE(to_push, 4) | (GET_BYTE(to_push, 5) << 8);
int wheel_speed_fr = GET_BYTE(to_push, 6) | (GET_BYTE(to_push, 7) << 8);
// Check for average front speed in excess of 0.3m/s, 1.08km/h
// DBC speed scale 0.0075: 0.3m/s = 144, sum both wheels to compare
volkswagen_moving = (wheel_speed_fl + wheel_speed_fr) > 288;
}

// Update driver input torque samples
// Signal: EPS_01.Driver_Strain (absolute torque)
// Signal: EPS_01.Driver_Strain_VZ (direction)
Expand Down Expand Up @@ -83,6 +145,16 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
volkswagen_gas_prev = gas;
}

// Exit controls on rising edge of brake press
// Signal: ESP_05.ESP_Fahrer_bremst
if ((bus == 0) && (addr == MSG_ESP_05)) {
bool brake_pressed = (GET_BYTE(to_push, 3) & 0x4) >> 2;
if (brake_pressed && (!(volkswagen_brake_pressed_prev) || volkswagen_moving)) {
controls_allowed = 0;
}
volkswagen_brake_pressed_prev = brake_pressed;
}

// 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;
Expand Down
1 change: 1 addition & 0 deletions board/safety_declarations.h
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver,
const int MAX_ALLOWANCE, const int DRIVER_FACTOR);
bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
float interpolate(struct lookup_t xy, float x);
void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]);
bool msg_allowed(int addr, int bus, const AddrBus addr_list[], int len);
int get_addr_check_index(CAN_FIFOMailBox_TypeDef *to_push, AddrCheckStruct addr_list[], const int len);
void update_counter(AddrCheckStruct addr_list[], int index, uint8_t counter);
Expand Down
1 change: 1 addition & 0 deletions requirements.txt
Original file line number Diff line number Diff line change
Expand Up @@ -9,3 +9,4 @@ requests
flake8==3.7.9
pylint==2.4.3
cffi==1.11.4
crcmod
2 changes: 2 additions & 0 deletions tests/safety/libpandasafety_py.py
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,8 @@
int get_volkswagen_gas_prev(void);
int get_volkswagen_torque_driver_min(void);
int get_volkswagen_torque_driver_max(void);
bool get_volkswagen_moving(void);
bool get_volkswagen_brake_pressed_prev(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);
Expand Down
11 changes: 11 additions & 0 deletions tests/safety/test.c
Original file line number Diff line number Diff line change
Expand Up @@ -232,6 +232,14 @@ void set_volkswagen_desired_torque_last(int t){
volkswagen_desired_torque_last = t;
}

int get_volkswagen_moving(void){
return volkswagen_moving;
}

int get_volkswagen_brake_pressed_prev(void){
return volkswagen_brake_pressed_prev;
}

int get_volkswagen_gas_prev(void){
return volkswagen_gas_prev;
}
Expand Down Expand Up @@ -334,6 +342,9 @@ void init_tests_subaru(void){

void init_tests_volkswagen(void){
init_tests();
volkswagen_moving = false;
volkswagen_brake_pressed_prev = false;
volkswagen_gas_prev = 0;
volkswagen_torque_driver.min = 0;
volkswagen_torque_driver.max = 0;
volkswagen_desired_torque_last = 0;
Expand Down
Loading

0 comments on commit 598074c

Please sign in to comment.