brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Your Name
2024-04-27 03:19:19 -05:00
parent df418b503f
commit 03af7b6107
306 changed files with 108529 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
panda/board/safety/safety_body.h Normal file
View File

@@ -0,0 +1,46 @@
const CanMsg BODY_TX_MSGS[] = {{0x250, 0, 8}, {0x250, 0, 6}, {0x251, 0, 5}, // body
{0x350, 0, 8}, {0x350, 0, 6}, {0x351, 0, 5}, // knee
{0x1, 0, 8}}; // CAN flasher
RxCheck body_rx_checks[] = {
{.msg = {{0x201, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}},
};
static void body_rx_hook(const CANPacket_t *to_push) {
// body is never at standstill
vehicle_moving = true;
if (GET_ADDR(to_push) == 0x201U) {
controls_allowed = true;
}
}
static bool body_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);
int len = GET_LEN(to_send);
if (!controls_allowed && (addr != 0x1)) {
tx = false;
}
// Allow going into CAN flashing mode for base & knee even if controls are not allowed
bool flash_msg = ((addr == 0x250) || (addr == 0x350)) && (len == 8);
if (!controls_allowed && (GET_BYTES(to_send, 0, 4) == 0xdeadfaceU) && (GET_BYTES(to_send, 4, 4) == 0x0ab00b1eU) && flash_msg) {
tx = true;
}
return tx;
}
static safety_config body_init(uint16_t param) {
UNUSED(param);
return BUILD_SAFETY_CFG(body_rx_checks, BODY_TX_MSGS);
}
const safety_hooks body_hooks = {
.init = body_init,
.rx = body_rx_hook,
.tx = body_tx_hook,
.fwd = default_fwd_hook,
};

296
panda/board/safety/safety_chrysler.h Normal file
View File

