From 02355f44df2e9fca06d565b2e30641ff8d62f1a3 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 21 Jan 2025 12:08:19 -0500
Subject: [PATCH] fix data type

---
 selfdrive/car/card.py    |  8 ++++--
 selfdrive/car/helpers.py | 59 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 65 insertions(+), 2 deletions(-)
 create mode 100644 selfdrive/car/helpers.py

diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py
index 959ecc567d..89aeaf720e 100755
--- a/selfdrive/car/card.py
+++ b/selfdrive/car/card.py
@@ -21,6 +21,7 @@ from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
 from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
 from openpilot.selfdrive.car.cruise import VCruiseHelper
 from openpilot.selfdrive.car.car_specific import MockCarState
+from openpilot.selfdrive.car.helpers import convert_to_capnp
 
 from openpilot.sunnypilot.mads.mads import MadsParams
 from openpilot.sunnypilot.selfdrive.car import interfaces
@@ -66,7 +67,8 @@ class Car:
   CI: CarInterfaceBase
   RI: RadarInterfaceBase
   CP: car.CarParams
-  CP_SP: custom.CarParamsSP
+  CP_SP: structs.CarParamsSP
+  CP_SP_capnp: custom.CarParamsSP
 
   def __init__(self, CI=None, RI=None) -> None:
     self.can_sock = messaging.sub_sock('can', timeout=20)
@@ -170,7 +172,9 @@ class Car:
     self.params.put_nonblocking("CarParamsPersistent", cp_bytes)
 
     # Write CarParamsSP for controls
-    cp_sp_bytes = self.CP_SP.to_bytes()
+    # convert to pycapnp representation for caching and logging
+    self.CP_capnp = convert_to_capnp(self.CP)
+    cp_sp_bytes = self.CP_SP_capnp.to_bytes()
     self.params.put("CarParamsSP", cp_sp_bytes)
     self.params.put_nonblocking("CarParamsSPCache", cp_sp_bytes)
     self.params.put_nonblocking("CarParamsSPPersistent", cp_sp_bytes)
diff --git a/selfdrive/car/helpers.py b/selfdrive/car/helpers.py
new file mode 100644
index 0000000000..95a4d9889b
--- /dev/null
+++ b/selfdrive/car/helpers.py
@@ -0,0 +1,59 @@
+import capnp
+from typing import Any
+
+from cereal import car
+from opendbc.car import structs
+
+_FIELDS = '__dataclass_fields__'  # copy of dataclasses._FIELDS
+
+
+def is_dataclass(obj):
+  """Similar to dataclasses.is_dataclass without instance type check checking"""
+  return hasattr(obj, _FIELDS)
+
+
+def _asdictref_inner(obj) -> dict[str, Any] | Any:
+  if is_dataclass(obj):
+    ret = {}
+    for field in getattr(obj, _FIELDS):  # similar to dataclasses.fields()
+      ret[field] = _asdictref_inner(getattr(obj, field))
+    return ret
+  elif isinstance(obj, (tuple, list)):
+    return type(obj)(_asdictref_inner(v) for v in obj)
+  else:
+    return obj
+
+
+def asdictref(obj) -> dict[str, Any]:
+  """
+  Similar to dataclasses.asdict without recursive type checking and copy.deepcopy
+  Note that the resulting dict will contain references to the original struct as a result
+  """
+  if not is_dataclass(obj):
+    raise TypeError("asdictref() should be called on dataclass instances")
+
+  return _asdictref_inner(obj)
+
+
+def convert_to_capnp(struct: structs.CarParams | structs.CarState | structs.CarControl.Actuators | structs.RadarData) -> capnp.lib.capnp._DynamicStructBuilder:
+  struct_dict = asdictref(struct)
+
+  if isinstance(struct, structs.CarParams):
+    del struct_dict['lateralTuning']
+    struct_capnp = car.CarParams.new_message(**struct_dict)
+
+    # this is the only union, special handling
+    which = struct.lateralTuning.which()
+    struct_capnp.lateralTuning.init(which)
+    lateralTuning_dict = asdictref(getattr(struct.lateralTuning, which))
+    setattr(struct_capnp.lateralTuning, which, lateralTuning_dict)
+  elif isinstance(struct, structs.CarState):
+    struct_capnp = car.CarState.new_message(**struct_dict)
+  elif isinstance(struct, structs.CarControl.Actuators):
+    struct_capnp = car.CarControl.Actuators.new_message(**struct_dict)
+  elif isinstance(struct, structs.RadarData):
+    struct_capnp = car.RadarData.new_message(**struct_dict)
+  else:
+    raise ValueError(f"Unsupported struct type: {type(struct)}")
+
+  return struct_capnp