Merge remote-tracking branch 'origin/cp-sp' into cp-sp-model

selfdrive/car/card.py

@@ -75,6 +75,9 @@ class Car:
     self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'])
     self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'])
 
+    sock_services = list(self.pm.sock.keys()) + ['carParamsSP']
+    self.pm = messaging.PubMaster(sock_services)
+
     self.can_rcv_cum_timeout_counter = 0
 
     self.CC_prev = car.CarControl.new_message()
@@ -191,9 +194,6 @@ class Car:
     # log fingerprint in sentry
     interfaces.log_fingerprint(self.CP)
 
-    sock_services = list(self.pm.sock.keys()) + ['carParamsSP']
-    self.pm = messaging.PubMaster(sock_services)
-
   def state_update(self) -> tuple[car.CarState, structs.RadarDataT | None]:
     """carState update loop, driven by can"""
 

selfdrive/controls/controlsd.py

@@ -1,5 +1,4 @@
 #!/usr/bin/env python3
-import os
 import math
 from typing import SupportsFloat
 
@@ -27,7 +26,6 @@ LaneChangeState = log.LaneChangeState
 LaneChangeDirection = log.LaneChangeDirection
 
 ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
-REPLAY = "REPLAY" in os.environ
 
 class Controls:
   def __init__(self) -> None:
@@ -36,12 +34,9 @@ class Controls:
     self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
     cloudlog.info("controlsd got CarParams")
 
-    if not REPLAY:
-      cloudlog.info("controlsd is waiting for CarParamsSP")
-      self.CP_SP = messaging.log_from_bytes(self.params.get("CarParamsSP", block=True), custom.CarParamsSP)
-      cloudlog.info("controlsd got CarParamsSP")
-    else:
-      self.CP_SP = None
+    cloudlog.info("controlsd is waiting for CarParamsSP")
+    self.CP_SP = messaging.log_from_bytes(self.params.get("CarParamsSP", block=True), custom.CarParamsSP)
+    cloudlog.info("controlsd got CarParamsSP")
 
     self.CI = get_car_interface(self.CP, self.CP_SP)
 

selfdrive/selfdrived/selfdrived.py

@@ -60,7 +60,7 @@ class SelfdriveD(CruiseHelper):
     else:
       self.CP = CP
 
-    if CP_SP is None and not REPLAY:
+    if CP_SP is None:
       cloudlog.info("selfdrived is waiting for CarParamsSP")
       self.CP_SP = messaging.log_from_bytes(self.params.get("CarParamsSP", block=True), custom.CarParamsSP)
       cloudlog.info("selfdrived got CarParamsSP")

selfdrive/test/process_replay/process_replay.py

@@ -23,7 +23,7 @@ from openpilot.common.prefix import OpenpilotPrefix
 from openpilot.common.timeout import Timeout
 from openpilot.common.realtime import DT_CTRL
 from panda.python import ALTERNATIVE_EXPERIENCE
-from openpilot.selfdrive.car.card import can_comm_callbacks
+from openpilot.selfdrive.car.card import can_comm_callbacks, convert_to_capnp
 from openpilot.system.manager.process_config import managed_processes
 from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state, available_streams
 from openpilot.selfdrive.test.process_replay.migration import migrate_all
@@ -344,6 +344,7 @@ def get_car_params_callback(rc, pm, msgs, fingerprint):
   if fingerprint:
     CarInterface, _, _, _ = interfaces[fingerprint]
     CP = CarInterface.get_non_essential_params(fingerprint)
+    CP, CP_SP = CarInterface.get_non_essential_params_sp(CP, fingerprint)
   else:
     can = DummySocket()
     sendcan = DummySocket()
@@ -364,12 +365,14 @@ def get_car_params_callback(rc, pm, msgs, fingerprint):
       with car.CarParams.from_bytes(cached_params_raw) as _cached_params:
         cached_params = _cached_params
 
-    CP = get_car(*can_callbacks, lambda obd: None, Params().get_bool("ExperimentalLongitudinalEnabled"), cached_params=cached_params).CP
+    _CI = get_car(*can_callbacks, lambda obd: None, Params().get_bool("ExperimentalLongitudinalEnabled"), cached_params=cached_params)
+    CP, CP_SP = _CI.CP, _CI.CP_SP
 
     if not params.get_bool("DisengageOnAccelerator"):
       CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
 
   params.put("CarParams", CP.to_bytes())
+  params.put("CarParamsSP", convert_to_capnp(CP_SP).to_bytes())
 
 
 def selfdrived_rcv_callback(msg, cfg, frame):