@@ -0,0 +1,296 @@
const SteeringLimits CHRYSLER_STEERING_LIMITS = {
.max_steer = 261,
.max_rt_delta = 112,
.max_rt_interval = 250000,
.max_rate_up = 3,
.max_rate_down = 3,
.max_torque_error = 80,
.type = TorqueMotorLimited,
};
const SteeringLimits CHRYSLER_RAM_DT_STEERING_LIMITS = {
.max_steer = 350,
.max_rt_delta = 112,
.max_rt_interval = 250000,
.max_rate_up = 6,
.max_rate_down = 6,
.max_torque_error = 80,
.type = TorqueMotorLimited,
};
const SteeringLimits CHRYSLER_RAM_HD_STEERING_LIMITS = {
.max_steer = 361,
.max_rt_delta = 182,
.max_rt_interval = 250000,
.max_rate_up = 14,
.max_rate_down = 14,
.max_torque_error = 80,
.type = TorqueMotorLimited,
};
typedef struct {
const int EPS_2;
const int ESP_1;
const int ESP_8;
const int ECM_5;
const int DAS_3;
const int DAS_6;
const int LKAS_COMMAND;
const int CRUISE_BUTTONS;
} ChryslerAddrs;
// CAN messages for Chrysler/Jeep platforms
const ChryslerAddrs CHRYSLER_ADDRS = {
.EPS_2 = 0x220, // EPS driver input torque
.ESP_1 = 0x140, // Brake pedal and vehicle speed
.ESP_8 = 0x11C, // Brake pedal and vehicle speed
.ECM_5 = 0x22F, // Throttle position sensor
.DAS_3 = 0x1F4, // ACC engagement states from DASM
.DAS_6 = 0x2A6, // LKAS HUD and auto headlight control from DASM
.LKAS_COMMAND = 0x292, // LKAS controls from DASM
.CRUISE_BUTTONS = 0x23B, // Cruise control buttons
};
// CAN messages for the 5th gen RAM DT platform
const ChryslerAddrs CHRYSLER_RAM_DT_ADDRS = {
.EPS_2 = 0x31, // EPS driver input torque
.ESP_1 = 0x83, // Brake pedal and vehicle speed
.ESP_8 = 0x79, // Brake pedal and vehicle speed
.ECM_5 = 0x9D, // Throttle position sensor
.DAS_3 = 0x99, // ACC engagement states from DASM
.DAS_6 = 0xFA, // LKAS HUD and auto headlight control from DASM
.LKAS_COMMAND = 0xA6, // LKAS controls from DASM
.CRUISE_BUTTONS = 0xB1, // Cruise control buttons
};
// CAN messages for the 5th gen RAM HD platform
const ChryslerAddrs CHRYSLER_RAM_HD_ADDRS = {
.EPS_2 = 0x220, // EPS driver input torque
.ESP_1 = 0x140, // Brake pedal and vehicle speed
.ESP_8 = 0x11C, // Brake pedal and vehicle speed
.ECM_5 = 0x22F, // Throttle position sensor
.DAS_3 = 0x1F4, // ACC engagement states from DASM
.DAS_6 = 0x275, // LKAS HUD and auto headlight control from DASM
.LKAS_COMMAND = 0x276, // LKAS controls from DASM
.CRUISE_BUTTONS = 0x23A, // Cruise control buttons
};
const CanMsg CHRYSLER_TX_MSGS[] = {
{CHRYSLER_ADDRS.CRUISE_BUTTONS, 0, 3},
{CHRYSLER_ADDRS.LKAS_COMMAND, 0, 6},
{CHRYSLER_ADDRS.DAS_6, 0, 8},
};
const CanMsg CHRYSLER_RAM_DT_TX_MSGS[] = {
{CHRYSLER_RAM_DT_ADDRS.CRUISE_BUTTONS, 2, 3},
{CHRYSLER_RAM_DT_ADDRS.LKAS_COMMAND, 0, 8},
{CHRYSLER_RAM_DT_ADDRS.DAS_6, 0, 8},
};
const CanMsg CHRYSLER_RAM_HD_TX_MSGS[] = {
{CHRYSLER_RAM_HD_ADDRS.CRUISE_BUTTONS, 2, 3},
{CHRYSLER_RAM_HD_ADDRS.LKAS_COMMAND, 0, 8},
{CHRYSLER_RAM_HD_ADDRS.DAS_6, 0, 8},
};
RxCheck chrysler_rx_checks[] = {
{.msg = {{CHRYSLER_ADDRS.EPS_2, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_ADDRS.ESP_1, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
//{.msg = {{ESP_8, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}}},
{.msg = {{514, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_ADDRS.ECM_5, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_ADDRS.DAS_3, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
};
RxCheck chrysler_ram_dt_rx_checks[] = {
{.msg = {{CHRYSLER_RAM_DT_ADDRS.EPS_2, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_DT_ADDRS.ESP_1, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_DT_ADDRS.ESP_8, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_DT_ADDRS.ECM_5, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_DT_ADDRS.DAS_3, 2, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
};
RxCheck chrysler_ram_hd_rx_checks[] = {
{.msg = {{CHRYSLER_RAM_HD_ADDRS.EPS_2, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_HD_ADDRS.ESP_1, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_HD_ADDRS.ESP_8, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_HD_ADDRS.ECM_5, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{CHRYSLER_RAM_HD_ADDRS.DAS_3, 2, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
};
const uint32_t CHRYSLER_PARAM_RAM_DT = 1U; // set for Ram DT platform
const uint32_t CHRYSLER_PARAM_RAM_HD = 2U; // set for Ram HD platform
enum {
CHRYSLER_RAM_DT,
CHRYSLER_RAM_HD,
CHRYSLER_PACIFICA, // plus Jeep
} chrysler_platform = CHRYSLER_PACIFICA;
const ChryslerAddrs *chrysler_addrs = &CHRYSLER_ADDRS;
static uint32_t chrysler_get_checksum(const CANPacket_t *to_push) {
int checksum_byte = GET_LEN(to_push) - 1U;
return (uint8_t)(GET_BYTE(to_push, checksum_byte));
}
static uint32_t chrysler_compute_checksum(const CANPacket_t *to_push) {
// TODO: clean this up
// http://illmatics.com/Remote%20Car%20Hacking.pdf
uint8_t checksum = 0xFFU;
int len = GET_LEN(to_push);
for (int j = 0; j < (len - 1); j++) {
uint8_t shift = 0x80U;
uint8_t curr = (uint8_t)GET_BYTE(to_push, j);
for (int i=0; i<8; i++) {
uint8_t bit_sum = curr & shift;
uint8_t temp_chk = checksum & 0x80U;
if (bit_sum != 0U) {
bit_sum = 0x1C;
if (temp_chk != 0U) {
bit_sum = 1;
}
checksum = checksum << 1;
temp_chk = checksum | 1U;
bit_sum ^= temp_chk;
} else {
if (temp_chk != 0U) {
bit_sum = 0x1D;
}
checksum = checksum << 1;
bit_sum ^= checksum;
}
checksum = bit_sum;
shift = shift >> 1;
}
}
return (uint8_t)(~checksum);
}
static uint8_t chrysler_get_counter(const CANPacket_t *to_push) {
return (uint8_t)(GET_BYTE(to_push, 6) >> 4);
}
static void chrysler_rx_hook(const CANPacket_t *to_push) {
const int bus = GET_BUS(to_push);
const int addr = GET_ADDR(to_push);
// Measured EPS torque
if ((bus == 0) && (addr == chrysler_addrs->EPS_2)) {
int torque_meas_new = ((GET_BYTE(to_push, 4) & 0x7U) << 8) + GET_BYTE(to_push, 5) - 1024U;
update_sample(&torque_meas, torque_meas_new);
}
// enter controls on rising edge of ACC, exit controls on ACC off
const int das_3_bus = (chrysler_platform == CHRYSLER_PACIFICA) ? 0 : 2;
if ((bus == das_3_bus) && (addr == chrysler_addrs->DAS_3)) {
acc_main_on = GET_BIT(to_push, 20U);
bool cruise_engaged = GET_BIT(to_push, 21U);
pcm_cruise_check(cruise_engaged);
}
// TODO: use the same message for both
// update vehicle moving
if ((chrysler_platform != CHRYSLER_PACIFICA) && (bus == 0) && (addr == chrysler_addrs->ESP_8)) {
vehicle_moving = ((GET_BYTE(to_push, 4) << 8) + GET_BYTE(to_push, 5)) != 0U;
}
if ((chrysler_platform == CHRYSLER_PACIFICA) && (bus == 0) && (addr == 514)) {
int speed_l = (GET_BYTE(to_push, 0) << 4) + (GET_BYTE(to_push, 1) >> 4);
int speed_r = (GET_BYTE(to_push, 2) << 4) + (GET_BYTE(to_push, 3) >> 4);
vehicle_moving = (speed_l != 0) || (speed_r != 0);
}
// exit controls on rising edge of gas press
if ((bus == 0) && (addr == chrysler_addrs->ECM_5)) {
gas_pressed = GET_BYTE(to_push, 0U) != 0U;
}
// exit controls on rising edge of brake press
if ((bus == 0) && (addr == chrysler_addrs->ESP_1)) {
brake_pressed = ((GET_BYTE(to_push, 0U) & 0xFU) >> 2U) == 1U;
}
generic_rx_checks((bus == 0) && (addr == chrysler_addrs->LKAS_COMMAND));
}
static bool chrysler_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);
// STEERING
if (addr == chrysler_addrs->LKAS_COMMAND) {
int start_byte = (chrysler_platform == CHRYSLER_PACIFICA) ? 0 : 1;
int desired_torque = ((GET_BYTE(to_send, start_byte) & 0x7U) << 8) | GET_BYTE(to_send, start_byte + 1);
desired_torque -= 1024;
const SteeringLimits limits = (chrysler_platform == CHRYSLER_PACIFICA) ? CHRYSLER_STEERING_LIMITS :
(chrysler_platform == CHRYSLER_RAM_DT) ? CHRYSLER_RAM_DT_STEERING_LIMITS : CHRYSLER_RAM_HD_STEERING_LIMITS;
bool steer_req = (chrysler_platform == CHRYSLER_PACIFICA) ? GET_BIT(to_send, 4U) : (GET_BYTE(to_send, 3) & 0x7U) == 2U;
if (steer_torque_cmd_checks(desired_torque, steer_req, limits)) {
tx = false;
}
}
// FORCE CANCEL: only the cancel button press is allowed
if (addr == chrysler_addrs->CRUISE_BUTTONS) {
const bool is_cancel = GET_BYTE(to_send, 0) == 1U;
const bool is_resume = GET_BYTE(to_send, 0) == 0x10U;
const bool allowed = is_cancel || (is_resume && controls_allowed);
if (!allowed) {
tx = false;
}
}
return tx;
}
static int chrysler_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
// forward to camera
if (bus_num == 0) {
bus_fwd = 2;
}
// forward all messages from camera except LKAS messages
const bool is_lkas = ((addr == chrysler_addrs->LKAS_COMMAND) || (addr == chrysler_addrs->DAS_6));
if ((bus_num == 2) && !is_lkas){
bus_fwd = 0;
}
return bus_fwd;
}
static safety_config chrysler_init(uint16_t param) {
safety_config ret;
bool enable_ram_dt = GET_FLAG(param, CHRYSLER_PARAM_RAM_DT);
if (enable_ram_dt) {
chrysler_platform = CHRYSLER_RAM_DT;
chrysler_addrs = &CHRYSLER_RAM_DT_ADDRS;
ret = BUILD_SAFETY_CFG(chrysler_ram_dt_rx_checks, CHRYSLER_RAM_DT_TX_MSGS);
#ifdef ALLOW_DEBUG
} else if (GET_FLAG(param, CHRYSLER_PARAM_RAM_HD)) {
chrysler_platform = CHRYSLER_RAM_HD;
chrysler_addrs = &CHRYSLER_RAM_HD_ADDRS;
ret = BUILD_SAFETY_CFG(chrysler_ram_hd_rx_checks, CHRYSLER_RAM_HD_TX_MSGS);
#endif
} else {
chrysler_platform = CHRYSLER_PACIFICA;
chrysler_addrs = &CHRYSLER_ADDRS;
ret = BUILD_SAFETY_CFG(chrysler_rx_checks, CHRYSLER_TX_MSGS);
}
return ret;
}
const safety_hooks chrysler_hooks = {
.init = chrysler_init,
.rx = chrysler_rx_hook,
.tx = chrysler_tx_hook,
.fwd = chrysler_fwd_hook,
.get_counter = chrysler_get_counter,
.get_checksum = chrysler_get_checksum,
.compute_checksum = chrysler_compute_checksum,
};

68
panda/board/safety/safety_defaults.h Normal file
View File

@@ -0,0 +1,68 @@
void default_rx_hook(const CANPacket_t *to_push) {
UNUSED(to_push);
}
// *** no output safety mode ***
static safety_config nooutput_init(uint16_t param) {
UNUSED(param);
return (safety_config){NULL, 0, NULL, 0};
}
static bool nooutput_tx_hook(const CANPacket_t *to_send) {
UNUSED(to_send);
return false;
}
static int default_fwd_hook(int bus_num, int addr) {
UNUSED(bus_num);
UNUSED(addr);
return -1;
}
const safety_hooks nooutput_hooks = {
.init = nooutput_init,
.rx = default_rx_hook,
.tx = nooutput_tx_hook,
.fwd = default_fwd_hook,
};
// *** all output safety mode ***
// Enables passthrough mode where relay is open and bus 0 gets forwarded to bus 2 and vice versa
const uint16_t ALLOUTPUT_PARAM_PASSTHROUGH = 1;
bool alloutput_passthrough = false;
static safety_config alloutput_init(uint16_t param) {
controls_allowed = true;
alloutput_passthrough = GET_FLAG(param, ALLOUTPUT_PARAM_PASSTHROUGH);
return (safety_config){NULL, 0, NULL, 0};
}
static bool alloutput_tx_hook(const CANPacket_t *to_send) {
UNUSED(to_send);
return true;
}
static int alloutput_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
UNUSED(addr);
if (alloutput_passthrough) {
if (bus_num == 0) {
bus_fwd = 2;
}
if (bus_num == 2) {
bus_fwd = 0;
}
}
return bus_fwd;
}
const safety_hooks alloutput_hooks = {
.init = alloutput_init,
.rx = default_rx_hook,
.tx = alloutput_tx_hook,
.fwd = alloutput_fwd_hook,
};

37
panda/board/safety/safety_elm327.h Normal file
View File

@@ -0,0 +1,37 @@
const int GM_CAMERA_DIAG_ADDR = 0x24B;
static bool elm327_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);
int len = GET_LEN(to_send);
// All ISO 15765-4 messages must be 8 bytes long
if (len != 8) {
tx = false;
}
// Check valid 29 bit send addresses for ISO 15765-4
// Check valid 11 bit send addresses for ISO 15765-4
if ((addr != 0x18DB33F1) && ((addr & 0x1FFF00FF) != 0x18DA00F1) &&
((addr & 0x1FFFFF00) != 0x600) && ((addr & 0x1FFFFF00) != 0x700) &&
(addr != GM_CAMERA_DIAG_ADDR)) {
tx = false;
}
// GM camera uses non-standard diagnostic address, this has no control message address collisions
if ((addr == GM_CAMERA_DIAG_ADDR) && (len == 8)) {
// Only allow known frame types for ISO 15765-2
if ((GET_BYTE(to_send, 0) & 0xF0U) > 0x30U) {
tx = false;
}
}
return tx;
}
// If current_board->has_obd and safety_param == 0, bus 1 is multiplexed to the OBD-II port
const safety_hooks elm327_hooks = {
.init = nooutput_init,
.rx = default_rx_hook,
.tx = elm327_tx_hook,
.fwd = default_fwd_hook,
};

424
panda/board/safety/safety_ford.h Normal file
View File

@@ -0,0 +1,424 @@
// Safety-relevant CAN messages for Ford vehicles.
#define FORD_EngBrakeData 0x165 // RX from PCM, for driver brake pedal and cruise state
#define FORD_EngVehicleSpThrottle 0x204 // RX from PCM, for driver throttle input
#define FORD_DesiredTorqBrk 0x213 // RX from ABS, for standstill state
#define FORD_BrakeSysFeatures 0x415 // RX from ABS, for vehicle speed
#define FORD_EngVehicleSpThrottle2 0x202 // RX from PCM, for second vehicle speed
#define FORD_Yaw_Data_FD1 0x91 // RX from RCM, for yaw rate
#define FORD_Steering_Data_FD1 0x083 // TX by OP, various driver switches and LKAS/CC buttons
#define FORD_ACCDATA 0x186 // TX by OP, ACC controls
#define FORD_ACCDATA_3 0x18A // TX by OP, ACC/TJA user interface
#define FORD_Lane_Assist_Data1 0x3CA // TX by OP, Lane Keep Assist
#define FORD_LateralMotionControl 0x3D3 // TX by OP, Lateral Control message
#define FORD_LateralMotionControl2 0x3D6 // TX by OP, alternate Lateral Control message
#define FORD_IPMA_Data 0x3D8 // TX by OP, IPMA and LKAS user interface
// CAN bus numbers.
#define FORD_MAIN_BUS 0U
#define FORD_CAM_BUS 2U
const CanMsg FORD_STOCK_TX_MSGS[] = {
{FORD_Steering_Data_FD1, 0, 8},
{FORD_Steering_Data_FD1, 2, 8},
{FORD_ACCDATA_3, 0, 8},
{FORD_Lane_Assist_Data1, 0, 8},
{FORD_LateralMotionControl, 0, 8},
{FORD_IPMA_Data, 0, 8},
};
const CanMsg FORD_LONG_TX_MSGS[] = {
{FORD_Steering_Data_FD1, 0, 8},
{FORD_Steering_Data_FD1, 2, 8},
{FORD_ACCDATA, 0, 8},
{FORD_ACCDATA_3, 0, 8},
{FORD_Lane_Assist_Data1, 0, 8},
{FORD_LateralMotionControl, 0, 8},
{FORD_IPMA_Data, 0, 8},
};
const CanMsg FORD_CANFD_STOCK_TX_MSGS[] = {
{FORD_Steering_Data_FD1, 0, 8},
{FORD_Steering_Data_FD1, 2, 8},
{FORD_ACCDATA_3, 0, 8},
{FORD_Lane_Assist_Data1, 0, 8},
{FORD_LateralMotionControl2, 0, 8},
{FORD_IPMA_Data, 0, 8},
};
const CanMsg FORD_CANFD_LONG_TX_MSGS[] = {
{FORD_Steering_Data_FD1, 0, 8},
{FORD_Steering_Data_FD1, 2, 8},
{FORD_ACCDATA, 0, 8},
{FORD_ACCDATA_3, 0, 8},
{FORD_Lane_Assist_Data1, 0, 8},
{FORD_LateralMotionControl2, 0, 8},
{FORD_IPMA_Data, 0, 8},
};
// warning: quality flags are not yet checked in openpilot's CAN parser,
// this may be the cause of blocked messages
RxCheck ford_rx_checks[] = {
{.msg = {{FORD_BrakeSysFeatures, 0, 8, .check_checksum = true, .max_counter = 15U, .quality_flag=true, .frequency = 50U}, { 0 }, { 0 }}},
// FORD_EngVehicleSpThrottle2 has a counter that either randomly skips or by 2, likely ECU bug
// Some hybrid models also experience a bug where this checksum mismatches for one or two frames under heavy acceleration with ACC
// It has been confirmed that the Bronco Sport's camera only disallows ACC for bad quality flags, not counters or checksums, so we match that
{.msg = {{FORD_EngVehicleSpThrottle2, 0, 8, .check_checksum = false, .quality_flag=true, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{FORD_Yaw_Data_FD1, 0, 8, .check_checksum = true, .max_counter = 255U, .quality_flag=true, .frequency = 100U}, { 0 }, { 0 }}},
// These messages have no counter or checksum
{.msg = {{FORD_EngBrakeData, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},
{.msg = {{FORD_EngVehicleSpThrottle, 0, 8, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{FORD_DesiredTorqBrk, 0, 8, .frequency = 50U}, { 0 }, { 0 }}},
};
static uint8_t ford_get_counter(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
uint8_t cnt = 0;
if (addr == FORD_BrakeSysFeatures) {
// Signal: VehVActlBrk_No_Cnt
cnt = (GET_BYTE(to_push, 2) >> 2) & 0xFU;
} else if (addr == FORD_Yaw_Data_FD1) {
// Signal: VehRollYaw_No_Cnt
cnt = GET_BYTE(to_push, 5);
} else {
}
return cnt;
}
static uint32_t ford_get_checksum(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
uint8_t chksum = 0;
if (addr == FORD_BrakeSysFeatures) {
// Signal: VehVActlBrk_No_Cs
chksum = GET_BYTE(to_push, 3);
} else if (addr == FORD_Yaw_Data_FD1) {
// Signal: VehRollYawW_No_Cs
chksum = GET_BYTE(to_push, 4);
} else {
}
return chksum;
}
static uint32_t ford_compute_checksum(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
uint8_t chksum = 0;
if (addr == FORD_BrakeSysFeatures) {
chksum += GET_BYTE(to_push, 0) + GET_BYTE(to_push, 1); // Veh_V_ActlBrk
chksum += GET_BYTE(to_push, 2) >> 6; // VehVActlBrk_D_Qf
chksum += (GET_BYTE(to_push, 2) >> 2) & 0xFU; // VehVActlBrk_No_Cnt
chksum = 0xFFU - chksum;
} else if (addr == FORD_Yaw_Data_FD1) {
chksum += GET_BYTE(to_push, 0) + GET_BYTE(to_push, 1); // VehRol_W_Actl
chksum += GET_BYTE(to_push, 2) + GET_BYTE(to_push, 3); // VehYaw_W_Actl
chksum += GET_BYTE(to_push, 5); // VehRollYaw_No_Cnt
chksum += GET_BYTE(to_push, 6) >> 6; // VehRolWActl_D_Qf
chksum += (GET_BYTE(to_push, 6) >> 4) & 0x3U; // VehYawWActl_D_Qf
chksum = 0xFFU - chksum;
} else {
}
return chksum;
}
static bool ford_get_quality_flag_valid(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
bool valid = false;
if (addr == FORD_BrakeSysFeatures) {
valid = (GET_BYTE(to_push, 2) >> 6) == 0x3U; // VehVActlBrk_D_Qf
} else if (addr == FORD_EngVehicleSpThrottle2) {
valid = ((GET_BYTE(to_push, 4) >> 5) & 0x3U) == 0x3U; // VehVActlEng_D_Qf
} else if (addr == FORD_Yaw_Data_FD1) {
valid = ((GET_BYTE(to_push, 6) >> 4) & 0x3U) == 0x3U; // VehYawWActl_D_Qf
} else {
}
return valid;
}
const uint16_t FORD_PARAM_LONGITUDINAL = 1;
const uint16_t FORD_PARAM_CANFD = 2;
bool ford_longitudinal = false;
bool ford_canfd = false;
const LongitudinalLimits FORD_LONG_LIMITS = {
// acceleration cmd limits (used for brakes)
// Signal: AccBrkTot_A_Rq
.max_accel = 5641, // 1.9999 m/s^s
.min_accel = 4231, // -3.4991 m/s^2
.inactive_accel = 5128, // -0.0008 m/s^2
// gas cmd limits
// Signal: AccPrpl_A_Rq & AccPrpl_A_Pred
.max_gas = 700, // 2.0 m/s^2
.min_gas = 450, // -0.5 m/s^2
.inactive_gas = 0, // -5.0 m/s^2
};
#define FORD_INACTIVE_CURVATURE 1000U
#define FORD_INACTIVE_CURVATURE_RATE 4096U
#define FORD_INACTIVE_PATH_OFFSET 512U
#define FORD_INACTIVE_PATH_ANGLE 1000U
#define FORD_CANFD_INACTIVE_CURVATURE_RATE 1024U
#define FORD_MAX_SPEED_DELTA 2.0 // m/s
static bool ford_lkas_msg_check(int addr) {
return (addr == FORD_ACCDATA_3)
|| (addr == FORD_Lane_Assist_Data1)
|| (addr == FORD_LateralMotionControl)
|| (addr == FORD_LateralMotionControl2)
|| (addr == FORD_IPMA_Data);
}
// Curvature rate limits
const SteeringLimits FORD_STEERING_LIMITS = {
.max_steer = 1000,
.angle_deg_to_can = 50000, // 1 / (2e-5) rad to can
.max_angle_error = 100, // 0.002 * FORD_STEERING_LIMITS.angle_deg_to_can
.angle_rate_up_lookup = {
{5., 25., 25.},
{0.0002, 0.0001, 0.0001}
},
.angle_rate_down_lookup = {
{5., 25., 25.},
{0.000225, 0.00015, 0.00015}
},
// no blending at low speed due to lack of torque wind-up and inaccurate current curvature
.angle_error_min_speed = 10.0, // m/s
.enforce_angle_error = true,
.inactive_angle_is_zero = true,
};
static void ford_rx_hook(const CANPacket_t *to_push) {
if (GET_BUS(to_push) == FORD_MAIN_BUS) {
int addr = GET_ADDR(to_push);
// Update in motion state from standstill signal
if (addr == FORD_DesiredTorqBrk) {
// Signal: VehStop_D_Stat
vehicle_moving = ((GET_BYTE(to_push, 3) >> 3) & 0x3U) != 1U;
}
// Update vehicle speed
if (addr == FORD_BrakeSysFeatures) {
// Signal: Veh_V_ActlBrk
UPDATE_VEHICLE_SPEED(((GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1)) * 0.01 / 3.6);
}
// Check vehicle speed against a second source
if (addr == FORD_EngVehicleSpThrottle2) {
// Disable controls if speeds from ABS and PCM ECUs are too far apart.
// Signal: Veh_V_ActlEng
float filtered_pcm_speed = ((GET_BYTE(to_push, 6) << 8) | GET_BYTE(to_push, 7)) * 0.01 / 3.6;
bool is_invalid_speed = ABS(filtered_pcm_speed - ((float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR)) > FORD_MAX_SPEED_DELTA;
if (is_invalid_speed) {
controls_allowed = false;
}
}
// Update vehicle yaw rate
if (addr == FORD_Yaw_Data_FD1) {
// Signal: VehYaw_W_Actl
float ford_yaw_rate = (((GET_BYTE(to_push, 2) << 8U) | GET_BYTE(to_push, 3)) * 0.0002) - 6.5;
float current_curvature = ford_yaw_rate / MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1);
// convert current curvature into units on CAN for comparison with desired curvature
update_sample(&angle_meas, ROUND(current_curvature * FORD_STEERING_LIMITS.angle_deg_to_can));
}
// Update gas pedal
if (addr == FORD_EngVehicleSpThrottle) {
// Pedal position: (0.1 * val) in percent
// Signal: ApedPos_Pc_ActlArb
gas_pressed = (((GET_BYTE(to_push, 0) & 0x03U) << 8) | GET_BYTE(to_push, 1)) > 0U;
}
// Update brake pedal and cruise state
if (addr == FORD_EngBrakeData) {
// Signal: BpedDrvAppl_D_Actl
brake_pressed = ((GET_BYTE(to_push, 0) >> 4) & 0x3U) == 2U;
// Signal: CcStat_D_Actl
unsigned int cruise_state = GET_BYTE(to_push, 1) & 0x07U;
acc_main_on = (cruise_state == 3U) ||(cruise_state == 4U) || (cruise_state == 5U);
bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U);
pcm_cruise_check(cruise_engaged);
}
// If steering controls messages are received on the destination bus, it's an indication
// that the relay might be malfunctioning.
bool stock_ecu_detected = ford_lkas_msg_check(addr);
if (ford_longitudinal) {
stock_ecu_detected = stock_ecu_detected || (addr == FORD_ACCDATA);
}
generic_rx_checks(stock_ecu_detected);
}
}
static bool ford_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);
// Safety check for ACCDATA accel and brake requests
if (addr == FORD_ACCDATA) {
// Signal: AccPrpl_A_Rq
int gas = ((GET_BYTE(to_send, 6) & 0x3U) << 8) | GET_BYTE(to_send, 7);
// Signal: AccPrpl_A_Pred
int gas_pred = ((GET_BYTE(to_send, 2) & 0x3U) << 8) | GET_BYTE(to_send, 3);
// Signal: AccBrkTot_A_Rq
int accel = ((GET_BYTE(to_send, 0) & 0x1FU) << 8) | GET_BYTE(to_send, 1);
// Signal: CmbbDeny_B_Actl
bool cmbb_deny = GET_BIT(to_send, 37U);
bool violation = false;
violation |= longitudinal_accel_checks(accel, FORD_LONG_LIMITS);
violation |= longitudinal_gas_checks(gas, FORD_LONG_LIMITS);
violation |= longitudinal_gas_checks(gas_pred, FORD_LONG_LIMITS);
// Safety check for stock AEB
violation |= cmbb_deny; // do not prevent stock AEB actuation
if (violation) {
tx = false;
}
}
// Safety check for Steering_Data_FD1 button signals
// Note: Many other signals in this message are not relevant to safety (e.g. blinkers, wiper switches, high beam)
// which we passthru in OP.
if (addr == FORD_Steering_Data_FD1) {
// Violation if resume button is pressed while controls not allowed, or
// if cancel button is pressed when cruise isn't engaged.
bool violation = false;
violation |= GET_BIT(to_send, 8U) && !cruise_engaged_prev; // Signal: CcAslButtnCnclPress (cancel)
violation |= GET_BIT(to_send, 25U) && !controls_allowed; // Signal: CcAsllButtnResPress (resume)
if (violation) {
tx = false;
}
}
// Safety check for Lane_Assist_Data1 action
if (addr == FORD_Lane_Assist_Data1) {
// Do not allow steering using Lane_Assist_Data1 (Lane-Departure Aid).
// This message must be sent for Lane Centering to work, and can include
// values such as the steering angle or lane curvature for debugging,
// but the action (LkaActvStats_D2_Req) must be set to zero.
unsigned int action = GET_BYTE(to_send, 0) >> 5;
if (action != 0U) {
tx = false;
}
}
// Safety check for LateralMotionControl action
if (addr == FORD_LateralMotionControl) {
// Signal: LatCtl_D_Rq
bool steer_control_enabled = ((GET_BYTE(to_send, 4) >> 2) & 0x7U) != 0U;
unsigned int raw_curvature = (GET_BYTE(to_send, 0) << 3) | (GET_BYTE(to_send, 1) >> 5);
unsigned int raw_curvature_rate = ((GET_BYTE(to_send, 1) & 0x1FU) << 8) | GET_BYTE(to_send, 2);
unsigned int raw_path_angle = (GET_BYTE(to_send, 3) << 3) | (GET_BYTE(to_send, 4) >> 5);
unsigned int raw_path_offset = (GET_BYTE(to_send, 5) << 2) | (GET_BYTE(to_send, 6) >> 6);
// These signals are not yet tested with the current safety limits
bool violation = (raw_curvature_rate != FORD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET);
// Check angle error and steer_control_enabled
int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature
violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS);
if (violation) {
tx = false;
}
}
// Safety check for LateralMotionControl2 action
if (addr == FORD_LateralMotionControl2) {
// Signal: LatCtl_D2_Rq
bool steer_control_enabled = ((GET_BYTE(to_send, 0) >> 4) & 0x7U) != 0U;
unsigned int raw_curvature = (GET_BYTE(to_send, 2) << 3) | (GET_BYTE(to_send, 3) >> 5);
unsigned int raw_curvature_rate = (GET_BYTE(to_send, 6) << 3) | (GET_BYTE(to_send, 7) >> 5);
unsigned int raw_path_angle = ((GET_BYTE(to_send, 3) & 0x1FU) << 6) | (GET_BYTE(to_send, 4) >> 2);
unsigned int raw_path_offset = ((GET_BYTE(to_send, 4) & 0x3U) << 8) | GET_BYTE(to_send, 5);
// These signals are not yet tested with the current safety limits
bool violation = (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET);
// Check angle error and steer_control_enabled
int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature
violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS);
if (violation) {
tx = false;
}
}
return tx;
}
static int ford_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
switch (bus_num) {
case FORD_MAIN_BUS: {
// Forward all traffic from bus 0 onward
bus_fwd = FORD_CAM_BUS;
break;
}
case FORD_CAM_BUS: {
if (ford_lkas_msg_check(addr)) {
// Block stock LKAS and UI messages
bus_fwd = -1;
} else if (ford_longitudinal && (addr == FORD_ACCDATA)) {
// Block stock ACC message
bus_fwd = -1;
} else {
// Forward remaining traffic
bus_fwd = FORD_MAIN_BUS;
}
break;
}
default: {
// No other buses should be in use; fallback to do-not-forward
bus_fwd = -1;
break;
}
}
return bus_fwd;
}
static safety_config ford_init(uint16_t param) {
UNUSED(param);
#ifdef ALLOW_DEBUG
ford_longitudinal = GET_FLAG(param, FORD_PARAM_LONGITUDINAL);
ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD);
#endif
safety_config ret;
if (ford_canfd) {
ret = ford_longitudinal ? BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_STOCK_TX_MSGS);
} else {
ret = ford_longitudinal ? BUILD_SAFETY_CFG(ford_rx_checks, FORD_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(ford_rx_checks, FORD_STOCK_TX_MSGS);
}
return ret;
}
const safety_hooks ford_hooks = {
.init = ford_init,
.rx = ford_rx_hook,
.tx = ford_tx_hook,
.fwd = ford_fwd_hook,
.get_counter = ford_get_counter,
.get_checksum = ford_get_checksum,
.compute_checksum = ford_compute_checksum,
.get_quality_flag_valid = ford_get_quality_flag_valid,
};

353
panda/board/safety/safety_gm.h Normal file
View File

@@ -0,0 +1,353 @@
const SteeringLimits GM_STEERING_LIMITS = {
.max_steer = 300,
.max_rate_up = 10,
.max_rate_down = 15,
.driver_torque_allowance = 65,
.driver_torque_factor = 4,
.max_rt_delta = 128,
.max_rt_interval = 250000,
.type = TorqueDriverLimited,
};
const LongitudinalLimits GM_ASCM_LONG_LIMITS = {
.max_gas = 3072,
.min_gas = 1404,
.inactive_gas = 1404,
.max_brake = 400,
};
const LongitudinalLimits GM_ASCM_LONG_LIMITS_SPORT = {
.max_gas = 8191,
.min_gas = 5500,
.inactive_gas = 5500,
.max_brake = 400,
};
const LongitudinalLimits GM_CAM_LONG_LIMITS = {
.max_gas = 3400,
.min_gas = 1514,
.inactive_gas = 1554,
.max_brake = 400,
};
const LongitudinalLimits GM_CAM_LONG_LIMITS_SPORT = {
.max_gas = 8848,
.min_gas = 5610,
.inactive_gas = 5650,
.max_brake = 400,
};
const LongitudinalLimits *gm_long_limits;
const int GM_STANDSTILL_THRSLD = 10; // 0.311kph
// panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches
// If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state
const int GM_GAS_INTERCEPTOR_THRESHOLD = 515; // (610 + 306.25) / 2 ratio between offset and gain from dbc file
#define GM_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks
const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {0x2CB, 0, 8}, {0x370, 0, 6}, {0x200, 0, 6}, // pt bus
{0xA1, 1, 7}, {0x306, 1, 8}, {0x308, 1, 7}, {0x310, 1, 2}, // obs bus
{0x315, 2, 5}, // ch bus
{0x104c006c, 3, 3}, {0x10400060, 3, 5}}; // gmlan
const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, {0x200, 0, 6}, {0x1E1, 0, 7}, // pt bus
{0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus
const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8}, {0x370, 0, 6}, {0x200, 0, 6}, // pt bus
{0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus
const CanMsg GM_SDGM_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, // pt bus
{0x184, 2, 8}}; // camera bus
const CanMsg GM_CC_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, // pt bus
{0x184, 2, 8}, {0x1E1, 2, 7}}; // camera bus
// TODO: do checksum and counter checks. Add correct timestep, 0.1s for now.
RxCheck gm_rx_checks[] = {
{.msg = {{0x184, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},
{.msg = {{0x34A, 0, 5, .frequency = 10U}, { 0 }, { 0 }}},
{.msg = {{0x1E1, 0, 7, .frequency = 10U}, // Non-SDGM Car
{0x1E1, 2, 7, .frequency = 100000U}}}, // SDGM Car
{.msg = {{0xF1, 0, 6, .frequency = 10U}, // Non-SDGM Car
{0xF1, 2, 6, .frequency = 100000U}}}, // SDGM Car
{.msg = {{0x1C4, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},
{.msg = {{0xC9, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},
};
const uint16_t GM_PARAM_HW_CAM = 1;
const uint16_t GM_PARAM_HW_CAM_LONG = 2;
const uint16_t GM_PARAM_HW_SDGM = 4;
const uint16_t GM_PARAM_CC_LONG = 8;
const uint16_t GM_PARAM_HW_ASCM_LONG = 16;
const uint16_t GM_PARAM_NO_CAMERA = 32;
const uint16_t GM_PARAM_NO_ACC = 64;
const uint16_t GM_PARAM_PEDAL_LONG = 128; // TODO: this can be inferred
const uint16_t GM_PARAM_PEDAL_INTERCEPTOR = 256;
enum {
GM_BTN_UNPRESS = 1,
GM_BTN_RESUME = 2,
GM_BTN_SET = 3,
GM_BTN_CANCEL = 6,
};
enum {GM_ASCM, GM_CAM, GM_SDGM} gm_hw = GM_ASCM;
bool gm_cam_long = false;
bool gm_pcm_cruise = false;
bool gm_cc_long = false;
bool gm_has_acc = true;
bool gm_pedal_long = false;
bool gm_skip_relay_check = false;
bool gm_force_ascm = false;
static void handle_gm_wheel_buttons(const CANPacket_t *to_push) {
int button = (GET_BYTE(to_push, 5) & 0x70U) >> 4;
// enter controls on falling edge of set or rising edge of resume (avoids fault)
bool set = (button != GM_BTN_SET) && (cruise_button_prev == GM_BTN_SET);
bool res = (button == GM_BTN_RESUME) && (cruise_button_prev != GM_BTN_RESUME);
if (set || res) {
controls_allowed = true;
}
// exit controls on cancel press
if (button == GM_BTN_CANCEL) {
controls_allowed = false;
}
cruise_button_prev = button;
}
static void gm_rx_hook(const CANPacket_t *to_push) {
if ((GET_BUS(to_push) == 2U) && (GET_ADDR(to_push) == 0x1E1) && (gm_hw == GM_SDGM)) {
// SDGM buttons are on bus 2
handle_gm_wheel_buttons(to_push);
}
if (GET_BUS(to_push) == 0U) {
int addr = GET_ADDR(to_push);
if (addr == 0x184) {
int torque_driver_new = ((GET_BYTE(to_push, 6) & 0x7U) << 8) | GET_BYTE(to_push, 7);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&torque_driver, torque_driver_new);
}
// sample rear wheel speeds
if (addr == 0x34A) {
int left_rear_speed = (GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1);
int right_rear_speed = (GET_BYTE(to_push, 2) << 8) | GET_BYTE(to_push, 3);
vehicle_moving = (left_rear_speed > GM_STANDSTILL_THRSLD) || (right_rear_speed > GM_STANDSTILL_THRSLD);
}
// ACC steering wheel buttons (GM_CAM and GM_SDGM are tied to the PCM)
if ((addr == 0x1E1) && (!gm_pcm_cruise || gm_cc_long) && (gm_hw != GM_SDGM)) {
handle_gm_wheel_buttons(to_push);
}
// Reference for brake pressed signals:
// https://github.com/commaai/openpilot/blob/master/selfdrive/car/gm/carstate.py
if ((addr == 0xBE) && (gm_hw == GM_ASCM)) {
brake_pressed = GET_BYTE(to_push, 1) >= 8U;
}
if ((addr == 0xC9) && ((gm_hw == GM_CAM) || (gm_hw == GM_SDGM))) {
brake_pressed = GET_BIT(to_push, 40U) != 0U;
}
if (addr == 0xC9) {
acc_main_on = GET_BIT(to_push, 29U) != 0U;
}
if (addr == 0x1C4) {
if (!enable_gas_interceptor) {
gas_pressed = GET_BYTE(to_push, 5) != 0U;
}
// enter controls on rising edge of ACC, exit controls when ACC off
if (gm_pcm_cruise && gm_has_acc) {
bool cruise_engaged = (GET_BYTE(to_push, 1) >> 5) != 0U;
pcm_cruise_check(cruise_engaged);
}
}
// Cruise check for CC only cars
if ((addr == 0x3D1) && !gm_has_acc) {
bool cruise_engaged = (GET_BYTE(to_push, 4) >> 7) != 0U;
if (gm_cc_long) {
pcm_cruise_check(cruise_engaged);
} else {
cruise_engaged_prev = cruise_engaged;
}
}
if (addr == 0xBD) {
regen_braking = (GET_BYTE(to_push, 0) >> 4) != 0U;
}
// Pedal Interceptor
if ((addr == 0x201) && enable_gas_interceptor) {
int gas_interceptor = GM_GET_INTERCEPTOR(to_push);
gas_pressed = gas_interceptor > GM_GAS_INTERCEPTOR_THRESHOLD;
gas_interceptor_prev = gas_interceptor;
}
bool stock_ecu_detected = (addr == 0x180); // ASCMLKASteeringCmd
// Check ASCMGasRegenCmd only if we're blocking it
if (!gm_pcm_cruise && !gm_pedal_long && (addr == 0x2CB)) {
stock_ecu_detected = true;
}
generic_rx_checks(stock_ecu_detected);
}
}
static bool gm_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);
// BRAKE: safety check
if (addr == 0x315) {
int brake = ((GET_BYTE(to_send, 0) & 0xFU) << 8) + GET_BYTE(to_send, 1);
brake = (0x1000 - brake) & 0xFFF;
if (longitudinal_brake_checks(brake, *gm_long_limits)) {
tx = false;
}
}
// LKA STEER: safety check
if (addr == 0x180) {
int desired_torque = ((GET_BYTE(to_send, 0) & 0x7U) << 8) + GET_BYTE(to_send, 1);
desired_torque = to_signed(desired_torque, 11);
bool steer_req = GET_BIT(to_send, 3U);
if (steer_torque_cmd_checks(desired_torque, steer_req, GM_STEERING_LIMITS)) {
tx = false;
}
}
// GAS: safety check (interceptor)
if (addr == 0x200) {
if (longitudinal_interceptor_checks(to_send)) {
tx = 0;
}
}
// GAS/REGEN: safety check
if (addr == 0x2CB) {
bool apply = GET_BIT(to_send, 0U);
int gas_regen = ((GET_BYTE(to_send, 1) & 0x1U) << 13) + ((GET_BYTE(to_send, 2) & 0xFFU) << 5) + ((GET_BYTE(to_send, 3) & 0xF8U) >> 3);
bool violation = false;
// Allow apply bit in pre-enabled and overriding states
violation |= !controls_allowed && apply;
violation |= longitudinal_gas_checks(gas_regen, *gm_long_limits);
if (violation) {
tx = false;
}
}
// BUTTONS: used for resume spamming and cruise cancellation with stock longitudinal
if ((addr == 0x1E1) && (gm_pcm_cruise || gm_pedal_long || gm_cc_long)) {
int button = (GET_BYTE(to_send, 5) >> 4) & 0x7U;
bool allowed_btn = (button == GM_BTN_CANCEL) && cruise_engaged_prev;
// For standard CC, allow spamming of SET / RESUME
if (gm_cc_long) {
allowed_btn |= cruise_engaged_prev && (button == GM_BTN_SET || button == GM_BTN_RESUME || button == GM_BTN_UNPRESS);
}
if (!allowed_btn) {
tx = false;
}
}
return tx;
}
static int gm_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
if ((gm_hw == GM_CAM) || (gm_hw == GM_SDGM)) {
if (bus_num == 0) {
// block PSCMStatus; forwarded through openpilot to hide an alert from the camera
bool is_pscm_msg = (addr == 0x184);
if (!is_pscm_msg) {
bus_fwd = 2;
}
}
if (bus_num == 2) {
// block lkas message and acc messages if gm_cam_long, forward all others
bool is_lkas_msg = (addr == 0x180);
bool is_acc_msg = (addr == 0x315) || (addr == 0x2CB) || (addr == 0x370);
bool block_msg = is_lkas_msg || (is_acc_msg && gm_cam_long);
if (!block_msg) {
bus_fwd = 0;
}
}
}
return bus_fwd;
}
static safety_config gm_init(uint16_t param) {
sport_mode = alternative_experience & ALT_EXP_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX;
if GET_FLAG(param, GM_PARAM_HW_CAM) {
gm_hw = GM_CAM;
} else if GET_FLAG(param, GM_PARAM_HW_SDGM) {
gm_hw = GM_SDGM;
} else {
gm_hw = GM_ASCM;
}
gm_force_ascm = GET_FLAG(param, GM_PARAM_HW_ASCM_LONG);
if (gm_hw == GM_ASCM || gm_force_ascm) {
if (sport_mode) {
gm_long_limits = &GM_ASCM_LONG_LIMITS_SPORT;
} else {
gm_long_limits = &GM_ASCM_LONG_LIMITS;
}
} else if ((gm_hw == GM_CAM) || (gm_hw == GM_SDGM)) {
if (sport_mode) {
gm_long_limits = &GM_CAM_LONG_LIMITS_SPORT;
} else {
gm_long_limits = &GM_CAM_LONG_LIMITS;
}
} else {
}
gm_pedal_long = GET_FLAG(param, GM_PARAM_PEDAL_LONG);
gm_cc_long = GET_FLAG(param, GM_PARAM_CC_LONG);
gm_cam_long = GET_FLAG(param, GM_PARAM_HW_CAM_LONG) && !gm_cc_long;
gm_pcm_cruise = ((gm_hw == GM_CAM) && (!gm_cam_long || gm_cc_long) && !gm_force_ascm && !gm_pedal_long) || (gm_hw == GM_SDGM);
gm_skip_relay_check = GET_FLAG(param, GM_PARAM_NO_CAMERA);
gm_has_acc = !GET_FLAG(param, GM_PARAM_NO_ACC);
enable_gas_interceptor = GET_FLAG(param, GM_PARAM_PEDAL_INTERCEPTOR);
safety_config ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_ASCM_TX_MSGS);
if (gm_hw == GM_CAM) {
if (gm_cc_long) {
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CC_LONG_TX_MSGS);
} else if (gm_cam_long) {
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_LONG_TX_MSGS);
} else {
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_TX_MSGS);
}
} else if (gm_hw == GM_SDGM) {
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_SDGM_TX_MSGS);
}
return ret;
}
const safety_hooks gm_hooks = {
.init = gm_init,
.rx = gm_rx_hook,
.tx = gm_tx_hook,
.fwd = gm_fwd_hook,
};

