From 2baa0ffed57b51d030d48dcab1ce5852ccd0ef27 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 29 Nov 2022 16:46:32 -0800 Subject: [PATCH] Safety: common acceleration cmd checks (#1171) * do toyota, hyundai, gm * comments * honda draft * Revert "honda draft" This reverts commit a1f466a5c9a974d199c07dfa4cc1e6edaab17971. * do tesla * vw draft * finish vw * fix safety * only accel * only accel * some clean up * fix mqb tests * rename --- board/safety.h | 10 ++++++++++ board/safety/safety_hyundai.h | 18 +++++++---------- board/safety/safety_hyundai_canfd.h | 9 ++------- board/safety/safety_tesla.h | 30 +++++++++------------------- board/safety/safety_toyota.h | 19 +++++++++--------- board/safety/safety_volkswagen_mqb.h | 17 ++++++---------- board/safety/safety_volkswagen_pq.h | 21 +++++++------------ board/safety_declarations.h | 8 ++++++++ tests/safety/test_volkswagen_mqb.py | 5 +++-- tests/safety/test_volkswagen_pq.py | 2 +- 10 files changed, 62 insertions(+), 77 deletions(-) diff --git a/board/safety.h b/board/safety.h index 01fea7a5b..63213f4e8 100644 --- a/board/safety.h +++ b/board/safety.h @@ -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) { diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index 0620e9197..ad8db0bbb 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -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); diff --git a/board/safety/safety_hyundai_canfd.h b/board/safety/safety_hyundai_canfd.h index 5086a39bc..abfe76496 100644 --- a/board/safety/safety_hyundai_canfd.h +++ b/board/safety/safety_hyundai_canfd.h @@ -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)) { diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h index 312074dec..1f9d3971f 100644 --- a/board/safety/safety_tesla.h +++ b/board/safety/safety_tesla.h @@ -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; } diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 510bc10e3..81ab8c149 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -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; } diff --git a/board/safety/safety_volkswagen_mqb.h b/board/safety/safety_volkswagen_mqb.h index 0371d41c4..539de695e 100644 --- a/board/safety/safety_volkswagen_mqb.h +++ b/board/safety/safety_volkswagen_mqb.h @@ -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; diff --git a/board/safety/safety_volkswagen_pq.h b/board/safety/safety_volkswagen_pq.h index 51dcd28f3..8deb50efa 100644 --- a/board/safety/safety_volkswagen_pq.h +++ b/board/safety/safety_volkswagen_pq.h @@ -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; } } diff --git a/board/safety_declarations.h b/board/safety_declarations.h index d50d4d1fe..947e91844 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -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); diff --git a/tests/safety/test_volkswagen_mqb.py b/tests/safety/test_volkswagen_mqb.py index 5ac1cb9fd..90d0c5088 100755 --- a/tests/safety/test_volkswagen_mqb.py +++ b/tests/safety/test_volkswagen_mqb.py @@ -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)) diff --git a/tests/safety/test_volkswagen_pq.py b/tests/safety/test_volkswagen_pq.py index 676e4ed8d..dc45e8473 100755 --- a/tests/safety/test_volkswagen_pq.py +++ b/tests/safety/test_volkswagen_pq.py @@ -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)