diff --git a/cereal b/cereal index 0cebb30e41..abe9f08fef 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 0cebb30e41c436b023910a7a03a22e601648cb58 +Subproject commit abe9f08fef072f536a53c3685654489ec63e01fa diff --git a/common/params.cc b/common/params.cc index 2330160173..0ce1263b37 100644 --- a/common/params.cc +++ b/common/params.cc @@ -187,7 +187,7 @@ std::unordered_map 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}, diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 6308667fae..f74aaaca69 100755 --- a/selfdrive/car/card.py +++ b/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. diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4895bf12d8..7e54db16f2 100755 --- a/selfdrive/controls/controlsd.py +++ b/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)