526
panda/board/safety/safety_honda.h Normal file
View File

@@ -0,0 +1,526 @@
const CanMsg HONDA_N_TX_MSGS[] = {{0xE4, 0, 5}, {0x194, 0, 4}, {0x1FA, 0, 8}, {0x30C, 0, 8}, {0x33D, 0, 5}};
const CanMsg HONDA_N_INTERCEPTOR_TX_MSGS[] = {{0xE4, 0, 5}, {0x194, 0, 4}, {0x1FA, 0, 8}, {0x200, 0, 6}, {0x30C, 0, 8}, {0x33D, 0, 5}};
const CanMsg HONDA_BOSCH_TX_MSGS[] = {{0xE4, 0, 5}, {0xE5, 0, 8}, {0x296, 1, 4}, {0x33D, 0, 5}, {0x33DA, 0, 5}, {0x33DB, 0, 8}}; // Bosch
const CanMsg HONDA_BOSCH_LONG_TX_MSGS[] = {{0xE4, 1, 5}, {0x1DF, 1, 8}, {0x1EF, 1, 8}, {0x1FA, 1, 8}, {0x30C, 1, 8}, {0x33D, 1, 5}, {0x33DA, 1, 5}, {0x33DB, 1, 8}, {0x39F, 1, 8}, {0x18DAB0F1, 1, 8}}; // Bosch w/ gas and brakes
const CanMsg HONDA_RADARLESS_TX_MSGS[] = {{0xE4, 0, 5}, {0x296, 2, 4}, {0x33D, 0, 8}}; // Bosch radarless
const CanMsg HONDA_RADARLESS_LONG_TX_MSGS[] = {{0xE4, 0, 5}, {0x33D, 0, 8}, {0x1C8, 0, 8}, {0x30C, 0, 8}}; // Bosch radarless w/ gas and brakes
// panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches
// If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state
// Threshold calculated from DBC gains: round(((83.3 / 0.253984064) + (83.3 / 0.126992032)) / 2) = 492
const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 492;
#define HONDA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks
const LongitudinalLimits HONDA_BOSCH_LONG_LIMITS = {
.max_accel = 200, // accel is used for brakes
.min_accel = -350,
.max_gas = 2000,
.inactive_gas = -30000,
};
const LongitudinalLimits HONDA_BOSCH_LONG_LIMITS_SPORT = {
.max_accel = 400, // accel is used for brakes
.min_accel = -350,
.max_gas = 2000,
.inactive_gas = -30000,
};
const LongitudinalLimits HONDA_NIDEC_LONG_LIMITS = {
.max_gas = 198, // 0xc6
.max_brake = 255,
.inactive_speed = 0,
};
// All common address checks except SCM_BUTTONS which isn't on one Nidec safety configuration
#define HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(pt_bus) \
{.msg = {{0x1A6, (pt_bus), 8, .check_checksum = true, .max_counter = 3U, .frequency = 25U}, /* SCM_BUTTONS */ \
{0x296, (pt_bus), 4, .check_checksum = true, .max_counter = 3U, .frequency = 25U}, { 0 }}}, \
{.msg = {{0x158, (pt_bus), 8, .check_checksum = true, .max_counter = 3U, .frequency = 100U}, { 0 }, { 0 }}}, /* ENGINE_DATA */ \
{.msg = {{0x17C, (pt_bus), 8, .check_checksum = true, .max_counter = 3U, .frequency = 100U}, { 0 }, { 0 }}}, /* POWERTRAIN_DATA */ \
#define HONDA_COMMON_RX_CHECKS(pt_bus) \
HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(pt_bus) \
{.msg = {{0x326, (pt_bus), 8, .check_checksum = true, .max_counter = 3U, .frequency = 10U}, { 0 }, { 0 }}}, /* SCM_FEEDBACK */ \
// Alternate brake message is used on some Honda Bosch, and Honda Bosch radarless (where PT bus is 0)
#define HONDA_ALT_BRAKE_ADDR_CHECK(pt_bus) \
{.msg = {{0x1BE, (pt_bus), 3, .check_checksum = true, .max_counter = 3U, .frequency = 50U}, { 0 }, { 0 }}}, /* BRAKE_MODULE */ \
// Nidec and bosch radarless has the powertrain bus on bus 0
RxCheck honda_common_rx_checks[] = {
HONDA_COMMON_RX_CHECKS(0)
};
RxCheck honda_common_interceptor_rx_checks[] = {
HONDA_COMMON_RX_CHECKS(0)
{.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
};
RxCheck honda_common_alt_brake_rx_checks[] = {
HONDA_COMMON_RX_CHECKS(0)
HONDA_ALT_BRAKE_ADDR_CHECK(0)
};
// For Nidecs with main on signal on an alternate msg (missing 0x326)
RxCheck honda_nidec_alt_rx_checks[] = {
HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(0)
};
RxCheck honda_nidec_alt_interceptor_rx_checks[] = {
HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(0)
{.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
};
// Bosch has pt on bus 1, verified 0x1A6 does not exist
RxCheck honda_bosch_rx_checks[] = {
HONDA_COMMON_RX_CHECKS(1)
};
RxCheck honda_bosch_alt_brake_rx_checks[] = {
HONDA_COMMON_RX_CHECKS(1)
HONDA_ALT_BRAKE_ADDR_CHECK(1)
};
const uint16_t HONDA_PARAM_ALT_BRAKE = 1;
const uint16_t HONDA_PARAM_BOSCH_LONG = 2;
const uint16_t HONDA_PARAM_NIDEC_ALT = 4;
const uint16_t HONDA_PARAM_RADARLESS = 8;
const uint16_t HONDA_PARAM_GAS_INTERCEPTOR = 16;
const uint16_t HONDA_PARAM_CLARITY = 32;
enum {
HONDA_BTN_NONE = 0,
HONDA_BTN_MAIN = 1,
HONDA_BTN_CANCEL = 2,
HONDA_BTN_SET = 3,
HONDA_BTN_RESUME = 4,
};
int honda_brake = 0;
bool honda_brake_switch_prev = false;
bool honda_alt_brake_msg = false;
bool honda_fwd_brake = false;
bool honda_bosch_long = false;
bool honda_bosch_radarless = false;
bool honda_clarity_brake_msg = false;
enum {HONDA_NIDEC, HONDA_BOSCH} honda_hw = HONDA_NIDEC;
int honda_get_pt_bus(void) {
return ((honda_hw == HONDA_BOSCH) && !honda_bosch_radarless) ? 1 : 0;
}
static uint32_t honda_get_checksum(const CANPacket_t *to_push) {
int checksum_byte = GET_LEN(to_push) - 1U;
return (uint8_t)(GET_BYTE(to_push, checksum_byte)) & 0xFU;
}
static uint32_t honda_compute_checksum(const CANPacket_t *to_push) {
int len = GET_LEN(to_push);
uint8_t checksum = 0U;
unsigned int addr = GET_ADDR(to_push);
while (addr > 0U) {
checksum += (uint8_t)(addr & 0xFU); addr >>= 4;
}
for (int j = 0; j < len; j++) {
uint8_t byte = GET_BYTE(to_push, j);
checksum += (uint8_t)(byte & 0xFU) + (byte >> 4U);
if (j == (len - 1)) {
checksum -= (byte & 0xFU); // remove checksum in message
}
}
return (uint8_t)((8U - checksum) & 0xFU);
}
static uint8_t honda_get_counter(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
uint8_t cnt = 0U;
if (addr == 0x201) {
// Signal: COUNTER_PEDAL
cnt = GET_BYTE(to_push, 4) & 0x0FU;
} else {
int counter_byte = GET_LEN(to_push) - 1U;
cnt = (GET_BYTE(to_push, counter_byte) >> 4U) & 0x3U;
}
return cnt;
}
static void honda_rx_hook(const CANPacket_t *to_push) {
const bool pcm_cruise = ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) || \
((honda_hw == HONDA_NIDEC) && !enable_gas_interceptor);
int pt_bus = honda_get_pt_bus();
int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);
int bus = GET_BUS(to_push);
// sample speed
if (addr == 0x158) {
// first 2 bytes
vehicle_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
}
// check ACC main state
// 0x326 for all Bosch and some Nidec, 0x1A6 for some Nidec
if ((addr == 0x326) || (addr == 0x1A6)) {
acc_main_on = GET_BIT(to_push, ((addr == 0x326) ? 28U : 47U));
if (!acc_main_on) {
controls_allowed = false;
}
}
// enter controls when PCM enters cruise state
if (pcm_cruise && (addr == 0x17C)) {
const bool cruise_engaged = GET_BIT(to_push, 38U);
// engage on rising edge
if (cruise_engaged && !cruise_engaged_prev) {
controls_allowed = true;
}
// Since some Nidec cars can brake down to 0 after the PCM disengages,
// we don't disengage when the PCM does.
if (!cruise_engaged && (honda_hw != HONDA_NIDEC)) {
controls_allowed = false;
}
cruise_engaged_prev = cruise_engaged;
}
// state machine to enter and exit controls for button enabling
// 0x1A6 for the ILX, 0x296 for the Civic Touring
if (((addr == 0x1A6) || (addr == 0x296)) && (bus == pt_bus)) {
int button = (GET_BYTE(to_push, 0) & 0xE0U) >> 5;
// enter controls on the falling edge of set or resume
bool set = (button != HONDA_BTN_SET) && (cruise_button_prev == HONDA_BTN_SET);
bool res = (button != HONDA_BTN_RESUME) && (cruise_button_prev == HONDA_BTN_RESUME);
if (acc_main_on && !pcm_cruise && (set || res)) {
controls_allowed = true;
}
// exit controls once main or cancel are pressed
if ((button == HONDA_BTN_MAIN) || (button == HONDA_BTN_CANCEL)) {
controls_allowed = false;
}
cruise_button_prev = button;
}
// user brake signal on 0x17C reports applied brake from computer brake on accord
// and crv, which prevents the usual brake safety from working correctly. these
// cars have a signal on 0x1BE which only detects user's brake being applied so
// in these cases, this is used instead.
// most hondas: 0x17C
// accord, crv: 0x1BE
if (honda_alt_brake_msg) {
if (addr == 0x1BE) {
brake_pressed = GET_BIT(to_push, 4U);
}
} else {
if (addr == 0x17C) {
// also if brake switch is 1 for two CAN frames, as brake pressed is delayed
const bool brake_switch = GET_BIT(to_push, 32U);
brake_pressed = (GET_BIT(to_push, 53U)) || (brake_switch && honda_brake_switch_prev);
honda_brake_switch_prev = brake_switch;
}
}
// length check because bosch hardware also uses this id (0x201 w/ len = 8)
if ((addr == 0x201) && (len == 6) && enable_gas_interceptor) {
int gas_interceptor = HONDA_GET_INTERCEPTOR(to_push);
gas_pressed = gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD;
gas_interceptor_prev = gas_interceptor;
}
if (!enable_gas_interceptor) {
if (addr == 0x17C) {
gas_pressed = GET_BYTE(to_push, 0) != 0U;
}
}
// disable stock Honda AEB in alternative experience
if (!(alternative_experience & ALT_EXP_DISABLE_STOCK_AEB)) {
if ((bus == 2) && (addr == 0x1FA)) {
bool honda_stock_aeb = GET_BIT(to_push, 29U);
int honda_stock_brake = (GET_BYTE(to_push, 0) << 2) | (GET_BYTE(to_push, 1) >> 6);
if (honda_clarity_brake_msg) {
honda_stock_brake = (GET_BYTE(to_push, 6) << 2) + ((GET_BYTE(to_push, 7) >> 6) & 0x3U);
}
// Forward AEB when stock braking is higher than openpilot braking
// only stop forwarding when AEB event is over
if (!honda_stock_aeb) {
honda_fwd_brake = false;
} else if (honda_stock_brake >= honda_brake) {
honda_fwd_brake = true;
} else {
// Leave Honda forward brake as is
}
}
}
int bus_rdr_car = (honda_hw == HONDA_BOSCH) ? 0 : 2; // radar bus, car side
bool stock_ecu_detected = false;
// If steering controls messages are received on the destination bus, it's an indication
// that the relay might be malfunctioning
if ((addr == 0xE4) || (addr == 0x194)) {
if (((honda_hw != HONDA_NIDEC) && (bus == bus_rdr_car)) || ((honda_hw == HONDA_NIDEC) && (bus == 0))) {
stock_ecu_detected = true;
}
}
// If Honda Bosch longitudinal mode is selected we need to ensure the radar is turned off
// Verify this by ensuring ACC_CONTROL (0x1DF) is not received on the PT bus
if (honda_bosch_long && !honda_bosch_radarless && (bus == pt_bus) && (addr == 0x1DF)) {
stock_ecu_detected = true;
}
generic_rx_checks(stock_ecu_detected);
}
static bool honda_tx_hook(const CANPacket_t *to_send) {
sport_mode = alternative_experience & ALT_EXP_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX;
bool tx = true;
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
int bus_pt = honda_get_pt_bus();
int bus_buttons = (honda_bosch_radarless) ? 2 : bus_pt; // the camera controls ACC on radarless Bosch cars
// ACC_HUD: safety check (nidec w/o pedal)
if ((addr == 0x30C) && (bus == bus_pt)) {
int pcm_speed = (GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1);
int pcm_gas = GET_BYTE(to_send, 2);
bool violation = false;
violation |= longitudinal_speed_checks(pcm_speed, HONDA_NIDEC_LONG_LIMITS);
violation |= longitudinal_gas_checks(pcm_gas, HONDA_NIDEC_LONG_LIMITS);
if (violation) {
tx = false;
}
}
// BRAKE: safety check (nidec)
if ((addr == 0x1FA) && (bus == bus_pt)) {
honda_brake = (GET_BYTE(to_send, 0) << 2) + ((GET_BYTE(to_send, 1) >> 6) & 0x3U);
if (honda_clarity_brake_msg) {
honda_brake = (GET_BYTE(to_send, 6) << 2) + ((GET_BYTE(to_send, 7) >> 6) & 0x3U);
}
if (longitudinal_brake_checks(honda_brake, HONDA_NIDEC_LONG_LIMITS)) {
tx = false;
}
if (honda_fwd_brake) {
tx = false;
}
}
// BRAKE/GAS: safety check (bosch)
if ((addr == 0x1DF) && (bus == bus_pt)) {
int accel = (GET_BYTE(to_send, 3) << 3) | ((GET_BYTE(to_send, 4) >> 5) & 0x7U);
accel = to_signed(accel, 11);
int gas = (GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1);
gas = to_signed(gas, 16);
bool violation = false;
if (sport_mode) {
violation |= longitudinal_accel_checks(accel, HONDA_BOSCH_LONG_LIMITS_SPORT);
violation |= longitudinal_gas_checks(gas, HONDA_BOSCH_LONG_LIMITS_SPORT);
} else {
violation |= longitudinal_accel_checks(accel, HONDA_BOSCH_LONG_LIMITS);
violation |= longitudinal_gas_checks(gas, HONDA_BOSCH_LONG_LIMITS);
}
if (violation) {
tx = false;
}
}
// ACCEL: safety check (radarless)
if ((addr == 0x1C8) && (bus == bus_pt)) {
int accel = (GET_BYTE(to_send, 0) << 4) | (GET_BYTE(to_send, 1) >> 4);
accel = to_signed(accel, 12);
bool violation = false;
if (sport_mode) {
violation |= longitudinal_accel_checks(accel, HONDA_BOSCH_LONG_LIMITS_SPORT);
} else {
violation |= longitudinal_accel_checks(accel, HONDA_BOSCH_LONG_LIMITS);
}
if (violation) {
tx = false;
}
}
// STEER: safety check
if ((addr == 0xE4) || (addr == 0x194)) {
bool aol_allowed = acc_main_on && (alternative_experience & ALT_EXP_DISABLE_DISENGAGE_ON_GAS);
if (!(controls_allowed || aol_allowed)) {
bool steer_applied = GET_BYTE(to_send, 0) | GET_BYTE(to_send, 1);
if (steer_applied) {
tx = false;
}
}
}
// Bosch supplemental control check
if (addr == 0xE5) {
if ((GET_BYTES(to_send, 0, 4) != 0x10800004U) || ((GET_BYTES(to_send, 4, 4) & 0x00FFFFFFU) != 0x0U)) {
tx = false;
}
}
// GAS: safety check (interceptor)
if (addr == 0x200) {
if (longitudinal_interceptor_checks(to_send)) {
tx = false;
}
}
// FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW
// ensuring that only the cancel button press is sent (VAL 2) when controls are off.
// This avoids unintended engagements while still allowing resume spam
if ((addr == 0x296) && !controls_allowed && (bus == bus_buttons)) {
if (((GET_BYTE(to_send, 0) >> 5) & 0x7U) != 2U) {
tx = false;
}
}
// Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
if (addr == 0x18DAB0F1) {
if ((GET_BYTES(to_send, 0, 4) != 0x00803E02U) || (GET_BYTES(to_send, 4, 4) != 0x0U)) {
tx = false;
}
}
return tx;
}
static safety_config honda_nidec_init(uint16_t param) {
honda_hw = HONDA_NIDEC;
honda_brake = 0;
honda_brake_switch_prev = false;
honda_fwd_brake = false;
honda_alt_brake_msg = false;
honda_bosch_long = false;
honda_bosch_radarless = false;
enable_gas_interceptor = GET_FLAG(param, HONDA_PARAM_GAS_INTERCEPTOR);
honda_clarity_brake_msg = GET_FLAG(param, HONDA_PARAM_CLARITY);
safety_config ret;
bool enable_nidec_alt = GET_FLAG(param, HONDA_PARAM_NIDEC_ALT);
if (enable_nidec_alt) {
enable_gas_interceptor ? SET_RX_CHECKS(honda_nidec_alt_interceptor_rx_checks, ret) : \
SET_RX_CHECKS(honda_nidec_alt_rx_checks, ret);
} else {
enable_gas_interceptor ? SET_RX_CHECKS(honda_common_interceptor_rx_checks, ret) : \
SET_RX_CHECKS(honda_common_rx_checks, ret);
}
if (enable_gas_interceptor) {
SET_TX_MSGS(HONDA_N_INTERCEPTOR_TX_MSGS, ret);
} else {
SET_TX_MSGS(HONDA_N_TX_MSGS, ret);
}
return ret;
}
static safety_config honda_bosch_init(uint16_t param) {
honda_hw = HONDA_BOSCH;
honda_brake_switch_prev = false;
honda_bosch_radarless = GET_FLAG(param, HONDA_PARAM_RADARLESS);
// Checking for alternate brake override from safety parameter
honda_alt_brake_msg = GET_FLAG(param, HONDA_PARAM_ALT_BRAKE);
// radar disabled so allow gas/brakes
#ifdef ALLOW_DEBUG
honda_bosch_long = GET_FLAG(param, HONDA_PARAM_BOSCH_LONG);
#endif
safety_config ret;
if (honda_bosch_radarless && honda_alt_brake_msg) {
SET_RX_CHECKS(honda_common_alt_brake_rx_checks, ret);
} else if (honda_bosch_radarless) {
SET_RX_CHECKS(honda_common_rx_checks, ret);
} else if (honda_alt_brake_msg) {
SET_RX_CHECKS(honda_bosch_alt_brake_rx_checks, ret);
} else {
SET_RX_CHECKS(honda_bosch_rx_checks, ret);
}
if (honda_bosch_radarless) {
honda_bosch_long ? SET_TX_MSGS(HONDA_RADARLESS_LONG_TX_MSGS, ret) : \
SET_TX_MSGS(HONDA_RADARLESS_TX_MSGS, ret);
} else {
honda_bosch_long ? SET_TX_MSGS(HONDA_BOSCH_LONG_TX_MSGS, ret) : \
SET_TX_MSGS(HONDA_BOSCH_TX_MSGS, ret);
}
return ret;
}
static int honda_nidec_fwd_hook(int bus_num, int addr) {
// fwd from car to camera. also fwd certain msgs from camera to car
// 0xE4 is steering on all cars except CRV and RDX, 0x194 for CRV and RDX,
// 0x1FA is brake control, 0x30C is acc hud, 0x33D is lkas hud
int bus_fwd = -1;
if (bus_num == 0) {
bus_fwd = 2;
}
if (bus_num == 2) {
// block stock lkas messages and stock acc messages (if OP is doing ACC)
bool is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D);
bool is_acc_hud_msg = addr == 0x30C;
bool is_brake_msg = addr == 0x1FA;
bool block_fwd = is_lkas_msg || is_acc_hud_msg || (is_brake_msg && !honda_fwd_brake);
if (!block_fwd) {
bus_fwd = 0;
}
}
return bus_fwd;
}
static int honda_bosch_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
if (bus_num == 0) {
bus_fwd = 2;
}
if (bus_num == 2) {
bool is_lkas_msg = (addr == 0xE4) || (addr == 0xE5) || (addr == 0x33D) || (addr == 0x33DA) || (addr == 0x33DB);
bool is_acc_msg = ((addr == 0x1C8) || (addr == 0x30C)) && honda_bosch_radarless && honda_bosch_long;
bool block_msg = is_lkas_msg || is_acc_msg;
if (!block_msg) {
bus_fwd = 0;
}
}
return bus_fwd;
}
const safety_hooks honda_nidec_hooks = {
.init = honda_nidec_init,
.rx = honda_rx_hook,
.tx = honda_tx_hook,
.fwd = honda_nidec_fwd_hook,
.get_counter = honda_get_counter,
.get_checksum = honda_get_checksum,
.compute_checksum = honda_compute_checksum,
};
const safety_hooks honda_bosch_hooks = {
.init = honda_bosch_init,
.rx = honda_rx_hook,
.tx = honda_tx_hook,
.fwd = honda_bosch_fwd_hook,
.get_counter = honda_get_counter,
.get_checksum = honda_get_checksum,
.compute_checksum = honda_compute_checksum,
};

