Skip to content

Commit

Permalink
initial refactor, passing tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@jyoung8607
jyoung8607 committed Feb 13, 2024
1 parent f25f2bd commit 525e3e9
Show file tree
Hide file tree
Showing 4 changed files with 28 additions and 94 deletions.
10 changes: 5 additions & 5 deletions board/safety/safety_volkswagen_common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,15 +33,15 @@ bool volkswagen_brake_pedal_switch = false;
bool volkswagen_brake_pressure_detected = false;


static uint32_t volkswagen_mlb_mqb_get_checksum(CANPacket_t *to_push) {
static uint32_t volkswagen_mlb_mqb_get_checksum(const CANPacket_t *to_push) {
return (uint8_t)GET_BYTE(to_push, 0);
}

static uint8_t volkswagen_mlb_mqb_get_counter(CANPacket_t *to_push) {
static uint8_t volkswagen_mlb_mqb_get_counter(const CANPacket_t *to_push) {
return (uint8_t)GET_BYTE(to_push, 1) & 0xFU;
}

static uint32_t volkswagen_mlb_mqb_compute_checksum(CANPacket_t *to_push) {
static uint32_t volkswagen_mlb_mqb_compute_crc(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);

Expand Down Expand Up @@ -76,7 +76,7 @@ static uint32_t volkswagen_mlb_mqb_compute_checksum(CANPacket_t *to_push) {
return (uint8_t)(crc ^ 0xFFU);
}

static int volkswagen_mlb_mqb_driver_input_torque(CANPacket_t *to_push) {
static int volkswagen_mlb_mqb_driver_input_torque(const CANPacket_t *to_push) {
// Signal: LH_EPS_03.EPS_Lenkmoment (absolute torque)
// Signal: LH_EPS_03.EPS_VZ_Lenkmoment (direction)
int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1FU) << 8);
Expand All @@ -87,7 +87,7 @@ static int volkswagen_mlb_mqb_driver_input_torque(CANPacket_t *to_push) {
return torque_driver_new;
}

static bool volkswagen_mlb_mqb_brake_pressure_threshold(CANPacket_t *to_push) {
static bool volkswagen_mlb_mqb_brake_pressure_threshold(const CANPacket_t *to_push) {
// Signal: ESP_05.ESP_Fahrer_bremst (ESP detected driver brake pressure above threshold)
bool brake_pressure_threshold = GET_BIT(to_push, 26U);
return brake_pressure_threshold;
Expand Down
53 changes: 18 additions & 35 deletions board/safety/safety_volkswagen_mlb.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,44 +12,30 @@ const SteeringLimits VOLKSWAGEN_MLB_STEERING_LIMITS = {
.type = TorqueDriverLimited,
};

// longitudinal limits
// acceleration in m/s2 * 1000 to avoid floating point math
const LongitudinalLimits VOLKSWAGEN_MLB_LONG_LIMITS = {
.max_accel = 2000,
.min_accel = -3500,
.inactive_accel = 3010, // VW sends one increment above the max range when inactive
};

// Transmit of LS_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
const CanMsg VOLKSWAGEN_MLB_STOCK_TX_MSGS[] = {{MSG_HCA_01, 0, 8}, {MSG_LS_01, 0, 4}, {MSG_LS_01, 2, 4}, {MSG_LDW_02, 0, 8}};

AddrCheckStruct volkswagen_mlb_addr_checks[] = {
{.msg = {{MSG_ESP_03, 0, 8, .check_checksum = false, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{MSG_LH_EPS_03, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
{.msg = {{MSG_ESP_05, 0, 8, .check_checksum = false, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{MSG_TSK_02, 0, 8, .check_checksum = false, .max_counter = 15U, .expected_timestep = 30000U}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_03, 0, 8, .check_checksum = false, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
RxCheck volkswagen_mlb_rx_checks[] = {
{.msg = {{MSG_ESP_03, 0, 8, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{MSG_LH_EPS_03, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{MSG_ESP_05, 0, 8, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{MSG_TSK_02, 0, 8, .check_checksum = false, .max_counter = 15U, .frequency = 33U}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_03, 0, 8, .check_checksum = false, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
};
#define VOLKSWAGEN_MLB_ADDR_CHECKS_LEN (sizeof(volkswagen_mlb_addr_checks) / sizeof(volkswagen_mlb_addr_checks[0]))
addr_checks volkswagen_mlb_rx_checks = {volkswagen_mlb_addr_checks, VOLKSWAGEN_MLB_ADDR_CHECKS_LEN};


static const addr_checks* volkswagen_mlb_init(uint16_t param) {
static safety_config volkswagen_mlb_init(uint16_t param) {
UNUSED(param);

volkswagen_brake_pedal_switch = false;
volkswagen_brake_pressure_detected = false;

gen_crc_lookup_table_8(0x2F, volkswagen_crc8_lut_8h2f);
return &volkswagen_mlb_rx_checks;
return BUILD_SAFETY_CFG(volkswagen_mlb_rx_checks, VOLKSWAGEN_MLB_STOCK_TX_MSGS);
}

static int volkswagen_mlb_rx_hook(CANPacket_t *to_push) {

bool valid = addr_safety_check(to_push, &volkswagen_mlb_rx_checks, volkswagen_mlb_mqb_get_checksum,
volkswagen_mlb_mqb_compute_checksum, volkswagen_mlb_mqb_get_counter, NULL);

if (valid && (GET_BUS(to_push) == 0U)) {
static void volkswagen_mlb_rx_hook(const CANPacket_t *to_push) {
if (GET_BUS(to_push) == 0U) {
int addr = GET_ADDR(to_push);

// Check all wheel speeds for any movement
Expand Down Expand Up @@ -106,14 +92,11 @@ static int volkswagen_mlb_rx_hook(CANPacket_t *to_push) {

generic_rx_checks((addr == MSG_HCA_01));
}
return valid;
}

static int volkswagen_mlb_tx_hook(CANPacket_t *to_send) {
static bool volkswagen_mlb_tx_hook(const CANPacket_t *to_send) {
int addr = GET_ADDR(to_send);
int tx = 1;

tx = msg_allowed(to_send, VOLKSWAGEN_MLB_STOCK_TX_MSGS, sizeof(VOLKSWAGEN_MLB_STOCK_TX_MSGS) / sizeof(VOLKSWAGEN_MLB_STOCK_TX_MSGS[0]));
bool tx = true;

// Safety check for HCA_01 Heading Control Assist torque
// Signal: HCA_01.Assist_Torque (absolute torque)
Expand All @@ -126,7 +109,7 @@ static int volkswagen_mlb_tx_hook(CANPacket_t *to_send) {
}

if (steer_torque_cmd_checks(desired_torque, -1, VOLKSWAGEN_MLB_STEERING_LIMITS)) {
tx = 0;
tx = false;
}
}

Expand All @@ -135,16 +118,14 @@ static int volkswagen_mlb_tx_hook(CANPacket_t *to_send) {
if ((addr == MSG_LS_01) && !controls_allowed) {
// disallow resume and set: bits 16 and 19
if (GET_BIT(to_send, 16U) || GET_BIT(to_send, 19U)) {
tx = 0;
tx = false;
}
}

// 1 allows the message through
return tx;
}

static int volkswagen_mlb_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
int addr = GET_ADDR(to_fwd);
static int volkswagen_mlb_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;

switch (bus_num) {
Expand Down Expand Up @@ -174,6 +155,8 @@ const safety_hooks volkswagen_mlb_hooks = {
.init = volkswagen_mlb_init,
.rx = volkswagen_mlb_rx_hook,
.tx = volkswagen_mlb_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
.fwd = volkswagen_mlb_fwd_hook,
.get_counter = volkswagen_mlb_mqb_get_counter,
.get_checksum = volkswagen_mlb_mqb_get_checksum,
.compute_checksum = volkswagen_mlb_mqb_compute_crc,
};
57 changes: 3 additions & 54 deletions board/safety/safety_volkswagen_mqb.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,19 +20,6 @@ const LongitudinalLimits VOLKSWAGEN_MQB_LONG_LIMITS = {
.inactive_accel = 3010, // VW sends one increment above the max range when inactive
};

#define MSG_ESP_19 0x0B2 // RX from ABS, for wheel speeds
#define MSG_LH_EPS_03 0x09F // RX from EPS, for driver steering torque
#define MSG_ESP_05 0x106 // RX from ABS, for brake switch state
#define MSG_TSK_06 0x120 // RX from ECU, for ACC status from drivetrain coordinator
#define MSG_MOTOR_20 0x121 // RX from ECU, for driver throttle input
#define MSG_ACC_06 0x122 // TX by OP, ACC control instructions to the drivetrain coordinator
#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_ACC_07 0x12E // TX by OP, ACC control instructions to the drivetrain coordinator
#define MSG_ACC_02 0x30C // TX by OP, ACC HUD data to the instrument cluster
#define MSG_MOTOR_14 0x3BE // RX from ECU, for brake switch status
#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 CanMsg VOLKSWAGEN_MQB_STOCK_TX_MSGS[] = {{MSG_HCA_01, 0, 8}, {MSG_GRA_ACC_01, 0, 8}, {MSG_GRA_ACC_01, 2, 8},
{MSG_LDW_02, 0, 8}, {MSG_LH_EPS_03, 2, 8}};
Expand All @@ -52,44 +39,6 @@ uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F
bool volkswagen_mqb_brake_pedal_switch = false;
bool volkswagen_mqb_brake_pressure_detected = false;

static uint32_t volkswagen_mqb_get_checksum(const CANPacket_t *to_push) {
return (uint8_t)GET_BYTE(to_push, 0);
}

static uint8_t volkswagen_mqb_get_counter(const CANPacket_t *to_push) {
// MQB message counters are consistently found at LSB 8.
return (uint8_t)GET_BYTE(to_push, 1) & 0xFU;
}

static uint32_t volkswagen_mqb_compute_crc(const CANPacket_t *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_mqb_get_counter(to_push);
if (addr == MSG_LH_EPS_03) {
crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter];
} else if (addr == MSG_ESP_05) {
crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter];
} else if (addr == MSG_TSK_06) {
crc ^= (uint8_t[]){0xC4,0xE2,0x4F,0xE4,0xF8,0x2F,0x56,0x81,0x9F,0xE5,0x83,0x44,0x05,0x3F,0x97,0xDF}[counter];
} else if (addr == MSG_MOTOR_20) {
crc ^= (uint8_t[]){0xE9,0x65,0xAE,0x6B,0x7B,0x35,0xE5,0x5F,0x4E,0xC7,0x86,0xA2,0xBB,0xDD,0xEB,0xB4}[counter];
} else {
// Undefined CAN message, CRC check expected to fail
}
crc = volkswagen_crc8_lut_8h2f[crc];

return (uint8_t)(crc ^ 0xFFU);
}

static safety_config volkswagen_mqb_init(uint16_t param) {
UNUSED(param);
Expand Down Expand Up @@ -289,7 +238,7 @@ const safety_hooks volkswagen_mqb_hooks = {
.rx = volkswagen_mqb_rx_hook,
.tx = volkswagen_mqb_tx_hook,
.fwd = volkswagen_mqb_fwd_hook,
.get_counter = volkswagen_mqb_get_counter,
.get_checksum = volkswagen_mqb_get_checksum,
.compute_checksum = volkswagen_mqb_compute_crc,
.get_counter = volkswagen_mlb_mqb_get_counter,
.get_checksum = volkswagen_mlb_mqb_get_checksum,
.compute_checksum = volkswagen_mlb_mqb_compute_crc,
};
2 changes: 2 additions & 0 deletions tests/safety/test_volkswagen_mlb.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,8 @@ class TestVolkswagenMlbSafety(common.PandaSafetyTest, common.DriverTorqueSteerin
DRIVER_TORQUE_ALLOWANCE = 60
DRIVER_TORQUE_FACTOR = 3

NO_STEER_REQ_BIT = True # FIXME: check to see if this is correct

@classmethod
def setUpClass(cls):
if cls.__name__ == "TestVolkswagenMlbSafety":
Expand Down

0 comments on commit 525e3e9

Please sign in to comment.