diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 5468e0f39..8d4bc3cb8 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -202,7 +202,20 @@ struct CarControlSP @0xa5cd762cd951a455 { struct Param { key @0 :Text; - value @1 :Text; + type @2 :ParamType; + value @3 :Data; + + valueDEPRECATED @1 :Text; # The data type change may cause issues with backwards compatibility. + } + + enum ParamType { + string @0; + bool @1; + int @2; + float @3; + time @4; + json @5; + bytes @6; } } diff --git a/opendbc_repo b/opendbc_repo index 69bae677e..2fa8f7e8d 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 69bae677ef0fc429300a778a96a513a3fd66874a +Subproject commit 2fa8f7e8d86a0941383879f713f5f569bc3c9ee1 diff --git a/panda b/panda index 2a70b09fb..df8221737 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 2a70b09fb9a28694a4b00f79d5f2d504147a7170 +Subproject commit df8221737ac55eb3dc7d31bc7e54647767132bbf diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 82c4c92b1..7273745c7 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -3,6 +3,7 @@ import capnp import numpy as np from cereal import log from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta +from openpilot.sunnypilot.models.helpers import plan_x_idxs_helper SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -95,8 +96,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # action modelV2.action = action - # times at X_IDXS of edges and lines aren't used - LINE_T_IDXS: list[float] = [] + # times at X_IDXS of edges and lines + LINE_T_IDXS: list[float] = plan_x_idxs_helper(ModelConstants, Plan, net_output_data) # lane lines modelV2.init('laneLines', 4) diff --git a/selfdrive/pandad/pandad.py b/selfdrive/pandad/pandad.py index 361c1f214..f4064ddcd 100755 --- a/selfdrive/pandad/pandad.py +++ b/selfdrive/pandad/pandad.py @@ -29,6 +29,12 @@ def flash_panda(panda_serial: str) -> Panda: HARDWARE.recover_internal_panda() raise + # skip flashing if the detected panda is not supported + supported_panda = check_panda_support(panda) + if not supported_panda: + cloudlog.warning(f"Panda {panda_serial} is not supported (hw_type: {panda.get_type()}), skipping flash...") + return panda + fw_signature = get_expected_signature(panda) internal_panda = panda.is_internal() @@ -36,12 +42,6 @@ def flash_panda(panda_serial: str) -> Panda: panda_signature = b"" if panda.bootstub else panda.get_signature() cloudlog.warning(f"Panda {panda_serial} connected, version: {panda_version}, signature {panda_signature.hex()[:16]}, expected {fw_signature.hex()[:16]}") - # skip flashing if the detected device is not supported from upstream - hw_type = panda.get_type() - if hw_type not in Panda.SUPPORTED_DEVICES: - cloudlog.warning(f"Panda {panda_serial} is not supported (hw_type: {hw_type}), skipping flash...") - return panda - if panda.bootstub or panda_signature != fw_signature: cloudlog.info("Panda firmware out of date, update required") panda.flash() @@ -67,6 +67,14 @@ def flash_panda(panda_serial: str) -> Panda: return panda +def check_panda_support(panda) -> bool: + hw_type = panda.get_type() + if hw_type in Panda.SUPPORTED_DEVICES: + return True + + return False + + def main() -> None: # signal pandad to close the relay and exit def signal_handler(signum, frame): @@ -140,6 +148,12 @@ def main() -> None: params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas)) for panda in pandas: + # skip health check if the detected panda is not supported + supported_panda = check_panda_support(panda) + if not supported_panda: + cloudlog.warning(f"Panda {panda.get_usb_serial()} is not supported (hw_type: {panda.get_type()}), skipping health check...") + continue + # check health for lost heartbeat health = panda.health() if health["heartbeat_lost"]: diff --git a/sunnypilot/modeld/fill_model_msg.py b/sunnypilot/modeld/fill_model_msg.py index dadffc843..a62c451ef 100644 --- a/sunnypilot/modeld/fill_model_msg.py +++ b/sunnypilot/modeld/fill_model_msg.py @@ -3,6 +3,7 @@ import capnp import numpy as np from cereal import log from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan +from openpilot.sunnypilot.models.helpers import plan_x_idxs_helper from openpilot.sunnypilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_lag_adjusted_curvature, MIN_SPEED SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -120,23 +121,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D modelV2_action.desiredCurvature = desired_curvature # times at X_IDXS according to model plan - PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N - PLAN_T_IDXS[0] = 0.0 - plan_x = net_output_data['plan'][0,:,Plan.POSITION][:,0].tolist() - for xidx in range(1, ModelConstants.IDX_N): - tidx = 0 - # increment tidx until we find an element that's further away than the current xidx - while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx+1] < ModelConstants.X_IDXS[xidx]: - tidx += 1 - if tidx == ModelConstants.IDX_N - 1: - # if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break - PLAN_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1] - break - # interpolate to find `t` for the current xidx - current_x_val = plan_x[tidx] - next_x_val = plan_x[tidx+1] - p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(next_x_val - current_x_val) > 1e-9 else float('nan') - PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx] + PLAN_T_IDXS: list[float] = plan_x_idxs_helper(ModelConstants, Plan, net_output_data) # lane lines modelV2.init('laneLines', 4) diff --git a/sunnypilot/modeld_v2/fill_model_msg.py b/sunnypilot/modeld_v2/fill_model_msg.py index c7de698f6..ee0eb4868 100644 --- a/sunnypilot/modeld_v2/fill_model_msg.py +++ b/sunnypilot/modeld_v2/fill_model_msg.py @@ -3,6 +3,7 @@ import capnp import numpy as np from cereal import log from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan +from openpilot.sunnypilot.models.helpers import plan_x_idxs_helper from openpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -118,8 +119,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # action (includes lateral planning now) modelV2.action = action - # times at X_IDXS of edges and lines aren't used - LINE_T_IDXS: list[float] = [] + # times at X_IDXS of edges and lines + LINE_T_IDXS: list[float] = plan_x_idxs_helper(ModelConstants, Plan, net_output_data) # lane lines modelV2.init('laneLines', 4) diff --git a/sunnypilot/models/helpers.py b/sunnypilot/models/helpers.py index ecf0a39b7..20b94fb61 100644 --- a/sunnypilot/models/helpers.py +++ b/sunnypilot/models/helpers.py @@ -185,3 +185,27 @@ def load_meta_constants(model_metadata): meta = MetaTombRaider return meta + + +# The following method(s) are modeld helper methods +def plan_x_idxs_helper(constants, plan, model_output) -> list[float]: + # times at X_IDXS according to plan. + LINE_T_IDXS = [np.nan] * constants.IDX_N + LINE_T_IDXS[0] = 0.0 + plan_x = model_output['plan'][0, :, plan.POSITION][:, 0].tolist() + for xidx in range(1, constants.IDX_N): + tidx = 0 + # increment tidx until we find an element that's further away than the current xidx + while tidx < constants.IDX_N - 1 and plan_x[tidx + 1] < constants.X_IDXS[xidx]: + tidx += 1 + if tidx == constants.IDX_N - 1: + # if the plan doesn't extend far enough, set plan_t to the max value (10s), then break + LINE_T_IDXS[xidx] = constants.T_IDXS[constants.IDX_N - 1] + break + # interpolate to find `t` for the current xidx + current_x_val = plan_x[tidx] + next_x_val = plan_x[tidx + 1] + p = (constants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs( + next_x_val - current_x_val) > 1e-9 else float('nan') + LINE_T_IDXS[xidx] = p * constants.T_IDXS[tidx + 1] + (1 - p) * constants.T_IDXS[tidx] + return LINE_T_IDXS diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py index 7e06ac77c..a096b7dc8 100644 --- a/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -73,7 +73,7 @@ class ControlsExt: # MADS state CC_SP.mads = sm['selfdriveStateSP'].mads - CC_SP.params = self.param_store.publish() + CC_SP.params = self.param_store.param_list return CC_SP diff --git a/sunnypilot/selfdrive/controls/lib/param_store.py b/sunnypilot/selfdrive/controls/lib/param_store.py index 2ef347318..65a017534 100644 --- a/sunnypilot/selfdrive/controls/lib/param_store.py +++ b/sunnypilot/selfdrive/controls/lib/param_store.py @@ -4,39 +4,41 @@ 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 +from sunnypilot.sunnylink.utils import get_param_as_byte + class ParamStore: keys: list[str] - values: dict[str, str] + _params: dict[str, custom.CarControlSP.Param] def __init__(self, CP: structs.CarParams): universal_params: list[str] = [] brand_params: list[str] = [] self.keys = universal_params + brand_params - self.values = {} - self.cached_params_list: list[capnp.lib.capnp._DynamicStructBuilder] | None = None + self._params = {} self.frame = 0 def update(self, params: Params) -> None: - if self.frame % 300 == 0: - old_values = dict(self.values) - self.values = {k: params.get(k) or "0" for k in self.keys} - if old_values != self.values: - self.cached_params_list = None - self.frame += 1 + if self.frame % 300 != 0: + return - def publish(self) -> list[capnp.lib.capnp._DynamicStructBuilder]: - if self.cached_params_list is None: - # TODO-SP: Why are we doing a list instead of a dictionary here? - self.cached_params_list = [custom.CarControlSP.Param(key=k, value=self.values[k]) for k in self.keys] - return self.cached_params_list + for key in self.keys: + param_type = params.get_type(key).name.lower() # Using string instead of number because its "loose" dependency, and could change by OP at anytime. + + # Over engineering opportunity: It's possible this conversion is slow, we may check the value as params returns it for cache purposes. Not today. + param_value = get_param_as_byte(key, params) + if (existing_param := self._params.get(key)) is not None and existing_param.value == param_value: + continue + + self._params[key] = custom.CarControlSP.Param(key=key, value=param_value, type=param_type) + + @property + def param_list(self) -> list[custom.CarControlSP.Param]: + return [v for k,v in self._params.items()] diff --git a/sunnypilot/sunnylink/utils.py b/sunnypilot/sunnylink/utils.py index 1310b91f0..91c078879 100644 --- a/sunnypilot/sunnylink/utils.py +++ b/sunnypilot/sunnylink/utils.py @@ -60,9 +60,9 @@ def get_api_token(): print(f"API Token: {token}") -def get_param_as_byte(param_name: str) -> bytes | None: +def get_param_as_byte(param_name: str, params=None) -> bytes | None: """Get a parameter as bytes. Returns None if the parameter does not exist.""" - params = Params() + params = params or Params() # Use existing Params instance if provided param = params.get(param_name) if param is None: return None @@ -85,6 +85,17 @@ def save_param_from_base64_encoded_string(param_name: str, base64_encoded_data: if is_compressed: value = gzip.decompress(value) + # We convert to string anything that isn't bytes first. We later transform further. + param_value = _convert_param_to_type(value, param_type) + params.put(param_name, param_value) + + +def _convert_param_to_type(value: bytes, param_type: ParamKeyType) -> bytes | str | int | float | bool | dict | None: + """ + Convert a byte value to the specified param type. Used internally when getting a Param to convert it to the right type. + If this method looks familiar, it's because on SP we have a similar one in openpilot/sunnypilot/car/__init__.py. + """ + # We convert to string anything that isn't bytes first. We later transform further. if param_type != ParamKeyType.BYTES: value = value.decode('utf-8') # type: ignore @@ -101,4 +112,5 @@ def save_param_from_base64_encoded_string(param_name: str, base64_encoded_data: value = str(value) # type: ignore elif param_type == ParamKeyType.JSON: value = json.loads(value) - params.put(param_name, value) + + return value