356
panda/board/safety/safety_hyundai.h Normal file
View File

@@ -0,0 +1,356 @@
#include "safety_hyundai_common.h"
#define HYUNDAI_LIMITS(steer, rate_up, rate_down) { \
.max_steer = (steer), \
.max_rate_up = (rate_up), \
.max_rate_down = (rate_down), \
.max_rt_delta = 112, \
.max_rt_interval = 250000, \
.driver_torque_allowance = 50, \
.driver_torque_factor = 2, \
.type = TorqueDriverLimited, \
/* the EPS faults when the steering angle is above a certain threshold for too long. to prevent this, */ \
/* we allow setting CF_Lkas_ActToi 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 SteeringLimits HYUNDAI_STEERING_LIMITS = HYUNDAI_LIMITS(384, 3, 7);
const SteeringLimits HYUNDAI_STEERING_LIMITS_ALT = HYUNDAI_LIMITS(270, 2, 3);
const LongitudinalLimits HYUNDAI_LONG_LIMITS = {
.max_accel = 200, // 1/100 m/s2
.min_accel = -350, // 1/100 m/s2
};
const LongitudinalLimits HYUNDAI_LONG_LIMITS_SPORT = {
.max_accel = 400, // 1/100 m/s2
.min_accel = -350, // 1/100 m/s2
};
const CanMsg HYUNDAI_TX_MSGS[] = {
{0x340, 0, 8}, // LKAS11 Bus 0
{0x4F1, 0, 4}, // CLU11 Bus 0
{0x485, 0, 4}, // LFAHDA_MFC Bus 0
};
const CanMsg HYUNDAI_LONG_TX_MSGS[] = {
{0x340, 0, 8}, // LKAS11 Bus 0
{0x4F1, 0, 4}, // CLU11 Bus 0
{0x485, 0, 4}, // LFAHDA_MFC Bus 0
{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)
};
const CanMsg HYUNDAI_CAMERA_SCC_TX_MSGS[] = {
{0x340, 0, 8}, // LKAS11 Bus 0
{0x4F1, 2, 4}, // CLU11 Bus 2
{0x485, 0, 4}, // LFAHDA_MFC Bus 0
};
#define HYUNDAI_COMMON_RX_CHECKS(legacy) \
{.msg = {{0x260, 0, 8, .check_checksum = true, .max_counter = 3U, .frequency = 100U}, \
{0x371, 0, 8, .frequency = 100U}, { 0 }}}, \
{.msg = {{0x386, 0, 8, .check_checksum = !(legacy), .max_counter = (legacy) ? 0U : 15U, .frequency = 100U}, { 0 }, { 0 }}}, \
{.msg = {{0x394, 0, 8, .check_checksum = !(legacy), .max_counter = (legacy) ? 0U : 7U, .frequency = 100U}, { 0 }, { 0 }}}, \
#define HYUNDAI_SCC12_ADDR_CHECK(scc_bus) \
{.msg = {{0x421, (scc_bus), 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
RxCheck hyundai_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_SCC12_ADDR_CHECK(0)
};
RxCheck hyundai_cam_scc_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
HYUNDAI_SCC12_ADDR_CHECK(2)
};
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, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
};
// older hyundai models have less checks due to missing counters and checksums
RxCheck hyundai_legacy_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(true)
HYUNDAI_SCC12_ADDR_CHECK(0)
};
bool hyundai_legacy = false;
static uint8_t hyundai_get_counter(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
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;
} else {
}
return cnt;
}
static uint32_t hyundai_get_checksum(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
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;
} else {
}
return chksum;
}
static uint32_t hyundai_compute_checksum(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
uint8_t chksum = 0;
if (addr == 0x386) {
// count the bits
for (int i = 0; i < 8; i++) {
uint8_t b = GET_BYTE(to_push, i);
for (int j = 0; j < 8; j++) {
uint8_t bit = 0;
// exclude checksum and counter
if (((i != 1) || (j < 6)) && ((i != 3) || (j < 6)) && ((i != 5) || (j < 6)) && ((i != 7) || (j < 6))) {
bit = (b >> (uint8_t)j) & 1U;
}
chksum += bit;
}
}
chksum = (chksum ^ 9U) & 15U;
} else {
// sum of nibbles
for (int i = 0; i < 8; i++) {
if ((addr == 0x394) && (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
}
chksum += (b % 16U) + (b / 16U);
}
chksum = (16U - (chksum % 16U)) % 16U;
}
return chksum;
}
static void hyundai_rx_hook(const CANPacket_t *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
// SCC12 is on bus 2 for camera-based SCC cars, bus 0 on all others
if ((addr == 0x421) && (((bus == 0) && !hyundai_camera_scc) || ((bus == 2) && hyundai_camera_scc))) {
// 2 bits: 13-14
int cruise_engaged = (GET_BYTES(to_push, 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;
// update array of samples
update_sample(&torque_driver, torque_driver_new);
}
// ACC steering wheel buttons
if (addr == 0x4F1) {
int cruise_button = GET_BYTE(to_push, 0) & 0x7U;
bool main_button = GET_BIT(to_push, 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 == 0x260) && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
gas_pressed = (GET_BYTE(to_push, 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;
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;
}
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;
}
generic_rx_checks(stock_ecu_detected);
}
}
static bool hyundai_tx_hook(const CANPacket_t *to_send) {
sport_mode = alternative_experience & ALT_EXP_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX;
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 ((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;
int aeb_decel_cmd = GET_BYTE(to_send, 2);
bool aeb_req = GET_BIT(to_send, 54U);
bool violation = false;
if (sport_mode) {
violation |= longitudinal_accel_checks(desired_accel_raw, HYUNDAI_LONG_LIMITS_SPORT);
violation |= longitudinal_accel_checks(desired_accel_val, HYUNDAI_LONG_LIMITS_SPORT);
} else {
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 (violation) {
tx = false;
}
}
// 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);
const SteeringLimits limits = hyundai_alt_limits ? HYUNDAI_STEERING_LIMITS_ALT : HYUNDAI_STEERING_LIMITS;
if (steer_torque_cmd_checks(desired_torque, steer_req, limits)) {
tx = false;
}
}
// 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)) {
tx = false;
}
}
// BUTTONS: used for resume spamming and cruise cancellation
if ((addr == 0x4F1) && !hyundai_longitudinal) {
int button = GET_BYTE(to_send, 0) & 0x7U;
bool allowed_resume = (button == 1) && controls_allowed;
bool allowed_cancel = (button == 4) && cruise_engaged_prev;
if (!(allowed_resume || allowed_cancel)) {
tx = false;
}
}
return tx;
}
static int hyundai_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
// forward cam to ccan and viceversa, except lkas cmd
if (bus_num == 0) {
bus_fwd = 2;
}
if ((bus_num == 2) && (addr != 0x340) && (addr != 0x485)) {
bus_fwd = 0;
}
return bus_fwd;
}
static safety_config hyundai_init(uint16_t param) {
hyundai_common_init(param);
hyundai_legacy = false;
if (hyundai_camera_scc) {
hyundai_longitudinal = false;
}
safety_config ret;
if (hyundai_longitudinal) {
ret = BUILD_SAFETY_CFG(hyundai_long_rx_checks, HYUNDAI_LONG_TX_MSGS);
} else if (hyundai_camera_scc) {
ret = BUILD_SAFETY_CFG(hyundai_cam_scc_rx_checks, HYUNDAI_CAMERA_SCC_TX_MSGS);
} else {
ret = BUILD_SAFETY_CFG(hyundai_rx_checks, HYUNDAI_TX_MSGS);
}
return ret;
}
static safety_config hyundai_legacy_init(uint16_t param) {
hyundai_common_init(param);
hyundai_legacy = true;
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,
};
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,
};

360
panda/board/safety/safety_hyundai_canfd.h Normal file
View File

@@ -0,0 +1,360 @@
#include "safety_hyundai_common.h"
const SteeringLimits HYUNDAI_CANFD_STEERING_LIMITS = {
.max_steer = 270,
.max_rt_delta = 112,
.max_rt_interval = 250000,
.max_rate_up = 2,
.max_rate_down = 3,
.driver_torque_allowance = 250,
.driver_torque_factor = 2,
.type = TorqueDriverLimited,
// 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
};
const CanMsg HYUNDAI_CANFD_HDA2_LONG_TX_MSGS[] = {
{0x50, 0, 16}, // LKAS
{0x1CF, 1, 8}, // CRUISE_BUTTON
{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
};
const CanMsg HYUNDAI_CANFD_HDA1_TX_MSGS[] = {
{0x12A, 0, 16}, // LFA
{0x1A0, 0, 32}, // CRUISE_INFO
{0x1CF, 2, 8}, // CRUISE_BUTTON
{0x1E0, 0, 16}, // LFAHDA_CLUSTER
};
// *** 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, .check_checksum = true, .max_counter = 0xffU, .frequency = 100U}, \
{0x100, (pt_bus), 32, .check_checksum = true, .max_counter = 0xffU, .frequency = 100U}, \
{0x105, (pt_bus), 32, .check_checksum = true, .max_counter = 0xffU, .frequency = 100U}}}, \
{.msg = {{0x175, (pt_bus), 24, .check_checksum = true, .max_counter = 0xffU, .frequency = 50U}, { 0 }, { 0 }}}, \
{.msg = {{0xa0, (pt_bus), 24, .check_checksum = true, .max_counter = 0xffU, .frequency = 100U}, { 0 }, { 0 }}}, \
{.msg = {{0xea, (pt_bus), 24, .check_checksum = true, .max_counter = 0xffU, .frequency = 100U}, { 0 }, { 0 }}}, \
#define HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(pt_bus) \
{.msg = {{0x1cf, (pt_bus), 8, .check_checksum = false, .max_counter = 0xfU, .frequency = 50U}, { 0 }, { 0 }}}, \
#define HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(pt_bus) \
{.msg = {{0x1aa, (pt_bus), 16, .check_checksum = false, .max_counter = 0xffU, .frequency = 50U}, { 0 }, { 0 }}}, \
// SCC_CONTROL (from ADAS unit or camera)
#define HYUNDAI_CANFD_SCC_ADDR_CHECK(scc_bus) \
{.msg = {{0x1a0, (scc_bus), 32, .check_checksum = true, .max_counter = 0xffU, .frequency = 50U}, { 0 }, { 0 }}}, \
// *** 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)
HYUNDAI_CANFD_SCC_ADDR_CHECK(1)
};
RxCheck hyundai_canfd_hda2_long_rx_checks[] = {
HYUNDAI_CANFD_COMMON_RX_CHECKS(1)
HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(1)
};
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;
int hyundai_canfd_hda2_get_lkas_addr(void) {
return hyundai_canfd_hda2_alt_steering ? 0x110 : 0x50;
}
static uint8_t hyundai_canfd_get_counter(const CANPacket_t *to_push) {
uint8_t ret = 0;
if (GET_LEN(to_push) == 8U) {
ret = GET_BYTE(to_push, 1) >> 4;
} else {
ret = GET_BYTE(to_push, 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);
return chksum;
}
static void hyundai_canfd_rx_hook(const CANPacket_t *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
const int pt_bus = hyundai_canfd_hda2 ? 1 : 0;
const int scc_bus = hyundai_camera_scc ? 2 : pt_bus;
if (bus == pt_bus) {
// driver torque
if (addr == 0xea) {
int torque_driver_new = ((GET_BYTE(to_push, 11) & 0x1fU) << 8U) | GET_BYTE(to_push, 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) {
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);
} else {
cruise_button = (GET_BYTE(to_push, 4) >> 4) & 0x7U;
main_button = GET_BIT(to_push, 34U);
}
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);
} else {
}
// brake press
if (addr == 0x175) {
brake_pressed = GET_BIT(to_push, 81U);
}
// vehicle moving
if (addr == 0xa0) {
uint32_t front_left_speed = GET_BYTES(to_push, 8, 2);
uint32_t rear_right_speed = GET_BYTES(to_push, 14, 2);
vehicle_moving = (front_left_speed > HYUNDAI_STANDSTILL_THRSLD) || (rear_right_speed > HYUNDAI_STANDSTILL_THRSLD);
}
}
if (bus == scc_bus) {
// cruise state
if ((addr == 0x1a0) && !hyundai_longitudinal) {
// 1=enabled, 2=driver override
int cruise_status = ((GET_BYTE(to_push, 8) >> 4) & 0x7U);
bool cruise_engaged = (cruise_status == 1) || (cruise_status == 2);
hyundai_common_cruise_state_check(cruise_engaged);
}
}
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_detected);
}
static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);
// 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);
if (steer_torque_cmd_checks(desired_torque, steer_req, HYUNDAI_CANFD_STEERING_LIMITS)) {
tx = false;
}
}
// cruise buttons check
if (addr == 0x1cf) {
int button = GET_BYTE(to_send, 2) & 0x7U;
bool is_cancel = (button == HYUNDAI_BTN_CANCEL);
bool is_resume = (button == HYUNDAI_BTN_RESUME);
bool allowed = (is_cancel && cruise_engaged_prev) || (is_resume && 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)) {
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;
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 {
// only used to cancel on here
if ((desired_accel_raw != 0) || (desired_accel_val != 0)) {
violation = true;
}
}
if (violation) {
tx = false;
}
}
return tx;
}
static int hyundai_canfd_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
if (bus_num == 0) {
bus_fwd = 2;
}
if (bus_num == 2) {
// LKAS for HDA2, LFA for HDA1
int hda2_lfa_block_addr = hyundai_canfd_hda2_alt_steering ? 0x362 : 0x2a4;
bool is_lkas_msg = ((addr == hyundai_canfd_hda2_get_lkas_addr()) || (addr == hda2_lfa_block_addr)) && hyundai_canfd_hda2;
bool is_lfa_msg = ((addr == 0x12a) && !hyundai_canfd_hda2);
// HUD icons
bool is_lfahda_msg = ((addr == 0x1e0) && !hyundai_canfd_hda2);
// CRUISE_INFO for non-HDA2, we send our own longitudinal commands
bool is_scc_msg = ((addr == 0x1a0) && hyundai_longitudinal && !hyundai_canfd_hda2);
bool block_msg = is_lkas_msg || is_lfa_msg || is_lfahda_msg || is_scc_msg;
if (!block_msg) {
bus_fwd = 0;
}
}
return bus_fwd;
}
static safety_config hyundai_canfd_init(uint16_t param) {
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);
// 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) {
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) {
ret = hyundai_canfd_hda2_alt_steering ? BUILD_SAFETY_CFG(hyundai_canfd_hda2_rx_checks, HYUNDAI_CANFD_HDA2_ALT_STEERING_TX_MSGS) : \
BUILD_SAFETY_CFG(hyundai_canfd_hda2_rx_checks, HYUNDAI_CANFD_HDA2_TX_MSGS);
} else if (!hyundai_camera_scc) {
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 {
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);
}
}
return ret;
}
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,
};

118
panda/board/safety/safety_hyundai_common.h Normal file
View File

@@ -0,0 +1,118 @@
#ifndef SAFETY_HYUNDAI_COMMON_H
#define SAFETY_HYUNDAI_COMMON_H
const int HYUNDAI_PARAM_EV_GAS = 1;
const int HYUNDAI_PARAM_HYBRID_GAS = 2;
const int HYUNDAI_PARAM_LONGITUDINAL = 4;
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 uint8_t HYUNDAI_PREV_BUTTON_SAMPLES = 8; // roughly 160 ms
const uint32_t HYUNDAI_STANDSTILL_THRSLD = 12; // 0.375 kph
enum {
HYUNDAI_BTN_NONE = 0,
HYUNDAI_BTN_RESUME = 1,
HYUNDAI_BTN_SET = 2,
HYUNDAI_BTN_CANCEL = 4,
};
// common state
bool hyundai_ev_gas_signal = false;
bool hyundai_hybrid_gas_signal = false;
bool hyundai_longitudinal = false;
bool hyundai_camera_scc = false;
bool hyundai_canfd_hda2 = false;
bool hyundai_alt_limits = false;
uint8_t hyundai_last_button_interaction; // button messages since the user pressed an enable button
uint16_t hyundai_canfd_crc_lut[256];
void hyundai_common_init(uint16_t param) {
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_alt_limits = GET_FLAG(param, HYUNDAI_PARAM_ALT_LIMITS);
hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES;
#ifdef ALLOW_DEBUG
hyundai_longitudinal = GET_FLAG(param, HYUNDAI_PARAM_LONGITUDINAL);
#else
hyundai_longitudinal = false;
#endif
}
void hyundai_common_cruise_state_check(const bool cruise_engaged) {
// some newer HKG models can re-enable after spamming cancel button,
// so keep track of user button presses to deny engagement if no interaction
// enter controls on rising edge of ACC and recent user button press, exit controls when ACC off
if (!hyundai_longitudinal) {
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) {
hyundai_last_button_interaction = 0U;
} else {
hyundai_last_button_interaction = MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES);
}
if (hyundai_longitudinal) {
// enter controls on falling edge of resume or set
bool set = (cruise_button != HYUNDAI_BTN_SET) && (cruise_button_prev == HYUNDAI_BTN_SET);
bool res = (cruise_button != HYUNDAI_BTN_RESUME) && (cruise_button_prev == HYUNDAI_BTN_RESUME);
if (set || res) {
controls_allowed = true;
}
// exit controls on cancel press
if (cruise_button == HYUNDAI_BTN_CANCEL) {
controls_allowed = false;
}
cruise_button_prev = cruise_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);
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)];
}
// 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 == 24) {
crc ^= 0x819dU;
} else if (len == 32) {
crc ^= 0x9f5bU;
} else {
}
return crc;
}
#endif

130
panda/board/safety/safety_mazda.h Normal file
View File

@@ -0,0 +1,130 @@
// CAN msgs we care about
#define MAZDA_LKAS 0x243
#define MAZDA_LKAS_HUD 0x440
#define MAZDA_CRZ_CTRL 0x21c
#define MAZDA_CRZ_BTNS 0x09d
#define MAZDA_STEER_TORQUE 0x240
#define MAZDA_ENGINE_DATA 0x202
#define MAZDA_PEDALS 0x165
// CAN bus numbers
#define MAZDA_MAIN 0
#define MAZDA_AUX 1
#define MAZDA_CAM 2
const SteeringLimits MAZDA_STEERING_LIMITS = {
.max_steer = 800,
.max_rate_up = 10,
.max_rate_down = 25,
.max_rt_delta = 300,
.max_rt_interval = 250000,
.driver_torque_factor = 1,
.driver_torque_allowance = 15,
.type = TorqueDriverLimited,
};
const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_CRZ_BTNS, 0, 8}, {MAZDA_LKAS_HUD, 0, 8}};
RxCheck mazda_rx_checks[] = {
{.msg = {{MAZDA_CRZ_CTRL, 0, 8, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{MAZDA_CRZ_BTNS, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},
{.msg = {{MAZDA_STEER_TORQUE, 0, 8, .frequency = 83U}, { 0 }, { 0 }}},
{.msg = {{MAZDA_ENGINE_DATA, 0, 8, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{MAZDA_PEDALS, 0, 8, .frequency = 50U}, { 0 }, { 0 }}},
};
// track msgs coming from OP so that we know what CAM msgs to drop and what to forward
static void mazda_rx_hook(const CANPacket_t *to_push) {
if ((int)GET_BUS(to_push) == MAZDA_MAIN) {
int addr = GET_ADDR(to_push);
if (addr == MAZDA_ENGINE_DATA) {
// sample speed: scale by 0.01 to get kph
int speed = (GET_BYTE(to_push, 2) << 8) | GET_BYTE(to_push, 3);
vehicle_moving = speed > 10; // moving when speed > 0.1 kph
}
if (addr == MAZDA_STEER_TORQUE) {
int torque_driver_new = GET_BYTE(to_push, 0) - 127U;
// update array of samples
update_sample(&torque_driver, torque_driver_new);
}
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == MAZDA_CRZ_CTRL) {
acc_main_on = GET_BIT(to_push, 17U);
bool cruise_engaged = GET_BYTE(to_push, 0) & 0x8U;
pcm_cruise_check(cruise_engaged);
}
if (addr == MAZDA_ENGINE_DATA) {
gas_pressed = (GET_BYTE(to_push, 4) || (GET_BYTE(to_push, 5) & 0xF0U));
}
if (addr == MAZDA_PEDALS) {
brake_pressed = (GET_BYTE(to_push, 0) & 0x10U);
}
generic_rx_checks((addr == MAZDA_LKAS));
}
}
static bool mazda_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
// Check if msg is sent on the main BUS
if (bus == MAZDA_MAIN) {
// steer cmd checks
if (addr == MAZDA_LKAS) {
int desired_torque = (((GET_BYTE(to_send, 0) & 0x0FU) << 8) | GET_BYTE(to_send, 1)) - 2048U;
if (steer_torque_cmd_checks(desired_torque, -1, MAZDA_STEERING_LIMITS)) {
tx = false;
}
}
// cruise buttons check
if (addr == MAZDA_CRZ_BTNS) {
// allow resume spamming while controls allowed, but
// only allow cancel while contrls not allowed
bool cancel_cmd = (GET_BYTE(to_send, 0) == 0x1U);
if (!controls_allowed && !cancel_cmd) {
tx = false;
}
}
}
return tx;
}
static int mazda_fwd_hook(int bus, int addr) {
int bus_fwd = -1;
if (bus == MAZDA_MAIN) {
bus_fwd = MAZDA_CAM;
} else if (bus == MAZDA_CAM) {
bool block = (addr == MAZDA_LKAS) || (addr == MAZDA_LKAS_HUD);
if (!block) {
bus_fwd = MAZDA_MAIN;
}
} else {
// don't fwd
}
return bus_fwd;
}
static safety_config mazda_init(uint16_t param) {
UNUSED(param);
return BUILD_SAFETY_CFG(mazda_rx_checks, MAZDA_TX_MSGS);
}
const safety_hooks mazda_hooks = {
.init = mazda_init,
.rx = mazda_rx_hook,
.tx = mazda_tx_hook,
.fwd = mazda_fwd_hook,
};

166
panda/board/safety/safety_nissan.h Normal file
View File

@@ -0,0 +1,166 @@
const SteeringLimits NISSAN_STEERING_LIMITS = {
.angle_deg_to_can = 100,
.angle_rate_up_lookup = {
{0., 5., 15.},
{5., .8, .15}
},
.angle_rate_down_lookup = {
{0., 5., 15.},
{5., 3.5, .4}
},
};
const CanMsg NISSAN_TX_MSGS[] = {
{0x169, 0, 8}, // LKAS
{0x2b1, 0, 8}, // PROPILOT_HUD
{0x4cc, 0, 8}, // PROPILOT_HUD_INFO_MSG
{0x20b, 2, 6}, // CRUISE_THROTTLE (X-Trail)
{0x20b, 1, 6}, // CRUISE_THROTTLE (Altima)
{0x280, 2, 8} // CANCEL_MSG (Leaf)
};
// Signals duplicated below due to the fact that these messages can come in on either CAN bus, depending on car model.
RxCheck nissan_rx_checks[] = {
{.msg = {{0x1b6, 0, 8, .frequency = 100U},
{0x1b6, 1, 8, .frequency = 100U}, { 0 }}}, // PRO_PILOT (100HZ)
{.msg = {{0x2, 0, 5, .frequency = 100U},
{0x2, 1, 5, .frequency = 100U}, { 0 }}}, // STEER_ANGLE_SENSOR
{.msg = {{0x285, 0, 8, .frequency = 50U},
{0x285, 1, 8, .frequency = 50U}, { 0 }}}, // WHEEL_SPEEDS_REAR
{.msg = {{0x30f, 2, 3, .frequency = 10U},
{0x30f, 1, 3, .frequency = 10U}, { 0 }}}, // CRUISE_STATE
{.msg = {{0x15c, 0, 8, .frequency = 50U},
{0x15c, 1, 8, .frequency = 50U},
{0x239, 0, 8, .frequency = 50U}}}, // GAS_PEDAL
{.msg = {{0x454, 0, 8, .frequency = 10U},
{0x454, 1, 8, .frequency = 10U},
{0x1cc, 0, 4, .frequency = 100U}}}, // DOORS_LIGHTS / BRAKE
};
// EPS Location. false = V-CAN, true = C-CAN
const int NISSAN_PARAM_ALT_EPS_BUS = 1;
bool nissan_alt_eps = false;
static void nissan_rx_hook(const CANPacket_t *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
if (bus == (nissan_alt_eps ? 1 : 0)) {
if (addr == 0x2) {
// Current steering angle
// Factor -0.1, little endian
int angle_meas_new = (GET_BYTES(to_push, 0, 4) & 0xFFFFU);
// Multiply by -10 to match scale of LKAS angle
angle_meas_new = to_signed(angle_meas_new, 16) * -10;
// update array of samples
update_sample(&angle_meas, angle_meas_new);
}
if (addr == 0x285) {
// Get current speed and standstill
uint16_t right_rear = (GET_BYTE(to_push, 0) << 8) | (GET_BYTE(to_push, 1));
uint16_t left_rear = (GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3));
vehicle_moving = (right_rear | left_rear) != 0U;
UPDATE_VEHICLE_SPEED((right_rear + left_rear) / 2.0 * 0.005 / 3.6);
}
if (addr == 0x1b6) {
acc_main_on = GET_BIT(to_push, 36U);
}
// X-Trail 0x15c, Leaf 0x239
if ((addr == 0x15c) || (addr == 0x239)) {
if (addr == 0x15c){
gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3U)) > 3U;
} else {
acc_main_on = GET_BIT(to_push, 17U);
gas_pressed = GET_BYTE(to_push, 0) > 3U;
}
}
// X-trail 0x454, Leaf 0x239
if ((addr == 0x454) || (addr == 0x239)) {
if (addr == 0x454){
brake_pressed = (GET_BYTE(to_push, 2) & 0x80U) != 0U;
} else {
brake_pressed = ((GET_BYTE(to_push, 4) >> 5) & 1U) != 0U;
}
}
}
// Handle cruise enabled
if ((addr == 0x30f) && (bus == (nissan_alt_eps ? 1 : 2))) {
bool cruise_engaged = (GET_BYTE(to_push, 0) >> 3) & 1U;
pcm_cruise_check(cruise_engaged);
}
generic_rx_checks((addr == 0x169) && (bus == 0));
}
static bool nissan_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);
bool violation = false;
// steer cmd checks
if (addr == 0x169) {
int desired_angle = ((GET_BYTE(to_send, 0) << 10) | (GET_BYTE(to_send, 1) << 2) | ((GET_BYTE(to_send, 2) >> 6) & 0x3U));
bool lka_active = (GET_BYTE(to_send, 6) >> 4) & 1U;
// Factor is -0.01, offset is 1310. Flip to correct sign, but keep units in CAN scale
desired_angle = -desired_angle + (1310.0f * NISSAN_STEERING_LIMITS.angle_deg_to_can);
if (steer_angle_cmd_checks(desired_angle, lka_active, NISSAN_STEERING_LIMITS)) {
violation = true;
}
}
// acc button check, only allow cancel button to be sent
if (addr == 0x20b) {
// Violation of any button other than cancel is pressed
violation |= ((GET_BYTE(to_send, 1) & 0x3dU) > 0U);
}
if (violation) {
tx = false;
}
return tx;
}
static int nissan_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
if (bus_num == 0) {
bool block_msg = (addr == 0x280); // CANCEL_MSG
if (!block_msg) {
bus_fwd = 2; // ADAS
}
}
if (bus_num == 2) {
// 0x169 is LKAS, 0x2b1 LKAS_HUD, 0x4cc LKAS_HUD_INFO_MSG
bool block_msg = ((addr == 0x169) || (addr == 0x2b1) || (addr == 0x4cc));
if (!block_msg) {
bus_fwd = 0; // V-CAN
}
}
return bus_fwd;
}
static safety_config nissan_init(uint16_t param) {
nissan_alt_eps = GET_FLAG(param, NISSAN_PARAM_ALT_EPS_BUS);
return BUILD_SAFETY_CFG(nissan_rx_checks, NISSAN_TX_MSGS);
}
const safety_hooks nissan_hooks = {
.init = nissan_init,
.rx = nissan_rx_hook,
.tx = nissan_tx_hook,
.fwd = nissan_fwd_hook,
};

295
panda/board/safety/safety_subaru.h Normal file
View File

@@ -0,0 +1,295 @@
#define SUBARU_STEERING_LIMITS_GENERATOR(steer_max, rate_up, rate_down) \
{ \
.max_steer = (steer_max), \
.max_rt_delta = 940, \
.max_rt_interval = 250000, \
.max_rate_up = (rate_up), \
.max_rate_down = (rate_down), \
.driver_torque_factor = 50, \
.driver_torque_allowance = 60, \
.type = TorqueDriverLimited, \
/* the EPS will temporary fault if the steering rate is too high, so we cut the \
the steering torque every 7 frames for 1 frame if the steering rate is high */ \
.min_valid_request_frames = 7, \
.max_invalid_request_frames = 1, \
.min_valid_request_rt_interval = 144000, /* 10% tolerance */ \
.has_steer_req_tolerance = true, \
}
const SteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(3071, 50, 70);
const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);
const LongitudinalLimits SUBARU_LONG_LIMITS = {
.min_gas = 808, // appears to be engine braking
.max_gas = 3400, // approx 2 m/s^2 when maxing cruise_rpm and cruise_throttle
.inactive_gas = 1818, // this is zero acceleration
.max_brake = 600, // approx -3.5 m/s^2
.min_transmission_rpm = 0,
.max_transmission_rpm = 2400,
};
#define MSG_SUBARU_Brake_Status 0x13c
#define MSG_SUBARU_CruiseControl 0x240
#define MSG_SUBARU_Throttle 0x40
#define MSG_SUBARU_Steering_Torque 0x119
#define MSG_SUBARU_Wheel_Speeds 0x13a
#define MSG_SUBARU_ES_LKAS 0x122
#define MSG_SUBARU_ES_LKAS_ANGLE 0x124
#define MSG_SUBARU_ES_Brake 0x220
#define MSG_SUBARU_ES_Distance 0x221
#define MSG_SUBARU_ES_Status 0x222
#define MSG_SUBARU_ES_DashStatus 0x321
#define MSG_SUBARU_ES_LKAS_State 0x322
#define MSG_SUBARU_ES_Infotainment 0x323
#define MSG_SUBARU_ES_UDS_Request 0x787
#define MSG_SUBARU_ES_HighBeamAssist 0x121
#define MSG_SUBARU_ES_STATIC_1 0x22a
#define MSG_SUBARU_ES_STATIC_2 0x325
#define SUBARU_MAIN_BUS 0
#define SUBARU_ALT_BUS 1
#define SUBARU_CAM_BUS 2
#define SUBARU_COMMON_TX_MSGS(alt_bus, lkas_msg) \
{lkas_msg, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_Distance, alt_bus, 8}, \
{MSG_SUBARU_ES_DashStatus, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_LKAS_State, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_Infotainment, SUBARU_MAIN_BUS, 8}, \
#define SUBARU_COMMON_LONG_TX_MSGS(alt_bus) \
{MSG_SUBARU_ES_Brake, alt_bus, 8}, \
{MSG_SUBARU_ES_Status, alt_bus, 8}, \
#define SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS() \
{MSG_SUBARU_ES_UDS_Request, SUBARU_CAM_BUS, 8}, \
{MSG_SUBARU_ES_HighBeamAssist, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_STATIC_1, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_STATIC_2, SUBARU_MAIN_BUS, 8}, \
#define SUBARU_COMMON_RX_CHECKS(alt_bus) \
{.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Wheel_Speeds, alt_bus, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_CruiseControl, alt_bus, 8, .check_checksum = true, .max_counter = 15U, .frequency = 20U}, { 0 }, { 0 }}}, \
const CanMsg SUBARU_TX_MSGS[] = {
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
};
const CanMsg SUBARU_LONG_TX_MSGS[] = {
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
SUBARU_COMMON_LONG_TX_MSGS(SUBARU_MAIN_BUS)
};
const CanMsg SUBARU_GEN2_TX_MSGS[] = {
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
};
const CanMsg SUBARU_GEN2_LONG_TX_MSGS[] = {
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
SUBARU_COMMON_LONG_TX_MSGS(SUBARU_ALT_BUS)
SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()
};
RxCheck subaru_rx_checks[] = {
SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS)
};
RxCheck subaru_gen2_rx_checks[] = {
SUBARU_COMMON_RX_CHECKS(SUBARU_ALT_BUS)
};
const uint16_t SUBARU_PARAM_GEN2 = 1;
const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
bool subaru_gen2 = false;
bool subaru_longitudinal = false;
static uint32_t subaru_get_checksum(const CANPacket_t *to_push) {
return (uint8_t)GET_BYTE(to_push, 0);
}
static uint8_t subaru_get_counter(const CANPacket_t *to_push) {
return (uint8_t)(GET_BYTE(to_push, 1) & 0xFU);
}
static uint32_t subaru_compute_checksum(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);
uint8_t checksum = (uint8_t)(addr) + (uint8_t)((unsigned int)(addr) >> 8U);
for (int i = 1; i < len; i++) {
checksum += (uint8_t)GET_BYTE(to_push, i);
}
return checksum;
}
static void subaru_rx_hook(const CANPacket_t *to_push) {
const int bus = GET_BUS(to_push);
const int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
int addr = GET_ADDR(to_push);
if ((addr == MSG_SUBARU_Steering_Torque) && (bus == SUBARU_MAIN_BUS)) {
int torque_driver_new;
torque_driver_new = ((GET_BYTES(to_push, 0, 4) >> 16) & 0x7FFU);
torque_driver_new = -1 * to_signed(torque_driver_new, 11);
update_sample(&torque_driver, torque_driver_new);
int angle_meas_new = (GET_BYTES(to_push, 4, 2) & 0xFFFFU);
// convert Steering_Torque -> Steering_Angle to centidegrees, to match the ES_LKAS_ANGLE angle request units
angle_meas_new = ROUND(to_signed(angle_meas_new, 16) * -2.17);
update_sample(&angle_meas, angle_meas_new);
}
// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) {
acc_main_on = GET_BIT(to_push, 40U);
bool cruise_engaged = GET_BIT(to_push, 41U);
pcm_cruise_check(cruise_engaged);
}
// update vehicle moving with any non-zero wheel speed
if ((addr == MSG_SUBARU_Wheel_Speeds) && (bus == alt_main_bus)) {
uint32_t fr = (GET_BYTES(to_push, 1, 3) >> 4) & 0x1FFFU;
uint32_t rr = (GET_BYTES(to_push, 3, 3) >> 1) & 0x1FFFU;
uint32_t rl = (GET_BYTES(to_push, 4, 3) >> 6) & 0x1FFFU;
uint32_t fl = (GET_BYTES(to_push, 6, 2) >> 3) & 0x1FFFU;
vehicle_moving = (fr > 0U) || (rr > 0U) || (rl > 0U) || (fl > 0U);
UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4U * 0.057);
}
if ((addr == MSG_SUBARU_Brake_Status) && (bus == alt_main_bus)) {
brake_pressed = GET_BIT(to_push, 62U);
}
if ((addr == MSG_SUBARU_Throttle) && (bus == SUBARU_MAIN_BUS)) {
gas_pressed = GET_BYTE(to_push, 4) != 0U;
}
generic_rx_checks((addr == MSG_SUBARU_ES_LKAS) && (bus == SUBARU_MAIN_BUS));
}
static bool subaru_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);
bool violation = false;
// steer cmd checks
if (addr == MSG_SUBARU_ES_LKAS) {
int desired_torque = ((GET_BYTES(to_send, 0, 4) >> 16) & 0x1FFFU);
desired_torque = -1 * to_signed(desired_torque, 13);
bool steer_req = GET_BIT(to_send, 29U);
const SteeringLimits limits = subaru_gen2 ? SUBARU_GEN2_STEERING_LIMITS : SUBARU_STEERING_LIMITS;
violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits);
}
// check es_brake brake_pressure limits
if (addr == MSG_SUBARU_ES_Brake) {
int es_brake_pressure = GET_BYTES(to_send, 2, 2);
violation |= longitudinal_brake_checks(es_brake_pressure, SUBARU_LONG_LIMITS);
}
// check es_distance cruise_throttle limits
if (addr == MSG_SUBARU_ES_Distance) {
int cruise_throttle = (GET_BYTES(to_send, 2, 2) & 0x1FFFU);
bool cruise_cancel = GET_BIT(to_send, 56U);
if (subaru_longitudinal) {
violation |= longitudinal_gas_checks(cruise_throttle, SUBARU_LONG_LIMITS);
} else {
// If openpilot is not controlling long, only allow ES_Distance for cruise cancel requests,
// (when Cruise_Cancel is true, and Cruise_Throttle is inactive)
violation |= (cruise_throttle != SUBARU_LONG_LIMITS.inactive_gas);
violation |= (!cruise_cancel);
}
}
// check es_status transmission_rpm limits
if (addr == MSG_SUBARU_ES_Status) {
int transmission_rpm = (GET_BYTES(to_send, 2, 2) & 0x1FFFU);
violation |= longitudinal_transmission_rpm_checks(transmission_rpm, SUBARU_LONG_LIMITS);
}
if (addr == MSG_SUBARU_ES_UDS_Request) {
// tester present ('\x02\x3E\x80\x00\x00\x00\x00\x00') is allowed for gen2 longitudinal to keep eyesight disabled
bool is_tester_present = (GET_BYTES(to_send, 0, 4) == 0x00803E02U) && (GET_BYTES(to_send, 4, 4) == 0x0U);
// reading ES button data by identifier (b'\x03\x22\x11\x30\x00\x00\x00\x00') is also allowed (DID 0x1130)
bool is_button_rdbi = (GET_BYTES(to_send, 0, 4) == 0x30112203U) && (GET_BYTES(to_send, 4, 4) == 0x0U);
violation |= !(is_tester_present || is_button_rdbi);
}
if (violation){
tx = false;
}
return tx;
}
static int subaru_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
if (bus_num == SUBARU_MAIN_BUS) {
bus_fwd = SUBARU_CAM_BUS; // to the eyesight camera
}
if (bus_num == SUBARU_CAM_BUS) {
// Global platform
bool block_lkas = ((addr == MSG_SUBARU_ES_LKAS) ||
(addr == MSG_SUBARU_ES_DashStatus) ||
(addr == MSG_SUBARU_ES_LKAS_State) ||
(addr == MSG_SUBARU_ES_Infotainment));
bool block_long = ((addr == MSG_SUBARU_ES_Brake) ||
(addr == MSG_SUBARU_ES_Distance) ||
(addr == MSG_SUBARU_ES_Status));
bool block_msg = block_lkas || (subaru_longitudinal && block_long);
if (!block_msg) {
bus_fwd = SUBARU_MAIN_BUS; // Main CAN
}
}
return bus_fwd;
}
static safety_config subaru_init(uint16_t param) {
subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
#ifdef ALLOW_DEBUG
subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
#endif
safety_config ret;
if (subaru_gen2) {
ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_TX_MSGS);
} else {
ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_TX_MSGS);
}
return ret;
}
const safety_hooks subaru_hooks = {
.init = subaru_init,
.rx = subaru_rx_hook,
.tx = subaru_tx_hook,
.fwd = subaru_fwd_hook,
.get_counter = subaru_get_counter,
.get_checksum = subaru_get_checksum,
.compute_checksum = subaru_compute_checksum,
};

