228 lines
9.1 KiB
C
228 lines
9.1 KiB
C
#pragma once
|
|
|
|
#include "iqdbc/safety/declarations.h"
|
|
#include "iqdbc/safety/modes/volkswagen_common.h"
|
|
|
|
#define MSG_LENKHILFE_3 0x0D0U // RX from EPS, for steering angle and driver steering torque
|
|
#define MSG_HCA_1 0x0D2U // TX by OP, Heading Control Assist steering torque
|
|
#define MSG_BREMSE_1 0x1A0U // RX from ABS, for ego speed
|
|
#define MSG_MOTOR_2 0x288U // RX from ECU, for CC state and brake switch state
|
|
#define MSG_ACC_SYSTEM 0x368U // TX by OP, longitudinal acceleration controls
|
|
#define MSG_MOTOR_3 0x380U // RX from ECU, for driver throttle input
|
|
#define MSG_GRA_NEU 0x38AU // TX by OP, ACC control buttons for cancel/resume
|
|
#define MSG_MOTOR_5 0x480U // RX from ECU, for ACC main switch state
|
|
#define MSG_ACC_GRA_ANZEIGE 0x56AU // TX by OP, ACC HUD
|
|
#define MSG_LDW_1 0x5BEU // TX by OP, Lane line recognition and text alerts
|
|
#define MSG_BLINKMODI_02 0x0AAU // TX by OP, Blinker control
|
|
#define MSG_APD_1 0x3D6U // TX by OP, CarParams
|
|
#define MSG_PQ_SAFETY_1 0x6A0U // RX by OP
|
|
|
|
static bool volkswagen_pq_alc_module_present = false;
|
|
static bool volkswagen_pq_acc_tsk_ready = false;
|
|
|
|
static uint32_t volkswagen_pq_get_checksum(const CANPacket_t *msg) {
|
|
return (uint32_t)msg->data[(msg->addr == MSG_MOTOR_5) ? 7 : 0];
|
|
}
|
|
|
|
static uint8_t volkswagen_pq_get_counter(const CANPacket_t *msg) {
|
|
uint8_t counter = 0U;
|
|
|
|
if (msg->addr == MSG_LENKHILFE_3) {
|
|
counter = (uint8_t)(msg->data[1] & 0xF0U) >> 4;
|
|
} else if (msg->addr == MSG_GRA_NEU) {
|
|
counter = (uint8_t)(msg->data[2] & 0xF0U) >> 4;
|
|
} else {
|
|
}
|
|
|
|
return counter;
|
|
}
|
|
|
|
static uint32_t volkswagen_pq_compute_checksum(const CANPacket_t *msg) {
|
|
int len = GET_LEN(msg);
|
|
uint8_t checksum = 0U;
|
|
int checksum_byte = (msg->addr == MSG_MOTOR_5) ? 7 : 0;
|
|
|
|
// Simple XOR over the payload, except for the byte where the checksum lives.
|
|
for (int i = 0; i < len; i++) {
|
|
if (i != checksum_byte) {
|
|
checksum ^= (uint8_t)msg->data[i];
|
|
}
|
|
}
|
|
|
|
return checksum;
|
|
}
|
|
|
|
static safety_config volkswagen_pq_init(uint16_t param) {
|
|
// Transmit of GRA_Neu is allowed on bus 0/1/2 for compatibility across camera and gateway integrations
|
|
static const CanMsg VOLKSWAGEN_PQ_STOCK_TX_MSGS[] = {{MSG_HCA_1, 0, 5, .check_relay = true}, {MSG_LDW_1, 0, 8, .check_relay = true},
|
|
{MSG_GRA_NEU, 0, 4, .check_relay = false}, {MSG_GRA_NEU, 1, 4, .check_relay = false},
|
|
{MSG_GRA_NEU, 2, 4, .check_relay = false}, {MSG_BLINKMODI_02, 0, 8, .check_relay = false},
|
|
{MSG_APD_1, 1, 8, .check_relay = false}};
|
|
|
|
static const CanMsg VOLKSWAGEN_PQ_LONG_TX_MSGS[] = {{MSG_HCA_1, 0, 5, .check_relay = true}, {MSG_LDW_1, 0, 8, .check_relay = true},
|
|
{MSG_ACC_SYSTEM, 0, 8, .check_relay = true}, {MSG_ACC_GRA_ANZEIGE, 0, 8, .check_relay = true},
|
|
{MSG_GRA_NEU, 1, 4, .check_relay = false}, {MSG_GRA_NEU, 2, 4, .check_relay = true},
|
|
{MSG_BLINKMODI_02, 0, 8, .check_relay = false}, {MSG_MOTOR_2, 2, 8, .check_relay = true},
|
|
{MSG_APD_1, 1, 8, .check_relay = false}};
|
|
|
|
static RxCheck volkswagen_pq_rx_checks[] = {
|
|
{.msg = {{MSG_LENKHILFE_3, 0, 6, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_BREMSE_1, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_MOTOR_2, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_MOTOR_3, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_MOTOR_5, 0, 8, 50U, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_GRA_NEU, 0, 4, 30U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_PQ_SAFETY_1, 1, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
};
|
|
|
|
volkswagen_common_init();
|
|
volkswagen_pq_alc_module_present = GET_FLAG(param, FLAG_VOLKSWAGEN_PQ_ALC_MODULE);
|
|
volkswagen_pq_acc_tsk_ready = false;
|
|
|
|
#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 = volkswagen_longitudinal ? BUILD_SAFETY_CFG(volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_LONG_TX_MSGS) : \
|
|
BUILD_SAFETY_CFG(volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_STOCK_TX_MSGS);
|
|
if (!volkswagen_pq_alc_module_present) {
|
|
ret.rx_checks_len -= 1;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
static void volkswagen_pq_rx_hook(const CANPacket_t *msg) {
|
|
if (msg->bus == 0U) {
|
|
// Update in-motion state from speed value.
|
|
// Signal: Bremse_1.BR1_Rad_kmh
|
|
if (msg->addr == MSG_BREMSE_1) {
|
|
int speed = ((msg->data[2] & 0xFEU) >> 1) | (msg->data[3] << 7);
|
|
vehicle_moving = speed > 0;
|
|
}
|
|
|
|
// Update driver input torque samples
|
|
// Signal: Lenkhilfe_3.LH3_LM (absolute torque)
|
|
// Signal: Lenkhilfe_3.LH3_LMSign (direction)
|
|
if (msg->addr == MSG_LENKHILFE_3) {
|
|
int torque_driver_new = msg->data[2] | ((msg->data[3] & 0x3U) << 8);
|
|
int sign = (msg->data[3] & 0x4U) >> 2;
|
|
if (sign == 1) {
|
|
torque_driver_new *= -1;
|
|
}
|
|
update_sample(&torque_driver, torque_driver_new);
|
|
|
|
uint16_t angle_raw = (uint16_t)msg->data[4] | (((uint16_t)msg->data[5] & 0x0FU) << 8);
|
|
bool angle_sign = ((msg->data[5] >> 4) & 0x1U) != 0U;
|
|
float angle_deg = (float)angle_raw * 0.15f;
|
|
vw_iq_measured_angle_deg = angle_sign ? -angle_deg : angle_deg;
|
|
}
|
|
|
|
if (volkswagen_longitudinal) {
|
|
if (msg->addr == MSG_MOTOR_5) {
|
|
acc_main_on = GET_BIT(msg, 50U);
|
|
if (!acc_main_on && !volkswagen_pq_acc_tsk_ready) {
|
|
controls_allowed = false;
|
|
}
|
|
}
|
|
|
|
if (msg->addr == MSG_MOTOR_2) {
|
|
volkswagen_pq_acc_tsk_ready = GET_BIT(msg, 21U);
|
|
if (!acc_main_on && !volkswagen_pq_acc_tsk_ready) {
|
|
controls_allowed = false;
|
|
}
|
|
}
|
|
|
|
if (msg->addr == MSG_GRA_NEU) {
|
|
bool set_button = GET_BIT(msg, 16U);
|
|
bool resume_button = GET_BIT(msg, 17U);
|
|
if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) {
|
|
controls_allowed = acc_main_on || volkswagen_pq_acc_tsk_ready;
|
|
}
|
|
volkswagen_set_button_prev = set_button;
|
|
volkswagen_resume_button_prev = resume_button;
|
|
if (GET_BIT(msg, 9U)) {
|
|
controls_allowed = false;
|
|
}
|
|
}
|
|
} else {
|
|
if (msg->addr == MSG_MOTOR_2) {
|
|
int acc_status = (msg->data[2] & 0xC0U) >> 6;
|
|
bool cruise_engaged = (acc_status == 1) || (acc_status == 2);
|
|
pcm_cruise_check(cruise_engaged);
|
|
}
|
|
}
|
|
|
|
if (msg->addr == MSG_MOTOR_3) {
|
|
gas_pressed = (msg->data[2]);
|
|
}
|
|
|
|
if (msg->addr == MSG_MOTOR_2) {
|
|
brake_pressed = (msg->data[2] & 0x1U);
|
|
}
|
|
}
|
|
if (msg->bus == 1U) {
|
|
if (volkswagen_pq_alc_module_present && (msg->addr == MSG_PQ_SAFETY_1)) {
|
|
const uint16_t desired_angle_raw = (uint16_t)msg->data[6] | (((uint16_t)msg->data[7] & 0x7FU) << 8);
|
|
const bool desired_angle_sign = (msg->data[7] & 0x80U) != 0U;
|
|
const float desired_angle_deg = (float)desired_angle_raw * 0.04375f;
|
|
vw_iq_alc_desired_angle_deg = desired_angle_sign ? -desired_angle_deg : desired_angle_deg;
|
|
}
|
|
}
|
|
}
|
|
|
|
static bool volkswagen_pq_tx_hook(const CANPacket_t *msg) {
|
|
bool tx = true;
|
|
|
|
if (msg->addr == MSG_APD_1) {
|
|
volkswagen_iq_decode_apd(msg);
|
|
}
|
|
|
|
if (msg->addr == MSG_HCA_1) {
|
|
const uint8_t hca_status = (msg->data[1] >> 4) & 0x0FU;
|
|
|
|
if (volkswagen_pq_alc_module_present && (hca_status == 8U)) {
|
|
if (volkswagen_iq_alc_angle_accel_check(false)) {
|
|
tx = false;
|
|
}
|
|
} else if ((hca_status == 5U) || (hca_status == 7U)) {
|
|
int desired_torque = msg->data[2] | ((msg->data[3] & 0x7FU) << 8);
|
|
desired_torque = desired_torque / 32;
|
|
int sign = (msg->data[3] & 0x80U) >> 7;
|
|
if (sign == 1) {
|
|
desired_torque *= -1;
|
|
}
|
|
|
|
if (volkswagen_iq_lat_accel_torque_check(desired_torque)) {
|
|
tx = false;
|
|
}
|
|
} else {
|
|
}
|
|
}
|
|
|
|
if (msg->addr == MSG_ACC_SYSTEM) {
|
|
int desired_accel = ((((msg->data[4] & 0x7U) << 8) | msg->data[3]) * 5U) - 7220U;
|
|
if (volkswagen_iq_long_accel_check(desired_accel)) {
|
|
tx = false;
|
|
}
|
|
}
|
|
|
|
if ((msg->addr == MSG_GRA_NEU) && !controls_allowed) {
|
|
if (GET_BIT(msg, 16U) || GET_BIT(msg, 17U)) {
|
|
tx = false;
|
|
}
|
|
}
|
|
|
|
return tx;
|
|
}
|
|
|
|
const safety_hooks volkswagen_pq_hooks = {
|
|
.init = volkswagen_pq_init,
|
|
.rx = volkswagen_pq_rx_hook,
|
|
.tx = volkswagen_pq_tx_hook,
|
|
.get_counter = volkswagen_pq_get_counter,
|
|
.get_checksum = volkswagen_pq_get_checksum,
|
|
.compute_checksum = volkswagen_pq_compute_checksum,
|
|
};
|