add vCruise

cereal

@@ -1 +1 @@
-Subproject commit 0cebb30e41c436b023910a7a03a22e601648cb58
+Subproject commit abe9f08fef072f536a53c3685654489ec63e01fa

common/params.cc

@@ -187,7 +187,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"PrimeType", PERSISTENT},
     {"RecordFront", PERSISTENT},
     {"RecordFrontLock", PERSISTENT},  // for the internal fleet
-    {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
+    {"ReplayCarState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"RouteCount", PERSISTENT},
     {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
     {"SshEnabled", PERSISTENT},

selfdrive/car/card.py

@@ -94,6 +94,13 @@ class Car:
     # card is driven by can recv, expected at 100Hz
     self.rk = Ratekeeper(100, print_delay_threshold=None)
 
+  def set_initial_state(self):
+    if REPLAY:
+      controls_state = self.params.get("ReplayCarState")
+      if controls_state is not None:
+        with log.ControlsState.from_bytes(controls_state) as controls_state:
+          self.v_cruise_helper.v_cruise_kph = controls_state.vCruise
+
   def state_update(self) -> car.CarState:
     """carState update loop, driven by can"""
 
@@ -121,6 +128,7 @@ class Car:
 
     if self.sm['controlsState'].initialized and not self.controlsState_prev.initialized:
       self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
+      self.set_initial_state()
       cloudlog.timestamp("Initialized")
 
     return CS
@@ -172,6 +180,8 @@ class Car:
     cs_send = messaging.new_message('carState')
     cs_send.valid = CS.canValid
     cs_send.carState = CS
+    cs_send.carState.vCruise = float(self.v_cruise_helper.v_cruise_kph)
+    cs_send.carState.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph)
     cs_send.carState.canRcvTimeout = self.can_rcv_timeout
     cs_send.carState.canErrorCounter = self.can_rcv_cum_timeout_counter
     cs_send.carState.cumLagMs = -self.rk.remaining * 1000.

selfdrive/controls/controlsd.py

@@ -424,7 +424,7 @@ class Controls:
 
     return CS
 
-  def state_transition(self, CS):
+  def state_transition(self):
     """Compute conditional state transitions and execute actions on state transitions"""
 
     # decrement the soft disable timer at every step, as it's reset on
@@ -555,7 +555,7 @@ class Controls:
 
     if not self.joystick_mode:
       # accel PID loop
-      pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.card.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS)
+      pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
       t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL
       actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
 
@@ -658,7 +658,7 @@ class Controls:
       CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
 
     hudControl = CC.hudControl
-    hudControl.setSpeed = float(self.card.v_cruise_helper.v_cruise_cluster_kph * CV.KPH_TO_MS)
+    hudControl.setSpeed = float(CS.vCruiseCluster * CV.KPH_TO_MS)
     hudControl.speedVisible = self.enabled
     hudControl.lanesVisible = self.enabled
     hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
@@ -741,8 +741,6 @@ class Controls:
     controlsState.engageable = not self.events.contains(ET.NO_ENTRY)
     controlsState.longControlState = self.LoC.long_control_state
     controlsState.vPid = float(self.LoC.v_pid)
-    controlsState.vCruise = float(self.card.v_cruise_helper.v_cruise_kph)
-    controlsState.vCruiseCluster = float(self.card.v_cruise_helper.v_cruise_cluster_kph)
     controlsState.upAccelCmd = float(self.LoC.pid.p)
     controlsState.uiAccelCmd = float(self.LoC.pid.i)
     controlsState.ufAccelCmd = float(self.LoC.pid.f)
@@ -792,7 +790,7 @@ class Controls:
 
     if not self.CP.passive and self.initialized:
       # Update control state
-      self.state_transition(CS)
+      self.state_transition()
     # cloudlog.timestamp("State transitioned")
 
     # Compute actuators (runs PID loops and lateral MPC)