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

IQ.Pilot Release Commit @ c744c08

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-07-26 19:09:55 -05:00
parent 152206f935
commit 6edfe55a1e
192 changed files with 10991 additions and 11109 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); // ~5.6 m/s^2
static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL + (EARTH_G * AVERAGE_ROAD_ROLL); // ~3.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); // ~4.4 m/s^2
static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - (EARTH_G * AVERAGE_ROAD_ROLL); // ~2.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); // ~5.6 m/s^2
static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL + (EARTH_G * AVERAGE_ROAD_ROLL); // ~3.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

626
iqdbc_repo/iqdbc/safety/modes/hyundai.h
View File

@@ -3,8 +3,6 @@
#include "iqdbc/safety/declarations.h"
#include "iqdbc/safety/modes/hyundai_common.h"
#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
#define HYUNDAI_LIMITS(steer, rate_up, rate_down) { \
.max_torque = (steer), \
.max_rate_up = (rate_up), \
@@ -23,84 +21,96 @@
extern const LongitudinalLimits HYUNDAI_LONG_LIMITS;
const LongitudinalLimits HYUNDAI_LONG_LIMITS = {
.max_accel = 250, // 1/100 m/s2
.min_accel = -400, // 1/100 m/s2
.max_accel = 200, // 1/100 m/s2
.min_accel = -350, // 1/100 m/s2
.zero_accel = 0,
};
#define HYUNDAI_COMMON_TX_MSGS(scc_bus) \
{0x340, 0, 8, .check_relay = true}, /* LKAS11 Bus 0 */ \
{0x4F1, scc_bus, 4, .check_relay = false}, /* CLU11 Bus 0 (radar-SCC) or 2 (camera-SCC) */ \
{0x485, 0, 4, .check_relay = true}, /* LFAHDA_MFC Bus 0 */ \
#define HYUNDAI_LONG_COMMON_TX_MSGS(scc_bus) \
HYUNDAI_COMMON_TX_MSGS(scc_bus) \
{0x420, 0, 8, .check_relay = true}, /* SCC11 Bus 0 */ \
{0x421, 0, 8, .check_relay = true}, /* SCC12 Bus 0 */ \
{0x50A, 0, 8, .check_relay = true}, /* SCC13 Bus 0 */ \
{0x389, 0, 8, .check_relay = true}, /* SCC14 Bus 0 */ \
{0x4A2, 0, 2, .check_relay = false}, /* FRT_RADAR11 Bus 0 */ \
#define HYUNDAI_COMMON_RX_CHECKS(legacy) \
{.msg = {{0x260, 0, 8, 100U, .max_counter = 3U, .ignore_quality_flag = true}, \
{0x371, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }}}, \
{.msg = {{0x386, 0, 8, 100U, .ignore_checksum = (legacy), .ignore_counter = (legacy), .max_counter = (legacy) ? 0U : 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x394, 0, 8, 100U, .ignore_checksum = (legacy), .ignore_counter = (legacy), .max_counter = (legacy) ? 0U : 7U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x251, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x4F1, 0, 4, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define HYUNDAI_SCC11_ADDR_CHECK(scc_bus) \
{.msg = {{0x420, (scc_bus), 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .frequency = 50U}, { 0 }, { 0 }}}, \
#define HYUNDAI_SCC12_ADDR_CHECK(scc_bus) \
{.msg = {{0x421, (scc_bus), 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define HYUNDAI_FCEV_GAS_ADDR_CHECK \
{.msg = {{0x91, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define HYUNDAI_LDA_BUTTON_ADDR_CHECK \
{.msg = {{0x391, 0, 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .frequency = 50U}, { 0 }, { 0 }}}, \
#define HYUNDAI_NON_SCC_HEV_ADDR_CHECK \
{.msg = {{0x595U, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define HYUNDAI_NON_SCC_EV_ADDR_CHECK \
{.msg = {{0x592U, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
static const CanMsg HYUNDAI_TX_MSGS[] = {
{0x340, 0, 8}, // LKAS11 Bus 0
{0x4F1, 0, 4}, // CLU11 Bus 0
{0x485, 0, 8}, // LFAHDA_MFC Bus 0
{593, 2, 8}, // MDPS12, Bus 2
{1056, 0, 8}, // SCC11, Bus 0
{1057, 0, 8}, // SCC12, Bus 0
{1290, 0, 8}, // SCC13, Bus 0
{905, 0, 8}, // SCC14, Bus 0
{909, 0, 8}, // FCA11 Bus 0
{1155, 0, 8}, // FCA12 Bus 0
{1186, 0, 8}, // FRT_RADAR11, Bus 0
{1265, 2, 4}, // CLU11, Bus 0, 2
{0x7D0, 0, 8}, // radar UDS TX addr Bus 0 (for radar disable) // 2000
{0x7b1, 0, 8},
HYUNDAI_COMMON_TX_MSGS(0)
};
#define HYUNDAI_COMMON_RX_CHECKS(legacy) \
{.msg = {{0x260, 0, 8, .max_counter = 3U, .frequency = 100U}, \
{0x371, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}, \
{0x91, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}}}, \
{.msg = {{0x386, 0, 8, .ignore_checksum = (legacy), .ignore_counter = (legacy), .max_counter = (legacy) ? 0U : 15U, .frequency = 100U}, { 0 }, { 0 }}}, \
{.msg = {{0x394, 0, 8, .ignore_checksum = (legacy), .ignore_counter = (legacy), .max_counter = (legacy) ? 0U : 7U, .frequency = 100U}, { 0 }, { 0 }}}, \
#define HYUNDAI_SCC12_ADDR_CHECK(scc_bus) \
{.msg = {{0x421, (scc_bus), 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
static bool hyundai_legacy = false;
static bool hyundai_cruise_buttons_alt = false;
static uint8_t hyundai_get_counter(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
static uint8_t hyundai_get_counter(const CANPacket_t *msg) {
uint8_t cnt = 0;
if (addr == 0x260) {
cnt = (GET_BYTE(to_push, 7) >> 4) & 0x3U;
} else if (addr == 0x386) {
cnt = ((GET_BYTE(to_push, 3) >> 6) << 2) | (GET_BYTE(to_push, 1) >> 6);
} else if (addr == 0x394) {
cnt = (GET_BYTE(to_push, 1) >> 5) & 0x7U;
} else if (addr == 0x421) {
cnt = GET_BYTE(to_push, 7) & 0xFU;
} else if (addr == 0x4F1) {
cnt = (GET_BYTE(to_push, 3) >> 4) & 0xFU;
if (msg->addr == 0x260U) {
cnt = (msg->data[7] >> 4) & 0x3U;
} else if (msg->addr == 0x386U) {
cnt = ((msg->data[3] >> 6) << 2) | (msg->data[1] >> 6);
} else if (msg->addr == 0x394U) {
cnt = (msg->data[1] >> 5) & 0x7U;
} else if (msg->addr == 0x421U) {
cnt = msg->data[7] & 0xFU;
} else if (msg->addr == 0x4F1U) {
cnt = (msg->data[3] >> 4) & 0xFU;
} else {
}
return cnt;
}
static uint32_t hyundai_get_checksum(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
static uint32_t hyundai_get_checksum(const CANPacket_t *msg) {
uint8_t chksum = 0;
if (addr == 0x260) {
chksum = GET_BYTE(to_push, 7) & 0xFU;
} else if (addr == 0x386) {
chksum = ((GET_BYTE(to_push, 7) >> 6) << 2) | (GET_BYTE(to_push, 5) >> 6);
} else if (addr == 0x394) {
chksum = GET_BYTE(to_push, 6) & 0xFU;
} else if (addr == 0x421) {
chksum = GET_BYTE(to_push, 7) >> 4;
if (msg->addr == 0x260U) {
chksum = msg->data[7] & 0xFU;
} else if (msg->addr == 0x386U) {
chksum = ((msg->data[7] >> 6) << 2) | (msg->data[5] >> 6);
} else if (msg->addr == 0x394U) {
chksum = msg->data[6] & 0xFU;
} else if (msg->addr == 0x421U) {
chksum = msg->data[7] >> 4;
} else {
}
return chksum;
}
static uint32_t hyundai_compute_checksum(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
static uint32_t hyundai_compute_checksum(const CANPacket_t *msg) {
uint8_t chksum = 0;
if (addr == 0x386) {
if (msg->addr == 0x386U) {
// count the bits
for (int i = 0; i < 8; i++) {
uint8_t b = GET_BYTE(to_push, i);
uint8_t b = msg->data[i];
for (int j = 0; j < 8; j++) {
uint8_t bit = 0;
// exclude checksum and counter
@@ -114,12 +124,12 @@ static uint32_t hyundai_compute_checksum(const CANPacket_t *to_push) {
} else {
// sum of nibbles
for (int i = 0; i < 8; i++) {
if ((addr == 0x394) && (i == 7)) {
if ((msg->addr == 0x394U) && (i == 7)) {
continue; // exclude
}
uint8_t b = GET_BYTE(to_push, i);
if (((addr == 0x260) && (i == 7)) || ((addr == 0x394) && (i == 6)) || ((addr == 0x421) && (i == 7))) {
b &= (addr == 0x421) ? 0x0FU : 0xF0U; // remove checksum
uint8_t b = msg->data[i];
if (((msg->addr == 0x260U) && (i == 7)) || ((msg->addr == 0x394U) && (i == 6)) || ((msg->addr == 0x421U) && (i == 7))) {
b &= (msg->addr == 0x421U) ? 0x0FU : 0xF0U; // remove checksum
}
chksum += (b % 16U) + (b / 16U);
}
@@ -129,115 +139,127 @@ static uint32_t hyundai_compute_checksum(const CANPacket_t *to_push) {
return chksum;
}
static void hyundai_rx_hook(const CANPacket_t *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
static void hyundai_rx_hook(const CANPacket_t *msg) {
// SCC12 is on bus 2 for camera-based SCC cars, bus 0 on all others
if (addr == 0x421) {
if (((bus == 0) && !hyundai_camera_scc) || ((bus == 2) && hyundai_camera_scc)) {
if (msg->addr == 0x421U) {
if (((msg->bus == 0U) && !hyundai_camera_scc) || ((msg->bus == 2U) && hyundai_camera_scc)) {
// 2 bits: 13-14
int cruise_engaged = (GET_BYTES(to_push, 0, 4) >> 13) & 0x3U;
int cruise_engaged = (GET_BYTES(msg, 0, 4) >> 13) & 0x3U;
hyundai_common_cruise_state_check(cruise_engaged);
}
}
if (bus == 0) {
if (addr == 0x251) {
int torque_driver_new = (GET_BYTES(to_push, 0, 2) & 0x7ffU) - 1024U;
if (msg->addr == 0x420U) {
if (((msg->bus == 0U) && !hyundai_camera_scc) || ((msg->bus == 2U) && hyundai_camera_scc)) {
if (!hyundai_longitudinal) {
acc_main_on = GET_BIT(msg, 0U);
}
}
}
if (msg->bus == 0U) {
if (msg->addr == 0x251U) {
int torque_driver_new = (GET_BYTES(msg, 0, 2) & 0x7ffU) - 1024U;
// update array of samples
update_sample(&torque_driver, torque_driver_new);
}
// ACC steering wheel buttons
if (addr == 1007) hyundai_cruise_buttons_alt = true; // CASPER_EV: 1007
if (addr == 1007) {
int cruise_button = (GET_BYTE(to_push, 7) >> 4) & 0x07U;
bool main_button = GET_BIT(to_push, 58U);
hyundai_common_cruise_buttons_check(cruise_button, main_button);
if (msg->addr == 0x391U) {
aol_button_press = GET_BIT(msg, 4U) ? AOL_BUTTON_PRESSED : AOL_BUTTON_NOT_PRESSED;
}
else if (addr == 0x4F1 && !hyundai_cruise_buttons_alt) {
int cruise_button = GET_BYTE(to_push, 0) & 0x7U;
bool main_button = GET_BIT(to_push, 3U);
// ACC steering wheel buttons
if (msg->addr == 0x4F1U) {
int cruise_button = msg->data[0] & 0x7U;
bool main_button = GET_BIT(msg, 3U);
hyundai_common_cruise_buttons_check(cruise_button, main_button);
}
// gas press, different for EV, hybrid, and ICE models
if ((addr == 0x371) && hyundai_ev_gas_signal) {
gas_pressed = (((GET_BYTE(to_push, 4) & 0x7FU) << 1) | GET_BYTE(to_push, 3) >> 7) != 0U;
} else if ((addr == 0x371) && hyundai_hybrid_gas_signal) {
gas_pressed = GET_BYTE(to_push, 7) != 0U;
} else if ((addr == 0x91) && hyundai_fcev_gas_signal) {
gas_pressed = GET_BYTE(to_push, 6) != 0U;
} else if ((addr == 0x260) && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
gas_pressed = (GET_BYTE(to_push, 7) >> 6) != 0U;
if ((msg->addr == 0x371U) && hyundai_ev_gas_signal) {
gas_pressed = (((msg->data[4] & 0x7FU) << 1) | (msg->data[3] >> 7)) != 0U;
} else if ((msg->addr == 0x371U) && hyundai_hybrid_gas_signal) {
gas_pressed = msg->data[7] != 0U;
} else if ((msg->addr == 0x91U) && hyundai_fcev_gas_signal) {
gas_pressed = msg->data[6] != 0U;
} else if ((msg->addr == 0x260U) && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
gas_pressed = (msg->data[7] >> 6) != 0U;
} else {
}
// sample wheel speed, averaging opposite corners
if (addr == 0x386) {
uint32_t front_left_speed = GET_BYTES(to_push, 0, 2) & 0x3FFFU;
uint32_t rear_right_speed = GET_BYTES(to_push, 6, 2) & 0x3FFFU;
if (msg->addr == 0x386U) {
uint32_t front_left_speed = GET_BYTES(msg, 0, 2) & 0x3FFFU;
uint32_t rear_right_speed = GET_BYTES(msg, 6, 2) & 0x3FFFU;
vehicle_moving = (front_left_speed > HYUNDAI_STANDSTILL_THRSLD) || (rear_right_speed > HYUNDAI_STANDSTILL_THRSLD);
}
if (addr == 0x394) {
brake_pressed = ((GET_BYTE(to_push, 5) >> 5U) & 0x3U) == 0x2U;
if (msg->addr == 0x394U) {
brake_pressed = ((msg->data[5] >> 5U) & 0x3U) == 0x2U;
}
bool stock_ecu_detected = (addr == 0x340);
// If openpilot is controlling longitudinal we need to ensure the radar is turned off
// Enforce by checking we don't see SCC12
if (hyundai_longitudinal && (addr == 0x421)) {
stock_ecu_detected = true;
if (msg->addr == 0x592U) {
acc_main_on = GET_BIT(msg, 34U);
bool cruise_engaged = GET_BIT(msg, 35U);
hyundai_common_cruise_state_check(cruise_engaged);
}
if (msg->addr == 0x595U) {
acc_main_on = GET_BIT(msg, 50U);
bool cruise_engaged = GET_BIT(msg, 51U);
hyundai_common_cruise_state_check(cruise_engaged);
}
if ((msg->addr == 0x260U) && hyundai_non_scc && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
acc_main_on = GET_BIT(msg, 25U);
bool cruise_engaged = GET_BIT(msg, 26U);
hyundai_common_cruise_state_check(cruise_engaged);
}
generic_rx_checks();
stock_ecu_check(stock_ecu_detected);
}
hyundai_common_reset_acc_main_on_mismatches();
}
uint32_t last_ts_lkas11_from_op = 0;
uint32_t last_ts_scc12_from_op = 0;
uint32_t last_ts_scc13_from_op = 0;
uint32_t last_ts_mdps12_from_op = 0;
uint32_t last_ts_fca11_from_op = 0;
uint32_t last_ts_fca12_from_op = 0;
uint32_t last_ts_lfahda_mfc_from_op = 0;
static bool hyundai_tx_hook(const CANPacket_t *to_send) {
const TorqueSteeringLimits HYUNDAI_STEERING_LIMITS = HYUNDAI_LIMITS(512, 10, 10);
const TorqueSteeringLimits HYUNDAI_STEERING_LIMITS_ALT = HYUNDAI_LIMITS(512, 10, 10);
static bool hyundai_tx_hook(const CANPacket_t *msg) {
const TorqueSteeringLimits HYUNDAI_STEERING_LIMITS = HYUNDAI_LIMITS(384, 3, 7);
const TorqueSteeringLimits HYUNDAI_STEERING_LIMITS_ALT = HYUNDAI_LIMITS(270, 2, 3);
const TorqueSteeringLimits HYUNDAI_STEERING_LIMITS_ALT_2 = HYUNDAI_LIMITS(170, 2, 3);
bool tx = true;
int addr = GET_ADDR(to_send);
// FCA11: Block any potential actuation
if (addr == 0x38D) {
int CR_VSM_DecCmd = GET_BYTE(to_send, 1);
bool FCA_CmdAct = GET_BIT(to_send, 20U);
bool CF_VSM_DecCmdAct = GET_BIT(to_send, 31U);
if (msg->addr == 0x38DU) {
int CR_VSM_DecCmd = msg->data[1];
bool FCA_CmdAct = GET_BIT(msg, 20U);
bool CF_VSM_DecCmdAct = GET_BIT(msg, 31U);
if ((CR_VSM_DecCmd != 0) || FCA_CmdAct || CF_VSM_DecCmdAct) {
tx = false;
}
}
// ACCEL: safety check
if (addr == 0x421) {
int desired_accel_raw = (((GET_BYTE(to_send, 4) & 0x7U) << 8) | GET_BYTE(to_send, 3)) - 1023U;
int desired_accel_val = ((GET_BYTE(to_send, 5) << 3) | (GET_BYTE(to_send, 4) >> 5)) - 1023U;
if (msg->addr == 0x420U) {
acc_main_on_tx = GET_BIT(msg, 0U);
hyundai_common_acc_main_on_sync();
}
int aeb_decel_cmd = GET_BYTE(to_send, 2);
bool aeb_req = GET_BIT(to_send, 54U);
// ACCEL: safety check
if (msg->addr == 0x421U) {
int desired_accel_raw = (((msg->data[4] & 0x7U) << 8) | msg->data[3]) - 1023U;
int desired_accel_val = ((msg->data[5] << 3) | (msg->data[4] >> 5)) - 1023U;
int aeb_decel_cmd = msg->data[2];
bool aeb_req = GET_BIT(msg, 54U);
bool violation = false;
violation |= longitudinal_accel_checks(desired_accel_raw, HYUNDAI_LONG_LIMITS);
violation |= longitudinal_accel_checks(desired_accel_val, HYUNDAI_LONG_LIMITS);
violation |= (aeb_decel_cmd != 0);
violation |= aeb_req;
if (!hyundai_escc) {
violation |= (aeb_decel_cmd != 0);
violation |= aeb_req;
}
if (violation) {
tx = false;
@@ -245,9 +267,9 @@ static bool hyundai_tx_hook(const CANPacket_t *to_send) {
}
// LKA STEER: safety check
if (addr == 0x340) {
int desired_torque = ((GET_BYTES(to_send, 0, 4) >> 16) & 0x7ffU) - 1024U;
bool steer_req = GET_BIT(to_send, 27U);
if (msg->addr == 0x340U) {
int desired_torque = ((GET_BYTES(msg, 0, 4) >> 16) & 0x7ffU) - 1024U;
bool steer_req = GET_BIT(msg, 27U);
const TorqueSteeringLimits limits = hyundai_alt_limits_2 ? HYUNDAI_STEERING_LIMITS_ALT_2 :
hyundai_alt_limits ? HYUNDAI_STEERING_LIMITS_ALT : HYUNDAI_STEERING_LIMITS;
@@ -258,199 +280,220 @@ static bool hyundai_tx_hook(const CANPacket_t *to_send) {
}
// UDS: Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
if (addr == 0x7D0) {
if ((GET_BYTES(to_send, 0, 4) != 0x00803E02U) || (GET_BYTES(to_send, 4, 4) != 0x0U)) {
if (msg->addr == 0x7D0U) {
if ((GET_BYTES(msg, 0, 4) != 0x00803E02U) || (GET_BYTES(msg, 4, 4) != 0x0U)) {
tx = false;
}
}
// BUTTONS: used for resume spamming and cruise cancellation
if ((addr == 0x4F1) && !hyundai_longitudinal) {
int button = GET_BYTE(to_send, 0) & 0x7U;
if ((msg->addr == 0x4F1U) && !hyundai_longitudinal) {
int button = msg->data[0] & 0x7U;
bool allowed_resume = (button == 1) && controls_allowed;
bool allowed_cancel = (button == 4) && (cruise_engaged_prev || controls_allowed);
if (!(allowed_resume || allowed_cancel)) {
bool allowed_set = (button == 2) && controls_allowed;
bool allowed_cancel = (button == 4) && cruise_engaged_prev;
if (!(allowed_resume || allowed_set || allowed_cancel)) {
tx = false;
}
}
uint32_t now = microsecond_timer_get();
if(addr == 832)
last_ts_lkas11_from_op = (tx == 0 ? 0 : now);
else if(addr == 1057)
last_ts_scc12_from_op = (tx == 0 ? 0 : now);
else if(addr == 593)
last_ts_mdps12_from_op = (tx == 0 ? 0 : now);
else if (addr == 909)
last_ts_fca11_from_op = (tx == 0 ? 0 : now);
else if (addr == 1155)
last_ts_fca12_from_op = (tx == 0 ? 0 : now);
else if(addr == 1290)
last_ts_scc13_from_op = (tx == 0 ? 0 : now);
else if(addr == 0x485)
last_ts_lfahda_mfc_from_op = (tx == 0 ? 0 : now);
return tx;
}
static bool hyundai_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
uint32_t now = microsecond_timer_get();
// forward cam to ccan and viceversa, except lkas cmd
if (bus_num == 0) {
bus_fwd = 2;
if(addr == 593) {
if(now - last_ts_mdps12_from_op < 200000) {
bus_fwd = -1;
}
}
}
if (bus_num == 2) {
bool is_lkas_msg = addr == 832;
bool is_lfahda_msg = addr == 1157;
bool is_scc_msg = addr == 1056 || addr == 1057 || addr == 905;
bool is_scc13_msg = addr == 1290;
bool is_fca11_msg = addr == 909;
bool is_fca12_msg = addr == 1155;
bool block_msg = is_lkas_msg || is_lfahda_msg || is_scc_msg || is_scc13_msg || is_fca11_msg || is_fca12_msg;
if (!block_msg) {
bus_fwd = 0;
}
else {
if(is_lkas_msg) {
if(now - last_ts_lkas11_from_op >= 200000) {
bus_fwd = 0;
}
}
else if(is_lfahda_msg) {
if (now - last_ts_lfahda_mfc_from_op >= 200000)
bus_fwd = 0;
}
else if (is_scc_msg) {
if (now - last_ts_scc12_from_op >= 400000)
bus_fwd = 0;
}
else if (is_scc13_msg) {
if (now - last_ts_scc13_from_op >= 800000)
bus_fwd = 0;
}
else if (is_fca11_msg) {
if (now - last_ts_fca11_from_op >= 400000)
bus_fwd = 0;
}
else if (is_fca12_msg) {
if (now - last_ts_fca12_from_op >= 400000)
bus_fwd = 0;
}
}
}
return bus_fwd == -1;
}
/* case
- legacy(on/off) + camera_scc(allways longitudinal on) + longitudinal(scc off)
*/
static safety_config hyundai_init_carrot(bool legacy_car) {
static const CanMsg HYUNDAI_LONG_TX_MSGS[] = {
{0x340, 0, 8}, // LKAS11 Bus 0
{0x4F1, 0, 4}, // CLU11 Bus 0
{0x485, 0, 8}, // LFAHDA_MFC Bus 0
{0x251, 2, 8}, // MDPS12 Bus 2
{0x420, 0, 8}, // SCC11 Bus 0
{0x421, 0, 8}, // SCC12 Bus 0
{0x50A, 0, 8}, // SCC13 Bus 0
{0x389, 0, 8}, // SCC14 Bus 0
{0x4A2, 0, 2}, // FRT_RADAR11 Bus 0
{0x38D, 0, 8}, // FCA11 Bus 0
{0x483, 0, 8}, // FCA12 Bus 0
{0x7D0, 0, 8}, // radar UDS TX addr Bus 0 (for radar disable)
};
static const CanMsg HYUNDAI_CAMERA_SCC_TX_MSGS[] = {
{0x340, 0, 8}, // LKAS11 Bus 0
{0x4F1, 2, 4}, // CLU11 Bus 2
{0x485, 0, 8}, // LFAHDA_MFC Bus 0
{593, 2, 8}, // MDPS12, Bus 2
{1056, 0, 8}, // SCC11, Bus 0
{1057, 0, 8}, // SCC12, Bus 0
{1290, 0, 8}, // SCC13, Bus 0
{905, 0, 8}, // SCC14, Bus 0
{909, 0, 8}, // FCA11 Bus 0
{1155, 0, 8}, // FCA12 Bus 0
{1186, 0, 8}, // FRT_RADAR11, Bus 0
{0x4F1, 0, 4}, // CLU11 Bus 0
};
safety_config ret;
if (hyundai_camera_scc) {
static RxCheck hyundai_cam_scc_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_SCC12_ADDR_CHECK(2)
};
static RxCheck hyundai_cam_scc_rx_checks_legacy[] = {
HYUNDAI_COMMON_RX_CHECKS(true)
HYUNDAI_SCC12_ADDR_CHECK(2)
};
if(legacy_car) ret = BUILD_SAFETY_CFG(hyundai_cam_scc_rx_checks_legacy, HYUNDAI_CAMERA_SCC_TX_MSGS);
else ret = BUILD_SAFETY_CFG(hyundai_cam_scc_rx_checks, HYUNDAI_CAMERA_SCC_TX_MSGS);
}
else if (hyundai_longitudinal) {
static RxCheck hyundai_long_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
// Use CLU11 (buttons) to manage controls allowed instead of SCC cruise state
{.msg = {{0x4F1, 0, 4, .ignore_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}
},
};
static RxCheck hyundai_long_rx_checks_legacy[] = {
HYUNDAI_COMMON_RX_CHECKS(true)
// Use CLU11 (buttons) to manage controls allowed instead of SCC cruise state
{.msg = {{0x4F1, 0, 4, .ignore_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}
},
};
if(legacy_car) ret = BUILD_SAFETY_CFG(hyundai_long_rx_checks_legacy, HYUNDAI_LONG_TX_MSGS);
else ret = BUILD_SAFETY_CFG(hyundai_long_rx_checks, HYUNDAI_LONG_TX_MSGS);
}
else {
static RxCheck hyundai_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_SCC12_ADDR_CHECK(0)
};
static RxCheck hyundai_rx_checks_legacy[] = {
HYUNDAI_COMMON_RX_CHECKS(true)
//HYUNDAI_SCC12_ADDR_CHECK(0)
};
if(legacy_car) ret = BUILD_SAFETY_CFG(hyundai_rx_checks_legacy, HYUNDAI_TX_MSGS);
else ret = BUILD_SAFETY_CFG(hyundai_rx_checks, HYUNDAI_TX_MSGS);
}
return ret;
}
static safety_config hyundai_init(uint16_t param) {
static const CanMsg HYUNDAI_LONG_TX_MSGS[] = {
HYUNDAI_LONG_COMMON_TX_MSGS(0)
{0x38D, 0, 8, .check_relay = false}, // FCA11 Bus 0
{0x483, 0, 8, .check_relay = false}, // FCA12 Bus 0
{0x7D0, 0, 8, .check_relay = false}, // radar UDS TX addr Bus 0 (for radar disable)
};
static const CanMsg HYUNDAI_CAMERA_SCC_TX_MSGS[] = {
HYUNDAI_COMMON_TX_MSGS(2)
};
static const CanMsg HYUNDAI_CAMERA_SCC_LONG_TX_MSGS[] = {
HYUNDAI_LONG_COMMON_TX_MSGS(2)
};
static const CanMsg HYUNDAI_LONG_ESCC_TX_MSGS[] = {
HYUNDAI_LONG_COMMON_TX_MSGS(0)
};
hyundai_common_init(param);
hyundai_legacy = false;
return hyundai_init_carrot(hyundai_legacy);
safety_config ret;
if (hyundai_longitudinal) {
// Use CLU11 (buttons) to manage controls allowed instead of SCC cruise state
static RxCheck hyundai_long_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
};
static RxCheck hyundai_lda_button_long_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
static RxCheck hyundai_fcev_long_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_FCEV_GAS_ADDR_CHECK
};
static RxCheck hyundai_fcev_lda_button_long_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_FCEV_GAS_ADDR_CHECK
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
if (hyundai_fcev_gas_signal) {
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_fcev_lda_button_long_rx_checks, ret);
} else {
SET_RX_CHECKS(hyundai_fcev_long_rx_checks, ret);
}
} else {
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_lda_button_long_rx_checks, ret);
} else {
SET_RX_CHECKS(hyundai_long_rx_checks, ret);
}
}
if (hyundai_escc) {
SET_TX_MSGS(HYUNDAI_LONG_ESCC_TX_MSGS, ret);
} else if (hyundai_camera_scc) {
SET_TX_MSGS(HYUNDAI_CAMERA_SCC_LONG_TX_MSGS, ret);
} else {
SET_TX_MSGS(HYUNDAI_LONG_TX_MSGS, ret);
}
} else if (hyundai_camera_scc) {
static RxCheck hyundai_cam_scc_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_SCC12_ADDR_CHECK(2)
HYUNDAI_SCC11_ADDR_CHECK(2)
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
ret = BUILD_SAFETY_CFG(hyundai_cam_scc_rx_checks, HYUNDAI_CAMERA_SCC_TX_MSGS);
} else {
static RxCheck hyundai_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_SCC12_ADDR_CHECK(0)
HYUNDAI_SCC11_ADDR_CHECK(0)
};
static RxCheck hyundai_lda_button_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_SCC12_ADDR_CHECK(0)
HYUNDAI_SCC11_ADDR_CHECK(0)
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
static RxCheck hyundai_fcev_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_SCC12_ADDR_CHECK(0)
HYUNDAI_SCC11_ADDR_CHECK(0)
HYUNDAI_FCEV_GAS_ADDR_CHECK
};
static RxCheck hyundai_fcev_lda_button_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_SCC12_ADDR_CHECK(0)
HYUNDAI_SCC11_ADDR_CHECK(0)
HYUNDAI_FCEV_GAS_ADDR_CHECK
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
static RxCheck hyundai_non_scc_addr_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
};
static RxCheck hyundai_non_scc_lda_button_addr_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
static RxCheck hyundai_hev_non_scc_addr_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_NON_SCC_HEV_ADDR_CHECK
};
static RxCheck hyundai_hev_non_scc_lda_button_addr_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_NON_SCC_HEV_ADDR_CHECK
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
static RxCheck hyundai_ev_non_scc_addr_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_NON_SCC_EV_ADDR_CHECK
};
static RxCheck hyundai_ev_non_scc_lda_button_addr_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_NON_SCC_EV_ADDR_CHECK
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
SET_TX_MSGS(HYUNDAI_TX_MSGS, ret);
if (hyundai_fcev_gas_signal) {
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_fcev_lda_button_rx_checks, ret);
} else {
SET_RX_CHECKS(hyundai_fcev_rx_checks, ret);
}
} else if (hyundai_non_scc) {
if (hyundai_ev_gas_signal) {
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_ev_non_scc_lda_button_addr_checks, ret);
} else {
SET_RX_CHECKS(hyundai_ev_non_scc_addr_checks, ret);
}
} else if (hyundai_hybrid_gas_signal) {
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_hev_non_scc_lda_button_addr_checks, ret);
} else {
SET_RX_CHECKS(hyundai_hev_non_scc_addr_checks, ret);
}
} else {
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_non_scc_lda_button_addr_checks, ret);
} else {
SET_RX_CHECKS(hyundai_non_scc_addr_checks, ret);
}
}
} else {
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_lda_button_rx_checks, ret);
} else {
SET_RX_CHECKS(hyundai_rx_checks, ret);
}
}
}
return ret;
}
static safety_config hyundai_legacy_init(uint16_t param) {
// older hyundai models have less checks due to missing counters and checksums
static RxCheck hyundai_legacy_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(true)
HYUNDAI_SCC12_ADDR_CHECK(0)
HYUNDAI_SCC11_ADDR_CHECK(0)
};
hyundai_common_init(param);
hyundai_legacy = true;
return hyundai_init_carrot(hyundai_legacy);
hyundai_longitudinal = false;
hyundai_camera_scc = false;
return BUILD_SAFETY_CFG(hyundai_legacy_rx_checks, HYUNDAI_TX_MSGS);
}
const safety_hooks hyundai_hooks = {
.init = hyundai_init,
.rx = hyundai_rx_hook,
.tx = hyundai_tx_hook,
.fwd = hyundai_fwd_hook,
.get_counter = hyundai_get_counter,
.get_checksum = hyundai_get_checksum,
.compute_checksum = hyundai_compute_checksum,
@@ -460,7 +503,6 @@ const safety_hooks hyundai_legacy_hooks = {
.init = hyundai_legacy_init,
.rx = hyundai_rx_hook,
.tx = hyundai_tx_hook,
.fwd = hyundai_fwd_hook,
.get_counter = hyundai_get_counter,
.get_checksum = hyundai_get_checksum,
.compute_checksum = hyundai_compute_checksum,

817
iqdbc_repo/iqdbc/safety/modes/hyundai_canfd.h
View File

@@ -3,550 +3,149 @@
#include "iqdbc/safety/declarations.h"
#include "iqdbc/safety/modes/hyundai_common.h"
#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
#pragma GCC diagnostic ignored "-Wunused-function"
#define HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(bus) \
{0x1CF, bus, 8, .check_relay = false}, /* CRUISE_BUTTON */ \
const TorqueSteeringLimits HYUNDAI_CANFD_STEERING_LIMITS = {
.max_torque = 512, //270,
.max_rt_delta = 112,
#define HYUNDAI_CANFD_LKA_STEERING_COMMON_TX_MSGS(a_can, e_can) \
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(e_can) \
{0x50, a_can, 16, .check_relay = (a_can) == 0}, /* LKAS */ \
{0x2A4, a_can, 24, .check_relay = (a_can) == 0}, /* CAM_0x2A4 */ \
.max_rate_up = 2,
.max_rate_down = 3,
.driver_torque_allowance = 250,
.driver_torque_multiplier = 2,
.type = TorqueDriverLimited,
#define HYUNDAI_CANFD_LKA_STEERING_ALT_COMMON_TX_MSGS(a_can, e_can) \
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(e_can) \
{0x110, a_can, 32, .check_relay = (a_can) == 0}, /* LKAS_ALT */ \
{0x362, a_can, 32, .check_relay = (a_can) == 0}, /* CAM_0x362 */ \
// the EPS faults when the steering angle is above a certain threshold for too long. to prevent this,
// we allow setting torque actuation bit to 0 while maintaining the requested torque value for two consecutive frames
.min_valid_request_frames = 89,
.max_invalid_request_frames = 2,
.min_valid_request_rt_interval = 810000, // 810ms; a ~10% buffer on cutting every 90 frames
.has_steer_req_tolerance = true,
};
const CanMsg HYUNDAI_CANFD_HDA2_TX_MSGS[] = {
{0x50, 0, 16}, // LKAS
{0x1CF, 1, 8}, // CRUISE_BUTTON
{0x2A4, 0, 24}, // CAM_0x2A4
};
const CanMsg HYUNDAI_CANFD_HDA2_ALT_STEERING_TX_MSGS[] = {
{0x110, 0, 32}, // LKAS_ALT
{0x1CF, 1, 8}, // CRUISE_BUTTON
{0x362, 0, 32}, // CAM_0x362
{0x1AA, 1, 16}, // CRUISE_ALT_BUTTONS , carrot
};
const CanMsg HYUNDAI_CANFD_HDA2_LONG_TX_MSGS[] = {
{0x50, 0, 16}, // LKAS
{0x1CF, 0, 8}, // CRUISE_BUTTON
{0x1CF, 1, 8}, // CRUISE_BUTTON
{0x1CF, 2, 8}, // CRUISE_BUTTON
{0x1AA, 0, 16}, // CRUISE_ALT_BUTTONS , carrot
{0x1AA, 1, 16}, // CRUISE_ALT_BUTTONS , carrot
{0x1AA, 2, 16}, // CRUISE_ALT_BUTTONS , carrot
{0x2A4, 0, 24}, // CAM_0x2A4
{0x51, 0, 32}, // ADRV_0x51
{0x730, 1, 8}, // tester present for ADAS ECU disable
{0x12A, 1, 16}, // LFA
{0x160, 1, 16}, // ADRV_0x160
{0x1E0, 1, 16}, // LFAHDA_CLUSTER
{0x1A0, 1, 32}, // CRUISE_INFO
{0x1EA, 1, 32}, // ADRV_0x1ea
{0x200, 1, 8}, // ADRV_0x200
{0x345, 1, 8}, // ADRV_0x345
{0x1DA, 1, 32}, // ADRV_0x1da
{0x12A, 0, 16}, // LFA
{0x1E0, 0, 16}, // LFAHDA_CLUSTER
{0x160, 0, 16}, // ADRV_0x160
{0x1EA, 0, 32}, // ADRV_0x1ea
{0x200, 0, 8}, // ADRV_0x200
{0x1A0, 0, 32}, // CRUISE_INFO
{0x345, 0, 8}, // ADRV_0x345
{0x1DA, 0, 32}, // ADRV_0x1da
{0x362, 0, 32}, // CAM_0x362
{0x362, 1, 32}, // CAM_0x362
{0x2a4, 1, 24}, // CAM_0x2a4
{0x110, 0, 32}, // LKAS_ALT (272)
{0x110, 1, 32}, // LKAS_ALT (272)
{0x50, 1, 16}, //
{0x51, 1, 32}, //
{353, 0, 32}, // ADRV_353
{354, 0, 32}, // CORNER_RADAR_HIGHWAY
{512, 0, 8}, // ADRV_0x200
{1187, 2, 8}, // 4A3
{1204, 2, 8}, // 4B4
{203, 0, 24}, // CB
{373, 2, 24}, // TCS(0x175)
{506, 2, 32}, // CLUSTER_SPEED_LIMIT
{234, 2, 24}, // MDPS
{687, 2, 8}, // STEER_TOUCH_2AF
{0x4BE, 2, 8}, // NEW_MSG_4BE (may be corner radar enabler x)
{0x4B9, 2, 8}, // NEW_MSG_4B9 (may be corner radar enabler)
};
const CanMsg HYUNDAI_CANFD_HDA1_TX_MSGS[] = {
{0x12A, 0, 16}, // LFA
{0x1A0, 0, 32}, // CRUISE_INFO
{0x1CF, 0, 8}, // CRUISE_BUTTON
{0x1CF, 2, 8}, // CRUISE_BUTTON
{0x1E0, 0, 16}, // LFAHDA_CLUSTER
{0x160, 0, 16}, // ADRV_0x160
{0x7D0, 0, 8}, // tester present for radar ECU disable
{0x1AA, 2, 16}, // CRUISE_ALT_BUTTONS , carrot
{203, 0, 24}, // CB
{373, 2, 24}, // TCS(0x175)
{353, 0, 32}, // ADRV_353
{354, 0, 32}, // CORNER_RADAR_HIGHWAY
{512, 0, 8}, // ADRV_0x200
{1187, 2, 8}, // 4A3
{1204, 2, 8}, // 4B4
{373, 2, 24}, // TCS(0x175)
{234, 2, 24}, // MDPS
{687, 2, 8}, // STEER_TOUCH_2AF
};
#define HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(e_can) \
{0x12A, e_can, 16, .check_relay = (e_can) == 0}, /* LFA */ \
{0x1E0, e_can, 16, .check_relay = (e_can) == 0}, /* LFAHDA_CLUSTER */ \
#define HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(e_can, longitudinal) \
{0x1A0, e_can, 32, .check_relay = (longitudinal)}, /* SCC_CONTROL */ \
// *** Addresses checked in rx hook ***
// EV, ICE, HYBRID: ACCELERATOR (0x35), ACCELERATOR_BRAKE_ALT (0x100), ACCELERATOR_ALT (0x105)
#define HYUNDAI_CANFD_COMMON_RX_CHECKS(pt_bus) \
{.msg = {{0x35, (pt_bus), 32, .max_counter = 0xffU, .frequency = 100U}, \
{0x100, (pt_bus), 32, .max_counter = 0xffU, .frequency = 100U}, \
{0x105, (pt_bus), 32, .max_counter = 0xffU, .frequency = 100U}}}, \
{.msg = {{0x175, (pt_bus), 24, .max_counter = 0xffU, .frequency = 50U}, { 0 }, { 0 }}}, \
{.msg = {{0xa0, (pt_bus), 24, .max_counter = 0xffU, .frequency = 100U}, { 0 }, { 0 }}}, \
{.msg = {{0xea, (pt_bus), 24, .max_counter = 0xffU, .frequency = 100U}, { 0 }, { 0 }}}, \
#define HYUNDAI_CANFD_COMMON_RX_CHECKS(pt_bus) \
{.msg = {{0x35, (pt_bus), 32, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}, \
{0x100, (pt_bus), 32, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}, \
{0x105, (pt_bus), 32, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}}}, \
{.msg = {{0x175, (pt_bus), 24, 50U, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0xa0, (pt_bus), 24, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0xea, (pt_bus), 24, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(pt_bus) \
{.msg = {{0x1cf, (pt_bus), 8, .ignore_checksum = true, .max_counter = 0xfU, .frequency = 50U}, { 0 }, { 0 }}}, \
#define HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(pt_bus) \
HYUNDAI_CANFD_COMMON_RX_CHECKS(pt_bus) \
{.msg = {{0x1cf, (pt_bus), 8, 50U, .ignore_checksum = true, .max_counter = 0xfU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(pt_bus) \
{.msg = {{0x1aa, (pt_bus), 16, .ignore_checksum = true, .max_counter = 0xffU, .frequency = 50U}, { 0 }, { 0 }}}, \
#define HYUNDAI_CANFD_ALT_BUTTONS_RX_CHECKS(pt_bus) \
HYUNDAI_CANFD_COMMON_RX_CHECKS(pt_bus) \
{.msg = {{0x1aa, (pt_bus), 16, 50U, .ignore_checksum = true, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
// SCC_CONTROL (from ADAS unit or camera)
#define HYUNDAI_CANFD_SCC_ADDR_CHECK(scc_bus) \
{.msg = {{0x1a0, (scc_bus), 32, .max_counter = 0xffU, .frequency = 50U}, { 0 }, { 0 }}}, \
#define HYUNDAI_CANFD_SCC_ADDR_CHECK(scc_bus) \
{.msg = {{0x1a0, (scc_bus), 32, 50U, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
//static bool hyundai_canfd_alt_buttons = false;
//static bool hyundai_canfd_hda2_alt_steering = false;
static bool hyundai_canfd_alt_buttons = false;
static bool hyundai_canfd_lka_steering_alt = false;
// *** Non-HDA2 checks ***
// Camera sends SCC messages on HDA1.
// Both button messages exist on some platforms, so we ensure we track the correct one using flag
RxCheck hyundai_canfd_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
};
RxCheck hyundai_canfd_alt_buttons_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
};
// Longitudinal checks for HDA1
RxCheck hyundai_canfd_long_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(0)
};
RxCheck hyundai_canfd_long_alt_buttons_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(0)
};
// Radar sends SCC messages on these cars instead of camera
RxCheck hyundai_canfd_radar_scc_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(0)
};
RxCheck hyundai_canfd_radar_scc_alt_buttons_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(0)
};
// *** HDA2 checks ***
// E-CAN is on bus 1, ADAS unit sends SCC messages on HDA2.
// Does not use the alt buttons message
RxCheck hyundai_canfd_hda2_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(1)
HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(1) // TODO: carrot: canival no 0x1cf
HYUNDAI_CANFD_SCC_ADDR_CHECK(1)
};
RxCheck hyundai_canfd_hda2_rx_checks_scc2[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(0) // TODO: carrot: canival no 0x1cf
HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
};
RxCheck hyundai_canfd_hda2_alt_buttons_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(1)
HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(1)
HYUNDAI_CANFD_SCC_ADDR_CHECK(1)
};
RxCheck hyundai_canfd_hda2_alt_buttons_rx_checks_scc2[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
};
RxCheck hyundai_canfd_hda2_long_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(1)
HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(1) // TODO: carrot: canival no 0x1cf
};
RxCheck hyundai_canfd_hda2_long_rx_checks_scc2[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(0)
};
RxCheck hyundai_canfd_hda2_long_alt_buttons_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(1)
HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(1)
};
RxCheck hyundai_canfd_hda2_long_alt_buttons_rx_checks_scc2[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(0)
};
const int HYUNDAI_PARAM_CANFD_ALT_BUTTONS = 32;
const int HYUNDAI_PARAM_CANFD_HDA2_ALT_STEERING = 128;
bool hyundai_canfd_alt_buttons = false;
bool hyundai_canfd_hda2_alt_steering = false;
bool hyundai_canfd_buffered_fwd = false;
int hyundai_canfd_hda2_get_lkas_addr(void) {
return hyundai_canfd_hda2_alt_steering ? 0x110 : 0x50;
static unsigned int hyundai_canfd_get_lka_addr(void) {
return hyundai_canfd_lka_steering_alt ? 0x110U : 0x50U;
}
static uint8_t hyundai_canfd_get_counter(const CANPacket_t* to_push) {
static uint8_t hyundai_canfd_get_counter(const CANPacket_t *msg) {
uint8_t ret = 0;
if (GET_LEN(to_push) == 8U) {
ret = GET_BYTE(to_push, 1) >> 4;
}
else {
ret = GET_BYTE(to_push, 2);
if (GET_LEN(msg) == 8U) {
ret = msg->data[1] >> 4;
} else {
ret = msg->data[2];
}
return ret;
}
static uint32_t hyundai_canfd_get_checksum(const CANPacket_t* to_push) {
uint32_t chksum = GET_BYTE(to_push, 0) | (GET_BYTE(to_push, 1) << 8);
static uint32_t hyundai_canfd_get_checksum(const CANPacket_t *msg) {
uint32_t chksum = msg->data[0] | (msg->data[1] << 8);
return chksum;
}
static void hyundai_canfd_rx_hook(const CANPacket_t *msg) {
typedef struct {
int addr;
int bus; // forwarding block <20><><EFBFBD> tx bus: 0 or 2
int hz;
uint32_t timeout_us;
uint32_t last_tx_us;
} CanfdTxState;
const unsigned pt_bus = hyundai_canfd_lka_steering ? 1U : 0U;
const unsigned int scc_bus = hyundai_camera_scc ? 2U : pt_bus;
// forwarding block<63><6B>: bus 0,2<><32> <20><><EFBFBD>
CanfdTxState canfd_tx_states[] = {
{0x50, 0, 100, 0U, 0U}, // 80: LKAS
{0x51, 0, 100, 0U, 0U}, // 81: ADRV_0x51
{0x110, 0, 100, 0U, 0U}, // 272: LKAS_ALT
{0x12A, 0, 100, 0U, 0U}, // 298: LFA
{0x160, 0, 50, 0U, 0U}, // 352: ADRV_0x160
{0x161, 0, 20, 0U, 0U}, // 353: ADRV_0x161
{0x162, 0, 20, 0U, 0U}, // 354: CCNC_0x162
{0x1A0, 0, 50, 0U, 0U}, // 416: SCC_CONTROL
{0x1DA, 0, 1, 0U, 0U}, // 474: ADRV_0x1da
{0x1E0, 0, 20, 0U, 0U}, // 480: LFAHDA_CLUSTER
{0x1EA, 0, 20, 0U, 0U}, // 490: ADRV_0x1ea
{0x200, 0, 20, 0U, 0U}, // 512: ADRV_0x200
{0x2A4, 0, 20, 0U, 0U}, // 676: CAM_0x2a4
{0x345, 0, 5, 0U, 0U}, // 837: ADRV_0x345
{0x362, 0, 10, 0U, 0U}, // 866: CAM_0x362
{0x0CB, 0, 100, 0U, 0U}, // 203: LFA_ALT
{0x175, 2, 50, 0U, 0U}, // 373: TCS
{0x1AA, 2, 50, 0U, 0U}, // 426: CRUISE_ALT_BUTTONS
{0x1CF, 2, 50, 0U, 0U}, // 463: CRUISE_BUTTON
{0x1FA, 2, 10, 0U, 0U}, // 506: CLUSTER_SPEED_LIMIT
{0x0EA, 2, 100, 0U, 0U}, // 234: MDPS
{0x2AF, 2, 10, 0U, 0U}, // 687: STEER_TOUCH_2AF
{0x4A3, 2, 5, 0U, 0U}, // 1187: HDA_INFO_4A3
{0x4B4, 2, 10, 0U, 0U}, // 1204: NEW_MSG_4B4
{0x4BE, 2, 10, 0U, 0U}, // 1214: NEW_MSG_4BE
{0x4B9, 2, 10, 0U, 0U}, // 1209: NEW_MSG_4B9
{0, 0, 0, 0U, 0U},
};
static CanfdTxState* find_canfd_tx_state(int bus, int addr) {
for (int i = 0; canfd_tx_states[i].addr > 0; i++) {
if ((canfd_tx_states[i].addr == addr) && (canfd_tx_states[i].bus == bus)) {
return &canfd_tx_states[i];
}
}
return NULL;
}
static void hyundai_canfd_set_counter(CANPacket_t* to_push, uint8_t counter) {
if (GET_LEN(to_push) == 8U) {
to_push->data[1] = (to_push->data[1] & 0x0FU) | ((counter & 0x0FU) << 4);
}
else {
to_push->data[2] = counter;
}
}
static void hyundai_canfd_set_checksum(CANPacket_t* to_push, uint16_t checksum) {
to_push->data[0] = (uint8_t)(checksum & 0xFFU);
to_push->data[1] = (uint8_t)((checksum >> 8U) & 0xFFU);
}
static void hyundai_canfd_update_checksum(CANPacket_t* to_push) {
to_push->data[0] = 0U;
to_push->data[1] = 0U;
uint32_t checksum = hyundai_common_canfd_compute_checksum(to_push);
hyundai_canfd_set_checksum(to_push, (uint16_t)checksum);
}
static void canfd_apply_counter_and_update_checksum(CANPacket_t* dst, uint8_t counter) {
hyundai_canfd_set_counter(dst, counter);
hyundai_canfd_update_checksum(dst);
}
static void canfd_record_tx_time(int bus, int addr, bool tx) {
CanfdTxState* st = find_canfd_tx_state(bus, addr);
if (st != NULL) {
st->last_tx_us = tx ? microsecond_timer_get() : 0U;
}
}
static bool canfd_should_block_fwd(int tx_bus, int addr, uint32_t now) {
CanfdTxState* st = find_canfd_tx_state(tx_bus, addr);
if (st == NULL) {
return false;
}
return (now - st->last_tx_us) < st->timeout_us;
}
#define CANFD_BFWD_MAX_QUEUE 2
#define CANFD_BFWD_REUSE_MAX 2
typedef struct {
int addr;
int dst_bus;
bool enabled;
bool started;
uint8_t head;
uint8_t tail;
uint8_t count;
uint8_t reuse_left;
bool has_last_pkt;
CANPacket_t last_pkt;
CANPacket_t q[CANFD_BFWD_MAX_QUEUE];
} CanfdBufferedFwd;
CanfdBufferedFwd canfd_bfwd[] = {
{.addr = 0x1A0, .dst_bus = 0, .enabled = true }, // SCC_CONTROL
{.addr = 0x12A, .dst_bus = 0, .enabled = true }, // LFA
{.addr = 0x0CB, .dst_bus = 0, .enabled = true }, // LFA_ALT
{.addr = 0x0EA, .dst_bus = 2, .enabled = true }, // MDPS
{.addr = 0x1AA, .dst_bus = 2, .enabled = true }, // CRUISE_ALT_BUTTONS
// {.addr = 0x1CF, .dst_bus = 2, .enabled = true }, // CRUISE_BUTTON
{.addr = 0x175, .dst_bus = 2, .enabled = true }, // TCS
{ 0 },
};
static void canfd_copy_packet(CANPacket_t* dst, const CANPacket_t* src) {
dst->fd = src->fd;
dst->returned = 0U;
dst->rejected = 0U;
dst->extended = src->extended;
dst->addr = src->addr;
dst->bus = src->bus;
dst->data_len_code = src->data_len_code;
for (uint8_t i = 0U; i < GET_LEN(src); i++) {
dst->data[i] = src->data[i];
}
}
static CanfdBufferedFwd* canfd_bfwd_find(int addr, int dst_bus) {
for (int i = 0; canfd_bfwd[i].addr > 0; i++) {
if (canfd_bfwd[i].enabled &&
(canfd_bfwd[i].addr == addr) &&
(canfd_bfwd[i].dst_bus == dst_bus)) {
return &canfd_bfwd[i];
}
}
return NULL;
}
static void canfd_bfwd_reset(CanfdBufferedFwd* st) {
st->started = false;
st->head = 0U;
st->tail = 0U;
st->count = 0U;
st->reuse_left = 0U;
st->has_last_pkt = false;
st->last_pkt = (CANPacket_t){0};
}
static void canfd_bfwd_push(CanfdBufferedFwd* st, const CANPacket_t* pkt) {
if ((st == NULL) || !st->enabled) return;
if (GET_BUS(pkt) != st->dst_bus) return;
// queue<75><65> <20>̹<EFBFBD> 2<><32><EFBFBD><EFBFBD> <20>̹<EFBFBD> <20><> packet<65><74> <20><><EFBFBD><EFBFBD>
if (st->count >= CANFD_BFWD_MAX_QUEUE) {
return;
}
canfd_copy_packet(&st->q[st->tail], pkt);
st->tail = (st->tail + 1U) % CANFD_BFWD_MAX_QUEUE;
st->count++;
st->started = true;
}
static bool canfd_bfwd_pop(CanfdBufferedFwd* st, CANPacket_t* pkt) {
if ((st == NULL) || !st->enabled) {
return false;
}
if (!st->started || (st->count == 0U)) {
return false;
}
canfd_copy_packet(pkt, &st->q[st->head]);
st->head = (st->head + 1U) % CANFD_BFWD_MAX_QUEUE;
st->count--;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> packet <20><><EFBFBD><EFBFBD>
canfd_copy_packet(&st->last_pkt, pkt);
st->has_last_pkt = true;
st->reuse_left = CANFD_BFWD_REUSE_MAX;
if (st->count == 0U) {
st->started = false;
}
return true;
}
static bool canfd_bfwd_reuse_last(CanfdBufferedFwd* st, CANPacket_t* pkt) {
if ((st == NULL) || !st->enabled) {
return false;
}
if (!st->has_last_pkt || (st->reuse_left == 0U)) {
return false;
}
canfd_copy_packet(pkt, &st->last_pkt);
st->reuse_left--;
return true;
}
static void hyundai_canfd_rx_hook(const CANPacket_t *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
int pt_bus = hyundai_canfd_hda2 ? 1 : 0;
const int scc_bus = hyundai_camera_scc ? 2 : pt_bus;
if (hyundai_camera_scc) pt_bus = 0;
if (bus == pt_bus) {
if (msg->bus == pt_bus) {
// driver torque
if (addr == 0xea) {
int torque_driver_new = ((GET_BYTE(to_push, 11) & 0x1fU) << 8U) | GET_BYTE(to_push, 10);
if (msg->addr == 0xeaU) {
int torque_driver_new = ((msg->data[11] & 0x1fU) << 8U) | msg->data[10];
torque_driver_new -= 4095;
update_sample(&torque_driver, torque_driver_new);
}
// cruise buttons
const int button_addr = hyundai_canfd_alt_buttons ? 0x1aa : 0x1cf;
if (addr == button_addr) {
const unsigned int button_addr = hyundai_canfd_alt_buttons ? 0x1aaU : 0x1cfU;
if (msg->addr == button_addr) {
bool main_button = false;
int cruise_button = 0;
if (addr == 0x1cf) {
cruise_button = GET_BYTE(to_push, 2) & 0x7U;
main_button = GET_BIT(to_push, 19U);
if (msg->addr == 0x1cfU) {
cruise_button = msg->data[2] & 0x7U;
main_button = GET_BIT(msg, 19U);
aol_button_press = GET_BIT(msg, 23U) ? AOL_BUTTON_PRESSED : AOL_BUTTON_NOT_PRESSED;
} else {
cruise_button = (GET_BYTE(to_push, 4) >> 4) & 0x7U;
main_button = GET_BIT(to_push, 34U);
cruise_button = (msg->data[4] >> 4) & 0x7U;
main_button = GET_BIT(msg, 34U);
aol_button_press = GET_BIT(msg, 39U) ? AOL_BUTTON_PRESSED : AOL_BUTTON_NOT_PRESSED;
}
hyundai_common_cruise_buttons_check(cruise_button, main_button);
}
// gas press, different for EV, hybrid, and ICE models
if ((addr == 0x35) && hyundai_ev_gas_signal) {
gas_pressed = GET_BYTE(to_push, 5) != 0U;
} else if ((addr == 0x105) && hyundai_hybrid_gas_signal) {
gas_pressed = GET_BIT(to_push, 103U) || (GET_BYTE(to_push, 13) != 0U) || GET_BIT(to_push, 112U);
} else if ((addr == 0x100) && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
gas_pressed = GET_BIT(to_push, 176U);
if ((msg->addr == 0x35U) && hyundai_ev_gas_signal) {
gas_pressed = msg->data[5] != 0U;
} else if ((msg->addr == 0x105U) && hyundai_hybrid_gas_signal) {
gas_pressed = GET_BIT(msg, 103U) || (msg->data[13] != 0U) || GET_BIT(msg, 112U);
} else if ((msg->addr == 0x100U) && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
gas_pressed = GET_BIT(msg, 176U);
} else {
}
// brake press
if (addr == 0x175) {
brake_pressed = GET_BIT(to_push, 81U);
if (msg->addr == 0x175U) {
brake_pressed = GET_BIT(msg, 81U);
}
// vehicle moving
if (addr == 0xa0) {
uint32_t fl = (GET_BYTES(to_push, 8, 2)) & 0x3FFFU;
uint32_t fr = (GET_BYTES(to_push, 10, 2)) & 0x3FFFU;
uint32_t rl = (GET_BYTES(to_push, 12, 2)) & 0x3FFFU;
uint32_t rr = (GET_BYTES(to_push, 14, 2)) & 0x3FFFU;
if (msg->addr == 0xa0U) {
uint32_t fl = (GET_BYTES(msg, 8, 2)) & 0x3FFFU;
uint32_t fr = (GET_BYTES(msg, 10, 2)) & 0x3FFFU;
uint32_t rl = (GET_BYTES(msg, 12, 2)) & 0x3FFFU;
uint32_t rr = (GET_BYTES(msg, 14, 2)) & 0x3FFFU;
vehicle_moving = (fl > HYUNDAI_STANDSTILL_THRSLD) || (fr > HYUNDAI_STANDSTILL_THRSLD) ||
(rl > HYUNDAI_STANDSTILL_THRSLD) || (rr > HYUNDAI_STANDSTILL_THRSLD);
// average of all 4 wheel speeds. Conversion: raw * 0.03125 / 3.6 = m/s
UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4.0 * 0.03125 / 3.6);
UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4.0 * 0.03125 * KPH_TO_MS);
}
}
if (bus == scc_bus) {
if (msg->bus == scc_bus) {
// cruise state
if ((addr == 0x1a0) && !hyundai_longitudinal) {
if ((msg->addr == 0x1a0U) && !hyundai_longitudinal) {
// 1=enabled, 2=driver override
int cruise_status = ((GET_BYTE(to_push, 8) >> 4) & 0x7U);
int cruise_status = ((msg->data[8] >> 4) & 0x7U);
bool cruise_engaged = (cruise_status == 1) || (cruise_status == 2);
hyundai_common_cruise_state_check(cruise_engaged);
acc_main_on = GET_BIT(msg, 66U);
}
}
const int steer_addr = hyundai_canfd_hda2 ? hyundai_canfd_hda2_get_lkas_addr() : 0x12a;
bool stock_ecu_detected = (addr == steer_addr) && (bus == 0);
if (hyundai_longitudinal) {
// on HDA2, ensure ADRV ECU is still knocked out
// on others, ensure accel msg is blocked from camera
const int stock_scc_bus = hyundai_canfd_hda2 ? 1 : 0;
stock_ecu_detected = stock_ecu_detected || ((addr == 0x1a0) && (bus == stock_scc_bus));
}
generic_rx_checks();
stock_ecu_check(stock_ecu_detected);
hyundai_common_reset_acc_main_on_mismatches();
}
static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send_const) {
CANPacket_t* to_send = (CANPacket_t*)to_send_const;
static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) {
const TorqueSteeringLimits HYUNDAI_CANFD_STEERING_LIMITS = {
.max_torque = 512,
.max_torque = 270,
.max_rt_delta = 112,
.max_rate_up = 10,
.max_rate_down = 10,
.max_rate_up = 2,
.max_rate_down = 3,
.driver_torque_allowance = 250,
.driver_torque_multiplier = 2,
.type = TorqueDriverLimited,
@@ -560,14 +159,12 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send_const) {
};
bool tx = true;
int addr = GET_ADDR(to_send);
bool violation = false;
// steering
const int steer_addr = (hyundai_canfd_hda2 && !hyundai_longitudinal) ? hyundai_canfd_hda2_get_lkas_addr() : 0x12a;
if (addr == steer_addr) {
int desired_torque = (((GET_BYTE(to_send, 6) & 0xFU) << 7U) | (GET_BYTE(to_send, 5) >> 1U)) - 1024U;
bool steer_req = GET_BIT(to_send, 52U);
const unsigned int steer_addr = (hyundai_canfd_lka_steering && !hyundai_longitudinal) ? hyundai_canfd_get_lka_addr() : 0x12aU;
if (msg->addr == steer_addr) {
int desired_torque = (((msg->data[6] & 0xFU) << 7U) | (msg->data[5] >> 1U)) - 1024U;
bool steer_req = GET_BIT(msg, 52U);
if (steer_torque_cmd_checks(desired_torque, steer_req, HYUNDAI_CANFD_STEERING_LIMITS)) {
tx = false;
@@ -575,166 +172,209 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send_const) {
}
// cruise buttons check
if (addr == 0x1cf) {
int button = GET_BYTE(to_send, 2) & 0x7U;
if (msg->addr == 0x1cfU) {
int button = msg->data[2] & 0x7U;
bool is_cancel = (button == HYUNDAI_BTN_CANCEL);
bool is_resume = (button == HYUNDAI_BTN_RESUME);
bool is_set = (button == HYUNDAI_BTN_SET);
bool allowed = (is_cancel && (cruise_engaged_prev || controls_allowed)) || (is_resume && controls_allowed);
bool allowed = (is_cancel && cruise_engaged_prev) || ((is_resume || is_set) && controls_allowed);
if (!allowed) {
tx = false;
}
}
// UDS: only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
if ((addr == 0x730) && hyundai_canfd_hda2) {
if ((GET_BYTES(to_send, 0, 4) != 0x00803E02U) || (GET_BYTES(to_send, 4, 4) != 0x0U)) {
if (((msg->addr == 0x730U) && hyundai_canfd_lka_steering) || ((msg->addr == 0x7D0U) && !hyundai_camera_scc)) {
if ((GET_BYTES(msg, 0, 4) != 0x00803E02U) || (GET_BYTES(msg, 4, 4) != 0x0U)) {
tx = false;
}
}
// ACCEL: safety check
if (addr == 0x1a0) {
int desired_accel_raw = (((GET_BYTE(to_send, 17) & 0x7U) << 8) | GET_BYTE(to_send, 16)) - 1023U;
int desired_accel_val = ((GET_BYTE(to_send, 18) << 4) | (GET_BYTE(to_send, 17) >> 4)) - 1023U;
if (msg->addr == 0x1a0U) {
int desired_accel_raw = (((msg->data[17] & 0x7U) << 8) | msg->data[16]) - 1023U;
int desired_accel_val = ((msg->data[18] << 4) | (msg->data[17] >> 4)) - 1023U;
bool violation = false;
if (hyundai_longitudinal) {
violation |= longitudinal_accel_checks(desired_accel_raw, HYUNDAI_LONG_LIMITS);
violation |= longitudinal_accel_checks(desired_accel_val, HYUNDAI_LONG_LIMITS);
}
else {
} else {
// only used to cancel on here
const int acc_mode = (GET_BYTE(to_send, 8) >> 4) & 0x7U;
const int acc_mode = (msg->data[8] >> 4) & 0x7U;
if (acc_mode != 4) {
violation = true;
}
if ((desired_accel_raw != 0) || (desired_accel_val != 0)) {
violation = true;
}
}
}
if (violation) {
tx = false;
}
else if (hyundai_canfd_buffered_fwd) {
CanfdBufferedFwd* bfwd = canfd_bfwd_find(addr, GET_BUS(to_send));
if (bfwd != NULL) {
canfd_bfwd_push(bfwd, to_send);
//tx = false;
return true;
if (violation) {
tx = false;
}
}
canfd_record_tx_time(GET_BUS(to_send), addr, tx);
acc_main_on_tx = GET_BIT(msg, 66U);
hyundai_common_acc_main_on_sync();
}
return tx;
}
static bool hyundai_canfd_fwd_hook(int bus_num, int addr) {
uint32_t now = microsecond_timer_get();
if (bus_num == 0) {
if (canfd_should_block_fwd(2, addr, now)) {
return true;
}
if (addr == 0x4B9) {
return true;
}
return false;
}
return (bus_num != 2) || canfd_should_block_fwd(0, addr, now);
}
static safety_config hyundai_canfd_init(uint16_t param) {
const uint16_t HYUNDAI_PARAM_CANFD_LKA_STEERING_ALT = 128;
const uint16_t HYUNDAI_PARAM_CANFD_ALT_BUTTONS = 32;
for (int i = 0; canfd_tx_states[i].addr > 0; i++) {
canfd_tx_states[i].timeout_us = (uint32_t)(1000000.0 / canfd_tx_states[i].hz) + 20000U;
canfd_tx_states[i].last_tx_us = 0U;
}
static const CanMsg HYUNDAI_CANFD_LKA_STEERING_TX_MSGS[] = {
HYUNDAI_CANFD_LKA_STEERING_COMMON_TX_MSGS(0, 1)
};
for (int i = 0; canfd_bfwd[i].addr > 0; i++) {
canfd_bfwd_reset(&canfd_bfwd[i]);
}
static const CanMsg HYUNDAI_CANFD_LKA_STEERING_ALT_TX_MSGS[] = {
HYUNDAI_CANFD_LKA_STEERING_ALT_COMMON_TX_MSGS(0, 1)
};
static const CanMsg HYUNDAI_CANFD_LKA_STEERING_LONG_TX_MSGS[] = {
HYUNDAI_CANFD_LKA_STEERING_COMMON_TX_MSGS(0, 1)
HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(1)
HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(1, true)
{0x51, 0, 32, .check_relay = false}, // ADRV_0x51
{0x730, 1, 8, .check_relay = false}, // tester present for ADAS ECU disable
{0x160, 1, 16, .check_relay = false}, // ADRV_0x160
{0x1EA, 1, 32, .check_relay = false}, // ADRV_0x1ea
{0x200, 1, 8, .check_relay = false}, // ADRV_0x200
{0x345, 1, 8, .check_relay = false}, // ADRV_0x345
{0x1DA, 1, 32, .check_relay = false}, // ADRV_0x1da
};
static const CanMsg HYUNDAI_CANFD_LFA_STEERING_TX_MSGS[] = {
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(2)
HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(0)
HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(0, false)
};
// ADRV_0x160 is checked for radar liveness
static const CanMsg HYUNDAI_CANFD_LFA_STEERING_LONG_TX_MSGS[] = {
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(2)
HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(0)
HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(0, true)
{0x160, 0, 16, .check_relay = true}, // ADRV_0x160
{0x7D0, 0, 8, .check_relay = false}, // tester present for radar ECU disable
};
// ADRV_0x160 is checked for relay malfunction
#define HYUNDAI_CANFD_LFA_STEERING_CAMERA_SCC_TX_MSGS(longitudinal) \
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(2) \
HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(0) \
HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(0, (longitudinal)) \
{0x160, 0, 16, .check_relay = (longitudinal)}, /* ADRV_0x160 */ \
hyundai_common_init(param);
gen_crc_lookup_table_16(0x1021, hyundai_canfd_crc_lut);
hyundai_canfd_alt_buttons = GET_FLAG(param, HYUNDAI_PARAM_CANFD_ALT_BUTTONS);
hyundai_canfd_hda2_alt_steering = GET_FLAG(param, HYUNDAI_PARAM_CANFD_HDA2_ALT_STEERING);
hyundai_canfd_buffered_fwd = hyundai_camera_scc;
hyundai_canfd_lka_steering_alt = GET_FLAG(param, HYUNDAI_PARAM_CANFD_LKA_STEERING_ALT);
// no long for radar-SCC HDA1 yet
//if (!hyundai_canfd_hda2 && !hyundai_camera_scc) {
// hyundai_longitudinal = false;
//}
safety_config ret;
if (hyundai_longitudinal) {
if (hyundai_canfd_hda2) {
if (hyundai_canfd_alt_buttons) { // carrot : for CANIVAL 4TH HDA2
if (hyundai_camera_scc) ret = BUILD_SAFETY_CFG(hyundai_canfd_hda2_long_alt_buttons_rx_checks_scc2, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS);
else ret = BUILD_SAFETY_CFG(hyundai_canfd_hda2_long_alt_buttons_rx_checks, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS);
}
else {
if (hyundai_camera_scc) ret = BUILD_SAFETY_CFG(hyundai_canfd_hda2_long_rx_checks_scc2, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS);
else ret = BUILD_SAFETY_CFG(hyundai_canfd_hda2_long_rx_checks, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS);
}
} else {
ret = hyundai_canfd_alt_buttons ? BUILD_SAFETY_CFG(hyundai_canfd_long_alt_buttons_rx_checks, HYUNDAI_CANFD_HDA1_TX_MSGS) : \
BUILD_SAFETY_CFG(hyundai_canfd_long_rx_checks, HYUNDAI_CANFD_HDA1_TX_MSGS);
}
} else {
if (hyundai_canfd_hda2 && hyundai_camera_scc) {
if (hyundai_canfd_alt_buttons) { // carrot : for CANIVAL 4TH HDA2
ret = hyundai_canfd_hda2_alt_steering ? BUILD_SAFETY_CFG(hyundai_canfd_hda2_alt_buttons_rx_checks_scc2, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(hyundai_canfd_hda2_alt_buttons_rx_checks_scc2, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS);
}
else {
ret = hyundai_canfd_hda2_alt_steering ? BUILD_SAFETY_CFG(hyundai_canfd_hda2_rx_checks_scc2, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(hyundai_canfd_hda2_rx_checks_scc2, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS);
}
}else if (hyundai_canfd_hda2) {
if (hyundai_canfd_alt_buttons) { // carrot : for CANIVAL 4TH HDA2
ret = hyundai_canfd_hda2_alt_steering ? BUILD_SAFETY_CFG(hyundai_canfd_hda2_alt_buttons_rx_checks, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(hyundai_canfd_hda2_alt_buttons_rx_checks, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS);
}
else {
ret = hyundai_canfd_hda2_alt_steering ? BUILD_SAFETY_CFG(hyundai_canfd_hda2_rx_checks, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(hyundai_canfd_hda2_rx_checks, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS);
}
} else if (!hyundai_camera_scc) {
static RxCheck hyundai_canfd_radar_scc_alt_buttons_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(0)
if (hyundai_canfd_lka_steering) {
static RxCheck hyundai_canfd_lka_steering_long_rx_checks[] = {
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(1)
};
ret = BUILD_SAFETY_CFG(hyundai_canfd_lka_steering_long_rx_checks, HYUNDAI_CANFD_LKA_STEERING_LONG_TX_MSGS);
} else {
// Longitudinal checks for LFA steering
static RxCheck hyundai_canfd_long_rx_checks[] = {
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(0)
};
static RxCheck hyundai_canfd_alt_buttons_long_rx_checks[] = {
HYUNDAI_CANFD_ALT_BUTTONS_RX_CHECKS(0)
};
static CanMsg hyundai_canfd_lfa_steering_camera_scc_tx_msgs[] = {
HYUNDAI_CANFD_LFA_STEERING_CAMERA_SCC_TX_MSGS(true)
};
if (hyundai_canfd_alt_buttons) {
SET_RX_CHECKS(hyundai_canfd_alt_buttons_long_rx_checks, ret);
} else {
SET_RX_CHECKS(hyundai_canfd_long_rx_checks, ret);
}
if (hyundai_camera_scc) {
SET_TX_MSGS(hyundai_canfd_lfa_steering_camera_scc_tx_msgs, ret);
} else {
SET_TX_MSGS(HYUNDAI_CANFD_LFA_STEERING_LONG_TX_MSGS, ret);
}
}
} else {
if (hyundai_canfd_lka_steering) {
// *** LKA steering checks ***
// E-CAN is on bus 1, SCC messages are sent on cars with ADRV ECU.
// Does not use the alt buttons message
static RxCheck hyundai_canfd_lka_steering_rx_checks[] = {
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(1)
HYUNDAI_CANFD_SCC_ADDR_CHECK(1)
};
SET_RX_CHECKS(hyundai_canfd_lka_steering_rx_checks, ret);
if (hyundai_canfd_lka_steering_alt) {
SET_TX_MSGS(HYUNDAI_CANFD_LKA_STEERING_ALT_TX_MSGS, ret);
} else {
SET_TX_MSGS(HYUNDAI_CANFD_LKA_STEERING_TX_MSGS, ret);
}
} else if (!hyundai_camera_scc) {
// Radar sends SCC messages on these cars instead of camera
static RxCheck hyundai_canfd_radar_scc_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(0)
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(0)
};
ret = hyundai_canfd_alt_buttons ? BUILD_SAFETY_CFG(hyundai_canfd_radar_scc_alt_buttons_rx_checks, HYUNDAI_CANFD_HDA1_TX_MSGS) : \
BUILD_SAFETY_CFG(hyundai_canfd_radar_scc_rx_checks, HYUNDAI_CANFD_HDA1_TX_MSGS);
} else {
// *** Non-HDA2 checks ***
static RxCheck hyundai_canfd_alt_buttons_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
static RxCheck hyundai_canfd_alt_buttons_radar_scc_rx_checks[] = {
HYUNDAI_CANFD_ALT_BUTTONS_RX_CHECKS(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(0)
};
// Camera sends SCC messages on HDA1.
SET_TX_MSGS(HYUNDAI_CANFD_LFA_STEERING_TX_MSGS, ret);
if (hyundai_canfd_alt_buttons) {
SET_RX_CHECKS(hyundai_canfd_alt_buttons_radar_scc_rx_checks, ret);
} else {
SET_RX_CHECKS(hyundai_canfd_radar_scc_rx_checks, ret);
}
} else {
// *** LFA steering checks ***
// Camera sends SCC messages on LFA steering cars.
// Both button messages exist on some platforms, so we ensure we track the correct one using flag
static RxCheck hyundai_canfd_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(0)
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
};
ret = hyundai_canfd_alt_buttons ? BUILD_SAFETY_CFG(hyundai_canfd_alt_buttons_rx_checks, HYUNDAI_CANFD_HDA1_TX_MSGS) : \
BUILD_SAFETY_CFG(hyundai_canfd_rx_checks, HYUNDAI_CANFD_HDA1_TX_MSGS);
static RxCheck hyundai_canfd_alt_buttons_rx_checks[] = {
HYUNDAI_CANFD_ALT_BUTTONS_RX_CHECKS(0)
HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
};
static CanMsg hyundai_canfd_lfa_steering_camera_scc_tx_msgs[] = {
HYUNDAI_CANFD_LFA_STEERING_CAMERA_SCC_TX_MSGS(false)
};
SET_TX_MSGS(hyundai_canfd_lfa_steering_camera_scc_tx_msgs, ret);
if (hyundai_canfd_alt_buttons) {
SET_RX_CHECKS(hyundai_canfd_alt_buttons_rx_checks, ret);
} else {
SET_RX_CHECKS(hyundai_canfd_rx_checks, ret);
}
}
}
@@ -745,7 +385,6 @@ const safety_hooks hyundai_canfd_hooks = {
.init = hyundai_canfd_init,
.rx = hyundai_canfd_rx_hook,
.tx = hyundai_canfd_tx_hook,
.fwd = hyundai_canfd_fwd_hook,
.get_counter = hyundai_canfd_get_counter,
.get_checksum = hyundai_canfd_get_checksum,
.compute_checksum = hyundai_common_canfd_compute_checksum,

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

@@ -2,20 +2,11 @@
#include "iqdbc/safety/declarations.h"
#define GET_ADDR(msg) ((msg)->addr)
#define GET_BUS(msg) ((msg)->bus)
#define GET_BYTE(msg, b) ((msg)->data[(b)])
static int cruise_main_prev = 0;
static void generic_rx_checks(void);
static void stock_ecu_check(bool stock_ecu_detected);
extern uint16_t hyundai_canfd_crc_lut[256];
uint16_t hyundai_canfd_crc_lut[256];
static const uint8_t HYUNDAI_PREV_BUTTON_SAMPLES = 8; // roughly 160 ms
//
extern const uint32_t HYUNDAI_STANDSTILL_THRSLD;
const uint32_t HYUNDAI_STANDSTILL_THRSLD = 12; // 0.375 kph
@@ -26,6 +17,13 @@ enum {
HYUNDAI_BTN_CANCEL = 4,
};
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,
};
// common state
extern bool hyundai_ev_gas_signal;
bool hyundai_ev_gas_signal = false;
@@ -39,8 +37,8 @@ bool hyundai_longitudinal = false;
extern bool hyundai_camera_scc;
bool hyundai_camera_scc = false;
extern bool hyundai_canfd_hda2;
bool hyundai_canfd_hda2 = false;
extern bool hyundai_canfd_lka_steering;
bool hyundai_canfd_lka_steering = false;
extern bool hyundai_alt_limits;
bool hyundai_alt_limits = false;
@@ -51,29 +49,57 @@ bool hyundai_fcev_gas_signal = false;
extern bool hyundai_alt_limits_2;
bool hyundai_alt_limits_2 = false;
// ESCC
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_has_lda_button;
bool hyundai_has_lda_button = false;
extern bool hyundai_non_scc;
bool hyundai_non_scc = false;
static uint8_t hyundai_last_button_interaction; // button messages since the user pressed an enable button
static bool main_button_prev;
static bool acc_main_on_prev;
static bool acc_main_on_tx;
static uint32_t acc_main_on_mismatches;
void hyundai_common_init(uint16_t param) {
const int HYUNDAI_PARAM_EV_GAS = 1;
const int HYUNDAI_PARAM_HYBRID_GAS = 2;
const int HYUNDAI_PARAM_CAMERA_SCC = 8;
const int HYUNDAI_PARAM_CANFD_HDA2 = 16;
const int HYUNDAI_PARAM_ALT_LIMITS = 64; // TODO: shift this down with the rest of the common flags
const int HYUNDAI_PARAM_FCEV_GAS = 256;
const int HYUNDAI_PARAM_ALT_LIMITS_2 = 512;
const uint16_t HYUNDAI_PARAM_EV_GAS = 1;
const uint16_t HYUNDAI_PARAM_HYBRID_GAS = 2;
const uint16_t HYUNDAI_PARAM_CAMERA_SCC = 8;
const uint16_t HYUNDAI_PARAM_CANFD_LKA_STEERING = 16;
const uint16_t HYUNDAI_PARAM_ALT_LIMITS = 64; // TODO: shift this down with the rest of the common flags
const uint16_t HYUNDAI_PARAM_FCEV_GAS = 256;
const uint16_t HYUNDAI_PARAM_ALT_LIMITS_2 = 512;
hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS);
hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS);
hyundai_camera_scc = GET_FLAG(param, HYUNDAI_PARAM_CAMERA_SCC);
hyundai_canfd_hda2 = GET_FLAG(param, HYUNDAI_PARAM_CANFD_HDA2);
hyundai_canfd_lka_steering = GET_FLAG(param, HYUNDAI_PARAM_CANFD_LKA_STEERING);
hyundai_alt_limits = GET_FLAG(param, HYUNDAI_PARAM_ALT_LIMITS);
hyundai_fcev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_FCEV_GAS);
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_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);
hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES;
main_button_prev = false;
acc_main_on_prev = false;
acc_main_on_tx = false;
acc_main_on_mismatches = 0U;
#ifdef ALLOW_DEBUG
const int HYUNDAI_PARAM_LONGITUDINAL = 4;
const uint16_t HYUNDAI_PARAM_LONGITUDINAL = 4;
hyundai_longitudinal = GET_FLAG(param, HYUNDAI_PARAM_LONGITUDINAL);
#else
hyundai_longitudinal = false;
@@ -86,27 +112,19 @@ void hyundai_common_cruise_state_check(const bool cruise_engaged) {
// enter controls on rising edge of ACC and recent user button press, exit controls when ACC off
if (!hyundai_longitudinal) {
hyundai_last_button_interaction = 0U; // carrot
//if (cruise_engaged && !cruise_engaged_prev && (hyundai_last_button_interaction < HYUNDAI_PREV_BUTTON_SAMPLES)) {
if (cruise_engaged) {
if (cruise_engaged && !cruise_engaged_prev && (hyundai_last_button_interaction < HYUNDAI_PREV_BUTTON_SAMPLES)) {
controls_allowed = true;
}
if (!cruise_engaged) {
controls_allowed = false;
}
cruise_engaged_prev = cruise_engaged;
}
}
void hyundai_common_cruise_buttons_check(const int cruise_button, const bool main_button) {
if(main_button && main_button != cruise_main_prev) {
acc_main_on = !acc_main_on;
}
cruise_main_prev = main_button;
if ((cruise_button == HYUNDAI_BTN_RESUME) || (cruise_button == HYUNDAI_BTN_SET) || (cruise_button == HYUNDAI_BTN_CANCEL) ||
(main_button)) {
if ((cruise_button == HYUNDAI_BTN_RESUME) || (cruise_button == HYUNDAI_BTN_SET) || (cruise_button == HYUNDAI_BTN_CANCEL) || main_button) {
hyundai_last_button_interaction = 0U;
} else {
hyundai_last_button_interaction = SAFETY_MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES);
@@ -123,38 +141,66 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai
// exit controls on cancel press
if (cruise_button == HYUNDAI_BTN_CANCEL) {
controls_allowed = false;
}
// toggle main cruise state on rising edge of main cruise button
if (main_button && !main_button_prev && hyundai_longitudinal_main_cruise_toggleable) {
acc_main_on = !acc_main_on;
}
cruise_button_prev = cruise_button;
main_button_prev = main_button;
}
}
uint32_t hyundai_common_canfd_compute_checksum(const CANPacket_t *to_push) {
int len = GET_LEN(to_push);
uint32_t address = GET_ADDR(to_push);
uint32_t hyundai_common_canfd_compute_checksum(const CANPacket_t *msg) {
int len = GET_LEN(msg);
uint32_t address = msg->addr;
uint16_t crc = 0;
for (int i = 2; i < len; i++) {
crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ GET_BYTE(to_push, i)];
crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ msg->data[i]];
}
// Add address to crc
crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ ((address >> 0U) & 0xFFU)];
crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ ((address >> 8U) & 0xFFU)];
if (len == 8) {
crc ^= 0x5f29U;
}
else if (len == 16) {
crc ^= 0x041dU;
}
else if (len == 24) {
if (len == 24) {
crc ^= 0x819dU;
}
else if (len == 32) {
} else if (len == 32) {
crc ^= 0x9f5bU;
} else {
}
return crc;
}
// reset mismatches on rising edge of acc_main_on to avoid rare race conditions when using non-PCM main cruise state
void hyundai_common_reset_acc_main_on_mismatches(void) {
if (acc_main_on && !acc_main_on_prev) {
acc_main_on_mismatches = 0U;
}
acc_main_on_prev = acc_main_on;
}
// exit lateral controls allowed if iqpilot and panda main cruise states are desynced
void hyundai_common_acc_main_on_sync(void) {
if (acc_main_on && !acc_main_on_tx) {
acc_main_on_mismatches += 1U;
if (acc_main_on_mismatches >= 3U) { // desync by 3 frames
acc_main_on = false;
aol_exit_controls(AOL_DISENGAGE_REASON_NON_PCM_ACC_MAIN_DESYNC);
}
} else {
acc_main_on_mismatches = 0U;
}
}
uint32_t get_acc_main_on_mismatches(void) {
return acc_main_on_mismatches;
}

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_legacy_das_steering = false;
static bool tesla_fsd_14 = false;
static bool tesla_stock_aeb = false;
// Only rising edges while controls are not allowed are considered for these systems:
@@ -50,9 +50,6 @@ 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;
@@ -60,8 +57,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) || (addr == 0x3E9)) {
// Signal: EPAS3S_sysStatusChecksum, DAS_controlChecksum, ESP_wheelRotationChecksum, DAS_bodyControlsChecksum
if ((addr == 0x370) || (addr == 0x2b9) || (addr == 0x155)) {
// Signal: EPAS3S_sysStatusChecksum, DAS_controlChecksum, ESP_wheelRotationChecksum
checksum_byte = 7;
} else if (addr == 0x488) {
// Signal: DAS_steeringControlChecksum
@@ -112,9 +109,18 @@ static bool tesla_get_quality_flag_valid(const CANPacket_t *msg) {
return valid;
}
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 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 void tesla_rx_hook(const CANPacket_t *msg) {
@@ -257,17 +263,19 @@ 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 = 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
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
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 == 1) || // ANGLE_CONTROL
(steer_control_type == 2); // LANE_KEEP_ASSIST
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
if (!valid_steer_control_type) {
violation = true;
}
@@ -317,11 +325,6 @@ 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;
}
@@ -348,11 +351,9 @@ static bool tesla_fwd_hook(int bus_num, int addr) {
if (tesla_longitudinal && (addr == 0x2b9) && !tesla_stock_aeb) {
block_msg = true;
}
}
}
return block_msg;
}
@@ -370,23 +371,8 @@ static safety_config tesla_init(uint16_t param) {
{0x27D, 0, 3, .check_relay = true, .disable_static_blocking = true}, // APS_eacMonitor
};
// 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);
const uint16_t TESLA_FLAG_FSD_14 = 2;
tesla_fsd_14 = GET_FLAG(param, TESLA_FLAG_FSD_14);
#ifdef ALLOW_DEBUG
const uint16_t TESLA_FLAG_LONGITUDINAL_CONTROL = 1;
@@ -415,11 +401,7 @@ static safety_config tesla_init(uint16_t param) {
};
safety_config ret;
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) {
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,15 +375,11 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
}
}
// UDS: Only tester present and door lock/unlock allowed on diagnostics address
// UDS: Only tester present ("\x0F\x02\x3E\x00\x00\x00\x00\x00") allowed on diagnostics address
if (msg->addr == 0x750U) {
// this address is sub-addressed. only allow tester present to radar (0xF)
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) {
bool invalid_uds_msg = (GET_BYTES(msg, 0, 4) != 0x003E020FU) || (GET_BYTES(msg, 4, 4) != 0x0U);
if (invalid_uds_msg) {
tx = 0;
}
}

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

@@ -10,14 +10,6 @@ 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
@@ -63,9 +55,6 @@ 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;
@@ -114,7 +103,6 @@ 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;
@@ -226,7 +214,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) | (vw_iq_aol_active ? 0x02U : 0x00U) | (vw_iq_no_cam ? 0x04U : 0x00U);
uint8_t flags = (vw_iq_apd_params_valid ? 0x01U : 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,16 +236,6 @@ 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,
@@ -253,6 +243,14 @@ static bool volkswagen_meb_tx_hook(const CANPacket_t *msg) {
.inactive_accel = 3010,
};
const CurvatureSteeringLimits VOLKSWAGEN_MEB_STEERING_LIMITS = {
.max_curvature = 29105,
.curvature_to_can = 149253.7313f,
.send_rate = 0.02f,
.inactive_curvature_is_zero = true,
.max_power = 225, // 90% (raw byte, 0.4 %/bit; matches Python STEERING_POWER_MAX = 90)
};
bool tx = true;
if (msg->addr == MSG_HCA_03) {

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

@@ -6,7 +6,6 @@
#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
@@ -16,16 +15,10 @@
#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];
@@ -64,39 +57,26 @@ 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_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}};
{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_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}};
{MSG_APD_1, 1, 8, .check_relay = false}};
static RxCheck volkswagen_pq_rx_checks[] = {
{.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_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_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
@@ -106,8 +86,7 @@ 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) : \
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);
BUILD_SAFETY_CFG(volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_STOCK_TX_MSGS);
if (!volkswagen_pq_alc_module_present) {
ret.rx_checks_len -= 1;
}
@@ -115,9 +94,7 @@ static safety_config volkswagen_pq_init(uint16_t param) {
}
static void volkswagen_pq_rx_hook(const CANPacket_t *msg) {
// 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) {
if (msg->bus == 0U) {
// Update in-motion state from speed value.
// Signal: Bremse_1.BR1_Rad_kmh
if (msg->addr == MSG_BREMSE_1) {
@@ -142,14 +119,9 @@ 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;
}
@@ -189,7 +161,8 @@ 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;
@@ -207,7 +180,6 @@ 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)) {
@@ -242,31 +214,13 @@ 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,

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

@@ -1,46 +1,296 @@
import unittest
from iqdbc.iqpilot.car.hyundai.values import HyundaiSafetyFlagsIQ
import iqdbc.safety.tests.common as common
from iqdbc.safety.tests.libsafety import libsafety_py
from iqdbc.safety.tests.common import make_msg
def packet(addr: int, bus: int, length: int, updates: dict[int, int] | None = None):
data = bytearray(length)
for index, value in (updates or {}).items():
data[index] = value
return libsafety_py.make_CANPacket(addr, bus, data)
class Buttons:
NONE = 0
RESUME = 1
SET = 2
CANCEL = 4
def classic_steer(torque: int, request: bool = True):
value = torque + 1024
word = (value << 16) | (int(request) << 27)
return packet(0x340, 0, 8, {i: (word >> (8 * i)) & 0xFF for i in range(4)})
PREV_BUTTON_SAMPLES = 8
ENABLE_BUTTONS = (Buttons.RESUME, Buttons.SET, Buttons.CANCEL)
def canfd_steer(addr: int, length: int, torque: int, request: bool = True):
value = torque + 1024
return packet(addr, 0, length, {
5: (value & 0x7F) << 1,
6: ((value >> 7) & 0xF) | (int(request) << 4),
})
class HyundaiButtonBase:
BUTTONS_TX_BUS = 0 # tx on this bus, rx on 0
SCC_BUS = 0 # rx on this bus
def test_button_sends(self):
"""
RES, SET and CANCEL buttons are allowed
- RES and SET allowed while controls allowed
- CANCEL allowed while cruise is enabled
"""
self.safety.set_controls_allowed(0)
self.assertFalse(self._tx(self._button_msg(Buttons.RESUME, bus=self.BUTTONS_TX_BUS)))
self.assertFalse(self._tx(self._button_msg(Buttons.SET, bus=self.BUTTONS_TX_BUS)))
self.safety.set_controls_allowed(1)
self.assertTrue(self._tx(self._button_msg(Buttons.RESUME, bus=self.BUTTONS_TX_BUS)))
self.assertTrue(self._tx(self._button_msg(Buttons.SET, bus=self.BUTTONS_TX_BUS)))
for enabled in (True, False):
self._rx(self._pcm_status_msg(enabled))
self.assertEqual(enabled, self._tx(self._button_msg(Buttons.CANCEL, bus=self.BUTTONS_TX_BUS)))
def test_enable_control_allowed_from_cruise(self):
"""
Hyundai non-longitudinal only enables on PCM rising edge and recent button press. Tests PCM enabling with:
- disallowed: No buttons
- disallowed: Buttons that don't enable cruise
- allowed: Buttons that do enable cruise
- allowed: Main button with all above combinations
"""
for main_button in (0, 1):
for btn in range(8):
for _ in range(PREV_BUTTON_SAMPLES): # reset
self._rx(self._button_msg(Buttons.NONE))
self._rx(self._pcm_status_msg(False))
self.assertFalse(self.safety.get_controls_allowed())
self._rx(self._button_msg(btn, main_button=main_button))
self._rx(self._pcm_status_msg(True))
controls_allowed = btn in ENABLE_BUTTONS or main_button
self.assertEqual(controls_allowed, self.safety.get_controls_allowed())
def test_sampling_cruise_buttons(self):
"""
Test that we allow controls on recent button press, but not as button leaves sliding window
"""
self._rx(self._button_msg(Buttons.SET))
for i in range(2 * PREV_BUTTON_SAMPLES):
self._rx(self._pcm_status_msg(False))
self.assertFalse(self.safety.get_controls_allowed())
self._rx(self._pcm_status_msg(True))
controls_allowed = i < PREV_BUTTON_SAMPLES
self.assertEqual(controls_allowed, self.safety.get_controls_allowed())
self._rx(self._button_msg(Buttons.NONE))
def classic_accel(accel: int, *, aeb_decel: int = 0, aeb_request: bool = False):
value = accel + 1023
return packet(0x421, 0, 8, {
2: aeb_decel,
3: value & 0xFF,
4: ((value >> 8) & 0x7) | ((value & 0x7) << 5),
5: (value >> 3) & 0xFF,
6: int(aeb_request) << 6,
})
class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest):
DISABLED_ECU_UDS_MSG: tuple[int, int]
DISABLED_ECU_ACTUATION_MSG: tuple[int, int]
def canfd_accel(accel: int, *, acc_mode: int = 0, bus: int = 0):
value = accel + 1023
return packet(0x1A0, bus, 32, {
8: (acc_mode & 0x7) << 4,
16: value & 0xFF,
17: ((value >> 8) & 0x7) | ((value & 0xF) << 4),
18: (value >> 4) & 0xFF,
})
@classmethod
def setUpClass(cls):
if cls.__name__ == "HyundaiLongitudinalBase":
cls.safety = None
raise unittest.SkipTest
# override these tests from CarSafetyTest, hyundai longitudinal uses button enable
def test_disable_control_allowed_from_cruise(self):
pass
TESTER_PRESENT = bytes.fromhex("023e800000000000")
def test_enable_control_allowed_from_cruise(self):
pass
def test_sampling_cruise_buttons(self):
pass
def test_cruise_engaged_prev(self):
pass
def test_button_sends(self):
pass
def _pcm_status_msg(self, enable):
raise Exception
def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
raise NotImplementedError
def _acc_state_msg(self, enable):
raise NotImplementedError
def _tx_acc_state_msg(self, enable):
raise NotImplementedError
def test_pcm_main_cruise_state_availability(self):
pass
def test_set_resume_buttons(self):
"""
SET and RESUME enter controls allowed on their falling edge.
"""
for btn_prev in range(8):
for btn_cur in range(8):
self._rx(self._button_msg(Buttons.NONE))
self.safety.set_controls_allowed(0)
for _ in range(10):
self._rx(self._button_msg(btn_prev))
self.assertFalse(self.safety.get_controls_allowed())
# should enter controls allowed on falling edge and not transitioning to cancel
should_enable = btn_cur != btn_prev and \
btn_cur != Buttons.CANCEL and \
btn_prev in (Buttons.RESUME, Buttons.SET)
self._rx(self._button_msg(btn_cur))
self.assertEqual(should_enable, self.safety.get_controls_allowed())
def test_cancel_button(self):
self.safety.set_controls_allowed(1)
self._rx(self._button_msg(Buttons.CANCEL))
self.assertFalse(self.safety.get_controls_allowed())
def test_main_cruise_button(self):
"""Test that main cruise button correctly toggles acc_main_on state"""
default_safety_mode = self.safety.get_current_safety_mode()
default_safety_param = self.safety.get_current_safety_param()
default_safety_param_iq = self.safety.get_current_safety_param_iq()
for enable_aol in (True, False):
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
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)
# Test initial state
self.safety.set_aol_params(enable_aol, False, False)
self.assertFalse(self.safety.get_acc_main_on())
self._rx(self._main_cruise_button_msg(0))
self._rx(self._main_cruise_button_msg(1))
self.assertEqual(enable_aol and main_cruise_toggleable, self.safety.get_controls_allowed_lat())
self._rx(self._main_cruise_button_msg(0))
self.assertEqual(enable_aol and main_cruise_toggleable, self.safety.get_controls_allowed_lat())
self._rx(self._main_cruise_button_msg(1))
self.assertFalse(self.safety.get_controls_allowed_lat())
for _ in range(10):
self._rx(self._main_cruise_button_msg(1))
self.assertFalse(self.safety.get_controls_allowed_lat())
self.safety.set_current_safety_param_iq(default_safety_param_iq)
def test_acc_main_sync_mismatches_reset(self):
"""Test that acc_main_on_mismatches resets properly on rising edge of main button"""
default_safety_mode = self.safety.get_current_safety_mode()
default_safety_param = self.safety.get_current_safety_param()
default_safety_param_iq = self.safety.get_current_safety_param_iq()
for enable_aol in (True, False):
with self.subTest("enable_aol", aol_enabled=enable_aol):
main_cruise_toggleable_flag = HyundaiSafetyFlagsIQ.LONG_MAIN_CRUISE_TOGGLEABLE
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)
self.safety.set_aol_params(enable_aol, False, False)
# Initial state
self._rx(self._main_cruise_button_msg(0))
self.assertFalse(self.safety.get_acc_main_on())
# Set up mismatch condition
self._rx(self._main_cruise_button_msg(1)) # Press button - acc_main_on = True
self._rx(self._main_cruise_button_msg(0)) # Release button
self._tx(self._tx_acc_state_msg(False)) # acc_main_on_tx = False
self.assertTrue(self.safety.get_acc_main_on())
self.assertEqual(1, self.safety.get_acc_main_on_mismatches())
# Rising edge of acc_main_on should reset
self._rx(self._main_cruise_button_msg(1)) # Press button again
self._rx(self._main_cruise_button_msg(0)) # Release button
self._tx(self._tx_acc_state_msg(False)) # acc_main_on_tx = False
self.assertFalse(self.safety.get_acc_main_on())
self.assertEqual(0, self.safety.get_acc_main_on_mismatches())
self.safety.set_current_safety_param_iq(default_safety_param_iq)
def test_acc_main_sync_mismatch_counter(self):
"""Test mismatch counter behavior and disengagement"""
default_safety_mode = self.safety.get_current_safety_mode()
default_safety_param = self.safety.get_current_safety_param()
default_safety_param_iq = self.safety.get_current_safety_param_iq()
for enable_aol in (True, False):
with self.subTest("enable_aol", aol_enabled=enable_aol):
main_cruise_toggleable_flag = HyundaiSafetyFlagsIQ.LONG_MAIN_CRUISE_TOGGLEABLE
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)
self.safety.set_aol_params(enable_aol, False, False)
self.safety.set_controls_allowed_lat(True)
# Start with matched states
self._rx(self._main_cruise_button_msg(0))
self._tx(self._tx_acc_state_msg(False))
self.assertEqual(0, self.safety.get_acc_main_on_mismatches())
# Create mismatch by enabling acc_main_on
self._rx(self._main_cruise_button_msg(1)) # Press button
self._rx(self._main_cruise_button_msg(0)) # Release button
self._tx(self._tx_acc_state_msg(False)) # acc_main_on_tx stays false
self.assertTrue(self.safety.get_acc_main_on())
self.assertEqual(1, self.safety.get_acc_main_on_mismatches())
# Second mismatch
self._tx(self._tx_acc_state_msg(False))
self.assertTrue(self.safety.get_acc_main_on())
self.assertEqual(2, self.safety.get_acc_main_on_mismatches())
# Third mismatch should trigger disengagement
self._tx(self._tx_acc_state_msg(False))
self.assertFalse(self.safety.get_acc_main_on())
self.assertFalse(self.safety.get_controls_allowed_lat())
# Counter should reset after disengagement
self._tx(self._tx_acc_state_msg(False))
self.assertEqual(0, self.safety.get_acc_main_on_mismatches())
self.safety.set_current_safety_param_iq(default_safety_param_iq)
def test_acc_main_sync_mismatch_recovery(self):
default_safety_mode = self.safety.get_current_safety_mode()
default_safety_param = self.safety.get_current_safety_param()
default_safety_param_iq = self.safety.get_current_safety_param_iq()
"""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
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)
self.safety.set_aol_params(enable_aol, False, False)
# Create initial mismatch
self._rx(self._main_cruise_button_msg(1)) # Press button
self._rx(self._main_cruise_button_msg(0)) # Release button
self._tx(self._tx_acc_state_msg(False)) # acc_main_on_tx = False
self.assertEqual(1, self.safety.get_acc_main_on_mismatches())
# Sync states
self._tx(self._tx_acc_state_msg(True)) # Match acc_main_on_tx to acc_main_on
self.assertEqual(0, self.safety.get_acc_main_on_mismatches())
self.safety.set_current_safety_param_iq(default_safety_param_iq)
def test_tester_present_allowed(self, ecu_disable: bool = True):
"""
Ensure tester present diagnostic message is allowed to keep ECU knocked out
for longitudinal control.
"""
addr, bus = self.DISABLED_ECU_UDS_MSG
for should_tx, msg in ((True, b"\x02\x3E\x80\x00\x00\x00\x00\x00"),
(False, b"\x03\xAA\xAA\x00\x00\x00\x00\x00")):
tester_present = libsafety_py.make_CANPacket(addr, bus, msg)
self.assertEqual(should_tx and ecu_disable, self._tx(tester_present))
def test_disabled_ecu_alive(self):
"""
If the ECU knockout failed, make sure the relay malfunction is shown
"""
addr, bus = self.DISABLED_ECU_ACTUATION_MSG
self.assertFalse(self.safety.get_relay_malfunction())
self._rx(make_msg(bus, addr, 8))
self.assertTrue(self.safety.get_relay_malfunction())

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

@@ -25,11 +25,6 @@ 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 = 5.0
SAFETY_ISO_LATERAL_ACCEL = 3.0
EARTH_G = 9.81
AVERAGE_ROAD_ROLL = 0.06
MAX_LATERAL_ACCEL = SAFETY_ISO_LATERAL_ACCEL - (EARTH_G * AVERAGE_ROAD_ROLL)

5
iqdbc_repo/iqdbc/safety/tests/test_gm.py
View File

@@ -7,8 +7,7 @@ from iqdbc.safety.tests.libsafety import libsafety_py
import iqdbc.safety.tests.common as common
from iqdbc.safety.tests.common import CANPackerSafety
# GM_PARAM_IQ_NON_ACC in safety/modes/gm.h (IQ safety framework flag)
GM_PARAM_IQ_NON_ACC = 1
from iqdbc.iqpilot.car.gm.values_ext import GMSafetyFlagsIQ
class Buttons:
@@ -237,7 +236,7 @@ class TestGmCameraNonACCSafety(TestGmCameraSafety):
self.packer = CANPackerSafety("gm_global_a_powertrain_generated")
self.packer_chassis = CANPackerSafety("gm_global_a_chassis")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_iq(GM_PARAM_IQ_NON_ACC)
self.safety.set_current_safety_param_iq(GMSafetyFlagsIQ.NON_ACC)
self.safety.set_safety_hooks(CarParams.SafetyModel.gm, GMSafetyFlags.HW_CAM | self.EXTRA_SAFETY_PARAM)
self.safety.init_tests()

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

@@ -9,7 +9,7 @@ from iqdbc.car.structs import CarParams
from iqdbc.safety.tests.common import CANPackerSafety, MAX_WRONG_COUNTERS
from iqdbc.safety.tests.gas_interceptor_common import GasInterceptorSafetyTest
from iqdbc.iqpilot.car.honda.iq_values import HondaSafetyFlagsIQ
from iqdbc.iqpilot.car.honda.values_ext import HondaSafetyFlagsIQ
HONDA_N_COMMON_TX_MSGS = [[0xE4, 0], [0x194, 0], [0x1FA, 0], [0x30C, 0], [0x33D, 0]]

626
iqdbc_repo/iqdbc/safety/tests/test_hyundai.py Normal file → Executable file
View File

@@ -1,107 +1,569 @@
import pytest
#!/usr/bin/env python3
from parameterized import parameterized_class
import random
import unittest
from iqdbc.car.hyundai.values import HyundaiSafetyFlags
from iqdbc.car.structs import CarParams
from iqdbc.safety.tests.hyundai_common import TESTER_PRESENT, classic_accel, classic_steer, packet
from iqdbc.safety.tests.libsafety import libsafety_py
import iqdbc.safety.tests.common as common
from iqdbc.safety.tests.common import CANPackerSafety
from iqdbc.safety.tests.hyundai_common import HyundaiButtonBase, HyundaiLongitudinalBase
from iqdbc.iqpilot.car.hyundai.values import HyundaiSafetyFlagsIQ
# LDA button availability
LDA_BUTTON = [
{"SAFETY_PARAM_IQ": HyundaiSafetyFlagsIQ.DEFAULT},
{"SAFETY_PARAM_IQ": HyundaiSafetyFlagsIQ.HAS_LDA_BUTTON},
]
# All combinations of non-SCC HEV/PHEV/EV cars
_ALL_NON_SCC_HEV_EV_COMBOS = [
# Hybrid
{"PCM_STATUS_MSG": ("E_CRUISE_CONTROL", "CRUISE_LAMP_S"),
"ACC_STATE_MSG": ("E_CRUISE_CONTROL", "CRUISE_LAMP_M"),
"GAS_MSG": ("E_EMS11", "CR_Vcu_AccPedDep_Pos"),
"SAFETY_PARAM": HyundaiSafetyFlags.HYBRID_GAS},
# EV
{"PCM_STATUS_MSG": ("LABEL11", "CC_ACT"),
"ACC_STATE_MSG": ("LABEL11", "CC_React"),
"GAS_MSG": ("E_EMS11", "Accel_Pedal_Pos"),
"SAFETY_PARAM": HyundaiSafetyFlags.EV_GAS},
]
ALL_NON_SCC_HEV_EV_COMBOS = [{**p, **lda} for lda in LDA_BUTTON for p in _ALL_NON_SCC_HEV_EV_COMBOS]
@pytest.fixture
def safety():
return libsafety_py.libsafety
# 4 bit checkusm used in some hyundai messages
# lives outside the can packer because we never send this msg
def checksum(msg):
addr, dat, bus = msg
chksum = 0
if addr == 0x386:
for i, b in enumerate(dat):
for j in range(8):
# exclude checksum and counter bits
if (i != 1 or j < 6) and (i != 3 or j < 6) and (i != 5 or j < 6) and (i != 7 or j < 6):
bit = (b >> j) & 1
else:
bit = 0
chksum += bit
chksum = (chksum ^ 9) & 0xF
ret = bytearray(dat)
ret[5] |= (chksum & 0x3) << 6
ret[7] |= (chksum & 0xc) << 4
else:
for i, b in enumerate(dat):
if addr in [0x260, 0x421] and i == 7:
b &= 0x0F if addr == 0x421 else 0xF0
elif addr == 0x394 and i == 6:
b &= 0xF0
elif addr == 0x394 and i == 7:
continue
chksum += sum(divmod(b, 16))
chksum = (16 - chksum) % 16
ret = bytearray(dat)
ret[6 if addr == 0x394 else 7] |= chksum << (4 if addr == 0x421 else 0)
return addr, ret, bus
@pytest.mark.parametrize("mode", (CarParams.SafetyModel.hyundai, CarParams.SafetyModel.hyundaiLegacy))
@pytest.mark.parametrize("param", (
0,
HyundaiSafetyFlags.EV_GAS,
HyundaiSafetyFlags.HYBRID_GAS,
HyundaiSafetyFlags.LONG,
HyundaiSafetyFlags.CAMERA_SCC,
HyundaiSafetyFlags.ALT_LIMITS,
HyundaiSafetyFlags.FCEV_GAS,
HyundaiSafetyFlags.ALT_LIMITS_2,
))
def test_classic_safety_configurations_initialize(safety, mode, param):
assert safety.set_safety_hooks(mode, param) == 0
safety.init_tests()
assert safety.get_current_safety_param() == param
@parameterized_class(LDA_BUTTON)
class TestHyundaiSafety(HyundaiButtonBase, common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest):
TX_MSGS = [[0x340, 0], [0x4F1, 0], [0x485, 0]]
STANDSTILL_THRESHOLD = 12 # 0.375 kph
RELAY_MALFUNCTION_ADDRS = {0: (0x340, 0x485)} # LKAS11
FWD_BLACKLISTED_ADDRS = {2: [0x340, 0x485]}
MAX_RATE_UP = 3
MAX_RATE_DOWN = 7
MAX_TORQUE_LOOKUP = [0], [384]
MAX_RT_DELTA = 112
DRIVER_TORQUE_ALLOWANCE = 50
DRIVER_TORQUE_FACTOR = 2
# Safety around steering req bit
MIN_VALID_STEERING_FRAMES = 89
MAX_INVALID_STEERING_FRAMES = 2
cnt_gas = 0
cnt_speed = 0
cnt_brake = 0
cnt_cruise = 0
cnt_button = 0
SAFETY_PARAM_IQ: int = 0
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiSafety":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_iq(self.SAFETY_PARAM_IQ)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0)
self.safety.init_tests()
def _button_msg(self, buttons, main_button=0, bus=0):
values = {"CF_Clu_CruiseSwState": buttons, "CF_Clu_CruiseSwMain": main_button, "CF_Clu_AliveCnt1": self.cnt_button}
self.__class__.cnt_button += 1
return self.packer.make_can_msg_safety("CLU11", bus, values)
def _user_gas_msg(self, gas):
values = {"CF_Ems_AclAct": gas, "AliveCounter": self.cnt_gas % 4}
self.__class__.cnt_gas += 1
return self.packer.make_can_msg_safety("EMS16", 0, values, fix_checksum=checksum)
def _user_brake_msg(self, brake):
values = {"DriverOverride": 2 if brake else random.choice((0, 1, 3)),
"AliveCounterTCS": self.cnt_brake % 8}
self.__class__.cnt_brake += 1
return self.packer.make_can_msg_safety("TCS13", 0, values, fix_checksum=checksum)
def _speed_msg(self, speed):
# safety doesn't scale, so undo the scaling
values = {"WHL_SPD_%s" % s: speed * 0.03125 for s in ["FL", "FR", "RL", "RR"]}
values["WHL_SPD_AliveCounter_LSB"] = (self.cnt_speed % 16) & 0x3
values["WHL_SPD_AliveCounter_MSB"] = (self.cnt_speed % 16) >> 2
self.__class__.cnt_speed += 1
return self.packer.make_can_msg_safety("WHL_SPD11", 0, values, fix_checksum=checksum)
def _pcm_status_msg(self, enable):
values = {"ACCMode": enable, "CR_VSM_Alive": self.cnt_cruise % 16}
self.__class__.cnt_cruise += 1
return self.packer.make_can_msg_safety("SCC12", self.SCC_BUS, values, fix_checksum=checksum)
def _torque_driver_msg(self, torque):
values = {"CR_Mdps_StrColTq": torque}
return self.packer.make_can_msg_safety("MDPS12", 0, values)
def _torque_cmd_msg(self, torque, steer_req=1):
values = {"CR_Lkas_StrToqReq": torque, "CF_Lkas_ActToi": steer_req}
return self.packer.make_can_msg_safety("LKAS11", 0, values)
def _acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
return self.packer.make_can_msg_safety("SCC11", self.SCC_BUS, values)
def _lkas_button_msg(self, enabled):
if self.SAFETY_PARAM_IQ & HyundaiSafetyFlagsIQ.HAS_LDA_BUTTON:
values = {"LDA_BTN": enabled}
return self.packer.make_can_msg_safety("BCM_PO_11", 0, values)
else:
raise NotImplementedError
def _main_cruise_button_msg(self, enabled):
return self._button_msg(0, enabled)
def test_pcm_main_cruise_state_availability(self):
"""Test that ACC main state is correctly set when receiving 0x420 message, toggling HYUNDAI_LONG flag"""
prior_safety_mode = self.safety.get_current_safety_mode()
prior_safety_param = self.safety.get_current_safety_param()
for hyundai_longitudinal in (True, False):
with self.subTest("hyundai_longitudinal", hyundai_longitudinal=hyundai_longitudinal):
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0 if hyundai_longitudinal else HyundaiSafetyFlags.LONG)
for should_turn_acc_main_on in (True, False):
with self.subTest("acc_main_on", should_turn_acc_main_on=should_turn_acc_main_on):
self._rx(self._acc_state_msg(should_turn_acc_main_on)) # Send the ACC state message
expected_acc_main = should_turn_acc_main_on and hyundai_longitudinal # ACC main should only be set if hyundai_longitudinal is True
self.assertEqual(expected_acc_main, self.safety.get_acc_main_on())
self.safety.set_safety_hooks(prior_safety_mode, prior_safety_param)
def test_enable_control_allowed_with_aol_button(self):
default_safety_mode = self.safety.get_current_safety_mode()
default_safety_param = self.safety.get_current_safety_param()
default_safety_param_iq = self.safety.get_current_safety_param_iq()
"""Toggle AOL with AOL button"""
try:
self._lkas_button_msg(False)
except NotImplementedError as err:
raise unittest.SkipTest("Skipping test because LDA button is not supported") from err
try:
for enable_aol in (True, False):
with self.subTest("enable_aol", aol_enabled=enable_aol):
for has_lda_button_param in (True, False):
with self.subTest("has_lda_button", has_lda_button_param=has_lda_button_param):
has_lda_button = HyundaiSafetyFlagsIQ.HAS_LDA_BUTTON if has_lda_button_param else 0
self.safety.set_current_safety_param_iq(has_lda_button)
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)
self.safety.set_aol_params(enable_aol, False, False)
self.assertEqual(enable_aol, self.safety.get_enable_aol())
self._rx(self._lkas_button_msg(True))
self._rx(self._speed_msg(0))
self._rx(self._lkas_button_msg(False))
self._rx(self._speed_msg(0))
self.assertEqual(enable_aol and has_lda_button_param, self.safety.get_controls_allowed_lat())
finally:
self.safety.set_current_safety_param_iq(default_safety_param_iq)
@pytest.mark.parametrize("mode", (CarParams.SafetyModel.hyundai, CarParams.SafetyModel.hyundaiLegacy))
def test_classic_tx_whitelist_and_steering_limits(safety, mode):
safety.set_safety_hooks(mode, 0)
safety.init_tests()
@parameterized_class(LDA_BUTTON)
class TestHyundaiSafetyAltLimits(TestHyundaiSafety):
MAX_RATE_UP = 2
MAX_RATE_DOWN = 3
MAX_TORQUE_LOOKUP = [0], [270]
assert not safety.safety_tx_hook(packet(0x123, 0, 8))
assert not safety.safety_tx_hook(packet(0x340, 1, 8))
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiSafetyAltLimits":
cls.safety = None
raise unittest.SkipTest
safety.set_controls_allowed(False)
assert safety.safety_tx_hook(classic_steer(0, False))
assert not safety.safety_tx_hook(classic_steer(1))
safety.set_controls_allowed(True)
assert safety.safety_tx_hook(classic_steer(10))
safety.set_desired_torque_last(512)
safety.set_rt_torque_last(512)
assert safety.safety_tx_hook(classic_steer(512))
assert not safety.safety_tx_hook(classic_steer(513))
assert not safety.safety_tx_hook(classic_steer(-513))
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_iq(self.SAFETY_PARAM_IQ)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.ALT_LIMITS)
self.safety.init_tests()
def test_classic_alt_limits_2(safety):
safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.ALT_LIMITS_2)
safety.init_tests()
safety.set_controls_allowed(True)
safety.set_desired_torque_last(170)
safety.set_rt_torque_last(170)
assert safety.safety_tx_hook(classic_steer(170))
assert not safety.safety_tx_hook(classic_steer(171))
@parameterized_class(LDA_BUTTON)
class TestHyundaiSafetyAltLimits2(TestHyundaiSafety):
MAX_RATE_UP = 2
MAX_RATE_DOWN = 3
MAX_TORQUE_LOOKUP = [0], [170]
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiSafetyAltLimits2":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_iq(self.SAFETY_PARAM_IQ)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.ALT_LIMITS_2)
self.safety.init_tests()
def test_classic_longitudinal_accel_and_aeb_guards(safety):
safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.LONG)
safety.init_tests()
@parameterized_class(LDA_BUTTON)
class TestHyundaiSafetyCameraSCC(TestHyundaiSafety):
BUTTONS_TX_BUS = 2 # tx on 2, rx on 0
SCC_BUS = 2 # rx on 2
safety.set_controls_allowed(False)
assert safety.safety_tx_hook(classic_accel(0))
assert not safety.safety_tx_hook(classic_accel(1))
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiSafetyCameraSCC":
cls.safety = None
raise unittest.SkipTest
safety.set_controls_allowed(True)
for accel in (-400, 0, 250):
assert safety.safety_tx_hook(classic_accel(accel))
for accel in (-401, 251):
assert not safety.safety_tx_hook(classic_accel(accel))
assert not safety.safety_tx_hook(classic_accel(0, aeb_decel=1))
assert not safety.safety_tx_hook(classic_accel(0, aeb_request=True))
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_iq(self.SAFETY_PARAM_IQ)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.CAMERA_SCC)
self.safety.init_tests()
assert safety.safety_tx_hook(packet(0x38D, 0, 8))
assert not safety.safety_tx_hook(packet(0x38D, 0, 8, {1: 1}))
assert not safety.safety_tx_hook(packet(0x38D, 0, 8, {2: 1 << 4}))
assert not safety.safety_tx_hook(packet(0x38D, 0, 8, {3: 1 << 7}))
def test_pcm_main_cruise_state_availability(self):
"""
Test that ACC main state is correctly set when receiving 0x420 message.
For camera SCC, ACC main should always be on when receiving 0x420 message
"""
for should_turn_acc_main_on in (True, False):
with self.subTest("acc_main_on", should_turn_acc_main_on=should_turn_acc_main_on):
self._rx(self._acc_state_msg(should_turn_acc_main_on))
self.assertEqual(should_turn_acc_main_on, self.safety.get_acc_main_on())
def test_classic_buttons(safety):
safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0)
safety.init_tests()
@parameterized_class(LDA_BUTTON)
class TestHyundaiSafetyFCEV(TestHyundaiSafety):
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiSafetyFCEV":
cls.safety = None
raise unittest.SkipTest
safety.set_controls_allowed(False)
assert not safety.safety_tx_hook(packet(0x4F1, 0, 4, {0: 1}))
assert not safety.safety_tx_hook(packet(0x4F1, 0, 4, {0: 2}))
assert not safety.safety_tx_hook(packet(0x4F1, 0, 4, {0: 4}))
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_iq(self.SAFETY_PARAM_IQ)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.FCEV_GAS)
self.safety.init_tests()
safety.set_controls_allowed(True)
assert safety.safety_tx_hook(packet(0x4F1, 0, 4, {0: 1}))
assert not safety.safety_tx_hook(packet(0x4F1, 0, 4, {0: 2}))
assert safety.safety_tx_hook(packet(0x4F1, 0, 4, {0: 4}))
safety.set_controls_allowed(False)
safety.set_cruise_engaged_prev(True)
assert safety.safety_tx_hook(packet(0x4F1, 0, 4, {0: 4}))
def _user_gas_msg(self, gas):
values = {"ACCELERATOR_PEDAL": gas}
return self.packer.make_can_msg_safety("FCEV_ACCELERATOR", 0, values)
def test_classic_diagnostic_payload_is_restricted(safety):
safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.LONG)
safety.init_tests()
assert safety.safety_tx_hook(packet(0x7D0, 0, 8, dict(enumerate(TESTER_PRESENT))))
assert not safety.safety_tx_hook(packet(0x7D0, 0, 8, {0: 3, 1: 0x22}))
class TestHyundaiLegacySafety(TestHyundaiSafety):
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiLegacy, 0)
self.safety.init_tests()
class TestHyundaiLegacySafetyEV(TestHyundaiSafety):
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiLegacy, HyundaiSafetyFlags.EV_GAS)
self.safety.init_tests()
def _user_gas_msg(self, gas):
values = {"Accel_Pedal_Pos": gas}
return self.packer.make_can_msg_safety("E_EMS11", 0, values, fix_checksum=checksum)
class TestHyundaiLegacySafetyHEV(TestHyundaiSafety):
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiLegacy, HyundaiSafetyFlags.HYBRID_GAS)
self.safety.init_tests()
def _user_gas_msg(self, gas):
values = {"CR_Vcu_AccPedDep_Pos": gas}
return self.packer.make_can_msg_safety("E_EMS11", 0, values, fix_checksum=checksum)
@parameterized_class(LDA_BUTTON)
class TestHyundaiLongitudinalSafety(HyundaiLongitudinalBase, TestHyundaiSafety):
TX_MSGS = [[0x340, 0], [0x4F1, 0], [0x485, 0], [0x420, 0], [0x421, 0], [0x50A, 0], [0x389, 0], [0x4A2, 0], [0x38D, 0], [0x483, 0], [0x7D0, 0]]
FWD_BLACKLISTED_ADDRS = {2: [0x340, 0x485, 0x421, 0x420, 0x50A, 0x389]}
RELAY_MALFUNCTION_ADDRS = {0: (0x340, 0x485, 0x421, 0x420, 0x50A, 0x389)} # LKAS11, LFAHDA_MFC, SCC12, SCC11, SCC13, SCC14
DISABLED_ECU_UDS_MSG = (0x7D0, 0)
DISABLED_ECU_ACTUATION_MSG = (0x421, 0)
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiLongitudinalSafety":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_iq(self.SAFETY_PARAM_IQ)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.LONG)
self.safety.init_tests()
def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
values = {
"aReqRaw": accel,
"aReqValue": accel,
"AEB_CmdAct": int(aeb_req),
"CR_VSM_DecCmd": aeb_decel,
}
return self.packer.make_can_msg_safety("SCC12", self.SCC_BUS, values)
def _fca11_msg(self, idx=0, vsm_aeb_req=False, fca_aeb_req=False, aeb_decel=0):
values = {
"CR_FCA_Alive": idx % 0xF,
"FCA_Status": 2,
"CR_VSM_DecCmd": aeb_decel,
"CF_VSM_DecCmdAct": int(vsm_aeb_req),
"FCA_CmdAct": int(fca_aeb_req),
}
return self.packer.make_can_msg_safety("FCA11", 0, values)
def _tx_acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
return self.packer.make_can_msg_safety("SCC11", 0, values)
def test_no_aeb_fca11(self):
self.assertTrue(self._tx(self._fca11_msg()))
self.assertFalse(self._tx(self._fca11_msg(vsm_aeb_req=True)))
self.assertFalse(self._tx(self._fca11_msg(fca_aeb_req=True)))
self.assertFalse(self._tx(self._fca11_msg(aeb_decel=1.0)))
def test_no_aeb_scc12(self):
self.assertTrue(self._tx(self._accel_msg(0)))
self.assertFalse(self._tx(self._accel_msg(0, aeb_req=True)))
self.assertFalse(self._tx(self._accel_msg(0, aeb_decel=1.0)))
class TestHyundaiLongitudinalSafetyCameraSCC(HyundaiLongitudinalBase, TestHyundaiSafety):
TX_MSGS = [[0x340, 0], [0x4F1, 2], [0x485, 0], [0x420, 0], [0x421, 0], [0x50A, 0], [0x389, 0], [0x4A2, 0]]
FWD_BLACKLISTED_ADDRS = {2: [0x340, 0x485, 0x420, 0x421, 0x50A, 0x389]}
RELAY_MALFUNCTION_ADDRS = {0: (0x340, 0x485, 0x421, 0x420, 0x50A, 0x389)} # LKAS11, LFAHDA_MFC, SCC12, SCC11, SCC13, SCC14
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_iq(HyundaiSafetyFlagsIQ.HAS_LDA_BUTTON)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.LONG | HyundaiSafetyFlags.CAMERA_SCC)
self.safety.init_tests()
def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
values = {
"aReqRaw": accel,
"aReqValue": accel,
"AEB_CmdAct": int(aeb_req),
"CR_VSM_DecCmd": aeb_decel,
}
return self.packer.make_can_msg_safety("SCC12", self.SCC_BUS, values)
def _tx_acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
return self.packer.make_can_msg_safety("SCC11", self.SCC_BUS, values)
def test_no_aeb_scc12(self):
self.assertTrue(self._tx(self._accel_msg(0)))
self.assertFalse(self._tx(self._accel_msg(0, aeb_req=True)))
self.assertFalse(self._tx(self._accel_msg(0, aeb_decel=1.0)))
def test_tester_present_allowed(self):
pass
def test_disabled_ecu_alive(self):
pass
@parameterized_class(LDA_BUTTON)
class TestHyundaiSafetyFCEVLong(TestHyundaiLongitudinalSafety, TestHyundaiSafetyFCEV):
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiSafetyFCEVLong":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_iq(self.SAFETY_PARAM_IQ)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.FCEV_GAS | HyundaiSafetyFlags.LONG)
self.safety.init_tests()
@parameterized_class(LDA_BUTTON)
class TestHyundaiLongitudinalESCCSafety(HyundaiLongitudinalBase, TestHyundaiSafety):
TX_MSGS = [[0x340, 0], [0x4F1, 0], [0x485, 0], [0x420, 0], [0x421, 0], [0x50A, 0], [0x389, 0]]
FWD_BLACKLISTED_ADDRS = {2: [0x340, 0x485, 0x420, 0x421, 0x50A, 0x389]}
RELAY_MALFUNCTION_ADDRS = {0: (0x340, 0x485, 0x420, 0x421, 0x50A, 0x389)} # LKAS11, LFAHDA_MFC, SCC12, SCC11, SCC13, SCC14
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiLongitudinalESCCSafety":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_iq(HyundaiSafetyFlagsIQ.ESCC | self.SAFETY_PARAM_IQ)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.LONG)
self.safety.init_tests()
def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
values = {
"aReqRaw": accel,
"aReqValue": accel,
}
return self.packer.make_can_msg_safety("SCC12", self.SCC_BUS, values)
def _tx_acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
return self.packer.make_can_msg_safety("SCC11", 0, values)
def test_tester_present_allowed(self):
pass
def test_disabled_ecu_alive(self):
pass
@parameterized_class(LDA_BUTTON)
class TestHyundaiNonSCCSafety(TestHyundaiSafety):
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiNonSCCSafety":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_iq(HyundaiSafetyFlagsIQ.NON_SCC | self.SAFETY_PARAM_IQ)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0)
self.safety.init_tests()
def _pcm_status_msg(self, enable):
values = {"CRUISE_LAMP_S": enable, "AliveCounter": self.cnt_gas % 4}
self.__class__.cnt_gas += 1
return self.packer.make_can_msg_safety("EMS16", 0, values, fix_checksum=checksum)
def _acc_state_msg(self, enable):
values = {"CRUISE_LAMP_M": enable, "AliveCounter": self.cnt_gas % 4}
self.__class__.cnt_gas += 1
return self.packer.make_can_msg_safety("EMS16", 0, values, fix_checksum=checksum)
def _user_gas_msg(self, gas: float, controls_allowed: bool = True):
values = {"CF_Ems_AclAct": gas, "CRUISE_LAMP_M": 1, "CRUISE_LAMP_S": controls_allowed, "AliveCounter": self.cnt_gas % 4}
self.__class__.cnt_gas += 1
return self.packer.make_can_msg_safety("EMS16", 0, values, fix_checksum=checksum)
def test_allow_engage_with_gas_pressed(self):
self._rx(self._user_gas_msg(1, self.safety.get_controls_allowed()))
self.safety.set_controls_allowed(True)
self._rx(self._user_gas_msg(1, self.safety.get_controls_allowed()))
self.assertTrue(self.safety.get_controls_allowed())
self._rx(self._user_gas_msg(1, self.safety.get_controls_allowed()))
self.assertTrue(self.safety.get_controls_allowed())
def test_no_disengage_on_gas(self):
self._rx(self._user_gas_msg(0, self.safety.get_controls_allowed()))
self.safety.set_controls_allowed(True)
self._rx(self._user_gas_msg(self.GAS_PRESSED_THRESHOLD + 1, self.safety.get_controls_allowed()))
# Test we allow lateral, but not longitudinal
self.assertTrue(self.safety.get_controls_allowed())
self.assertFalse(self.safety.get_longitudinal_brake_allowed())
self.assertTrue(self.safety.get_longitudinal_gas_allowed())
# Make sure we can re-gain longitudinal actuation
self._rx(self._user_gas_msg(0, self.safety.get_controls_allowed()))
self.assertTrue(self.safety.get_longitudinal_brake_allowed())
self.assertTrue(self.safety.get_longitudinal_gas_allowed())
@parameterized_class(ALL_NON_SCC_HEV_EV_COMBOS)
class TestHyundaiNonSCCSafety_HEV_EV(TestHyundaiSafety):
PCM_STATUS_MSG = ("", "")
ACC_STATE_MSG = ("", "")
GAS_MSG = ("", "")
SAFETY_PARAM = 0
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiNonSCCSafety_HEV_EV":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_iq(HyundaiSafetyFlagsIQ.NON_SCC | self.SAFETY_PARAM_IQ)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, self.SAFETY_PARAM)
self.safety.init_tests()
def _pcm_status_msg(self, enable):
values = {self.PCM_STATUS_MSG[1]: enable}
return self.packer.make_can_msg_safety(self.PCM_STATUS_MSG[0], 0, values)
def _acc_state_msg(self, enable):
values = {self.ACC_STATE_MSG[1]: enable}
return self.packer.make_can_msg_safety(self.ACC_STATE_MSG[0], 0, values)
def _user_gas_msg(self, gas):
values = {self.GAS_MSG[1]: gas}
return self.packer.make_can_msg_safety(self.GAS_MSG[0], 0, values, fix_checksum=checksum)
if __name__ == "__main__":
unittest.main()

370
iqdbc_repo/iqdbc/safety/tests/test_hyundai_canfd.py Normal file → Executable file
View File

@@ -1,104 +1,312 @@
import pytest
#!/usr/bin/env python3
from parameterized import parameterized_class
import unittest
from iqdbc.car.hyundai.values import HyundaiSafetyFlags
from iqdbc.car.structs import CarParams
from iqdbc.safety.tests.hyundai_common import TESTER_PRESENT, canfd_accel, canfd_steer, packet
from iqdbc.safety.tests.libsafety import libsafety_py
import iqdbc.safety.tests.common as common
from iqdbc.safety.tests.common import CANPackerSafety
from iqdbc.safety.tests.hyundai_common import HyundaiButtonBase, HyundaiLongitudinalBase
# All combinations of radar/camera-SCC and gas/hybrid/EV cars
ALL_GAS_EV_HYBRID_COMBOS = [
# Radar SCC
{"GAS_MSG": ("ACCELERATOR_BRAKE_ALT", "ACCELERATOR_PEDAL_PRESSED"), "SCC_BUS": 0, "SAFETY_PARAM": 0},
{"GAS_MSG": ("ACCELERATOR", "ACCELERATOR_PEDAL"), "SCC_BUS": 0, "SAFETY_PARAM": HyundaiSafetyFlags.EV_GAS},
{"GAS_MSG": ("ACCELERATOR_ALT", "ACCELERATOR_PEDAL"), "SCC_BUS": 0, "SAFETY_PARAM": HyundaiSafetyFlags.HYBRID_GAS},
# Camera SCC
{"GAS_MSG": ("ACCELERATOR_BRAKE_ALT", "ACCELERATOR_PEDAL_PRESSED"), "SCC_BUS": 2, "SAFETY_PARAM": HyundaiSafetyFlags.CAMERA_SCC},
{"GAS_MSG": ("ACCELERATOR", "ACCELERATOR_PEDAL"), "SCC_BUS": 2, "SAFETY_PARAM": HyundaiSafetyFlags.EV_GAS | HyundaiSafetyFlags.CAMERA_SCC},
{"GAS_MSG": ("ACCELERATOR_ALT", "ACCELERATOR_PEDAL"), "SCC_BUS": 2, "SAFETY_PARAM": HyundaiSafetyFlags.HYBRID_GAS | HyundaiSafetyFlags.CAMERA_SCC},
]
@pytest.fixture
def safety():
return libsafety_py.libsafety
class TestHyundaiCanfdBase(HyundaiButtonBase, common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest):
TX_MSGS = [[0x50, 0], [0x1CF, 1], [0x2A4, 0]]
STANDSTILL_THRESHOLD = 12 # 0.375 kph
FWD_BLACKLISTED_ADDRS = {2: [0x50, 0x2a4]}
MAX_RATE_UP = 2
MAX_RATE_DOWN = 3
MAX_TORQUE_LOOKUP = [0], [270]
MAX_RT_DELTA = 112
DRIVER_TORQUE_ALLOWANCE = 250
DRIVER_TORQUE_FACTOR = 2
# Safety around steering req bit
MIN_VALID_STEERING_FRAMES = 89
MAX_INVALID_STEERING_FRAMES = 2
PT_BUS = 0
SCC_BUS = 2
STEER_BUS = 0
STEER_MSG = ""
GAS_MSG = ("", "")
BUTTONS_TX_BUS = 1
def _torque_driver_msg(self, torque):
values = {"STEERING_COL_TORQUE": torque}
return self.packer.make_can_msg_safety("MDPS", self.PT_BUS, values)
def _torque_cmd_msg(self, torque, steer_req=1):
values = {"TORQUE_REQUEST": torque, "STEER_REQ": steer_req}
return self.packer.make_can_msg_safety(self.STEER_MSG, self.STEER_BUS, values)
def _speed_msg(self, speed):
values = {f"WHL_Spd{pos}Val": speed * 0.03125 for pos in ["FL", "FR", "RL", "RR"]}
return self.packer.make_can_msg_safety("WHEEL_SPEEDS", self.PT_BUS, values)
def _user_brake_msg(self, brake):
values = {"DriverBraking": brake}
return self.packer.make_can_msg_safety("TCS", self.PT_BUS, values)
def _user_gas_msg(self, gas):
values = {self.GAS_MSG[1]: gas}
return self.packer.make_can_msg_safety(self.GAS_MSG[0], self.PT_BUS, values)
def _pcm_status_msg(self, enable):
values = {"ACCMode": 1 if enable else 0}
return self.packer.make_can_msg_safety("SCC_CONTROL", self.SCC_BUS, values)
def _button_msg(self, buttons, main_button=0, bus=None):
if bus is None:
bus = self.PT_BUS
values = {
"CRUISE_BUTTONS": buttons,
"ADAPTIVE_CRUISE_MAIN_BTN": main_button,
}
return self.packer.make_can_msg_safety("CRUISE_BUTTONS", bus, values)
def _acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
return self.packer.make_can_msg_safety("SCC_CONTROL", self.SCC_BUS, values)
def _lkas_button_msg(self, enabled):
values = {"LDA_BTN": enabled}
return self.packer.make_can_msg_safety("CRUISE_BUTTONS", self.PT_BUS, values)
def _main_cruise_button_msg(self, enabled):
return self._button_msg(0, enabled)
@pytest.mark.parametrize("param", (
0,
HyundaiSafetyFlags.EV_GAS,
HyundaiSafetyFlags.HYBRID_GAS,
HyundaiSafetyFlags.LONG,
HyundaiSafetyFlags.CAMERA_SCC,
HyundaiSafetyFlags.CANFD_LKA_STEERING,
HyundaiSafetyFlags.CANFD_ALT_BUTTONS,
HyundaiSafetyFlags.CANFD_LKA_STEERING | HyundaiSafetyFlags.CANFD_LKA_STEERING_ALT,
HyundaiSafetyFlags.CANFD_LKA_STEERING | HyundaiSafetyFlags.LONG,
HyundaiSafetyFlags.CANFD_LKA_STEERING | HyundaiSafetyFlags.LONG | HyundaiSafetyFlags.CANFD_ALT_BUTTONS,
))
def test_canfd_safety_configurations_initialize(safety, param):
assert safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, param) == 0
safety.init_tests()
assert safety.get_current_safety_param() == param
class TestHyundaiCanfdLFASteeringBase(TestHyundaiCanfdBase):
TX_MSGS = [[0x12A, 0], [0x1A0, 1], [0x1CF, 0], [0x1E0, 0]]
RELAY_MALFUNCTION_ADDRS = {0: (0x12A, 0x1E0)} # LFA, LFAHDA_CLUSTER
FWD_BLACKLISTED_ADDRS = {2: [0x12A, 0x1E0]}
STEER_MSG = "LFA"
BUTTONS_TX_BUS = 2
SAFETY_PARAM: int
@classmethod
def setUpClass(cls):
super().setUpClass()
if cls.__name__ in ("TestHyundaiCanfdLFASteering", "TestHyundaiCanfdLFASteeringAltButtons"):
cls.packer = None
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_canfd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, self.SAFETY_PARAM)
self.safety.init_tests()
@pytest.mark.parametrize(("param", "addr", "length"), (
(0, 0x12A, 16),
(HyundaiSafetyFlags.CANFD_LKA_STEERING, 0x50, 16),
(HyundaiSafetyFlags.CANFD_LKA_STEERING | HyundaiSafetyFlags.CANFD_LKA_STEERING_ALT, 0x110, 32),
(HyundaiSafetyFlags.CANFD_LKA_STEERING | HyundaiSafetyFlags.LONG, 0x12A, 16),
))
def test_canfd_steering_limits(safety, param, addr, length):
safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, param)
safety.init_tests()
safety.set_controls_allowed(False)
assert safety.safety_tx_hook(canfd_steer(addr, length, 0, False))
assert not safety.safety_tx_hook(canfd_steer(addr, length, 1))
safety.set_controls_allowed(True)
assert safety.safety_tx_hook(canfd_steer(addr, length, 10))
safety.set_desired_torque_last(512)
safety.set_rt_torque_last(512)
assert safety.safety_tx_hook(canfd_steer(addr, length, 512))
assert not safety.safety_tx_hook(canfd_steer(addr, length, 513))
assert not safety.safety_tx_hook(canfd_steer(addr, length, -513))
@parameterized_class(ALL_GAS_EV_HYBRID_COMBOS)
class TestHyundaiCanfdLFASteering(TestHyundaiCanfdLFASteeringBase):
pass
def test_canfd_tx_whitelist_and_buttons(safety):
safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, 0)
safety.init_tests()
assert not safety.safety_tx_hook(packet(0x123, 0, 8))
assert not safety.safety_tx_hook(packet(0x12A, 1, 16))
class TestHyundaiCanfdLFASteeringAltButtonsBase(TestHyundaiCanfdLFASteeringBase):
safety.set_controls_allowed(False)
assert not safety.safety_tx_hook(packet(0x1CF, 0, 8, {2: 1}))
assert not safety.safety_tx_hook(packet(0x1CF, 0, 8, {2: 2}))
assert not safety.safety_tx_hook(packet(0x1CF, 0, 8, {2: 4}))
SAFETY_PARAM: int
safety.set_controls_allowed(True)
assert safety.safety_tx_hook(packet(0x1CF, 0, 8, {2: 1}))
assert not safety.safety_tx_hook(packet(0x1CF, 0, 8, {2: 2}))
assert safety.safety_tx_hook(packet(0x1CF, 0, 8, {2: 4}))
def setUp(self):
self.packer = CANPackerSafety("hyundai_canfd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.CANFD_ALT_BUTTONS | self.SAFETY_PARAM)
self.safety.init_tests()
safety.set_controls_allowed(False)
safety.set_cruise_engaged_prev(True)
assert safety.safety_tx_hook(packet(0x1CF, 0, 8, {2: 4}))
def _button_msg(self, buttons, main_button=0, bus=1):
values = {
"CRUISE_BUTTONS": buttons,
"ADAPTIVE_CRUISE_MAIN_BTN": main_button,
}
return self.packer.make_can_msg_safety("CRUISE_BUTTONS_ALT", self.PT_BUS, values)
def _lkas_button_msg(self, enabled):
values = {"LDA_BTN": enabled}
return self.packer.make_can_msg_safety("CRUISE_BUTTONS_ALT", self.PT_BUS, values)
def _acc_cancel_msg(self, cancel, accel=0):
values = {"ACCMode": 4 if cancel else 0, "aReqRaw": accel, "aReqValue": accel}
return self.packer.make_can_msg_safety("SCC_CONTROL", self.PT_BUS, values)
def test_button_sends(self):
"""
No button send allowed with alt buttons.
"""
for enabled in (True, False):
for btn in range(8):
self.safety.set_controls_allowed(enabled)
self.assertFalse(self._tx(self._button_msg(btn)))
def test_acc_cancel(self):
# FIXME: the CANFD_ALT_BUTTONS cars are the only ones that use SCC_CONTROL to cancel, why can't we use buttons?
for enabled in (True, False):
self.safety.set_controls_allowed(enabled)
self.assertTrue(self._tx(self._acc_cancel_msg(True)))
self.assertFalse(self._tx(self._acc_cancel_msg(True, accel=1)))
self.assertFalse(self._tx(self._acc_cancel_msg(False)))
def test_canfd_stock_longitudinal_only_allows_cancel(safety):
safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, 0)
safety.init_tests()
assert safety.safety_tx_hook(canfd_accel(0, acc_mode=4))
assert not safety.safety_tx_hook(canfd_accel(1, acc_mode=4))
assert not safety.safety_tx_hook(canfd_accel(0, acc_mode=0))
@parameterized_class(ALL_GAS_EV_HYBRID_COMBOS)
class TestHyundaiCanfdLFASteeringAltButtons(TestHyundaiCanfdLFASteeringAltButtonsBase):
pass
def test_canfd_longitudinal_accel_limits(safety):
safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.LONG)
safety.init_tests()
class TestHyundaiCanfdLKASteeringEV(TestHyundaiCanfdBase):
safety.set_controls_allowed(False)
assert safety.safety_tx_hook(canfd_accel(0))
assert not safety.safety_tx_hook(canfd_accel(1))
TX_MSGS = [[0x50, 0], [0x1CF, 1], [0x2A4, 0]]
RELAY_MALFUNCTION_ADDRS = {0: (0x50, 0x2a4)} # LKAS, CAM_0x2A4
FWD_BLACKLISTED_ADDRS = {2: [0x50, 0x2a4]}
safety.set_controls_allowed(True)
for accel in (-400, 0, 250):
assert safety.safety_tx_hook(canfd_accel(accel))
for accel in (-401, 251):
assert not safety.safety_tx_hook(canfd_accel(accel))
PT_BUS = 1
SCC_BUS = 1
STEER_MSG = "LKAS"
GAS_MSG = ("ACCELERATOR", "ACCELERATOR_PEDAL")
def setUp(self):
self.packer = CANPackerSafety("hyundai_canfd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.CANFD_LKA_STEERING | HyundaiSafetyFlags.EV_GAS)
self.safety.init_tests()
def test_canfd_hda2_diagnostic_payload_is_restricted(safety):
param = HyundaiSafetyFlags.CANFD_LKA_STEERING | HyundaiSafetyFlags.LONG
safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, param)
safety.init_tests()
assert safety.safety_tx_hook(packet(0x730, 1, 8, dict(enumerate(TESTER_PRESENT))))
assert not safety.safety_tx_hook(packet(0x730, 1, 8, {0: 3, 1: 0x22}))
# TODO: Handle ICE and HEV configurations once we see cars that use the new messages
class TestHyundaiCanfdLKASteeringAltEV(TestHyundaiCanfdBase):
TX_MSGS = [[0x110, 0], [0x1CF, 1], [0x362, 0]]
RELAY_MALFUNCTION_ADDRS = {0: (0x110, 0x362)} # LKAS_ALT, CAM_0x362
FWD_BLACKLISTED_ADDRS = {2: [0x110, 0x362]}
PT_BUS = 1
SCC_BUS = 1
STEER_MSG = "LKAS_ALT"
GAS_MSG = ("ACCELERATOR", "ACCELERATOR_PEDAL")
def setUp(self):
self.packer = CANPackerSafety("hyundai_canfd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.CANFD_LKA_STEERING | HyundaiSafetyFlags.EV_GAS |
HyundaiSafetyFlags.CANFD_LKA_STEERING_ALT)
self.safety.init_tests()
class TestHyundaiCanfdLKASteeringLongEV(HyundaiLongitudinalBase, TestHyundaiCanfdLKASteeringEV):
TX_MSGS = [[0x50, 0], [0x1CF, 1], [0x2A4, 0], [0x51, 0], [0x730, 1], [0x12a, 1], [0x160, 1],
[0x1e0, 1], [0x1a0, 1], [0x1ea, 1], [0x200, 1], [0x345, 1], [0x1da, 1]]
RELAY_MALFUNCTION_ADDRS = {0: (0x50, 0x2a4), 1: (0x1a0,)} # LKAS, CAM_0x2A4, SCC_CONTROL
DISABLED_ECU_UDS_MSG = (0x730, 1)
DISABLED_ECU_ACTUATION_MSG = (0x1a0, 1)
STEER_MSG = "LFA"
GAS_MSG = ("ACCELERATOR", "ACCELERATOR_PEDAL")
STEER_BUS = 1
def setUp(self):
self.packer = CANPackerSafety("hyundai_canfd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.CANFD_LKA_STEERING |
HyundaiSafetyFlags.LONG | HyundaiSafetyFlags.EV_GAS)
self.safety.init_tests()
def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
values = {
"aReqRaw": accel,
"aReqValue": accel,
}
return self.packer.make_can_msg_safety("SCC_CONTROL", self.PT_BUS, values)
def _tx_acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
return self.packer.make_can_msg_safety("SCC_CONTROL", self.PT_BUS, values)
# Tests longitudinal for ICE, hybrid, EV cars with LFA steering
class TestHyundaiCanfdLFASteeringLongBase(HyundaiLongitudinalBase, TestHyundaiCanfdLFASteeringBase):
FWD_BLACKLISTED_ADDRS = {2: [0x12a, 0x1e0, 0x1a0, 0x160]}
RELAY_MALFUNCTION_ADDRS = {0: (0x12A, 0x1E0, 0x1a0, 0x160)} # LFA, LFAHDA_CLUSTER, SCC_CONTROL, ADRV_0x160
DISABLED_ECU_UDS_MSG = (0x7D0, 0)
DISABLED_ECU_ACTUATION_MSG = (0x1a0, 0)
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiCanfdLFASteeringLongBase":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_canfd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.LONG | self.SAFETY_PARAM)
self.safety.init_tests()
def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
values = {
"aReqRaw": accel,
"aReqValue": accel,
}
return self.packer.make_can_msg_safety("SCC_CONTROL", self.PT_BUS, values)
def _tx_acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
return self.packer.make_can_msg_safety("SCC_CONTROL", self.PT_BUS, values)
# no knockout
def test_tester_present_allowed(self):
pass
@parameterized_class(ALL_GAS_EV_HYBRID_COMBOS)
class TestHyundaiCanfdLFASteeringLong(TestHyundaiCanfdLFASteeringLongBase):
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiCanfdLFASteeringLong":
cls.safety = None
raise unittest.SkipTest
@parameterized_class(ALL_GAS_EV_HYBRID_COMBOS)
class TestHyundaiCanfdLFASteeringLongAltButtons(TestHyundaiCanfdLFASteeringLongBase, TestHyundaiCanfdLFASteeringAltButtonsBase):
@classmethod
def setUpClass(cls):
if cls.__name__ == "TestHyundaiCanfdLFASteeringLongAltButtons":
cls.safety = None
raise unittest.SkipTest
def setUp(self):
self.packer = CANPackerSafety("hyundai_canfd_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.LONG | HyundaiSafetyFlags.CANFD_ALT_BUTTONS | self.SAFETY_PARAM)
self.safety.init_tests()
def test_acc_cancel(self):
# Alt buttons does not use SCC_CONTROL to cancel if longitudinal
pass
if __name__ == "__main__":
unittest.main()

39
iqdbc_repo/iqdbc/safety/tests/test_hyundai_controller.py
View File

@@ -1,39 +0,0 @@
import pytest
from iqdbc.car import gen_empty_fingerprint, structs
from iqdbc.car.hyundai.interface import CarInterface
from iqdbc.car.hyundai.values import CAR
from iqdbc.safety.tests.libsafety import libsafety_py
@pytest.mark.parametrize("candidate", list(CAR), ids=lambda candidate: candidate.value)
@pytest.mark.parametrize("alpha_long", (False, True), ids=("stock_long", "openpilot_long"))
def test_controller_frames_match_configured_safety(candidate, alpha_long, monkeypatch, tmp_path):
"""Run real controller output through the safety configuration selected for every HKG platform."""
monkeypatch.setenv("PARAMS_ROOT", str(tmp_path / candidate.value))
fingerprint = gen_empty_fingerprint()
cp = CarInterface.get_params(candidate, fingerprint, [], alpha_long, False, False)
cp_iq = CarInterface.get_params_iq(cp, candidate, fingerprint, [], alpha_long, False, False)
interface = CarInterface(cp, cp_iq)
interface.update([])
safety_config = cp.safetyConfigs[-1]
safety = libsafety_py.libsafety
assert safety.set_safety_hooks(safety_config.safetyModel.raw, safety_config.safetyParam) == 0
safety.init_tests()
safety.set_controls_allowed(True)
control = structs.CarControl.new_message()
control.enabled = True
control.latActive = True
control.longActive = alpha_long
control.actuators.torque = 0.01
control.actuators.accel = 0.0
for frame in range(20):
_, can_sends = interface.apply(control.as_reader(), structs.IQCarControl())
assert isinstance(can_sends, list)
for address, data, bus in can_sends:
packet = libsafety_py.make_CANPacket(address, bus, data)
rejection = f"{candidate.value} frame {frame}: safety rejected address={address:#x} bus={bus} data={data.hex()}"
assert safety.safety_tx_hook(packet), rejection

6
iqdbc_repo/iqdbc/safety/tests/test_nissan.py
View File

@@ -2,10 +2,8 @@
import unittest
from iqdbc.car.nissan.values import NissanSafetyFlags
from iqdbc.iqpilot.car.nissan.values import NissanSafetyFlagsIQ
from iqdbc.car.structs import CarParams
# NISSAN_PARAM_IQ_LEAF in safety/modes/nissan.h (IQ safety framework flag)
NISSAN_PARAM_IQ_LEAF = 1
from iqdbc.safety.tests.libsafety import libsafety_py
import iqdbc.safety.tests.common as common
from iqdbc.safety.tests.common import CANPackerSafety
@@ -107,7 +105,7 @@ class TestNissanLeafSafety(TestNissanSafety):
def setUp(self):
self.packer = CANPackerSafety("nissan_leaf_2018_generated")
self.safety = libsafety_py.libsafety
self.safety.set_current_safety_param_iq(NISSAN_PARAM_IQ_LEAF)
self.safety.set_current_safety_param_iq(NissanSafetyFlagsIQ.LEAF)
self.safety.set_safety_hooks(CarParams.SafetyModel.nissan, 0)
self.safety.init_tests()

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

@@ -11,7 +11,8 @@ 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.values import CarControllerParams, TeslaSafetyFlags, CANBUS
from iqdbc.car.tesla.teslacan import get_steer_ctrl_type
from iqdbc.car.tesla.values import CarControllerParams, TeslaSafetyFlags, TeslaFlags, CANBUS
from iqdbc.car.structs import CarParams
from iqdbc.car.vehicle_model import VehicleModel
from iqdbc.can import CANDefine
@@ -22,7 +23,6 @@ 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,7 +34,6 @@ 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]}
@@ -84,7 +83,11 @@ 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):
values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": int(state) << self.STEER_TYPE_SHIFT}
# 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}
if increment_timer:
self.safety.set_timer(self.cnt_angle_cmd * int(1e6 / self.LATERAL_FREQUENCY))
self.__class__.cnt_angle_cmd += 1
@@ -404,9 +407,8 @@ class TestTeslaStockSafety(TestTeslaSafetyBase):
self.assertFalse(self._tx(no_aeb_msg))
class TestTeslaLegacyDasSteeringStockSafety(TestTeslaStockSafety):
SAFETY_PARAM = TeslaSafetyFlags.LEGACY_DAS_STEERING
STEER_TYPE_SHIFT = 1
class TestTeslaFSD14StockSafety(TestTeslaStockSafety):
SAFETY_PARAM = TeslaSafetyFlags.FSD_14
class TestTeslaLongitudinalSafety(TestTeslaSafetyBase):
@@ -457,18 +459,14 @@ class TestTeslaLongitudinalSafety(TestTeslaSafetyBase):
self.assertFalse(self._tx(self._long_control_msg(set_speed=0, accel_limits=(-0.1, -0.1))))
class TestTeslaLegacyDasSteeringLongitudinalSafety(TestTeslaLongitudinalSafety):
SAFETY_PARAM = TeslaSafetyFlags.LONG_CONTROL | TeslaSafetyFlags.LEGACY_DAS_STEERING
STEER_TYPE_SHIFT = 1
class TestTeslaFSD14LongitudinalSafety(TestTeslaLongitudinalSafety):
SAFETY_PARAM = TeslaSafetyFlags.LONG_CONTROL | TeslaSafetyFlags.FSD_14
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,7 +20,6 @@ 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):
@@ -44,7 +43,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", 1, values)
return self.packer.make_can_msg_safety("Bremse_1", 0, values)
# Brake light switch (shared message Motor_2)
def _user_brake_msg(self, brake):
@@ -64,7 +63,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", 1, values)
return self.packer.make_can_msg_safety("Lenkhilfe_3", 0, values)
# openpilot steering output torque
def _torque_cmd_msg(self, torque, steer_req=1, hca_status=7):
@@ -77,17 +76,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", 1, values)
return self.packer.make_can_msg_safety("Motor_2", 0, 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", 1, values)
return self.packer.make_can_msg_safety("Motor_5", 0, 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", 1, values)
return self.packer.make_can_msg_safety("Motor_3", 0, values)
# Cruise control buttons (GRA_Neu)
def _button_msg(self, _set=False, resume=False, cancel=False, bus=2):
@@ -117,7 +116,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], [MSG_IQ, 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]]
FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_1, MSG_LDW_1]}
def setUp(self):
@@ -140,11 +139,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_MOTOR_5, 2], [MSG_APD_1, 1], [MSG_IQ, 1]]
FWD_BLACKLISTED_ADDRS = {0: [MSG_MOTOR_2, MSG_MOTOR_5, MSG_GRA_NEU],
[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],
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, MSG_MOTOR_5)}
2: (MSG_MOTOR_2, MSG_GRA_NEU)}
INACTIVE_ACCEL = 3.01
def setUp(self):
@@ -169,20 +168,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=1))
self._rx(self._button_msg(bus=1))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0))
self._rx(self._button_msg(bus=0))
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=1))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0))
self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} rising edge")
self._rx(self._button_msg(bus=1))
self._rx(self._button_msg(bus=0))
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=1))
self._rx(self._button_msg(cancel=True, bus=0))
self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after cancel")
def test_main_switch(self):
@@ -224,8 +223,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=1))
self._rx(self._button_msg(bus=1))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0))
self._rx(self._button_msg(bus=0))
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):
@@ -251,42 +250,5 @@ 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()