From d0d44a51a0ff66c1b2da56b3d9864e10ecc2174d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 13 May 2024 16:18:55 -0700 Subject: [PATCH] card: create pedal pressed event (#32417) * card: create pedal pressed event (#32393) * move pedalPressed to card * rm * needs to be a builder * move these * clean up * reader later * Update ref_commit * moved to card --- selfdrive/car/card.py | 26 ++++++++++++++++++++++-- selfdrive/car/interfaces.py | 5 ++--- selfdrive/car/tests/test_models.py | 11 +++++----- selfdrive/controls/controlsd.py | 10 --------- selfdrive/test/process_replay/ref_commit | 2 +- 5 files changed, 32 insertions(+), 22 deletions(-) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index ff1950fbef..ed916a6fe1 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -14,9 +14,12 @@ from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp from openpilot.selfdrive.car.car_helpers import get_car, get_one_can from openpilot.selfdrive.car.interfaces import CarInterfaceBase +from openpilot.selfdrive.controls.lib.events import Events REPLAY = "REPLAY" in os.environ +EventName = car.CarEvent.EventName + class CarD: CI: CarInterfaceBase @@ -47,9 +50,9 @@ class CarD: self.CI, self.CP = CI, CI.CP # set alternative experiences from parameters - disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") + self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") self.CP.alternativeExperience = 0 - if not disengage_on_accelerator: + if not self.disengage_on_accelerator: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") @@ -73,6 +76,9 @@ class CarD: self.params.put_nonblocking("CarParamsCache", cp_bytes) self.params.put_nonblocking("CarParamsPersistent", cp_bytes) + self.CS_prev = car.CarState.new_message() + self.events = Events() + def initialize(self): """Initialize CarInterface, once controls are ready""" self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) @@ -100,10 +106,26 @@ class CarD: if can_rcv_valid and REPLAY: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime + self.update_events(CS) self.state_publish(CS) + CS = CS.as_reader() + self.CS_prev = CS return CS + def update_events(self, CS: car.CarState) -> car.CarState: + self.events.clear() + + self.events.add_from_msg(CS.events) + + # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0 + if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \ + (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \ + (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): + self.events.add(EventName.pedalPressed) + + CS.events = self.events.to_msg() + def state_publish(self, CS: car.CarState): """carState and carParams publish loop""" diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index a8a6f0b238..982d987d61 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -242,11 +242,10 @@ class CarInterfaceBase(ABC): ret.cruiseState.speedCluster = ret.cruiseState.speed # copy back for next iteration - reader = ret.as_reader() if self.CS is not None: - self.CS.out = reader + self.CS.out = ret.as_reader() - return reader + return ret def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True, diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 2a0626c590..dc3d4256a2 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -15,12 +15,12 @@ from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import gen_empty_fingerprint +from openpilot.selfdrive.car.card import CarD from openpilot.selfdrive.car.fingerprints import all_known_cars, MIGRATION from openpilot.selfdrive.car.car_helpers import FRAME_FINGERPRINT, interfaces from openpilot.selfdrive.car.honda.values import CAR as HONDA, HondaFlags from openpilot.selfdrive.car.tests.routes import non_tested_cars, routes, CarTestRoute from openpilot.selfdrive.car.values import Platform -from openpilot.selfdrive.controls.controlsd import Controls from openpilot.selfdrive.test.helpers import read_segment_list from openpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT from openpilot.tools.lib.logreader import LogReader, internal_source, openpilotci_source @@ -406,8 +406,7 @@ class TestCarModelBase(unittest.TestCase): controls_allowed_prev = False CS_prev = car.CarState.new_message() checks = defaultdict(int) - controlsd = Controls(CI=self.CI) - controlsd.initialized = True + card = CarD(CI=self.CI) for idx, can in enumerate(self.can_msgs): CS = self.CI.update(CC, (can.as_builder().to_bytes(), )) for msg in filter(lambda m: m.src in range(64), can.can): @@ -452,10 +451,10 @@ class TestCarModelBase(unittest.TestCase): checks['cruiseState'] += CS.cruiseState.enabled != self.safety.get_cruise_engaged_prev() else: # Check for enable events on rising edge of controls allowed - controlsd.update_events(CS) - controlsd.CS_prev = CS + card.update_events(CS) + card.CS_prev = CS button_enable = (any(evt.enable for evt in CS.events) and - not any(evt == EventName.pedalPressed for evt in controlsd.events.names)) + not any(evt == EventName.pedalPressed for evt in card.events.names)) mismatch = button_enable != (self.safety.get_controls_allowed() and not controls_allowed_prev) checks['controlsAllowed'] += mismatch controls_allowed_prev = self.safety.get_controls_allowed() diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 370a3e1e95..95985dc3c2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -96,7 +96,6 @@ class Controls: self.joystick_mode = self.params.get_bool("JoystickDebugMode") # read params - self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") self.is_metric = self.params.get_bool("IsMetric") self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") @@ -111,7 +110,6 @@ class Controls: if not self.CP.openpilotLongitudinalControl: self.params.remove("ExperimentalMode") - self.CS_prev = car.CarState.new_message() self.AM = AlertManager() self.events = Events() @@ -205,12 +203,6 @@ class Controls: if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed: self.events.add(EventName.resumeBlocked) - # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0 - if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \ - (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \ - (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): - self.events.add(EventName.pedalPressed) - if not self.CP.notCar: self.events.add_from_msg(self.sm['driverMonitoringState'].events) @@ -815,8 +807,6 @@ class Controls: # Publish data self.publish_logs(CS, start_time, CC, lac_log) - self.CS_prev = CS - def read_personality_param(self): try: return int(self.params.get('LongitudinalPersonality')) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index f3298b2ac8..a61fd8e23b 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -ef0c8cb36b9cda6381412493555c21a87360e539 \ No newline at end of file +e5a385503e4307ae77c73736a602380b08e61334