127
panda/board/safety/safety_subaru_preglobal.h Normal file
View File

@@ -0,0 +1,127 @@
const SteeringLimits SUBARU_PG_STEERING_LIMITS = {
.max_steer = 2047,
.max_rt_delta = 940,
.max_rt_interval = 250000,
.max_rate_up = 50,
.max_rate_down = 70,
.driver_torque_factor = 10,
.driver_torque_allowance = 75,
.type = TorqueDriverLimited,
};
// Preglobal platform
// 0x161 is ES_CruiseThrottle
// 0x164 is ES_LKAS
#define MSG_SUBARU_PG_CruiseControl 0x144
#define MSG_SUBARU_PG_Throttle 0x140
#define MSG_SUBARU_PG_Wheel_Speeds 0xD4
#define MSG_SUBARU_PG_Brake_Pedal 0xD1
#define MSG_SUBARU_PG_ES_LKAS 0x164
#define MSG_SUBARU_PG_ES_Distance 0x161
#define MSG_SUBARU_PG_Steering_Torque 0x371
#define SUBARU_PG_MAIN_BUS 0
#define SUBARU_PG_CAM_BUS 2
const CanMsg SUBARU_PG_TX_MSGS[] = {
{MSG_SUBARU_PG_ES_Distance, SUBARU_PG_MAIN_BUS, 8},
{MSG_SUBARU_PG_ES_LKAS, SUBARU_PG_MAIN_BUS, 8}
};
// TODO: do checksum and counter checks after adding the signals to the outback dbc file
RxCheck subaru_preglobal_rx_checks[] = {
{.msg = {{MSG_SUBARU_PG_Throttle, SUBARU_PG_MAIN_BUS, 8, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_PG_Steering_Torque, SUBARU_PG_MAIN_BUS, 8, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_PG_CruiseControl, SUBARU_PG_MAIN_BUS, 8, .frequency = 20U}, { 0 }, { 0 }}},
};
const int SUBARU_PG_PARAM_REVERSED_DRIVER_TORQUE = 1;
bool subaru_pg_reversed_driver_torque = false;
static void subaru_preglobal_rx_hook(const CANPacket_t *to_push) {
const int bus = GET_BUS(to_push);
if (bus == SUBARU_PG_MAIN_BUS) {
int addr = GET_ADDR(to_push);
if (addr == MSG_SUBARU_PG_Steering_Torque) {
int torque_driver_new;
torque_driver_new = (GET_BYTE(to_push, 3) >> 5) + (GET_BYTE(to_push, 4) << 3);
torque_driver_new = to_signed(torque_driver_new, 11);
torque_driver_new = subaru_pg_reversed_driver_torque ? -torque_driver_new : torque_driver_new;
update_sample(&torque_driver, torque_driver_new);
}
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == MSG_SUBARU_PG_CruiseControl) {
acc_main_on = GET_BIT(to_push, 48U);
bool cruise_engaged = GET_BIT(to_push, 49U);
pcm_cruise_check(cruise_engaged);
}
// update vehicle moving with any non-zero wheel speed
if (addr == MSG_SUBARU_PG_Wheel_Speeds) {
vehicle_moving = ((GET_BYTES(to_push, 0, 4) >> 12) != 0U) || (GET_BYTES(to_push, 4, 4) != 0U);
}
if (addr == MSG_SUBARU_PG_Brake_Pedal) {
brake_pressed = ((GET_BYTES(to_push, 0, 4) >> 16) & 0xFFU) > 0U;
}
if (addr == MSG_SUBARU_PG_Throttle) {
gas_pressed = GET_BYTE(to_push, 0) != 0U;
}
generic_rx_checks((addr == MSG_SUBARU_PG_ES_LKAS));
}
}
static bool subaru_preglobal_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);
// steer cmd checks
if (addr == MSG_SUBARU_PG_ES_LKAS) {
int desired_torque = ((GET_BYTES(to_send, 0, 4) >> 8) & 0x1FFFU);
desired_torque = -1 * to_signed(desired_torque, 13);
bool steer_req = GET_BIT(to_send, 24U);
if (steer_torque_cmd_checks(desired_torque, steer_req, SUBARU_PG_STEERING_LIMITS)) {
tx = false;
}
}
return tx;
}
static int subaru_preglobal_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
if (bus_num == SUBARU_PG_MAIN_BUS) {
bus_fwd = SUBARU_PG_CAM_BUS; // Camera CAN
}
if (bus_num == SUBARU_PG_CAM_BUS) {
bool block_msg = ((addr == MSG_SUBARU_PG_ES_Distance) || (addr == MSG_SUBARU_PG_ES_LKAS));
if (!block_msg) {
bus_fwd = SUBARU_PG_MAIN_BUS; // Main CAN
}
}
return bus_fwd;
}
static safety_config subaru_preglobal_init(uint16_t param) {
subaru_pg_reversed_driver_torque = GET_FLAG(param, SUBARU_PG_PARAM_REVERSED_DRIVER_TORQUE);
return BUILD_SAFETY_CFG(subaru_preglobal_rx_checks, SUBARU_PG_TX_MSGS);
}
const safety_hooks subaru_preglobal_hooks = {
.init = subaru_preglobal_init,
.rx = subaru_preglobal_rx_hook,
.tx = subaru_preglobal_tx_hook,
.fwd = subaru_preglobal_fwd_hook,
};

