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/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