From b4af8a9b0a2e17fdfc89d344c64678ef51305c24 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 15 May 2024 16:51:16 -0700 Subject: [PATCH] simple state machine in card (doesn't work as is) --- cereal | 2 +- selfdrive/car/card.py | 23 ++++++++++++++++------- selfdrive/controls/controlsd.py | 9 +++++++++ 3 files changed, 26 insertions(+), 8 deletions(-) diff --git a/cereal b/cereal index 0cebb30e41..4d2ead17cf 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 0cebb30e41c436b023910a7a03a22e601648cb58 +Subproject commit 4d2ead17cf67e23831bd73f8fb036c06846c23f7 diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index e65b27f9f7..67b79e8565 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -15,7 +15,7 @@ from openpilot.common.swaglog import cloudlog 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 +from openpilot.selfdrive.controls.lib.events import Events, ET REPLAY = "REPLAY" in os.environ @@ -88,6 +88,8 @@ class Car: self.events = Events() + self.enabled_requested = False + # card is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) @@ -136,13 +138,20 @@ class Car: CS.events = self.events.to_msg() def state_transition(self, CS: car.CarState): - self.v_cruise_helper.update_v_cruise(CS, self.sm['controlsState'].enabled, self.is_metric) + # card maintains its own requested state to ensure controlsd doesn't miss any enable/disable events + if self.events.contains(ET.ENABLE): + if not self.events.contains(ET.NO_ENTRY): + self.enabled_requested = True - controlsState = self.sm['controlsState'] - if self.controlsState_prev.state == State.disabled: - # TODO: use ENABLED_STATES from controlsd? it includes softDisabling which isn't possible here - if controlsState.state in (State.preEnabled, State.overriding, State.enabled): - self.v_cruise_helper.initialize_v_cruise(CS, controlsState.experimentalMode) + else: + if self.events.contains(ET.USER_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE): + self.enabled_requested = False + + # sync state with controlsd on non-car disable events + if not self.sm['controlsState'].enabled and self.controlsState_prev.enabled: + self.enabled_requested = False + + CS.enabledRequested = self.enabled_requested def state_publish(self, CS: car.CarState): """carState and carParams publish loop""" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 309ec1b33d..0dd844f92f 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -147,6 +147,7 @@ class Controls: self.personality = self.read_personality_param() self.v_cruise_helper = VCruiseHelper(self.CP) self.recalibrating_seen = False + self.CS_prev = car.CarState.new_message() self.can_log_mono_time = 0 @@ -212,6 +213,12 @@ class Controls: if CS.canValid: self.events.add_from_msg(CS.events) + # FIXME: immediate disable from card won't show up as alerts anymore because user disable has higher priority :/ + if CS.enabledRequested and not self.CS_prev.enabledRequested: + self.events.add(EventName.pcmEnable) + elif not CS.enabledRequested and self.CS_prev.enabledRequested: + self.events.add(EventName.pcmDisable) + # Create events for temperature, disk space, and memory if self.sm['deviceState'].thermalStatus >= ThermalStatus.red: self.events.add(EventName.overheat) @@ -811,6 +818,8 @@ class Controls: self.publish_logs(CS, start_time, CC, lac_log) cloudlog.timestamp("Logs published") + self.CS_prev = CS + def read_personality_param(self): try: return int(self.params.get('LongitudinalPersonality'))