CarControlSP: live params (#943)

* car: initialize sunnypilot interface immediately

* init

* Revert "car: initialize sunnypilot interface immediately"

This reverts commit 2e0fc4d87eb312a3a74a61bb8cbc780e33efbfc7.

* show int

* show int

* work properly with cereal

* unused

* in its own module

* bump

* not yet

* not yet

* list comp and use structs directly

* allow default key if needed

* lint

* send it with None

* bump

cereal/custom.capnp

@@ -167,6 +167,12 @@ struct CarParamsSP @0x80ae746ee2596b11 {
 
 struct CarControlSP @0xa5cd762cd951a455 {
   mads @0 :ModularAssistiveDrivingSystem;
+  params @1 :List(Param);
+
+  struct Param {
+    key @0 :Text;
+    value @1 :Text;
+  }
 }
 
 struct BackupManagerSP @0xf98d843bfd7004a3 {

selfdrive/car/helpers.py

@@ -55,5 +55,6 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct
   struct_dataclass = structs.CarControlSP(**remove_deprecated({k: v for k, v in struct_dict.items() if not isinstance(k, dict)}))
 
   struct_dataclass.mads = structs.ModularAssistiveDrivingSystem(**remove_deprecated(struct_dict.get('mads', {})))
+  struct_dataclass.params = [structs.CarControlSP.Param(**remove_deprecated(p)) for p in struct_dict.get('params', [])]
 
   return struct_dataclass

selfdrive/controls/controlsd.py

@@ -1,5 +1,7 @@
 #!/usr/bin/env python3
 import math
+import threading
+import time
 from typing import SupportsFloat
 
 from cereal import car, log
@@ -36,7 +38,7 @@ class Controls(ControlsExt):
     cloudlog.info("controlsd got CarParams")
 
     # Initialize sunnypilot controlsd extension
-    ControlsExt.__init__(self, self.params)
+    ControlsExt.__init__(self, self.CP, self.params)
 
     self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP)
 
@@ -222,14 +224,27 @@ class Controls(ControlsExt):
     cc_send.carControl = CC
     self.pm.send('carControl', cc_send)
 
+  def params_thread(self, evt):
+    while not evt.is_set():
+      self.get_params_sp()
+
+      time.sleep(0.1)
+
   def run(self):
     rk = Ratekeeper(100, print_delay_threshold=None)
-    while True:
+    e = threading.Event()
-      self.update()
+    t = threading.Thread(target=self.params_thread, args=(e,))
-      CC, lac_log = self.state_control()
+    try:
-      self.publish(CC, lac_log)
+      t.start()
-      self.run_ext(self.sm, self.pm)
+      while True:
-      rk.monitor_time()
+        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()
 
 
 def main():

sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -4,16 +4,22 @@ 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 cereal.messaging as messaging
 from cereal import custom
 
+from opendbc.car import structs
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
+from openpilot.sunnypilot.selfdrive.controls.lib.param_store import ParamStore
 
 
 class ControlsExt:
-  def __init__(self, params: Params):
+  def __init__(self, CP: structs.CarParams, params: Params):
+    self.CP = CP
+    self.params = params
+    self.param_store = ParamStore(self.CP)
+    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)
     cloudlog.info("controlsd_ext got CarParamsSP")
@@ -21,6 +27,9 @@ class ControlsExt:
     self.sm_services_ext = ['selfdriveStateSP']
     self.pm_services_ext = ['carControlSP']
 
+  def get_params_sp(self) -> None:
+    self.param_store.update(self.params)
+
   @staticmethod
   def get_lat_active(sm: messaging.SubMaster) -> bool:
     ss_sp = sm['selfdriveStateSP']
@@ -31,13 +40,14 @@ class ControlsExt:
     # MADS not available, use stock state to engage
     return bool(sm['selfdriveState'].active)
 
-  @staticmethod
+  def state_control_ext(self, sm: messaging.SubMaster) -> custom.CarControlSP:
-  def state_control_ext(sm: messaging.SubMaster) -> custom.CarControlSP:
     CC_SP = custom.CarControlSP.new_message()
 
     # MADS state
     CC_SP.mads = sm['selfdriveStateSP'].mads
 
+    CC_SP.params = self.param_store.publish()
+
     return CC_SP
 
   @staticmethod

sunnypilot/selfdrive/controls/lib/param_store.py

@@ -0,0 +1,35 @@
+"""
+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 capnp
+
+from cereal import custom
+
+from opendbc.car import structs
+from openpilot.common.params import Params
+
+
+class ParamStore:
+  keys: list[str]
+  values: dict[str, str]
+
+  def __init__(self, CP: structs.CarParams):
+    universal_params: list[str] = []
+    brand_params: list[str] = []
+
+    self.keys = universal_params + brand_params
+    self.values = {}
+
+  def update(self, params: Params) -> None:
+    self.values = {k: params.get(k, encoding='utf8') or "0" for k in self.keys}
+
+  def publish(self) -> list[capnp.lib.capnp._DynamicStructBuilder]:
+    params_list: list[capnp.lib.capnp._DynamicStructBuilder] = []
+
+    for k in self.keys:
+      params_list.append(custom.CarControlSP.Param(key=k, value=self.values[k]))
+
+    return params_list