From 06754cffbd5ad7a5d86eca61824f43e859821d79 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 15 May 2024 17:53:51 -0700 Subject: [PATCH] poll carState without conflate --- selfdrive/car/card.py | 15 ++++++++------- selfdrive/controls/controlsd.py | 16 +++++++++++----- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index e65b27f9f7..cdc07014f8 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -136,13 +136,14 @@ 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) - - 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) + pass + # self.v_cruise_helper.update_v_cruise(CS, self.sm['controlsState'].enabled, self.is_metric) + # + # 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) 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..5aef6de150 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -85,16 +85,17 @@ class Controls: self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] self.log_sock = messaging.sub_sock('androidLog') + self.car_state_sock = messaging.sub_sock('carState', timeout=20) ignore = self.sensor_packets + ['testJoystick'] if SIMULATION: ignore += ['driverCameraState', 'managerState'] - self.sm = messaging.SubMaster(['carState', 'deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', + self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick'] + self.camera_packets + self.sensor_packets, - poll='carState', - ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ]) + ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ], + frequency=int(1/DT_CTRL)) # read params self.joystick_mode = self.params.get_bool("JoystickDebugMode") @@ -112,6 +113,7 @@ class Controls: if not self.CP.openpilotLongitudinalControl: self.params.remove("ExperimentalMode") + self.CS_prev = car.CarState.new_message() self.AM = AlertManager() self.events = Events() @@ -384,10 +386,12 @@ class Controls: def data_sample(self): """Receive data from sockets""" - self.sm.update(20) + CS = messaging.recv_one(self.car_state_sock) + if CS is None: + CS = self.CS_prev cloudlog.timestamp('Got carState') - CS = self.sm['carState'] + self.sm.update(0) if not self.initialized: all_valid = CS.canValid and self.sm.all_checks() @@ -811,6 +815,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'))