249
panda/board/safety/safety_tesla.h Normal file
View File

@@ -0,0 +1,249 @@
const SteeringLimits TESLA_STEERING_LIMITS = {
.angle_deg_to_can = 10,
.angle_rate_up_lookup = {
{0., 5., 15.},
{10., 1.6, .3}
},
.angle_rate_down_lookup = {
{0., 5., 15.},
{10., 7.0, .8}
},
};
const LongitudinalLimits TESLA_LONG_LIMITS = {
.max_accel = 425, // 2. m/s^2
.min_accel = 287, // -3.52 m/s^2 // TODO: limit to -3.48
.inactive_accel = 375, // 0. m/s^2
};
const int TESLA_FLAG_POWERTRAIN = 1;
const int TESLA_FLAG_LONGITUDINAL_CONTROL = 2;
const int TESLA_FLAG_RAVEN = 4;
const CanMsg TESLA_TX_MSGS[] = {
{0x488, 0, 4}, // DAS_steeringControl
{0x45, 0, 8}, // STW_ACTN_RQ
{0x45, 2, 8}, // STW_ACTN_RQ
{0x2b9, 0, 8}, // DAS_control
};
const CanMsg TESLA_PT_TX_MSGS[] = {
{0x2bf, 0, 8}, // DAS_control
};
RxCheck tesla_rx_checks[] = {
{.msg = {{0x2b9, 2, 8, .frequency = 25U}, { 0 }, { 0 }}}, // DAS_control
{.msg = {{0x370, 0, 8, .frequency = 25U}, { 0 }, { 0 }}}, // EPAS_sysStatus
{.msg = {{0x108, 0, 8, .frequency = 100U}, { 0 }, { 0 }}}, // DI_torque1
{.msg = {{0x118, 0, 6, .frequency = 100U}, { 0 }, { 0 }}}, // DI_torque2
{.msg = {{0x20a, 0, 8, .frequency = 50U}, { 0 }, { 0 }}}, // BrakeMessage
{.msg = {{0x368, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, // DI_state
{.msg = {{0x318, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, // GTW_carState
};
RxCheck tesla_raven_rx_checks[] = {
{.msg = {{0x2b9, 2, 8, .frequency = 25U}, { 0 }, { 0 }}}, // DAS_control
{.msg = {{0x131, 2, 8, .frequency = 100U}, { 0 }, { 0 }}}, // EPAS3P_sysStatus
{.msg = {{0x108, 0, 8, .frequency = 100U}, { 0 }, { 0 }}}, // DI_torque1
{.msg = {{0x118, 0, 6, .frequency = 100U}, { 0 }, { 0 }}}, // DI_torque2
{.msg = {{0x20a, 0, 8, .frequency = 50U}, { 0 }, { 0 }}}, // BrakeMessage
{.msg = {{0x368, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, // DI_state
{.msg = {{0x318, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, // GTW_carState
};
RxCheck tesla_pt_rx_checks[] = {
{.msg = {{0x106, 0, 8, .frequency = 100U}, { 0 }, { 0 }}}, // DI_torque1
{.msg = {{0x116, 0, 6, .frequency = 100U}, { 0 }, { 0 }}}, // DI_torque2
{.msg = {{0x1f8, 0, 8, .frequency = 50U}, { 0 }, { 0 }}}, // BrakeMessage
{.msg = {{0x2bf, 2, 8, .frequency = 25U}, { 0 }, { 0 }}}, // DAS_control
{.msg = {{0x256, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, // DI_state
};
bool tesla_longitudinal = false;
bool tesla_powertrain = false; // Are we the second panda intercepting the powertrain bus?
bool tesla_raven = false;
bool tesla_stock_aeb = false;
static void tesla_rx_hook(const CANPacket_t *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
if (!tesla_powertrain) {
if((!tesla_raven && (addr == 0x370) && (bus == 0)) || (tesla_raven && (addr == 0x131) && (bus == 2))) {
// Steering angle: (0.1 * val) - 819.2 in deg.
// Store it 1/10 deg to match steering request
int angle_meas_new = (((GET_BYTE(to_push, 4) & 0x3FU) << 8) | GET_BYTE(to_push, 5)) - 8192U;
update_sample(&angle_meas, angle_meas_new);
}
}
if(bus == 0) {
if(addr == (tesla_powertrain ? 0x116 : 0x118)) {
// Vehicle speed: ((0.05 * val) - 25) * MPH_TO_MPS
float speed = (((((GET_BYTE(to_push, 3) & 0x0FU) << 8) | (GET_BYTE(to_push, 2))) * 0.05) - 25) * 0.447;
vehicle_moving = ABS(speed) > 0.1;
UPDATE_VEHICLE_SPEED(speed);
}
if(addr == (tesla_powertrain ? 0x106 : 0x108)) {
// Gas pressed
gas_pressed = (GET_BYTE(to_push, 6) != 0U);
}
if(addr == (tesla_powertrain ? 0x1f8 : 0x20a)) {
// Brake pressed
brake_pressed = (((GET_BYTE(to_push, 0) & 0x0CU) >> 2) != 1U);
}
if(addr == (tesla_powertrain ? 0x256 : 0x368)) {
// Cruise state
int cruise_state = (GET_BYTE(to_push, 1) >> 4);
acc_main_on = (cruise_state == 1) || // STANDBY
(cruise_state == 2) || // ENABLED
(cruise_state == 3) || // STANDSTILL
(cruise_state == 4) || // OVERRIDE
(cruise_state == 6) || // PRE_FAULT
(cruise_state == 7); // PRE_CANCEL
bool cruise_engaged = (cruise_state == 2) || // ENABLED
(cruise_state == 3) || // STANDSTILL
(cruise_state == 4) || // OVERRIDE
(cruise_state == 6) || // PRE_FAULT
(cruise_state == 7); // PRE_CANCEL
pcm_cruise_check(cruise_engaged);
}
}
if (bus == 2) {
int das_control_addr = (tesla_powertrain ? 0x2bf : 0x2b9);
if (tesla_longitudinal && (addr == das_control_addr)) {
// "AEB_ACTIVE"
tesla_stock_aeb = ((GET_BYTE(to_push, 2) & 0x03U) == 1U);
}
}
if (tesla_powertrain) {
// 0x2bf: DAS_control should not be received on bus 0
generic_rx_checks((addr == 0x2bf) && (bus == 0));
} else {
// 0x488: DAS_steeringControl should not be received on bus 0
generic_rx_checks((addr == 0x488) && (bus == 0));
}
}
static bool tesla_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);
bool violation = false;
if(!tesla_powertrain && (addr == 0x488)) {
// Steering control: (0.1 * val) - 1638.35 in deg.
// We use 1/10 deg as a unit here
int raw_angle_can = (((GET_BYTE(to_send, 0) & 0x7FU) << 8) | GET_BYTE(to_send, 1));
int desired_angle = raw_angle_can - 16384;
int steer_control_type = GET_BYTE(to_send, 2) >> 6;
bool steer_control_enabled = (steer_control_type != 0) && // NONE
(steer_control_type != 3); // DISABLED
if (steer_angle_cmd_checks(desired_angle, steer_control_enabled, TESLA_STEERING_LIMITS)) {
violation = true;
}
}
if (!tesla_powertrain && (addr == 0x45)) {
// No button other than cancel can be sent by us
int control_lever_status = (GET_BYTE(to_send, 0) & 0x3FU);
if (control_lever_status != 1) {
violation = true;
}
}
if(addr == (tesla_powertrain ? 0x2bf : 0x2b9)) {
// DAS_control: longitudinal control message
if (tesla_longitudinal) {
// No AEB events may be sent by openpilot
int aeb_event = GET_BYTE(to_send, 2) & 0x03U;
if (aeb_event != 0) {
violation = true;
}
// Don't send messages when the stock AEB system is active
if (tesla_stock_aeb) {
violation = true;
}
// Don't allow any acceleration limits above the safety limits
int raw_accel_max = ((GET_BYTE(to_send, 6) & 0x1FU) << 4) | (GET_BYTE(to_send, 5) >> 4);
int raw_accel_min = ((GET_BYTE(to_send, 5) & 0x0FU) << 5) | (GET_BYTE(to_send, 4) >> 3);
violation |= longitudinal_accel_checks(raw_accel_max, TESLA_LONG_LIMITS);
violation |= longitudinal_accel_checks(raw_accel_min, TESLA_LONG_LIMITS);
} else {
violation = true;
}
}
if (violation) {
tx = false;
}
return tx;
}
static int tesla_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
if(bus_num == 0) {
// Chassis/PT to autopilot
bus_fwd = 2;
}
if(bus_num == 2) {
// Autopilot to chassis/PT
int das_control_addr = (tesla_powertrain ? 0x2bf : 0x2b9);
bool block_msg = false;
if (!tesla_powertrain && (addr == 0x488)) {
block_msg = true;
}
if (tesla_longitudinal && (addr == das_control_addr) && !tesla_stock_aeb) {
block_msg = true;
}
if(!block_msg) {
bus_fwd = 0;
}
}
return bus_fwd;
}
static safety_config tesla_init(uint16_t param) {
tesla_powertrain = GET_FLAG(param, TESLA_FLAG_POWERTRAIN);
tesla_longitudinal = GET_FLAG(param, TESLA_FLAG_LONGITUDINAL_CONTROL);
tesla_raven = GET_FLAG(param, TESLA_FLAG_RAVEN);
tesla_stock_aeb = false;
safety_config ret;
if (tesla_powertrain) {
ret = BUILD_SAFETY_CFG(tesla_pt_rx_checks, TESLA_PT_TX_MSGS);
} else if (tesla_raven) {
ret = BUILD_SAFETY_CFG(tesla_raven_rx_checks, TESLA_TX_MSGS);
} else {
ret = BUILD_SAFETY_CFG(tesla_rx_checks, TESLA_TX_MSGS);
}
return ret;
}
const safety_hooks tesla_hooks = {
.init = tesla_init,
.rx = tesla_rx_hook,
.tx = tesla_tx_hook,
.fwd = tesla_fwd_hook,
};

434
panda/board/safety/safety_toyota.h Normal file
View File

@@ -0,0 +1,434 @@
const SteeringLimits TOYOTA_STEERING_LIMITS = {
.max_steer = 1500,
.max_rate_up = 15, // ramp up slow
.max_rate_down = 25, // ramp down fast
.max_torque_error = 350, // max torque cmd in excess of motor torque
.max_rt_delta = 450, // the real time limit is 1800/sec, a 20% buffer
.max_rt_interval = 250000,
.type = TorqueMotorLimited,
// the EPS faults when the steering angle rate is above a certain threshold for too long. to prevent this,
// we allow setting STEER_REQUEST bit to 0 while maintaining the requested torque value for a single frame
.min_valid_request_frames = 18,
.max_invalid_request_frames = 1,
.min_valid_request_rt_interval = 170000, // 170ms; a ~10% buffer on cutting every 19 frames
.has_steer_req_tolerance = true,
// LTA angle limits
// factor for STEER_TORQUE_SENSOR->STEER_ANGLE and STEERING_LTA->STEER_ANGLE_CMD (1 / 0.0573)
.angle_deg_to_can = 17.452007,
.angle_rate_up_lookup = {
{5., 25., 25.},
{0.3, 0.15, 0.15}
},
.angle_rate_down_lookup = {
{5., 25., 25.},
{0.36, 0.26, 0.26}
},
};
const int TOYOTA_LTA_MAX_ANGLE = 1657; // EPS only accepts up to 94.9461
const int TOYOTA_LTA_MAX_MEAS_TORQUE = 1500;
const int TOYOTA_LTA_MAX_DRIVER_TORQUE = 150;
// longitudinal limits
const LongitudinalLimits TOYOTA_LONG_LIMITS = {
.max_accel = 2000, // 2.0 m/s2
.min_accel = -3500, // -3.5 m/s2
};
const LongitudinalLimits TOYOTA_LONG_LIMITS_SPORT = {
.max_accel = 4000, // 4.0 m/s2
.min_accel = -3500, // -3.5 m/s2
};
// panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches
// If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state
// Threshold calculated from DBC gains: round((((15 + 75.555) / 0.159375) + ((15 + 151.111) / 0.159375)) / 2) = 805
const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805;
#define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks
// Stock longitudinal
#define TOYOTA_COMMON_TX_MSGS \
{0x2E4, 0, 5}, {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + LTA + ACC & PCM cancel cmds */ \
#define TOYOTA_COMMON_LONG_TX_MSGS \
TOYOTA_COMMON_TX_MSGS \
{0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8}, /* DSU bus 0 */ \
{0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4}, /* DSU bus 1 */ \
{0x411, 0, 8}, /* PCS_HUD */ \
{0x750, 0, 8}, /* radar diagnostic address */ \
{0x1D3, 0, 8}, \
const CanMsg TOYOTA_TX_MSGS[] = {
TOYOTA_COMMON_TX_MSGS
};
const CanMsg TOYOTA_LONG_TX_MSGS[] = {
TOYOTA_COMMON_LONG_TX_MSGS
};
const CanMsg TOYOTA_INTERCEPTOR_TX_MSGS[] = {
TOYOTA_COMMON_LONG_TX_MSGS
{0x200, 0, 6}, // gas interceptor
};
#define TOYOTA_COMMON_RX_CHECKS(lta) \
{.msg = {{ 0xaa, 0, 8, .check_checksum = false, .frequency = 83U}, { 0 }, { 0 }}}, \
{.msg = {{0x260, 0, 8, .check_checksum = true, .quality_flag = (lta), .frequency = 50U}, { 0 }, { 0 }}}, \
{.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, { 0 }, { 0 }}}, \
{.msg = {{0x1D3, 0, 8, .check_checksum = true, .frequency = 33U}, { 0 }, { 0 }}}, \
{.msg = {{0x224, 0, 8, .check_checksum = false, .frequency = 40U}, \
{0x226, 0, 8, .check_checksum = false, .frequency = 40U}, { 0 }}}, \
RxCheck toyota_lka_rx_checks[] = {
TOYOTA_COMMON_RX_CHECKS(false)
};
RxCheck toyota_lka_interceptor_rx_checks[] = {
TOYOTA_COMMON_RX_CHECKS(false)
{.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
};
// Check the quality flag for angle measurement when using LTA, since it's not set on TSS-P cars
RxCheck toyota_lta_rx_checks[] = {
TOYOTA_COMMON_RX_CHECKS(true)
};
RxCheck toyota_lta_interceptor_rx_checks[] = {
TOYOTA_COMMON_RX_CHECKS(true)
{.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
};
// safety param flags
// first byte is for EPS factor, second is for flags
const uint32_t TOYOTA_PARAM_OFFSET = 8U;
const uint32_t TOYOTA_EPS_FACTOR = (1UL << TOYOTA_PARAM_OFFSET) - 1U;
const uint32_t TOYOTA_PARAM_ALT_BRAKE = 1UL << TOYOTA_PARAM_OFFSET;
const uint32_t TOYOTA_PARAM_STOCK_LONGITUDINAL = 2UL << TOYOTA_PARAM_OFFSET;
const uint32_t TOYOTA_PARAM_LTA = 4UL << TOYOTA_PARAM_OFFSET;
const uint32_t TOYOTA_PARAM_GAS_INTERCEPTOR = 8UL << TOYOTA_PARAM_OFFSET;
bool toyota_alt_brake = false;
bool toyota_stock_longitudinal = false;
bool toyota_lta = false;
int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
static uint32_t toyota_compute_checksum(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);
uint8_t checksum = (uint8_t)(addr) + (uint8_t)((unsigned int)(addr) >> 8U) + (uint8_t)(len);
for (int i = 0; i < (len - 1); i++) {
checksum += (uint8_t)GET_BYTE(to_push, i);
}
return checksum;
}
static uint32_t toyota_get_checksum(const CANPacket_t *to_push) {
int checksum_byte = GET_LEN(to_push) - 1U;
return (uint8_t)(GET_BYTE(to_push, checksum_byte));
}
static uint8_t toyota_get_counter(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
uint8_t cnt = 0U;
if (addr == 0x201) {
// Signal: COUNTER_PEDAL
cnt = GET_BYTE(to_push, 4) & 0x0FU;
}
return cnt;
}
static bool toyota_get_quality_flag_valid(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
bool valid = false;
if (addr == 0x260) {
valid = !GET_BIT(to_push, 3U); // STEER_ANGLE_INITIALIZING
}
return valid;
}
static void toyota_rx_hook(const CANPacket_t *to_push) {
if (GET_BUS(to_push) == 0U) {
int addr = GET_ADDR(to_push);
// get eps motor torque (0.66 factor in dbc)
if (addr == 0x260) {
int torque_meas_new = (GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6);
torque_meas_new = to_signed(torque_meas_new, 16);
// scale by dbc_factor
torque_meas_new = (torque_meas_new * toyota_dbc_eps_torque_factor) / 100;
// update array of sample
update_sample(&torque_meas, torque_meas_new);
// increase torque_meas by 1 to be conservative on rounding
torque_meas.min--;
torque_meas.max++;
// driver torque for angle limiting
int torque_driver_new = (GET_BYTE(to_push, 1) << 8) | GET_BYTE(to_push, 2);
torque_driver_new = to_signed(torque_driver_new, 16);
update_sample(&torque_driver, torque_driver_new);
// LTA request angle should match current angle while inactive, clipped to max accepted angle.
// note that angle can be relative to init angle on some TSS2 platforms, LTA has the same offset
bool steer_angle_initializing = GET_BIT(to_push, 3U);
if (!steer_angle_initializing) {
int angle_meas_new = (GET_BYTE(to_push, 3) << 8U) | GET_BYTE(to_push, 4);
angle_meas_new = CLAMP(to_signed(angle_meas_new, 16), -TOYOTA_LTA_MAX_ANGLE, TOYOTA_LTA_MAX_ANGLE);
update_sample(&angle_meas, angle_meas_new);
}
}
if (addr == 0x1D3) {
acc_main_on = GET_BIT(to_push, 15U);
}
// enter controls on rising edge of ACC, exit controls on ACC off
// exit controls on rising edge of gas press
if (addr == 0x1D2) {
// 5th bit is CRUISE_ACTIVE
bool cruise_engaged = GET_BIT(to_push, 5U);
pcm_cruise_check(cruise_engaged);
// sample gas pedal
if (!enable_gas_interceptor) {
gas_pressed = !GET_BIT(to_push, 4U);
}
}
// sample speed
if (addr == 0xaa) {
int speed = 0;
// sum 4 wheel speeds. conversion: raw * 0.01 - 67.67
for (uint8_t i = 0U; i < 8U; i += 2U) {
int wheel_speed = (GET_BYTE(to_push, i) << 8U) | GET_BYTE(to_push, (i + 1U));
speed += wheel_speed - 6767;
}
// check that all wheel speeds are at zero value
vehicle_moving = speed != 0;
UPDATE_VEHICLE_SPEED(speed / 4.0 * 0.01 / 3.6);
}
// most cars have brake_pressed on 0x226, corolla and rav4 on 0x224
if (((addr == 0x224) && toyota_alt_brake) || ((addr == 0x226) && !toyota_alt_brake)) {
uint8_t bit = (addr == 0x224) ? 5U : 37U;
brake_pressed = GET_BIT(to_push, bit);
}
// sample gas interceptor
if ((addr == 0x201) && enable_gas_interceptor) {
int gas_interceptor = TOYOTA_GET_INTERCEPTOR(to_push);
gas_pressed = gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD;
// TODO: remove this, only left in for gas_interceptor_prev test
gas_interceptor_prev = gas_interceptor;
}
bool stock_ecu_detected = addr == 0x2E4; // STEERING_LKA
if (!toyota_stock_longitudinal && (addr == 0x343)) {
stock_ecu_detected = true; // ACC_CONTROL
}
generic_rx_checks(stock_ecu_detected);
}
}
static bool toyota_tx_hook(const CANPacket_t *to_send) {
sport_mode = alternative_experience & ALT_EXP_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX;
bool tx = true;
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
// Check if msg is sent on BUS 0
if (bus == 0) {
// GAS PEDAL: safety check
if (addr == 0x200) {
if (longitudinal_interceptor_checks(to_send)) {
tx = false;
}
}
// ACCEL: safety check on byte 1-2
if (addr == 0x343) {
int desired_accel = (GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1);
desired_accel = to_signed(desired_accel, 16);
bool violation = false;
if (sport_mode) {
violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS_SPORT);
} else {
violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS);
}
// only ACC messages that cancel are allowed when openpilot is not controlling longitudinal
if (toyota_stock_longitudinal) {
bool cancel_req = GET_BIT(to_send, 24U);
if (!cancel_req) {
violation = true;
}
if (desired_accel != TOYOTA_LONG_LIMITS.inactive_accel) {
violation = true;
}
}
if (violation) {
tx = false;
}
}
// AEB: block all actuation. only used when DSU is unplugged
if (addr == 0x283) {
// only allow the checksum, which is the last byte
bool block = (GET_BYTES(to_send, 0, 4) != 0U) || (GET_BYTE(to_send, 4) != 0U) || (GET_BYTE(to_send, 5) != 0U);
if (block) {
tx = false;
}
}
// LTA angle steering check
if (addr == 0x191) {
// check the STEER_REQUEST, STEER_REQUEST_2, TORQUE_WIND_DOWN, STEER_ANGLE_CMD signals
bool lta_request = GET_BIT(to_send, 0U);
bool lta_request2 = GET_BIT(to_send, 25U);
int torque_wind_down = GET_BYTE(to_send, 5);
int lta_angle = (GET_BYTE(to_send, 1) << 8) | GET_BYTE(to_send, 2);
lta_angle = to_signed(lta_angle, 16);
bool steer_control_enabled = lta_request || lta_request2;
if (!toyota_lta) {
// using torque (LKA), block LTA msgs with actuation requests
if (steer_control_enabled || (lta_angle != 0) || (torque_wind_down != 0)) {
tx = false;
}
} else {
// check angle rate limits and inactive angle
if (steer_angle_cmd_checks(lta_angle, steer_control_enabled, TOYOTA_STEERING_LIMITS)) {
tx = false;
}
if (lta_request != lta_request2) {
tx = false;
}
// TORQUE_WIND_DOWN is gated on steer request
if (!steer_control_enabled && (torque_wind_down != 0)) {
tx = false;
}
// TORQUE_WIND_DOWN can only be no or full torque
if ((torque_wind_down != 0) && (torque_wind_down != 100)) {
tx = false;
}
// check if we should wind down torque
int driver_torque = MIN(ABS(torque_driver.min), ABS(torque_driver.max));
if ((driver_torque > TOYOTA_LTA_MAX_DRIVER_TORQUE) && (torque_wind_down != 0)) {
tx = false;
}
int eps_torque = MIN(ABS(torque_meas.min), ABS(torque_meas.max));
if ((eps_torque > TOYOTA_LTA_MAX_MEAS_TORQUE) && (torque_wind_down != 0)) {
tx = false;
}
}
}
// STEER: safety check on bytes 2-3
if (addr == 0x2E4) {
int desired_torque = (GET_BYTE(to_send, 1) << 8) | GET_BYTE(to_send, 2);
desired_torque = to_signed(desired_torque, 16);
bool steer_req = GET_BIT(to_send, 0U);
// When using LTA (angle control), assert no actuation on LKA message
if (!toyota_lta) {
if (steer_torque_cmd_checks(desired_torque, steer_req, TOYOTA_STEERING_LIMITS)) {
tx = false;
}
} else {
if ((desired_torque != 0) || steer_req) {
tx = false;
}
}
}
}
// UDS: Only tester present ("\x0F\x02\x3E\x00\x00\x00\x00\x00") allowed on diagnostics address
if (addr == 0x750) {
// this address is sub-addressed. only allow tester present to radar (0xF)
bool invalid_uds_msg = (GET_BYTES(to_send, 0, 4) != 0x003E020FU) || (GET_BYTES(to_send, 4, 4) != 0x0U);
if (invalid_uds_msg) {
tx = 0;
}
}
return tx;
}
static safety_config toyota_init(uint16_t param) {
toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE);
toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL);
toyota_lta = GET_FLAG(param, TOYOTA_PARAM_LTA);
enable_gas_interceptor = GET_FLAG(param, TOYOTA_PARAM_GAS_INTERCEPTOR);
toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR;
// Gas interceptor should not be used if openpilot is not controlling longitudinal
if (toyota_stock_longitudinal) {
enable_gas_interceptor = false;
}
safety_config ret;
if (toyota_stock_longitudinal) {
SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
} else {
enable_gas_interceptor ? SET_TX_MSGS(TOYOTA_INTERCEPTOR_TX_MSGS, ret) : \
SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret);
}
if (enable_gas_interceptor) {
toyota_lta ? SET_RX_CHECKS(toyota_lta_interceptor_rx_checks, ret) : \
SET_RX_CHECKS(toyota_lka_interceptor_rx_checks, ret);
} else {
toyota_lta ? SET_RX_CHECKS(toyota_lta_rx_checks, ret) : \
SET_RX_CHECKS(toyota_lka_rx_checks, ret);
}
return ret;
}
static int toyota_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
if (bus_num == 0) {
bus_fwd = 2;
}
if (bus_num == 2) {
// block stock lkas messages and stock acc messages (if OP is doing ACC)
// in TSS2, 0x191 is LTA which we need to block to avoid controls collision
bool is_lkas_msg = ((addr == 0x2E4) || (addr == 0x412) || (addr == 0x191));
// in TSS2 the camera does ACC as well, so filter 0x343
bool is_acc_msg = (addr == 0x343);
bool block_msg = is_lkas_msg || (is_acc_msg && !toyota_stock_longitudinal);
if (!block_msg) {
bus_fwd = 0;
}
}
return bus_fwd;
}
const safety_hooks toyota_hooks = {
.init = toyota_init,
.rx = toyota_rx_hook,
.tx = toyota_tx_hook,
.fwd = toyota_fwd_hook,
.get_checksum = toyota_get_checksum,
.compute_checksum = toyota_compute_checksum,
.get_counter = toyota_get_counter,
.get_quality_flag_valid = toyota_get_quality_flag_valid,
};

10
panda/board/safety/safety_volkswagen_common.h Normal file
View File

@@ -0,0 +1,10 @@
#ifndef SAFETY_VOLKSWAGEN_COMMON_H
#define SAFETY_VOLKSWAGEN_COMMON_H
const uint16_t FLAG_VOLKSWAGEN_LONG_CONTROL = 1;
bool volkswagen_longitudinal = false;
bool volkswagen_set_button_prev = false;
bool volkswagen_resume_button_prev = false;
#endif

312
panda/board/safety/safety_volkswagen_mqb.h Normal file
View File

@@ -0,0 +1,312 @@
#include "safety_volkswagen_common.h"
// lateral limits
const SteeringLimits VOLKSWAGEN_MQB_STEERING_LIMITS = {
.max_steer = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated)
.max_rt_delta = 75, // 4 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 50 ; 50 * 1.5 for safety pad = 75
.max_rt_interval = 250000, // 250ms between real time checks
.max_rate_up = 4, // 2.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
.max_rate_down = 10, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
.driver_torque_allowance = 80,
.driver_torque_factor = 3,
.type = TorqueDriverLimited,
};
// longitudinal limits
// acceleration in m/s2 * 1000 to avoid floating point math
const LongitudinalLimits VOLKSWAGEN_MQB_LONG_LIMITS = {
.max_accel = 2000,
.min_accel = -3500,
.inactive_accel = 3010, // VW sends one increment above the max range when inactive
};
const LongitudinalLimits VOLKSWAGEN_MQB_LONG_LIMITS_SPORT = {
.max_accel = 4000,
.min_accel = -3500,
.inactive_accel = 3010, // VW sends one increment above the max range when inactive
};
#define MSG_ESP_19 0x0B2 // RX from ABS, for wheel speeds
#define MSG_LH_EPS_03 0x09F // RX from EPS, for driver steering torque
#define MSG_ESP_05 0x106 // RX from ABS, for brake switch state
#define MSG_TSK_06 0x120 // RX from ECU, for ACC status from drivetrain coordinator
#define MSG_MOTOR_20 0x121 // RX from ECU, for driver throttle input
#define MSG_ACC_06 0x122 // TX by OP, ACC control instructions to the drivetrain coordinator
#define MSG_HCA_01 0x126 // TX by OP, Heading Control Assist steering torque
#define MSG_GRA_ACC_01 0x12B // TX by OP, ACC control buttons for cancel/resume
#define MSG_ACC_07 0x12E // TX by OP, ACC control instructions to the drivetrain coordinator
#define MSG_ACC_02 0x30C // TX by OP, ACC HUD data to the instrument cluster
#define MSG_MOTOR_14 0x3BE // RX from ECU, for brake switch status
#define MSG_LDW_02 0x397 // TX by OP, Lane line recognition and text alerts
// Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
const CanMsg VOLKSWAGEN_MQB_STOCK_TX_MSGS[] = {{MSG_HCA_01, 0, 8}, {MSG_GRA_ACC_01, 0, 8}, {MSG_GRA_ACC_01, 2, 8},
{MSG_LDW_02, 0, 8}, {MSG_LH_EPS_03, 2, 8}};
const CanMsg VOLKSWAGEN_MQB_LONG_TX_MSGS[] = {{MSG_HCA_01, 0, 8}, {MSG_LDW_02, 0, 8}, {MSG_LH_EPS_03, 2, 8},
{MSG_ACC_02, 0, 8}, {MSG_ACC_06, 0, 8}, {MSG_ACC_07, 0, 8}};
RxCheck volkswagen_mqb_rx_checks[] = {
{.msg = {{MSG_ESP_19, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{MSG_LH_EPS_03, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{MSG_ESP_05, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{MSG_TSK_06, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_20, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_14, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 10U}, { 0 }, { 0 }}},
{.msg = {{MSG_GRA_ACC_01, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 33U}, { 0 }, { 0 }}},
};
uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR
bool volkswagen_mqb_brake_pedal_switch = false;
bool volkswagen_mqb_brake_pressure_detected = false;
static uint32_t volkswagen_mqb_get_checksum(const CANPacket_t *to_push) {
return (uint8_t)GET_BYTE(to_push, 0);
}
static uint8_t volkswagen_mqb_get_counter(const CANPacket_t *to_push) {
// MQB message counters are consistently found at LSB 8.
return (uint8_t)GET_BYTE(to_push, 1) & 0xFU;
}
static uint32_t volkswagen_mqb_compute_crc(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);
// This is CRC-8H2F/AUTOSAR with a twist. See the OpenDBC implementation
// of this algorithm for a version with explanatory comments.
uint8_t crc = 0xFFU;
for (int i = 1; i < len; i++) {
crc ^= (uint8_t)GET_BYTE(to_push, i);
crc = volkswagen_crc8_lut_8h2f[crc];
}
uint8_t counter = volkswagen_mqb_get_counter(to_push);
if (addr == MSG_LH_EPS_03) {
crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter];
} else if (addr == MSG_ESP_05) {
crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter];
} else if (addr == MSG_TSK_06) {
crc ^= (uint8_t[]){0xC4,0xE2,0x4F,0xE4,0xF8,0x2F,0x56,0x81,0x9F,0xE5,0x83,0x44,0x05,0x3F,0x97,0xDF}[counter];
} else if (addr == MSG_MOTOR_20) {
crc ^= (uint8_t[]){0xE9,0x65,0xAE,0x6B,0x7B,0x35,0xE5,0x5F,0x4E,0xC7,0x86,0xA2,0xBB,0xDD,0xEB,0xB4}[counter];
} else if (addr == MSG_GRA_ACC_01) {
crc ^= (uint8_t[]){0x6A,0x38,0xB4,0x27,0x22,0xEF,0xE1,0xBB,0xF8,0x80,0x84,0x49,0xC7,0x9E,0x1E,0x2B}[counter];
} else {
// Undefined CAN message, CRC check expected to fail
}
crc = volkswagen_crc8_lut_8h2f[crc];
return (uint8_t)(crc ^ 0xFFU);
}
static safety_config volkswagen_mqb_init(uint16_t param) {
UNUSED(param);
volkswagen_set_button_prev = false;
volkswagen_resume_button_prev = false;
volkswagen_mqb_brake_pedal_switch = false;
volkswagen_mqb_brake_pressure_detected = false;
#ifdef ALLOW_DEBUG
volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL);
#endif
gen_crc_lookup_table_8(0x2F, volkswagen_crc8_lut_8h2f);
return volkswagen_longitudinal ? BUILD_SAFETY_CFG(volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_STOCK_TX_MSGS);
}
static void volkswagen_mqb_rx_hook(const CANPacket_t *to_push) {
if (GET_BUS(to_push) == 0U) {
int addr = GET_ADDR(to_push);
// Update in-motion state by sampling wheel speeds
if (addr == MSG_ESP_19) {
// sum 4 wheel speeds
int speed = 0;
for (uint8_t i = 0U; i < 8U; i += 2U) {
int wheel_speed = GET_BYTE(to_push, i) | (GET_BYTE(to_push, i + 1U) << 8);
speed += wheel_speed;
}
// Check all wheel speeds for any movement
vehicle_moving = speed > 0;
}
// Update driver input torque samples
// Signal: LH_EPS_03.EPS_Lenkmoment (absolute torque)
// Signal: LH_EPS_03.EPS_VZ_Lenkmoment (direction)
if (addr == MSG_LH_EPS_03) {
int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1FU) << 8);
int sign = (GET_BYTE(to_push, 6) & 0x80U) >> 7;
if (sign == 1) {
torque_driver_new *= -1;
}
update_sample(&torque_driver, torque_driver_new);
}
if (addr == MSG_TSK_06) {
// When using stock ACC, enter controls on rising edge of stock ACC engage, exit on disengage
// Always exit controls on main switch off
// Signal: TSK_06.TSK_Status
int acc_status = (GET_BYTE(to_push, 3) & 0x7U);
bool cruise_engaged = (acc_status == 3) || (acc_status == 4) || (acc_status == 5);
acc_main_on = cruise_engaged || (acc_status == 2);
if (!volkswagen_longitudinal) {
pcm_cruise_check(cruise_engaged);
}
if (!acc_main_on) {
controls_allowed = false;
}
}
if (addr == MSG_GRA_ACC_01) {
// If using openpilot longitudinal, enter controls on falling edge of Set or Resume with main switch on
// Signal: GRA_ACC_01.GRA_Tip_Setzen
// Signal: GRA_ACC_01.GRA_Tip_Wiederaufnahme
if (volkswagen_longitudinal) {
bool set_button = GET_BIT(to_push, 16U);
bool resume_button = GET_BIT(to_push, 19U);
if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) {
controls_allowed = acc_main_on;
}
volkswagen_set_button_prev = set_button;
volkswagen_resume_button_prev = resume_button;
}
// Always exit controls on rising edge of Cancel
// Signal: GRA_ACC_01.GRA_Abbrechen
if (GET_BIT(to_push, 13U)) {
controls_allowed = false;
}
}
// Signal: Motor_20.MO_Fahrpedalrohwert_01
if (addr == MSG_MOTOR_20) {
gas_pressed = ((GET_BYTES(to_push, 0, 4) >> 12) & 0xFFU) != 0U;
}
// Signal: Motor_14.MO_Fahrer_bremst (ECU detected brake pedal switch F63)
if (addr == MSG_MOTOR_14) {
volkswagen_mqb_brake_pedal_switch = (GET_BYTE(to_push, 3) & 0x10U) >> 4;
}
// Signal: ESP_05.ESP_Fahrer_bremst (ESP detected driver brake pressure above platform specified threshold)
if (addr == MSG_ESP_05) {
volkswagen_mqb_brake_pressure_detected = (GET_BYTE(to_push, 3) & 0x4U) >> 2;
}
brake_pressed = volkswagen_mqb_brake_pedal_switch || volkswagen_mqb_brake_pressure_detected;
generic_rx_checks((addr == MSG_HCA_01));
}
}
static bool volkswagen_mqb_tx_hook(const CANPacket_t *to_send) {
sport_mode = alternative_experience & ALT_EXP_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX;
int addr = GET_ADDR(to_send);
bool tx = true;
// Safety check for HCA_01 Heading Control Assist torque
// Signal: HCA_01.HCA_01_LM_Offset (absolute torque)
// Signal: HCA_01.HCA_01_LM_OffSign (direction)
if (addr == MSG_HCA_01) {
int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x1U) << 8);
bool sign = GET_BIT(to_send, 31U);
if (sign) {
desired_torque *= -1;
}
bool steer_req = GET_BIT(to_send, 30U);
if (steer_torque_cmd_checks(desired_torque, steer_req, VOLKSWAGEN_MQB_STEERING_LIMITS)) {
tx = false;
}
}
// Safety check for both ACC_06 and ACC_07 acceleration requests
// To avoid floating point math, scale upward and compare to pre-scaled safety m/s2 boundaries
if ((addr == MSG_ACC_06) || (addr == MSG_ACC_07)) {
bool violation = false;
int desired_accel = 0;
if (addr == MSG_ACC_06) {
// Signal: ACC_06.ACC_Sollbeschleunigung_02 (acceleration in m/s2, scale 0.005, offset -7.22)
desired_accel = ((((GET_BYTE(to_send, 4) & 0x7U) << 8) | GET_BYTE(to_send, 3)) * 5U) - 7220U;
} else {
// Signal: ACC_07.ACC_Folgebeschl (acceleration in m/s2, scale 0.03, offset -4.6)
int secondary_accel = (GET_BYTE(to_send, 4) * 30U) - 4600U;
violation |= (secondary_accel != 3020); // enforce always inactive (one increment above max range) at this time
// Signal: ACC_07.ACC_Sollbeschleunigung_02 (acceleration in m/s2, scale 0.005, offset -7.22)
desired_accel = (((GET_BYTE(to_send, 7) << 3) | ((GET_BYTE(to_send, 6) & 0xE0U) >> 5)) * 5U) - 7220U;
}
if (sport_mode) {
if (desired_accel != 0) {
violation |= longitudinal_accel_checks(desired_accel, VOLKSWAGEN_MQB_LONG_LIMITS_SPORT);
}
} else {
violation |= longitudinal_accel_checks(desired_accel, VOLKSWAGEN_MQB_LONG_LIMITS);
}
if (violation) {
tx = false;
}
}
// FORCE CANCEL: ensuring that only the cancel button press is sent when controls are off.
// This avoids unintended engagements while still allowing resume spam
if ((addr == MSG_GRA_ACC_01) && !controls_allowed) {
// disallow resume and set: bits 16 and 19
if ((GET_BYTE(to_send, 2) & 0x9U) != 0U) {
tx = false;
}
}
return tx;
}
static int volkswagen_mqb_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
switch (bus_num) {
case 0:
if (addr == MSG_LH_EPS_03) {
// openpilot needs to replace apparent driver steering input torque to pacify VW Emergency Assist
bus_fwd = -1;
} else {
// Forward all remaining traffic from Extended CAN onward
bus_fwd = 2;
}
break;
case 2:
if ((addr == MSG_HCA_01) || (addr == MSG_LDW_02)) {
// openpilot takes over LKAS steering control and related HUD messages from the camera
bus_fwd = -1;
} else if (volkswagen_longitudinal && ((addr == MSG_ACC_02) || (addr == MSG_ACC_06) || (addr == MSG_ACC_07))) {
// openpilot takes over acceleration/braking control and related HUD messages from the stock ACC radar
bus_fwd = -1;
} else {
// Forward all remaining traffic from Extended CAN devices to J533 gateway
bus_fwd = 0;
}
break;
default:
// No other buses should be in use; fallback to do-not-forward
bus_fwd = -1;
break;
}
return bus_fwd;
}
const safety_hooks volkswagen_mqb_hooks = {
.init = volkswagen_mqb_init,
.rx = volkswagen_mqb_rx_hook,
.tx = volkswagen_mqb_tx_hook,
.fwd = volkswagen_mqb_fwd_hook,
.get_counter = volkswagen_mqb_get_counter,
.get_checksum = volkswagen_mqb_get_checksum,
.compute_checksum = volkswagen_mqb_compute_crc,
};

