mirror of https://github.com/commaai/panda.git
Volkswagen safety updates: Phase 1 (#444)
* 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
This commit is contained in:
Jason Young
GitHub
parent
769ade0511
commit
ccf75c456f
|
|
@ -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
|
||||
|
|
@ -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},
|
||||
|
|
|
|||
|
|
@ -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 int volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
static void volkswagen_mqb_init(int16_t param) {
|
||||
UNUSED(param);
|
||||
|
||||
bool valid = addr_safety_check(to_push, volkswagen_rx_checks, VOLKSWAGEN_RX_CHECK_LEN,
|
||||
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_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);
|
||||
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 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_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;
|
||||
}
|
||||
|
||||
// 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
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
// 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]))) {
|
||||
if (!msg_allowed(addr, bus, VOLKSWAGEN_MQB_TX_MSGS, VOLKSWAGEN_MQB_TX_MSGS_LEN) || relay_malfunction) {
|
||||
tx = 0;
|
||||
}
|
||||
|
||||
if (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;
|
||||
}
|
||||
}
|
||||
|
|
@ -158,25 +172,23 @@ 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;
|
||||
}
|
||||
|
|
@ -184,12 +196,13 @@ static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
|||
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]),
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -91,10 +91,12 @@ void set_subaru_rt_torque_last(int t);
|
|||
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);
|
||||
|
||||
""")
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -3,19 +3,27 @@ import unittest
|
|||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
|
||||
from panda.tests.safety.common import test_relay_malfunction, make_msg, \
|
||||
test_manually_enable_controls_allowed, test_spam_can_buses
|
||||
|
||||
MAX_RATE_UP = 4
|
||||
MAX_RATE_DOWN = 10
|
||||
MAX_STEER = 250
|
||||
|
||||
MAX_RT_DELTA = 75
|
||||
RT_INTERVAL = 250000
|
||||
|
||||
DRIVER_TORQUE_ALLOWANCE = 80
|
||||
DRIVER_TORQUE_FACTOR = 3
|
||||
|
||||
TX_MSGS = [[0x126, 0], [0x12B, 0], [0x12B, 2], [0x397, 0]]
|
||||
MSG_EPS_01 = 0x9F # RX from EPS, for driver steering torque
|
||||
MSG_ACC_06 = 0x122 # RX from ACC radar, for status and engagement
|
||||
MSG_MOTOR_20 = 0x121 # RX from ECU, for driver throttle input
|
||||
MSG_HCA_01 = 0x126 # TX by OP, Heading Control Assist steering torque
|
||||
MSG_GRA_ACC_01 = 0x12B # TX by OP, ACC control buttons for cancel/resume
|
||||
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
|
||||
TX_MSGS = [[MSG_HCA_01, 0], [MSG_GRA_ACC_01, 0], [MSG_GRA_ACC_01, 2], [MSG_LDW_02, 0]]
|
||||
|
||||
def sign(a):
|
||||
if a > 0:
|
||||
|
|
@ -23,40 +31,50 @@ def sign(a):
|
|||
else:
|
||||
return -1
|
||||
|
||||
class TestVolkswagenSafety(unittest.TestCase):
|
||||
class TestVolkswagenMqbSafety(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN, 0)
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN_MQB, 0)
|
||||
cls.safety.init_tests_volkswagen()
|
||||
|
||||
def _set_prev_torque(self, t):
|
||||
self.safety.set_volkswagen_desired_torque_last(t)
|
||||
self.safety.set_volkswagen_rt_torque_last(t)
|
||||
|
||||
def _torque_driver_msg(self, torque):
|
||||
to_send = make_msg(0, 0x9F)
|
||||
# Driver steering input torque
|
||||
def _eps_01_msg(self, torque):
|
||||
to_send = make_msg(0, MSG_EPS_01)
|
||||
t = abs(torque)
|
||||
to_send[0].RDHR = ((t & 0x1FFF) << 8)
|
||||
if torque < 0:
|
||||
to_send[0].RDHR |= 0x1 << 23
|
||||
return to_send
|
||||
|
||||
def _torque_msg(self, torque):
|
||||
to_send = make_msg(0, 0x126)
|
||||
# openpilot steering output torque
|
||||
def _hca_01_msg(self, torque):
|
||||
to_send = make_msg(0, MSG_HCA_01)
|
||||
t = abs(torque)
|
||||
to_send[0].RDLR = (t & 0xFFF) << 16
|
||||
if torque < 0:
|
||||
to_send[0].RDLR |= 0x1 << 31
|
||||
return to_send
|
||||
|
||||
def _gas_msg(self, gas):
|
||||
to_send = make_msg(0, 0x121)
|
||||
# ACC engagement status
|
||||
def _acc_06_msg(self, status):
|
||||
to_send = make_msg(0, MSG_ACC_06)
|
||||
to_send[0].RDHR = (status & 0x7) << 28
|
||||
return to_send
|
||||
|
||||
# Driver throttle input
|
||||
def _motor_20_msg(self, gas):
|
||||
to_send = make_msg(0, MSG_MOTOR_20)
|
||||
to_send[0].RDLR = (gas & 0xFF) << 12
|
||||
return to_send
|
||||
|
||||
def _button_msg(self, bit):
|
||||
to_send = make_msg(2, 0x12B)
|
||||
# Cruise control buttons
|
||||
def _gra_acc_01_msg(self, bit):
|
||||
to_send = make_msg(2, MSG_GRA_ACC_01)
|
||||
to_send[0].RDLR = 1 << bit
|
||||
return to_send
|
||||
|
||||
|
|
@ -64,52 +82,49 @@ class TestVolkswagenSafety(unittest.TestCase):
|
|||
test_spam_can_buses(self, TX_MSGS)
|
||||
|
||||
def test_relay_malfunction(self):
|
||||
test_relay_malfunction(self, 0x126)
|
||||
test_relay_malfunction(self, MSG_HCA_01)
|
||||
|
||||
def test_prev_gas(self):
|
||||
for g in range(0, 256):
|
||||
self.safety.safety_rx_hook(self._gas_msg(g))
|
||||
self.safety.safety_rx_hook(self._motor_20_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())
|
||||
|
||||
def test_enable_control_allowed_from_cruise(self):
|
||||
to_push = make_msg(0, 0x122)
|
||||
to_push[0].RDHR = 0x30000000
|
||||
self.safety.safety_rx_hook(to_push)
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.safety_rx_hook(self._acc_06_msg(3))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_disable_control_allowed_from_cruise(self):
|
||||
to_push = make_msg(0, 0x122)
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(to_push)
|
||||
self.safety.safety_rx_hook(self._acc_06_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_disengage_on_gas(self):
|
||||
self.safety.safety_rx_hook(self._gas_msg(0))
|
||||
self.safety.safety_rx_hook(self._motor_20_msg(0))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.safety.safety_rx_hook(self._motor_20_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_allow_engage_with_gas_pressed(self):
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.safety.safety_rx_hook(self._motor_20_msg(1))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.safety.safety_rx_hook(self._motor_20_msg(1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.safety.safety_rx_hook(self._motor_20_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):
|
||||
self.safety.set_controls_allowed(enabled)
|
||||
self._set_prev_torque(t)
|
||||
if abs(t) > MAX_STEER or (not enabled and abs(t) > 0):
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(t)))
|
||||
else:
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(t)))
|
||||
|
||||
def test_manually_enable_controls_allowed(self):
|
||||
test_manually_enable_controls_allowed(self)
|
||||
|
|
@ -119,27 +134,27 @@ class TestVolkswagenSafety(unittest.TestCase):
|
|||
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)))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_CANCEL)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_RESUME)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._gra_acc_01_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)))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_RESUME)))
|
||||
|
||||
def test_non_realtime_limit_up(self):
|
||||
self.safety.set_volkswagen_torque_driver(0, 0)
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
self._set_prev_torque(0)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP)))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(MAX_RATE_UP)))
|
||||
self._set_prev_torque(0)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP)))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(-MAX_RATE_UP)))
|
||||
|
||||
self._set_prev_torque(0)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(MAX_RATE_UP + 1)))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._set_prev_torque(0)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(-MAX_RATE_UP - 1)))
|
||||
|
||||
def test_non_realtime_limit_down(self):
|
||||
self.safety.set_volkswagen_torque_driver(0, 0)
|
||||
|
|
@ -153,10 +168,10 @@ class TestVolkswagenSafety(unittest.TestCase):
|
|||
t *= -sign
|
||||
self.safety.set_volkswagen_torque_driver(t, t)
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign)))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(MAX_STEER * sign)))
|
||||
|
||||
self.safety.set_volkswagen_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(-MAX_STEER)))
|
||||
|
||||
# spot check some individual cases
|
||||
for sign in [-1, 1]:
|
||||
|
|
@ -165,21 +180,20 @@ class TestVolkswagenSafety(unittest.TestCase):
|
|||
delta = 1 * sign
|
||||
self._set_prev_torque(torque_desired)
|
||||
self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired)))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(torque_desired)))
|
||||
self._set_prev_torque(torque_desired + delta)
|
||||
self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(torque_desired + delta)))
|
||||
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0)))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(0)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
|
||||
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
|
||||
|
||||
def test_realtime_limits(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
|
@ -190,25 +204,44 @@ class TestVolkswagenSafety(unittest.TestCase):
|
|||
self.safety.set_volkswagen_torque_driver(0, 0)
|
||||
for t in np.arange(0, MAX_RT_DELTA, 1):
|
||||
t *= sign
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(t)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
|
||||
self._set_prev_torque(0)
|
||||
for t in np.arange(0, MAX_RT_DELTA, 1):
|
||||
t *= sign
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(t)))
|
||||
|
||||
# Increase timer to update rt_torque_last
|
||||
self.safety.set_timer(RT_INTERVAL + 1)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(sign * (MAX_RT_DELTA - 1))))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
|
||||
def test_torque_measurements(self):
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(50))
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(-50))
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(0))
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(0))
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(0))
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(0))
|
||||
|
||||
self.assertEqual(-50, self.safety.get_volkswagen_torque_driver_min())
|
||||
self.assertEqual(50, self.safety.get_volkswagen_torque_driver_max())
|
||||
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(0))
|
||||
self.assertEqual(0, self.safety.get_volkswagen_torque_driver_max())
|
||||
self.assertEqual(-50, self.safety.get_volkswagen_torque_driver_min())
|
||||
|
||||
self.safety.safety_rx_hook(self._eps_01_msg(0))
|
||||
self.assertEqual(0, self.safety.get_volkswagen_torque_driver_max())
|
||||
self.assertEqual(0, self.safety.get_volkswagen_torque_driver_min())
|
||||
|
||||
|
||||
def test_fwd_hook(self):
|
||||
buss = list(range(0x0, 0x3))
|
||||
msgs = list(range(0x1, 0x800))
|
||||
blocked_msgs_0to2 = []
|
||||
blocked_msgs_2to0 = [0x126, 0x397]
|
||||
blocked_msgs_2to0 = [MSG_HCA_01, MSG_LDW_02]
|
||||
for b in buss:
|
||||
for m in msgs:
|
||||
if b == 0:
|
||||
|
|
@ -18,7 +18,7 @@ logs = [
|
|||
("02ec6bea180a4d36|2019-04-17--11-21-35.bz2", Panda.SAFETY_HYUNDAI, 0), # HYUNDAI.SANTA_FE
|
||||
("6fb4948a7ebe670e|2019-11-12--00-35-53.bz2", Panda.SAFETY_CHRYSLER, 0), # CHRYSLER.PACIFICA_2018_HYBRID
|
||||
("791340bc01ed993d|2019-04-08--10-26-00.bz2", Panda.SAFETY_SUBARU, 0), # SUBARU.IMPREZA
|
||||
("b0c9d2329ad1606b|2019-11-17--17-06-13.bz2", Panda.SAFETY_VOLKSWAGEN, 0), # VOLKSWAGEN.GOLF
|
||||
("b0c9d2329ad1606b|2019-11-17--17-06-13.bz2", Panda.SAFETY_VOLKSWAGEN_MQB, 0), # VOLKSWAGEN.GOLF
|
||||
]
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
|||
Loading…
Reference in New Issue