From 983a8b6458bd8d69ee0564250b7ddcb4f3e28457 Mon Sep 17 00:00:00 2001 From: Eric Brown Date: Mon, 20 May 2024 20:55:27 -0600 Subject: [PATCH] Pedal safety logic --- board/safety/safety_gm.h | 39 +++++++++++++-- python/__init__.py | 2 + tests/libpanda/safety_helpers.py | 2 +- tests/safety/test_gm.py | 85 ++++++++++++++++++++++++++++++++ 4 files changed, 124 insertions(+), 4 deletions(-) diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index 09ac34ecbd..cb5973279f 100644 --- a/board/safety/safety_gm.h +++ b/board/safety/safety_gm.h @@ -27,6 +27,11 @@ const LongitudinalLimits *gm_long_limits; const int GM_STANDSTILL_THRSLD = 10; // 0.311kph +// panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches +// If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state +const int GM_GAS_INTERCEPTOR_THRESHOLD = 515; // (610 + 306.25) / 2 ratio between offset and gain from dbc file +#define GM_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks + const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {0x2CB, 0, 8}, {0x370, 0, 6}, // pt bus {0xA1, 1, 7}, {0x306, 1, 8}, {0x308, 1, 7}, {0x310, 1, 2}, // obs bus {0x315, 2, 5}}; // ch bus @@ -34,7 +39,7 @@ const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, { const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, // pt bus {0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus -const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8}, {0x370, 0, 6}, // pt bus +const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8}, {0x370, 0, 6}, {0x200, 0, 6}, // pt bus {0x184, 2, 8}}; // camera bus // TODO: do checksum and counter checks. Add correct timestep, 0.1s for now. @@ -51,6 +56,8 @@ RxCheck gm_rx_checks[] = { const uint16_t GM_PARAM_HW_CAM = 1; const uint16_t GM_PARAM_HW_CAM_LONG = 2; +const uint16_t GM_PARAM_NO_ACC = 4; +const uint16_t GM_PARAM_PEDAL_INTERCEPTOR = 8; enum { GM_BTN_UNPRESS = 1, @@ -66,6 +73,7 @@ typedef enum { GmHardware gm_hw = GM_ASCM; bool gm_cam_long = false; bool gm_pcm_cruise = false; +bool gm_has_acc = true; static void gm_rx_hook(const CANPacket_t *to_push) { if (GET_BUS(to_push) == 0U) { @@ -115,19 +123,35 @@ static void gm_rx_hook(const CANPacket_t *to_push) { } if (addr == 0x1C4) { - gas_pressed = GET_BYTE(to_push, 5) != 0U; + if (!enable_gas_interceptor) { + gas_pressed = GET_BYTE(to_push, 5) != 0U; + } // enter controls on rising edge of ACC, exit controls when ACC off - if (gm_pcm_cruise) { + if (gm_pcm_cruise && gm_has_acc) { bool cruise_engaged = (GET_BYTE(to_push, 1) >> 5) != 0U; pcm_cruise_check(cruise_engaged); } } + // Cruise check for CC only cars + if ((addr == 0x3D1) && !gm_has_acc) { + bool cruise_engaged = (GET_BYTE(to_push, 4) >> 7) != 0U; + cruise_engaged_prev = cruise_engaged; + } + if (addr == 0xBD) { regen_braking = (GET_BYTE(to_push, 0) >> 4) != 0U; } + // Pedal Interceptor + if ((addr == 0x201) && enable_gas_interceptor) { + int gas_interceptor = GM_GET_INTERCEPTOR(to_push); + gas_pressed = gas_interceptor > GM_GAS_INTERCEPTOR_THRESHOLD; + gas_interceptor_prev = gas_interceptor; +// gm_pcm_cruise = false; + } + bool stock_ecu_detected = (addr == 0x180); // ASCMLKASteeringCmd // Check ASCMGasRegenCmd only if we're blocking it @@ -163,6 +187,13 @@ static bool gm_tx_hook(const CANPacket_t *to_send) { } } + // GAS: safety check (interceptor) + if (addr == 0x200) { + if (longitudinal_interceptor_checks(to_send)) { + tx = 0; + } + } + // GAS/REGEN: safety check if (addr == 0x2CB) { bool apply = GET_BIT(to_send, 0U); @@ -229,6 +260,8 @@ static safety_config gm_init(uint16_t param) { #ifdef ALLOW_DEBUG gm_cam_long = GET_FLAG(param, GM_PARAM_HW_CAM_LONG); + gm_has_acc = !GET_FLAG(param, GM_PARAM_NO_ACC); + enable_gas_interceptor = GET_FLAG(param, GM_PARAM_PEDAL_INTERCEPTOR); #endif gm_pcm_cruise = (gm_hw == GM_CAM) && !gm_cam_long; diff --git a/python/__init__.py b/python/__init__.py index 35a91b7713..8dfeee131d 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -233,6 +233,8 @@ class Panda: FLAG_GM_HW_CAM = 1 FLAG_GM_HW_CAM_LONG = 2 + FLAG_GM_NO_ACC = 4 + FLAG_GM_GAS_INTERCEPTOR = 8 FLAG_FORD_LONG_CONTROL = 1 FLAG_FORD_CANFD = 2 diff --git a/tests/libpanda/safety_helpers.py b/tests/libpanda/safety_helpers.py index 28f3349dc6..80ee18688c 100644 --- a/tests/libpanda/safety_helpers.py +++ b/tests/libpanda/safety_helpers.py @@ -61,7 +61,7 @@ def set_alternative_experience(self, mode: int) -> None: ... def get_alternative_experience(self) -> int: ... def set_relay_malfunction(self, c: bool) -> None: ... def get_relay_malfunction(self) -> bool: ... - def get_gas_interceptor_prev(self) -> int: ... + def get_gas_interceptor_prev(self) -> bool: ... def get_gas_pressed_prev(self) -> bool: ... def get_brake_pressed_prev(self) -> bool: ... def get_regen_braking_prev(self) -> bool: ... diff --git a/tests/safety/test_gm.py b/tests/safety/test_gm.py index c6c5ac6b3a..b6b2fe7797 100755 --- a/tests/safety/test_gm.py +++ b/tests/safety/test_gm.py @@ -223,5 +223,90 @@ def setUp(self): self.safety.init_tests() +##### OPGM TESTS ##### + +def interceptor_msg(gas, addr): + to_send = common.make_msg(0, addr, 6) + to_send[0].data[0] = (gas & 0xFF00) >> 8 + to_send[0].data[1] = gas & 0xFF + to_send[0].data[2] = (gas & 0xFF00) >> 8 + to_send[0].data[3] = gas & 0xFF + return to_send + + +class TestGmInterceptorSafety(common.GasInterceptorSafetyTest, TestGmCameraSafety): + INTERCEPTOR_THRESHOLD = 515 + + def setUp(self): + self.packer = CANPackerPanda("gm_global_a_powertrain_generated") + self.packer_chassis = CANPackerPanda("gm_global_a_chassis") + self.safety = libpanda_py.libpanda + self.safety.set_safety_hooks( + Panda.SAFETY_GM, + Panda.FLAG_GM_HW_CAM | Panda.FLAG_GM_NO_ACC | Panda.FLAG_GM_PEDAL_LONG | Panda.FLAG_GM_GAS_INTERCEPTOR) + self.safety.init_tests() + + def test_pcm_sets_cruise_engaged(self): + for enabled in [True, False]: + self._rx(self._pcm_status_msg(enabled)) + self.assertEqual(enabled, self.safety.get_cruise_engaged_prev()) + + def test_no_pcm_enable(self): + self._rx(self._interceptor_user_gas(0)) + self.safety.set_controls_allowed(0) + self.assertFalse(self.safety.get_controls_allowed()) + self._rx(self._pcm_status_msg(True)) + self.assertFalse(self.safety.get_controls_allowed()) + self.assertTrue(self.safety.get_cruise_engaged_prev()) + + def test_no_response_to_acc_pcm_message(self): + self._rx(self._interceptor_user_gas(0)) + def _acc_pcm_msg(enable): + values = {"CruiseState": enable} + return self.packer.make_can_msg_panda("AcceleratorPedal2", 0, values) + for enable in [True, False]: + self.safety.set_controls_allowed(enable) + self._rx(_acc_pcm_msg(True)) + self.assertEqual(enable, self.safety.get_controls_allowed()) + self._rx(_acc_pcm_msg(False)) + self.assertEqual(enable, self.safety.get_controls_allowed()) + + def test_buttons(self): + self._rx(self._interceptor_user_gas(0)) + # Only CANCEL button is allowed while cruise is enabled + self.safety.set_controls_allowed(0) + for btn in range(8): + self.assertFalse(self._tx(self._button_msg(btn))) + + self.safety.set_controls_allowed(1) + for btn in range(8): + self.assertFalse(self._tx(self._button_msg(btn))) + + self.safety.set_controls_allowed(1) + for enabled in (True, False): + self._rx(self._pcm_status_msg(enabled)) + self.assertEqual(enabled, self._tx(self._button_msg(Buttons.CANCEL))) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_fwd_hook(self): + pass + + def test_disable_control_allowed_from_cruise(self): + pass + + def test_enable_control_allowed_from_cruise(self): + pass + + def _interceptor_gas_cmd(self, gas): + return interceptor_msg(gas, 0x200) + + def _interceptor_user_gas(self, gas): + return interceptor_msg(gas, 0x201) + + def _pcm_status_msg(self, enable): + values = {"CruiseActive": enable} + return self.packer.make_can_msg_panda("ECMCruiseControl", 0, values) + + if __name__ == "__main__": unittest.main()