IQ.Lvbs/IQ.Pilot
8
7
Fork 3
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Release Commit @ b581b58

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-07-21 17:51:52 -05:00
parent 78fc163202
commit ccb06b3624
125 changed files with 1991 additions and 6182 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
iqdbc_repo/iqdbc/safety/lateral.h
View File

@@ -6,7 +6,7 @@ static const float ISO_LATERAL_ACCEL = 3.0; // m/s^2
static const float EARTH_G = 9.81;
static const float AVERAGE_ROAD_ROLL = 0.06; // ~3.4 degrees, 6% superelevation
static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL + (EARTH_G * AVERAGE_ROAD_ROLL); // ~3.6 m/s^2
static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL + (EARTH_G * AVERAGE_ROAD_ROLL); // ~5.6 m/s^2
bool is_lat_active(void) {
return controls_allowed || aol_is_lateral_control_allowed_by_aol();
@@ -233,7 +233,7 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
if (limits.angle_is_curvature) {
// Limit to average banked road since safety doesn't have the roll
static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - (EARTH_G * AVERAGE_ROAD_ROLL); // ~2.4 m/s^2
static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - (EARTH_G * AVERAGE_ROAD_ROLL); // ~4.4 m/s^2
// Allow small tolerance by using minimum speed and rounding curvature up
const float speed_lower = SAFETY_MAX(vehicle_speed.min / VEHICLE_SPEED_FACTOR, 1.0);
@@ -301,7 +301,7 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co
// TODO: remove the inaccurate breakpoint angle limiting function above and always use this one
// Highway curves are rolled in the direction of the turn, add tolerance to compensate
static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL + (EARTH_G * AVERAGE_ROAD_ROLL); // ~3.6 m/s^2
static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL + (EARTH_G * AVERAGE_ROAD_ROLL); // ~5.6 m/s^2
// Lower than ISO 11270 lateral jerk limit, which is 5.0 m/s^3
static const float MAX_LATERAL_JERK = 3.0 + (EARTH_G * AVERAGE_ROAD_ROLL); // ~3.6 m/s^3

16
iqdbc_repo/iqdbc/safety/modes/hyundai_common.h
View File

@@ -18,10 +18,10 @@ enum {
};
enum {
HYUNDAI_PARAM_IQ_ESCC = 1,
HYUNDAI_PARAM_IQ_LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE = 2,
HYUNDAI_PARAM_IQ_HAS_LDA_BUTTON = 4,
HYUNDAI_PARAM_IQ_NON_SCC = 8,
HYUNDAI_PARAM_IQ_ESCC = 16,
HYUNDAI_PARAM_IQ_MAIN_BTN_LONG_TOGGLE = 32,
HYUNDAI_PARAM_IQ_HAS_LDA_BUTTON = 64,
HYUNDAI_PARAM_IQ_NON_SCC = 128,
};
// common state
@@ -53,8 +53,8 @@ bool hyundai_alt_limits_2 = false;
extern bool hyundai_escc;
bool hyundai_escc = false;
extern bool hyundai_longitudinal_main_cruise_toggleable;
bool hyundai_longitudinal_main_cruise_toggleable = false;
extern bool hyundai_main_btn_long_toggle;
bool hyundai_main_btn_long_toggle = false;
extern bool hyundai_has_lda_button;
bool hyundai_has_lda_button = false;
@@ -87,7 +87,7 @@ void hyundai_common_init(uint16_t param) {
hyundai_alt_limits_2 = GET_FLAG(param, HYUNDAI_PARAM_ALT_LIMITS_2);
hyundai_escc = GET_FLAG(current_safety_param_iq, HYUNDAI_PARAM_IQ_ESCC);
hyundai_longitudinal_main_cruise_toggleable = GET_FLAG(current_safety_param_iq, HYUNDAI_PARAM_IQ_LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE);
hyundai_main_btn_long_toggle = GET_FLAG(current_safety_param_iq, HYUNDAI_PARAM_IQ_MAIN_BTN_LONG_TOGGLE);
hyundai_has_lda_button = GET_FLAG(current_safety_param_iq, HYUNDAI_PARAM_IQ_HAS_LDA_BUTTON);
hyundai_non_scc = GET_FLAG(current_safety_param_iq, HYUNDAI_PARAM_IQ_NON_SCC);
@@ -144,7 +144,7 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai
}
// toggle main cruise state on rising edge of main cruise button
if (main_button && !main_button_prev && hyundai_longitudinal_main_cruise_toggleable) {
if (main_button && !main_button_prev && hyundai_main_btn_long_toggle) {
acc_main_on = !acc_main_on;
}

70
iqdbc_repo/iqdbc/safety/modes/tesla.h
View File

@@ -17,7 +17,7 @@
{.msg = {{0x3DF, 1, 8, 2U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* UI_status2 */ \
static bool tesla_longitudinal = false;
static bool tesla_fsd_14 = false;
static bool tesla_legacy_das_steering = false;
static bool tesla_stock_aeb = false;
// Only rising edges while controls are not allowed are considered for these systems:
@@ -50,6 +50,9 @@ static uint8_t tesla_get_counter(const CANPacket_t *msg) {
} else if (msg->addr == 0x370U) {
// Signal: EPAS3S_sysStatusCounter
cnt = msg->data[6] & 0x0FU;
} else if (msg->addr == 0x3E9U) {
// Signal: DAS_bodyControlsCounter
cnt = msg->data[6] >> 4;
} else {
}
return cnt;
@@ -57,8 +60,8 @@ static uint8_t tesla_get_counter(const CANPacket_t *msg) {
static int _tesla_get_checksum_byte(const int addr) {
int checksum_byte = -1;
if ((addr == 0x370) || (addr == 0x2b9) || (addr == 0x155)) {
// Signal: EPAS3S_sysStatusChecksum, DAS_controlChecksum, ESP_wheelRotationChecksum
if ((addr == 0x370) || (addr == 0x2b9) || (addr == 0x155) || (addr == 0x3E9)) {
// Signal: EPAS3S_sysStatusChecksum, DAS_controlChecksum, ESP_wheelRotationChecksum, DAS_bodyControlsChecksum
checksum_byte = 7;
} else if (addr == 0x488) {
// Signal: DAS_steeringControlChecksum
@@ -109,18 +112,9 @@ static bool tesla_get_quality_flag_valid(const CANPacket_t *msg) {
return valid;
}
static int tesla_get_steer_ctrl_type(const int ctrl_type) {
// Returns ANGLE_CONTROL-equivalent control type for FSD 14
int steer_ctrl_type = ctrl_type;
if (tesla_fsd_14) {
if (ctrl_type == 1) {
steer_ctrl_type = 2;
} else if (ctrl_type == 2) {
steer_ctrl_type = 1;
} else {
}
}
return steer_ctrl_type;
static int tesla_get_steer_ctrl_type(const uint8_t byte2) {
// Older Tesla firmware used a 2-bit field (now 3-bit) for DAS_steeringControlType
return tesla_legacy_das_steering ? (byte2 >> 6) : ((byte2 >> 5) & 0x07U);
}
static void tesla_rx_hook(const CANPacket_t *msg) {
@@ -263,19 +257,17 @@ static bool tesla_tx_hook(const CANPacket_t *msg) {
// We use 1/10 deg as a unit here
int raw_angle_can = ((msg->data[0] & 0x7FU) << 8) | msg->data[1];
int desired_angle = raw_angle_can - 16384;
int steer_control_type = msg->data[2] >> 6;
const int angle_ctrl_type = tesla_get_steer_ctrl_type(1);
const int lkas_ctrl_type = tesla_get_steer_ctrl_type(2);
bool steer_control_enabled = (steer_control_type == angle_ctrl_type) || // ANGLE_CONTROL
(steer_control_type == lkas_ctrl_type); // LANE_KEEP_ASSIST
int steer_control_type = tesla_get_steer_ctrl_type(msg->data[2]);
bool steer_control_enabled = (steer_control_type == 1) || // ANGLE_CONTROL
(steer_control_type == 2); // LANE_KEEP_ASSIST
if (steer_angle_cmd_checks_vm(desired_angle, steer_control_enabled, TESLA_STEERING_LIMITS, TESLA_STEERING_PARAMS)) {
violation = true;
}
bool valid_steer_control_type = (steer_control_type == 0) || // NONE
(steer_control_type == angle_ctrl_type) || // ANGLE_CONTROL
(steer_control_type == lkas_ctrl_type); // LANE_KEEP_ASSIST
bool valid_steer_control_type = (steer_control_type == 0) || // NONE
(steer_control_type == 1) || // ANGLE_CONTROL
(steer_control_type == 2); // LANE_KEEP_ASSIST
if (!valid_steer_control_type) {
violation = true;
}
@@ -325,6 +317,11 @@ static bool tesla_tx_hook(const CANPacket_t *msg) {
}
}
// DAS_bodyControls (blinker MITM on vehicle bus) is body control only, not motion
// actuation. openpilot copies the stock frame verbatim and only flips the turn-indicator
// bits, so we don't value-check it here — rejecting a frame would break the counter
// sequence the body controller validates. The TX whitelist still gates the address/bus.
if (violation) {
tx = false;
}
@@ -351,9 +348,11 @@ static bool tesla_fwd_hook(int bus_num, int addr) {
if (tesla_longitudinal && (addr == 0x2b9) && !tesla_stock_aeb) {
block_msg = true;
}
}
}
return block_msg;
}
@@ -371,8 +370,23 @@ static safety_config tesla_init(uint16_t param) {
{0x27D, 0, 3, .check_relay = true, .disable_static_blocking = true}, // APS_eacMonitor
};
const uint16_t TESLA_FLAG_FSD_14 = 2;
tesla_fsd_14 = GET_FLAG(param, TESLA_FLAG_FSD_14);
// With vehicle bus harness: adds DAS_bodyControls on bus 1 for blinker control
static const CanMsg TESLA_VEHICLE_BUS_TX_MSGS[] = {
{0x488, 0, 4, .check_relay = true, .disable_static_blocking = true}, // DAS_steeringControl
{0x2b9, 0, 8, .check_relay = false}, // DAS_control (for cancel)
{0x27D, 0, 3, .check_relay = true, .disable_static_blocking = true}, // APS_eacMonitor
{0x3E9, 1, 8, .check_relay = false}, // DAS_bodyControls (blinker)
};
static const CanMsg TESLA_VEHICLE_BUS_LONG_TX_MSGS[] = {
{0x488, 0, 4, .check_relay = true, .disable_static_blocking = true}, // DAS_steeringControl
{0x2b9, 0, 8, .check_relay = true, .disable_static_blocking = true}, // DAS_control
{0x27D, 0, 3, .check_relay = true, .disable_static_blocking = true}, // APS_eacMonitor
{0x3E9, 1, 8, .check_relay = false}, // DAS_bodyControls (blinker)
};
const uint16_t TESLA_FLAG_LEGACY_DAS_STEERING = 2;
tesla_legacy_das_steering = GET_FLAG(param, TESLA_FLAG_LEGACY_DAS_STEERING);
#ifdef ALLOW_DEBUG
const uint16_t TESLA_FLAG_LONGITUDINAL_CONTROL = 1;
@@ -401,7 +415,11 @@ static safety_config tesla_init(uint16_t param) {
};
safety_config ret;
if (tesla_longitudinal) {
if (tesla_has_vehicle_bus && tesla_longitudinal) {
SET_TX_MSGS(TESLA_VEHICLE_BUS_LONG_TX_MSGS, ret);
} else if (tesla_has_vehicle_bus) {
SET_TX_MSGS(TESLA_VEHICLE_BUS_TX_MSGS, ret);
} else if (tesla_longitudinal) {
SET_TX_MSGS(TESLA_M3_Y_LONG_TX_MSGS, ret);
} else {
SET_TX_MSGS(TESLA_M3_Y_TX_MSGS, ret);

10
iqdbc_repo/iqdbc/safety/modes/toyota.h
View File

@@ -375,11 +375,15 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
}
}
// UDS: Only tester present ("\x0F\x02\x3E\x00\x00\x00\x00\x00") allowed on diagnostics address
// UDS: Only tester present and door lock/unlock allowed on diagnostics address
if (msg->addr == 0x750U) {
// this address is sub-addressed. only allow tester present to radar (0xF)
bool invalid_uds_msg = (GET_BYTES(msg, 0, 4) != 0x003E020FU) || (GET_BYTES(msg, 4, 4) != 0x0U);
if (invalid_uds_msg) {
bool valid_tester_present = (GET_BYTES(msg, 0, 4) == 0x003E020FU) && (GET_BYTES(msg, 4, 4) == 0x0U);
// BCM door lock/unlock routine (0x40=BCM sub-addr, 0x05=len, 0x30 0x11=routine ID)
// Byte 5: 0x80=lock, 0x40=unlock. Only these two values allowed.
bool valid_door_lock = (GET_BYTES(msg, 0, 4) == 0x11300540U) &&
((GET_BYTES(msg, 5, 1) == 0x80U) || (GET_BYTES(msg, 5, 1) == 0x40U));
if (!valid_tester_present && !valid_door_lock) {
tx = 0;
}
}

14
iqdbc_repo/iqdbc/safety/modes/volkswagen_common.h
View File

@@ -10,6 +10,14 @@ extern const uint16_t FLAG_VOLKSWAGEN_ALLOW_LONG_ACCEL_WITH_GAS_PRESSED;
const uint16_t FLAG_VOLKSWAGEN_ALLOW_LONG_ACCEL_WITH_GAS_PRESSED = 8;
extern const uint16_t FLAG_VOLKSWAGEN_PQ_ALC_MODULE;
const uint16_t FLAG_VOLKSWAGEN_PQ_ALC_MODULE = 32;
extern const uint16_t FLAG_VOLKSWAGEN_PQ_LOWLINE;
const uint16_t FLAG_VOLKSWAGEN_PQ_LOWLINE = 64;
extern const uint16_t FLAG_VOLKSWAGEN_PQ_NO_CAM_BUS;
const uint16_t FLAG_VOLKSWAGEN_PQ_NO_CAM_BUS = 128;
extern const uint16_t FLAG_VOLKSWAGEN_PQ_ACC_FTS_EPB;
const uint16_t FLAG_VOLKSWAGEN_PQ_ACC_FTS_EPB = 256;
extern const uint16_t FLAG_VOLKSWAGEN_PQ_SNG_ECD;
const uint16_t FLAG_VOLKSWAGEN_PQ_SNG_ECD = 512;
static uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR
@@ -55,6 +63,9 @@ float vw_iq_measured_angle_deg = 0.0f;
extern bool vw_iq_aol_active;
bool vw_iq_aol_active = false;
extern bool vw_iq_no_cam;
bool vw_iq_no_cam = false;
extern float vw_iq_angle_offset_deg;
float vw_iq_angle_offset_deg = 0.0f;
@@ -103,6 +114,7 @@ static void volkswagen_common_init(void) {
vw_iq_apd_wheelbase = 0.0f;
vw_iq_apd_params_valid = false;
vw_iq_aol_active = false;
vw_iq_no_cam = false;
vw_iq_angle_offset_deg = 0.0f;
vw_iq_alc_desired_angle_deg = 0.0f;
vw_iq_alc_active = false;
@@ -214,7 +226,7 @@ static void volkswagen_iq_send_debug_la(uint32_t debug_addr, uint8_t bus) {
float speed_kmh = ((float)(vehicle_speed.min) / VEHICLE_SPEED_FACTOR) * 3.6f;
uint16_t spd_raw = (uint16_t)(speed_kmh * 100.0f);
int16_t ang_raw = (int16_t)(vw_iq_measured_angle_deg * 100.0f);
uint8_t flags = (vw_iq_apd_params_valid ? 0x01U : 0x00U);
uint8_t flags = (vw_iq_apd_params_valid ? 0x01U : 0x00U) | (vw_iq_aol_active ? 0x02U : 0x00U) | (vw_iq_no_cam ? 0x04U : 0x00U);
msg.data[0] = (uint8_t)(la_raw & 0xFFU);
msg.data[1] = (uint8_t)((la_raw >> 8) & 0xFFU);

18
iqdbc_repo/iqdbc/safety/modes/volkswagen_meb.h
View File

@@ -236,6 +236,16 @@ static void volkswagen_meb_rx_hook(const CANPacket_t *msg) {
}
}
// 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,
@@ -243,14 +253,6 @@ static bool volkswagen_meb_tx_hook(const CANPacket_t *msg) {
.inactive_accel = 3010,
};
const CurvatureSteeringLimits VOLKSWAGEN_MEB_STEERING_LIMITS = {
.max_curvature = 32767, // TEST: 15-bit max, no curvature ceiling
.curvature_to_can = 149253.7313f,
.send_rate = 0.02f,
.inactive_curvature_is_zero = true,
.max_power = 65535, // TEST: no power ceiling
};
bool tx = true;
if (msg->addr == MSG_HCA_03) {

72
iqdbc_repo/iqdbc/safety/modes/volkswagen_pq.h
View File

@@ -6,6 +6,7 @@
#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_3 0x380U // RX from ECU
#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
@@ -15,10 +16,16 @@
#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_SNG_1 0x3D7U // TX by OP
#define MSG_PQ_SAFETY_1 0x6A0U // RX by OP
#define MSG_PQ_DEBUG_LA 0x6A1U // TX by panda, internal safety state debug
#define MSG_IQ 0x6A1U // TX by OP
static bool volkswagen_pq_alc_module_present = false;
static bool volkswagen_pq_acc_tsk_ready = false;
static bool volkswagen_pq_lowline = false;
static bool volkswagen_pq_acc_fts_epb = false;
static bool volkswagen_pq_sng_ecd = false;
static uint32_t volkswagen_pq_get_checksum(const CANPacket_t *msg) {
return (uint32_t)msg->data[(msg->addr == MSG_MOTOR_5) ? 7 : 0];
@@ -57,26 +64,39 @@ static safety_config volkswagen_pq_init(uint16_t param) {
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}};
{MSG_APD_1, 1, 8, .check_relay = false}, {MSG_IQ, 1, 8, .check_relay = false}};
// Lowline (non-ECAN) lateral-only cars: ptCAN (bus 1) is the only active bus, no J533 gateway.
// HCA_1 and lateral messages go directly on bus 1 to the EPS. GRA_Neu bus 0 dropped (dead).
static const CanMsg VOLKSWAGEN_PQ_STOCK_TX_MSGS_BUS1[] = {{MSG_HCA_1, 1, 5, .check_relay = true}, {MSG_LDW_1, 1, 8, .check_relay = true},
{MSG_GRA_NEU, 1, 4, .check_relay = false}, {MSG_GRA_NEU, 2, 4, .check_relay = false},
{MSG_BLINKMODI_02, 1, 8, .check_relay = false},
{MSG_APD_1, 1, 8, .check_relay = false}, {MSG_IQ, 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}};
{MSG_MOTOR_5, 2, 8, .check_relay = true}, {MSG_MOTOR_3, 1, 8, .check_relay = false},
{MSG_APD_1, 1, 8, .check_relay = false}, {MSG_IQ, 1, 8, .check_relay = false},
{MSG_SNG_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_LENKHILFE_3, 1, 6, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_BREMSE_1, 1, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_2, 1, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_3, 1, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_5, 1, 8, 50U, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_GRA_NEU, 1, 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_lowline = GET_FLAG(param, FLAG_VOLKSWAGEN_PQ_LOWLINE);
vw_iq_no_cam = GET_FLAG(param, FLAG_VOLKSWAGEN_PQ_NO_CAM_BUS);
volkswagen_pq_acc_fts_epb = GET_FLAG(param, FLAG_VOLKSWAGEN_PQ_ACC_FTS_EPB);
volkswagen_pq_sng_ecd = GET_FLAG(param, FLAG_VOLKSWAGEN_PQ_SNG_ECD);
volkswagen_pq_acc_tsk_ready = false;
#ifdef ALLOW_DEBUG
@@ -86,7 +106,8 @@ static safety_config volkswagen_pq_init(uint16_t param) {
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);
volkswagen_pq_lowline ? BUILD_SAFETY_CFG(volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_STOCK_TX_MSGS_BUS1) : \
BUILD_SAFETY_CFG(volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_STOCK_TX_MSGS);
if (!volkswagen_pq_alc_module_present) {
ret.rx_checks_len -= 1;
}
@@ -94,7 +115,9 @@ static safety_config volkswagen_pq_init(uint16_t param) {
}
static void volkswagen_pq_rx_hook(const CANPacket_t *msg) {
if (msg->bus == 0U) {
// All PQ RX processing is on bus 1 (ptCAN). Messages exist on both bus 0 and bus 1 for ECAN
// gateway cars; on lowline non-ECAN cars bus 1 is the only active bus.
if (msg->bus == 1U) {
// Update in-motion state from speed value.
// Signal: Bremse_1.BR1_Rad_kmh
if (msg->addr == MSG_BREMSE_1) {
@@ -119,9 +142,14 @@ static void volkswagen_pq_rx_hook(const CANPacket_t *msg) {
vw_iq_measured_angle_deg = angle_sign ? -angle_deg : angle_deg;
}
// acc_main_on tracked unconditionally so main-switch disengagement works for both long
// and lateral-only (pcmCruise) configurations.
if (msg->addr == MSG_MOTOR_5) {
acc_main_on = GET_BIT(msg, 50U);
}
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;
}
@@ -161,8 +189,7 @@ static void volkswagen_pq_rx_hook(const CANPacket_t *msg) {
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;
@@ -180,6 +207,7 @@ static bool volkswagen_pq_tx_hook(const CANPacket_t *msg) {
}
if (msg->addr == MSG_HCA_1) {
volkswagen_iq_send_debug_la(MSG_PQ_DEBUG_LA, 1U);
const uint8_t hca_status = (msg->data[1] >> 4) & 0x0FU;
if (volkswagen_pq_alc_module_present && (hca_status == 8U)) {
@@ -214,13 +242,31 @@ static bool volkswagen_pq_tx_hook(const CANPacket_t *msg) {
}
}
if (msg->addr == MSG_MOTOR_3) {
if (!volkswagen_pq_acc_fts_epb) {
tx = false;
}
}
if (msg->addr == MSG_SNG_1) {
if (!volkswagen_pq_sng_ecd) {
tx = false;
}
}
return tx;
}
static bool volkswagen_pq_fwd_hook(int bus_num, int addr) {
SAFETY_UNUSED(addr);
return vw_iq_no_cam && (bus_num == 0);
}
const safety_hooks volkswagen_pq_hooks = {
.init = volkswagen_pq_init,
.rx = volkswagen_pq_rx_hook,
.tx = volkswagen_pq_tx_hook,
.fwd = volkswagen_pq_fwd_hook,
.get_counter = volkswagen_pq_get_counter,
.get_checksum = volkswagen_pq_get_checksum,
.compute_checksum = volkswagen_pq_compute_checksum,

8
iqdbc_repo/iqdbc/safety/tests/hyundai_common.py
View File

@@ -150,7 +150,7 @@ class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest):
with self.subTest("enable_aol", aol_enabled=enable_aol):
for main_cruise_toggleable in (True, False):
with self.subTest("main_cruise_toggleable", main_cruise_toggleable=main_cruise_toggleable):
main_cruise_toggleable_flag = HyundaiSafetyFlagsIQ.LONG_MAIN_CRUISE_TOGGLEABLE if main_cruise_toggleable else 0
main_cruise_toggleable_flag = HyundaiSafetyFlagsIQ.MAIN_BTN_LONG_TOGGLE if main_cruise_toggleable else 0
self.safety.set_current_safety_param_iq(default_safety_param_iq | main_cruise_toggleable_flag)
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)
@@ -182,7 +182,7 @@ class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest):
for enable_aol in (True, False):
with self.subTest("enable_aol", aol_enabled=enable_aol):
main_cruise_toggleable_flag = HyundaiSafetyFlagsIQ.LONG_MAIN_CRUISE_TOGGLEABLE
main_cruise_toggleable_flag = HyundaiSafetyFlagsIQ.MAIN_BTN_LONG_TOGGLE
self.safety.set_current_safety_param_iq(default_safety_param_iq | main_cruise_toggleable_flag)
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)
@@ -215,7 +215,7 @@ class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest):
for enable_aol in (True, False):
with self.subTest("enable_aol", aol_enabled=enable_aol):
main_cruise_toggleable_flag = HyundaiSafetyFlagsIQ.LONG_MAIN_CRUISE_TOGGLEABLE
main_cruise_toggleable_flag = HyundaiSafetyFlagsIQ.MAIN_BTN_LONG_TOGGLE
self.safety.set_current_safety_param_iq(default_safety_param_iq | main_cruise_toggleable_flag)
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)
@@ -256,7 +256,7 @@ class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest):
"""Test that mismatch counter resets when states resync"""
for enable_aol in (True, False):
with self.subTest("enable_aol", aol_enabled=enable_aol):
main_cruise_toggleable_flag = HyundaiSafetyFlagsIQ.LONG_MAIN_CRUISE_TOGGLEABLE
main_cruise_toggleable_flag = HyundaiSafetyFlagsIQ.MAIN_BTN_LONG_TOGGLE
self.safety.set_current_safety_param_iq(default_safety_param_iq | main_cruise_toggleable_flag)
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)

5
iqdbc_repo/iqdbc/safety/tests/libsafety/SConscript
View File

@@ -25,6 +25,11 @@ env = Environment(
tools=["default", "compilation_db"],
)
# short colored build output, if the top-level pretty tool is present (main-repo build)
_pretty = Dir('#site_scons/site_tools').File('pretty.py')
if _pretty.exists():
env.Tool('pretty', toolpath=[_pretty.dir.abspath])
# The Mull plugin injects mutations that are dormant unless run with mull-runner
if system == "Darwin":
mull_plugin = Dir('#').abspath + '/.mull/lib/mull-ir-frontend-18'

2
iqdbc_repo/iqdbc/safety/tests/test_ford.py
View File

@@ -22,7 +22,7 @@ MSG_LateralMotionControl = 0x3D3 # TX by OP, Lateral Control message
MSG_LateralMotionControl2 = 0x3D6 # TX by OP, alternate Lateral Control message
MSG_IPMA_Data = 0x3D8 # TX by OP, IPMA and LKAS user interface
SAFETY_ISO_LATERAL_ACCEL = 3.0
SAFETY_ISO_LATERAL_ACCEL = 5.0
EARTH_G = 9.81
AVERAGE_ROAD_ROLL = 0.06
MAX_LATERAL_ACCEL = SAFETY_ISO_LATERAL_ACCEL - (EARTH_G * AVERAGE_ROAD_ROLL)

24
iqdbc_repo/iqdbc/safety/tests/test_tesla.py
View File

@@ -11,8 +11,7 @@ except ImportError:
pytest.skip("requires openpilot dependencies", allow_module_level=True)
from iqdbc.car.lateral import get_max_angle_delta_vm, get_max_angle_vm
from iqdbc.car.tesla.teslacan import get_steer_ctrl_type
from iqdbc.car.tesla.values import CarControllerParams, TeslaSafetyFlags, TeslaFlags, CANBUS
from iqdbc.car.tesla.values import CarControllerParams, TeslaSafetyFlags, CANBUS
from iqdbc.car.structs import CarParams
from iqdbc.car.vehicle_model import VehicleModel
from iqdbc.can import CANDefine
@@ -23,6 +22,7 @@ from iqdbc.safety.tests.common import CANPackerSafety, MAX_SPEED_DELTA, MAX_WRON
MSG_DAS_steeringControl = 0x488
MSG_APS_eacMonitor = 0x27d
MSG_DAS_Control = 0x2b9
MSG_DAS_bodyControls = 0x3E9
def round_angle(apply_angle, can_offset=0):
@@ -34,6 +34,7 @@ def round_angle(apply_angle, can_offset=0):
class TestTeslaSafetyBase(common.CarSafetyTest, common.AngleSteeringSafetyTest, common.LongitudinalAccelSafetyTest):
SAFETY_PARAM = 0
STEER_TYPE_SHIFT = 0 # legacy firmware uses a 2-bit field, one bit up from the 3-bit signal
RELAY_MALFUNCTION_ADDRS = {0: (MSG_DAS_steeringControl, MSG_APS_eacMonitor)}
FWD_BLACKLISTED_ADDRS = {2: [MSG_DAS_steeringControl, MSG_APS_eacMonitor]}
@@ -83,11 +84,7 @@ class TestTeslaSafetyBase(common.CarSafetyTest, common.AngleSteeringSafetyTest,
self.safety.init_tests()
def _angle_cmd_msg(self, angle: float, state: bool | int, increment_timer: bool = True, bus: int = 0):
# If FSD 14, translate steer control type to new flipped definition
if self.safety.get_current_safety_param() & TeslaSafetyFlags.FSD_14:
state = get_steer_ctrl_type(TeslaFlags.FSD_14, int(state))
values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": state}
values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": int(state) << self.STEER_TYPE_SHIFT}
if increment_timer:
self.safety.set_timer(self.cnt_angle_cmd * int(1e6 / self.LATERAL_FREQUENCY))
self.__class__.cnt_angle_cmd += 1
@@ -407,8 +404,9 @@ class TestTeslaStockSafety(TestTeslaSafetyBase):
self.assertFalse(self._tx(no_aeb_msg))
class TestTeslaFSD14StockSafety(TestTeslaStockSafety):
SAFETY_PARAM = TeslaSafetyFlags.FSD_14
class TestTeslaLegacyDasSteeringStockSafety(TestTeslaStockSafety):
SAFETY_PARAM = TeslaSafetyFlags.LEGACY_DAS_STEERING
STEER_TYPE_SHIFT = 1
class TestTeslaLongitudinalSafety(TestTeslaSafetyBase):
@@ -459,14 +457,18 @@ class TestTeslaLongitudinalSafety(TestTeslaSafetyBase):
self.assertFalse(self._tx(self._long_control_msg(set_speed=0, accel_limits=(-0.1, -0.1))))
class TestTeslaFSD14LongitudinalSafety(TestTeslaLongitudinalSafety):
SAFETY_PARAM = TeslaSafetyFlags.LONG_CONTROL | TeslaSafetyFlags.FSD_14
class TestTeslaLegacyDasSteeringLongitudinalSafety(TestTeslaLongitudinalSafety):
SAFETY_PARAM = TeslaSafetyFlags.LONG_CONTROL | TeslaSafetyFlags.LEGACY_DAS_STEERING
STEER_TYPE_SHIFT = 1
class TestTeslaVehicleBusSafety(TestTeslaSafetyBase):
LONGITUDINAL = False
# With the vehicle bus harness, DAS_bodyControls is also TX'd on bus 1 (blinker MITM)
TX_MSGS = [*TestTeslaSafetyBase.TX_MSGS, [MSG_DAS_bodyControls, 1]]
def setUp(self):
super().setUp()
self.safety = libsafety_py.libsafety

70
iqdbc_repo/iqdbc/safety/tests/test_volkswagen_pq.py
View File

@@ -20,6 +20,7 @@ MSG_ACC_GRA_ANZEIGE = 0x56A # TX by OP, ACC HUD
MSG_LDW_1 = 0x5BE # TX by OP, Lane line recognition and text alerts
MSG_BLINKMODI_02 = 0x0AA # TX by OP, turn signal control
MSG_APD_1 = 0x3D6 # TX by OP, CarParams
MSG_IQ = 0x6A1 # TX by OP
class TestVolkswagenPqSafetyBase(common.CarSafetyTest):
@@ -43,7 +44,7 @@ class TestVolkswagenPqSafetyBase(common.CarSafetyTest):
# Ego speed (Bremse_1)
def _speed_msg(self, speed):
values = {"BR1_Rad_kmh": speed}
return self.packer.make_can_msg_safety("Bremse_1", 0, values)
return self.packer.make_can_msg_safety("Bremse_1", 1, values)
# Brake light switch (shared message Motor_2)
def _user_brake_msg(self, brake):
@@ -63,7 +64,7 @@ class TestVolkswagenPqSafetyBase(common.CarSafetyTest):
# Driver steering input torque
def _torque_driver_msg(self, torque):
values = {"LH3_LM": abs(torque), "LH3_LMSign": torque < 0}
return self.packer.make_can_msg_safety("Lenkhilfe_3", 0, values)
return self.packer.make_can_msg_safety("Lenkhilfe_3", 1, values)
# openpilot steering output torque
def _torque_cmd_msg(self, torque, steer_req=1, hca_status=7):
@@ -76,17 +77,17 @@ class TestVolkswagenPqSafetyBase(common.CarSafetyTest):
values = {"MO2_BLS": brake_pressed,
"MO2_Sta_GRA": cruise_engaged,
"MO2_Status_TSK": tsk_status}
return self.packer.make_can_msg_safety("Motor_2", 0, values)
return self.packer.make_can_msg_safety("Motor_2", 1, values)
# ACC main switch status
def _motor_5_msg(self, main_switch=False):
values = {"MO5_GRA_Hauptsch": main_switch}
return self.packer.make_can_msg_safety("Motor_5", 0, values)
return self.packer.make_can_msg_safety("Motor_5", 1, values)
# Driver throttle input (Motor_3)
def _user_gas_msg(self, gas):
values = {"MO3_Pedalwert": gas}
return self.packer.make_can_msg_safety("Motor_3", 0, values)
return self.packer.make_can_msg_safety("Motor_3", 1, values)
# Cruise control buttons (GRA_Neu)
def _button_msg(self, _set=False, resume=False, cancel=False, bus=2):
@@ -116,7 +117,7 @@ class TestVolkswagenPqSafetyBase(common.CarSafetyTest):
class TestVolkswagenPqStockSafety(TestVolkswagenPqSafetyBase):
# Transmit of GRA_Neu is allowed on bus 0/1/2 to keep compatibility with gateway and camera integration
TX_MSGS = [[MSG_HCA_1, 0], [MSG_GRA_NEU, 0], [MSG_GRA_NEU, 1], [MSG_GRA_NEU, 2], [MSG_LDW_1, 0], [MSG_BLINKMODI_02, 0], [MSG_APD_1, 1]]
TX_MSGS = [[MSG_HCA_1, 0], [MSG_GRA_NEU, 0], [MSG_GRA_NEU, 1], [MSG_GRA_NEU, 2], [MSG_LDW_1, 0], [MSG_BLINKMODI_02, 0], [MSG_APD_1, 1], [MSG_IQ, 1]]
FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_1, MSG_LDW_1]}
def setUp(self):
@@ -139,11 +140,11 @@ class TestVolkswagenPqLongSafety(TestVolkswagenPqSafetyBase, common.Longitudinal
tsk_status = True
TX_MSGS = [[MSG_HCA_1, 0], [MSG_LDW_1, 0], [MSG_ACC_SYSTEM, 0], [MSG_ACC_GRA_ANZEIGE, 0],
[MSG_GRA_NEU, 1], [MSG_GRA_NEU, 2], [MSG_BLINKMODI_02, 0], [MSG_MOTOR_2, 2], [MSG_APD_1, 1]]
FWD_BLACKLISTED_ADDRS = {0: [MSG_MOTOR_2, MSG_GRA_NEU],
[MSG_GRA_NEU, 1], [MSG_GRA_NEU, 2], [MSG_BLINKMODI_02, 0], [MSG_MOTOR_2, 2], [MSG_MOTOR_5, 2], [MSG_APD_1, 1], [MSG_IQ, 1]]
FWD_BLACKLISTED_ADDRS = {0: [MSG_MOTOR_2, MSG_MOTOR_5, MSG_GRA_NEU],
2: [MSG_HCA_1, MSG_LDW_1, MSG_ACC_SYSTEM, MSG_ACC_GRA_ANZEIGE]}
RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_1, MSG_LDW_1, MSG_ACC_SYSTEM, MSG_ACC_GRA_ANZEIGE),
2: (MSG_MOTOR_2, MSG_GRA_NEU)}
2: (MSG_MOTOR_2, MSG_GRA_NEU, MSG_MOTOR_5)}
INACTIVE_ACCEL = 3.01
def setUp(self):
@@ -168,20 +169,20 @@ class TestVolkswagenPqLongSafety(TestVolkswagenPqSafetyBase, common.Longitudinal
# ACC main switch must be on, engage on falling edge
self.safety.set_controls_allowed(0)
self._rx(self._motor_5_msg(main_switch=False))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0))
self._rx(self._button_msg(bus=0))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=1))
self._rx(self._button_msg(bus=1))
self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} with main switch off")
self._rx(self._motor_5_msg(main_switch=True))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=1))
self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} rising edge")
self._rx(self._button_msg(bus=0))
self._rx(self._button_msg(bus=1))
self.assertTrue(self.safety.get_controls_allowed(), f"controls not allowed on {button} falling edge")
def test_cancel_button(self):
# Disable on rising edge of cancel button
self._rx(self._motor_5_msg(main_switch=True))
self.safety.set_controls_allowed(1)
self._rx(self._button_msg(cancel=True, bus=0))
self._rx(self._button_msg(cancel=True, bus=1))
self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after cancel")
def test_main_switch(self):
@@ -223,8 +224,8 @@ class TestVolkswagenPqLongSafety(TestVolkswagenPqSafetyBase, common.Longitudinal
self.safety.set_controls_allowed(0)
self._rx(self._motor_5_msg(main_switch=False))
self._rx(self._motor_2_msg(tsk_status=True))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0))
self._rx(self._button_msg(bus=0))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=1))
self._rx(self._button_msg(bus=1))
self.assertTrue(self.safety.get_controls_allowed(), f"controls not allowed on {button} falling edge with TSK ready")
def test_torque_cmd_enable_variants(self):
@@ -250,5 +251,42 @@ class TestVolkswagenPqLongSafety(TestVolkswagenPqSafetyBase, common.Longitudinal
self.safety.set_controls_allowed(True)
self.assertTrue(self._tx(self._accel_msg(0.5)))
class TestVolkswagenPqLowlineSafety(TestVolkswagenPqSafetyBase):
"""Non-ECAN lateral-only PQ cars: bus 0 dead, TX on bus 1 (ptCAN) directly to EPS."""
TX_MSGS = [[MSG_HCA_1, 1], [MSG_GRA_NEU, 1], [MSG_GRA_NEU, 2], [MSG_LDW_1, 1], [MSG_BLINKMODI_02, 1], [MSG_APD_1, 1], [MSG_IQ, 1]]
FWD_BUS_LOOKUP = {2: 0}
FWD_BLACKLISTED_ADDRS = {}
RELAY_MALFUNCTION_ADDRS = {1: (MSG_HCA_1, MSG_LDW_1)}
def setUp(self):
self.packer = CANPackerSafety("vw_pq")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenPq, VolkswagenSafetyFlags.PQ_LOWLINE | VolkswagenSafetyFlags.PQ_NO_CAM_BUS)
self.safety.init_tests()
def _torque_cmd_msg(self, torque, steer_req=1, hca_status=7):
values = {"LM_Offset": abs(torque), "LM_OffSign": torque < 0, "HCA_Status": hca_status if steer_req else 3}
return self.packer.make_can_msg_safety("HCA_1", 1, values)
def test_spam_cancel_safety_check(self):
self.safety.set_controls_allowed(0)
self.assertTrue(self._tx(self._button_msg(cancel=True)))
self.assertFalse(self._tx(self._button_msg(resume=True)))
self.assertFalse(self._tx(self._button_msg(_set=True)))
self.safety.set_controls_allowed(1)
self.assertTrue(self._tx(self._button_msg(resume=True)))
class TestVolkswagenPqNoCamSafety(TestVolkswagenPqStockSafety):
FWD_BUS_LOOKUP = {2: 0}
def setUp(self):
self.packer = CANPackerSafety("vw_pq")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenPq, VolkswagenSafetyFlags.PQ_NO_CAM_BUS)
self.safety.init_tests()
if __name__ == "__main__":
unittest.main()