controlsd_ext: use time.monotonic to check params intervals (#1481)

* controlsd_ext: use time.monotonic to check params intervals

* test

* Revert "test"

This reverts commit 151ac3bc6812cdd1693c1a7650f44bb3bcd8910e.

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
@@ -22,8 +20,6 @@ from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
 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
@@ -33,7 +29,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")
@@ -42,7 +38,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)
 
@@ -231,30 +226,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():

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']):