300 lines
12 KiB
C
300 lines
12 KiB
C
#pragma once
|
|
|
|
#include "iqdbc/safety/declarations.h"
|
|
#include "iqdbc/safety/modes/volkswagen_common.h"
|
|
|
|
#define MSG_ESC_51 0xFCU
|
|
#define MSG_Motor_54 0x14CU // RX from ECU; CRC table entry kept, not in rx_checks (Motor_51 used for gas)
|
|
#define MSG_HCA_03 0x303U
|
|
#define MSG_QFK_01 0x13DU
|
|
#define MSG_MEB_ACC_01 0x300U
|
|
#define MSG_ACC_18 0x14DU
|
|
#define MSG_Motor_51 0x10BU
|
|
#define MSG_TA_01 0x26BU
|
|
#define MSG_EA_01 0x1A4U
|
|
#define MSG_EA_02 0x1F0U
|
|
#define MSG_KLR_01 0x25DU
|
|
#define MSG_AWV_03 0xDBU // TX by OP (camera harness): AEB control replacement for disabled radar
|
|
#define MSG_MEB_DISTANCE_01 0x24FU // TX by OP (camera harness): empty radar object list for disabled radar
|
|
#define MSG_UDS_FUNCTIONAL 0x700U // TX by OP (camera harness): TesterPresent keepalive for radar programming session
|
|
|
|
static uint32_t volkswagen_meb_compute_crc(const CANPacket_t *msg) {
|
|
const int len = GET_LEN(msg);
|
|
|
|
uint8_t crc = 0xFFU;
|
|
for (int i = 1; i < len; i++) {
|
|
crc ^= (uint8_t)msg->data[i];
|
|
crc = volkswagen_crc8_lut_8h2f[crc];
|
|
}
|
|
|
|
const uint8_t counter = volkswagen_mqb_meb_get_counter(msg);
|
|
if (msg->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 (msg->addr == MSG_GRA_ACC_01) {
|
|
crc ^= (uint8_t[]){0x6A,0x38,0xB4,0x27,0x22,0xEF,0xE1,0xBB,0xF8,0x80,0x84,0x49,0xC7,0x9E,0x1E,0x2B}[counter];
|
|
} else if (msg->addr == MSG_QFK_01) {
|
|
crc ^= (uint8_t[]){0x20,0xCA,0x68,0xD5,0x1B,0x31,0xE2,0xDA,0x08,0x0A,0xD4,0xDE,0x9C,0xE4,0x35,0x5B}[counter];
|
|
} else if (msg->addr == MSG_ESC_51) {
|
|
crc ^= (uint8_t[]){0x77,0x5C,0xA0,0x89,0x4B,0x7C,0xBB,0xD6,0x1F,0x6C,0x4F,0xF6,0x20,0x2B,0x43,0xDD}[counter];
|
|
} else if (msg->addr == MSG_Motor_54) {
|
|
crc ^= (uint8_t[]){0x16,0x35,0x59,0x15,0x9A,0x2A,0x97,0xB8,0x0E,0x4E,0x30,0xCC,0xB3,0x07,0x01,0xAD}[counter];
|
|
} else if (msg->addr == MSG_Motor_51) {
|
|
crc ^= (uint8_t[]){0x77,0x5C,0xA0,0x89,0x4B,0x7C,0xBB,0xD6,0x1F,0x6C,0x4F,0xF6,0x20,0x2B,0x43,0xDD}[counter];
|
|
} else if (msg->addr == MSG_MOTOR_14) {
|
|
crc ^= (uint8_t[]){0x1F,0x28,0xC6,0x85,0xE6,0xF8,0xB0,0x19,0x5B,0x64,0x35,0x21,0xE4,0xF7,0x9C,0x24}[counter];
|
|
} else if (msg->addr == MSG_KLR_01) {
|
|
crc ^= (uint8_t[]){0xDA,0x6B,0x0E,0xB2,0x78,0xBD,0x5A,0x81,0x7B,0xD6,0x41,0x39,0x76,0xB6,0xD7,0x35}[counter];
|
|
} else if (msg->addr == MSG_EA_02) {
|
|
crc ^= (uint8_t[]){0x2F,0x3C,0x22,0x60,0x18,0xEB,0x63,0x76,0xC5,0x91,0x0F,0x27,0x34,0x04,0x7F,0x02}[counter];
|
|
}
|
|
crc = volkswagen_crc8_lut_8h2f[crc];
|
|
|
|
return (uint8_t)(crc ^ 0xFFU);
|
|
}
|
|
|
|
static uint32_t volkswagen_meb_gen2_compute_crc(const CANPacket_t *msg) {
|
|
if (!volkswagen_alt_crc_variant_1) {
|
|
return volkswagen_meb_compute_crc(msg);
|
|
}
|
|
|
|
int len = GET_LEN(msg);
|
|
if (msg->addr == MSG_QFK_01) {
|
|
len = 28;
|
|
} else if (msg->addr == MSG_ESC_51) {
|
|
len = 60;
|
|
} else if (msg->addr == MSG_Motor_51) {
|
|
len = 44;
|
|
} else {
|
|
return volkswagen_meb_compute_crc(msg);
|
|
}
|
|
|
|
uint8_t crc = 0xFFU;
|
|
for (int i = 1; i < len; i++) {
|
|
crc ^= (uint8_t)msg->data[i];
|
|
crc = volkswagen_crc8_lut_8h2f[crc];
|
|
}
|
|
|
|
const uint8_t counter = volkswagen_mqb_meb_get_counter(msg);
|
|
if (msg->addr == MSG_QFK_01) {
|
|
crc ^= (uint8_t[]){0x18,0x71,0x10,0x8D,0xD7,0xAA,0xB0,0x78,0xAC,0x12,0xAE,0x0C,0xDD,0xF1,0x85,0x68}[counter];
|
|
} else if (msg->addr == MSG_ESC_51) {
|
|
crc ^= (uint8_t[]){0x69,0xDC,0xF9,0x64,0x6A,0xCE,0x55,0x2C,0xC4,0x38,0x8F,0xD1,0xC6,0x43,0xB4,0xB1}[counter];
|
|
} else if (msg->addr == MSG_Motor_51) {
|
|
crc ^= (uint8_t[]){0x2C,0xB1,0x1A,0x75,0xBB,0x65,0x79,0x47,0x81,0x2B,0xCC,0x96,0x17,0xDB,0xC0,0x94}[counter];
|
|
} else {
|
|
return volkswagen_meb_compute_crc(msg);
|
|
}
|
|
|
|
crc = (uint8_t)(volkswagen_crc8_lut_8h2f[crc] ^ 0xFFU);
|
|
if (crc != msg->data[0]) {
|
|
return volkswagen_meb_compute_crc(msg);
|
|
}
|
|
return (uint8_t)crc;
|
|
}
|
|
|
|
static safety_config volkswagen_meb_init(uint16_t param) {
|
|
static const CanMsg VOLKSWAGEN_MEB_STOCK_TX_MSGS[] = {
|
|
{MSG_HCA_03, 0, 24, .check_relay = true},
|
|
{MSG_GRA_ACC_01, 0, 8, .check_relay = false},
|
|
{MSG_EA_01, 0, 8, .check_relay = false},
|
|
{MSG_EA_02, 0, 8, .check_relay = true},
|
|
{MSG_KLR_01, 0, 8, .check_relay = false},
|
|
{MSG_KLR_01, 2, 8, .check_relay = true},
|
|
{MSG_GRA_ACC_01, 2, 8, .check_relay = false},
|
|
{MSG_LDW_02, 0, 8, .check_relay = true},
|
|
};
|
|
|
|
static const CanMsg VOLKSWAGEN_MEB_LONG_TX_MSGS[] = {
|
|
{MSG_HCA_03, 0, 24, .check_relay = true},
|
|
{MSG_MEB_ACC_01, 0, 48, .check_relay = true},
|
|
{MSG_ACC_18, 0, 32, .check_relay = true},
|
|
{MSG_EA_01, 0, 8, .check_relay = false},
|
|
{MSG_EA_02, 0, 8, .check_relay = true},
|
|
{MSG_KLR_01, 0, 8, .check_relay = false},
|
|
{MSG_KLR_01, 2, 8, .check_relay = true},
|
|
{MSG_LDW_02, 0, 8, .check_relay = true},
|
|
{MSG_TA_01, 0, 8, .check_relay = true},
|
|
// Camera harness radar-disable replacement messages
|
|
{MSG_AWV_03, 0, 48, .check_relay = false}, // AEB control (replaces radar AWV_03 output)
|
|
{MSG_MEB_DISTANCE_01, 0, 64, .check_relay = false}, // Empty radar object list (replaces Strukturen_01)
|
|
{MSG_UDS_FUNCTIONAL, 0, 8, .check_relay = false}, // TesterPresent keepalive to 0x700 (holds radar in programming session)
|
|
};
|
|
|
|
// Motor_54 removed: gas detection uses Motor_51 (Motor_54 has unreliable offset on MQBevo)
|
|
static RxCheck volkswagen_meb_rx_checks[] = {
|
|
{.msg = {{MSG_LH_EPS_03, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_MOTOR_14, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_GRA_ACC_01, 0, 8, 33U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_QFK_01, 0, 32, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_Motor_51, 0, 32, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_ESC_51, 0, 48, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
};
|
|
|
|
// Gen2: Motor_51 and ESC_51 have larger message lengths (more signals)
|
|
static RxCheck volkswagen_meb_gen2_rx_checks[] = {
|
|
{.msg = {{MSG_LH_EPS_03, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_MOTOR_14, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_GRA_ACC_01, 0, 8, 33U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_QFK_01, 0, 32, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_Motor_51, 0, 48, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_ESC_51, 0, 64, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
};
|
|
|
|
volkswagen_common_init();
|
|
|
|
volkswagen_alt_crc_variant_1 = GET_FLAG(param, FLAG_VOLKSWAGEN_ALT_CRC_VARIANT_1);
|
|
volkswagen_no_gas_offset = GET_FLAG(param, FLAG_VOLKSWAGEN_NO_GAS_OFFSET);
|
|
|
|
#ifdef ALLOW_DEBUG
|
|
volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL);
|
|
volkswagen_allow_long_accel_with_gas_pressed = GET_FLAG(param, FLAG_VOLKSWAGEN_ALLOW_LONG_ACCEL_WITH_GAS_PRESSED);
|
|
#else
|
|
SAFETY_UNUSED(param);
|
|
#endif
|
|
|
|
safety_config ret;
|
|
if (volkswagen_longitudinal) {
|
|
SET_TX_MSGS(VOLKSWAGEN_MEB_LONG_TX_MSGS, ret);
|
|
} else {
|
|
SET_TX_MSGS(VOLKSWAGEN_MEB_STOCK_TX_MSGS, ret);
|
|
}
|
|
|
|
if (volkswagen_alt_crc_variant_1) {
|
|
SET_RX_CHECKS(volkswagen_meb_gen2_rx_checks, ret);
|
|
} else {
|
|
SET_RX_CHECKS(volkswagen_meb_rx_checks, ret);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
static void volkswagen_meb_rx_hook(const CANPacket_t *msg) {
|
|
if (msg->bus != 0U) {
|
|
return;
|
|
}
|
|
|
|
if (msg->addr == MSG_ESC_51) {
|
|
const uint32_t fr = msg->data[10] | (msg->data[11] << 8);
|
|
const uint32_t rl = msg->data[12] | (msg->data[13] << 8);
|
|
const uint32_t rr = msg->data[14] | (msg->data[15] << 8);
|
|
const uint32_t fl = msg->data[8] | (msg->data[9] << 8);
|
|
|
|
vehicle_moving = (fr > 0U) || (rr > 0U) || (rl > 0U) || (fl > 0U);
|
|
UPDATE_VEHICLE_SPEED(((fr + rr + rl + fl) / 4U) * 0.0075f / 3.6f);
|
|
}
|
|
|
|
if (msg->addr == MSG_QFK_01) {
|
|
int current_curvature = ((msg->data[6] & 0x7FU) << 8) | msg->data[5];
|
|
const bool current_curvature_sign = GET_BIT(msg, 55U);
|
|
if (!current_curvature_sign) {
|
|
current_curvature *= -1;
|
|
}
|
|
update_sample(&angle_meas, current_curvature);
|
|
}
|
|
|
|
if (msg->addr == MSG_LH_EPS_03) {
|
|
update_sample(&torque_driver, volkswagen_mlb_mqb_driver_input_torque(msg));
|
|
}
|
|
|
|
if (msg->addr == MSG_Motor_51) {
|
|
const int acc_status = ((msg->data[11] >> 0) & 0x07U);
|
|
const bool cruise_engaged = (acc_status == 3) || (acc_status == 4) || (acc_status == 5);
|
|
acc_main_on = cruise_engaged || (acc_status == 2);
|
|
|
|
if (!volkswagen_longitudinal) {
|
|
pcm_cruise_check(cruise_engaged);
|
|
}
|
|
|
|
if (!acc_main_on) {
|
|
controls_allowed = false;
|
|
}
|
|
|
|
// Gas detection from Motor_51 Accel_Pedal_Pressure signal (start bit 12, 9 bits, little-endian)
|
|
// Motor_54 avoided: its Accelerator_Pressure signal has unreliable offset on MQBevo
|
|
const int accel_pedal_value = ((msg->data[1] >> 4) & 0x0FU) | ((msg->data[2] & 0x1FU) << 4);
|
|
gas_pressed = accel_pedal_value > 0;
|
|
}
|
|
|
|
if (msg->addr == MSG_GRA_ACC_01) {
|
|
if (volkswagen_longitudinal) {
|
|
const bool set_button = GET_BIT(msg, 16U);
|
|
const bool resume_button = GET_BIT(msg, 19U);
|
|
if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) {
|
|
controls_allowed = acc_main_on;
|
|
}
|
|
volkswagen_set_button_prev = set_button;
|
|
volkswagen_resume_button_prev = resume_button;
|
|
}
|
|
|
|
if (GET_BIT(msg, 13U)) {
|
|
controls_allowed = false;
|
|
}
|
|
}
|
|
|
|
if (msg->addr == MSG_MOTOR_14) {
|
|
brake_pressed = GET_BIT(msg, 28U);
|
|
}
|
|
}
|
|
|
|
// Lateral limits for curvature-based steering (HCA_03)
|
|
// max_power matches 50% (~125/255) of the byte range, safely above Python's STEERING_POWER_MAX of 90
|
|
static const CurvatureSteeringLimits VOLKSWAGEN_MEB_STEERING_LIMITS = {
|
|
.max_curvature = 29105, // 0.195 rad/m
|
|
.curvature_to_can = 149253.7313f, // 1 / 6.7e-6 rad/m to CAN units
|
|
.send_rate = 0.02f,
|
|
.inactive_curvature_is_zero = true,
|
|
.max_power = 125, // ~50% of byte range; Python STEERING_POWER_MAX is 90
|
|
};
|
|
|
|
static bool volkswagen_meb_tx_hook(const CANPacket_t *msg) {
|
|
const LongitudinalLimits VOLKSWAGEN_MEB_LONG_LIMITS = {
|
|
.max_accel = 2000,
|
|
.min_accel = -3500,
|
|
.inactive_accel = 3010,
|
|
};
|
|
|
|
bool tx = true;
|
|
|
|
if (msg->addr == MSG_HCA_03) {
|
|
int desired_curvature_raw = GET_BYTES(msg, 3, 2) & 0x7FFFU;
|
|
const bool desired_curvature_sign = GET_BIT(msg, 39U);
|
|
if (!desired_curvature_sign) {
|
|
desired_curvature_raw *= -1;
|
|
}
|
|
|
|
const bool steer_req = (((msg->data[1] >> 4) & 0x0FU) == 4U);
|
|
const int steer_power = msg->data[2];
|
|
|
|
if (steer_power_cmd_checks(steer_power, steer_req, VOLKSWAGEN_MEB_STEERING_LIMITS)) {
|
|
tx = true; // TODO: revert to tx = false once port is verified working
|
|
}
|
|
if (steer_curvature_cmd_checks_average(desired_curvature_raw, steer_req, VOLKSWAGEN_MEB_STEERING_LIMITS)) {
|
|
tx = true; // TODO: revert to tx = false once port is verified working
|
|
}
|
|
}
|
|
|
|
if (msg->addr == MSG_ACC_18) {
|
|
const int desired_accel = ((((msg->data[4] & 0x7U) << 8) | msg->data[3]) * 5U) - 7220U;
|
|
if (volkswagen_longitudinal_accel_checks(desired_accel, VOLKSWAGEN_MEB_LONG_LIMITS)) {
|
|
tx = true; // TODO: revert to tx = false once port is verified working
|
|
}
|
|
}
|
|
|
|
if ((msg->addr == MSG_GRA_ACC_01) && !controls_allowed) {
|
|
if ((msg->data[2] & 0x9U) != 0U) {
|
|
tx = false;
|
|
}
|
|
}
|
|
|
|
return tx;
|
|
}
|
|
|
|
const safety_hooks volkswagen_meb_hooks = {
|
|
.init = volkswagen_meb_init,
|
|
.rx = volkswagen_meb_rx_hook,
|
|
.tx = volkswagen_meb_tx_hook,
|
|
.get_counter = volkswagen_mqb_meb_get_counter,
|
|
.get_checksum = volkswagen_mqb_meb_get_checksum,
|
|
.compute_checksum = volkswagen_meb_gen2_compute_crc,
|
|
};
|