From 1e37dcac9a14e8af24e4f90d3e2e003cbf183509 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 11 Jun 2024 07:33:27 -0700 Subject: [PATCH] OPGM - Pedal interceptor longitudinal control Co-Authored-By: Eric Brown <13560103+nworb-cire@users.noreply.github.com> --- cereal/car.capnp | 5 +- panda/board/safety.h | 6 ++ panda/board/safety/safety_gm.h | 61 ++++++++++++++---- panda/board/safety_declarations.h | 3 + panda/python/__init__.py | 3 +- panda/tests/libpanda/safety_helpers.h | 4 ++ panda/tests/libpanda/safety_helpers.py | 2 + panda/tests/safety/common.py | 87 +++++++++++++++++++++++++- panda/tests/safety/test_gm.py | 85 +++++++++++++++++++++++++ selfdrive/car/__init__.py | 35 +++++++++++ selfdrive/car/gm/carcontroller.py | 34 +++++++++- selfdrive/car/gm/carstate.py | 23 ++++++- selfdrive/car/gm/interface.py | 39 ++++++++++-- selfdrive/car/gm/values.py | 1 + selfdrive/controls/lib/events.py | 8 +++ 15 files changed, 371 insertions(+), 25 deletions(-) mode change 100755 => 100644 selfdrive/car/gm/interface.py diff --git a/cereal/car.capnp b/cereal/car.capnp index c17e1c063..a7d299a8e 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -117,6 +117,9 @@ struct CarEvent @0x9b1657f34caf3ad3 { paramsdPermanentError @119; actuatorsApiUnavailable @120; + # FrogPilot Events + pedalInterceptorNoBrake @134; + radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; radarCommIssueDEPRECATED @67; @@ -682,7 +685,7 @@ struct CarParams { gateway @1; # Integration at vehicle's CAN gateway } - enableGasInterceptorDEPRECATED @2 :Bool; + enableGasInterceptor @2 :Bool; enableCameraDEPRECATED @4 :Bool; enableApgsDEPRECATED @6 :Bool; steerRateCostDEPRECATED @33 :Float32; diff --git a/panda/board/safety.h b/panda/board/safety.h index 46cf4b49f..1516fac84 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -327,6 +327,8 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { // reset state set by safety mode safety_mode_cnt = 0U; relay_malfunction = false; + enable_gas_interceptor = false; + gas_interceptor_prev = 0; gas_pressed = false; gas_pressed_prev = false; brake_pressed = false; @@ -541,6 +543,10 @@ bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limit return violation; } +bool longitudinal_interceptor_checks(const CANPacket_t *to_send) { + return (!get_longitudinal_allowed() || brake_pressed_prev) && (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1)); +} + // Safety checks for torque-based steering commands bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLimits limits) { bool violation = false; diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 73e357b36..35735a96d 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -27,14 +27,19 @@ const LongitudinalLimits *gm_long_limits; const int GM_STANDSTILL_THRSLD = 10; // 0.311kph -const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {0x2CB, 0, 8}, {0x370, 0, 6}, // pt bus +// panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches +// If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state +const int GM_GAS_INTERCEPTOR_THRESHOLD = 515; // (610 + 306.25) / 2 ratio between offset and gain from dbc file +#define GM_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks + +const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {0x2CB, 0, 8}, {0x370, 0, 6}, {0x200, 0, 6}, // pt bus {0xA1, 1, 7}, {0x306, 1, 8}, {0x308, 1, 7}, {0x310, 1, 2}, // obs bus {0x315, 2, 5}}; // ch bus -const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, // pt bus +const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, {0x200, 0, 6}, // 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,7 +56,11 @@ 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_HW_ASCM_LONG = 8; const uint16_t GM_PARAM_NO_CAMERA = 16; +const uint16_t GM_PARAM_NO_ACC = 32; +const uint16_t GM_PARAM_PEDAL_LONG = 128; // TODO: this can be inferred +const uint16_t GM_PARAM_PEDAL_INTERCEPTOR = 256; enum { GM_BTN_UNPRESS = 1, @@ -67,7 +76,10 @@ typedef enum { GmHardware gm_hw = GM_ASCM; bool gm_cam_long = false; bool gm_pcm_cruise = false; +bool gm_has_acc = true; +bool gm_pedal_long = false; bool gm_skip_relay_check = false; +bool gm_force_ascm = false; static void gm_rx_hook(const CANPacket_t *to_push) { if (GET_BUS(to_push) == 0U) { @@ -117,23 +129,39 @@ 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 - if (!gm_pcm_cruise && (addr == 0x2CB)) { + if (!gm_pcm_cruise && !gm_pedal_long && (addr == 0x2CB)) { stock_ecu_detected = true; } generic_rx_checks(stock_ecu_detected); @@ -165,6 +193,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); @@ -181,7 +216,7 @@ static bool gm_tx_hook(const CANPacket_t *to_send) { } // BUTTONS: used for resume spamming and cruise cancellation with stock longitudinal - if ((addr == 0x1E1) && gm_pcm_cruise) { + if ((addr == 0x1E1) && (gm_pcm_cruise || gm_pedal_long)) { int button = (GET_BYTE(to_send, 5) >> 4) & 0x7U; bool allowed_cancel = (button == 6) && cruise_engaged_prev; @@ -222,18 +257,20 @@ static int gm_fwd_hook(int bus_num, int addr) { static safety_config gm_init(uint16_t param) { gm_hw = GET_FLAG(param, GM_PARAM_HW_CAM) ? GM_CAM : GM_ASCM; - if (gm_hw == GM_ASCM) { + gm_force_ascm = GET_FLAG(param, GM_PARAM_HW_ASCM_LONG); + + if (gm_hw == GM_ASCM || gm_force_ascm) { gm_long_limits = &GM_ASCM_LONG_LIMITS; } else if (gm_hw == GM_CAM) { gm_long_limits = &GM_CAM_LONG_LIMITS; } else { } -#ifdef ALLOW_DEBUG - gm_cam_long = GET_FLAG(param, GM_PARAM_HW_CAM_LONG); -#endif - gm_pcm_cruise = (gm_hw == GM_CAM) && !gm_cam_long; + gm_pedal_long = GET_FLAG(param, GM_PARAM_PEDAL_LONG); + gm_pcm_cruise = ((gm_hw == GM_CAM) && !gm_cam_long && !gm_force_ascm && !gm_pedal_long); gm_skip_relay_check = GET_FLAG(param, GM_PARAM_NO_CAMERA); + gm_has_acc = !GET_FLAG(param, GM_PARAM_NO_ACC); + enable_gas_interceptor = GET_FLAG(param, GM_PARAM_PEDAL_INTERCEPTOR); safety_config ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_ASCM_TX_MSGS); if (gm_hw == GM_CAM) { diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h index e3cdc47cd..6df5b18f9 100644 --- a/panda/board/safety_declarations.h +++ b/panda/board/safety_declarations.h @@ -203,6 +203,7 @@ bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limit bool longitudinal_gas_checks(int desired_gas, const LongitudinalLimits limits); bool longitudinal_transmission_rpm_checks(int desired_transmission_rpm, const LongitudinalLimits limits); bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limits); +bool longitudinal_interceptor_checks(const CANPacket_t *to_send); void pcm_cruise_check(bool cruise_engaged); void safety_tick(const safety_config *safety_config); @@ -210,6 +211,8 @@ void safety_tick(const safety_config *safety_config); // This can be set by the safety hooks bool controls_allowed = false; bool relay_malfunction = false; +bool enable_gas_interceptor = false; +int gas_interceptor_prev = 0; bool gas_pressed = false; bool gas_pressed_prev = false; bool brake_pressed = false; diff --git a/panda/python/__init__.py b/panda/python/__init__.py index 7ab83f399..ae28419a7 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -224,7 +224,8 @@ class Panda: FLAG_GM_HW_CAM = 1 FLAG_GM_HW_CAM_LONG = 2 - FLAG_GM_NO_CAMERA = 16 + FLAG_GM_PEDAL_LONG = 128 # TODO: This can be inferred + FLAG_GM_GAS_INTERCEPTOR = 256 FLAG_FORD_LONG_CONTROL = 1 FLAG_FORD_CANFD = 2 diff --git a/panda/tests/libpanda/safety_helpers.h b/panda/tests/libpanda/safety_helpers.h index 36887c896..074463d31 100644 --- a/panda/tests/libpanda/safety_helpers.h +++ b/panda/tests/libpanda/safety_helpers.h @@ -43,6 +43,10 @@ bool get_relay_malfunction(void){ return relay_malfunction; } +int get_gas_interceptor_prev(void){ + return gas_interceptor_prev; +} + bool get_gas_pressed_prev(void){ return gas_pressed_prev; } diff --git a/panda/tests/libpanda/safety_helpers.py b/panda/tests/libpanda/safety_helpers.py index ea41264ae..80ee18688 100644 --- a/panda/tests/libpanda/safety_helpers.py +++ b/panda/tests/libpanda/safety_helpers.py @@ -10,6 +10,7 @@ def setup_safety_helpers(ffi): int get_alternative_experience(void); void set_relay_malfunction(bool c); bool get_relay_malfunction(void); + int get_gas_interceptor_prev(void); bool get_gas_pressed_prev(void); bool get_brake_pressed_prev(void); bool get_regen_braking_prev(void); @@ -60,6 +61,7 @@ class PandaSafety(Protocol): 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) -> 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/panda/tests/safety/common.py b/panda/tests/safety/common.py index e111ff7ef..ccd92568e 100644 --- a/panda/tests/safety/common.py +++ b/panda/tests/safety/common.py @@ -118,6 +118,91 @@ class PandaSafetyTestBase(unittest.TestCase): self.assertEqual(meas_max_func(), 0) +class GasInterceptorSafetyTest(PandaSafetyTestBase): + + INTERCEPTOR_THRESHOLD = 0 + + cnt_gas_cmd = 0 + cnt_user_gas = 0 + + packer: CANPackerPanda + + @classmethod + def setUpClass(cls): + if cls.__name__ == "GasInterceptorSafetyTest" or cls.__name__.endswith("Base"): + cls.safety = None + raise unittest.SkipTest + + def _interceptor_gas_cmd(self, gas: int): + values: dict[str, float | int] = {"COUNTER_PEDAL": self.__class__.cnt_gas_cmd & 0xF} + if gas > 0: + values["GAS_COMMAND"] = gas * 255. + values["GAS_COMMAND2"] = gas * 255. + self.__class__.cnt_gas_cmd += 1 + return self.packer.make_can_msg_panda("GAS_COMMAND", 0, values) + + def _interceptor_user_gas(self, gas: int): + values = {"INTERCEPTOR_GAS": gas, "INTERCEPTOR_GAS2": gas, + "COUNTER_PEDAL": self.__class__.cnt_user_gas} + self.__class__.cnt_user_gas += 1 + return self.packer.make_can_msg_panda("GAS_SENSOR", 0, values) + + # Skip non-interceptor user gas tests + def test_prev_gas(self): + pass + + def test_disengage_on_gas(self): + pass + + def test_alternative_experience_no_disengage_on_gas(self): + pass + + def test_prev_gas_interceptor(self): + self._rx(self._interceptor_user_gas(0x0)) + self.assertFalse(self.safety.get_gas_interceptor_prev()) + self._rx(self._interceptor_user_gas(0x1000)) + self.assertTrue(self.safety.get_gas_interceptor_prev()) + self._rx(self._interceptor_user_gas(0x0)) + + def test_disengage_on_gas_interceptor(self): + for g in range(0x1000): + self._rx(self._interceptor_user_gas(0)) + self.safety.set_controls_allowed(True) + self._rx(self._interceptor_user_gas(g)) + remain_enabled = g <= self.INTERCEPTOR_THRESHOLD + self.assertEqual(remain_enabled, self.safety.get_controls_allowed()) + self._rx(self._interceptor_user_gas(0)) + + def test_alternative_experience_no_disengage_on_gas_interceptor(self): + self.safety.set_controls_allowed(True) + self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS) + for g in range(0x1000): + self._rx(self._interceptor_user_gas(g)) + # Test we allow lateral, but not longitudinal + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(g <= self.INTERCEPTOR_THRESHOLD, self.safety.get_longitudinal_allowed()) + # Make sure we can re-gain longitudinal actuation + self._rx(self._interceptor_user_gas(0)) + self.assertTrue(self.safety.get_longitudinal_allowed()) + + def test_allow_engage_with_gas_interceptor_pressed(self): + self._rx(self._interceptor_user_gas(0x1000)) + self.safety.set_controls_allowed(1) + self._rx(self._interceptor_user_gas(0x1000)) + self.assertTrue(self.safety.get_controls_allowed()) + self._rx(self._interceptor_user_gas(0)) + + def test_gas_interceptor_safety_check(self): + for gas in np.arange(0, 4000, 100): + for controls_allowed in [True, False]: + self.safety.set_controls_allowed(controls_allowed) + if controls_allowed: + send = True + else: + send = gas == 0 + self.assertEqual(send, self._tx(self._interceptor_gas_cmd(gas))) + + class LongitudinalAccelSafetyTest(PandaSafetyTestBase, abc.ABC): MAX_ACCEL: float = 2.0 @@ -763,7 +848,7 @@ class PandaSafetyTest(PandaSafetyTestBase): continue if {attr, current_test}.issubset({'TestVolkswagenPqSafety', 'TestVolkswagenPqStockSafety', 'TestVolkswagenPqLongSafety'}): continue - if {attr, current_test}.issubset({'TestGmCameraSafety', 'TestGmCameraLongitudinalSafety'}): + if {attr, current_test}.issubset({'TestGmCameraSafety', 'TestGmCameraLongitudinalSafety', 'TestGmInterceptorSafety'}): continue if attr.startswith('TestFord') and current_test.startswith('TestFord'): continue diff --git a/panda/tests/safety/test_gm.py b/panda/tests/safety/test_gm.py index c6c5ac6b3..b6b2fe779 100644 --- a/panda/tests/safety/test_gm.py +++ b/panda/tests/safety/test_gm.py @@ -223,5 +223,90 @@ class TestGmCameraLongitudinalSafety(GmLongitudinalBase, TestGmCameraSafetyBase) 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() diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 4a1df550d..cb978f6c8 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -165,6 +165,41 @@ def common_fault_avoidance(fault_condition: bool, request: bool, above_limit_fra return above_limit_frames, request +def crc8_pedal(data): + crc = 0xFF # standard init value + poly = 0xD5 # standard crc8: x8+x7+x6+x4+x2+1 + size = len(data) + for i in range(size - 1, -1, -1): + crc ^= data[i] + for _ in range(8): + if ((crc & 0x80) != 0): + crc = ((crc << 1) ^ poly) & 0xFF + else: + crc <<= 1 + return crc + + +def create_gas_interceptor_command(packer, gas_amount, idx): + # Common gas pedal msg generator + enable = gas_amount > 0.001 + + values = { + "ENABLE": enable, + "COUNTER_PEDAL": idx & 0xF, + } + + if enable: + values["GAS_COMMAND"] = gas_amount * 255. + values["GAS_COMMAND2"] = gas_amount * 255. + + dat = packer.make_can_msg("GAS_COMMAND", 0, values)[2] + + checksum = crc8_pedal(dat[:-1]) + values["CHECKSUM_PEDAL"] = checksum + + return packer.make_can_msg("GAS_COMMAND", 0, values) + + def make_can_msg(addr, dat, bus): return [addr, 0, dat, bus] diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 4130524eb..0861a5012 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -1,9 +1,9 @@ from cereal import car from openpilot.common.conversions import Conversions as CV -from openpilot.common.numpy_fast import interp +from openpilot.common.numpy_fast import interp, clip from openpilot.common.realtime import DT_CTRL from opendbc.can.packer import CANPacker -from openpilot.selfdrive.car import apply_driver_steer_torque_limits +from openpilot.selfdrive.car import apply_driver_steer_torque_limits, create_gas_interceptor_command from openpilot.selfdrive.car.gm import gmcan from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR from openpilot.selfdrive.car.interfaces import CarControllerBase @@ -39,6 +39,21 @@ class CarController(CarControllerBase): self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar']) self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis']) + @staticmethod + def calc_pedal_command(accel: float, long_active: bool) -> float: + if not long_active: return 0. + + zero = 0.15625 # 40/256 + if accel > 0.: + # Scales the accel from 0-1 to 0.156-1 + pedal_gas = clip(((1 - zero) * accel + zero), 0., 1.) + else: + # if accel is negative, -0.1 -> 0.015625 + pedal_gas = clip(zero + accel, 0., zero) # Make brake the same size as gas, but clip to regen + + return pedal_gas + + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl @@ -87,6 +102,7 @@ class CarController(CarControllerBase): # Gas/regen, brakes, and UI commands - all at 25Hz if self.frame % 4 == 0: stopping = actuators.longControlState == LongCtrlState.stopping + interceptor_gas_cmd = 0 if not CC.longActive: # ASCM sends max regen when not enabled self.apply_gas = self.params.INACTIVE_REGEN @@ -98,11 +114,16 @@ class CarController(CarControllerBase): # FIXME: brakes aren't applied immediately when enabling at a stop if stopping: self.apply_gas = self.params.INACTIVE_REGEN + if self.CP.carFingerprint in CC_ONLY_CAR: + # gas interceptor only used for full long control on cars without ACC + interceptor_gas_cmd = self.calc_pedal_command(actuators.accel, CC.longActive) idx = (self.frame // 4) % 4 at_full_stop = CC.longActive and CS.out.standstill near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE) + if self.CP.enableGasInterceptor: + can_sends.append(create_gas_interceptor_command(self.packer_pt, interceptor_gas_cmd, idx)) if self.CP.carFingerprint not in CC_ONLY_CAR: friction_brake_bus = CanBus.CHASSIS # GM Camera exceptions @@ -140,6 +161,15 @@ class CarController(CarControllerBase): if self.CP.networkLocation == NetworkLocation.gateway and self.frame % self.params.ADAS_KEEPALIVE_STEP == 0: can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) + # TODO: integrate this with the code block below? + if ( + (self.CP.flags & GMFlags.PEDAL_LONG.value) # Always cancel stock CC when using pedal interceptor + or (self.CP.flags & GMFlags.CC_LONG.value and not CC.enabled) # Cancel stock CC if OP is not active + ) and CS.out.cruiseState.enabled: + if (self.frame - self.last_button_frame) * DT_CTRL > 0.04: + self.last_button_frame = self.frame + can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, (CS.buttons_counter + 1) % 4, CruiseButtons.CANCEL)) + else: # While car is braking, cancel button causes ECM to enter a soft disable state with a fault status. # A delayed cancellation allows camera to cancel and avoids a fault when user depresses brake quickly diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 11f2ed612..cf639f472 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -9,6 +9,7 @@ from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRES TransmissionType = car.CarParams.TransmissionType NetworkLocation = car.CarParams.NetworkLocation +GearShifter = car.CarState.GearShifter STANDSTILL_THRESHOLD = 10 * 0.0311 * CV.KPH_TO_MS @@ -29,6 +30,9 @@ class CarState(CarStateBase): self.prev_distance_button = 0 self.distance_button = 0 + self.single_pedal_mode = False + self.pedal_steady = 0. + def update(self, pt_cp, cam_cp, loopback_cp): ret = car.CarState.new_message() @@ -82,9 +86,14 @@ class CarState(CarStateBase): # Regen braking is braking if self.CP.transmissionType == TransmissionType.direct: ret.regenBraking = pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0 + self.single_pedal_mode = ret.gearShifter == GearShifter.low or pt_cp.vl["EVDriveMode"]["SinglePedalModeActive"] == 1 - ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. - ret.gasPressed = ret.gas > 1e-5 + if self.CP.enableGasInterceptor: + ret.gas = (pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2. + ret.gasPressed = ret.gas > 3 + else: + ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. + ret.gasPressed = ret.gas > 1e-5 ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"] ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelRate"] @@ -181,13 +190,21 @@ class CarState(CarStateBase): messages.append(("EBCMBrakePedalPosition", 100)) if CP.transmissionType == TransmissionType.direct: - messages.append(("EBCMRegenPaddle", 50)) + messages += [ + ("EBCMRegenPaddle", 50), + ("EVDriveMode", 0), + ] if CP.carFingerprint in CC_ONLY_CAR: messages += [ ("ECMCruiseControl", 10), ] + if CP.enableGasInterceptor: + messages += [ + ("GAS_SENSOR", 50), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.POWERTRAIN) @staticmethod diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py old mode 100755 new mode 100644 index f51fd14d8..9c63e8b9b --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -20,6 +20,7 @@ NetworkLocation = car.CarParams.NetworkLocation BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} +PEDAL_MSG = 0x201 CAM_MSG = 0x320 # AEBCmd # TODO: Is this always linked to camera presence? ACCELERATOR_POS_MSG = 0xbe @@ -97,6 +98,9 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] ret.autoResumeSng = False ret.enableBsm = 0x142 in fingerprint[CanBus.POWERTRAIN] + if PEDAL_MSG in fingerprint[0]: + ret.enableGasInterceptor = True + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_GAS_INTERCEPTOR if candidate in EV_CAR: ret.transmissionType = TransmissionType.direct @@ -137,11 +141,9 @@ class CarInterface(CarInterfaceBase): # Tuning ret.longitudinalTuning.kiV = [2.4, 1.5] - # These cars have been put into dashcam only due to both a lack of users and test coverage. - # These cars likely still work fine. Once a user confirms each car works and a test route is - # added to selfdrive/car/tests/routes.py, we can remove it from this list. - ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.CHEVROLET_MALIBU, CAR.BUICK_REGAL} or \ - (ret.networkLocation == NetworkLocation.gateway and ret.radarUnavailable) + if ret.enableGasInterceptor: + # Need to set ASCM long limits when using pedal interceptor, instead of camera ACC long limits + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_ASCM_LONG # Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below. ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] @@ -188,6 +190,10 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + if ret.enableGasInterceptor: + # ACC Bolts use pedal for full longitudinal control, not just sng + ret.flags |= GMFlags.PEDAL_LONG.value + elif candidate == CAR.CHEVROLET_SILVERADO: # On the Bolt, the ECM and camera independently check that you are either above 5 kph or at a stop # with foot on brake to allow engagement, but this platform only has that check in the camera. @@ -214,6 +220,23 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.CADILLAC_CT6_CC: CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + if ret.enableGasInterceptor: + ret.networkLocation = NetworkLocation.fwdCamera + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM + ret.minEnableSpeed = -1 + ret.pcmCruise = False + ret.openpilotLongitudinalControl = True + ret.stoppingControl = True + ret.autoResumeSng = True + + if candidate in CC_ONLY_CAR: + ret.flags |= GMFlags.PEDAL_LONG.value + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_PEDAL_LONG + # Note: Low speed, stop and go not tested. Should be fairly smooth on highway + ret.longitudinalTuning.kiBP = [0.0, 5., 35.] + ret.longitudinalTuning.kiV = [0.0, 0.35, 0.5] + ret.longitudinalTuning.kf = 0.15 + ret.stoppingDecelRate = 0.8 if candidate in CC_ONLY_CAR: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC @@ -259,6 +282,12 @@ class CarInterface(CarInterfaceBase): if ret.vEgo < self.CP.minSteerSpeed: events.add(EventName.belowSteerSpeed) + if (self.CP.flags & GMFlags.PEDAL_LONG.value) and \ + self.CP.transmissionType == TransmissionType.direct and \ + not self.CS.single_pedal_mode and \ + c.longActive: + events.add(EventName.pedalInterceptorNoBrake) + ret.events = events.to_msg() return ret diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 8446abf5f..a895e8f2f 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -213,6 +213,7 @@ class CanBus: DROPPED = 192 class GMFlags(IntFlag): + PEDAL_LONG = 1 NO_CAMERA = 4 NO_ACCELERATOR_POS_MSG = 8 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index b01818d70..4d57bc6f6 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -952,6 +952,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"), }, + EventName.pedalInterceptorNoBrake: { + ET.WARNING: Alert( + "Braking Unavailable", + "Shift to L", + AlertStatus.userPrompt, AlertSize.mid, + Priority.HIGH, VisualAlert.wrongGear, AudibleAlert.promptRepeat, 4.), + }, + }