sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
@@ -0,0 +1,45 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
static void body_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->addr == 0x201U) {
|
||||
controls_allowed = true;
|
||||
}
|
||||
}
|
||||
|
||||
static bool body_tx_hook(const CANPacket_t *msg) {
|
||||
bool tx = true;
|
||||
|
||||
if (!controls_allowed && (msg->addr != 0x1U)) {
|
||||
tx = false;
|
||||
}
|
||||
|
||||
// Allow going into CAN flashing mode even if controls are not allowed
|
||||
bool flash_msg = (msg->addr == 0x250U) && (GET_LEN(msg) == 8U);
|
||||
if (!controls_allowed && (GET_BYTES(msg, 0, 4) == 0xdeadfaceU) && (GET_BYTES(msg, 4, 4) == 0x0ab00b1eU) && flash_msg) {
|
||||
tx = true;
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
static safety_config body_init(uint16_t param) {
|
||||
static RxCheck body_rx_checks[] = {
|
||||
{.msg = {{0x201, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
static const CanMsg BODY_TX_MSGS[] = {{0x250, 0, 8, .check_relay = false}, {0x250, 0, 6, .check_relay = false}, {0x251, 0, 5, .check_relay = false}, // body
|
||||
{0x1, 0, 8, .check_relay = false}}; // CAN flasher
|
||||
|
||||
SAFETY_UNUSED(param);
|
||||
safety_config ret = BUILD_SAFETY_CFG(body_rx_checks, BODY_TX_MSGS);
|
||||
ret.disable_forwarding = true;
|
||||
return ret;
|
||||
}
|
||||
|
||||
const safety_hooks body_hooks = {
|
||||
.init = body_init,
|
||||
.rx = body_rx_hook,
|
||||
.tx = body_tx_hook,
|
||||
};
|
||||
@@ -0,0 +1,247 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/chrysler_common.h"
|
||||
|
||||
// Chrysler Pacifica/Jeep addresses
|
||||
#define CHRYSLER_EPS_2 0x220 // EPS driver input torque
|
||||
#define CHRYSLER_ESP_1 0x140 // Brake pedal and vehicle speed
|
||||
#define CHRYSLER_ESP_8 0x11C // Brake pedal and vehicle speed
|
||||
#define CHRYSLER_ECM_5 0x22F // Throttle position sensor
|
||||
#define CHRYSLER_DAS_3 0x1F4 // ACC engagement states from DASM
|
||||
#define CHRYSLER_DAS_6 0x2A6 // LKAS HUD and auto headlight control from DASM
|
||||
#define CHRYSLER_LKAS_COMMAND 0x292 // LKAS controls from DASM
|
||||
#define CHRYSLER_CRUISE_BUTTONS 0x23B // Cruise control buttons
|
||||
#define CHRYSLER_LKAS_HEARTBIT 0x2D9 // LKAS HEARTBIT from DASM
|
||||
#define CHRYSLER_TRACTION_BUTTON 0x330 // Traction control button
|
||||
#define CHRYSLER_Center_Stack_2 0x000 // Placeholder, does not exist
|
||||
|
||||
// RAM DT addresses
|
||||
#define CHRYSLER_RAM_DT_EPS_2 0x31
|
||||
#define CHRYSLER_RAM_DT_ESP_1 0x83
|
||||
#define CHRYSLER_RAM_DT_ESP_8 0x79
|
||||
#define CHRYSLER_RAM_DT_ECM_5 0x9D
|
||||
#define CHRYSLER_RAM_DT_DAS_3 0x99
|
||||
#define CHRYSLER_RAM_DT_DAS_6 0xFA
|
||||
#define CHRYSLER_RAM_DT_LKAS_COMMAND 0xA6
|
||||
#define CHRYSLER_RAM_DT_CRUISE_BUTTONS 0xB1
|
||||
#define CHRYSLER_RAM_DT_LKAS_HEARTBIT 0x00 // Placeholder, does not exist
|
||||
#define CHRYSLER_RAM_DT_TRACTION_BUTTON 0x00 // Placeholder, does not exist
|
||||
#define CHRYSLER_RAM_DT_Center_Stack_2 0x28A
|
||||
|
||||
// RAM HD addresses
|
||||
#define CHRYSLER_RAM_HD_EPS_2 0x220
|
||||
#define CHRYSLER_RAM_HD_ESP_1 0x140
|
||||
#define CHRYSLER_RAM_HD_ESP_8 0x11C
|
||||
#define CHRYSLER_RAM_HD_ECM_5 0x22F
|
||||
#define CHRYSLER_RAM_HD_DAS_3 0x1F4
|
||||
#define CHRYSLER_RAM_HD_DAS_6 0x275
|
||||
#define CHRYSLER_RAM_HD_LKAS_COMMAND 0x276
|
||||
#define CHRYSLER_RAM_HD_CRUISE_BUTTONS 0x23A
|
||||
#define CHRYSLER_RAM_HD_LKAS_HEARTBIT 0x00 // Placeholder, does not exist
|
||||
#define CHRYSLER_RAM_HD_TRACTION_BUTTON 0x00 // Placeholder, does not exist
|
||||
#define CHRYSLER_RAM_HD_Center_Stack_2 0x28A
|
||||
|
||||
typedef enum {
|
||||
CHRYSLER_RAM_DT,
|
||||
CHRYSLER_RAM_HD,
|
||||
CHRYSLER_PACIFICA, // plus Jeep
|
||||
} ChryslerPlatform;
|
||||
static ChryslerPlatform chrysler_platform;
|
||||
|
||||
#define CHRYSLER_ADDR(name) ((uint32_t)((chrysler_platform == CHRYSLER_RAM_DT) ? CHRYSLER_RAM_DT_##name : \
|
||||
((chrysler_platform == CHRYSLER_RAM_HD) ? CHRYSLER_RAM_HD_##name : CHRYSLER_##name)))
|
||||
|
||||
|
||||
static uint8_t chrysler_get_counter(const CANPacket_t *msg) {
|
||||
return (uint8_t)(msg->data[6] >> 4);
|
||||
}
|
||||
|
||||
static void chrysler_rx_hook(const CANPacket_t *msg) {
|
||||
// Measured EPS torque
|
||||
if ((msg->bus == 0U) && (msg->addr == CHRYSLER_ADDR(EPS_2))) {
|
||||
int torque_meas_new = ((msg->data[4] & 0x7U) << 8) + msg->data[5] - 1024U;
|
||||
update_sample(&torque_meas, torque_meas_new);
|
||||
}
|
||||
|
||||
// enter controls on rising edge of ACC, exit controls on ACC off
|
||||
const unsigned int das_3_bus = (chrysler_platform == CHRYSLER_PACIFICA) ? 0U : 2U;
|
||||
if ((msg->bus == das_3_bus) && (msg->addr == CHRYSLER_ADDR(DAS_3))) {
|
||||
bool cruise_engaged = GET_BIT(msg, 21U);
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
acc_main_on = GET_BIT(msg, 20U);
|
||||
}
|
||||
|
||||
// TODO: use the same message for both
|
||||
// update vehicle moving
|
||||
if ((chrysler_platform != CHRYSLER_PACIFICA) && (msg->bus == 0U) && (msg->addr == CHRYSLER_ADDR(ESP_8))) {
|
||||
vehicle_moving = ((msg->data[4] << 8) + msg->data[5]) != 0U;
|
||||
}
|
||||
if ((chrysler_platform == CHRYSLER_PACIFICA) && (msg->bus == 0U) && (msg->addr == 514U)) {
|
||||
int speed_l = (msg->data[0] << 4) + (msg->data[1] >> 4);
|
||||
int speed_r = (msg->data[2] << 4) + (msg->data[3] >> 4);
|
||||
vehicle_moving = (speed_l != 0) || (speed_r != 0);
|
||||
}
|
||||
|
||||
// exit controls on rising edge of gas press
|
||||
if ((msg->bus == 0U) && (msg->addr == CHRYSLER_ADDR(ECM_5))) {
|
||||
gas_pressed = msg->data[0U] != 0U;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of brake press
|
||||
if ((msg->bus == 0U) && (msg->addr == CHRYSLER_ADDR(ESP_1))) {
|
||||
brake_pressed = ((msg->data[0U] & 0xFU) >> 2U) == 1U;
|
||||
}
|
||||
|
||||
if ((chrysler_platform == CHRYSLER_PACIFICA) && (msg->bus == 0U) && (msg->addr == CHRYSLER_ADDR(TRACTION_BUTTON))) {
|
||||
mads_button_press = GET_BIT(msg, 53U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
|
||||
}
|
||||
|
||||
if ((chrysler_platform != CHRYSLER_PACIFICA) && (msg->bus == 0U)) {
|
||||
if (msg->addr == CHRYSLER_ADDR(Center_Stack_2)) {
|
||||
mads_button_press = GET_BIT(msg, 57U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool chrysler_tx_hook(const CANPacket_t *msg) {
|
||||
const TorqueSteeringLimits CHRYSLER_STEERING_LIMITS = {
|
||||
.max_torque = 261,
|
||||
.max_rt_delta = 112,
|
||||
.max_rate_up = 3,
|
||||
.max_rate_down = 3,
|
||||
.max_torque_error = 80,
|
||||
.type = TorqueMotorLimited,
|
||||
};
|
||||
|
||||
const TorqueSteeringLimits CHRYSLER_RAM_DT_STEERING_LIMITS = {
|
||||
.max_torque = 350,
|
||||
.max_rt_delta = 112,
|
||||
.max_rate_up = 6,
|
||||
.max_rate_down = 6,
|
||||
.max_torque_error = 80,
|
||||
.type = TorqueMotorLimited,
|
||||
};
|
||||
|
||||
const TorqueSteeringLimits CHRYSLER_RAM_HD_STEERING_LIMITS = {
|
||||
.max_torque = 361,
|
||||
.max_rt_delta = 182,
|
||||
.max_rate_up = 14,
|
||||
.max_rate_down = 14,
|
||||
.max_torque_error = 80,
|
||||
.type = TorqueMotorLimited,
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
|
||||
// STEERING
|
||||
if (msg->addr == CHRYSLER_ADDR(LKAS_COMMAND)) {
|
||||
int start_byte = (chrysler_platform == CHRYSLER_PACIFICA) ? 0 : 1;
|
||||
int desired_torque = ((msg->data[start_byte] & 0x7U) << 8) | msg->data[start_byte + 1];
|
||||
desired_torque -= 1024;
|
||||
|
||||
const TorqueSteeringLimits 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(msg, 4U) : (msg->data[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 (msg->addr == CHRYSLER_ADDR(CRUISE_BUTTONS)) {
|
||||
const bool is_cancel = msg->data[0] == 1U;
|
||||
const bool is_accel = msg->data[0] == 0x04U;
|
||||
const bool is_decel = msg->data[0] == 0x08U;
|
||||
const bool is_resume = msg->data[0] == 0x10U;
|
||||
const bool allowed = is_cancel || ((is_resume || is_accel || is_decel) && controls_allowed);
|
||||
if (!allowed) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
static safety_config chrysler_init(uint16_t param) {
|
||||
const uint32_t CHRYSLER_PARAM_RAM_DT = 1U; // set for Ram DT platform
|
||||
|
||||
static RxCheck chrysler_ram_dt_rx_checks[] = {
|
||||
{.msg = {{CHRYSLER_RAM_DT_EPS_2, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{CHRYSLER_RAM_DT_ESP_1, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{CHRYSLER_RAM_DT_ESP_8, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{CHRYSLER_RAM_DT_ECM_5, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{CHRYSLER_RAM_DT_DAS_3, 2, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{CHRYSLER_RAM_DT_Center_Stack_2, 0, 8, 1U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .ignore_frequency_check = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
static RxCheck chrysler_rx_checks[] = {
|
||||
{.msg = {{CHRYSLER_EPS_2, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{CHRYSLER_ESP_1, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{514, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{CHRYSLER_ECM_5, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{CHRYSLER_DAS_3, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{CHRYSLER_TRACTION_BUTTON, 0, 8, 1U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .ignore_frequency_check = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
static const CanMsg CHRYSLER_TX_MSGS[] = {
|
||||
{CHRYSLER_CRUISE_BUTTONS, 0, 3, .check_relay = false},
|
||||
{CHRYSLER_LKAS_COMMAND, 0, 6, .check_relay = true},
|
||||
{CHRYSLER_DAS_6, 0, 8, .check_relay = true},
|
||||
{CHRYSLER_LKAS_HEARTBIT, 0, 5, .check_relay = true},
|
||||
};
|
||||
|
||||
static const CanMsg CHRYSLER_RAM_DT_TX_MSGS[] = {
|
||||
{CHRYSLER_RAM_DT_CRUISE_BUTTONS, 2, 3, .check_relay = false},
|
||||
{CHRYSLER_RAM_DT_LKAS_COMMAND, 0, 8, .check_relay = true},
|
||||
{CHRYSLER_RAM_DT_DAS_6, 0, 8, .check_relay = true},
|
||||
};
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
static RxCheck chrysler_ram_hd_rx_checks[] = {
|
||||
{.msg = {{CHRYSLER_RAM_HD_EPS_2, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{CHRYSLER_RAM_HD_ESP_1, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{CHRYSLER_RAM_HD_ESP_8, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{CHRYSLER_RAM_HD_ECM_5, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{CHRYSLER_RAM_HD_DAS_3, 2, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{CHRYSLER_RAM_HD_Center_Stack_2, 0, 8, 1U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .ignore_frequency_check = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
static const CanMsg CHRYSLER_RAM_HD_TX_MSGS[] = {
|
||||
{CHRYSLER_RAM_HD_CRUISE_BUTTONS, 2, 3, .check_relay = false},
|
||||
{CHRYSLER_RAM_HD_LKAS_COMMAND, 0, 8, .check_relay = true},
|
||||
{CHRYSLER_RAM_HD_DAS_6, 0, 8, .check_relay = true},
|
||||
};
|
||||
|
||||
const uint32_t CHRYSLER_PARAM_RAM_HD = 2U; // set for Ram HD platform
|
||||
bool enable_ram_hd = GET_FLAG(param, CHRYSLER_PARAM_RAM_HD);
|
||||
#endif
|
||||
|
||||
safety_config ret;
|
||||
|
||||
bool enable_ram_dt = GET_FLAG(param, CHRYSLER_PARAM_RAM_DT);
|
||||
|
||||
if (enable_ram_dt) {
|
||||
chrysler_platform = CHRYSLER_RAM_DT;
|
||||
ret = BUILD_SAFETY_CFG(chrysler_ram_dt_rx_checks, CHRYSLER_RAM_DT_TX_MSGS);
|
||||
#ifdef ALLOW_DEBUG
|
||||
} else if (enable_ram_hd) {
|
||||
chrysler_platform = CHRYSLER_RAM_HD;
|
||||
ret = BUILD_SAFETY_CFG(chrysler_ram_hd_rx_checks, CHRYSLER_RAM_HD_TX_MSGS);
|
||||
#endif
|
||||
} else {
|
||||
chrysler_platform = CHRYSLER_PACIFICA;
|
||||
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,
|
||||
.get_counter = chrysler_get_counter,
|
||||
.get_checksum = chrysler_get_checksum,
|
||||
.compute_checksum = chrysler_compute_checksum,
|
||||
};
|
||||
@@ -0,0 +1,39 @@
|
||||
#pragma once
|
||||
|
||||
static uint32_t chrysler_get_checksum(const CANPacket_t *msg) {
|
||||
int checksum_byte = GET_LEN(msg) - 1U;
|
||||
return (uint8_t)(msg->data[checksum_byte]);
|
||||
}
|
||||
|
||||
static uint32_t chrysler_compute_checksum(const CANPacket_t *msg) {
|
||||
// TODO: clean this up
|
||||
// http://illmatics.com/Remote%20Car%20Hacking.pdf
|
||||
uint8_t checksum = 0xFFU;
|
||||
int len = GET_LEN(msg);
|
||||
for (int j = 0; j < (len - 1); j++) {
|
||||
uint8_t shift = 0x80U;
|
||||
uint8_t curr = (uint8_t)msg->data[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);
|
||||
}
|
||||
@@ -0,0 +1,107 @@
|
||||
#include "opendbc/safety/modes/chrysler_common.h"
|
||||
|
||||
|
||||
static safety_config chrysler_cusw_init(uint16_t param) {
|
||||
SAFETY_UNUSED(param);
|
||||
|
||||
static const CanMsg CHRYSLER_CUSW_TX_MSGS[] = {
|
||||
{0x1F6U, 0, 4, .check_relay = true},
|
||||
{0x5DCU, 0, 4, .check_relay = true},
|
||||
{0x2FAU, 0, 3, .check_relay = false},
|
||||
};
|
||||
|
||||
static RxCheck chrysler_cusw_rx_checks[] = {
|
||||
{.msg = {{0x1E4U, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{0x1E8U, 0, 5, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{0x1ECU, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{0x1FEU, 0, 5, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{0x2ECU, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
return BUILD_SAFETY_CFG(chrysler_cusw_rx_checks, CHRYSLER_CUSW_TX_MSGS);
|
||||
}
|
||||
|
||||
static void chrysler_cusw_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->bus == 0U) {
|
||||
if (msg->addr == 0x1ECU) {
|
||||
// Signal: EPS_STATUS.TORQUE_MOTOR
|
||||
int torque_meas_new = ((msg->data[3] & 0xFU) << 8) + msg->data[4] - 2048U;
|
||||
update_sample(&torque_meas, torque_meas_new);
|
||||
}
|
||||
|
||||
if (msg->addr == 0x2ECU) {
|
||||
// Signal: ACC_CONTROL.ACC_ACTIVE
|
||||
bool cruise_engaged = GET_BIT(msg, 7U);
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
}
|
||||
|
||||
if (msg->addr == 0x1E4U) {
|
||||
// Signal: BRAKE_1.VEHICLE_SPEED
|
||||
vehicle_moving = (((msg->data[4] & 0x7U) << 8) + msg->data[5]) != 0U;
|
||||
}
|
||||
|
||||
if (msg->addr == 0x1FEU) {
|
||||
// Signal: ACCEL_GAS.GAS_HUMAN
|
||||
gas_pressed = msg->data[1] != 0U;
|
||||
}
|
||||
|
||||
if (msg->addr == 0x1E8U) {
|
||||
// Signal: BRAKE_3.DRIVER_BRAKE_SWITCH
|
||||
brake_pressed = GET_BIT(msg, 18U);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool chrysler_cusw_tx_hook(const CANPacket_t *msg) {
|
||||
const TorqueSteeringLimits CHRYSLER_CUSW_STEERING_LIMITS = {
|
||||
.max_torque = 250, // TODO: Find the actual cap, think we're faulting before 261
|
||||
.max_rt_delta = 150,
|
||||
.max_rate_up = 4,
|
||||
.max_rate_down = 4,
|
||||
.max_torque_error = 80,
|
||||
.type = TorqueMotorLimited,
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
|
||||
if (msg->addr == 0x1F6U) {
|
||||
// Signal: LKAS_COMMAND.STEERING_TORQUE
|
||||
int desired_torque = ((msg->data[0]) << 3) | ((msg->data[1] & 0xE0U) >> 5);
|
||||
desired_torque -= 1024;
|
||||
|
||||
// Signal: LKAS_COMMAND.LKAS_CONTROL_BIT
|
||||
const bool steer_req = GET_BIT(msg, 12U);
|
||||
if (steer_torque_cmd_checks(desired_torque, steer_req, CHRYSLER_CUSW_STEERING_LIMITS)) {
|
||||
tx = false;
|
||||
// FIXME: too many problems here right now, hotwire things while investigating
|
||||
// tx = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->addr == 0x2FAU) {
|
||||
// Signal: CRUISE_BUTTONS.ACC_Cancel
|
||||
// Signal: CRUISE_BUTTONS.ACC_Resume
|
||||
const bool is_cancel = GET_BIT(msg, 0U);
|
||||
const bool is_resume = GET_BIT(msg, 4U);
|
||||
const bool allowed = is_cancel || (is_resume && controls_allowed);
|
||||
if (!allowed) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
static uint8_t chrysler_cusw_get_counter(const CANPacket_t *msg) {
|
||||
int counter_byte = GET_LEN(msg) - 2U;
|
||||
return (uint8_t)(msg->data[counter_byte] & 0xFU);
|
||||
}
|
||||
|
||||
const safety_hooks chrysler_cusw_hooks = {
|
||||
.init = chrysler_cusw_init,
|
||||
.rx = chrysler_cusw_rx_hook,
|
||||
.tx = chrysler_cusw_tx_hook,
|
||||
.get_counter = chrysler_cusw_get_counter,
|
||||
.get_checksum = chrysler_get_checksum,
|
||||
.compute_checksum = chrysler_compute_checksum,
|
||||
};
|
||||
@@ -0,0 +1,51 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// GCOV_EXCL_START
|
||||
// Unreachable by design (doesn't define any rx msgs)
|
||||
void default_rx_hook(const CANPacket_t *msg) {
|
||||
SAFETY_UNUSED(msg);
|
||||
}
|
||||
// GCOV_EXCL_STOP
|
||||
|
||||
// *** no output safety mode ***
|
||||
|
||||
static safety_config nooutput_init(uint16_t param) {
|
||||
SAFETY_UNUSED(param);
|
||||
return (safety_config){NULL, 0, NULL, 0, true}; // NOLINT(readability/braces)
|
||||
}
|
||||
|
||||
// GCOV_EXCL_START
|
||||
// Unreachable by design (doesn't define any tx msgs)
|
||||
static bool nooutput_tx_hook(const CANPacket_t *msg) {
|
||||
SAFETY_UNUSED(msg);
|
||||
return false;
|
||||
}
|
||||
// GCOV_EXCL_STOP
|
||||
|
||||
const safety_hooks nooutput_hooks = {
|
||||
.init = nooutput_init,
|
||||
.rx = default_rx_hook,
|
||||
.tx = nooutput_tx_hook,
|
||||
};
|
||||
|
||||
// *** all output safety mode ***
|
||||
static safety_config alloutput_init(uint16_t param) {
|
||||
// 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;
|
||||
controls_allowed = true;
|
||||
bool alloutput_passthrough = GET_FLAG(param, ALLOUTPUT_PARAM_PASSTHROUGH);
|
||||
return (safety_config){NULL, 0, NULL, 0, !alloutput_passthrough}; // NOLINT(readability/braces)
|
||||
}
|
||||
|
||||
static bool alloutput_tx_hook(const CANPacket_t *msg) {
|
||||
SAFETY_UNUSED(msg);
|
||||
return true;
|
||||
}
|
||||
|
||||
const safety_hooks alloutput_hooks = {
|
||||
.init = alloutput_init,
|
||||
.rx = default_rx_hook,
|
||||
.tx = alloutput_tx_hook,
|
||||
};
|
||||
@@ -0,0 +1,40 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/defaults.h"
|
||||
|
||||
static bool elm327_tx_hook(const CANPacket_t *msg) {
|
||||
const unsigned int GM_CAMERA_DIAG_ADDR = 0x24BU;
|
||||
|
||||
bool tx = true;
|
||||
int len = GET_LEN(msg);
|
||||
|
||||
// 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 ((msg->addr != 0x18DB33F1U) && ((msg->addr & 0x1FFF00FFU) != 0x18DA00F1U) &&
|
||||
((msg->addr & 0x1FFFFF00U) != 0x600U) && ((msg->addr & 0x1FFFFF00U) != 0x700U) &&
|
||||
(msg->addr != GM_CAMERA_DIAG_ADDR)) {
|
||||
tx = false;
|
||||
}
|
||||
|
||||
// GM camera uses non-standard diagnostic address, this has no control message address collisions
|
||||
if ((msg->addr == GM_CAMERA_DIAG_ADDR) && (len == 8)) {
|
||||
// Only allow known frame types for ISO 15765-2
|
||||
if ((msg->data[0] & 0xF0U) > 0x30U) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
return tx;
|
||||
}
|
||||
|
||||
// If 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,
|
||||
};
|
||||
@@ -0,0 +1,343 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// Safety-relevant CAN messages for Ford vehicles.
|
||||
#define FORD_EngBrakeData 0x165U // RX from PCM, for driver brake pedal and cruise state
|
||||
#define FORD_EngVehicleSpThrottle 0x204U // RX from PCM, for driver throttle input
|
||||
#define FORD_DesiredTorqBrk 0x213U // RX from ABS, for standstill state
|
||||
#define FORD_BrakeSysFeatures 0x415U // RX from ABS, for vehicle speed
|
||||
#define FORD_EngVehicleSpThrottle2 0x202U // RX from PCM, for second vehicle speed
|
||||
#define FORD_Yaw_Data_FD1 0x91U // RX from RCM, for yaw rate
|
||||
#define FORD_Steering_Data_FD1 0x083U // TX by OP, various driver switches and LKAS/CC buttons
|
||||
#define FORD_ACCDATA 0x186U // TX by OP, ACC controls
|
||||
#define FORD_ACCDATA_3 0x18AU // TX by OP, ACC/TJA user interface
|
||||
#define FORD_Lane_Assist_Data1 0x3CAU // TX by OP, Lane Keep Assist
|
||||
#define FORD_LateralMotionControl 0x3D3U // TX by OP, Lateral Control message
|
||||
#define FORD_LateralMotionControl2 0x3D6U // TX by OP, alternate Lateral Control message
|
||||
#define FORD_IPMA_Data 0x3D8U // TX by OP, IPMA and LKAS user interface
|
||||
|
||||
// CAN bus numbers.
|
||||
#define FORD_MAIN_BUS 0U
|
||||
#define FORD_CAM_BUS 2U
|
||||
|
||||
static uint8_t ford_get_counter(const CANPacket_t *msg) {
|
||||
uint8_t cnt = 0;
|
||||
if (msg->addr == FORD_BrakeSysFeatures) {
|
||||
// Signal: VehVActlBrk_No_Cnt
|
||||
cnt = (msg->data[2] >> 2) & 0xFU;
|
||||
} else if (msg->addr == FORD_Yaw_Data_FD1) {
|
||||
// Signal: VehRollYaw_No_Cnt
|
||||
cnt = msg->data[5];
|
||||
} else {
|
||||
}
|
||||
return cnt;
|
||||
}
|
||||
|
||||
static uint32_t ford_get_checksum(const CANPacket_t *msg) {
|
||||
uint8_t chksum = 0;
|
||||
if (msg->addr == FORD_BrakeSysFeatures) {
|
||||
// Signal: VehVActlBrk_No_Cs
|
||||
chksum = msg->data[3];
|
||||
} else if (msg->addr == FORD_Yaw_Data_FD1) {
|
||||
// Signal: VehRollYawW_No_Cs
|
||||
chksum = msg->data[4];
|
||||
} else {
|
||||
}
|
||||
return chksum;
|
||||
}
|
||||
|
||||
static uint32_t ford_compute_checksum(const CANPacket_t *msg) {
|
||||
uint8_t chksum = 0;
|
||||
if (msg->addr == FORD_BrakeSysFeatures) {
|
||||
chksum += msg->data[0] + msg->data[1]; // Veh_V_ActlBrk
|
||||
chksum += msg->data[2] >> 6; // VehVActlBrk_D_Qf
|
||||
chksum += (msg->data[2] >> 2) & 0xFU; // VehVActlBrk_No_Cnt
|
||||
chksum = 0xFFU - chksum;
|
||||
} else if (msg->addr == FORD_Yaw_Data_FD1) {
|
||||
chksum += msg->data[0] + msg->data[1]; // VehRol_W_Actl
|
||||
chksum += msg->data[2] + msg->data[3]; // VehYaw_W_Actl
|
||||
chksum += msg->data[5]; // VehRollYaw_No_Cnt
|
||||
chksum += msg->data[6] >> 6; // VehRolWActl_D_Qf
|
||||
chksum += (msg->data[6] >> 4) & 0x3U; // VehYawWActl_D_Qf
|
||||
chksum = 0xFFU - chksum;
|
||||
} else {
|
||||
}
|
||||
return chksum;
|
||||
}
|
||||
|
||||
static bool ford_get_quality_flag_valid(const CANPacket_t *msg) {
|
||||
bool valid = false;
|
||||
if (msg->addr == FORD_BrakeSysFeatures) {
|
||||
valid = (msg->data[2] >> 6) == 0x3U; // VehVActlBrk_D_Qf
|
||||
} else if (msg->addr == FORD_EngVehicleSpThrottle2) {
|
||||
valid = ((msg->data[4] >> 5) & 0x3U) == 0x3U; // VehVActlEng_D_Qf
|
||||
} else if (msg->addr == FORD_Yaw_Data_FD1) {
|
||||
valid = ((msg->data[6] >> 4) & 0x3U) == 0x3U; // VehYawWActl_D_Qf
|
||||
} else {
|
||||
}
|
||||
return valid;
|
||||
}
|
||||
|
||||
#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
|
||||
|
||||
static const CurvatureSteeringLimits FORD_STEERING_LIMITS = {
|
||||
.max_curvature = 1000, // 0.02 rad/m * curvature_to_can
|
||||
.curvature_to_can = 50000, // CAN units per rad/m
|
||||
.frequency = 20, // Hz
|
||||
.max_curvature_error = 100, // 0.002 rad/m * curvature_to_can
|
||||
.curvature_error_min_speed = 10.0, // m/s
|
||||
.max_steer_power = 0, // disabled, Ford has no steed power signal
|
||||
.inactive_curvature_is_zero = true, // Ford EPS expects curvature=0 when inactive
|
||||
};
|
||||
|
||||
static void ford_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->bus == FORD_MAIN_BUS) {
|
||||
// Update in motion state from standstill signal
|
||||
if (msg->addr == FORD_DesiredTorqBrk) {
|
||||
// Signal: VehStop_D_Stat
|
||||
vehicle_moving = ((msg->data[3] >> 3) & 0x3U) != 1U;
|
||||
}
|
||||
|
||||
// Update vehicle speed
|
||||
if (msg->addr == FORD_BrakeSysFeatures) {
|
||||
// Signal: Veh_V_ActlBrk
|
||||
UPDATE_VEHICLE_SPEED(((msg->data[0] << 8) | msg->data[1]) * 0.01 * KPH_TO_MS);
|
||||
}
|
||||
|
||||
// Check vehicle speed against a second source
|
||||
if (msg->addr == FORD_EngVehicleSpThrottle2) {
|
||||
// Disable controls if speeds from ABS and PCM ECUs are too far apart.
|
||||
// Signal: Veh_V_ActlEng
|
||||
float filtered_pcm_speed = ((msg->data[6] << 8) | msg->data[7]) * 0.01 * KPH_TO_MS;
|
||||
UPDATE_VEHICLE_SPEED_2(filtered_pcm_speed);
|
||||
}
|
||||
|
||||
// Update vehicle yaw rate
|
||||
if (msg->addr == FORD_Yaw_Data_FD1) {
|
||||
// Signal: VehYaw_W_Actl
|
||||
// TODO: we should use the speed which results in the closest angle measurement to the desired angle
|
||||
float ford_yaw_rate = (((msg->data[2] << 8U) | msg->data[3]) * 0.0002) - 6.5;
|
||||
float current_curvature = ford_yaw_rate / SAFETY_MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1);
|
||||
// convert current curvature into units on CAN for comparison with desired curvature
|
||||
update_sample(&curvature_state.meas, ROUND(current_curvature * FORD_STEERING_LIMITS.curvature_to_can));
|
||||
}
|
||||
|
||||
// Update gas pedal
|
||||
if (msg->addr == FORD_EngVehicleSpThrottle) {
|
||||
// Pedal position: (0.1 * val) in percent
|
||||
// Signal: ApedPos_Pc_ActlArb
|
||||
gas_pressed = (((msg->data[0] & 0x03U) << 8) | msg->data[1]) > 0U;
|
||||
}
|
||||
|
||||
// Update brake pedal and cruise state
|
||||
if (msg->addr == FORD_EngBrakeData) {
|
||||
// Signal: BpedDrvAppl_D_Actl
|
||||
brake_pressed = ((msg->data[0] >> 4) & 0x3U) == 2U;
|
||||
|
||||
// Signal: CcStat_D_Actl
|
||||
unsigned int cruise_state = msg->data[1] & 0x07U;
|
||||
bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U);
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
acc_main_on = (cruise_state == 3U) || cruise_engaged;
|
||||
}
|
||||
|
||||
if (msg->addr == FORD_Steering_Data_FD1) {
|
||||
mads_button_press = GET_BIT(msg, 40U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool ford_tx_hook(const CANPacket_t *msg) {
|
||||
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
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
|
||||
// Safety check for ACCDATA accel and brake requests
|
||||
if (msg->addr == FORD_ACCDATA) {
|
||||
// Signal: AccPrpl_A_Rq
|
||||
int gas = ((msg->data[6] & 0x3U) << 8) | msg->data[7];
|
||||
// Signal: AccPrpl_A_Pred
|
||||
int gas_pred = ((msg->data[2] & 0x3U) << 8) | msg->data[3];
|
||||
// Signal: AccBrkTot_A_Rq
|
||||
int accel = ((msg->data[0] & 0x1FU) << 8) | msg->data[1];
|
||||
// Signal: CmbbDeny_B_Actl
|
||||
bool cmbb_deny = (msg->data[4] >> 5) & 1U;
|
||||
|
||||
// Signal: AccBrkPrchg_B_Rq & AccBrkDecel_B_Rq
|
||||
bool brake_actuation = ((msg->data[6] >> 6) & 1U) || ((msg->data[6] >> 7) & 1U);
|
||||
|
||||
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
|
||||
|
||||
violation |= !get_longitudinal_allowed() && brake_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 (msg->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 |= ((msg->data[1] >> 0) & 1U) && !cruise_engaged_prev; // Signal: CcAslButtnCnclPress (cancel)
|
||||
violation |= ((msg->data[3] >> 1) & 1U) && !controls_allowed; // Signal: CcAsllButtnResPress (resume)
|
||||
|
||||
if (violation) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Safety check for Lane_Assist_Data1 action
|
||||
if (msg->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 = msg->data[0] >> 5;
|
||||
if (action != 0U) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Safety check for LateralMotionControl action
|
||||
if (msg->addr == FORD_LateralMotionControl) {
|
||||
// Signal: LatCtl_D_Rq
|
||||
bool steer_control_enabled = ((msg->data[4] >> 2) & 0x7U) != 0U;
|
||||
unsigned int raw_curvature = (msg->data[0] << 3) | (msg->data[1] >> 5);
|
||||
unsigned int raw_curvature_rate = ((msg->data[1] & 0x1FU) << 8) | msg->data[2];
|
||||
unsigned int raw_path_angle = (msg->data[3] << 3) | (msg->data[4] >> 5);
|
||||
unsigned int raw_path_offset = (msg->data[5] << 2) | (msg->data[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.curvature_to_can to get real curvature
|
||||
violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, FORD_STEERING_LIMITS);
|
||||
|
||||
if (violation) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Safety check for LateralMotionControl2 action
|
||||
if (msg->addr == FORD_LateralMotionControl2) {
|
||||
// Signal: LatCtl_D2_Rq
|
||||
bool steer_control_enabled = ((msg->data[0] >> 4) & 0x7U) != 0U;
|
||||
unsigned int raw_curvature = (msg->data[2] << 3) | (msg->data[3] >> 5);
|
||||
unsigned int raw_curvature_rate = (msg->data[6] << 3) | (msg->data[7] >> 5);
|
||||
unsigned int raw_path_angle = ((msg->data[3] & 0x1FU) << 6) | (msg->data[4] >> 2);
|
||||
unsigned int raw_path_offset = ((msg->data[4] & 0x3U) << 8) | msg->data[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.curvature_to_can to get real curvature
|
||||
violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, FORD_STEERING_LIMITS);
|
||||
|
||||
if (violation) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
static safety_config ford_init(uint16_t param) {
|
||||
// warning: quality flags are not yet checked in openpilot's CAN parser,
|
||||
// this may be the cause of blocked messages
|
||||
static RxCheck ford_rx_checks[] = {
|
||||
{.msg = {{FORD_BrakeSysFeatures, 0, 8, 50U, .max_counter = 15U}, { 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, 50U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{FORD_Yaw_Data_FD1, 0, 8, 100U, .max_counter = 255U}, { 0 }, { 0 }}},
|
||||
// These messages have no counter or checksum
|
||||
{.msg = {{FORD_EngBrakeData, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{FORD_EngVehicleSpThrottle, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{FORD_DesiredTorqBrk, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{FORD_Steering_Data_FD1, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
#define FORD_COMMON_TX_MSGS \
|
||||
{FORD_Steering_Data_FD1, 0, 8, .check_relay = false}, \
|
||||
{FORD_Steering_Data_FD1, 2, 8, .check_relay = false}, \
|
||||
{FORD_ACCDATA_3, 0, 8, .check_relay = true}, \
|
||||
{FORD_Lane_Assist_Data1, 0, 8, .check_relay = true}, \
|
||||
{FORD_IPMA_Data, 0, 8, .check_relay = true}, \
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
static const CanMsg FORD_CANFD_LONG_TX_MSGS[] = {
|
||||
FORD_COMMON_TX_MSGS
|
||||
{FORD_ACCDATA, 0, 8, .check_relay = true},
|
||||
{FORD_LateralMotionControl2, 0, 8, .check_relay = true},
|
||||
};
|
||||
#endif
|
||||
|
||||
static const CanMsg FORD_CANFD_STOCK_TX_MSGS[] = {
|
||||
FORD_COMMON_TX_MSGS
|
||||
{FORD_LateralMotionControl2, 0, 8, .check_relay = true},
|
||||
};
|
||||
|
||||
static const CanMsg FORD_LONG_TX_MSGS[] = {
|
||||
FORD_COMMON_TX_MSGS
|
||||
{FORD_ACCDATA, 0, 8, .check_relay = true},
|
||||
{FORD_LateralMotionControl, 0, 8, .check_relay = true},
|
||||
};
|
||||
|
||||
const uint16_t FORD_PARAM_CANFD = 2;
|
||||
const bool ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD);
|
||||
|
||||
safety_config ret;
|
||||
if (ford_canfd) {
|
||||
ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_STOCK_TX_MSGS);
|
||||
#ifdef ALLOW_DEBUG
|
||||
const uint16_t FORD_PARAM_LONGITUDINAL = 1;
|
||||
if (GET_FLAG(param, FORD_PARAM_LONGITUDINAL)) {
|
||||
ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_LONG_TX_MSGS);
|
||||
}
|
||||
#endif
|
||||
} else {
|
||||
ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_LONG_TX_MSGS);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
const safety_hooks ford_hooks = {
|
||||
.init = ford_init,
|
||||
.rx = ford_rx_hook,
|
||||
.tx = ford_tx_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,
|
||||
};
|
||||
@@ -0,0 +1,283 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// TODO: do checksum and counter checks. Add correct timestep, 0.1s for now.
|
||||
#define GM_COMMON_RX_CHECKS \
|
||||
{.msg = {{0x184, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x34A, 0, 5, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x1E1, 0, 7, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0xBE, 0, 6, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, /* Volt, Silverado, Acadia Denali */ \
|
||||
{0xBE, 0, 7, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, /* Bolt EUV */ \
|
||||
{0xBE, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}}}, /* Escalade */ \
|
||||
{.msg = {{0x1C4, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0xC9, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define GM_EV_COMMON_ADDR_CHECK \
|
||||
{.msg = {{0xBD, 0, 7, 40U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define GM_NON_ACC_ADDR_CHECK \
|
||||
{.msg = {{0x3D1, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
static const LongitudinalLimits *gm_long_limits;
|
||||
|
||||
enum {
|
||||
GM_BTN_UNPRESS = 1,
|
||||
GM_BTN_RESUME = 2,
|
||||
GM_BTN_SET = 3,
|
||||
GM_BTN_CANCEL = 6,
|
||||
};
|
||||
|
||||
typedef enum {
|
||||
GM_ASCM,
|
||||
GM_CAM
|
||||
} GmHardware;
|
||||
static GmHardware gm_hw = GM_ASCM;
|
||||
static bool gm_pcm_cruise = false;
|
||||
static bool gm_non_acc = false;
|
||||
|
||||
static void gm_rx_hook(const CANPacket_t *msg) {
|
||||
const int GM_STANDSTILL_THRSLD = 10; // 0.311kph
|
||||
|
||||
if (msg->bus == 0U) {
|
||||
if (msg->addr == 0x184U) {
|
||||
int torque_driver_new = ((msg->data[6] & 0x7U) << 8) | msg->data[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 (msg->addr == 0x34AU) {
|
||||
int left_rear_speed = (msg->data[0] << 8) | msg->data[1];
|
||||
int right_rear_speed = (msg->data[2] << 8) | msg->data[3];
|
||||
vehicle_moving = (left_rear_speed > GM_STANDSTILL_THRSLD) || (right_rear_speed > GM_STANDSTILL_THRSLD);
|
||||
}
|
||||
|
||||
// ACC steering wheel buttons (GM_CAM is tied to the PCM)
|
||||
if ((msg->addr == 0x1E1U) && !gm_pcm_cruise) {
|
||||
int button = (msg->data[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;
|
||||
}
|
||||
|
||||
// Reference for brake pressed signals:
|
||||
// https://github.com/commaai/openpilot/blob/master/selfdrive/car/gm/carstate.py
|
||||
if ((msg->addr == 0xBEU) && (gm_hw == GM_ASCM)) {
|
||||
brake_pressed = msg->data[1] >= 8U;
|
||||
}
|
||||
|
||||
if ((msg->addr == 0xC9U) && (gm_hw == GM_CAM)) {
|
||||
brake_pressed = GET_BIT(msg, 40U);
|
||||
}
|
||||
|
||||
if (msg->addr == 0x1C4U) {
|
||||
gas_pressed = msg->data[5] != 0U;
|
||||
|
||||
// enter controls on rising edge of ACC, exit controls when ACC off
|
||||
if (gm_pcm_cruise && !gm_non_acc) {
|
||||
bool cruise_engaged = (msg->data[1] >> 5) != 0U;
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->addr == 0xBDU) {
|
||||
regen_braking = (msg->data[0] >> 4) != 0U;
|
||||
}
|
||||
|
||||
if (msg->addr == 0xC9U) {
|
||||
acc_main_on = GET_BIT(msg, 29U);
|
||||
}
|
||||
|
||||
if (msg->addr == 0x3D1U) {
|
||||
bool cruise_engaged = GET_BIT(msg, 39U);
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool gm_tx_hook(const CANPacket_t *msg) {
|
||||
const TorqueSteeringLimits GM_STEERING_LIMITS = {
|
||||
.max_torque = 300,
|
||||
.max_rate_up = 10,
|
||||
.max_rate_down = 15,
|
||||
.driver_torque_allowance = 65,
|
||||
.driver_torque_multiplier = 4,
|
||||
.max_rt_delta = 128,
|
||||
.type = TorqueDriverLimited,
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
|
||||
// BRAKE: safety check
|
||||
if (msg->addr == 0x315U) {
|
||||
int brake = ((msg->data[0] & 0xFU) << 8) + msg->data[1];
|
||||
brake = (0x1000 - brake) & 0xFFF;
|
||||
if (longitudinal_brake_checks(brake, *gm_long_limits)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// LKA STEER: safety check
|
||||
if (msg->addr == 0x180U) {
|
||||
int desired_torque = ((msg->data[0] & 0x7U) << 8) + msg->data[1];
|
||||
desired_torque = to_signed(desired_torque, 11);
|
||||
|
||||
bool steer_req = GET_BIT(msg, 3U);
|
||||
|
||||
if (steer_torque_cmd_checks(desired_torque, steer_req, GM_STEERING_LIMITS)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// GAS/REGEN: safety check
|
||||
if (msg->addr == 0x2CBU) {
|
||||
bool apply = GET_BIT(msg, 0U);
|
||||
// convert float CAN signal to an int for gas checks: 22534 / 0.125 = 180272
|
||||
int gas_regen = (((msg->data[1] & 0x7U) << 16) | (msg->data[2] << 8) | msg->data[3]) - 180272U;
|
||||
|
||||
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 ((msg->addr == 0x1E1U) && gm_pcm_cruise) {
|
||||
int button = (msg->data[5] >> 4) & 0x7U;
|
||||
|
||||
bool allowed_cancel = (button == 6) && cruise_engaged_prev;
|
||||
if (!allowed_cancel) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
static safety_config gm_init(uint16_t param) {
|
||||
const uint16_t GM_PARAM_HW_CAM = 1;
|
||||
const uint16_t GM_PARAM_EV = 4;
|
||||
|
||||
// common safety checks assume unscaled integer values
|
||||
static const int GM_GAS_TO_CAN = 8; // 1 / 0.125
|
||||
|
||||
static const LongitudinalLimits GM_ASCM_LONG_LIMITS = {
|
||||
.max_gas = 1018 * GM_GAS_TO_CAN,
|
||||
.min_gas = -650 * GM_GAS_TO_CAN,
|
||||
.inactive_gas = -650 * GM_GAS_TO_CAN,
|
||||
.max_brake = 400,
|
||||
};
|
||||
|
||||
static const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4, .check_relay = true}, {0x409, 0, 7, .check_relay = false}, {0x40A, 0, 7, .check_relay = false}, {0x2CB, 0, 8, .check_relay = true}, {0x370, 0, 6, .check_relay = false}, // pt bus
|
||||
{0xA1, 1, 7, .check_relay = false}, {0x306, 1, 8, .check_relay = false}, {0x308, 1, 7, .check_relay = false}, {0x310, 1, 2, .check_relay = false}, // obs bus
|
||||
{0x315, 2, 5, .check_relay = false}}; // ch bus
|
||||
|
||||
|
||||
static const LongitudinalLimits GM_CAM_LONG_LIMITS = {
|
||||
.max_gas = 1346 * GM_GAS_TO_CAN,
|
||||
.min_gas = -540 * GM_GAS_TO_CAN,
|
||||
.inactive_gas = -500 * GM_GAS_TO_CAN,
|
||||
.max_brake = 400,
|
||||
};
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
// block PSCMStatus (0x184); forwarded through openpilot to hide an alert from the camera
|
||||
static const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4, .check_relay = true}, {0x315, 0, 5, .check_relay = true}, {0x2CB, 0, 8, .check_relay = true}, {0x370, 0, 6, .check_relay = true}, // pt bus
|
||||
{0x184, 2, 8, .check_relay = true}}; // camera bus
|
||||
#endif
|
||||
|
||||
|
||||
static RxCheck gm_rx_checks[] = {
|
||||
GM_COMMON_RX_CHECKS
|
||||
};
|
||||
|
||||
static RxCheck gm_ev_rx_checks[] = {
|
||||
GM_COMMON_RX_CHECKS
|
||||
GM_EV_COMMON_ADDR_CHECK
|
||||
};
|
||||
|
||||
static RxCheck gm_non_acc_rx_checks[] = {
|
||||
GM_COMMON_RX_CHECKS
|
||||
GM_NON_ACC_ADDR_CHECK
|
||||
};
|
||||
|
||||
static RxCheck gm_non_acc_ev_rx_checks[] = {
|
||||
GM_COMMON_RX_CHECKS
|
||||
GM_EV_COMMON_ADDR_CHECK
|
||||
GM_NON_ACC_ADDR_CHECK
|
||||
};
|
||||
|
||||
static const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4, .check_relay = true}, // pt bus
|
||||
{0x1E1, 2, 7, .check_relay = false}, {0x184, 2, 8, .check_relay = true}}; // camera bus
|
||||
|
||||
if (GET_FLAG(param, GM_PARAM_HW_CAM)) {
|
||||
gm_hw = GM_CAM;
|
||||
gm_long_limits = &GM_CAM_LONG_LIMITS;
|
||||
} else {
|
||||
gm_hw = GM_ASCM;
|
||||
gm_long_limits = &GM_ASCM_LONG_LIMITS;
|
||||
}
|
||||
|
||||
gm_pcm_cruise = (gm_hw == GM_CAM);
|
||||
|
||||
const uint16_t GM_PARAM_SP_NON_ACC = 1;
|
||||
gm_non_acc = GET_FLAG(current_safety_param_sp, GM_PARAM_SP_NON_ACC);
|
||||
|
||||
safety_config ret;
|
||||
if (gm_hw == GM_CAM) {
|
||||
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_TX_MSGS);
|
||||
#ifdef ALLOW_DEBUG
|
||||
const uint16_t GM_PARAM_HW_CAM_LONG = 2;
|
||||
const bool gm_cam_long = GET_FLAG(param, GM_PARAM_HW_CAM_LONG);
|
||||
gm_pcm_cruise = !gm_cam_long;
|
||||
if (gm_cam_long) {
|
||||
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_CAM_LONG_TX_MSGS);
|
||||
}
|
||||
#endif
|
||||
} else {
|
||||
ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_ASCM_TX_MSGS);
|
||||
}
|
||||
|
||||
const bool gm_ev = GET_FLAG(param, GM_PARAM_EV);
|
||||
if (gm_ev) {
|
||||
SET_RX_CHECKS(gm_ev_rx_checks, ret);
|
||||
}
|
||||
|
||||
if (gm_non_acc) {
|
||||
SET_TX_MSGS(GM_CAM_TX_MSGS, ret);
|
||||
if (gm_ev) {
|
||||
SET_RX_CHECKS(gm_non_acc_ev_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(gm_non_acc_rx_checks, ret);
|
||||
}
|
||||
}
|
||||
|
||||
// ASCM does not forward any messages
|
||||
if (gm_hw == GM_ASCM) {
|
||||
ret.disable_forwarding = true;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
const safety_hooks gm_hooks = {
|
||||
.init = gm_init,
|
||||
.rx = gm_rx_hook,
|
||||
.tx = gm_tx_hook,
|
||||
};
|
||||
@@ -0,0 +1,533 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// 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, 25U, .max_counter = 3U, .ignore_quality_flag = true}, /* SCM_BUTTONS */ \
|
||||
{0x296, (pt_bus), 4, 25U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }}}, \
|
||||
{.msg = {{0x158, (pt_bus), 8, 100U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* ENGINE_DATA */ \
|
||||
{.msg = {{0x17C, (pt_bus), 8, 100U, .max_counter = 3U, .ignore_quality_flag = true}, { 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, 10U, .max_counter = 3U, .ignore_quality_flag = true}, { 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, 50U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* BRAKE_MODULE */ \
|
||||
|
||||
#define HONDA_GAS_INTERCEPTOR_ADDR_CHECK \
|
||||
{.msg = {{0x201, 0, 6, 50U, .max_counter = 15U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define HONDA_N_COMMON_TX_MSGS \
|
||||
{0xE4, 0, 5, .check_relay = true}, \
|
||||
{0x194, 0, 4, .check_relay = true}, \
|
||||
{0x1FA, 0, 8, .check_relay = false}, \
|
||||
{0x30C, 0, 8, .check_relay = true}, \
|
||||
{0x33D, 0, 5, .check_relay = true}, \
|
||||
|
||||
enum {
|
||||
HONDA_BTN_NONE = 0,
|
||||
HONDA_BTN_MAIN = 1,
|
||||
HONDA_BTN_CANCEL = 2,
|
||||
HONDA_BTN_SET = 3,
|
||||
HONDA_BTN_RESUME = 4,
|
||||
};
|
||||
|
||||
static int honda_brake = 0;
|
||||
static bool honda_brake_switch_prev = false;
|
||||
static bool honda_alt_brake_msg = false;
|
||||
static bool honda_fwd_brake = false;
|
||||
static bool honda_bosch_long = false;
|
||||
static bool honda_bosch_radarless = false;
|
||||
static bool honda_bosch_canfd = false;
|
||||
static bool honda_nidec_hybrid = false;
|
||||
typedef enum {HONDA_NIDEC, HONDA_BOSCH} HondaHw;
|
||||
static HondaHw honda_hw = HONDA_NIDEC;
|
||||
|
||||
|
||||
static unsigned int honda_get_pt_bus(void) {
|
||||
return ((honda_hw == HONDA_BOSCH) && !honda_bosch_radarless && !honda_bosch_canfd) ? 1U : 0U;
|
||||
}
|
||||
|
||||
static uint32_t honda_get_checksum(const CANPacket_t *msg) {
|
||||
int checksum_byte = GET_LEN(msg) - 1U;
|
||||
return (uint8_t)(msg->data[checksum_byte]) & 0xFU;
|
||||
}
|
||||
|
||||
static uint32_t honda_compute_checksum(const CANPacket_t *msg) {
|
||||
int len = GET_LEN(msg);
|
||||
uint8_t checksum = 0U;
|
||||
unsigned int addr = msg->addr;
|
||||
while (addr > 0U) {
|
||||
checksum += (uint8_t)(addr & 0xFU); addr >>= 4;
|
||||
}
|
||||
for (int j = 0; j < len; j++) {
|
||||
uint8_t byte = msg->data[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 *msg) {
|
||||
uint8_t cnt = 0U;
|
||||
if (msg->addr == 0x201U) {
|
||||
// Signal: PEDAL_COUNTER
|
||||
cnt = msg->data[4] & 0x0FU;
|
||||
} else {
|
||||
int counter_byte = GET_LEN(msg) - 1U;
|
||||
cnt = (msg->data[counter_byte] >> 4U) & 0x3U;
|
||||
}
|
||||
return cnt;
|
||||
}
|
||||
|
||||
static int HONDA_GET_INTERCEPTOR(const CANPacket_t *msg) {
|
||||
uint16_t val1 = (uint16_t)((uint16_t)msg->data[0] << 8U) | (uint16_t)msg->data[1];
|
||||
uint16_t val2 = (uint16_t)((uint16_t)msg->data[2] << 8U) | (uint16_t)msg->data[3];
|
||||
uint16_t avg = (uint16_t)((val1 + val2) / 2U);
|
||||
|
||||
return (int)avg;
|
||||
}
|
||||
|
||||
static void honda_rx_hook(const CANPacket_t *msg) {
|
||||
const bool pcm_cruise = ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) || ((honda_hw == HONDA_NIDEC) && !enable_gas_interceptor);
|
||||
unsigned int pt_bus = honda_get_pt_bus();
|
||||
|
||||
// sample speed
|
||||
if (msg->addr == 0x158U) {
|
||||
vehicle_moving = msg->data[0] | msg->data[1];
|
||||
}
|
||||
|
||||
// check ACC main state
|
||||
// 0x326 for all Bosch and some Nidec, 0x1A6 for some Nidec
|
||||
if ((msg->addr == 0x326U) || (msg->addr == 0x1A6U)) {
|
||||
acc_main_on = GET_BIT(msg, ((msg->addr == 0x326U) ? 28U : 47U));
|
||||
if (!acc_main_on) {
|
||||
controls_allowed = false;
|
||||
}
|
||||
}
|
||||
|
||||
// enter controls when PCM enters cruise state
|
||||
if (pcm_cruise && (msg->addr == 0x17CU)) {
|
||||
const bool cruise_engaged = GET_BIT(msg, 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 (((msg->addr == 0x1A6U) || (msg->addr == 0x296U)) && (msg->bus == pt_bus)) {
|
||||
int button = (msg->data[0] & 0xE0U) >> 5;
|
||||
|
||||
int cruise_setting = (msg->data[(msg->addr == 0x296U) ? 0U : 5U] & 0x0CU) >> 2U;
|
||||
if (cruise_setting == 1) {
|
||||
mads_button_press = MADS_BUTTON_PRESSED;
|
||||
} else if (cruise_setting == 0) {
|
||||
mads_button_press = MADS_BUTTON_NOT_PRESSED;
|
||||
} else {
|
||||
}
|
||||
|
||||
// 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 (msg->addr == 0x1BEU) {
|
||||
brake_pressed = GET_BIT(msg, 4U);
|
||||
}
|
||||
} else {
|
||||
if (msg->addr == 0x17CU) {
|
||||
// also if brake switch is 1 for two CAN frames, as brake pressed is delayed
|
||||
const bool brake_switch = GET_BIT(msg, 32U);
|
||||
brake_pressed = (GET_BIT(msg, 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 (msg->addr == 0x201U) {
|
||||
// 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_thrsld = 492;
|
||||
|
||||
int gas_interceptor = HONDA_GET_INTERCEPTOR(msg);
|
||||
gas_pressed = gas_interceptor > honda_gas_interceptor_thrsld;
|
||||
|
||||
gas_interceptor_prev = gas_interceptor;
|
||||
}
|
||||
|
||||
if (!enable_gas_interceptor) {
|
||||
if (msg->addr == 0x17CU) {
|
||||
gas_pressed = msg->data[0] != 0U;
|
||||
}
|
||||
}
|
||||
|
||||
// disable stock Honda AEB in alternative experience
|
||||
if (!(alternative_experience & ALT_EXP_DISABLE_STOCK_AEB)) {
|
||||
if ((msg->bus == 2U) && (msg->addr == 0x1FAU)) {
|
||||
bool honda_stock_aeb = GET_BIT(msg, 29U);
|
||||
int honda_stock_brake = (msg->data[0] << 2) | (msg->data[1] >> 6);
|
||||
|
||||
if (honda_nidec_hybrid) {
|
||||
honda_stock_brake = (msg->data[6] << 2) | (msg->data[7] >> 6);
|
||||
}
|
||||
|
||||
// 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
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool honda_tx_hook(const CANPacket_t *msg) {
|
||||
|
||||
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_NIDEC_LONG_LIMITS = {
|
||||
.max_gas = 198, // 0xc6
|
||||
.max_brake = 255,
|
||||
|
||||
.inactive_speed = 0,
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
|
||||
unsigned int bus_pt = honda_get_pt_bus();
|
||||
unsigned int bus_buttons = (honda_bosch_radarless) ? 2U : bus_pt; // the camera controls ACC on radarless Bosch cars
|
||||
|
||||
// ACC_HUD: safety check (nidec w/o pedal)
|
||||
if ((msg->addr == 0x30CU) && (msg->bus == bus_pt)) {
|
||||
int pcm_speed = (msg->data[0] << 8) | msg->data[1];
|
||||
int pcm_gas = msg->data[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 ((msg->addr == 0x1FAU) && (msg->bus == bus_pt)) {
|
||||
honda_brake = (msg->data[0] << 2) + ((msg->data[1] >> 6) & 0x3U);
|
||||
|
||||
if (honda_nidec_hybrid) {
|
||||
honda_brake = (msg->data[6] << 2) + ((msg->data[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 ((msg->addr == 0x1DFU) && (msg->bus == bus_pt)) {
|
||||
int accel = (msg->data[3] << 3) | ((msg->data[4] >> 5) & 0x7U);
|
||||
accel = to_signed(accel, 11);
|
||||
|
||||
int gas = (msg->data[0] << 8) | msg->data[1];
|
||||
gas = to_signed(gas, 16);
|
||||
|
||||
bool violation = false;
|
||||
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 ((msg->addr == 0x1C8U) && (msg->bus == bus_pt)) {
|
||||
int accel = (msg->data[0] << 4) | (msg->data[1] >> 4);
|
||||
accel = to_signed(accel, 12);
|
||||
|
||||
bool violation = false;
|
||||
violation |= longitudinal_accel_checks(accel, HONDA_BOSCH_LONG_LIMITS);
|
||||
if (violation) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// STEER: safety check
|
||||
if ((msg->addr == 0xE4U) || (msg->addr == 0x194U)) {
|
||||
if (!(controls_allowed || controls_allowed_lateral)) {
|
||||
bool steer_applied = msg->data[0] | msg->data[1];
|
||||
if (steer_applied) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Bosch supplemental control check
|
||||
if (msg->addr == 0xE5U) {
|
||||
if ((GET_BYTES(msg, 0, 4) != 0x10800004U) || ((GET_BYTES(msg, 4, 4) & 0x00FFFFFFU) != 0x0U)) {
|
||||
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 ((msg->addr == 0x296U) && !controls_allowed && (msg->bus == bus_buttons)) {
|
||||
if (((msg->data[0] >> 5) & 0x7U) != 2U) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
|
||||
if (msg->addr == 0x18DAB0F1U) {
|
||||
if ((GET_BYTES(msg, 0, 4) != 0x00803E02U) || (GET_BYTES(msg, 4, 4) != 0x0U)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// GAS: safety check (interceptor)
|
||||
if (msg->addr == 0x200U) {
|
||||
if (longitudinal_interceptor_checks(msg)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
static safety_config honda_nidec_init(uint16_t param) {
|
||||
// 0x1FA is dynamically forwarded based on stock AEB
|
||||
// 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
|
||||
static CanMsg HONDA_N_TX_MSGS[] = {HONDA_N_COMMON_TX_MSGS};
|
||||
|
||||
static CanMsg HONDA_N_INTERCEPTOR_TX_MSGS[] = {
|
||||
HONDA_N_COMMON_TX_MSGS
|
||||
{0x200, 0, 6, .check_relay = false},
|
||||
};
|
||||
|
||||
const uint16_t HONDA_PARAM_NIDEC_ALT = 4;
|
||||
|
||||
const uint16_t HONDA_PARAM_SP_NIDEC_HYBRID = 1;
|
||||
const uint16_t HONDA_PARAM_GAS_INTERCEPTOR = 2;
|
||||
|
||||
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;
|
||||
honda_bosch_canfd = false;
|
||||
|
||||
safety_config ret;
|
||||
|
||||
bool enable_nidec_alt = GET_FLAG(param, HONDA_PARAM_NIDEC_ALT);
|
||||
|
||||
honda_nidec_hybrid = GET_FLAG(current_safety_param_sp, HONDA_PARAM_SP_NIDEC_HYBRID);
|
||||
enable_gas_interceptor = GET_FLAG(current_safety_param_sp, HONDA_PARAM_GAS_INTERCEPTOR);
|
||||
|
||||
if (enable_nidec_alt) {
|
||||
// For Nidecs with main on signal on an alternate msg (missing 0x326)
|
||||
static RxCheck honda_nidec_alt_rx_checks[] = {
|
||||
HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(0)
|
||||
{.msg = {{0x1FA, 2, 8, 50U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // BRAKE_COMMAND
|
||||
};
|
||||
|
||||
SET_RX_CHECKS(honda_nidec_alt_rx_checks, ret);
|
||||
} else {
|
||||
// Nidec includes BRAKE_COMMAND
|
||||
static RxCheck honda_nidec_common_rx_checks[] = {
|
||||
HONDA_COMMON_RX_CHECKS(0)
|
||||
{.msg = {{0x1FA, 2, 8, 50U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // BRAKE_COMMAND
|
||||
};
|
||||
|
||||
SET_RX_CHECKS(honda_nidec_common_rx_checks, ret);
|
||||
}
|
||||
|
||||
SET_TX_MSGS(HONDA_N_TX_MSGS, ret);
|
||||
|
||||
if (enable_gas_interceptor) {
|
||||
if (enable_nidec_alt) {
|
||||
static RxCheck honda_nidec_alt_interceptor_rx_checks[] = {
|
||||
HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(0)
|
||||
{.msg = {{0x1FA, 2, 8, 50U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // BRAKE_COMMAND
|
||||
HONDA_GAS_INTERCEPTOR_ADDR_CHECK
|
||||
};
|
||||
|
||||
SET_RX_CHECKS(honda_nidec_alt_interceptor_rx_checks, ret);
|
||||
} else {
|
||||
static RxCheck honda_nidec_common_interceptor_rx_checks[] = {
|
||||
HONDA_COMMON_RX_CHECKS(0)
|
||||
{.msg = {{0x1FA, 2, 8, 50U, .max_counter = 3U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // BRAKE_COMMAND
|
||||
HONDA_GAS_INTERCEPTOR_ADDR_CHECK
|
||||
};
|
||||
|
||||
SET_RX_CHECKS(honda_nidec_common_interceptor_rx_checks, ret);
|
||||
}
|
||||
|
||||
SET_TX_MSGS(HONDA_N_INTERCEPTOR_TX_MSGS, ret);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static safety_config honda_bosch_init(uint16_t param) {
|
||||
static CanMsg HONDA_BOSCH_TX_MSGS[] = {{0xE4, 0, 5, .check_relay = true}, {0xE5, 0, 8, .check_relay = true}, {0x296, 1, 4, .check_relay = false},
|
||||
{0x33D, 0, 5, .check_relay = true}, {0x33D, 0, 8, .check_relay = true}, {0x33DA, 0, 5, .check_relay = true}, {0x33DB, 0, 8, .check_relay = true}}; // Bosch
|
||||
|
||||
static CanMsg HONDA_BOSCH_LONG_TX_MSGS[] = {{0xE4, 1, 5, .check_relay = true}, {0x1DF, 1, 8, .check_relay = true}, {0x1EF, 1, 8, .check_relay = false},
|
||||
{0x1FA, 1, 8, .check_relay = false}, {0x30C, 1, 8, .check_relay = false}, {0x33D, 1, 5, .check_relay = true}, {0x33D, 1, 8, .check_relay = true},
|
||||
{0x33DA, 1, 5, .check_relay = true}, {0x33DB, 1, 8, .check_relay = true}, {0x39F, 1, 8, .check_relay = false},
|
||||
{0x18DAB0F1, 1, 8, .check_relay = false}}; // Bosch w/ gas and brakes
|
||||
|
||||
static CanMsg HONDA_RADARLESS_TX_MSGS[] = {{0xE4, 0, 5, .check_relay = true}, {0x296, 2, 4, .check_relay = false}, {0x33D, 0, 8, .check_relay = true}}; // Bosch radarless
|
||||
|
||||
static CanMsg HONDA_RADARLESS_LONG_TX_MSGS[] = {{0xE4, 0, 5, .check_relay = true}, {0x33D, 0, 8, .check_relay = true}, {0x1C8, 0, 8, .check_relay = true},
|
||||
{0x30C, 0, 8, .check_relay = true}}; // Bosch radarless w/ gas and brakes
|
||||
|
||||
static CanMsg HONDA_CANFD_TX_MSGS[] = {{0xE4, 0, 5, .check_relay = true}, {0x296, 0, 4, .check_relay = false}, {0x33D, 0, 8, .check_relay = true}};
|
||||
|
||||
|
||||
const uint16_t HONDA_PARAM_ALT_BRAKE = 1;
|
||||
const uint16_t HONDA_PARAM_RADARLESS = 8;
|
||||
const uint16_t HONDA_PARAM_BOSCH_CANFD = 16;
|
||||
|
||||
// Bosch radarless has the powertrain bus on bus 0
|
||||
static RxCheck honda_bosch_pt0_rx_checks[] = {
|
||||
HONDA_COMMON_RX_CHECKS(0)
|
||||
};
|
||||
|
||||
static RxCheck honda_bosch_pt0_alt_brake_rx_checks[] = {
|
||||
HONDA_COMMON_RX_CHECKS(0)
|
||||
HONDA_ALT_BRAKE_ADDR_CHECK(0)
|
||||
};
|
||||
|
||||
// Bosch has powertrain on bus 1, verified 0x1A6 does not exist
|
||||
static RxCheck honda_bosch_pt1_rx_checks[] = {
|
||||
HONDA_COMMON_RX_CHECKS(1)
|
||||
};
|
||||
|
||||
static RxCheck honda_bosch_pt1_alt_brake_rx_checks[] = {
|
||||
HONDA_COMMON_RX_CHECKS(1)
|
||||
HONDA_ALT_BRAKE_ADDR_CHECK(1)
|
||||
};
|
||||
|
||||
honda_hw = HONDA_BOSCH;
|
||||
honda_brake_switch_prev = false;
|
||||
honda_bosch_radarless = GET_FLAG(param, HONDA_PARAM_RADARLESS);
|
||||
honda_bosch_canfd = GET_FLAG(param, HONDA_PARAM_BOSCH_CANFD);
|
||||
// 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
|
||||
const uint16_t HONDA_PARAM_BOSCH_LONG = 2;
|
||||
honda_bosch_long = GET_FLAG(param, HONDA_PARAM_BOSCH_LONG);
|
||||
#endif
|
||||
|
||||
safety_config ret;
|
||||
if (honda_bosch_radarless || honda_bosch_canfd) {
|
||||
if (honda_alt_brake_msg) {
|
||||
SET_RX_CHECKS(honda_bosch_pt0_alt_brake_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(honda_bosch_pt0_rx_checks, ret);
|
||||
}
|
||||
} else {
|
||||
if (honda_alt_brake_msg) {
|
||||
SET_RX_CHECKS(honda_bosch_pt1_alt_brake_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(honda_bosch_pt1_rx_checks, ret);
|
||||
}
|
||||
}
|
||||
|
||||
if (honda_bosch_radarless) {
|
||||
if (honda_bosch_long) {
|
||||
SET_TX_MSGS(HONDA_RADARLESS_LONG_TX_MSGS, ret);
|
||||
} else {
|
||||
SET_TX_MSGS(HONDA_RADARLESS_TX_MSGS, ret);
|
||||
}
|
||||
} else if (honda_bosch_canfd) {
|
||||
SET_TX_MSGS(HONDA_CANFD_TX_MSGS, ret);
|
||||
} else {
|
||||
if (honda_bosch_long) {
|
||||
SET_TX_MSGS(HONDA_BOSCH_LONG_TX_MSGS, ret);
|
||||
} else {
|
||||
SET_TX_MSGS(HONDA_BOSCH_TX_MSGS, ret);
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
static bool honda_nidec_fwd_hook(int bus_num, int addr) {
|
||||
bool block_msg = false;
|
||||
|
||||
if (bus_num == 2) {
|
||||
// forwarded if stock AEB is active
|
||||
bool is_brake_msg = addr == 0x1FA;
|
||||
block_msg = is_brake_msg && !honda_fwd_brake;
|
||||
}
|
||||
|
||||
return block_msg;
|
||||
}
|
||||
|
||||
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,
|
||||
.get_counter = honda_get_counter,
|
||||
.get_checksum = honda_get_checksum,
|
||||
.compute_checksum = honda_compute_checksum,
|
||||
};
|
||||
@@ -0,0 +1,508 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/hyundai_common.h"
|
||||
|
||||
#define HYUNDAI_LIMITS(steer, rate_up, rate_down) { \
|
||||
.max_torque = (steer), \
|
||||
.max_rate_up = (rate_up), \
|
||||
.max_rate_down = (rate_down), \
|
||||
.max_rt_delta = 112, \
|
||||
.driver_torque_allowance = 50, \
|
||||
.driver_torque_multiplier = 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, \
|
||||
}
|
||||
|
||||
extern const LongitudinalLimits HYUNDAI_LONG_LIMITS;
|
||||
const LongitudinalLimits HYUNDAI_LONG_LIMITS = {
|
||||
.max_accel = 200, // 1/100 m/s2
|
||||
.min_accel = -350, // 1/100 m/s2
|
||||
};
|
||||
|
||||
#define HYUNDAI_COMMON_TX_MSGS(scc_bus) \
|
||||
{0x340, 0, 8, .check_relay = true}, /* LKAS11 Bus 0 */ \
|
||||
{0x4F1, scc_bus, 4, .check_relay = false}, /* CLU11 Bus 0 (radar-SCC) or 2 (camera-SCC) */ \
|
||||
{0x485, 0, 4, .check_relay = true}, /* LFAHDA_MFC Bus 0 */ \
|
||||
|
||||
#define HYUNDAI_LONG_COMMON_TX_MSGS(scc_bus) \
|
||||
HYUNDAI_COMMON_TX_MSGS(scc_bus) \
|
||||
{0x420, 0, 8, .check_relay = true}, /* SCC11 Bus 0 */ \
|
||||
{0x421, 0, 8, .check_relay = true}, /* SCC12 Bus 0 */ \
|
||||
{0x50A, 0, 8, .check_relay = true}, /* SCC13 Bus 0 */ \
|
||||
{0x389, 0, 8, .check_relay = true}, /* SCC14 Bus 0 */ \
|
||||
{0x4A2, 0, 2, .check_relay = false}, /* FRT_RADAR11 Bus 0 */ \
|
||||
|
||||
#define HYUNDAI_COMMON_RX_CHECKS(legacy) \
|
||||
{.msg = {{0x260, 0, 8, 100U, .max_counter = 3U, .ignore_quality_flag = true}, \
|
||||
{0x371, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }}}, \
|
||||
{.msg = {{0x386, 0, 8, 100U, .ignore_checksum = (legacy), .ignore_counter = (legacy), .max_counter = (legacy) ? 0U : 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x394, 0, 8, 100U, .ignore_checksum = (legacy), .ignore_counter = (legacy), .max_counter = (legacy) ? 0U : 7U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x251, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x4F1, 0, 4, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define HYUNDAI_SCC11_ADDR_CHECK(scc_bus) \
|
||||
{.msg = {{0x420, (scc_bus), 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .frequency = 50U}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define HYUNDAI_SCC12_ADDR_CHECK(scc_bus) \
|
||||
{.msg = {{0x421, (scc_bus), 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define HYUNDAI_FCEV_GAS_ADDR_CHECK \
|
||||
{.msg = {{0x91, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define HYUNDAI_LDA_BUTTON_ADDR_CHECK \
|
||||
{.msg = {{0x391, 0, 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .frequency = 50U}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define HYUNDAI_NON_SCC_HEV_ADDR_CHECK \
|
||||
{.msg = {{0x595U, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define HYUNDAI_NON_SCC_EV_ADDR_CHECK \
|
||||
{.msg = {{0x592U, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
static const CanMsg HYUNDAI_TX_MSGS[] = {
|
||||
HYUNDAI_COMMON_TX_MSGS(0)
|
||||
};
|
||||
|
||||
static bool hyundai_legacy = false;
|
||||
|
||||
static uint8_t hyundai_get_counter(const CANPacket_t *msg) {
|
||||
|
||||
uint8_t cnt = 0;
|
||||
if (msg->addr == 0x260U) {
|
||||
cnt = (msg->data[7] >> 4) & 0x3U;
|
||||
} else if (msg->addr == 0x386U) {
|
||||
cnt = ((msg->data[3] >> 6) << 2) | (msg->data[1] >> 6);
|
||||
} else if (msg->addr == 0x394U) {
|
||||
cnt = (msg->data[1] >> 5) & 0x7U;
|
||||
} else if (msg->addr == 0x421U) {
|
||||
cnt = msg->data[7] & 0xFU;
|
||||
} else if (msg->addr == 0x4F1U) {
|
||||
cnt = (msg->data[3] >> 4) & 0xFU;
|
||||
} else {
|
||||
}
|
||||
return cnt;
|
||||
}
|
||||
|
||||
static uint32_t hyundai_get_checksum(const CANPacket_t *msg) {
|
||||
|
||||
uint8_t chksum = 0;
|
||||
if (msg->addr == 0x260U) {
|
||||
chksum = msg->data[7] & 0xFU;
|
||||
} else if (msg->addr == 0x386U) {
|
||||
chksum = ((msg->data[7] >> 6) << 2) | (msg->data[5] >> 6);
|
||||
} else if (msg->addr == 0x394U) {
|
||||
chksum = msg->data[6] & 0xFU;
|
||||
} else if (msg->addr == 0x421U) {
|
||||
chksum = msg->data[7] >> 4;
|
||||
} else {
|
||||
}
|
||||
return chksum;
|
||||
}
|
||||
|
||||
static uint32_t hyundai_compute_checksum(const CANPacket_t *msg) {
|
||||
uint8_t chksum = 0;
|
||||
if (msg->addr == 0x386U) {
|
||||
// count the bits
|
||||
for (int i = 0; i < 8; i++) {
|
||||
uint8_t b = msg->data[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 ((msg->addr == 0x394U) && (i == 7)) {
|
||||
continue; // exclude
|
||||
}
|
||||
uint8_t b = msg->data[i];
|
||||
if (((msg->addr == 0x260U) && (i == 7)) || ((msg->addr == 0x394U) && (i == 6)) || ((msg->addr == 0x421U) && (i == 7))) {
|
||||
b &= (msg->addr == 0x421U) ? 0x0FU : 0xF0U; // remove checksum
|
||||
}
|
||||
chksum += (b % 16U) + (b / 16U);
|
||||
}
|
||||
chksum = (16U - (chksum % 16U)) % 16U;
|
||||
}
|
||||
|
||||
return chksum;
|
||||
}
|
||||
|
||||
static void hyundai_rx_hook(const CANPacket_t *msg) {
|
||||
|
||||
// SCC12 is on bus 2 for camera-based SCC cars, bus 0 on all others
|
||||
if (msg->addr == 0x421U) {
|
||||
if (((msg->bus == 0U) && !hyundai_camera_scc) || ((msg->bus == 2U) && hyundai_camera_scc)) {
|
||||
// 2 bits: 13-14
|
||||
int cruise_engaged = (GET_BYTES(msg, 0, 4) >> 13) & 0x3U;
|
||||
hyundai_common_cruise_state_check(cruise_engaged);
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->addr == 0x420U) {
|
||||
if (((msg->bus == 0U) && !hyundai_camera_scc) || ((msg->bus == 2U) && hyundai_camera_scc)) {
|
||||
if (!hyundai_longitudinal) {
|
||||
acc_main_on = GET_BIT(msg, 0U);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->bus == 0U) {
|
||||
if (msg->addr == 0x251U) {
|
||||
int torque_driver_new = (GET_BYTES(msg, 0, 2) & 0x7ffU) - 1024U;
|
||||
// update array of samples
|
||||
update_sample(&torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
if (msg->addr == 0x391U) {
|
||||
mads_button_press = GET_BIT(msg, 4U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
|
||||
}
|
||||
|
||||
// ACC steering wheel buttons
|
||||
if (msg->addr == 0x4F1U) {
|
||||
int cruise_button = msg->data[0] & 0x7U;
|
||||
bool main_button = GET_BIT(msg, 3U);
|
||||
hyundai_common_cruise_buttons_check(cruise_button, main_button);
|
||||
}
|
||||
|
||||
// gas press, different for EV, hybrid, and ICE models
|
||||
if ((msg->addr == 0x371U) && hyundai_ev_gas_signal) {
|
||||
gas_pressed = (((msg->data[4] & 0x7FU) << 1) | (msg->data[3] >> 7)) != 0U;
|
||||
} else if ((msg->addr == 0x371U) && hyundai_hybrid_gas_signal) {
|
||||
gas_pressed = msg->data[7] != 0U;
|
||||
} else if ((msg->addr == 0x91U) && hyundai_fcev_gas_signal) {
|
||||
gas_pressed = msg->data[6] != 0U;
|
||||
} else if ((msg->addr == 0x260U) && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
|
||||
gas_pressed = (msg->data[7] >> 6) != 0U;
|
||||
} else {
|
||||
}
|
||||
|
||||
// sample wheel speed, averaging opposite corners
|
||||
if (msg->addr == 0x386U) {
|
||||
uint32_t front_left_speed = GET_BYTES(msg, 0, 2) & 0x3FFFU;
|
||||
uint32_t rear_right_speed = GET_BYTES(msg, 6, 2) & 0x3FFFU;
|
||||
vehicle_moving = (front_left_speed > HYUNDAI_STANDSTILL_THRSLD) || (rear_right_speed > HYUNDAI_STANDSTILL_THRSLD);
|
||||
}
|
||||
|
||||
if (msg->addr == 0x394U) {
|
||||
brake_pressed = ((msg->data[5] >> 5U) & 0x3U) == 0x2U;
|
||||
}
|
||||
|
||||
if (msg->addr == 0x592U) {
|
||||
acc_main_on = GET_BIT(msg, 34U);
|
||||
bool cruise_engaged = GET_BIT(msg, 35U);
|
||||
hyundai_common_cruise_state_check(cruise_engaged);
|
||||
}
|
||||
|
||||
if (msg->addr == 0x595U) {
|
||||
acc_main_on = GET_BIT(msg, 50U);
|
||||
bool cruise_engaged = GET_BIT(msg, 51U);
|
||||
hyundai_common_cruise_state_check(cruise_engaged);
|
||||
}
|
||||
|
||||
if ((msg->addr == 0x260U) && hyundai_non_scc && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
|
||||
acc_main_on = GET_BIT(msg, 25U);
|
||||
bool cruise_engaged = GET_BIT(msg, 26U);
|
||||
hyundai_common_cruise_state_check(cruise_engaged);
|
||||
}
|
||||
}
|
||||
|
||||
hyundai_common_reset_acc_main_on_mismatches();
|
||||
}
|
||||
|
||||
static bool hyundai_tx_hook(const CANPacket_t *msg) {
|
||||
const TorqueSteeringLimits HYUNDAI_STEERING_LIMITS = HYUNDAI_LIMITS(384, 3, 7);
|
||||
const TorqueSteeringLimits HYUNDAI_STEERING_LIMITS_ALT = HYUNDAI_LIMITS(270, 2, 3);
|
||||
const TorqueSteeringLimits HYUNDAI_STEERING_LIMITS_ALT_2 = HYUNDAI_LIMITS(170, 2, 3);
|
||||
|
||||
bool tx = true;
|
||||
|
||||
// FCA11: Block any potential actuation
|
||||
if (msg->addr == 0x38DU) {
|
||||
int CR_VSM_DecCmd = msg->data[1];
|
||||
bool FCA_CmdAct = GET_BIT(msg, 20U);
|
||||
bool CF_VSM_DecCmdAct = GET_BIT(msg, 31U);
|
||||
|
||||
if ((CR_VSM_DecCmd != 0) || FCA_CmdAct || CF_VSM_DecCmdAct) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->addr == 0x420U) {
|
||||
acc_main_on_tx = GET_BIT(msg, 0U);
|
||||
hyundai_common_acc_main_on_sync();
|
||||
}
|
||||
|
||||
// ACCEL: safety check
|
||||
if (msg->addr == 0x421U) {
|
||||
int desired_accel_raw = (((msg->data[4] & 0x7U) << 8) | msg->data[3]) - 1023U;
|
||||
int desired_accel_val = ((msg->data[5] << 3) | (msg->data[4] >> 5)) - 1023U;
|
||||
|
||||
int aeb_decel_cmd = msg->data[2];
|
||||
bool aeb_req = GET_BIT(msg, 54U);
|
||||
|
||||
bool violation = false;
|
||||
|
||||
violation |= longitudinal_accel_checks(desired_accel_raw, HYUNDAI_LONG_LIMITS);
|
||||
violation |= longitudinal_accel_checks(desired_accel_val, HYUNDAI_LONG_LIMITS);
|
||||
if (!hyundai_escc) {
|
||||
violation |= (aeb_decel_cmd != 0);
|
||||
violation |= aeb_req;
|
||||
}
|
||||
|
||||
if (violation) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// LKA STEER: safety check
|
||||
if (msg->addr == 0x340U) {
|
||||
int desired_torque = ((GET_BYTES(msg, 0, 4) >> 16) & 0x7ffU) - 1024U;
|
||||
bool steer_req = GET_BIT(msg, 27U);
|
||||
|
||||
const TorqueSteeringLimits limits = hyundai_alt_limits_2 ? HYUNDAI_STEERING_LIMITS_ALT_2 :
|
||||
hyundai_alt_limits ? HYUNDAI_STEERING_LIMITS_ALT : HYUNDAI_STEERING_LIMITS;
|
||||
|
||||
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 (msg->addr == 0x7D0U) {
|
||||
if ((GET_BYTES(msg, 0, 4) != 0x00803E02U) || (GET_BYTES(msg, 4, 4) != 0x0U)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// BUTTONS: used for resume spamming and cruise cancellation
|
||||
if ((msg->addr == 0x4F1U) && !hyundai_longitudinal) {
|
||||
int button = msg->data[0] & 0x7U;
|
||||
|
||||
bool allowed_resume = (button == 1) && controls_allowed;
|
||||
bool allowed_set = (button == 2) && controls_allowed;
|
||||
bool allowed_cancel = (button == 4) && cruise_engaged_prev;
|
||||
if (!(allowed_resume || allowed_set || allowed_cancel)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
static safety_config hyundai_init(uint16_t param) {
|
||||
static const CanMsg HYUNDAI_LONG_TX_MSGS[] = {
|
||||
HYUNDAI_LONG_COMMON_TX_MSGS(0)
|
||||
{0x38D, 0, 8, .check_relay = false}, // FCA11 Bus 0
|
||||
{0x483, 0, 8, .check_relay = false}, // FCA12 Bus 0
|
||||
{0x7D0, 0, 8, .check_relay = false}, // radar UDS TX addr Bus 0 (for radar disable)
|
||||
};
|
||||
|
||||
static const CanMsg HYUNDAI_CAMERA_SCC_TX_MSGS[] = {
|
||||
HYUNDAI_COMMON_TX_MSGS(2)
|
||||
};
|
||||
|
||||
static const CanMsg HYUNDAI_CAMERA_SCC_LONG_TX_MSGS[] = {
|
||||
HYUNDAI_LONG_COMMON_TX_MSGS(2)
|
||||
};
|
||||
|
||||
static const CanMsg HYUNDAI_LONG_ESCC_TX_MSGS[] = {
|
||||
HYUNDAI_LONG_COMMON_TX_MSGS(0)
|
||||
};
|
||||
|
||||
hyundai_common_init(param);
|
||||
hyundai_legacy = false;
|
||||
|
||||
safety_config ret;
|
||||
if (hyundai_longitudinal) {
|
||||
// Use CLU11 (buttons) to manage controls allowed instead of SCC cruise state
|
||||
static RxCheck hyundai_long_rx_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
};
|
||||
|
||||
static RxCheck hyundai_lda_button_long_rx_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
HYUNDAI_LDA_BUTTON_ADDR_CHECK
|
||||
};
|
||||
|
||||
static RxCheck hyundai_fcev_long_rx_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
HYUNDAI_FCEV_GAS_ADDR_CHECK
|
||||
};
|
||||
|
||||
static RxCheck hyundai_fcev_lda_button_long_rx_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
HYUNDAI_FCEV_GAS_ADDR_CHECK
|
||||
HYUNDAI_LDA_BUTTON_ADDR_CHECK
|
||||
};
|
||||
|
||||
if (hyundai_fcev_gas_signal) {
|
||||
if (hyundai_has_lda_button) {
|
||||
SET_RX_CHECKS(hyundai_fcev_lda_button_long_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(hyundai_fcev_long_rx_checks, ret);
|
||||
}
|
||||
} else {
|
||||
if (hyundai_has_lda_button) {
|
||||
SET_RX_CHECKS(hyundai_lda_button_long_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(hyundai_long_rx_checks, ret);
|
||||
}
|
||||
}
|
||||
if (hyundai_escc) {
|
||||
SET_TX_MSGS(HYUNDAI_LONG_ESCC_TX_MSGS, ret);
|
||||
} else if (hyundai_camera_scc) {
|
||||
SET_TX_MSGS(HYUNDAI_CAMERA_SCC_LONG_TX_MSGS, ret);
|
||||
} else {
|
||||
SET_TX_MSGS(HYUNDAI_LONG_TX_MSGS, ret);
|
||||
}
|
||||
|
||||
} else if (hyundai_camera_scc) {
|
||||
static RxCheck hyundai_cam_scc_rx_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
HYUNDAI_SCC12_ADDR_CHECK(2)
|
||||
HYUNDAI_SCC11_ADDR_CHECK(2)
|
||||
HYUNDAI_LDA_BUTTON_ADDR_CHECK
|
||||
};
|
||||
|
||||
ret = BUILD_SAFETY_CFG(hyundai_cam_scc_rx_checks, HYUNDAI_CAMERA_SCC_TX_MSGS);
|
||||
} else {
|
||||
static RxCheck hyundai_rx_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
HYUNDAI_SCC12_ADDR_CHECK(0)
|
||||
HYUNDAI_SCC11_ADDR_CHECK(0)
|
||||
};
|
||||
|
||||
static RxCheck hyundai_lda_button_rx_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
HYUNDAI_SCC12_ADDR_CHECK(0)
|
||||
HYUNDAI_SCC11_ADDR_CHECK(0)
|
||||
HYUNDAI_LDA_BUTTON_ADDR_CHECK
|
||||
};
|
||||
|
||||
static RxCheck hyundai_fcev_rx_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
HYUNDAI_SCC12_ADDR_CHECK(0)
|
||||
HYUNDAI_SCC11_ADDR_CHECK(0)
|
||||
HYUNDAI_FCEV_GAS_ADDR_CHECK
|
||||
};
|
||||
|
||||
static RxCheck hyundai_fcev_lda_button_rx_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
HYUNDAI_SCC12_ADDR_CHECK(0)
|
||||
HYUNDAI_SCC11_ADDR_CHECK(0)
|
||||
HYUNDAI_FCEV_GAS_ADDR_CHECK
|
||||
HYUNDAI_LDA_BUTTON_ADDR_CHECK
|
||||
};
|
||||
|
||||
static RxCheck hyundai_non_scc_addr_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
};
|
||||
|
||||
static RxCheck hyundai_non_scc_lda_button_addr_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
HYUNDAI_LDA_BUTTON_ADDR_CHECK
|
||||
};
|
||||
|
||||
static RxCheck hyundai_hev_non_scc_addr_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
HYUNDAI_NON_SCC_HEV_ADDR_CHECK
|
||||
};
|
||||
|
||||
static RxCheck hyundai_hev_non_scc_lda_button_addr_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
HYUNDAI_NON_SCC_HEV_ADDR_CHECK
|
||||
HYUNDAI_LDA_BUTTON_ADDR_CHECK
|
||||
};
|
||||
|
||||
static RxCheck hyundai_ev_non_scc_addr_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
HYUNDAI_NON_SCC_EV_ADDR_CHECK
|
||||
};
|
||||
|
||||
static RxCheck hyundai_ev_non_scc_lda_button_addr_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
HYUNDAI_NON_SCC_EV_ADDR_CHECK
|
||||
HYUNDAI_LDA_BUTTON_ADDR_CHECK
|
||||
};
|
||||
|
||||
SET_TX_MSGS(HYUNDAI_TX_MSGS, ret);
|
||||
if (hyundai_fcev_gas_signal) {
|
||||
if (hyundai_has_lda_button) {
|
||||
SET_RX_CHECKS(hyundai_fcev_lda_button_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(hyundai_fcev_rx_checks, ret);
|
||||
}
|
||||
} else if (hyundai_non_scc) {
|
||||
if (hyundai_ev_gas_signal) {
|
||||
if (hyundai_has_lda_button) {
|
||||
SET_RX_CHECKS(hyundai_ev_non_scc_lda_button_addr_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(hyundai_ev_non_scc_addr_checks, ret);
|
||||
}
|
||||
} else if (hyundai_hybrid_gas_signal) {
|
||||
if (hyundai_has_lda_button) {
|
||||
SET_RX_CHECKS(hyundai_hev_non_scc_lda_button_addr_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(hyundai_hev_non_scc_addr_checks, ret);
|
||||
}
|
||||
} else {
|
||||
if (hyundai_has_lda_button) {
|
||||
SET_RX_CHECKS(hyundai_non_scc_lda_button_addr_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(hyundai_non_scc_addr_checks, ret);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if (hyundai_has_lda_button) {
|
||||
SET_RX_CHECKS(hyundai_lda_button_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(hyundai_rx_checks, ret);
|
||||
}
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
static safety_config hyundai_legacy_init(uint16_t param) {
|
||||
// older hyundai models have less checks due to missing counters and checksums
|
||||
static RxCheck hyundai_legacy_rx_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(true)
|
||||
HYUNDAI_SCC12_ADDR_CHECK(0)
|
||||
HYUNDAI_SCC11_ADDR_CHECK(0)
|
||||
};
|
||||
|
||||
hyundai_common_init(param);
|
||||
hyundai_legacy = true;
|
||||
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,
|
||||
.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,
|
||||
.get_counter = hyundai_get_counter,
|
||||
.get_checksum = hyundai_get_checksum,
|
||||
.compute_checksum = hyundai_compute_checksum,
|
||||
};
|
||||
@@ -0,0 +1,391 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/hyundai_common.h"
|
||||
|
||||
#define HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(bus) \
|
||||
{0x1CF, bus, 8, .check_relay = false}, /* CRUISE_BUTTON */ \
|
||||
|
||||
#define HYUNDAI_CANFD_LKA_STEER_MSG_COMMON_TX_MSGS(a_can, e_can) \
|
||||
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(e_can) \
|
||||
{0x50, a_can, 16, .check_relay = (a_can) == 0}, /* LKAS */ \
|
||||
{0x2A4, a_can, 24, .check_relay = (a_can) == 0}, /* CAM_0x2A4 */ \
|
||||
|
||||
#define HYUNDAI_CANFD_LKA_STEER_MSG_ALT_COMMON_TX_MSGS(a_can, e_can) \
|
||||
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(e_can) \
|
||||
{0x110, a_can, 32, .check_relay = (a_can) == 0}, /* LKAS_ALT */ \
|
||||
{0x362, a_can, 32, .check_relay = (a_can) == 0}, /* CAM_0x362 */ \
|
||||
|
||||
#define HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(e_can) \
|
||||
{0x12A, e_can, 16, .check_relay = (e_can) == 0}, /* LFA */ \
|
||||
{0x1E0, e_can, 16, .check_relay = (e_can) == 0}, /* LFAHDA_CLUSTER */ \
|
||||
|
||||
#define HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(e_can, longitudinal) \
|
||||
{0x1A0, e_can, 32, .check_relay = (longitudinal)}, /* SCC_CONTROL */ \
|
||||
|
||||
// *** Addresses checked in rx hook ***
|
||||
// EV, ICE, HYBRID: ACCELERATOR (0x35), ACCELERATOR_BRAKE_ALT (0x100), ACCELERATOR_ALT (0x105)
|
||||
#define HYUNDAI_CANFD_COMMON_RX_CHECKS(pt_bus) \
|
||||
{.msg = {{0x35, (pt_bus), 32, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}, \
|
||||
{0x100, (pt_bus), 32, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}, \
|
||||
{0x105, (pt_bus), 32, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}}}, \
|
||||
{.msg = {{0x175, (pt_bus), 24, 50U, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0xa0, (pt_bus), 24, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0xea, (pt_bus), 24, 100U, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(pt_bus) \
|
||||
HYUNDAI_CANFD_COMMON_RX_CHECKS(pt_bus) \
|
||||
{.msg = {{0x1cf, (pt_bus), 8, 50U, .ignore_checksum = true, .max_counter = 0xfU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define HYUNDAI_CANFD_ALT_BUTTONS_RX_CHECKS(pt_bus) \
|
||||
HYUNDAI_CANFD_COMMON_RX_CHECKS(pt_bus) \
|
||||
{.msg = {{0x1aa, (pt_bus), 16, 50U, .ignore_checksum = true, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
// SCC_CONTROL (from ADAS unit or camera)
|
||||
#define HYUNDAI_CANFD_SCC_ADDR_CHECK(scc_bus) \
|
||||
{.msg = {{0x1a0, (scc_bus), 32, 50U, .max_counter = 0xffU, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
static bool hyundai_canfd_alt_buttons = false;
|
||||
static bool hyundai_canfd_lka_steer_msg_alt = false;
|
||||
|
||||
static unsigned int hyundai_canfd_get_lka_addr(void) {
|
||||
return hyundai_canfd_lka_steer_msg_alt ? 0x110U : 0x50U;
|
||||
}
|
||||
|
||||
static uint8_t hyundai_canfd_get_counter(const CANPacket_t *msg) {
|
||||
uint8_t ret = 0;
|
||||
if (GET_LEN(msg) == 8U) {
|
||||
ret = msg->data[1] >> 4;
|
||||
} else {
|
||||
ret = msg->data[2];
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
static uint32_t hyundai_canfd_get_checksum(const CANPacket_t *msg) {
|
||||
uint32_t chksum = msg->data[0] | (msg->data[1] << 8);
|
||||
return chksum;
|
||||
}
|
||||
|
||||
static void hyundai_canfd_rx_hook(const CANPacket_t *msg) {
|
||||
|
||||
const unsigned pt_bus = hyundai_canfd_lka_steer_msg ? 1U : 0U;
|
||||
const unsigned int scc_bus = hyundai_camera_scc ? 2U : pt_bus;
|
||||
|
||||
if (msg->bus == pt_bus) {
|
||||
// driver torque
|
||||
if (msg->addr == 0xeaU) {
|
||||
int torque_driver_new = ((msg->data[11] & 0x1fU) << 8U) | msg->data[10];
|
||||
torque_driver_new -= 4095;
|
||||
update_sample(&torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
// cruise buttons
|
||||
const unsigned int button_addr = hyundai_canfd_alt_buttons ? 0x1aaU : 0x1cfU;
|
||||
if (msg->addr == button_addr) {
|
||||
bool main_button = false;
|
||||
int cruise_button = 0;
|
||||
if (msg->addr == 0x1cfU) {
|
||||
cruise_button = msg->data[2] & 0x7U;
|
||||
main_button = GET_BIT(msg, 19U);
|
||||
mads_button_press = GET_BIT(msg, 23U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
|
||||
} else {
|
||||
cruise_button = (msg->data[4] >> 4) & 0x7U;
|
||||
main_button = GET_BIT(msg, 34U);
|
||||
mads_button_press = GET_BIT(msg, 39U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
|
||||
}
|
||||
hyundai_common_cruise_buttons_check(cruise_button, main_button);
|
||||
}
|
||||
|
||||
// gas press, different for EV, hybrid, and ICE models
|
||||
if ((msg->addr == 0x35U) && hyundai_ev_gas_signal) {
|
||||
gas_pressed = msg->data[5] != 0U;
|
||||
} else if ((msg->addr == 0x105U) && hyundai_hybrid_gas_signal) {
|
||||
gas_pressed = GET_BIT(msg, 103U) || (msg->data[13] != 0U) || GET_BIT(msg, 112U);
|
||||
} else if ((msg->addr == 0x100U) && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
|
||||
gas_pressed = GET_BIT(msg, 176U);
|
||||
} else {
|
||||
}
|
||||
|
||||
// brake press
|
||||
if (msg->addr == 0x175U) {
|
||||
brake_pressed = GET_BIT(msg, 81U);
|
||||
}
|
||||
|
||||
// vehicle moving
|
||||
if (msg->addr == 0xa0U) {
|
||||
uint32_t fl = (GET_BYTES(msg, 8, 2)) & 0x3FFFU;
|
||||
uint32_t fr = (GET_BYTES(msg, 10, 2)) & 0x3FFFU;
|
||||
uint32_t rl = (GET_BYTES(msg, 12, 2)) & 0x3FFFU;
|
||||
uint32_t rr = (GET_BYTES(msg, 14, 2)) & 0x3FFFU;
|
||||
vehicle_moving = (fl > HYUNDAI_STANDSTILL_THRSLD) || (fr > HYUNDAI_STANDSTILL_THRSLD) ||
|
||||
(rl > HYUNDAI_STANDSTILL_THRSLD) || (rr > HYUNDAI_STANDSTILL_THRSLD);
|
||||
|
||||
// average of all 4 wheel speeds. Conversion: raw * 0.03125 / 3.6 = m/s
|
||||
UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4.0 * 0.03125 * KPH_TO_MS);
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->bus == scc_bus) {
|
||||
// cruise state
|
||||
if ((msg->addr == 0x1a0U) && !hyundai_longitudinal) {
|
||||
// 1=enabled, 2=driver override
|
||||
int cruise_status = ((msg->data[8] >> 4) & 0x7U);
|
||||
bool cruise_engaged = (cruise_status == 1) || (cruise_status == 2);
|
||||
hyundai_common_cruise_state_check(cruise_engaged);
|
||||
acc_main_on = GET_BIT(msg, 66U);
|
||||
}
|
||||
}
|
||||
|
||||
hyundai_common_reset_acc_main_on_mismatches();
|
||||
}
|
||||
|
||||
static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) {
|
||||
const TorqueSteeringLimits HYUNDAI_CANFD_STEERING_LIMITS = {
|
||||
.max_torque = 270,
|
||||
.max_rt_delta = 112,
|
||||
.max_rate_up = 2,
|
||||
.max_rate_down = 3,
|
||||
.driver_torque_allowance = 250,
|
||||
.driver_torque_multiplier = 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,
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
|
||||
// steering
|
||||
const unsigned int steer_addr = (hyundai_canfd_lka_steer_msg && !hyundai_longitudinal) ? hyundai_canfd_get_lka_addr() : 0x12aU;
|
||||
if (msg->addr == steer_addr) {
|
||||
int desired_torque = (((msg->data[6] & 0xFU) << 7U) | (msg->data[5] >> 1U)) - 1024U;
|
||||
bool steer_req = GET_BIT(msg, 52U);
|
||||
|
||||
if (steer_torque_cmd_checks(desired_torque, steer_req, HYUNDAI_CANFD_STEERING_LIMITS)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// cruise buttons check
|
||||
if (msg->addr == 0x1cfU) {
|
||||
int button = msg->data[2] & 0x7U;
|
||||
bool is_cancel = (button == HYUNDAI_BTN_CANCEL);
|
||||
bool is_resume = (button == HYUNDAI_BTN_RESUME);
|
||||
bool is_set = (button == HYUNDAI_BTN_SET);
|
||||
|
||||
bool allowed = (is_cancel && cruise_engaged_prev) || ((is_resume || is_set) && controls_allowed);
|
||||
if (!allowed) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// UDS: only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
|
||||
if (((msg->addr == 0x730U) && hyundai_canfd_lka_steer_msg) || ((msg->addr == 0x7D0U) && !hyundai_camera_scc)) {
|
||||
if ((GET_BYTES(msg, 0, 4) != 0x00803E02U) || (GET_BYTES(msg, 4, 4) != 0x0U)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// ACCEL: safety check
|
||||
if (msg->addr == 0x1a0U) {
|
||||
int desired_accel_raw = (((msg->data[17] & 0x7U) << 8) | msg->data[16]) - 1023U;
|
||||
int desired_accel_val = ((msg->data[18] << 4) | (msg->data[17] >> 4)) - 1023U;
|
||||
|
||||
bool violation = false;
|
||||
|
||||
if (hyundai_longitudinal) {
|
||||
violation |= longitudinal_accel_checks(desired_accel_raw, HYUNDAI_LONG_LIMITS);
|
||||
violation |= longitudinal_accel_checks(desired_accel_val, HYUNDAI_LONG_LIMITS);
|
||||
} else {
|
||||
// only used to cancel on here
|
||||
const int acc_mode = (msg->data[8] >> 4) & 0x7U;
|
||||
if (acc_mode != 4) {
|
||||
violation = true;
|
||||
}
|
||||
|
||||
if ((desired_accel_raw != 0) || (desired_accel_val != 0)) {
|
||||
violation = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (violation) {
|
||||
tx = false;
|
||||
}
|
||||
|
||||
acc_main_on_tx = GET_BIT(msg, 66U);
|
||||
hyundai_common_acc_main_on_sync();
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
static safety_config hyundai_canfd_init(uint16_t param) {
|
||||
const uint16_t HYUNDAI_PARAM_CANFD_LKA_STEER_MSG_ALT = 128;
|
||||
const uint16_t HYUNDAI_PARAM_CANFD_ALT_BUTTONS = 32;
|
||||
|
||||
static const CanMsg HYUNDAI_CANFD_LKA_STEER_MSG_TX_MSGS[] = {
|
||||
HYUNDAI_CANFD_LKA_STEER_MSG_COMMON_TX_MSGS(0, 1)
|
||||
};
|
||||
|
||||
static const CanMsg HYUNDAI_CANFD_LKA_STEER_MSG_ALT_TX_MSGS[] = {
|
||||
HYUNDAI_CANFD_LKA_STEER_MSG_ALT_COMMON_TX_MSGS(0, 1)
|
||||
};
|
||||
|
||||
static const CanMsg HYUNDAI_CANFD_LKA_STEER_MSG_LONG_TX_MSGS[] = {
|
||||
HYUNDAI_CANFD_LKA_STEER_MSG_COMMON_TX_MSGS(0, 1)
|
||||
HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(1)
|
||||
HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(1, true)
|
||||
{0x51, 0, 32, .check_relay = false}, // ADRV_0x51
|
||||
{0x730, 1, 8, .check_relay = false}, // tester present for ADAS ECU disable
|
||||
{0x160, 1, 16, .check_relay = false}, // ADRV_0x160
|
||||
{0x1EA, 1, 32, .check_relay = false}, // ADRV_0x1ea
|
||||
{0x200, 1, 8, .check_relay = false}, // ADRV_0x200
|
||||
{0x345, 1, 8, .check_relay = false}, // ADRV_0x345
|
||||
{0x1DA, 1, 32, .check_relay = false}, // ADRV_0x1da
|
||||
};
|
||||
|
||||
static const CanMsg HYUNDAI_CANFD_LFA_STEERING_TX_MSGS[] = {
|
||||
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(2)
|
||||
HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(0)
|
||||
HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(0, false)
|
||||
};
|
||||
|
||||
// ADRV_0x160 is checked for radar liveness
|
||||
static const CanMsg HYUNDAI_CANFD_LFA_STEERING_LONG_TX_MSGS[] = {
|
||||
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(2)
|
||||
HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(0)
|
||||
HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(0, true)
|
||||
{0x160, 0, 16, .check_relay = true}, // ADRV_0x160
|
||||
{0x7D0, 0, 8, .check_relay = false}, // tester present for radar ECU disable
|
||||
};
|
||||
|
||||
// ADRV_0x160 is checked for relay malfunction
|
||||
#define HYUNDAI_CANFD_LFA_STEERING_CAMERA_SCC_TX_MSGS(longitudinal) \
|
||||
HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(2) \
|
||||
HYUNDAI_CANFD_LFA_STEERING_COMMON_TX_MSGS(0) \
|
||||
HYUNDAI_CANFD_SCC_CONTROL_COMMON_TX_MSGS(0, (longitudinal)) \
|
||||
{0x160, 0, 16, .check_relay = (longitudinal)}, /* ADRV_0x160 */ \
|
||||
|
||||
hyundai_common_init(param);
|
||||
|
||||
gen_crc_lookup_table_16(0x1021, hyundai_canfd_crc_lut);
|
||||
hyundai_canfd_alt_buttons = GET_FLAG(param, HYUNDAI_PARAM_CANFD_ALT_BUTTONS);
|
||||
hyundai_canfd_lka_steer_msg_alt = GET_FLAG(param, HYUNDAI_PARAM_CANFD_LKA_STEER_MSG_ALT);
|
||||
|
||||
safety_config ret;
|
||||
if (hyundai_longitudinal) {
|
||||
if (hyundai_canfd_lka_steer_msg) {
|
||||
static RxCheck hyundai_canfd_lka_steer_msg_long_rx_checks[] = {
|
||||
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(1)
|
||||
};
|
||||
|
||||
ret = BUILD_SAFETY_CFG(hyundai_canfd_lka_steer_msg_long_rx_checks, HYUNDAI_CANFD_LKA_STEER_MSG_LONG_TX_MSGS);
|
||||
|
||||
} else {
|
||||
// Longitudinal checks for LFA steering
|
||||
static RxCheck hyundai_canfd_long_rx_checks[] = {
|
||||
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(0)
|
||||
};
|
||||
|
||||
static RxCheck hyundai_canfd_alt_buttons_long_rx_checks[] = {
|
||||
HYUNDAI_CANFD_ALT_BUTTONS_RX_CHECKS(0)
|
||||
};
|
||||
|
||||
static CanMsg hyundai_canfd_lfa_steering_camera_scc_tx_msgs[] = {
|
||||
HYUNDAI_CANFD_LFA_STEERING_CAMERA_SCC_TX_MSGS(true)
|
||||
};
|
||||
|
||||
if (hyundai_canfd_alt_buttons) {
|
||||
SET_RX_CHECKS(hyundai_canfd_alt_buttons_long_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(hyundai_canfd_long_rx_checks, ret);
|
||||
}
|
||||
|
||||
if (hyundai_camera_scc) {
|
||||
SET_TX_MSGS(hyundai_canfd_lfa_steering_camera_scc_tx_msgs, ret);
|
||||
} else {
|
||||
SET_TX_MSGS(HYUNDAI_CANFD_LFA_STEERING_LONG_TX_MSGS, ret);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
if (hyundai_canfd_lka_steer_msg) {
|
||||
// *** LKA steering checks ***
|
||||
// E-CAN is on bus 1, SCC messages are sent on cars with ADRV ECU.
|
||||
// Does not use the alt buttons message
|
||||
static RxCheck hyundai_canfd_lka_steer_msg_rx_checks[] = {
|
||||
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(1)
|
||||
HYUNDAI_CANFD_SCC_ADDR_CHECK(1)
|
||||
};
|
||||
|
||||
SET_RX_CHECKS(hyundai_canfd_lka_steer_msg_rx_checks, ret);
|
||||
if (hyundai_canfd_lka_steer_msg_alt) {
|
||||
SET_TX_MSGS(HYUNDAI_CANFD_LKA_STEER_MSG_ALT_TX_MSGS, ret);
|
||||
} else {
|
||||
SET_TX_MSGS(HYUNDAI_CANFD_LKA_STEER_MSG_TX_MSGS, ret);
|
||||
}
|
||||
|
||||
} else if (!hyundai_camera_scc) {
|
||||
// Radar sends SCC messages on these cars instead of camera
|
||||
static RxCheck hyundai_canfd_radar_scc_rx_checks[] = {
|
||||
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(0)
|
||||
HYUNDAI_CANFD_SCC_ADDR_CHECK(0)
|
||||
};
|
||||
|
||||
static RxCheck hyundai_canfd_alt_buttons_radar_scc_rx_checks[] = {
|
||||
HYUNDAI_CANFD_ALT_BUTTONS_RX_CHECKS(0)
|
||||
HYUNDAI_CANFD_SCC_ADDR_CHECK(0)
|
||||
};
|
||||
|
||||
SET_TX_MSGS(HYUNDAI_CANFD_LFA_STEERING_TX_MSGS, ret);
|
||||
|
||||
if (hyundai_canfd_alt_buttons) {
|
||||
SET_RX_CHECKS(hyundai_canfd_alt_buttons_radar_scc_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(hyundai_canfd_radar_scc_rx_checks, ret);
|
||||
}
|
||||
|
||||
} else {
|
||||
// *** LFA steering checks ***
|
||||
// Camera sends SCC messages on LFA steering cars.
|
||||
// Both button messages exist on some platforms, so we ensure we track the correct one using flag
|
||||
static RxCheck hyundai_canfd_rx_checks[] = {
|
||||
HYUNDAI_CANFD_STD_BUTTONS_RX_CHECKS(0)
|
||||
HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
|
||||
};
|
||||
|
||||
static RxCheck hyundai_canfd_alt_buttons_rx_checks[] = {
|
||||
HYUNDAI_CANFD_ALT_BUTTONS_RX_CHECKS(0)
|
||||
HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
|
||||
};
|
||||
|
||||
static CanMsg hyundai_canfd_lfa_steering_camera_scc_tx_msgs[] = {
|
||||
HYUNDAI_CANFD_LFA_STEERING_CAMERA_SCC_TX_MSGS(false)
|
||||
};
|
||||
|
||||
SET_TX_MSGS(hyundai_canfd_lfa_steering_camera_scc_tx_msgs, ret);
|
||||
|
||||
if (hyundai_canfd_alt_buttons) {
|
||||
SET_RX_CHECKS(hyundai_canfd_alt_buttons_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(hyundai_canfd_rx_checks, ret);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
const safety_hooks hyundai_canfd_hooks = {
|
||||
.init = hyundai_canfd_init,
|
||||
.rx = hyundai_canfd_rx_hook,
|
||||
.tx = hyundai_canfd_tx_hook,
|
||||
.get_counter = hyundai_canfd_get_counter,
|
||||
.get_checksum = hyundai_canfd_get_checksum,
|
||||
.compute_checksum = hyundai_common_canfd_compute_checksum,
|
||||
};
|
||||
@@ -0,0 +1,206 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
extern uint16_t hyundai_canfd_crc_lut[256];
|
||||
uint16_t hyundai_canfd_crc_lut[256];
|
||||
|
||||
static const uint8_t HYUNDAI_PREV_BUTTON_SAMPLES = 8; // roughly 160 ms
|
||||
|
||||
extern const uint32_t HYUNDAI_STANDSTILL_THRSLD;
|
||||
const uint32_t HYUNDAI_STANDSTILL_THRSLD = 12; // 0.375 kph
|
||||
|
||||
enum {
|
||||
HYUNDAI_BTN_NONE = 0,
|
||||
HYUNDAI_BTN_RESUME = 1,
|
||||
HYUNDAI_BTN_SET = 2,
|
||||
HYUNDAI_BTN_CANCEL = 4,
|
||||
};
|
||||
|
||||
enum {
|
||||
HYUNDAI_PARAM_SP_ESCC = 1,
|
||||
HYUNDAI_PARAM_SP_LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE = 2,
|
||||
HYUNDAI_PARAM_SP_HAS_LDA_BUTTON = 4,
|
||||
HYUNDAI_PARAM_SP_NON_SCC = 8,
|
||||
};
|
||||
|
||||
// common state
|
||||
extern bool hyundai_ev_gas_signal;
|
||||
bool hyundai_ev_gas_signal = false;
|
||||
|
||||
extern bool hyundai_hybrid_gas_signal;
|
||||
bool hyundai_hybrid_gas_signal = false;
|
||||
|
||||
extern bool hyundai_longitudinal;
|
||||
bool hyundai_longitudinal = false;
|
||||
|
||||
extern bool hyundai_camera_scc;
|
||||
bool hyundai_camera_scc = false;
|
||||
|
||||
extern bool hyundai_canfd_lka_steer_msg;
|
||||
bool hyundai_canfd_lka_steer_msg = false;
|
||||
|
||||
extern bool hyundai_alt_limits;
|
||||
bool hyundai_alt_limits = false;
|
||||
|
||||
extern bool hyundai_fcev_gas_signal;
|
||||
bool hyundai_fcev_gas_signal = false;
|
||||
|
||||
extern bool hyundai_alt_limits_2;
|
||||
bool hyundai_alt_limits_2 = false;
|
||||
|
||||
// ESCC
|
||||
extern bool hyundai_escc;
|
||||
bool hyundai_escc = false;
|
||||
|
||||
extern bool hyundai_longitudinal_main_cruise_toggleable;
|
||||
bool hyundai_longitudinal_main_cruise_toggleable = false;
|
||||
|
||||
extern bool hyundai_has_lda_button;
|
||||
bool hyundai_has_lda_button = false;
|
||||
|
||||
extern bool hyundai_non_scc;
|
||||
bool hyundai_non_scc = false;
|
||||
|
||||
static uint8_t hyundai_last_button_interaction; // button messages since the user pressed an enable button
|
||||
|
||||
static bool main_button_prev;
|
||||
static bool acc_main_on_prev;
|
||||
static bool acc_main_on_tx;
|
||||
static uint32_t acc_main_on_mismatches;
|
||||
|
||||
void hyundai_common_init(uint16_t param) {
|
||||
const uint16_t HYUNDAI_PARAM_EV_GAS = 1;
|
||||
const uint16_t HYUNDAI_PARAM_HYBRID_GAS = 2;
|
||||
const uint16_t HYUNDAI_PARAM_CAMERA_SCC = 8;
|
||||
const uint16_t HYUNDAI_PARAM_CANFD_LKA_STEER_MSG = 16;
|
||||
const uint16_t HYUNDAI_PARAM_ALT_LIMITS = 64; // TODO: shift this down with the rest of the common flags
|
||||
const uint16_t HYUNDAI_PARAM_FCEV_GAS = 256;
|
||||
const uint16_t HYUNDAI_PARAM_ALT_LIMITS_2 = 512;
|
||||
|
||||
hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS);
|
||||
hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS);
|
||||
hyundai_camera_scc = GET_FLAG(param, HYUNDAI_PARAM_CAMERA_SCC);
|
||||
hyundai_canfd_lka_steer_msg = GET_FLAG(param, HYUNDAI_PARAM_CANFD_LKA_STEER_MSG);
|
||||
hyundai_alt_limits = GET_FLAG(param, HYUNDAI_PARAM_ALT_LIMITS);
|
||||
hyundai_fcev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_FCEV_GAS);
|
||||
hyundai_alt_limits_2 = GET_FLAG(param, HYUNDAI_PARAM_ALT_LIMITS_2);
|
||||
|
||||
hyundai_escc = GET_FLAG(current_safety_param_sp, HYUNDAI_PARAM_SP_ESCC);
|
||||
hyundai_longitudinal_main_cruise_toggleable = GET_FLAG(current_safety_param_sp, HYUNDAI_PARAM_SP_LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE);
|
||||
hyundai_has_lda_button = GET_FLAG(current_safety_param_sp, HYUNDAI_PARAM_SP_HAS_LDA_BUTTON);
|
||||
hyundai_non_scc = GET_FLAG(current_safety_param_sp, HYUNDAI_PARAM_SP_NON_SCC);
|
||||
|
||||
hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES;
|
||||
|
||||
main_button_prev = false;
|
||||
acc_main_on_prev = false;
|
||||
acc_main_on_tx = false;
|
||||
acc_main_on_mismatches = 0U;
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
const uint16_t HYUNDAI_PARAM_LONGITUDINAL = 4;
|
||||
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 ((cruise_button == HYUNDAI_BTN_RESUME) || (cruise_button == HYUNDAI_BTN_SET) || (cruise_button == HYUNDAI_BTN_CANCEL) || main_button) {
|
||||
hyundai_last_button_interaction = 0U;
|
||||
} else {
|
||||
hyundai_last_button_interaction = SAFETY_MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
// toggle main cruise state on rising edge of main cruise button
|
||||
if (main_button && !main_button_prev && hyundai_longitudinal_main_cruise_toggleable) {
|
||||
acc_main_on = !acc_main_on;
|
||||
}
|
||||
|
||||
cruise_button_prev = cruise_button;
|
||||
main_button_prev = main_button;
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t hyundai_common_canfd_compute_checksum(const CANPacket_t *msg) {
|
||||
int len = GET_LEN(msg);
|
||||
uint32_t address = msg->addr;
|
||||
|
||||
uint16_t crc = 0;
|
||||
|
||||
for (int i = 2; i < len; i++) {
|
||||
crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ msg->data[i]];
|
||||
}
|
||||
|
||||
// Add address to crc
|
||||
crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ ((address >> 0U) & 0xFFU)];
|
||||
crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ ((address >> 8U) & 0xFFU)];
|
||||
|
||||
if (len == 24) {
|
||||
crc ^= 0x819dU;
|
||||
} else if (len == 32) {
|
||||
crc ^= 0x9f5bU;
|
||||
} else {
|
||||
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
// reset mismatches on rising edge of acc_main_on to avoid rare race conditions when using non-PCM main cruise state
|
||||
void hyundai_common_reset_acc_main_on_mismatches(void) {
|
||||
if (acc_main_on && !acc_main_on_prev) {
|
||||
acc_main_on_mismatches = 0U;
|
||||
}
|
||||
|
||||
acc_main_on_prev = acc_main_on;
|
||||
}
|
||||
|
||||
// exit lateral controls allowed if sunnypilot and panda main cruise states are desynced
|
||||
void hyundai_common_acc_main_on_sync(void) {
|
||||
if (acc_main_on && !acc_main_on_tx) {
|
||||
acc_main_on_mismatches += 1U;
|
||||
|
||||
if (acc_main_on_mismatches >= 3U) { // desync by 3 frames
|
||||
acc_main_on = false;
|
||||
mads_exit_controls(MADS_DISENGAGE_REASON_NON_PCM_ACC_MAIN_DESYNC);
|
||||
}
|
||||
} else {
|
||||
acc_main_on_mismatches = 0U;
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t get_acc_main_on_mismatches(void) {
|
||||
return acc_main_on_mismatches;
|
||||
}
|
||||
@@ -0,0 +1,106 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// CAN msgs we care about
|
||||
#define MAZDA_LKAS 0x243U
|
||||
#define MAZDA_LKAS_HUD 0x440U
|
||||
#define MAZDA_CRZ_CTRL 0x21cU
|
||||
#define MAZDA_CRZ_BTNS 0x09dU
|
||||
#define MAZDA_STEER_TORQUE 0x240U
|
||||
#define MAZDA_ENGINE_DATA 0x202U
|
||||
#define MAZDA_PEDALS 0x165U
|
||||
|
||||
// CAN bus numbers
|
||||
#define MAZDA_MAIN 0
|
||||
#define MAZDA_CAM 2
|
||||
|
||||
// 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 *msg) {
|
||||
if ((int)msg->bus == MAZDA_MAIN) {
|
||||
if (msg->addr == MAZDA_ENGINE_DATA) {
|
||||
// sample speed: scale by 0.01 to get kph
|
||||
int speed = (msg->data[2] << 8) | msg->data[3];
|
||||
vehicle_moving = speed > 10; // moving when speed > 0.1 kph
|
||||
}
|
||||
|
||||
if (msg->addr == MAZDA_STEER_TORQUE) {
|
||||
int torque_driver_new = msg->data[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 (msg->addr == MAZDA_CRZ_CTRL) {
|
||||
bool cruise_engaged = msg->data[0] & 0x8U;
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
acc_main_on = GET_BIT(msg, 17U);
|
||||
}
|
||||
|
||||
if (msg->addr == MAZDA_ENGINE_DATA) {
|
||||
gas_pressed = (msg->data[4] || (msg->data[5] & 0xF0U));
|
||||
}
|
||||
|
||||
if (msg->addr == MAZDA_PEDALS) {
|
||||
brake_pressed = (msg->data[0] & 0x10U);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool mazda_tx_hook(const CANPacket_t *msg) {
|
||||
const TorqueSteeringLimits MAZDA_STEERING_LIMITS = {
|
||||
.max_torque = 800,
|
||||
.max_rate_up = 10,
|
||||
.max_rate_down = 25,
|
||||
.max_rt_delta = 300,
|
||||
.driver_torque_multiplier = 1,
|
||||
.driver_torque_allowance = 15,
|
||||
.type = TorqueDriverLimited,
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
// Check if msg is sent on the main BUS
|
||||
if (msg->bus == (unsigned char)MAZDA_MAIN) {
|
||||
// steer cmd checks
|
||||
if (msg->addr == MAZDA_LKAS) {
|
||||
int desired_torque = (((msg->data[0] & 0x0FU) << 8) | msg->data[1]) - 2048U;
|
||||
|
||||
if (steer_torque_cmd_checks(desired_torque, -1, MAZDA_STEERING_LIMITS)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// cruise buttons check
|
||||
if (msg->addr == MAZDA_CRZ_BTNS) {
|
||||
// allow resume spamming while controls allowed, but
|
||||
// only allow cancel while controls not allowed
|
||||
bool cancel_cmd = (msg->data[0] == 0x1U);
|
||||
if (!controls_allowed && !cancel_cmd) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
static safety_config mazda_init(uint16_t param) {
|
||||
static const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8, .check_relay = true}, {MAZDA_CRZ_BTNS, 0, 8, .check_relay = false}, {MAZDA_LKAS_HUD, 0, 8, .check_relay = true}};
|
||||
|
||||
static RxCheck mazda_rx_checks[] = {
|
||||
{.msg = {{MAZDA_CRZ_CTRL, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MAZDA_CRZ_BTNS, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MAZDA_STEER_TORQUE, 0, 8, 83U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MAZDA_ENGINE_DATA, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MAZDA_PEDALS, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
SAFETY_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,
|
||||
};
|
||||
@@ -0,0 +1,190 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
#define NISSAN_COMMON_RX_CHECKS \
|
||||
{.msg = {{0x185, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* STEER_TORQUE_SENSOR */ \
|
||||
{.msg = {{0x285, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \
|
||||
{0x285, 1, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }}}, /* WHEEL_SPEEDS_REAR */ \
|
||||
{.msg = {{0x30f, 2, 3, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \
|
||||
{0x30f, 1, 3, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }}}, /* CRUISE_STATE */ \
|
||||
{.msg = {{0x15c, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \
|
||||
{0x15c, 1, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \
|
||||
{0x239, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}}}, /* GAS_PEDAL */ \
|
||||
{.msg = {{0x454, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \
|
||||
{0x454, 1, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \
|
||||
{0x1cc, 0, 4, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}}}, /* DOORS_LIGHTS / BRAKE */ \
|
||||
|
||||
#define NISSAN_PRO_PILOT_RX_CHECKS(alt_eps_bus) \
|
||||
{.msg = {{0x1B6, alt_eps_bus, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
static bool nissan_alt_eps = false;
|
||||
|
||||
static const AngleSteeringLimits NISSAN_STEERING_LIMITS = {
|
||||
.max_angle = 60000, // 600 deg, reasonable limit
|
||||
.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}
|
||||
},
|
||||
};
|
||||
|
||||
static void nissan_rx_hook(const CANPacket_t *msg) {
|
||||
|
||||
// Altima: on camera bus, others: on pt bus
|
||||
if (msg->bus == 0U) {
|
||||
if (msg->addr == 0x185U) {
|
||||
// Current steering angle
|
||||
int angle_meas_new = (msg->data[2] << 10) | (msg->data[3] << 2) | (msg->data[4] >> 6);
|
||||
// Factor is -0.01, offset is 1310. Flip to correct sign, but keep units in CAN scale
|
||||
angle_meas_new = -angle_meas_new + (1310.0f * NISSAN_STEERING_LIMITS.angle_deg_to_can);
|
||||
|
||||
// update array of samples
|
||||
update_sample(&angle_meas, angle_meas_new);
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->bus == (nissan_alt_eps ? 1U : 0U)) {
|
||||
if (msg->addr == 0x285U) {
|
||||
// Get current speed and standstill
|
||||
uint16_t right_rear = (msg->data[0] << 8) | (msg->data[1]);
|
||||
uint16_t left_rear = (msg->data[2] << 8) | (msg->data[3]);
|
||||
vehicle_moving = (right_rear | left_rear) != 0U;
|
||||
UPDATE_VEHICLE_SPEED((right_rear + left_rear) / 2.0 * 0.005 * KPH_TO_MS);
|
||||
}
|
||||
|
||||
// X-Trail 0x15c, Leaf 0x239
|
||||
if ((msg->addr == 0x15cU) || (msg->addr == 0x239U)) {
|
||||
if (msg->addr == 0x15cU){
|
||||
gas_pressed = ((msg->data[5] << 2) | ((msg->data[6] >> 6) & 0x3U)) > 3U;
|
||||
} else {
|
||||
gas_pressed = msg->data[0] > 3U;
|
||||
}
|
||||
}
|
||||
|
||||
// X-trail 0x454, Leaf 0x239
|
||||
if ((msg->addr == 0x454U) || (msg->addr == 0x239U)) {
|
||||
if (msg->addr == 0x454U){
|
||||
brake_pressed = (msg->data[2] & 0x80U) != 0U;
|
||||
} else {
|
||||
brake_pressed = ((msg->data[4] >> 5) & 1U) != 0U;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Handle cruise enabled
|
||||
if ((msg->addr == 0x30fU) && (msg->bus == (nissan_alt_eps ? 1U : 2U))) {
|
||||
bool cruise_engaged = (msg->data[0] >> 3) & 1U;
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
}
|
||||
|
||||
if ((msg->addr == 0x239U) && (msg->bus == 0U)) {
|
||||
acc_main_on = GET_BIT(msg, 17U);
|
||||
}
|
||||
|
||||
if ((msg->addr == 0x1B6U) && (msg->bus == (nissan_alt_eps ? 2U : 1U))) {
|
||||
acc_main_on = GET_BIT(msg, 36U);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static bool nissan_tx_hook(const CANPacket_t *msg) {
|
||||
bool tx = true;
|
||||
bool violation = false;
|
||||
|
||||
// steer cmd checks
|
||||
if (msg->addr == 0x169U) {
|
||||
int desired_angle = ((msg->data[0] << 10) | (msg->data[1] << 2) | ((msg->data[2] >> 6) & 0x3U));
|
||||
bool lka_active = (msg->data[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 (msg->addr == 0x20bU) {
|
||||
// Violation of any button other than cancel is pressed
|
||||
violation |= ((msg->data[1] & 0x3dU) > 0U);
|
||||
}
|
||||
|
||||
if (violation) {
|
||||
tx = false;
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
|
||||
static safety_config nissan_init(uint16_t param) {
|
||||
static const CanMsg NISSAN_TX_MSGS_LEAF[] = {
|
||||
{0x169, 0, 8, .check_relay = true}, // LKAS
|
||||
{0x2b1, 0, 8, .check_relay = true}, // PROPILOT_HUD
|
||||
{0x4cc, 0, 8, .check_relay = true}, // PROPILOT_HUD_INFO_MSG
|
||||
{0x239, 2, 8, .check_relay = true} // CRUISE_THROTTLE (Leaf)
|
||||
};
|
||||
|
||||
static const CanMsg NISSAN_TX_MSGS_XTRAIL[] = {
|
||||
{0x169, 0, 8, .check_relay = true}, // LKAS
|
||||
{0x2b1, 0, 8, .check_relay = true}, // PROPILOT_HUD
|
||||
{0x4cc, 0, 8, .check_relay = true}, // PROPILOT_HUD_INFO_MSG
|
||||
{0x20b, 2, 6, .check_relay = false} // CRUISE_THROTTLE (X-Trail)
|
||||
};
|
||||
|
||||
static const CanMsg NISSAN_TX_MSGS_ALTIMA[] = {
|
||||
{0x169, 0, 8, .check_relay = true}, // LKAS
|
||||
{0x2b1, 0, 8, .check_relay = true}, // PROPILOT_HUD
|
||||
{0x4cc, 0, 8, .check_relay = true}, // PROPILOT_HUD_INFO_MSG
|
||||
{0x20b, 1, 6, .check_relay = false} // CRUISE_THROTTLE (Altima)
|
||||
};
|
||||
|
||||
// Signals duplicated below due to the fact that these messages can come in on either CAN bus, depending on car model.
|
||||
static RxCheck nissan_rx_checks[] = {
|
||||
NISSAN_COMMON_RX_CHECKS
|
||||
NISSAN_PRO_PILOT_RX_CHECKS(1)
|
||||
};
|
||||
|
||||
static RxCheck nissan_alt_eps_rx_checks[] = {
|
||||
NISSAN_COMMON_RX_CHECKS
|
||||
NISSAN_PRO_PILOT_RX_CHECKS(2)
|
||||
};
|
||||
|
||||
static RxCheck nissan_leaf_rx_checks[] = {
|
||||
NISSAN_COMMON_RX_CHECKS
|
||||
};
|
||||
|
||||
// EPS Location. false = V-CAN, true = C-CAN
|
||||
const uint16_t NISSAN_PARAM_ALT_EPS_BUS = 1;
|
||||
|
||||
const uint16_t NISSAN_PARAM_SP_LEAF = 1;
|
||||
|
||||
nissan_alt_eps = GET_FLAG(param, NISSAN_PARAM_ALT_EPS_BUS);
|
||||
const bool nissan_leaf = GET_FLAG(current_safety_param_sp, NISSAN_PARAM_SP_LEAF);
|
||||
|
||||
safety_config ret;
|
||||
if (nissan_leaf) {
|
||||
SET_TX_MSGS(NISSAN_TX_MSGS_LEAF, ret);
|
||||
SET_RX_CHECKS(nissan_leaf_rx_checks, ret);
|
||||
} else if (nissan_alt_eps) {
|
||||
SET_TX_MSGS(NISSAN_TX_MSGS_ALTIMA, ret);
|
||||
SET_RX_CHECKS(nissan_alt_eps_rx_checks, ret);
|
||||
} else {
|
||||
SET_TX_MSGS(NISSAN_TX_MSGS_XTRAIL, ret);
|
||||
SET_RX_CHECKS(nissan_rx_checks, ret);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
const safety_hooks nissan_hooks = {
|
||||
.init = nissan_init,
|
||||
.rx = nissan_rx_hook,
|
||||
.tx = nissan_tx_hook,
|
||||
};
|
||||
@@ -0,0 +1,144 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
#define PSA_STEERING 757U // RX from XXX, driver torque
|
||||
#define PSA_STEERING_ALT 773U // RX from EPS, steering angle
|
||||
#define PSA_DYN_CMM 520U // RX from CMM, gas pedal
|
||||
#define PSA_HS2_DYN_ABR_38D 909U // RX from UC_FREIN, speed
|
||||
#define PSA_HS2_DAT_MDD_CMD_452 1106U // RX from BSI, cruise state
|
||||
#define PSA_DAT_BSI 1042U // RX from BSI, brake
|
||||
#define PSA_LANE_KEEP_ASSIST 1010U // TX from OP, EPS
|
||||
|
||||
// CAN bus
|
||||
#define PSA_MAIN_BUS 0U
|
||||
#define PSA_ADAS_BUS 1U
|
||||
#define PSA_CAM_BUS 2U
|
||||
|
||||
static uint8_t psa_get_counter(const CANPacket_t *msg) {
|
||||
uint8_t cnt = 0;
|
||||
if (msg->addr == PSA_HS2_DAT_MDD_CMD_452) {
|
||||
cnt = (msg->data[3] >> 4) & 0xFU;
|
||||
} else if (msg->addr == PSA_HS2_DYN_ABR_38D) {
|
||||
cnt = (msg->data[5] >> 4) & 0xFU;
|
||||
} else {
|
||||
}
|
||||
return cnt;
|
||||
}
|
||||
|
||||
static uint32_t psa_get_checksum(const CANPacket_t *msg) {
|
||||
return msg->data[5] & 0xFU;
|
||||
}
|
||||
|
||||
static uint8_t _psa_compute_checksum(const CANPacket_t *msg, uint8_t chk_ini, int chk_pos) {
|
||||
int len = GET_LEN(msg);
|
||||
|
||||
uint8_t sum = 0;
|
||||
for (int i = 0; i < len; i++) {
|
||||
uint8_t b = msg->data[i];
|
||||
|
||||
if (i == chk_pos) {
|
||||
// set checksum in low nibble to 0
|
||||
b &= 0xF0U;
|
||||
}
|
||||
sum += (b >> 4) + (b & 0xFU);
|
||||
}
|
||||
return (chk_ini - sum) & 0xFU;
|
||||
}
|
||||
|
||||
static uint32_t psa_compute_checksum(const CANPacket_t *msg) {
|
||||
uint8_t chk = 0;
|
||||
if (msg->addr == PSA_HS2_DAT_MDD_CMD_452) {
|
||||
chk = _psa_compute_checksum(msg, 0x4, 5);
|
||||
} else if (msg->addr == PSA_HS2_DYN_ABR_38D) {
|
||||
chk = _psa_compute_checksum(msg, 0x7, 5);
|
||||
} else {
|
||||
}
|
||||
return chk;
|
||||
}
|
||||
|
||||
static void psa_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->bus == PSA_MAIN_BUS) {
|
||||
if (msg->addr == PSA_DYN_CMM) {
|
||||
gas_pressed = msg->data[3] > 0U; // P002_Com_rAPP
|
||||
}
|
||||
if (msg->addr == PSA_STEERING_ALT) {
|
||||
int angle_meas_new = to_signed((msg->data[0] << 8) | msg->data[1], 16); // ANGLE
|
||||
update_sample(&angle_meas, angle_meas_new);
|
||||
}
|
||||
if (msg->addr == PSA_HS2_DYN_ABR_38D) {
|
||||
int speed = (msg->data[0] << 8) | msg->data[1];
|
||||
vehicle_moving = speed > 0;
|
||||
UPDATE_VEHICLE_SPEED(speed * 0.01 * KPH_TO_MS); // VITESSE_VEHICULE_ROUES
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->bus == PSA_ADAS_BUS) {
|
||||
if (msg->addr == PSA_HS2_DAT_MDD_CMD_452) {
|
||||
pcm_cruise_check((msg->data[2U] >> 7U) & 1U); // RVV_ACC_ACTIVATION_REQ
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (msg->bus == PSA_CAM_BUS) {
|
||||
if (msg->addr == PSA_DAT_BSI) {
|
||||
brake_pressed = (msg->data[0U] >> 5U) & 1U; // P013_MainBrake
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool psa_tx_hook(const CANPacket_t *msg) {
|
||||
bool tx = true;
|
||||
static const AngleSteeringLimits PSA_STEERING_LIMITS = {
|
||||
.max_angle = 3900,
|
||||
.angle_deg_to_can = 10,
|
||||
.angle_rate_up_lookup = {
|
||||
{0., 5., 25.},
|
||||
{2.5, 1.5, .2},
|
||||
},
|
||||
.angle_rate_down_lookup = {
|
||||
{0., 5., 25.},
|
||||
{5., 2., .3},
|
||||
},
|
||||
};
|
||||
|
||||
// Safety check for LKA
|
||||
if (msg->addr == PSA_LANE_KEEP_ASSIST) {
|
||||
// SET_ANGLE
|
||||
int desired_angle = to_signed((msg->data[6] << 6) | ((msg->data[7] & 0xFCU) >> 2), 14);
|
||||
// TORQUE_FACTOR
|
||||
bool lka_active = ((msg->data[5] & 0xFEU) >> 1) == 100U;
|
||||
|
||||
if (steer_angle_cmd_checks(desired_angle, lka_active, PSA_STEERING_LIMITS)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
return tx;
|
||||
}
|
||||
|
||||
static safety_config psa_init(uint16_t param) {
|
||||
SAFETY_UNUSED(param);
|
||||
static const CanMsg PSA_TX_MSGS[] = {
|
||||
{PSA_LANE_KEEP_ASSIST, PSA_MAIN_BUS, 8, .check_relay = true}, // EPS steering
|
||||
};
|
||||
|
||||
static RxCheck psa_rx_checks[] = {
|
||||
{.msg = {{PSA_HS2_DAT_MDD_CMD_452, PSA_ADAS_BUS, 6, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // cruise state
|
||||
{.msg = {{PSA_HS2_DYN_ABR_38D, PSA_MAIN_BUS, 8, 25U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // speed
|
||||
{.msg = {{PSA_STEERING_ALT, PSA_MAIN_BUS, 7, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // steering angle
|
||||
{.msg = {{PSA_STEERING, PSA_MAIN_BUS, 7, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // driver torque
|
||||
{.msg = {{PSA_DYN_CMM, PSA_MAIN_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // gas pedal
|
||||
{.msg = {{PSA_DAT_BSI, PSA_CAM_BUS, 8, 20U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // brake
|
||||
};
|
||||
|
||||
return BUILD_SAFETY_CFG(psa_rx_checks, PSA_TX_MSGS);
|
||||
}
|
||||
|
||||
const safety_hooks psa_hooks = {
|
||||
.init = psa_init,
|
||||
.rx = psa_rx_hook,
|
||||
.tx = psa_tx_hook,
|
||||
.get_counter = psa_get_counter,
|
||||
.get_checksum = psa_get_checksum,
|
||||
.compute_checksum = psa_compute_checksum,
|
||||
};
|
||||
@@ -0,0 +1,182 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
static uint8_t rivian_get_counter(const CANPacket_t *msg) {
|
||||
// Signal: ESP_Status_Counter, VDM_PropStatus_Counter
|
||||
return msg->data[1] & 0xFU;
|
||||
}
|
||||
|
||||
static uint32_t rivian_get_checksum(const CANPacket_t *msg) {
|
||||
// Signal: ESP_Status_Checksum, VDM_PropStatus_Checksum
|
||||
return msg->data[0];
|
||||
}
|
||||
|
||||
static uint8_t _rivian_compute_checksum(const CANPacket_t *msg, uint8_t poly, uint8_t xor_output) {
|
||||
int len = GET_LEN(msg);
|
||||
|
||||
uint8_t crc = 0;
|
||||
// Skip the checksum byte
|
||||
for (int i = 1; i < len; i++) {
|
||||
crc ^= msg->data[i];
|
||||
for (int j = 0; j < 8; j++) {
|
||||
if ((crc & 0x80U) != 0U) {
|
||||
crc = (crc << 1) ^ poly;
|
||||
} else {
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return crc ^ xor_output;
|
||||
}
|
||||
|
||||
static uint32_t rivian_compute_checksum(const CANPacket_t *msg) {
|
||||
uint8_t chksum = 0;
|
||||
if (msg->addr == 0x208U) {
|
||||
chksum = _rivian_compute_checksum(msg, 0x1D, 0xB1);
|
||||
} else if (msg->addr == 0x150U) {
|
||||
chksum = _rivian_compute_checksum(msg, 0x1D, 0x9A);
|
||||
} else {
|
||||
}
|
||||
return chksum;
|
||||
}
|
||||
|
||||
static bool rivian_get_quality_flag_valid(const CANPacket_t *msg) {
|
||||
bool valid = false;
|
||||
if (msg->addr == 0x208U) {
|
||||
valid = ((msg->data[3] >> 3) & 0x3U) == 0x1U; // ESP_Vehicle_Speed_Q
|
||||
} else if (msg->addr == 0x150U) {
|
||||
valid = (msg->data[1] >> 6) == 0x1U; // VDM_VehicleSpeedQ
|
||||
} else {
|
||||
}
|
||||
return valid;
|
||||
}
|
||||
|
||||
static void rivian_rx_hook(const CANPacket_t *msg) {
|
||||
|
||||
if (msg->bus == 0U) {
|
||||
// Vehicle speed
|
||||
if (msg->addr == 0x208U) {
|
||||
float speed = ((msg->data[6] << 8) | msg->data[7]) * 0.01;
|
||||
vehicle_moving = speed > 0.0;
|
||||
UPDATE_VEHICLE_SPEED(speed * KPH_TO_MS);
|
||||
}
|
||||
|
||||
// Gas pressed and second speed source for variable torque limit
|
||||
if (msg->addr == 0x150U) {
|
||||
gas_pressed = msg->data[3] | (msg->data[4] & 0xC0U);
|
||||
|
||||
// Disable controls if speeds from VDM and ESP ECUs are too far apart.
|
||||
float vdm_speed = ((msg->data[5] << 8) | msg->data[6]) * 0.01 * KPH_TO_MS;
|
||||
speed_mismatch_check(vdm_speed);
|
||||
}
|
||||
|
||||
// Driver torque
|
||||
if (msg->addr == 0x380U) {
|
||||
int torque_driver_new = (((msg->data[2] << 4) | (msg->data[3] >> 4))) - 2050U;
|
||||
update_sample(&torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
// Brake pressed
|
||||
if (msg->addr == 0x38fU) {
|
||||
brake_pressed = (msg->data[2] >> 7) & 1U;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->bus == 2U) {
|
||||
// Cruise state
|
||||
if (msg->addr == 0x100U) {
|
||||
const int feature_status = msg->data[2] >> 5U;
|
||||
pcm_cruise_check(feature_status == 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool rivian_tx_hook(const CANPacket_t *msg) {
|
||||
// Rivian utilizes more torque at low speed to maintain the same lateral accel
|
||||
const TorqueSteeringLimits RIVIAN_STEERING_LIMITS = {
|
||||
.max_torque = 350,
|
||||
.dynamic_max_torque = true,
|
||||
.max_torque_lookup = {
|
||||
{9., 17., 17.},
|
||||
{350, 250, 250},
|
||||
},
|
||||
.max_rate_up = 3,
|
||||
.max_rate_down = 5,
|
||||
.max_rt_delta = 125,
|
||||
.driver_torque_multiplier = 2,
|
||||
.driver_torque_allowance = 100,
|
||||
.type = TorqueDriverLimited,
|
||||
};
|
||||
|
||||
const LongitudinalLimits RIVIAN_LONG_LIMITS = {
|
||||
.max_accel = 200,
|
||||
.min_accel = -350,
|
||||
.inactive_accel = 0,
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
|
||||
if (msg->bus == 0U) {
|
||||
// Steering control
|
||||
if (msg->addr == 0x120U) {
|
||||
int desired_torque = ((msg->data[2] << 3U) | (msg->data[3] >> 5U)) - 1024U;
|
||||
bool steer_req = (msg->data[3] >> 4) & 1U;
|
||||
|
||||
if (steer_torque_cmd_checks(desired_torque, steer_req, RIVIAN_STEERING_LIMITS)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Longitudinal control
|
||||
if (msg->addr == 0x160U) {
|
||||
int raw_accel = ((msg->data[2] << 3) | (msg->data[3] >> 5)) - 1024U;
|
||||
if (longitudinal_accel_checks(raw_accel, RIVIAN_LONG_LIMITS)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
static safety_config rivian_init(uint16_t param) {
|
||||
// SCCM_WheelTouch: for hiding hold wheel alert
|
||||
// VDM_AdasSts: for canceling stock ACC
|
||||
// 0x120 = ACM_lkaHbaCmd, 0x321 = SCCM_WheelTouch, 0x162 = VDM_AdasSts
|
||||
static const CanMsg RIVIAN_TX_MSGS[] = {{0x120, 0, 8, .check_relay = true}, {0x321, 2, 7, .check_relay = true}, {0x162, 2, 8, .check_relay = true}};
|
||||
// 0x160 = ACM_longitudinalRequest
|
||||
static const CanMsg RIVIAN_LONG_TX_MSGS[] = {{0x120, 0, 8, .check_relay = true}, {0x321, 2, 7, .check_relay = true}, {0x160, 0, 5, .check_relay = true}};
|
||||
|
||||
static RxCheck rivian_rx_checks[] = {
|
||||
{.msg = {{0x208, 0, 8, 50U, .max_counter = 14U}, { 0 }, { 0 }}}, // ESP_Status (speed)
|
||||
{.msg = {{0x150, 0, 7, 50U, .max_counter = 14U}, { 0 }, { 0 }}}, // VDM_PropStatus (gas pedal & 2nd speed)
|
||||
{.msg = {{0x380, 0, 5, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // EPAS_SystemStatus (driver torque)
|
||||
{.msg = {{0x38f, 0, 6, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // iBESP2 (brakes)
|
||||
{.msg = {{0x100, 2, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // ACM_Status (cruise state)
|
||||
};
|
||||
|
||||
bool rivian_longitudinal = false;
|
||||
|
||||
SAFETY_UNUSED(param);
|
||||
#ifdef ALLOW_DEBUG
|
||||
const int FLAG_RIVIAN_LONG_CONTROL = 1;
|
||||
rivian_longitudinal = GET_FLAG(param, FLAG_RIVIAN_LONG_CONTROL);
|
||||
#endif
|
||||
|
||||
// FIXME: cppcheck thinks that rivian_longitudinal is always false. This is not true
|
||||
// if ALLOW_DEBUG is defined but cppcheck is run without ALLOW_DEBUG
|
||||
// cppcheck-suppress knownConditionTrueFalse
|
||||
return rivian_longitudinal ? BUILD_SAFETY_CFG(rivian_rx_checks, RIVIAN_LONG_TX_MSGS) : \
|
||||
BUILD_SAFETY_CFG(rivian_rx_checks, RIVIAN_TX_MSGS);
|
||||
}
|
||||
|
||||
const safety_hooks rivian_hooks = {
|
||||
.init = rivian_init,
|
||||
.rx = rivian_rx_hook,
|
||||
.tx = rivian_tx_hook,
|
||||
.get_counter = rivian_get_counter,
|
||||
.get_checksum = rivian_get_checksum,
|
||||
.compute_checksum = rivian_compute_checksum,
|
||||
.get_quality_flag_valid = rivian_get_quality_flag_valid,
|
||||
};
|
||||
@@ -0,0 +1,282 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/subaru_common.h"
|
||||
|
||||
#define SUBARU_STEERING_LIMITS_GENERATOR(steer_max, rate_up, rate_down) \
|
||||
{ \
|
||||
.max_torque = (steer_max), \
|
||||
.max_rt_delta = 940, \
|
||||
.max_rate_up = (rate_up), \
|
||||
.max_rate_down = (rate_down), \
|
||||
.driver_torque_multiplier = 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, \
|
||||
}
|
||||
|
||||
#define MSG_SUBARU_Brake_Status 0x13cU
|
||||
#define MSG_SUBARU_CruiseControl 0x240U
|
||||
#define MSG_SUBARU_Throttle 0x40U
|
||||
#define MSG_SUBARU_Steering_Torque 0x119U
|
||||
#define MSG_SUBARU_Wheel_Speeds 0x13aU
|
||||
#define MSG_SUBARU_Brake_Pedal 0x139U
|
||||
|
||||
#define MSG_SUBARU_ES_LKAS 0x122U
|
||||
#define MSG_SUBARU_ES_Brake 0x220U
|
||||
#define MSG_SUBARU_ES_Distance 0x221U
|
||||
#define MSG_SUBARU_ES_Status 0x222U
|
||||
#define MSG_SUBARU_ES_DashStatus 0x321U
|
||||
#define MSG_SUBARU_ES_LKAS_State 0x322U
|
||||
#define MSG_SUBARU_ES_Infotainment 0x323U
|
||||
|
||||
#define MSG_SUBARU_ES_UDS_Request 0x787U
|
||||
|
||||
#define MSG_SUBARU_ES_HighBeamAssist 0x22AU
|
||||
#define MSG_SUBARU_ES_STATIC_1 0x325U
|
||||
#define MSG_SUBARU_ES_STATIC_2 0x121U
|
||||
|
||||
#define SUBARU_MAIN_BUS 0U
|
||||
#define SUBARU_ALT_BUS 1U
|
||||
#define SUBARU_CAM_BUS 2U
|
||||
|
||||
#define SUBARU_BASE_TX_MSGS(alt_bus, lkas_msg) \
|
||||
{lkas_msg, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
|
||||
{MSG_SUBARU_ES_DashStatus, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
|
||||
{MSG_SUBARU_ES_LKAS_State, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
|
||||
{MSG_SUBARU_ES_Infotainment, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
|
||||
|
||||
#define SUBARU_COMMON_TX_MSGS(alt_bus) \
|
||||
{MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = false}, \
|
||||
|
||||
#define SUBARU_COMMON_LONG_TX_MSGS(alt_bus) \
|
||||
{MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = true}, \
|
||||
{MSG_SUBARU_ES_Brake, alt_bus, 8, .check_relay = true}, \
|
||||
{MSG_SUBARU_ES_Status, alt_bus, 8, .check_relay = true}, \
|
||||
|
||||
#define SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS() \
|
||||
{MSG_SUBARU_ES_UDS_Request, SUBARU_CAM_BUS, 8, .check_relay = false}, \
|
||||
{MSG_SUBARU_ES_HighBeamAssist, SUBARU_MAIN_BUS, 8, .check_relay = false}, \
|
||||
{MSG_SUBARU_ES_STATIC_1, SUBARU_MAIN_BUS, 8, .check_relay = false}, \
|
||||
{MSG_SUBARU_ES_STATIC_2, SUBARU_MAIN_BUS, 8, .check_relay = false}, \
|
||||
|
||||
#define SUBARU_STOP_AND_GO_TX_MSGS \
|
||||
{MSG_SUBARU_Throttle, SUBARU_CAM_BUS, 8, .check_relay = true}, \
|
||||
{MSG_SUBARU_Brake_Pedal, SUBARU_CAM_BUS, 8, .check_relay = true}, \
|
||||
|
||||
#define SUBARU_COMMON_RX_CHECKS(alt_bus) \
|
||||
{.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_SUBARU_Wheel_Speeds, alt_bus, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_SUBARU_CruiseControl, alt_bus, 8, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_SUBARU_ES_LKAS_State, SUBARU_CAM_BUS, 8, 10U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
static bool subaru_gen2 = false;
|
||||
static bool subaru_longitudinal = false;
|
||||
|
||||
static uint32_t subaru_get_checksum(const CANPacket_t *msg) {
|
||||
return (uint8_t)msg->data[0];
|
||||
}
|
||||
|
||||
static uint8_t subaru_get_counter(const CANPacket_t *msg) {
|
||||
return (uint8_t)(msg->data[1] & 0xFU);
|
||||
}
|
||||
|
||||
static uint32_t subaru_compute_checksum(const CANPacket_t *msg) {
|
||||
int len = GET_LEN(msg);
|
||||
uint8_t checksum = (uint8_t)(msg->addr) + (uint8_t)((unsigned int)(msg->addr) >> 8U);
|
||||
for (int i = 1; i < len; i++) {
|
||||
checksum += (uint8_t)msg->data[i];
|
||||
}
|
||||
return checksum;
|
||||
}
|
||||
|
||||
static void subaru_rx_hook(const CANPacket_t *msg) {
|
||||
const unsigned int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
|
||||
|
||||
if ((msg->addr == MSG_SUBARU_Steering_Torque) && (msg->bus == SUBARU_MAIN_BUS)) {
|
||||
int torque_driver_new;
|
||||
torque_driver_new = ((GET_BYTES(msg, 0, 4) >> 16) & 0x7FFU);
|
||||
torque_driver_new = -1 * to_signed(torque_driver_new, 11);
|
||||
update_sample(&torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
if ((msg->addr == MSG_SUBARU_ES_LKAS_State) && (msg->bus == SUBARU_CAM_BUS)) {
|
||||
int lkas_hud = (msg->data[2] & 0x0CU) >> 2U;
|
||||
if ((lkas_hud >= 1) && (lkas_hud <= 3)) {
|
||||
mads_button_press = MADS_BUTTON_PRESSED;
|
||||
}
|
||||
}
|
||||
|
||||
// enter controls on rising edge of ACC, exit controls on ACC off
|
||||
if ((msg->addr == MSG_SUBARU_CruiseControl) && (msg->bus == alt_main_bus)) {
|
||||
bool cruise_engaged = (msg->data[5] >> 1) & 1U;
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
acc_main_on = GET_BIT(msg, 40U);
|
||||
}
|
||||
|
||||
// update vehicle moving with any non-zero wheel speed
|
||||
if ((msg->addr == MSG_SUBARU_Wheel_Speeds) && (msg->bus == alt_main_bus)) {
|
||||
uint32_t fr = (GET_BYTES(msg, 1, 3) >> 4) & 0x1FFFU;
|
||||
uint32_t rr = (GET_BYTES(msg, 3, 3) >> 1) & 0x1FFFU;
|
||||
uint32_t rl = (GET_BYTES(msg, 4, 3) >> 6) & 0x1FFFU;
|
||||
uint32_t fl = (GET_BYTES(msg, 6, 2) >> 3) & 0x1FFFU;
|
||||
|
||||
vehicle_moving = (fr > 0U) || (rr > 0U) || (rl > 0U) || (fl > 0U);
|
||||
|
||||
UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4.0 * 0.057 * KPH_TO_MS);
|
||||
}
|
||||
|
||||
if ((msg->addr == MSG_SUBARU_Brake_Status) && (msg->bus == alt_main_bus)) {
|
||||
brake_pressed = (msg->data[7] >> 6) & 1U;
|
||||
}
|
||||
|
||||
if ((msg->addr == MSG_SUBARU_Throttle) && (msg->bus == SUBARU_MAIN_BUS)) {
|
||||
gas_pressed = msg->data[4] != 0U;
|
||||
}
|
||||
}
|
||||
|
||||
static bool subaru_tx_hook(const CANPacket_t *msg) {
|
||||
const TorqueSteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
|
||||
const TorqueSteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1500, 35, 50);
|
||||
|
||||
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 = 3600,
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
bool violation = false;
|
||||
|
||||
// steer cmd checks
|
||||
if (msg->addr == MSG_SUBARU_ES_LKAS) {
|
||||
int desired_torque = ((GET_BYTES(msg, 0, 4) >> 16) & 0x1FFFU);
|
||||
desired_torque = -1 * to_signed(desired_torque, 13);
|
||||
|
||||
bool steer_req = (msg->data[3] >> 5) & 1U;
|
||||
|
||||
const TorqueSteeringLimits 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 (msg->addr == MSG_SUBARU_ES_Brake) {
|
||||
int es_brake_pressure = GET_BYTES(msg, 2, 2);
|
||||
violation |= longitudinal_brake_checks(es_brake_pressure, SUBARU_LONG_LIMITS);
|
||||
}
|
||||
|
||||
// check es_distance cruise_throttle limits
|
||||
if (msg->addr == MSG_SUBARU_ES_Distance) {
|
||||
int cruise_throttle = (GET_BYTES(msg, 2, 2) & 0x1FFFU);
|
||||
bool cruise_cancel = (msg->data[7] >> 0) & 1U;
|
||||
|
||||
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 (msg->addr == MSG_SUBARU_ES_Status) {
|
||||
int transmission_rpm = (GET_BYTES(msg, 2, 2) & 0x1FFFU);
|
||||
violation |= longitudinal_transmission_rpm_checks(transmission_rpm, SUBARU_LONG_LIMITS);
|
||||
}
|
||||
|
||||
if (msg->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(msg, 0, 4) == 0x00803E02U) && (GET_BYTES(msg, 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(msg, 0, 4) == 0x30112203U) && (GET_BYTES(msg, 4, 4) == 0x0U);
|
||||
|
||||
violation |= !(is_tester_present || is_button_rdbi);
|
||||
}
|
||||
|
||||
if (violation){
|
||||
tx = false;
|
||||
}
|
||||
return tx;
|
||||
}
|
||||
|
||||
static safety_config subaru_init(uint16_t param) {
|
||||
static const CanMsg SUBARU_TX_MSGS[] = {
|
||||
SUBARU_BASE_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
};
|
||||
|
||||
static const CanMsg SUBARU_LONG_TX_MSGS[] = {
|
||||
SUBARU_BASE_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
|
||||
SUBARU_COMMON_LONG_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
};
|
||||
|
||||
static const CanMsg SUBARU_GEN2_TX_MSGS[] = {
|
||||
SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
|
||||
};
|
||||
|
||||
static const CanMsg SUBARU_GEN2_LONG_TX_MSGS[] = {
|
||||
SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
|
||||
SUBARU_COMMON_LONG_TX_MSGS(SUBARU_ALT_BUS)
|
||||
SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()
|
||||
};
|
||||
|
||||
static const CanMsg subaru_stop_and_go_tx_msgs[] = {
|
||||
SUBARU_BASE_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
SUBARU_STOP_AND_GO_TX_MSGS
|
||||
};
|
||||
|
||||
static RxCheck subaru_rx_checks[] = {
|
||||
SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS)
|
||||
};
|
||||
|
||||
static RxCheck subaru_gen2_rx_checks[] = {
|
||||
SUBARU_COMMON_RX_CHECKS(SUBARU_ALT_BUS)
|
||||
};
|
||||
|
||||
const uint16_t SUBARU_PARAM_GEN2 = 1;
|
||||
|
||||
subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
|
||||
|
||||
subaru_common_init();
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
|
||||
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) : \
|
||||
subaru_stop_and_go ? BUILD_SAFETY_CFG(subaru_rx_checks, subaru_stop_and_go_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,
|
||||
.get_counter = subaru_get_counter,
|
||||
.get_checksum = subaru_get_checksum,
|
||||
.compute_checksum = subaru_compute_checksum,
|
||||
};
|
||||
@@ -0,0 +1,30 @@
|
||||
/**
|
||||
* Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
*
|
||||
* This file is part of sunnypilot and is licensed under the MIT License.
|
||||
* See the LICENSE.md file in the root directory for more details.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
extern bool subaru_stop_and_go;
|
||||
bool subaru_stop_and_go = false;
|
||||
|
||||
void subaru_common_init(void) {
|
||||
const uint16_t SUBARU_PARAM_SP_STOP_AND_GO = 1;
|
||||
|
||||
subaru_stop_and_go = GET_FLAG(current_safety_param_sp, SUBARU_PARAM_SP_STOP_AND_GO);
|
||||
}
|
||||
|
||||
/*
|
||||
bool subaru_common_stop_and_go_throttle_check(const int throttle_pedal) {
|
||||
bool violation = throttle_pedal != 5U || !controls_allowed || vehicle_moving;
|
||||
return violation;
|
||||
}
|
||||
|
||||
bool subaru_common_stop_and_go_brake_pedal_check(const int speed, const bool is_preglobal) {
|
||||
int val = is_preglobal ? 1U : 3U;
|
||||
bool violation = speed != val || !controls_allowed || vehicle_moving;
|
||||
return violation;
|
||||
}
|
||||
*/
|
||||
@@ -0,0 +1,124 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/subaru_common.h"
|
||||
|
||||
// Preglobal platform
|
||||
// 0x161 is ES_CruiseThrottle
|
||||
// 0x164 is ES_LKAS
|
||||
|
||||
#define MSG_SUBARU_PG_CruiseControl 0x144U
|
||||
#define MSG_SUBARU_PG_Throttle 0x140U
|
||||
#define MSG_SUBARU_PG_Wheel_Speeds 0xD4U
|
||||
#define MSG_SUBARU_PG_Brake_Pedal 0xD1U
|
||||
#define MSG_SUBARU_PG_ES_LKAS 0x164U
|
||||
#define MSG_SUBARU_PG_ES_Distance 0x161U
|
||||
#define MSG_SUBARU_PG_Steering_Torque 0x371U
|
||||
|
||||
#define SUBARU_PG_MAIN_BUS 0U
|
||||
#define SUBARU_PG_CAM_BUS 2U
|
||||
|
||||
#define SUBARU_PG_COMMON_TX_MSGS \
|
||||
{MSG_SUBARU_PG_ES_Distance, SUBARU_PG_MAIN_BUS, 8, .check_relay = true}, \
|
||||
{MSG_SUBARU_PG_ES_LKAS, SUBARU_PG_MAIN_BUS, 8, .check_relay = true}, \
|
||||
|
||||
#define SUBARU_PG_STOP_AND_GO_TX_MSGS \
|
||||
{MSG_SUBARU_PG_Throttle, SUBARU_PG_CAM_BUS, 8, .check_relay = false}, \
|
||||
{MSG_SUBARU_PG_Brake_Pedal, SUBARU_PG_CAM_BUS, 4, .check_relay = false}, \
|
||||
|
||||
static bool subaru_pg_reversed_driver_torque = false;
|
||||
|
||||
static void subaru_preglobal_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->bus == SUBARU_PG_MAIN_BUS) {
|
||||
if (msg->addr == MSG_SUBARU_PG_Steering_Torque) {
|
||||
int torque_driver_new;
|
||||
torque_driver_new = (msg->data[3] >> 5) + (msg->data[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 (msg->addr == MSG_SUBARU_PG_CruiseControl) {
|
||||
bool cruise_engaged = (msg->data[6] >> 1) & 1U;
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
acc_main_on = GET_BIT(msg, 48U);
|
||||
}
|
||||
|
||||
// update vehicle moving with any non-zero wheel speed
|
||||
if (msg->addr == MSG_SUBARU_PG_Wheel_Speeds) {
|
||||
vehicle_moving = ((GET_BYTES(msg, 0, 4) >> 12) != 0U) || (GET_BYTES(msg, 4, 4) != 0U);
|
||||
}
|
||||
|
||||
if (msg->addr == MSG_SUBARU_PG_Brake_Pedal) {
|
||||
brake_pressed = ((GET_BYTES(msg, 0, 4) >> 16) & 0xFFU) > 0U;
|
||||
}
|
||||
|
||||
if (msg->addr == MSG_SUBARU_PG_Throttle) {
|
||||
gas_pressed = msg->data[0] != 0U;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool subaru_preglobal_tx_hook(const CANPacket_t *msg) {
|
||||
const TorqueSteeringLimits SUBARU_PG_STEERING_LIMITS = {
|
||||
.max_torque = 2047,
|
||||
.max_rt_delta = 940,
|
||||
.max_rate_up = 50,
|
||||
.max_rate_down = 70,
|
||||
.driver_torque_multiplier = 10,
|
||||
.driver_torque_allowance = 75,
|
||||
.type = TorqueDriverLimited,
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
|
||||
// steer cmd checks
|
||||
if (msg->addr == MSG_SUBARU_PG_ES_LKAS) {
|
||||
int desired_torque = ((GET_BYTES(msg, 0, 4) >> 8) & 0x1FFFU);
|
||||
desired_torque = -1 * to_signed(desired_torque, 13);
|
||||
|
||||
bool steer_req = (msg->data[3] >> 0) & 1U;
|
||||
|
||||
if (steer_torque_cmd_checks(desired_torque, steer_req, SUBARU_PG_STEERING_LIMITS)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
return tx;
|
||||
}
|
||||
|
||||
static safety_config subaru_preglobal_init(uint16_t param) {
|
||||
static const CanMsg SUBARU_PG_TX_MSGS[] = {
|
||||
SUBARU_PG_COMMON_TX_MSGS
|
||||
};
|
||||
|
||||
static const CanMsg subaru_pg_stop_and_go_tx_msgs[] = {
|
||||
SUBARU_PG_COMMON_TX_MSGS
|
||||
SUBARU_PG_STOP_AND_GO_TX_MSGS
|
||||
};
|
||||
|
||||
// TODO: do checksum and counter checks after adding the signals to the outback dbc file
|
||||
static RxCheck subaru_preglobal_rx_checks[] = {
|
||||
{.msg = {{MSG_SUBARU_PG_Throttle, SUBARU_PG_MAIN_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_SUBARU_PG_Steering_Torque, SUBARU_PG_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_SUBARU_PG_CruiseControl, SUBARU_PG_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_SUBARU_PG_Wheel_Speeds, SUBARU_PG_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_SUBARU_PG_Brake_Pedal, SUBARU_PG_MAIN_BUS, 4, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
subaru_common_init();
|
||||
|
||||
const uint16_t SUBARU_PG_PARAM_REVERSED_DRIVER_TORQUE = 4;
|
||||
|
||||
subaru_pg_reversed_driver_torque = GET_FLAG(param, SUBARU_PG_PARAM_REVERSED_DRIVER_TORQUE);
|
||||
|
||||
safety_config ret = subaru_stop_and_go ? BUILD_SAFETY_CFG(subaru_preglobal_rx_checks, subaru_pg_stop_and_go_tx_msgs) : \
|
||||
BUILD_SAFETY_CFG(subaru_preglobal_rx_checks, SUBARU_PG_TX_MSGS);
|
||||
return ret;
|
||||
}
|
||||
|
||||
const safety_hooks subaru_preglobal_hooks = {
|
||||
.init = subaru_preglobal_init,
|
||||
.rx = subaru_preglobal_rx_hook,
|
||||
.tx = subaru_preglobal_tx_hook,
|
||||
};
|
||||
@@ -0,0 +1,427 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
#define TESLA_COMMON_RX_CHECKS \
|
||||
{.msg = {{0x2b9, 2, 8, 25U, .max_counter = 7U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* DAS_control */ \
|
||||
{.msg = {{0x488, 2, 4, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* DAS_steeringControl */ \
|
||||
{.msg = {{0x257, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* DI_speed (speed in kph) */ \
|
||||
{.msg = {{0x155, 0, 8, 50U, .max_counter = 15U}, { 0 }, { 0 }}}, /* ESP_B (2nd speed in kph) */ \
|
||||
{.msg = {{0x370, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* EPAS3S_sysStatus (steering angle) */ \
|
||||
{.msg = {{0x118, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* DI_systemStatus (gas pedal) */ \
|
||||
{.msg = {{0x145, 0, 8, 50U, .max_counter = 15U}, { 0 }, { 0 }}}, /* ESP_status (brakes) */ \
|
||||
{.msg = {{0x286, 0, 8, 10U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* DI_state (acc state) */ \
|
||||
{.msg = {{0x311, 0, 7, 10U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* UI_warning (blinkers, buckle switch & doors) */ \
|
||||
|
||||
#define TESLA_VEHICLE_BUS_ADDR_CHECK \
|
||||
{.msg = {{0x3DF, 1, 8, 2U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .ignore_frequency_check = true}, { 0 }, { 0 }}}, /* UI_status2 */ \
|
||||
|
||||
static bool tesla_longitudinal = false;
|
||||
static bool tesla_fsd_14 = false;
|
||||
static bool tesla_stock_aeb = false;
|
||||
|
||||
// Only rising edges while controls are not allowed are considered for these systems:
|
||||
// TODO: Only LKAS (non-emergency) is currently supported since we've only seen it
|
||||
static bool tesla_stock_lkas = false;
|
||||
static bool tesla_stock_lkas_prev = false;
|
||||
|
||||
// Only Summon is currently supported due to Autopark not setting Autopark state properly
|
||||
static bool tesla_autopark = false;
|
||||
static bool tesla_autopark_prev = false;
|
||||
|
||||
// Detected VEHICLE bus
|
||||
extern bool tesla_has_vehicle_bus;
|
||||
bool tesla_has_vehicle_bus = false;
|
||||
|
||||
static uint8_t tesla_get_counter(const CANPacket_t *msg) {
|
||||
|
||||
uint8_t cnt = 0;
|
||||
if (msg->addr == 0x2b9U) {
|
||||
// Signal: DAS_controlCounter
|
||||
cnt = msg->data[6] >> 5;
|
||||
} else if (msg->addr == 0x488U) {
|
||||
// Signal: DAS_steeringControlCounter
|
||||
cnt = msg->data[2] & 0x0FU;
|
||||
} else if ((msg->addr == 0x257U) || (msg->addr == 0x118U) || (msg->addr == 0x145U) || (msg->addr == 0x286U) || (msg->addr == 0x311U)) {
|
||||
// Signal: DI_speedCounter, DI_systemStatusCounter, ESP_statusCounter, DI_locStatusCounter, UI_warningCounter
|
||||
cnt = msg->data[1] & 0x0FU;
|
||||
} else if (msg->addr == 0x155U) {
|
||||
// Signal: ESP_wheelRotationCounter
|
||||
cnt = msg->data[6] >> 4;
|
||||
} else if (msg->addr == 0x370U) {
|
||||
// Signal: EPAS3S_sysStatusCounter
|
||||
cnt = msg->data[6] & 0x0FU;
|
||||
} else {
|
||||
}
|
||||
return cnt;
|
||||
}
|
||||
|
||||
static int _tesla_get_checksum_byte(const int addr) {
|
||||
int checksum_byte = -1;
|
||||
if ((addr == 0x370) || (addr == 0x2b9) || (addr == 0x155)) {
|
||||
// Signal: EPAS3S_sysStatusChecksum, DAS_controlChecksum, ESP_wheelRotationChecksum
|
||||
checksum_byte = 7;
|
||||
} else if (addr == 0x488) {
|
||||
// Signal: DAS_steeringControlChecksum
|
||||
checksum_byte = 3;
|
||||
} else if ((addr == 0x257) || (addr == 0x118) || (addr == 0x145) || (addr == 0x286) || (addr == 0x311)) {
|
||||
// Signal: DI_speedChecksum, DI_systemStatusChecksum, ESP_statusChecksum, DI_locStatusChecksum, UI_warningChecksum
|
||||
checksum_byte = 0;
|
||||
} else {
|
||||
}
|
||||
return checksum_byte;
|
||||
}
|
||||
|
||||
static uint32_t tesla_get_checksum(const CANPacket_t *msg) {
|
||||
uint8_t chksum = 0;
|
||||
int checksum_byte = _tesla_get_checksum_byte(msg->addr);
|
||||
if (checksum_byte != -1) {
|
||||
chksum = msg->data[checksum_byte];
|
||||
}
|
||||
return chksum;
|
||||
}
|
||||
|
||||
static uint32_t tesla_compute_checksum(const CANPacket_t *msg) {
|
||||
uint8_t chksum = 0;
|
||||
int checksum_byte = _tesla_get_checksum_byte(msg->addr);
|
||||
|
||||
if (checksum_byte != -1) {
|
||||
chksum = (uint8_t)((msg->addr & 0xFFU) + ((msg->addr >> 8) & 0xFFU));
|
||||
int len = GET_LEN(msg);
|
||||
for (int i = 0; i < len; i++) {
|
||||
if (i != checksum_byte) {
|
||||
chksum += msg->data[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
return chksum;
|
||||
}
|
||||
|
||||
static bool tesla_get_quality_flag_valid(const CANPacket_t *msg) {
|
||||
|
||||
bool valid = false;
|
||||
if (msg->addr == 0x155U) {
|
||||
valid = (msg->data[5] & 0x1U) == 0x1U; // ESP_wheelSpeedsQF
|
||||
} else if (msg->addr == 0x145U) {
|
||||
int user_brake_status = (msg->data[3] >> 5) & 0x03U;
|
||||
valid = (user_brake_status != 0) && (user_brake_status != 3); // ESP_driverBrakeApply=NotInit_orOff, Faulty_SNA
|
||||
} else {
|
||||
}
|
||||
return valid;
|
||||
}
|
||||
|
||||
static int tesla_get_steer_ctrl_type(const int ctrl_type) {
|
||||
// Returns ANGLE_CONTROL-equivalent control type for FSD 14
|
||||
int steer_ctrl_type = ctrl_type;
|
||||
if (tesla_fsd_14) {
|
||||
if (ctrl_type == 1) {
|
||||
steer_ctrl_type = 2;
|
||||
} else if (ctrl_type == 2) {
|
||||
steer_ctrl_type = 1;
|
||||
} else {
|
||||
}
|
||||
}
|
||||
return steer_ctrl_type;
|
||||
}
|
||||
|
||||
static void tesla_rx_hook(const CANPacket_t *msg) {
|
||||
|
||||
if (msg->bus == 0U) {
|
||||
// Steering angle: (0.1 * val) - 819.2 in deg.
|
||||
if (msg->addr == 0x370U) {
|
||||
// Store it 1/10 deg to match steering request
|
||||
const int angle_meas_new = (((msg->data[4] & 0x3FU) << 8) | msg->data[5]) - 8192U;
|
||||
update_sample(&angle_meas, angle_meas_new);
|
||||
|
||||
const int hands_on_level = msg->data[4] >> 6; // EPAS3S_handsOnLevel
|
||||
const int eac_status = msg->data[6] >> 5; // EPAS3S_eacStatus
|
||||
const int eac_error_code = msg->data[2] >> 4; // EPAS3S_eacErrorCode
|
||||
|
||||
// Disengage on normal user override, or if high angle rate fault from user overriding extremely quickly
|
||||
steering_disengage = (hands_on_level >= 3) || ((eac_status == 0) && (eac_error_code == 9));
|
||||
}
|
||||
|
||||
// Vehicle speed (DI_speed)
|
||||
if (msg->addr == 0x257U) {
|
||||
// Vehicle speed: ((val * 0.08) - 40) / MS_TO_KPH
|
||||
float speed = ((((msg->data[2] << 4) | (msg->data[1] >> 4)) * 0.08) - 40.) * KPH_TO_MS;
|
||||
UPDATE_VEHICLE_SPEED(speed);
|
||||
}
|
||||
|
||||
// 2nd vehicle speed (ESP_B)
|
||||
if (msg->addr == 0x155U) {
|
||||
// Disable controls if speeds from DI (Drive Inverter) and ESP ECUs are too far apart.
|
||||
float esp_speed = (((msg->data[6] & 0x0FU) << 6) | (msg->data[5] >> 2)) * 0.5 * KPH_TO_MS;
|
||||
speed_mismatch_check(esp_speed);
|
||||
}
|
||||
|
||||
// Gas pressed
|
||||
if (msg->addr == 0x118U) {
|
||||
gas_pressed = (msg->data[4] != 0U);
|
||||
}
|
||||
|
||||
// Brake pressed
|
||||
if (msg->addr == 0x145U) {
|
||||
brake_pressed = ((msg->data[3] >> 5) & 0x03U) == 2U;
|
||||
}
|
||||
|
||||
// Cruise and Autopark/Summon state
|
||||
if (msg->addr == 0x286U) {
|
||||
// Autopark state
|
||||
int autopark_state = (msg->data[3] >> 1) & 0x0FU; // DI_autoparkState
|
||||
bool tesla_autopark_now = (autopark_state == 3) || // ACTIVE
|
||||
(autopark_state == 4) || // COMPLETE
|
||||
(autopark_state == 9); // SELFPARK_STARTED
|
||||
|
||||
// Only consider rising edges while controls are not allowed
|
||||
if (tesla_autopark_now && !tesla_autopark_prev && !cruise_engaged_prev) {
|
||||
tesla_autopark = true;
|
||||
}
|
||||
if (!tesla_autopark_now) {
|
||||
tesla_autopark = false;
|
||||
}
|
||||
tesla_autopark_prev = tesla_autopark_now;
|
||||
|
||||
// Cruise state
|
||||
int cruise_state = (msg->data[1] >> 4) & 0x07U;
|
||||
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
|
||||
cruise_engaged = cruise_engaged && !tesla_autopark;
|
||||
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
}
|
||||
|
||||
if (msg->addr == 0x155U) {
|
||||
vehicle_moving = !GET_BIT(msg, 41U); // ESP_vehicleStandstillSts
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->bus == 1U) {
|
||||
if (msg->addr == 0x3DFU) {
|
||||
mads_button_press = (msg->data[3] == 3U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->bus == 2U) {
|
||||
// DAS_control
|
||||
if (msg->addr == 0x2b9U) {
|
||||
// "AEB_ACTIVE"
|
||||
tesla_stock_aeb = (msg->data[2] & 0x03U) == 1U;
|
||||
}
|
||||
|
||||
// DAS_steeringControl
|
||||
if (msg->addr == 0x488U) {
|
||||
int steering_control_type = msg->data[2] >> 6;
|
||||
bool tesla_stock_lkas_now = steering_control_type == tesla_get_steer_ctrl_type(2); // "LANE_KEEP_ASSIST"
|
||||
|
||||
// Only consider rising edges while controls are not allowed
|
||||
if (tesla_stock_lkas_now && !tesla_stock_lkas_prev && !(controls_allowed || controls_allowed_lateral)) {
|
||||
tesla_stock_lkas = true;
|
||||
}
|
||||
if (!tesla_stock_lkas_now) {
|
||||
tesla_stock_lkas = false;
|
||||
}
|
||||
tesla_stock_lkas_prev = tesla_stock_lkas_now;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static bool tesla_tx_hook(const CANPacket_t *msg) {
|
||||
const AngleSteeringLimits TESLA_STEERING_LIMITS = {
|
||||
.max_angle = 3600, // 360 deg, EPAS faults above this
|
||||
.angle_deg_to_can = 10,
|
||||
.frequency = 50U,
|
||||
};
|
||||
|
||||
// NOTE: based off TESLA_MODEL_Y to match openpilot
|
||||
const AngleSteeringParams TESLA_STEERING_PARAMS = {
|
||||
.slip_factor = -0.000580374383851451, // calc_slip_factor(VM)
|
||||
.steer_ratio = 12.,
|
||||
.wheelbase = 2.89,
|
||||
};
|
||||
|
||||
const LongitudinalLimits TESLA_LONG_LIMITS = {
|
||||
.max_accel = 425, // 2 m/s^2
|
||||
.min_accel = 288, // -3.48 m/s^2
|
||||
.inactive_accel = 375, // 0. m/s^2
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
bool violation = false;
|
||||
|
||||
// Don't send any messages when Autopark is active
|
||||
if (tesla_autopark) {
|
||||
violation = true;
|
||||
}
|
||||
|
||||
// Steering control: (0.1 * val) - 1638.35 in deg.
|
||||
if (msg->addr == 0x488U) {
|
||||
// We use 1/10 deg as a unit here
|
||||
int raw_angle_can = ((msg->data[0] & 0x7FU) << 8) | msg->data[1];
|
||||
int desired_angle = raw_angle_can - 16384;
|
||||
int steer_control_type = msg->data[2] >> 6;
|
||||
const int angle_ctrl_type = tesla_get_steer_ctrl_type(1);
|
||||
const int lkas_ctrl_type = tesla_get_steer_ctrl_type(2);
|
||||
bool steer_control_enabled = (steer_control_type == angle_ctrl_type) || // ANGLE_CONTROL
|
||||
(steer_control_type == lkas_ctrl_type); // LANE_KEEP_ASSIST
|
||||
|
||||
if (steer_angle_cmd_checks_vm(desired_angle, steer_control_enabled, TESLA_STEERING_LIMITS, TESLA_STEERING_PARAMS)) {
|
||||
violation = true;
|
||||
}
|
||||
|
||||
bool valid_steer_control_type = (steer_control_type == 0) || // NONE
|
||||
(steer_control_type == angle_ctrl_type) || // ANGLE_CONTROL
|
||||
(steer_control_type == lkas_ctrl_type); // LANE_KEEP_ASSIST
|
||||
if (!valid_steer_control_type) {
|
||||
violation = true;
|
||||
}
|
||||
|
||||
if (tesla_stock_lkas) {
|
||||
// Don't allow any steering commands when stock LKAS is active
|
||||
violation = true;
|
||||
}
|
||||
}
|
||||
|
||||
// DAS_control: longitudinal control message
|
||||
if (msg->addr == 0x2b9U) {
|
||||
// No AEB events may be sent by openpilot
|
||||
int aeb_event = msg->data[2] & 0x03U;
|
||||
if (aeb_event != 0) {
|
||||
violation = true;
|
||||
}
|
||||
|
||||
// Don't send long/cancel messages when the stock AEB system is active
|
||||
if (tesla_stock_aeb) {
|
||||
violation = true;
|
||||
}
|
||||
|
||||
int raw_accel_max = ((msg->data[6] & 0x1FU) << 4) | (msg->data[5] >> 4);
|
||||
int raw_accel_min = ((msg->data[5] & 0x0FU) << 5) | (msg->data[4] >> 3);
|
||||
int acc_state = msg->data[1] >> 4;
|
||||
|
||||
if (tesla_longitudinal) {
|
||||
// Prevent both acceleration from being negative, as this could cause the car to reverse after coming to standstill
|
||||
if ((raw_accel_max < TESLA_LONG_LIMITS.inactive_accel) && (raw_accel_min < TESLA_LONG_LIMITS.inactive_accel)) {
|
||||
violation = true;
|
||||
}
|
||||
|
||||
// Don't allow any acceleration limits above the safety limits
|
||||
violation |= longitudinal_accel_checks(raw_accel_max, TESLA_LONG_LIMITS);
|
||||
violation |= longitudinal_accel_checks(raw_accel_min, TESLA_LONG_LIMITS);
|
||||
} else {
|
||||
// Can only send cancel longitudinal messages when not controlling longitudinal
|
||||
if (acc_state != 13) { // ACC_CANCEL_GENERIC_SILENT
|
||||
violation = true;
|
||||
}
|
||||
|
||||
// No actuation is allowed when not controlling longitudinal
|
||||
if ((raw_accel_max != TESLA_LONG_LIMITS.inactive_accel) || (raw_accel_min != TESLA_LONG_LIMITS.inactive_accel)) {
|
||||
violation = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (violation) {
|
||||
tx = false;
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
static bool tesla_fwd_hook(int bus_num, int addr) {
|
||||
bool block_msg = false;
|
||||
|
||||
if (bus_num == 2) {
|
||||
if (!tesla_autopark) {
|
||||
// APS_eacMonitor
|
||||
if (addr == 0x27d) {
|
||||
block_msg = true;
|
||||
}
|
||||
|
||||
// DAS_steeringControl
|
||||
if ((addr == 0x488) && !tesla_stock_lkas) {
|
||||
block_msg = true;
|
||||
}
|
||||
|
||||
// DAS_control
|
||||
if (tesla_longitudinal && (addr == 0x2b9) && !tesla_stock_aeb) {
|
||||
block_msg = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return block_msg;
|
||||
}
|
||||
|
||||
static safety_config tesla_init(uint16_t param) {
|
||||
|
||||
static const CanMsg TESLA_M3_Y_TX_MSGS[] = {
|
||||
{0x488, 0, 4, .check_relay = true, .disable_static_blocking = true}, // DAS_steeringControl
|
||||
{0x2b9, 0, 8, .check_relay = false}, // DAS_control (for cancel)
|
||||
{0x27D, 0, 3, .check_relay = true, .disable_static_blocking = true}, // APS_eacMonitor
|
||||
};
|
||||
|
||||
static const CanMsg TESLA_M3_Y_LONG_TX_MSGS[] = {
|
||||
{0x488, 0, 4, .check_relay = true, .disable_static_blocking = true}, // DAS_steeringControl
|
||||
{0x2b9, 0, 8, .check_relay = true, .disable_static_blocking = true}, // DAS_control
|
||||
{0x27D, 0, 3, .check_relay = true, .disable_static_blocking = true}, // APS_eacMonitor
|
||||
};
|
||||
|
||||
const uint16_t TESLA_FLAG_FSD_14 = 2;
|
||||
tesla_fsd_14 = GET_FLAG(param, TESLA_FLAG_FSD_14);
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
const uint16_t TESLA_FLAG_LONGITUDINAL_CONTROL = 1;
|
||||
tesla_longitudinal = GET_FLAG(param, TESLA_FLAG_LONGITUDINAL_CONTROL);
|
||||
#endif
|
||||
|
||||
const uint16_t TESLA_PARAM_SP_VEHICLE_BUS = 1;
|
||||
|
||||
tesla_has_vehicle_bus = GET_FLAG(current_safety_param_sp, TESLA_PARAM_SP_VEHICLE_BUS);
|
||||
|
||||
tesla_stock_aeb = false;
|
||||
tesla_stock_lkas = false;
|
||||
tesla_stock_lkas_prev = false;
|
||||
// we need to assume Autopark/Summon on startup since DI_state is a low freq msg.
|
||||
// this is so that we don't fault if starting while these systems are active
|
||||
tesla_autopark = true;
|
||||
tesla_autopark_prev = false;
|
||||
|
||||
static RxCheck tesla_model3_y_rx_checks[] = {
|
||||
TESLA_COMMON_RX_CHECKS
|
||||
};
|
||||
|
||||
static RxCheck tesla_model3_y_vehicle_bus_rx_checks[] = {
|
||||
TESLA_COMMON_RX_CHECKS
|
||||
TESLA_VEHICLE_BUS_ADDR_CHECK
|
||||
};
|
||||
|
||||
safety_config ret;
|
||||
if (tesla_longitudinal) {
|
||||
SET_TX_MSGS(TESLA_M3_Y_LONG_TX_MSGS, ret);
|
||||
} else {
|
||||
SET_TX_MSGS(TESLA_M3_Y_TX_MSGS, ret);
|
||||
}
|
||||
|
||||
if (tesla_has_vehicle_bus) {
|
||||
SET_RX_CHECKS(tesla_model3_y_vehicle_bus_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(tesla_model3_y_rx_checks, ret);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
const safety_hooks tesla_hooks = {
|
||||
.init = tesla_init,
|
||||
.rx = tesla_rx_hook,
|
||||
.tx = tesla_tx_hook,
|
||||
.fwd = tesla_fwd_hook,
|
||||
.get_counter = tesla_get_counter,
|
||||
.get_checksum = tesla_get_checksum,
|
||||
.compute_checksum = tesla_compute_checksum,
|
||||
.get_quality_flag_valid = tesla_get_quality_flag_valid,
|
||||
};
|
||||
@@ -0,0 +1,576 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// Stock longitudinal
|
||||
#define TOYOTA_BASE_TX_MSGS \
|
||||
{0x191, 0, 8, .check_relay = true}, {0x412, 0, 8, .check_relay = true}, {0x1D2, 0, 8, .check_relay = false}, /* LKAS + LTA + PCM cancel cmd */ \
|
||||
|
||||
#define TOYOTA_COMMON_TX_MSGS \
|
||||
TOYOTA_BASE_TX_MSGS \
|
||||
{0x2E4, 0, 5, .check_relay = true}, \
|
||||
{0x343, 0, 8, .check_relay = false}, /* ACC cancel cmd */ \
|
||||
|
||||
#define TOYOTA_COMMON_SECOC_TX_MSGS \
|
||||
TOYOTA_BASE_TX_MSGS \
|
||||
{0x2E4, 0, 8, .check_relay = true}, {0x131, 0, 8, .check_relay = true}, \
|
||||
{0x343, 0, 8, .check_relay = false}, /* ACC cancel cmd */ \
|
||||
|
||||
#define TOYOTA_COMMON_LONG_TX_MSGS \
|
||||
TOYOTA_COMMON_TX_MSGS \
|
||||
/* DSU bus 0 */ \
|
||||
{0x283, 0, 7, .check_relay = false}, {0x2E6, 0, 8, .check_relay = false}, {0x2E7, 0, 8, .check_relay = false}, {0x33E, 0, 7, .check_relay = false}, \
|
||||
{0x344, 0, 8, .check_relay = false}, {0x365, 0, 7, .check_relay = false}, {0x366, 0, 7, .check_relay = false}, {0x4CB, 0, 8, .check_relay = false}, \
|
||||
/* DSU bus 1 */ \
|
||||
{0x128, 1, 6, .check_relay = false}, {0x141, 1, 4, .check_relay = false}, {0x160, 1, 8, .check_relay = false}, {0x161, 1, 7, .check_relay = false}, \
|
||||
{0x470, 1, 4, .check_relay = false}, \
|
||||
/* PCS_HUD */ \
|
||||
{0x411, 0, 8, .check_relay = false}, \
|
||||
/* radar diagnostic address */ \
|
||||
{0x750, 0, 8, .check_relay = false}, \
|
||||
/* ACC */ \
|
||||
{0x343, 0, 8, .check_relay = true}, \
|
||||
|
||||
#define TOYOTA_COMMON_SECOC_LONG_TX_MSGS \
|
||||
TOYOTA_COMMON_SECOC_TX_MSGS \
|
||||
{0x343, 0, 8, .check_relay = true}, \
|
||||
{0x183, 0, 8, .check_relay = true}, /* ACC_CONTROL_2 */ \
|
||||
|
||||
#define TOYOTA_COMMON_RX_CHECKS(lta) \
|
||||
{.msg = {{ 0xaa, 0, 8, 83U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x260, 0, 8, 50U, .ignore_counter = true, .ignore_quality_flag=!(lta)}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define TOYOTA_RX_CHECKS(lta) \
|
||||
TOYOTA_COMMON_RX_CHECKS(lta) \
|
||||
{.msg = {{0x1D2, 0, 8, 33U, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x226, 0, 8, 40U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define TOYOTA_ALT_BRAKE_RX_CHECKS(lta) \
|
||||
TOYOTA_COMMON_RX_CHECKS(lta) \
|
||||
{.msg = {{0x1D2, 0, 8, 33U, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x224, 0, 8, 40U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define TOYOTA_SECOC_RX_CHECKS \
|
||||
TOYOTA_COMMON_RX_CHECKS(false) \
|
||||
{.msg = {{0x176, 0, 8, 32U, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x116, 0, 8, 42U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x101, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define TOYOTA_PCM_CRUISE_2_ADDR_CHECK \
|
||||
{.msg = {{0x1D3, 0, 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .frequency = 33U}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define TOYOTA_DSU_CRUISE_ADDR_CHECK \
|
||||
{.msg = {{0x365, 0, 7, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .frequency = 5U, .ignore_frequency_check = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define TOYOTA_GAS_INTERCEPTOR_ADDR_CHECK \
|
||||
{.msg = {{0x201, 0, 6, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
static bool toyota_secoc = false;
|
||||
static bool toyota_alt_brake = false;
|
||||
static bool toyota_stock_longitudinal = false;
|
||||
static bool toyota_lta = false;
|
||||
static 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 *msg) {
|
||||
int len = GET_LEN(msg);
|
||||
uint8_t checksum = (uint8_t)(msg->addr) + (uint8_t)((unsigned int)(msg->addr) >> 8U) + (uint8_t)(len);
|
||||
for (int i = 0; i < (len - 1); i++) {
|
||||
checksum += (uint8_t)msg->data[i];
|
||||
}
|
||||
return checksum;
|
||||
}
|
||||
|
||||
static uint32_t toyota_get_checksum(const CANPacket_t *msg) {
|
||||
int checksum_byte = GET_LEN(msg) - 1U;
|
||||
return (uint8_t)(msg->data[checksum_byte]);
|
||||
}
|
||||
|
||||
static bool toyota_get_quality_flag_valid(const CANPacket_t *msg) {
|
||||
bool valid = false;
|
||||
if (msg->addr == 0x260U) {
|
||||
valid = !GET_BIT(msg, 3U); // STEER_TORQUE_SENSOR.STEER_ANGLE_INITIALIZING
|
||||
} else if (msg->addr == 0xaaU) { // WHEEL_SPEEDS
|
||||
// each wheel speed is 1-bit fault + 15-bit speed
|
||||
valid = true;
|
||||
for (uint8_t i = 0U; i < 4U; i += 1U) {
|
||||
if (GET_BIT(msg, (i * 16U) + 7U)) { // WHEEL_SPEED_xx_FAULT
|
||||
valid = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
}
|
||||
return valid;
|
||||
}
|
||||
|
||||
static int TOYOTA_GET_INTERCEPTOR(const CANPacket_t *msg) {
|
||||
uint16_t val1 = (uint16_t)((uint16_t)msg->data[0] << 8U) | (uint16_t)msg->data[1];
|
||||
uint16_t val2 = (uint16_t)((uint16_t)msg->data[2] << 8U) | (uint16_t)msg->data[3];
|
||||
uint16_t avg = (uint16_t)((val1 + val2) / 2U);
|
||||
|
||||
return (int)avg;
|
||||
}
|
||||
|
||||
static void toyota_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->bus == 0U) {
|
||||
|
||||
// get eps motor torque (0.66 factor in dbc)
|
||||
if (msg->addr == 0x260U) {
|
||||
int torque_meas_new = (msg->data[5] << 8) | msg->data[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 = (msg->data[1] << 8) | msg->data[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(msg, 3U);
|
||||
if (!steer_angle_initializing) {
|
||||
int angle_meas_new = (msg->data[3] << 8U) | msg->data[4];
|
||||
angle_meas_new = to_signed(angle_meas_new, 16);
|
||||
update_sample(&angle_meas, angle_meas_new);
|
||||
}
|
||||
}
|
||||
|
||||
// enter controls on rising edge of ACC, exit controls on ACC off
|
||||
// exit controls on rising edge of gas press, if not alternative experience
|
||||
// exit controls on rising edge of brake press
|
||||
if (toyota_secoc) {
|
||||
if (msg->addr == 0x176U) {
|
||||
bool cruise_engaged = GET_BIT(msg, 5U); // PCM_CRUISE.CRUISE_ACTIVE
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
}
|
||||
if (msg->addr == 0x116U) {
|
||||
gas_pressed = msg->data[1] != 0U; // GAS_PEDAL.GAS_PEDAL_USER
|
||||
}
|
||||
if (msg->addr == 0x101U) {
|
||||
brake_pressed = GET_BIT(msg, 3U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_rav4_prime_generated.dbc)
|
||||
}
|
||||
} else {
|
||||
if (msg->addr == 0x1D2U) {
|
||||
bool cruise_engaged = GET_BIT(msg, 5U); // PCM_CRUISE.CRUISE_ACTIVE
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
|
||||
if (!enable_gas_interceptor) {
|
||||
gas_pressed = !GET_BIT(msg, 4U); // PCM_CRUISE.GAS_RELEASED
|
||||
}
|
||||
}
|
||||
if (!toyota_alt_brake && (msg->addr == 0x226U)) {
|
||||
brake_pressed = GET_BIT(msg, 37U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_nodsu_pt_generated.dbc)
|
||||
}
|
||||
if (toyota_alt_brake && (msg->addr == 0x224U)) {
|
||||
brake_pressed = GET_BIT(msg, 5U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_new_mc_pt_generated.dbc)
|
||||
}
|
||||
}
|
||||
|
||||
// sample speed
|
||||
if (msg->addr == 0xaaU) {
|
||||
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 = ((msg->data[i] & 0x7FU) << 8U) | msg->data[(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 * KPH_TO_MS);
|
||||
}
|
||||
|
||||
if (msg->addr == 0x1D3U) {
|
||||
acc_main_on = GET_BIT(msg, 15U);
|
||||
}
|
||||
|
||||
if (msg->addr == 0x365U) {
|
||||
acc_main_on = GET_BIT(msg, 0U);
|
||||
}
|
||||
|
||||
// sample gas interceptor
|
||||
if (msg->addr == 0x201U) {
|
||||
// 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;
|
||||
|
||||
int gas_interceptor = TOYOTA_GET_INTERCEPTOR(msg);
|
||||
gas_pressed = gas_interceptor > toyota_gas_interceptor_thrsld;
|
||||
|
||||
gas_interceptor_prev = gas_interceptor;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool toyota_tx_hook(const CANPacket_t *msg) {
|
||||
const TorqueSteeringLimits TOYOTA_TORQUE_STEERING_LIMITS = {
|
||||
.max_torque = 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
|
||||
.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 = 17,
|
||||
.max_invalid_request_frames = 1,
|
||||
.min_valid_request_rt_interval = 162000, // 162ms; a ~10% buffer on cutting every 18 frames
|
||||
.has_steer_req_tolerance = true,
|
||||
};
|
||||
|
||||
static const AngleSteeringLimits TOYOTA_ANGLE_STEERING_LIMITS = {
|
||||
// LTA angle limits
|
||||
// factor for STEER_TORQUE_SENSOR->STEER_ANGLE and STEERING_LTA->STEER_ANGLE_CMD (1 / 0.0573)
|
||||
.max_angle = 1657, // EPS only accepts up to 94.9461
|
||||
.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_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
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
|
||||
// Check if msg is sent on BUS 0
|
||||
if (msg->bus == 0U) {
|
||||
// ACCEL: safety check on byte 1-2
|
||||
if (msg->addr == 0x343U) {
|
||||
int desired_accel = (msg->data[0] << 8) | msg->data[1];
|
||||
desired_accel = to_signed(desired_accel, 16);
|
||||
|
||||
bool violation = false;
|
||||
if (toyota_secoc) {
|
||||
// SecOC cars move accel to 0x183. Only allow inactive accel on 0x343 to match stock behavior
|
||||
violation = desired_accel != TOYOTA_LONG_LIMITS.inactive_accel;
|
||||
}
|
||||
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(msg, 24U);
|
||||
if (!cancel_req) {
|
||||
violation = true;
|
||||
}
|
||||
if (desired_accel != TOYOTA_LONG_LIMITS.inactive_accel) {
|
||||
violation = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (violation) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->addr == 0x183U) {
|
||||
int desired_accel = (msg->data[0] << 8) | msg->data[1];
|
||||
desired_accel = to_signed(desired_accel, 16);
|
||||
|
||||
tx = !longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS);
|
||||
}
|
||||
|
||||
// AEB: block all actuation. only used when DSU is unplugged
|
||||
if (msg->addr == 0x283U) {
|
||||
// only allow the checksum, which is the last byte
|
||||
bool block = (GET_BYTES(msg, 0, 4) != 0U) || (msg->data[4] != 0U) || (msg->data[5] != 0U);
|
||||
if (block) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// STEERING_LTA angle steering check
|
||||
if (msg->addr == 0x191U) {
|
||||
// check the STEER_REQUEST, STEER_REQUEST_2, TORQUE_WIND_DOWN, STEER_ANGLE_CMD signals
|
||||
bool lta_request = GET_BIT(msg, 0U);
|
||||
bool lta_request2 = GET_BIT(msg, 25U);
|
||||
int torque_wind_down = msg->data[5];
|
||||
int lta_angle = (msg->data[1] << 8) | msg->data[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_ANGLE_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 = SAFETY_MIN(SAFETY_ABS(torque_driver.min), SAFETY_ABS(torque_driver.max));
|
||||
if ((driver_torque > TOYOTA_LTA_MAX_DRIVER_TORQUE) && (torque_wind_down != 0)) {
|
||||
tx = false;
|
||||
}
|
||||
|
||||
int eps_torque = SAFETY_MIN(SAFETY_ABS(torque_meas.min), SAFETY_ABS(torque_meas.max));
|
||||
if ((eps_torque > TOYOTA_LTA_MAX_MEAS_TORQUE) && (torque_wind_down != 0)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// STEERING_LTA_2 angle steering check (SecOC)
|
||||
if (toyota_secoc && (msg->addr == 0x131U)) {
|
||||
// SecOC cars block any form of LTA actuation for now
|
||||
bool lta_request = GET_BIT(msg, 3U); // STEERING_LTA_2.STEER_REQUEST
|
||||
bool lta_request2 = GET_BIT(msg, 0U); // STEERING_LTA_2.STEER_REQUEST_2
|
||||
int lta_angle_msb = msg->data[2]; // STEERING_LTA_2.STEER_ANGLE_CMD (MSB)
|
||||
int lta_angle_lsb = msg->data[3]; // STEERING_LTA_2.STEER_ANGLE_CMD (LSB)
|
||||
|
||||
bool actuation = lta_request || lta_request2 || (lta_angle_msb != 0) || (lta_angle_lsb != 0);
|
||||
if (actuation) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// STEER: safety check on bytes 2-3
|
||||
if (msg->addr == 0x2E4U) {
|
||||
int desired_torque = (msg->data[1] << 8) | msg->data[2];
|
||||
desired_torque = to_signed(desired_torque, 16);
|
||||
bool steer_req = GET_BIT(msg, 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_TORQUE_STEERING_LIMITS)) {
|
||||
tx = false;
|
||||
}
|
||||
} else {
|
||||
if ((desired_torque != 0) || steer_req) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// GAS PEDAL: safety check
|
||||
if (msg->addr == 0x200U) {
|
||||
if (longitudinal_interceptor_checks(msg)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// UDS: Only tester present ("\x0F\x02\x3E\x00\x00\x00\x00\x00") allowed on diagnostics address
|
||||
if (msg->addr == 0x750U) {
|
||||
// this address is sub-addressed. only allow tester present to radar (0xF)
|
||||
bool invalid_uds_msg = (GET_BYTES(msg, 0, 4) != 0x003E020FU) || (GET_BYTES(msg, 4, 4) != 0x0U);
|
||||
if (invalid_uds_msg) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
static safety_config toyota_init(uint16_t param) {
|
||||
static const CanMsg TOYOTA_TX_MSGS[] = {
|
||||
TOYOTA_COMMON_TX_MSGS
|
||||
};
|
||||
|
||||
static const CanMsg TOYOTA_SECOC_TX_MSGS[] = {
|
||||
TOYOTA_COMMON_SECOC_TX_MSGS
|
||||
};
|
||||
|
||||
static const CanMsg TOYOTA_LONG_TX_MSGS[] = {
|
||||
TOYOTA_COMMON_LONG_TX_MSGS
|
||||
};
|
||||
|
||||
static const CanMsg TOYOTA_SECOC_LONG_TX_MSGS[] = {
|
||||
TOYOTA_COMMON_SECOC_LONG_TX_MSGS
|
||||
};
|
||||
|
||||
static const CanMsg TOYOTA_INTERCEPTOR_TX_MSGS[] = {
|
||||
TOYOTA_COMMON_LONG_TX_MSGS
|
||||
{0x200, 0, 6, .check_relay = false}, // gas interceptor
|
||||
};
|
||||
|
||||
// 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 uint16_t TOYOTA_PARAM_SP_UNSUPPORTED_DSU = 1;
|
||||
const uint16_t TOYTOA_PARAM_SP_GAS_INTERCEPTOR = 2;
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
const uint32_t TOYOTA_PARAM_SECOC = 8UL << TOYOTA_PARAM_OFFSET;
|
||||
toyota_secoc = GET_FLAG(param, TOYOTA_PARAM_SECOC);
|
||||
#endif
|
||||
|
||||
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);
|
||||
toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR;
|
||||
|
||||
const bool toyota_unsupported_dsu = GET_FLAG(current_safety_param_sp, TOYOTA_PARAM_SP_UNSUPPORTED_DSU);
|
||||
enable_gas_interceptor = GET_FLAG(current_safety_param_sp, TOYTOA_PARAM_SP_GAS_INTERCEPTOR);
|
||||
|
||||
// gas interceptor should not be used if openpilot is not controlling longitudinal or is a TSK car
|
||||
if (toyota_stock_longitudinal || toyota_secoc) {
|
||||
enable_gas_interceptor = false;
|
||||
}
|
||||
|
||||
safety_config ret;
|
||||
if (toyota_secoc) {
|
||||
if (toyota_stock_longitudinal) {
|
||||
SET_TX_MSGS(TOYOTA_SECOC_TX_MSGS, ret);
|
||||
} else {
|
||||
SET_TX_MSGS(TOYOTA_SECOC_LONG_TX_MSGS, ret);
|
||||
}
|
||||
} else {
|
||||
if (toyota_stock_longitudinal) {
|
||||
SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
|
||||
} else {
|
||||
SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret);
|
||||
}
|
||||
}
|
||||
|
||||
if (toyota_secoc) {
|
||||
static RxCheck toyota_secoc_rx_checks[] = {
|
||||
TOYOTA_SECOC_RX_CHECKS
|
||||
TOYOTA_PCM_CRUISE_2_ADDR_CHECK
|
||||
};
|
||||
|
||||
SET_RX_CHECKS(toyota_secoc_rx_checks, ret);
|
||||
} else if (toyota_lta) {
|
||||
// Check the quality flag for angle measurement when using LTA, since it's not set on TSS-P cars
|
||||
static RxCheck toyota_lta_rx_checks[] = {
|
||||
TOYOTA_RX_CHECKS(true)
|
||||
TOYOTA_PCM_CRUISE_2_ADDR_CHECK
|
||||
};
|
||||
|
||||
SET_RX_CHECKS(toyota_lta_rx_checks, ret);
|
||||
} else {
|
||||
static RxCheck toyota_lka_rx_checks[] = {
|
||||
TOYOTA_RX_CHECKS(false)
|
||||
TOYOTA_PCM_CRUISE_2_ADDR_CHECK
|
||||
};
|
||||
static RxCheck toyota_lka_alt_brake_rx_checks[] = {
|
||||
TOYOTA_ALT_BRAKE_RX_CHECKS(false)
|
||||
TOYOTA_PCM_CRUISE_2_ADDR_CHECK
|
||||
};
|
||||
static RxCheck toyota_lka_unsupported_dsu_rx_checks[] = {
|
||||
TOYOTA_RX_CHECKS(false)
|
||||
TOYOTA_DSU_CRUISE_ADDR_CHECK
|
||||
};
|
||||
static RxCheck toyota_lka_alt_brake_unsupported_dsu_rx_checks[] = {
|
||||
TOYOTA_ALT_BRAKE_RX_CHECKS(false)
|
||||
TOYOTA_DSU_CRUISE_ADDR_CHECK
|
||||
};
|
||||
|
||||
if (!toyota_alt_brake) {
|
||||
if (toyota_unsupported_dsu) {
|
||||
SET_RX_CHECKS(toyota_lka_unsupported_dsu_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(toyota_lka_rx_checks, ret);
|
||||
}
|
||||
} else {
|
||||
if (toyota_unsupported_dsu) {
|
||||
SET_RX_CHECKS(toyota_lka_alt_brake_unsupported_dsu_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(toyota_lka_alt_brake_rx_checks, ret);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (enable_gas_interceptor) {
|
||||
SET_TX_MSGS(TOYOTA_INTERCEPTOR_TX_MSGS, ret);
|
||||
|
||||
if (toyota_lta) {
|
||||
static RxCheck toyota_lta_interceptor_rx_checks[] = {
|
||||
TOYOTA_RX_CHECKS(true)
|
||||
TOYOTA_PCM_CRUISE_2_ADDR_CHECK
|
||||
TOYOTA_GAS_INTERCEPTOR_ADDR_CHECK
|
||||
};
|
||||
|
||||
SET_RX_CHECKS(toyota_lta_interceptor_rx_checks, ret);
|
||||
} else {
|
||||
static RxCheck toyota_lka_interceptor_rx_checks[] = {
|
||||
TOYOTA_RX_CHECKS(false)
|
||||
TOYOTA_PCM_CRUISE_2_ADDR_CHECK
|
||||
TOYOTA_GAS_INTERCEPTOR_ADDR_CHECK
|
||||
};
|
||||
static RxCheck toyota_lka_alt_brake_interceptor_rx_checks[] = {
|
||||
TOYOTA_ALT_BRAKE_RX_CHECKS(false)
|
||||
TOYOTA_PCM_CRUISE_2_ADDR_CHECK
|
||||
TOYOTA_GAS_INTERCEPTOR_ADDR_CHECK
|
||||
};
|
||||
static RxCheck toyota_lka_unsupported_dsu_interceptor_rx_checks[] = {
|
||||
TOYOTA_RX_CHECKS(false)
|
||||
TOYOTA_DSU_CRUISE_ADDR_CHECK
|
||||
TOYOTA_GAS_INTERCEPTOR_ADDR_CHECK
|
||||
};
|
||||
static RxCheck toyota_lka_alt_brake_unsupported_dsu_interceptor_rx_checks[] = {
|
||||
TOYOTA_ALT_BRAKE_RX_CHECKS(false)
|
||||
TOYOTA_DSU_CRUISE_ADDR_CHECK
|
||||
TOYOTA_GAS_INTERCEPTOR_ADDR_CHECK
|
||||
};
|
||||
|
||||
if (!toyota_alt_brake) {
|
||||
if (toyota_unsupported_dsu) {
|
||||
SET_RX_CHECKS(toyota_lka_unsupported_dsu_interceptor_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(toyota_lka_interceptor_rx_checks, ret);
|
||||
}
|
||||
} else {
|
||||
if (toyota_unsupported_dsu) {
|
||||
SET_RX_CHECKS(toyota_lka_alt_brake_unsupported_dsu_interceptor_rx_checks, ret);
|
||||
} else {
|
||||
SET_RX_CHECKS(toyota_lka_alt_brake_interceptor_rx_checks, ret);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
const safety_hooks toyota_hooks = {
|
||||
.init = toyota_init,
|
||||
.rx = toyota_rx_hook,
|
||||
.tx = toyota_tx_hook,
|
||||
.get_checksum = toyota_get_checksum,
|
||||
.compute_checksum = toyota_compute_checksum,
|
||||
.get_quality_flag_valid = toyota_get_quality_flag_valid,
|
||||
};
|
||||
@@ -0,0 +1,110 @@
|
||||
#pragma once
|
||||
|
||||
extern const uint16_t FLAG_VOLKSWAGEN_LONG_CONTROL;
|
||||
const uint16_t FLAG_VOLKSWAGEN_LONG_CONTROL = 1;
|
||||
|
||||
static uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR
|
||||
|
||||
extern bool volkswagen_longitudinal;
|
||||
bool volkswagen_longitudinal = false;
|
||||
|
||||
extern bool volkswagen_set_button_prev;
|
||||
bool volkswagen_set_button_prev = false;
|
||||
|
||||
extern bool volkswagen_resume_button_prev;
|
||||
bool volkswagen_resume_button_prev = false;
|
||||
|
||||
extern bool volkswagen_brake_pedal_switch;
|
||||
extern bool volkswagen_brake_pressure_detected;
|
||||
bool volkswagen_brake_pedal_switch = false;
|
||||
bool volkswagen_brake_pressure_detected = false;
|
||||
|
||||
#define MSG_LH_EPS_03 0x09FU // RX from EPS, for driver steering torque
|
||||
#define MSG_ESP_19 0x0B2U // RX from ABS, for wheel speeds
|
||||
#define MSG_ESP_05 0x106U // RX from ABS, for brake switch state
|
||||
#define MSG_TSK_06 0x120U // RX from ECU, for ACC status from drivetrain coordinator
|
||||
#define MSG_MOTOR_20 0x121U // RX from ECU, for driver throttle input
|
||||
#define MSG_ACC_06 0x122U // TX by OP, ACC control instructions to the drivetrain coordinator
|
||||
#define MSG_HCA_01 0x126U // TX by OP, Heading Control Assist steering torque
|
||||
#define MSG_GRA_ACC_01 0x12BU // TX by OP, ACC control buttons for cancel/resume
|
||||
#define MSG_ACC_07 0x12EU // TX by OP, ACC control instructions to the drivetrain coordinator
|
||||
#define MSG_ACC_02 0x30CU // TX by OP, ACC HUD data to the instrument cluster
|
||||
#define MSG_LDW_02 0x397U // TX by OP, Lane line recognition and text alerts
|
||||
#define MSG_MOTOR_14 0x3BEU // RX from ECU, for brake switch status
|
||||
|
||||
// MLB only messages
|
||||
#define MSG_ESP_03 0x103U // RX from ABS, for wheel speeds
|
||||
#define MSG_LS_01 0x10BU // TX by OP, ACC control buttons for cancel/resume
|
||||
#define MSG_MOTOR_03 0x105U // RX from ECU, for driver throttle input and brake switch status
|
||||
#define MSG_TSK_04 0x10EU // RX from ECU, for ACC status from drivetrain coordinator
|
||||
|
||||
static void volkswagen_common_init(void) {
|
||||
volkswagen_set_button_prev = false;
|
||||
volkswagen_resume_button_prev = false;
|
||||
volkswagen_brake_pedal_switch = false;
|
||||
volkswagen_brake_pressure_detected = false;
|
||||
gen_crc_lookup_table_8(0x2F, volkswagen_crc8_lut_8h2f);
|
||||
return;
|
||||
}
|
||||
|
||||
static uint32_t volkswagen_mqb_meb_get_checksum(const CANPacket_t *msg) {
|
||||
return (uint8_t)msg->data[0];
|
||||
}
|
||||
|
||||
static uint8_t volkswagen_mqb_meb_get_counter(const CANPacket_t *msg) {
|
||||
// MQB/MEB message counters are consistently found at LSB 8.
|
||||
return (uint8_t)msg->data[1] & 0xFU;
|
||||
}
|
||||
|
||||
static uint32_t volkswagen_mqb_meb_compute_crc(const CANPacket_t *msg) {
|
||||
int len = GET_LEN(msg);
|
||||
|
||||
// This is CRC-8H2F/AUTOSAR with a twist. See the opendbc/car/volkswagen/ implementation
|
||||
// of this algorithm for a version with explanatory comments.
|
||||
|
||||
uint8_t crc = 0xFFU;
|
||||
for (int i = 1; i < len; i++) {
|
||||
crc ^= (uint8_t)msg->data[i];
|
||||
crc = volkswagen_crc8_lut_8h2f[crc];
|
||||
}
|
||||
|
||||
uint8_t counter = volkswagen_mqb_meb_get_counter(msg);
|
||||
if (msg->addr == MSG_LH_EPS_03) {
|
||||
crc ^= (uint8_t[]){0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5}[counter];
|
||||
} else if (msg->addr == MSG_ESP_05) {
|
||||
crc ^= (uint8_t[]){0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07}[counter];
|
||||
} else if (msg->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 (msg->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 (msg->addr == MSG_GRA_ACC_01) {
|
||||
crc ^= (uint8_t[]){0x6A, 0x38, 0xB4, 0x27, 0x22, 0xEF, 0xE1, 0xBB, 0xF8, 0x80, 0x84, 0x49, 0xC7, 0x9E, 0x1E, 0x2B}[counter];
|
||||
} else {
|
||||
// Undefined CAN message, CRC check expected to fail
|
||||
}
|
||||
crc = volkswagen_crc8_lut_8h2f[crc];
|
||||
|
||||
return (uint8_t)(crc ^ 0xFFU);
|
||||
}
|
||||
|
||||
static int volkswagen_mlb_mqb_driver_input_torque(const CANPacket_t *msg) {
|
||||
// Signal: LH_EPS_03.EPS_Lenkmoment (absolute torque)
|
||||
// Signal: LH_EPS_03.EPS_VZ_Lenkmoment (direction)
|
||||
int torque_driver_new = msg->data[5] | ((msg->data[6] & 0x1FU) << 8);
|
||||
bool sign = GET_BIT(msg, 55U);
|
||||
if (sign) {
|
||||
torque_driver_new *= -1;
|
||||
}
|
||||
return torque_driver_new;
|
||||
}
|
||||
|
||||
static int volkswagen_mlb_mqb_steering_control_torque(const CANPacket_t *msg) {
|
||||
// Signal: HCA_01.HCA_01_LM_Offset (absolute torque)
|
||||
// Signal: HCA_01.HCA_01_LM_OffSign (direction)
|
||||
int desired_torque = msg->data[2] | ((msg->data[3] & 0x1U) << 8);
|
||||
bool sign = GET_BIT(msg, 31U);
|
||||
if (sign) {
|
||||
desired_torque *= -1;
|
||||
}
|
||||
return desired_torque;
|
||||
}
|
||||
@@ -0,0 +1,284 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/volkswagen_common.h"
|
||||
|
||||
#define MSG_ESC_51 0xFCU // RX, for wheel speeds
|
||||
#define MSG_ACC_18 0x14DU // TX by OP, ACC control instructions to the drivetrain coordinator
|
||||
#define MSG_ESP_21 0xFDU // RX, redundant vehicle speed source
|
||||
#define MSG_HCA_03 0x303U
|
||||
#define MSG_ACC_19 0x300U // TX by OP, ACC HUD data to the instrument cluster
|
||||
#define MSG_QFK_01 0x13DU
|
||||
#define MSG_Motor_51 0x10BU // RX for TSK state and accel pedal
|
||||
#define MSG_KLR_01 0x25DU // TX, for capacitive steering wheel
|
||||
#define MSG_TA_01 0x26BU // TX by OP, Travel Assist status
|
||||
|
||||
static bool volkswagen_meb_alt_crc = false;
|
||||
|
||||
#define VOLKSWAGEN_MEB_COMMON_RX_CHECKS \
|
||||
{.msg = {{MSG_LH_EPS_03, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_MOTOR_14, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_GRA_ACC_01, 0, 8, 33U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_QFK_01, 0, 32, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_ESP_21, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
|
||||
static uint32_t volkswagen_meb_compute_crc(const CANPacket_t *msg) {
|
||||
int len = GET_LEN(msg);
|
||||
|
||||
uint8_t crc = 0xFFU;
|
||||
for (int i = 1; i < len; i++) {
|
||||
crc ^= (uint8_t)msg->data[i];
|
||||
crc = volkswagen_crc8_lut_8h2f[crc];
|
||||
}
|
||||
|
||||
uint8_t counter = volkswagen_mqb_meb_get_counter(msg);
|
||||
if (msg->addr == MSG_LH_EPS_03) {
|
||||
crc ^= (uint8_t[]){0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5}[counter];
|
||||
} else if (msg->addr == MSG_GRA_ACC_01) {
|
||||
crc ^= (uint8_t[]){0x6A, 0x38, 0xB4, 0x27, 0x22, 0xEF, 0xE1, 0xBB, 0xF8, 0x80, 0x84, 0x49, 0xC7, 0x9E, 0x1E, 0x2B}[counter];
|
||||
} else if (msg->addr == MSG_QFK_01) {
|
||||
crc ^= (uint8_t[]){0x20, 0xCA, 0x68, 0xD5, 0x1B, 0x31, 0xE2, 0xDA, 0x08, 0x0A, 0xD4, 0xDE, 0x9C, 0xE4, 0x35, 0x5B}[counter];
|
||||
} else if (msg->addr == MSG_ESC_51) {
|
||||
crc ^= (uint8_t[]){0x77, 0x5C, 0xA0, 0x89, 0x4B, 0x7C, 0xBB, 0xD6, 0x1F, 0x6C, 0x4F, 0xF6, 0x20, 0x2B, 0x43, 0xDD}[counter];
|
||||
} else if (msg->addr == MSG_ESP_21) {
|
||||
crc ^= (uint8_t[]){0xB4, 0xEF, 0xF8, 0x49, 0x1E, 0xE5, 0xC2, 0xC0, 0x97, 0x19, 0x3C, 0xC9, 0xF1, 0x98, 0xD6, 0x61}[counter];
|
||||
} else if (msg->addr == MSG_Motor_51) {
|
||||
crc ^= (uint8_t[]){0x77, 0x5C, 0xA0, 0x89, 0x4B, 0x7C, 0xBB, 0xD6, 0x1F, 0x6C, 0x4F, 0xF6, 0x20, 0x2B, 0x43, 0xDD}[counter];
|
||||
} else {
|
||||
// Undefined CAN message, CRC check expected to fail
|
||||
}
|
||||
crc = volkswagen_crc8_lut_8h2f[crc];
|
||||
|
||||
return (uint8_t)(crc ^ 0xFFU);
|
||||
}
|
||||
|
||||
static uint32_t volkswagen_meb_alt_crc_compute(const CANPacket_t *msg) {
|
||||
uint32_t ret = volkswagen_meb_compute_crc(msg);
|
||||
int len = 0;
|
||||
if (volkswagen_meb_alt_crc) {
|
||||
if (msg->addr == MSG_QFK_01) {
|
||||
len = 28;
|
||||
} else if (msg->addr == MSG_ESC_51) {
|
||||
len = 60;
|
||||
} else if (msg->addr == MSG_Motor_51) {
|
||||
len = 44;
|
||||
} else {
|
||||
len = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (len > 0) {
|
||||
uint8_t crc = 0xFFU;
|
||||
for (int i = 1; i < len; i++) {
|
||||
crc ^= (uint8_t)msg->data[i];
|
||||
crc = volkswagen_crc8_lut_8h2f[crc];
|
||||
}
|
||||
|
||||
uint8_t counter = volkswagen_mqb_meb_get_counter(msg);
|
||||
if (msg->addr == MSG_QFK_01) {
|
||||
crc ^= (uint8_t[]){0x18, 0x71, 0x10, 0x8D, 0xD7, 0xAA, 0xB0, 0x78, 0xAC, 0x12, 0xAE, 0x0C, 0xDD, 0xF1, 0x85, 0x68}[counter];
|
||||
} else if (msg->addr == MSG_ESC_51) {
|
||||
crc ^= (uint8_t[]){0x69, 0xDC, 0xF9, 0x64, 0x6A, 0xCE, 0x55, 0x2C, 0xC4, 0x38, 0x8F, 0xD1, 0xC6, 0x43, 0xB4, 0xB1}[counter];
|
||||
} else {
|
||||
crc ^= (uint8_t[]){0x2C, 0xB1, 0x1A, 0x75, 0xBB, 0x65, 0x79, 0x47, 0x81, 0x2B, 0xCC, 0x96, 0x17, 0xDB, 0xC0, 0x94}[counter];
|
||||
}
|
||||
|
||||
crc = (uint8_t)(volkswagen_crc8_lut_8h2f[crc] ^ 0xFFU);
|
||||
if (crc == msg->data[0]) {
|
||||
ret = crc;
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
static safety_config volkswagen_meb_init(uint16_t param) {
|
||||
// Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
|
||||
static const CanMsg VOLKSWAGEN_MEB_STOCK_TX_MSGS[] = {
|
||||
{MSG_HCA_03, 0, 24, .check_relay = true},
|
||||
{MSG_GRA_ACC_01, 0, 8, .check_relay = false},
|
||||
{MSG_GRA_ACC_01, 2, 8, .check_relay = false},
|
||||
{MSG_LDW_02, 0, 8, .check_relay = true},
|
||||
{MSG_KLR_01, 0, 8, .check_relay = false},
|
||||
{MSG_KLR_01, 2, 8, .check_relay = true},
|
||||
};
|
||||
|
||||
static const CanMsg VOLKSWAGEN_MEB_LONG_TX_MSGS[] = {
|
||||
{MSG_HCA_03, 0, 24, .check_relay = true},
|
||||
{MSG_LDW_02, 0, 8, .check_relay = true},
|
||||
{MSG_KLR_01, 0, 8, .check_relay = false},
|
||||
{MSG_KLR_01, 2, 8, .check_relay = true},
|
||||
{MSG_ACC_19, 0, 48, .check_relay = true},
|
||||
{MSG_ACC_18, 0, 32, .check_relay = true},
|
||||
{MSG_TA_01, 0, 8, .check_relay = true},
|
||||
};
|
||||
|
||||
static RxCheck volkswagen_meb_rx_checks[] = {
|
||||
VOLKSWAGEN_MEB_COMMON_RX_CHECKS
|
||||
{.msg = {{MSG_Motor_51, 0, 32, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_ESC_51, 0, 48, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
static RxCheck volkswagen_meb_gen2_rx_checks[] = {
|
||||
VOLKSWAGEN_MEB_COMMON_RX_CHECKS
|
||||
{.msg = {{MSG_Motor_51, 0, 48, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_ESC_51, 0, 64, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
volkswagen_common_init();
|
||||
const uint16_t FLAG_VOLKSWAGEN_MEB_ALT_CRC = 2;
|
||||
volkswagen_meb_alt_crc = GET_FLAG(param, FLAG_VOLKSWAGEN_MEB_ALT_CRC);
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL);
|
||||
#endif
|
||||
|
||||
safety_config ret;
|
||||
if (volkswagen_longitudinal && volkswagen_meb_alt_crc) {
|
||||
ret = BUILD_SAFETY_CFG(volkswagen_meb_gen2_rx_checks, VOLKSWAGEN_MEB_LONG_TX_MSGS);
|
||||
} else if (volkswagen_longitudinal) {
|
||||
ret = BUILD_SAFETY_CFG(volkswagen_meb_rx_checks, VOLKSWAGEN_MEB_LONG_TX_MSGS);
|
||||
} else if (volkswagen_meb_alt_crc) {
|
||||
ret = BUILD_SAFETY_CFG(volkswagen_meb_gen2_rx_checks, VOLKSWAGEN_MEB_STOCK_TX_MSGS);
|
||||
} else {
|
||||
ret = BUILD_SAFETY_CFG(volkswagen_meb_rx_checks, VOLKSWAGEN_MEB_STOCK_TX_MSGS);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
static void volkswagen_meb_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->bus == 0U) {
|
||||
// Update in-motion state by sampling wheel speeds
|
||||
if (msg->addr == MSG_ESC_51) {
|
||||
uint32_t fl = msg->data[8] | (msg->data[9] << 8);
|
||||
uint32_t fr = msg->data[10] | (msg->data[11] << 8);
|
||||
uint32_t rl = msg->data[12] | (msg->data[13] << 8);
|
||||
uint32_t rr = msg->data[14] | (msg->data[15] << 8);
|
||||
vehicle_moving = (fr > 0U) || (rr > 0U) || (rl > 0U) || (fl > 0U);
|
||||
UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4.0 * 0.0075 * KPH_TO_MS);
|
||||
}
|
||||
|
||||
// Check vehicle speed with redundant source
|
||||
if (msg->addr == MSG_ESP_21) {
|
||||
// Signal: ESP_v_Signal
|
||||
float esp_speed = ((msg->data[5] << 8) | msg->data[4]) * 0.01 * KPH_TO_MS;
|
||||
UPDATE_VEHICLE_SPEED_2(esp_speed);
|
||||
}
|
||||
|
||||
if (msg->addr == MSG_QFK_01) {
|
||||
int current_curvature = ((msg->data[6] & 0x7FU) << 8) | msg->data[5];
|
||||
current_curvature *= GET_BIT(msg, 55U) ? 1 : -1;
|
||||
update_sample(&curvature_state.meas, current_curvature);
|
||||
}
|
||||
|
||||
if (msg->addr == MSG_LH_EPS_03) {
|
||||
update_sample(&torque_driver, volkswagen_mlb_mqb_driver_input_torque(msg));
|
||||
}
|
||||
|
||||
if (msg->addr == MSG_Motor_51) {
|
||||
int acc_status = (msg->data[11] & 0x07U);
|
||||
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;
|
||||
}
|
||||
|
||||
int accel_pedal_value = ((msg->data[1] >> 4) & 0x0FU) | ((msg->data[2] & 0x1FU) << 4);
|
||||
gas_pressed = accel_pedal_value > 0;
|
||||
}
|
||||
|
||||
if (msg->addr == MSG_GRA_ACC_01) {
|
||||
// If 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(msg, 16U);
|
||||
bool resume_button = GET_BIT(msg, 19U);
|
||||
if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) {
|
||||
controls_allowed = acc_main_on;
|
||||
}
|
||||
volkswagen_set_button_prev = set_button;
|
||||
volkswagen_resume_button_prev = resume_button;
|
||||
}
|
||||
|
||||
// Always exit controls on rising edge of Cancel
|
||||
if (GET_BIT(msg, 13U)) {
|
||||
controls_allowed = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->addr == MSG_MOTOR_14) {
|
||||
brake_pressed = GET_BIT(msg, 28U);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool volkswagen_meb_tx_hook(const CANPacket_t *msg) {
|
||||
// acceleration in m/s2 * 1000 to avoid floating point math
|
||||
const LongitudinalLimits VOLKSWAGEN_MEB_LONG_LIMITS = {
|
||||
.max_accel = 2000,
|
||||
.min_accel = -3500,
|
||||
.inactive_accel = 3010, // VW sends one increment above the max range when inactive
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
|
||||
// Safety check for MSG_ACC_18 acceleration requests
|
||||
if (msg->addr == MSG_ACC_18) {
|
||||
// Signal: ACC_18.ACC_Sollbeschleunigung_02 (acceleration in m/s2, scale 0.005, offset -7.22)
|
||||
int desired_accel = ((((msg->data[4] & 0x7U) << 8) | msg->data[3]) * 5U) - 7220U;
|
||||
// allow ACCEL_OVERRIDE (0) while controls are allowed even when the driver is on the gas
|
||||
bool accel_override = controls_allowed && (desired_accel == 0);
|
||||
if (!accel_override && longitudinal_accel_checks(desired_accel, VOLKSWAGEN_MEB_LONG_LIMITS)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->addr == MSG_HCA_03) {
|
||||
const CurvatureSteeringLimits VOLKSWAGEN_MEB_STEERING_LIMITS = {
|
||||
.max_curvature = 29105,
|
||||
.curvature_to_can = 149253.7313f,
|
||||
.frequency = 50, // Hz
|
||||
.max_curvature_error = 0, // disabled, MEB doesn't track rack
|
||||
.curvature_error_min_speed = 0.0, // disabled
|
||||
.max_steer_power = 125,
|
||||
.inactive_curvature_is_zero = false, // MEB winds down with measured curvature
|
||||
};
|
||||
|
||||
int desired_curvature_raw = GET_BYTES(msg, 3, 2) & 0x7FFFU;
|
||||
|
||||
bool desired_curvature_sign = GET_BIT(msg, 39U);
|
||||
if (!desired_curvature_sign) {
|
||||
desired_curvature_raw *= -1;
|
||||
}
|
||||
|
||||
bool steer_req = (((msg->data[1] >> 4) & 0x0FU) == 4U);
|
||||
int steer_power = msg->data[2];
|
||||
|
||||
if (steer_curvature_cmd_checks(desired_curvature_raw, steer_power, steer_req, VOLKSWAGEN_MEB_STEERING_LIMITS)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
if ((msg->addr == MSG_GRA_ACC_01) && !controls_allowed) {
|
||||
// only allow cancel button: bit 13
|
||||
if (!GET_BIT(msg, 13U)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
const safety_hooks volkswagen_meb_hooks = {
|
||||
.init = volkswagen_meb_init,
|
||||
.rx = volkswagen_meb_rx_hook,
|
||||
.tx = volkswagen_meb_tx_hook,
|
||||
.get_counter = volkswagen_mqb_meb_get_counter,
|
||||
.get_checksum = volkswagen_mqb_meb_get_checksum,
|
||||
.compute_checksum = volkswagen_meb_alt_crc_compute,
|
||||
};
|
||||
@@ -0,0 +1,127 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/volkswagen_common.h"
|
||||
|
||||
|
||||
static safety_config volkswagen_mlb_init(uint16_t param) {
|
||||
// Transmit of LS_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
|
||||
static const CanMsg VOLKSWAGEN_MLB_STOCK_TX_MSGS[] = {{MSG_HCA_01, 0, 8, .check_relay = true}, {MSG_LDW_02, 0, 8, .check_relay = true},
|
||||
{MSG_LS_01, 0, 4, .check_relay = false}, {MSG_LS_01, 2, 4, .check_relay = false}};
|
||||
|
||||
static RxCheck volkswagen_mlb_rx_checks[] = {
|
||||
// TODO: implement checksum validation
|
||||
{.msg = {{MSG_ESP_03, 0, 8, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_LH_EPS_03, 0, 8, 100U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_ESP_05, 0, 8, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_TSK_04, 1, 8, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_MOTOR_03, 0, 8, 100U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_LS_01, 0, 4, 10U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
SAFETY_UNUSED(param);
|
||||
volkswagen_common_init();
|
||||
|
||||
return BUILD_SAFETY_CFG(volkswagen_mlb_rx_checks, VOLKSWAGEN_MLB_STOCK_TX_MSGS);
|
||||
}
|
||||
|
||||
static void volkswagen_mlb_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->bus == 0U) {
|
||||
// Check all wheel speeds for any movement
|
||||
// Signals: ESP_03.ESP_[VL|VR|HL|HR]_Radgeschw
|
||||
if (msg->addr == MSG_ESP_03) {
|
||||
uint32_t speed = 0;
|
||||
speed += ((msg->data[3] & 0xFU) << 8) | msg->data[2]; // FL
|
||||
speed += (msg->data[4] << 4) | (msg->data[3] >> 4); // FR
|
||||
speed += ((msg->data[6] & 0xFU) << 8) | msg->data[5]; // RL
|
||||
speed += (msg->data[7] << 4) | (msg->data[6] >> 4); // RR
|
||||
vehicle_moving = speed > 0U;
|
||||
}
|
||||
|
||||
// Update driver input torque
|
||||
if (msg->addr == MSG_LH_EPS_03) {
|
||||
update_sample(&torque_driver, volkswagen_mlb_mqb_driver_input_torque(msg));
|
||||
}
|
||||
|
||||
if (msg->addr == MSG_LS_01) {
|
||||
// Always exit controls on rising edge of Cancel
|
||||
// Signal: LS_01.LS_Abbrechen
|
||||
if (GET_BIT(msg, 13U)) {
|
||||
controls_allowed = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Signal: Motor_03.MO_Fahrpedalrohwert_01
|
||||
// Signal: Motor_03.MO_Fahrer_bremst
|
||||
if (msg->addr == MSG_MOTOR_03) {
|
||||
gas_pressed = msg->data[6] != 0U;
|
||||
volkswagen_brake_pedal_switch = GET_BIT(msg, 35U);
|
||||
}
|
||||
|
||||
if (msg->addr == MSG_ESP_05) {
|
||||
volkswagen_brake_pressure_detected = GET_BIT(msg, 26U);
|
||||
}
|
||||
|
||||
brake_pressed = volkswagen_brake_pedal_switch || volkswagen_brake_pressure_detected;
|
||||
|
||||
}
|
||||
|
||||
if (msg->bus == 1U) {
|
||||
if (msg->addr == MSG_TSK_04) {
|
||||
// When using stock ACC, enter controls on rising edge of stock ACC engage, exit on disengage
|
||||
// Signal: TSK_04.TSK_Status_GRA_ACC_02
|
||||
int acc_status = (msg->data[7] & 0xC0U) >> 6;
|
||||
bool cruise_engaged = (acc_status == 1) || (acc_status == 2);
|
||||
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool volkswagen_mlb_tx_hook(const CANPacket_t *msg) {
|
||||
// lateral limits
|
||||
const TorqueSteeringLimits VOLKSWAGEN_MLB_STEERING_LIMITS = {
|
||||
.max_torque = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated)
|
||||
.max_rt_delta = 169, // 10 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 112.5 ; 112.5 * 1.5 for safety pad = 168.75
|
||||
.max_rate_up = 9, // 5.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 = 60,
|
||||
.driver_torque_multiplier = 3,
|
||||
.type = TorqueDriverLimited,
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
|
||||
// Safety check for HCA_01 Heading Control Assist torque
|
||||
if (msg->addr == MSG_HCA_01) {
|
||||
int desired_torque = volkswagen_mlb_mqb_steering_control_torque(msg);
|
||||
|
||||
int steer_status = msg->data[4] & 0xFU;
|
||||
bool steer_req = (steer_status == 5) || (steer_status == 7);
|
||||
|
||||
if (steer_torque_cmd_checks(desired_torque, steer_req, VOLKSWAGEN_MLB_STEERING_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 ((msg->addr == MSG_LS_01) && !controls_allowed) {
|
||||
// disallow resume and set: bits 16 and 19
|
||||
if (GET_BIT(msg, 16U) || GET_BIT(msg, 19U)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
// TODO: rename these functions to MXB or something
|
||||
const safety_hooks volkswagen_mlb_hooks = {
|
||||
.init = volkswagen_mlb_init,
|
||||
.rx = volkswagen_mlb_rx_hook,
|
||||
.tx = volkswagen_mlb_tx_hook,
|
||||
.get_counter = volkswagen_mqb_meb_get_counter,
|
||||
.get_checksum = volkswagen_mqb_meb_get_checksum,
|
||||
.compute_checksum = volkswagen_mqb_meb_compute_crc,
|
||||
};
|
||||
@@ -0,0 +1,189 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/volkswagen_common.h"
|
||||
|
||||
static safety_config volkswagen_mqb_init(uint16_t param) {
|
||||
// Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
|
||||
// MSG_LH_EPS_03: openpilot needs to replace apparent driver steering input torque to pacify VW Emergency Assist
|
||||
static const CanMsg VOLKSWAGEN_MQB_STOCK_TX_MSGS[] = {{MSG_HCA_01, 0, 8, .check_relay = true}, {MSG_GRA_ACC_01, 0, 8, .check_relay = false}, {MSG_GRA_ACC_01, 2, 8, .check_relay = false},
|
||||
{MSG_LDW_02, 0, 8, .check_relay = true}, {MSG_LH_EPS_03, 2, 8, .check_relay = true}};
|
||||
|
||||
static const CanMsg VOLKSWAGEN_MQB_LONG_TX_MSGS[] = {{MSG_HCA_01, 0, 8, .check_relay = true}, {MSG_LDW_02, 0, 8, .check_relay = true}, {MSG_LH_EPS_03, 2, 8, .check_relay = true},
|
||||
{MSG_ACC_02, 0, 8, .check_relay = true}, {MSG_ACC_06, 0, 8, .check_relay = true}, {MSG_ACC_07, 0, 8, .check_relay = true}};
|
||||
|
||||
static RxCheck volkswagen_mqb_rx_checks[] = {
|
||||
{.msg = {{MSG_ESP_19, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_LH_EPS_03, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_ESP_05, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_TSK_06, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_MOTOR_20, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_MOTOR_14, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_GRA_ACC_01, 0, 8, 33U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
volkswagen_common_init();
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL);
|
||||
#else
|
||||
SAFETY_UNUSED(param);
|
||||
#endif
|
||||
|
||||
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 *msg) {
|
||||
if (msg->bus == 0U) {
|
||||
// Update in-motion state by sampling wheel speeds
|
||||
if (msg->addr == MSG_ESP_19) {
|
||||
// sum 4 wheel speeds
|
||||
int speed = 0;
|
||||
for (uint8_t i = 0U; i < 8U; i += 2U) {
|
||||
int wheel_speed = msg->data[i] | (msg->data[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 (msg->addr == MSG_LH_EPS_03) {
|
||||
update_sample(&torque_driver, volkswagen_mlb_mqb_driver_input_torque(msg));
|
||||
}
|
||||
|
||||
if (msg->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 = (msg->data[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 (msg->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(msg, 16U);
|
||||
bool resume_button = GET_BIT(msg, 19U);
|
||||
if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) {
|
||||
controls_allowed = acc_main_on;
|
||||
}
|
||||
volkswagen_set_button_prev = set_button;
|
||||
volkswagen_resume_button_prev = resume_button;
|
||||
}
|
||||
// Always exit controls on rising edge of Cancel
|
||||
// Signal: GRA_ACC_01.GRA_Abbrechen
|
||||
if (GET_BIT(msg, 13U)) {
|
||||
controls_allowed = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Signal: Motor_20.MO_Fahrpedalrohwert_01
|
||||
if (msg->addr == MSG_MOTOR_20) {
|
||||
gas_pressed = ((GET_BYTES(msg, 0, 4) >> 12) & 0xFFU) != 0U;
|
||||
}
|
||||
|
||||
// Signal: Motor_14.MO_Fahrer_bremst (ECU detected brake pedal switch F63)
|
||||
if (msg->addr == MSG_MOTOR_14) {
|
||||
volkswagen_brake_pedal_switch = GET_BIT(msg, 28U);
|
||||
}
|
||||
|
||||
// Signal: ESP_05.ESP_Fahrer_bremst (ESP detected driver brake pressure above platform specified threshold)
|
||||
if (msg->addr == MSG_ESP_05) {
|
||||
volkswagen_brake_pressure_detected = GET_BIT(msg, 26U);
|
||||
}
|
||||
|
||||
brake_pressed = volkswagen_brake_pedal_switch || volkswagen_brake_pressure_detected;
|
||||
}
|
||||
}
|
||||
|
||||
static bool volkswagen_mqb_tx_hook(const CANPacket_t *msg) {
|
||||
// lateral limits
|
||||
const TorqueSteeringLimits VOLKSWAGEN_MQB_STEERING_LIMITS = {
|
||||
.max_torque = 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_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_multiplier = 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
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
|
||||
// Safety check for HCA_01 Heading Control Assist torque
|
||||
if (msg->addr == MSG_HCA_01) {
|
||||
int desired_torque = volkswagen_mlb_mqb_steering_control_torque(msg);
|
||||
bool steer_req = GET_BIT(msg, 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 ((msg->addr == MSG_ACC_06) || (msg->addr == MSG_ACC_07)) {
|
||||
bool violation = false;
|
||||
int desired_accel = 0;
|
||||
|
||||
if (msg->addr == MSG_ACC_06) {
|
||||
// Signal: ACC_06.ACC_Sollbeschleunigung_02 (acceleration in m/s2, scale 0.005, offset -7.22)
|
||||
desired_accel = ((((msg->data[4] & 0x7U) << 8) | msg->data[3]) * 5U) - 7220U;
|
||||
} else {
|
||||
// Signal: ACC_07.ACC_Folgebeschl (acceleration in m/s2, scale 0.03, offset -4.6)
|
||||
int secondary_accel = (msg->data[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 = (((msg->data[7] << 3) | ((msg->data[6] & 0xE0U) >> 5)) * 5U) - 7220U;
|
||||
}
|
||||
|
||||
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 ((msg->addr == MSG_GRA_ACC_01) && !controls_allowed) {
|
||||
// disallow resume and set: bits 16 and 19
|
||||
if ((msg->data[2] & 0x9U) != 0U) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
const safety_hooks volkswagen_mqb_hooks = {
|
||||
.init = volkswagen_mqb_init,
|
||||
.rx = volkswagen_mqb_rx_hook,
|
||||
.tx = volkswagen_mqb_tx_hook,
|
||||
.get_counter = volkswagen_mqb_meb_get_counter,
|
||||
.get_checksum = volkswagen_mqb_meb_get_checksum,
|
||||
.compute_checksum = volkswagen_mqb_meb_compute_crc,
|
||||
};
|
||||
@@ -0,0 +1,219 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/volkswagen_common.h"
|
||||
|
||||
#define MSG_LENKHILFE_3 0x0D0U // RX from EPS, for steering angle and driver steering torque
|
||||
#define MSG_HCA_1 0x0D2U // TX by OP, Heading Control Assist steering torque
|
||||
#define MSG_BREMSE_1 0x1A0U // RX from ABS, for ego speed
|
||||
#define MSG_MOTOR_2 0x288U // RX from ECU, for CC state and brake switch state
|
||||
#define MSG_ACC_SYSTEM 0x368U // TX by OP, longitudinal acceleration controls
|
||||
#define MSG_MOTOR_3 0x380U // RX from ECU, for driver throttle input
|
||||
#define MSG_GRA_NEU 0x38AU // TX by OP, ACC control buttons for cancel/resume
|
||||
#define MSG_MOTOR_5 0x480U // RX from ECU, for ACC main switch state
|
||||
#define MSG_ACC_GRA_ANZEIGE 0x56AU // TX by OP, ACC HUD
|
||||
#define MSG_LDW_1 0x5BEU // TX by OP, Lane line recognition and text alerts
|
||||
|
||||
static uint32_t volkswagen_pq_get_checksum(const CANPacket_t *msg) {
|
||||
return (uint32_t)msg->data[(msg->addr == MSG_MOTOR_5) ? 7 : 0];
|
||||
}
|
||||
|
||||
static uint8_t volkswagen_pq_get_counter(const CANPacket_t *msg) {
|
||||
uint8_t counter = 0U;
|
||||
|
||||
if (msg->addr == MSG_LENKHILFE_3) {
|
||||
counter = (uint8_t)(msg->data[1] & 0xF0U) >> 4;
|
||||
} else if (msg->addr == MSG_GRA_NEU) {
|
||||
counter = (uint8_t)(msg->data[2] & 0xF0U) >> 4;
|
||||
} else {
|
||||
}
|
||||
|
||||
return counter;
|
||||
}
|
||||
|
||||
static uint32_t volkswagen_pq_compute_checksum(const CANPacket_t *msg) {
|
||||
int len = GET_LEN(msg);
|
||||
uint8_t checksum = 0U;
|
||||
int checksum_byte = (msg->addr == MSG_MOTOR_5) ? 7 : 0;
|
||||
|
||||
// Simple XOR over the payload, except for the byte where the checksum lives.
|
||||
for (int i = 0; i < len; i++) {
|
||||
if (i != checksum_byte) {
|
||||
checksum ^= (uint8_t)msg->data[i];
|
||||
}
|
||||
}
|
||||
|
||||
return checksum;
|
||||
}
|
||||
|
||||
static safety_config volkswagen_pq_init(uint16_t param) {
|
||||
// Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
|
||||
static const CanMsg VOLKSWAGEN_PQ_STOCK_TX_MSGS[] = {{MSG_HCA_1, 0, 5, .check_relay = true}, {MSG_LDW_1, 0, 8, .check_relay = true},
|
||||
{MSG_GRA_NEU, 0, 4, .check_relay = false}, {MSG_GRA_NEU, 2, 4, .check_relay = false}};
|
||||
|
||||
static const CanMsg VOLKSWAGEN_PQ_LONG_TX_MSGS[] = {{MSG_HCA_1, 0, 5, .check_relay = true}, {MSG_LDW_1, 0, 8, .check_relay = true},
|
||||
{MSG_ACC_SYSTEM, 0, 8, .check_relay = true}, {MSG_ACC_GRA_ANZEIGE, 0, 8, .check_relay = true}};
|
||||
|
||||
static RxCheck volkswagen_pq_rx_checks[] = {
|
||||
{.msg = {{MSG_LENKHILFE_3, 0, 6, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_BREMSE_1, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_MOTOR_2, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_MOTOR_3, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_MOTOR_5, 0, 8, 50U, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_GRA_NEU, 0, 4, 30U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
volkswagen_common_init();
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL);
|
||||
#else
|
||||
SAFETY_UNUSED(param);
|
||||
#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 *msg) {
|
||||
if (msg->bus == 0U) {
|
||||
// Update in-motion state from speed value.
|
||||
// Signal: Bremse_1.BR1_Rad_kmh
|
||||
if (msg->addr == MSG_BREMSE_1) {
|
||||
int speed = ((msg->data[2] & 0xFEU) >> 1) | (msg->data[3] << 7);
|
||||
vehicle_moving = speed > 0;
|
||||
}
|
||||
|
||||
// Update driver input torque samples
|
||||
// Signal: Lenkhilfe_3.LH3_LM (absolute torque)
|
||||
// Signal: Lenkhilfe_3.LH3_LMSign (direction)
|
||||
if (msg->addr == MSG_LENKHILFE_3) {
|
||||
int torque_driver_new = msg->data[2] | ((msg->data[3] & 0x3U) << 8);
|
||||
int sign = (msg->data[3] & 0x4U) >> 2;
|
||||
if (sign == 1) {
|
||||
torque_driver_new *= -1;
|
||||
}
|
||||
update_sample(&torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
if (volkswagen_longitudinal) {
|
||||
if (msg->addr == MSG_MOTOR_5) {
|
||||
// ACC main switch on is a prerequisite to enter controls, exit controls immediately on main switch off
|
||||
// Signal: Motor_5.MO5_GRA_Hauptsch
|
||||
acc_main_on = GET_BIT(msg, 50U);
|
||||
if (!acc_main_on) {
|
||||
controls_allowed = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->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(msg, 16U);
|
||||
bool resume_button = GET_BIT(msg, 17U);
|
||||
if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) {
|
||||
controls_allowed = acc_main_on;
|
||||
}
|
||||
volkswagen_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(msg, 9U)) {
|
||||
controls_allowed = false;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if (msg->addr == MSG_MOTOR_2) {
|
||||
// Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages
|
||||
// Signal: Motor_2.MO2_Sta_GRA
|
||||
int acc_status = (msg->data[2] & 0xC0U) >> 6;
|
||||
bool cruise_engaged = (acc_status == 1) || (acc_status == 2);
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
}
|
||||
}
|
||||
|
||||
// Signal: Motor_3.MO3_Pedalwert
|
||||
if (msg->addr == MSG_MOTOR_3) {
|
||||
gas_pressed = (msg->data[2]);
|
||||
}
|
||||
|
||||
// Signal: Motor_2.MO2_BLS
|
||||
if (msg->addr == MSG_MOTOR_2) {
|
||||
brake_pressed = (msg->data[2] & 0x1U);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool volkswagen_pq_tx_hook(const CANPacket_t *msg) {
|
||||
// lateral limits
|
||||
const TorqueSteeringLimits VOLKSWAGEN_PQ_STEERING_LIMITS = {
|
||||
.max_torque = 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_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_multiplier = 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
|
||||
};
|
||||
|
||||
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 (msg->addr == MSG_HCA_1) {
|
||||
int desired_torque = msg->data[2] | ((msg->data[3] & 0x7FU) << 8);
|
||||
desired_torque = desired_torque / 32; // DBC scale from PQ network to centi-Nm
|
||||
int sign = (msg->data[3] & 0x80U) >> 7;
|
||||
if (sign == 1) {
|
||||
desired_torque *= -1;
|
||||
}
|
||||
|
||||
uint32_t hca_status = ((msg->data[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 (msg->addr == MSG_ACC_SYSTEM) {
|
||||
// Signal: ACC_System.ACS_Sollbeschl (acceleration in m/s2, scale 0.005, offset -7.22)
|
||||
int desired_accel = ((((msg->data[4] & 0x7U) << 8) | msg->data[3]) * 5U) - 7220U;
|
||||
|
||||
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 ((msg->addr == MSG_GRA_NEU) && !controls_allowed) {
|
||||
// Signal: GRA_Neu.GRA_Neu_Setzen
|
||||
// Signal: GRA_Neu.GRA_Neu_Recall
|
||||
if (GET_BIT(msg, 16U) || GET_BIT(msg, 17U)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
const safety_hooks volkswagen_pq_hooks = {
|
||||
.init = volkswagen_pq_init,
|
||||
.rx = volkswagen_pq_rx_hook,
|
||||
.tx = volkswagen_pq_tx_hook,
|
||||
.get_counter = volkswagen_pq_get_counter,
|
||||
.get_checksum = volkswagen_pq_get_checksum,
|
||||
.compute_checksum = volkswagen_pq_compute_checksum,
|
||||
};
|
||||
Reference in new issue
Block a user