consider controlsd state machine in card: not fully done

selfdrive/car/card.py

@@ -4,7 +4,7 @@ import time
 
 import cereal.messaging as messaging
 
-from cereal import car
+from cereal import car, log
 
 from panda import ALTERNATIVE_EXPERIENCE
 
@@ -20,6 +20,7 @@ from openpilot.selfdrive.controls.lib.events import Events
 
 REPLAY = "REPLAY" in os.environ
 
+State = log.ControlsState.OpenpilotState
 EventName = car.CarEvent.EventName
 ButtonType = car.CarState.ButtonEvent.Type
 
@@ -57,6 +58,9 @@ class Car:
     else:
       self.CI, self.CP = CI, CI.CP
 
+    # read params
+    self.is_metric = self.params.get_bool("IsMetric")
+
     # set alternative experiences from parameters
     self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
     self.CP.alternativeExperience = 0
@@ -115,6 +119,10 @@ class Car:
     if can_rcv_valid and REPLAY:
       self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
 
+    if self.sm['controlsState'].initialized and not self.controlsState_prev.initialized:
+      self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
+      cloudlog.timestamp("Initialized")
+
     return CS
 
   def update_events(self, CS: car.CarState) -> car.CarState:
@@ -135,6 +143,15 @@ class Car:
 
     CS.events = self.events.to_msg()
 
+  def state_transition(self, CS: car.CarState):
+    self.v_cruise_helper.update_v_cruise(CS, self.sm['controlsState'].enabled, self.is_metric)
+
+    controlsState = self.sm['controlsState']
+    if self.controlsState_prev.state == State.disabled:
+      # TODO: use ENABLED_STATES from controlsd? it includes softDisabling which isn't possible here
+      if controlsState.state in (State.preEnabled, State.overriding, State.enabled):
+       self.v_cruise_helper.initialize_v_cruise(CS, controlsState.experimentalMode)
+
   def state_publish(self, CS: car.CarState):
     """carState and carParams publish loop"""
 
@@ -184,14 +201,13 @@ class Car:
 
     self.update_events(CS)
 
+    if not self.CP.passive and self.sm['controlsState'].initialized:
+      self.state_transition(CS)
+
     self.state_publish(CS)
     cloudlog.timestamp("State published")
 
     controlsState = self.sm['controlsState']
-    if controlsState.initialized and not self.controlsState_prev.initialized:
-      self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
-      cloudlog.timestamp("Initialized")
-
     if not self.CP.passive and controlsState.initialized:
       self.controls_update(CS, self.sm['carControl'])
       cloudlog.timestamp("Controls updated")

selfdrive/controls/controlsd.py

@@ -96,9 +96,8 @@ class Controls:
                                   poll='carState',
                                   ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ])
 
-    self.joystick_mode = self.params.get_bool("JoystickDebugMode")
-
     # read params
+    self.joystick_mode = self.params.get_bool("JoystickDebugMode")
     self.is_metric = self.params.get_bool("IsMetric")
     self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
 
@@ -168,10 +167,11 @@ class Controls:
 
   def set_initial_state(self):
     if REPLAY:
-      controls_state = self.params.get("ReplayControlsState")
-      if controls_state is not None:
-        with log.ControlsState.from_bytes(controls_state) as controls_state:
-          self.card.v_cruise_helper.v_cruise_kph = controls_state.vCruise
+      # TODO: move this to card
+      # controls_state = self.params.get("ReplayControlsState")
+      # if controls_state is not None:
+      #   with log.ControlsState.from_bytes(controls_state) as controls_state:
+      #     self.card.v_cruise_helper.v_cruise_kph = controls_state.vCruise
 
       if any(ps.controlsAllowed for ps in self.sm['pandaStates']):
         self.state = State.enabled
@@ -427,8 +427,6 @@ class Controls:
   def state_transition(self, CS):
     """Compute conditional state transitions and execute actions on state transitions"""
 
-    self.card.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric)
-
     # decrement the soft disable timer at every step, as it's reset on
     # entrance in SOFT_DISABLING state
     self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
@@ -502,7 +500,6 @@ class Controls:
           else:
             self.state = State.enabled
           self.current_alert_types.append(ET.ENABLE)
-          self.card.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode)
 
     # Check if openpilot is engaged and actuators are enabled
     self.enabled = self.state in ENABLED_STATES