mawei/panda
1
0
Fork 0
mirror of https://github.com/commaai/panda.git synced 2026-06-11 22:55:01 +08:00
Code Issues Packages Projects Releases Wiki Activity

Safety: common acceleration cmd checks (#1171)

Browse Source 
* do toyota, hyundai, gm

* comments

* honda draft

* Revert "honda draft"

This reverts commit a1f466a5c9.

* do tesla

* vw draft

* finish vw

* fix safety

* only accel

* only accel

* some clean up

* fix mqb tests

* rename
This commit is contained in:
Shane Smiskol
2022-11-29 16:46:32 -08:00
committed by GitHub
parent 80dac4cd94
commit 2baa0ffed5
10 changed files with 62 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
board/safety.h
View File

@@ -483,6 +483,16 @@ float interpolate(struct lookup_t xy, float x) {
return ret;
}
// Safety checks for longitudinal actuation
bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits, const bool longitudinal_allowed) {
bool violation = false;
if (!longitudinal_allowed) {
violation |= desired_accel != limits.inactive_accel;
} else {
violation |= max_limit_check(desired_accel, limits.max_accel, limits.min_accel);
}
return violation;
}
// Safety checks for torque-based steering commands
bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLimits limits) {

18
board/safety/safety_hyundai.h
View File

@@ -18,8 +18,10 @@ const SteeringLimits HYUNDAI_STEERING_LIMITS = {
.has_steer_req_tolerance = true,
};
const int HYUNDAI_MAX_ACCEL = 200; // 1/100 m/s2
const int HYUNDAI_MIN_ACCEL = -350; // 1/100 m/s2
const LongitudinalLimits HYUNDAI_LONG_LIMITS = {
.max_accel = 200, // 1/100 m/s2
.min_accel = -350, // 1/100 m/s2
};
const CanMsg HYUNDAI_TX_MSGS[] = {
{832, 0, 8}, // LKAS11 Bus 0
@@ -257,16 +259,10 @@ static int hyundai_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) {
int aeb_decel_cmd = GET_BYTE(to_send, 2);
int aeb_req = GET_BIT(to_send, 54U);
bool violation = 0;
if (!longitudinal_allowed) {
if ((desired_accel_raw != 0) || (desired_accel_val != 0)) {
violation = 1;
}
}
violation |= max_limit_check(desired_accel_raw, HYUNDAI_MAX_ACCEL, HYUNDAI_MIN_ACCEL);
violation |= max_limit_check(desired_accel_val, HYUNDAI_MAX_ACCEL, HYUNDAI_MIN_ACCEL);
bool violation = false;
violation |= longitudinal_accel_checks(desired_accel_raw, HYUNDAI_LONG_LIMITS, longitudinal_allowed);
violation |= longitudinal_accel_checks(desired_accel_val, HYUNDAI_LONG_LIMITS, longitudinal_allowed);
violation |= (aeb_decel_cmd != 0);
violation |= (aeb_req != 0);

9
board/safety/safety_hyundai_canfd.h
View File

@@ -278,13 +278,8 @@ static int hyundai_canfd_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed
bool violation = false;
if (hyundai_longitudinal) {
if (!longitudinal_allowed) {
if ((desired_accel_raw != 0) || (desired_accel_val != 0)) {
violation = true;
}
}
violation |= max_limit_check(desired_accel_raw, HYUNDAI_MAX_ACCEL, HYUNDAI_MIN_ACCEL);
violation |= max_limit_check(desired_accel_val, HYUNDAI_MAX_ACCEL, HYUNDAI_MIN_ACCEL);
violation |= longitudinal_accel_checks(desired_accel_raw, HYUNDAI_LONG_LIMITS, longitudinal_allowed);
violation |= longitudinal_accel_checks(desired_accel_val, HYUNDAI_LONG_LIMITS, longitudinal_allowed);
} else {
// only used to cancel on here
if ((desired_accel_raw != 0) || (desired_accel_val != 0)) {

30
board/safety/safety_tesla.h
View File

@@ -7,8 +7,13 @@ const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_DOWN = {
{5., 3.5, .8}};
const int TESLA_DEG_TO_CAN = 10;
const float TESLA_MAX_ACCEL = 2.0; // m/s^2
const float TESLA_MIN_ACCEL = -3.5; // m/s^2
const LongitudinalLimits TESLA_LONG_LIMITS = {
.max_accel = 425, // 2. m/s^2
.min_accel = 287, // -3.52 m/s^2 // TODO: limit to -3.48
.inactive_accel = 375, // 0. m/s^2
};
const int TESLA_FLAG_POWERTRAIN = 1;
const int TESLA_FLAG_LONGITUDINAL_CONTROL = 2;
@@ -26,8 +31,6 @@ const CanMsg TESLA_PT_TX_MSGS[] = {
};
#define TESLA_PT_TX_LEN (sizeof(TESLA_PT_TX_MSGS) / sizeof(TESLA_PT_TX_MSGS[0]))
const int TESLA_NO_ACCEL_VALUE = 375; // value sent when not requesting acceleration
AddrCheckStruct tesla_addr_checks[] = {
{.msg = {{0x370, 0, 8, .expected_timestep = 40000U}, { 0 }, { 0 }}}, // EPAS_sysStatus (25Hz)
{.msg = {{0x108, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // DI_torque1 (100Hz)
@@ -185,23 +188,8 @@ static int tesla_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) {
// Don't allow any acceleration limits above the safety limits
int raw_accel_max = ((GET_BYTE(to_send, 6) & 0x1FU) << 4) | (GET_BYTE(to_send, 5) >> 4);
int raw_accel_min = ((GET_BYTE(to_send, 5) & 0x0FU) << 5) | (GET_BYTE(to_send, 4) >> 3);
float accel_max = (0.04 * raw_accel_max) - 15;
float accel_min = (0.04 * raw_accel_min) - 15;
if ((accel_max > TESLA_MAX_ACCEL) || (accel_min > TESLA_MAX_ACCEL)){
violation = true;
}
if ((accel_max < TESLA_MIN_ACCEL) || (accel_min < TESLA_MIN_ACCEL)){
violation = true;
}
// Don't allow longitudinal actuation if controls aren't allowed
if (!longitudinal_allowed) {
if ((raw_accel_max != TESLA_NO_ACCEL_VALUE) || (raw_accel_min != TESLA_NO_ACCEL_VALUE)) {
violation = true;
}
}
violation |= longitudinal_accel_checks(raw_accel_max, TESLA_LONG_LIMITS, longitudinal_allowed);
violation |= longitudinal_accel_checks(raw_accel_min, TESLA_LONG_LIMITS, longitudinal_allowed);
} else {
violation = true;
}

19
board/safety/safety_toyota.h
View File

@@ -16,8 +16,10 @@ const SteeringLimits TOYOTA_STEERING_LIMITS = {
};
// longitudinal limits
const int TOYOTA_MAX_ACCEL = 2000; // 2.0 m/s2
const int TOYOTA_MIN_ACCEL = -3500; // -3.5 m/s2
const LongitudinalLimits TOYOTA_LONG_LIMITS = {
.max_accel = 2000, // 2.0 m/s2
.min_accel = -3500, // -3.5 m/s2
};
// panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches
// If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state
@@ -156,22 +158,19 @@ static int toyota_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) {
if (addr == 0x343) {
int desired_accel = (GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1);
desired_accel = to_signed(desired_accel, 16);
if (!longitudinal_allowed || toyota_stock_longitudinal) {
if (desired_accel != 0) {
tx = 0;
}
}
bool violation = false;
violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS, longitudinal_allowed);
violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS, !toyota_stock_longitudinal);
// only ACC messages that cancel are allowed when openpilot is not controlling longitudinal
if (toyota_stock_longitudinal) {
bool cancel_req = GET_BIT(to_send, 24U) != 0U;
if (!cancel_req) {
tx = 0;
violation = true;
}
}
bool violation = max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL);
if (violation) {
tx = 0;
}

17
board/safety/safety_volkswagen_mqb.h
View File

@@ -14,8 +14,11 @@ const SteeringLimits VOLKSWAGEN_MQB_STEERING_LIMITS = {
// longitudinal limits
// acceleration in m/s2 * 1000 to avoid floating point math
const int VOLKSWAGEN_MQB_MAX_ACCEL = 2000;
const int VOLKSWAGEN_MQB_MIN_ACCEL = -3500;
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
};
#define MSG_ESP_19 0x0B2 // RX from ABS, for wheel speeds
#define MSG_LH_EPS_03 0x09F // RX from EPS, for driver steering torque
@@ -243,15 +246,7 @@ static int volkswagen_mqb_tx_hook(CANPacket_t *to_send, bool longitudinal_allowe
desired_accel = (((GET_BYTE(to_send, 7) << 3) | ((GET_BYTE(to_send, 6) & 0xE0U) >> 5)) * 5U) - 7220U;
}
// VW send one increment above the max range when inactive
if (desired_accel == 3010) {
desired_accel = 0;
}
if (!controls_allowed && (desired_accel != 0)) {
violation = 1;
}
violation |= max_limit_check(desired_accel, VOLKSWAGEN_MQB_MAX_ACCEL, VOLKSWAGEN_MQB_MIN_ACCEL);
violation |= longitudinal_accel_checks(desired_accel, VOLKSWAGEN_MQB_LONG_LIMITS, longitudinal_allowed);
if (violation) {
tx = 0;

21
board/safety/safety_volkswagen_pq.h
View File

@@ -14,8 +14,11 @@ const SteeringLimits VOLKSWAGEN_PQ_STEERING_LIMITS = {
// longitudinal limits
// acceleration in m/s2 * 1000 to avoid floating point math
const int VOLKSWAGEN_PQ_MAX_ACCEL = 2000;
const int VOLKSWAGEN_PQ_MIN_ACCEL = -3500;
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
};
#define MSG_LENKHILFE_3 0x0D0 // RX from EPS, for steering angle and driver steering torque
#define MSG_HCA_1 0x0D2 // TX by OP, Heading Control Assist steering torque
@@ -203,20 +206,10 @@ static int volkswagen_pq_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed
// Safety check for acceleration commands
// To avoid floating point math, scale upward and compare to pre-scaled safety m/s2 boundaries
if (addr == MSG_ACC_SYSTEM) {
bool violation = 0;
int desired_accel = 0;
// Signal: ACC_System.ACS_Sollbeschl (acceleration in m/s2, scale 0.005, offset -7.22)
desired_accel = ((((GET_BYTE(to_send, 4) & 0x7U) << 8) | GET_BYTE(to_send, 3)) * 5U) - 7220U;
int desired_accel = ((((GET_BYTE(to_send, 4) & 0x7U) << 8) | GET_BYTE(to_send, 3)) * 5U) - 7220U;
// VW sends one increment above the max range when inactive
if (!longitudinal_allowed) {
violation |= desired_accel != 3010;
} else {
violation |= max_limit_check(desired_accel, VOLKSWAGEN_PQ_MAX_ACCEL, VOLKSWAGEN_PQ_MIN_ACCEL);
}
if (violation) {
if (longitudinal_accel_checks(desired_accel, VOLKSWAGEN_PQ_LONG_LIMITS, longitudinal_allowed)) {
tx = 0;
}
}

8
board/safety_declarations.h
View File

@@ -54,6 +54,13 @@ typedef struct {
const bool has_steer_req_tolerance;
} SteeringLimits;
typedef struct {
// acceleration cmd limits
const int max_accel;
const int min_accel;
const int inactive_accel;
} LongitudinalLimits;
typedef struct {
const int addr;
const int bus;
@@ -113,6 +120,7 @@ void generic_rx_checks(bool stock_ecu_detected);
void relay_malfunction_set(void);
void relay_malfunction_reset(void);
bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLimits limits);
bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits, const bool longitudinal_allowed);
void pcm_cruise_check(bool cruise_engaged);
typedef const addr_checks* (*safety_hook_init)(uint16_t param);

5
tests/safety/test_volkswagen_mqb.py
View File

@@ -161,6 +161,7 @@ class TestVolkswagenMqbLongSafety(TestVolkswagenMqbSafety):
TX_MSGS = [[MSG_HCA_01, 0], [MSG_LDW_02, 0], [MSG_ACC_02, 0], [MSG_ACC_06, 0], [MSG_ACC_07, 0]]
FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_01, MSG_LDW_02, MSG_ACC_02, MSG_ACC_06, MSG_ACC_07]}
FWD_BUS_LOOKUP = {0: 2, 2: 0}
INACTIVE_ACCEL = 3.01
def setUp(self):
self.packer = CANPackerPanda("vw_mqb_2010")
@@ -207,9 +208,9 @@ class TestVolkswagenMqbLongSafety(TestVolkswagenMqbSafety):
def test_accel_safety_check(self):
for controls_allowed in [True, False]:
for accel in np.arange(MIN_ACCEL - 1, MAX_ACCEL + 1, 0.01):
for accel in np.arange(MIN_ACCEL - 2, MAX_ACCEL + 2, 0.03):
accel = round(accel, 2) # floats might not hit exact boundary conditions without rounding
send = MIN_ACCEL <= accel <= MAX_ACCEL if controls_allowed else accel == 0
send = MIN_ACCEL <= accel <= MAX_ACCEL if controls_allowed else accel == self.INACTIVE_ACCEL
self.safety.set_controls_allowed(controls_allowed)
# primary accel request used by ECU
self.assertEqual(send, self._tx(self._acc_06_msg(accel)), (controls_allowed, accel))

2
tests/safety/test_volkswagen_pq.py
View File

@@ -194,7 +194,7 @@ class TestVolkswagenPqLongSafety(TestVolkswagenPqSafety):
def test_accel_safety_check(self):
for controls_allowed in [True, False]:
for accel in np.arange(MIN_ACCEL - 2, MAX_ACCEL + 2, 0.01):
for accel in np.arange(MIN_ACCEL - 2, MAX_ACCEL + 2, 0.005):
accel = round(accel, 2) # floats might not hit exact boundary conditions without rounding
send = MIN_ACCEL <= accel <= MAX_ACCEL if controls_allowed else accel == self.INACTIVE_ACCEL
self.safety.set_controls_allowed(controls_allowed)