From b51043770b1accf00a05f74cd4ec71a9405b6ba2 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 24 May 2025 16:20:57 -0400 Subject: [PATCH] `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 | 6 ++++ opendbc_repo | 2 +- selfdrive/car/helpers.py | 1 + selfdrive/controls/controlsd.py | 29 +++++++++++---- .../selfdrive/controls/controlsd_ext.py | 18 +++++++--- .../selfdrive/controls/lib/param_store.py | 35 +++++++++++++++++++ 6 files changed, 79 insertions(+), 12 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/lib/param_store.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index eb932b484..503a2be86 100644 --- a/cereal/custom.capnp +++ b/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 { diff --git a/opendbc_repo b/opendbc_repo index ac3d40b75..52d2957b4 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit ac3d40b7570d85ba4f8056ac542a10cc8a82ccea +Subproject commit 52d2957b47360e36c02d626ed02981123af79b24 diff --git a/selfdrive/car/helpers.py b/selfdrive/car/helpers.py index 3f7a322c0..6ebacfc97 100644 --- a/selfdrive/car/helpers.py +++ b/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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a5d97c3db..576e73b7e 100755 --- a/selfdrive/controls/controlsd.py +++ b/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: - self.update() - CC, lac_log = self.state_control() - self.publish(CC, lac_log) - self.run_ext(self.sm, self.pm) - rk.monitor_time() + 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() def main(): diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py index 68131001a..4978c0133 100644 --- a/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/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(sm: messaging.SubMaster) -> custom.CarControlSP: + def state_control_ext(self, 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 diff --git a/sunnypilot/selfdrive/controls/lib/param_store.py b/sunnypilot/selfdrive/controls/lib/param_store.py new file mode 100644 index 000000000..b2701bc01 --- /dev/null +++ b/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