Merge branch 'master' of https://github.com/sunnypilot/sunnypilot

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;
   }
 }
 

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)

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"]:

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)

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)

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

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
 

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()]

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