diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b381879a7a..4f1cd2b0c1 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1,7 +1,5 @@ #!/usr/bin/env python3 import math -import threading -import time from numbers import Number from cereal import car, log @@ -21,8 +19,6 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose -from openpilot.sunnypilot.livedelay.helpers import get_lat_delay -from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase from openpilot.sunnypilot.selfdrive.controls.controlsd_ext import ControlsExt State = log.SelfdriveState.OpenpilotState @@ -32,7 +28,7 @@ LaneChangeDirection = log.LaneChangeDirection ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys()) -class Controls(ControlsExt, ModelStateBase): +class Controls(ControlsExt): def __init__(self) -> None: self.params = Params() cloudlog.info("controlsd is waiting for CarParams") @@ -41,7 +37,6 @@ class Controls(ControlsExt, ModelStateBase): # Initialize sunnypilot controlsd extension and base model state ControlsExt.__init__(self, self.CP, self.params) - ModelStateBase.__init__(self) self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP) @@ -229,30 +224,15 @@ class Controls(ControlsExt, ModelStateBase): cc_send.carControl = CC self.pm.send('carControl', cc_send) - def params_thread(self, evt): - while not evt.is_set(): - self.get_params_sp() - - if self.CP.lateralTuning.which() == 'torque': - self.lat_delay = get_lat_delay(self.params, self.sm["liveDelay"].lateralDelay) - - time.sleep(0.1) - def run(self): rk = Ratekeeper(100, print_delay_threshold=None) - e = threading.Event() - t = threading.Thread(target=self.params_thread, args=(e,)) - try: - t.start() - while True: - self.update() - CC, lac_log = self.state_control() - self.publish(CC, lac_log) - self.run_ext(self.sm, self.pm) - rk.monitor_time() - finally: - e.set() - t.join() + while True: + self.update() + CC, lac_log = self.state_control() + self.publish(CC, lac_log) + self.get_params_sp(self.sm) + self.run_ext(self.sm, self.pm) + rk.monitor_time() def main(): diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py index 8caeeaeabc..3f6053d158 100644 --- a/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -4,21 +4,27 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ +import time + import cereal.messaging as messaging from cereal import log, custom from opendbc.car import structs from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD +from openpilot.sunnypilot.livedelay.helpers import get_lat_delay +from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral -class ControlsExt: +class ControlsExt(ModelStateBase): def __init__(self, CP: structs.CarParams, params: Params): + ModelStateBase.__init__(self) self.CP = CP self.params = params + self._param_update_time: float = 0.0 self.blinker_pause_lateral = BlinkerPauseLateral() - self.get_params_sp() cloudlog.info("controlsd_ext is waiting for CarParamsSP") self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP) @@ -27,8 +33,14 @@ class ControlsExt: self.sm_services_ext = ['radarState', 'selfdriveStateSP'] self.pm_services_ext = ['carControlSP'] - def get_params_sp(self) -> None: - self.blinker_pause_lateral.get_params() + def get_params_sp(self, sm: messaging.SubMaster) -> None: + if time.monotonic() - self._param_update_time > PARAMS_UPDATE_PERIOD: + self.blinker_pause_lateral.get_params() + + if self.CP.lateralTuning.which() == 'torque': + self.lat_delay = get_lat_delay(self.params, sm["liveDelay"].lateralDelay) + + self._param_update_time = time.monotonic() def get_lat_active(self, sm: messaging.SubMaster) -> bool: if self.blinker_pause_lateral.update(sm['carState']):