269
panda/board/safety/safety_volkswagen_pq.h Normal file
View File

@@ -0,0 +1,269 @@
#include "safety_volkswagen_common.h"
// lateral limits
const SteeringLimits VOLKSWAGEN_PQ_STEERING_LIMITS = {
.max_steer = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated)
.max_rt_delta = 113, // 6 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 75 ; 125 * 1.5 for safety pad = 113
.max_rt_interval = 250000, // 250ms between real time checks
.max_rate_up = 6, // 3.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
.max_rate_down = 10, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
.driver_torque_factor = 3,
.driver_torque_allowance = 80,
.type = TorqueDriverLimited,
};
// longitudinal limits
// acceleration in m/s2 * 1000 to avoid floating point math
const LongitudinalLimits VOLKSWAGEN_PQ_LONG_LIMITS = {
.max_accel = 2000,
.min_accel = -3500,
.inactive_accel = 3010, // VW sends one increment above the max range when inactive
};
const LongitudinalLimits VOLKSWAGEN_PQ_LONG_LIMITS_SPORT = {
.max_accel = 4000,
.min_accel = -3500,
.inactive_accel = 3010, // VW sends one increment above the max range when inactive
};
#define MSG_LENKHILFE_3 0x0D0 // RX from EPS, for steering angle and driver steering torque
#define MSG_HCA_1 0x0D2 // TX by OP, Heading Control Assist steering torque
#define MSG_BREMSE_1 0x1A0 // RX from ABS, for ego speed
#define MSG_MOTOR_2 0x288 // RX from ECU, for CC state and brake switch state
#define MSG_ACC_SYSTEM 0x368 // TX by OP, longitudinal acceleration controls
#define MSG_MOTOR_3 0x380 // RX from ECU, for driver throttle input
#define MSG_GRA_NEU 0x38A // TX by OP, ACC control buttons for cancel/resume
#define MSG_MOTOR_5 0x480 // RX from ECU, for ACC main switch state
#define MSG_ACC_GRA_ANZEIGE 0x56A // TX by OP, ACC HUD
#define MSG_LDW_1 0x5BE // TX by OP, Lane line recognition and text alerts
// Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
const CanMsg VOLKSWAGEN_PQ_STOCK_TX_MSGS[] = {{MSG_HCA_1, 0, 5}, {MSG_LDW_1, 0, 8},
{MSG_GRA_NEU, 0, 4}, {MSG_GRA_NEU, 2, 4}};
const CanMsg VOLKSWAGEN_PQ_LONG_TX_MSGS[] = {{MSG_HCA_1, 0, 5}, {MSG_LDW_1, 0, 8},
{MSG_ACC_SYSTEM, 0, 8}, {MSG_ACC_GRA_ANZEIGE, 0, 8}};
RxCheck volkswagen_pq_rx_checks[] = {
{.msg = {{MSG_LENKHILFE_3, 0, 6, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{MSG_BREMSE_1, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_2, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_3, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{MSG_MOTOR_5, 0, 8, .check_checksum = true, .max_counter = 0U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{MSG_GRA_NEU, 0, 4, .check_checksum = true, .max_counter = 15U, .frequency = 30U}, { 0 }, { 0 }}},
};
static uint32_t volkswagen_pq_get_checksum(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
return (uint32_t)GET_BYTE(to_push, (addr == MSG_MOTOR_5) ? 7 : 0);
}
static uint8_t volkswagen_pq_get_counter(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
uint8_t counter = 0U;
if (addr == MSG_LENKHILFE_3) {
counter = (uint8_t)(GET_BYTE(to_push, 1) & 0xF0U) >> 4;
} else if (addr == MSG_GRA_NEU) {
counter = (uint8_t)(GET_BYTE(to_push, 2) & 0xF0U) >> 4;
} else {
}
return counter;
}
static uint32_t volkswagen_pq_compute_checksum(const CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);
uint8_t checksum = 0U;
int checksum_byte = (addr == MSG_MOTOR_5) ? 7 : 0;
// Simple XOR over the payload, except for the byte where the checksum lives.
for (int i = 0; i < len; i++) {
if (i != checksum_byte) {
checksum ^= (uint8_t)GET_BYTE(to_push, i);
}
}
return checksum;
}
static safety_config volkswagen_pq_init(uint16_t param) {
UNUSED(param);
volkswagen_set_button_prev = false;
volkswagen_resume_button_prev = false;
#ifdef ALLOW_DEBUG
volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL);
#endif
return volkswagen_longitudinal ? BUILD_SAFETY_CFG(volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_STOCK_TX_MSGS);
}
static void volkswagen_pq_rx_hook(const CANPacket_t *to_push) {
if (GET_BUS(to_push) == 0U) {
int addr = GET_ADDR(to_push);
// Update in-motion state from speed value.
// Signal: Bremse_1.Geschwindigkeit_neu__Bremse_1_
if (addr == MSG_BREMSE_1) {
int speed = ((GET_BYTE(to_push, 2) & 0xFEU) >> 1) | (GET_BYTE(to_push, 3) << 7);
vehicle_moving = speed > 0;
}
// Update driver input torque samples
// Signal: Lenkhilfe_3.LH3_LM (absolute torque)
// Signal: Lenkhilfe_3.LH3_LMSign (direction)
if (addr == MSG_LENKHILFE_3) {
int torque_driver_new = GET_BYTE(to_push, 2) | ((GET_BYTE(to_push, 3) & 0x3U) << 8);
int sign = (GET_BYTE(to_push, 3) & 0x4U) >> 2;
if (sign == 1) {
torque_driver_new *= -1;
}
update_sample(&torque_driver, torque_driver_new);
}
if (volkswagen_longitudinal) {
if (addr == MSG_MOTOR_5) {
// ACC main switch on is a prerequisite to enter controls, exit controls immediately on main switch off
// Signal: Motor_5.GRA_Hauptschalter
acc_main_on = GET_BIT(to_push, 50U);
if (!acc_main_on) {
controls_allowed = false;
}
}
if (addr == MSG_GRA_NEU) {
// If ACC main switch is on, enter controls on falling edge of Set or Resume
// Signal: GRA_Neu.GRA_Neu_Setzen
// Signal: GRA_Neu.GRA_Neu_Recall
bool set_button = GET_BIT(to_push, 16U);
bool resume_button = GET_BIT(to_push, 17U);
if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) {
controls_allowed = acc_main_on;
}
volkswagen_set_button_prev = set_button;
volkswagen_resume_button_prev = resume_button;
// Exit controls on rising edge of Cancel, override Set/Resume if present simultaneously
// Signal: GRA_ACC_01.GRA_Abbrechen
if (GET_BIT(to_push, 9U)) {
controls_allowed = false;
}
}
} else {
if (addr == MSG_MOTOR_2) {
// Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages
// Signal: Motor_2.GRA_Status
int acc_status = (GET_BYTE(to_push, 2) & 0xC0U) >> 6;
bool cruise_engaged = (acc_status == 1) || (acc_status == 2);
pcm_cruise_check(cruise_engaged);
}
}
// Signal: Motor_3.Fahrpedal_Rohsignal
if (addr == MSG_MOTOR_3) {
gas_pressed = (GET_BYTE(to_push, 2));
}
// Signal: Motor_2.Bremslichtschalter
if (addr == MSG_MOTOR_2) {
brake_pressed = (GET_BYTE(to_push, 2) & 0x1U);
}
generic_rx_checks((addr == MSG_HCA_1));
}
}
static bool volkswagen_pq_tx_hook(const CANPacket_t *to_send) {
sport_mode = alternative_experience & ALT_EXP_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX;
int addr = GET_ADDR(to_send);
bool tx = true;
// Safety check for HCA_1 Heading Control Assist torque
// Signal: HCA_1.LM_Offset (absolute torque)
// Signal: HCA_1.LM_Offsign (direction)
if (addr == MSG_HCA_1) {
int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x7FU) << 8);
desired_torque = desired_torque / 32; // DBC scale from PQ network to centi-Nm
int sign = (GET_BYTE(to_send, 3) & 0x80U) >> 7;
if (sign == 1) {
desired_torque *= -1;
}
uint32_t hca_status = ((GET_BYTE(to_send, 1) >> 4) & 0xFU);
bool steer_req = ((hca_status == 5U) || (hca_status == 7U));
if (steer_torque_cmd_checks(desired_torque, steer_req, VOLKSWAGEN_PQ_STEERING_LIMITS)) {
tx = false;
}
}
// Safety check for acceleration commands
// To avoid floating point math, scale upward and compare to pre-scaled safety m/s2 boundaries
if (addr == MSG_ACC_SYSTEM) {
// Signal: ACC_System.ACS_Sollbeschl (acceleration in m/s2, scale 0.005, offset -7.22)
int desired_accel = ((((GET_BYTE(to_send, 4) & 0x7U) << 8) | GET_BYTE(to_send, 3)) * 5U) - 7220U;
if (sport_mode) {
if (longitudinal_accel_checks(desired_accel, VOLKSWAGEN_PQ_LONG_LIMITS_SPORT)) {
tx = false;
}
} else {
if (longitudinal_accel_checks(desired_accel, VOLKSWAGEN_PQ_LONG_LIMITS)) {
tx = false;
}
}
}
// FORCE CANCEL: ensuring that only the cancel button press is sent when controls are off.
// This avoids unintended engagements while still allowing resume spam
if ((addr == MSG_GRA_NEU) && !controls_allowed) {
// Signal: GRA_Neu.GRA_Neu_Setzen
// Signal: GRA_Neu.GRA_Neu_Recall
if (GET_BIT(to_send, 16U) || GET_BIT(to_send, 17U)) {
tx = false;
}
}
return tx;
}
static int volkswagen_pq_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
switch (bus_num) {
case 0:
// Forward all traffic from the Extended CAN onward
bus_fwd = 2;
break;
case 2:
if ((addr == MSG_HCA_1) || (addr == MSG_LDW_1)) {
// openpilot takes over LKAS steering control and related HUD messages from the camera
bus_fwd = -1;
} else if (volkswagen_longitudinal && ((addr == MSG_ACC_SYSTEM) || (addr == MSG_ACC_GRA_ANZEIGE))) {
// openpilot takes over acceleration/braking control and related HUD messages from the stock ACC radar
} else {
// Forward all remaining traffic from Extended CAN devices to J533 gateway
bus_fwd = 0;
}
break;
default:
// No other buses should be in use; fallback to do-not-forward
bus_fwd = -1;
break;
}
return bus_fwd;
}
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,
};