Car: CarStateSP (#972)

* Car: `CarStateSP`

* fix tests

* just skeleton for now

* bump

cereal/custom.capnp

@@ -223,7 +223,7 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
   }
 }
 
-struct CustomReserved7 @0xb86e6369214c01c8 {
+struct CarStateSP @0xb86e6369214c01c8 {
 }
 
 struct CustomReserved8 @0xf416ec09499d9d19 {

cereal/log.capnp

@@ -2609,7 +2609,7 @@ struct Event {
     carParamsSP @111 :Custom.CarParamsSP;
     carControlSP @112 :Custom.CarControlSP;
     backupManagerSP @113 :Custom.BackupManagerSP;
-    customReserved7 @114 :Custom.CustomReserved7;
+    carStateSP @114 :Custom.CarStateSP;
     customReserved8 @115 :Custom.CustomReserved8;
     customReserved9 @116 :Custom.CustomReserved9;
     customReserved10 @136 :Custom.CustomReserved10;

cereal/services.py

@@ -83,6 +83,7 @@ _services: dict[str, tuple] = {
   "onroadEventsSP": (True, 1., 1),
   "carParamsSP": (True, 0.02, 1),
   "carControlSP": (True, 100., 10),
+  "carStateSP": (True, 100., 10),
 
   # debug
   "uiDebug": (True, 0., 1),

selfdrive/car/car_specific.py

@@ -1,4 +1,4 @@
-from cereal import car, log
+from cereal import car, log, custom
 import cereal.messaging as messaging
 from opendbc.car import DT_CTRL, structs
 from opendbc.car.interfaces import MAX_CTRL_SPEED
@@ -16,14 +16,14 @@ class MockCarState:
   def __init__(self):
     self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal'])
 
-  def update(self, CS: car.CarState):
+  def update(self, CS: car.CarState, CS_SP: custom.CarStateSP):
     self.sm.update(0)
     gps_sock = 'gpsLocationExternal' if self.sm.recv_frame['gpsLocationExternal'] > 1 else 'gpsLocation'
 
     CS.vEgo = self.sm[gps_sock].speed
     CS.vEgoRaw = self.sm[gps_sock].speed
 
-    return CS
+    return CS, CS_SP
 
 
 class CarSpecificEvents:

selfdrive/car/card.py

@@ -73,12 +73,13 @@ class Car:
   def __init__(self, CI=None, RI=None) -> None:
     self.can_sock = messaging.sub_sock('can', timeout=20)
     self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP'])
-    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP'])
+    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP'])
 
     self.can_rcv_cum_timeout_counter = 0
 
     self.CC_prev = car.CarControl.new_message()
     self.CS_prev = car.CarState.new_message()
+    self.CS_SP_prev = custom.CarStateSP.new_message()
     self.initialized_prev = False
 
     self.last_actuators_output = structs.CarControl.Actuators()
@@ -188,16 +189,17 @@ class Car:
     # log fingerprint in sentry
     sunnypilot_interfaces.log_fingerprint(self.CP)
 
-  def state_update(self) -> tuple[car.CarState, structs.RadarDataT | None]:
+  def state_update(self) -> tuple[car.CarState, custom.CarStateSP, structs.RadarDataT | None]:
     """carState update loop, driven by can"""
 
     can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
     can_list = can_capnp_to_list(can_strs)
 
     # Update carState from CAN
-    CS = self.CI.update(can_list)
+    CS, CS_SP = self.CI.update(can_list)
+    CS_SP = convert_to_capnp(CS_SP)
     if self.CP.brand == 'mock':
-      CS = self.mock_carstate.update(CS)
+      CS, CS_SP = self.mock_carstate.update(CS, CS_SP)
 
     # Update radar tracks from CAN
     RD: structs.RadarDataT | None = self.RI.update(can_list)
@@ -222,9 +224,9 @@ class Car:
     CS.vCruise = float(self.v_cruise_helper.v_cruise_kph)
     CS.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph)
 
-    return CS, RD
+    return CS, CS_SP, RD
 
-  def state_publish(self, CS: car.CarState, RD: structs.RadarDataT | None):
+  def state_publish(self, CS: car.CarState, CS_SP: custom.CarStateSP, RD: structs.RadarDataT | None):
     """carState and carParams publish loop"""
 
     # carParams - logged every 50 seconds (> 1 per segment)
@@ -261,6 +263,11 @@ class Car:
       cp_sp_send.carParamsSP = self.CP_SP_capnp
       self.pm.send('carParamsSP', cp_sp_send)
 
+    cs_sp_send = messaging.new_message('carStateSP')
+    cs_sp_send.valid = CS.canValid
+    cs_sp_send.carStateSP = CS_SP
+    self.pm.send('carStateSP', cs_sp_send)
+
   def controls_update(self, CS: car.CarState, CC: car.CarControl, CC_SP: custom.CarControlSP):
     """control update loop, driven by carControl"""
 
@@ -280,9 +287,9 @@ class Car:
       self.CC_prev = CC
 
   def step(self):
-    CS, RD = self.state_update()
+    CS, CS_SP, RD = self.state_update()
 
-    self.state_publish(CS, RD)
+    self.state_publish(CS, CS_SP, RD)
 
     initialized = (not any(e.name == EventName.selfdriveInitializing for e in self.sm['onroadEvents']) and
                    self.sm.seen['onroadEvents'])
@@ -291,6 +298,7 @@ class Car:
 
     self.initialized_prev = initialized
     self.CS_prev = CS
+    self.CS_SP_prev = CS_SP
 
   def params_thread(self, evt):
     while not evt.is_set():

selfdrive/car/helpers.py

@@ -35,11 +35,13 @@ def asdictref(obj) -> dict[str, Any]:
   return _asdictref_inner(obj)
 
 
-def convert_to_capnp(struct: structs.CarParamsSP) -> capnp.lib.capnp._DynamicStructBuilder:
+def convert_to_capnp(struct: structs.CarParamsSP | structs.CarStateSP) -> capnp.lib.capnp._DynamicStructBuilder:
   struct_dict = asdictref(struct)
 
   if isinstance(struct, structs.CarParamsSP):
     struct_capnp = custom.CarParamsSP.new_message(**struct_dict)
+  elif isinstance(struct, structs.CarStateSP):
+    struct_capnp = custom.CarStateSP.new_message(**struct_dict)
   else:
     raise ValueError(f"Unsupported struct type: {type(struct)}")
 

selfdrive/car/tests/test_models.py

@@ -204,7 +204,7 @@ class TestCarModelBase(unittest.TestCase):
     CC_SP = structs.CarControlSP()
 
     for i, msg in enumerate(self.can_msgs):
-      CS = self.CI.update(msg)
+      CS, _ = self.CI.update(msg)
       self.CI.apply(CC, CC_SP, msg[0])
 
       if CS.canValid:
@@ -349,7 +349,7 @@ class TestCarModelBase(unittest.TestCase):
       self.safety.safety_rx_hook(to_send)
 
       can = [(int(time.monotonic() * 1e9), [CanData(address=address, dat=dat, src=bus)])]
-      CS = self.CI.update(can)
+      CS, _ = self.CI.update(can)
 
       if self.safety.get_gas_pressed_prev() != prev_panda_gas:
         self.assertEqual(CS.gasPressed, self.safety.get_gas_pressed_prev())
@@ -409,7 +409,8 @@ class TestCarModelBase(unittest.TestCase):
     checks = defaultdict(int)
     vehicle_speed_seen = self.CP.steerControlType == SteerControlType.angle and not self.CP.notCar
     for idx, can in enumerate(self.can_msgs):
-      CS = self.CI.update(can).as_reader()
+      CS, _ = self.CI.update(can)
+      CS = CS.as_reader()
       for msg in filter(lambda m: m.src < 64, can[1]):
         to_send = libsafety_py.make_CANPacket(msg.address, msg.src % 4, msg.dat)
         ret = self.safety.safety_rx_hook(to_send)