card: process that abstracts car interface and CAN (#32380)

* format card

* standalone process

* no class member CS, there's no point

also can be confusing; what else could be using this?

* rename CoS

* Update selfdrive/controls/controlsd.py

* never works first time :D

* canRcvTimeout is bool

* hack

* add cpu

* see what testing closet comes up with

* first

* some clean up

* support passable CI, fix test models

* fix startup alert

* process replay changes

* test_fuzzy

* gate carOutput valid on carControl valid

* we should publish after we update carOutput

* controlsd was using actuatorsOutput from 2 frames ago for torque, not the most up to date

* check all checks for carControl in case controlsd dies

* log more timestamps

* more generic latency logger; needs some clean up

latency_logger.py was difficult to understand and modify

* card polls on can and carControl to get latest carControl possible

* temp try to send earlier

* add log

* remove latencylogger

* no mpld3!

* old loop

* detect first event

* normal send

* revert "card polls on can and carControl to get latest carControl possible"

how it was is best

* sheesh! update should be first

* first timestamp

* temp comment ( timestamp is slow :( )

* more final ordering, and make polling on/off test repeatable

* Received can

* new plot timestamps

* clean up

* no poll

* add controllers (draft)

* Revert "add controllers (draft)"

This reverts commit e2c3f01.

* fix that

* conventions

* just use CS

* consider controlsd state machine in card: not fully done

* hmm it's just becoming controlsd

* rm debugging

* Revert "hmm it's just becoming controlsd"

This reverts commit 534a357.

* Revert "just use CS"

This reverts commit 9fa7406.

* add vCruise

* migrate car state

* Revert "migrate car state"

This reverts commit 4ae86ca.

* Revert "add vCruise"

This reverts commit af247a8.

* simple state machine in card (doesn't work as is)

* Revert "simple state machine in card (doesn't work as is)"

This reverts commit b4af8a9.

* poll carState without conflate

* bump

* remove state transition

* fix

* update refs

* ignore cumLagMs and don't ignore valid

* fix controls mismatch; controlsd used to set alt exp

* controlsd_config_callback not needed for card

* revert ref temp

* update refs

* no poll

* not builder!

* test fix

* need to migrate initialized

* CC will be a reader

* more as_reader!

* fix None

* init after publish like before - no real difference

* controlsd clean up

* remove redundant check and check passive for init

* stash

* flip

* migrate missing carOutput for controlsd

* Update ref_commit

* bump cereal

* comment

* no class params

* no class

* Revert "no class"

This reverts commit 5499b83.

* add todo

* regen and update refs

* fix

* update refs

* and fix that

* should be controlsstate

* remove controlsState migration

CoS.initialized isn't needed yet

* fix

* flip!

* bump

* fix that

* update refs

* fix

* if canValid goes false, controlsd would still send

* bump

* rm diff

* need to be very careful with initializing

* update refs

selfdrive/car/body/carcontroller.py

@@ -75,7 +75,7 @@ def update(self, CC, CS, now_nanos):
     can_sends = []
     can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r))
 
-    new_actuators = CC.actuators.copy()
+    new_actuators = CC.actuators.as_builder()
     new_actuators.accel = torque_l
     new_actuators.steer = torque_r
     new_actuators.steerOutputCan = torque_r

selfdrive/car/card.py

@@ -9,7 +9,7 @@
 from panda import ALTERNATIVE_EXPERIENCE
 
 from openpilot.common.params import Params
-from openpilot.common.realtime import DT_CTRL
+from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL
 
 from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
 from openpilot.selfdrive.car.car_helpers import get_car, get_one_can
@@ -21,41 +21,43 @@
 EventName = car.CarEvent.EventName
 
 
-class CarD:
+class Car:
   CI: CarInterfaceBase
 
   def __init__(self, CI=None):
     self.can_sock = messaging.sub_sock('can', timeout=20)
-    self.sm = messaging.SubMaster(['pandaStates'])
+    self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'])
     self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput'])
 
     self.can_rcv_timeout_counter = 0  # consecutive timeout count
     self.can_rcv_cum_timeout_counter = 0  # cumulative timeout count
 
     self.CC_prev = car.CarControl.new_message()
+    self.CS_prev = car.CarState.new_message()
+    self.initialized_prev = False
 
-    self.last_actuators = None
+    self.last_actuators_output = car.CarControl.Actuators.new_message()
 
-    self.params = Params()
+    params = Params()
 
     if CI is None:
       # wait for one pandaState and one CAN packet
       print("Waiting for CAN messages...")
       get_one_can(self.can_sock)
 
       num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates)
-      experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled")
+      experimental_long_allowed = params.get_bool("ExperimentalLongitudinalEnabled")
       self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas)
     else:
       self.CI, self.CP = CI, CI.CP
 
     # set alternative experiences from parameters
-    self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
+    self.disengage_on_accelerator = params.get_bool("DisengageOnAccelerator")
     self.CP.alternativeExperience = 0
     if not self.disengage_on_accelerator:
       self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
 
-    openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle")
+    openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
 
     controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly
 
@@ -66,22 +68,20 @@ def __init__(self, CI=None):
       self.CP.safetyConfigs = [safety_config]
 
     # Write previous route's CarParams
-    prev_cp = self.params.get("CarParamsPersistent")
+    prev_cp = params.get("CarParamsPersistent")
     if prev_cp is not None:
-      self.params.put("CarParamsPrevRoute", prev_cp)
+      params.put("CarParamsPrevRoute", prev_cp)
 
     # Write CarParams for controls and radard
     cp_bytes = self.CP.to_bytes()
-    self.params.put("CarParams", cp_bytes)
-    self.params.put_nonblocking("CarParamsCache", cp_bytes)
-    self.params.put_nonblocking("CarParamsPersistent", cp_bytes)
+    params.put("CarParams", cp_bytes)
+    params.put_nonblocking("CarParamsCache", cp_bytes)
+    params.put_nonblocking("CarParamsPersistent", cp_bytes)
 
-    self.CS_prev = car.CarState.new_message()
     self.events = Events()
 
-  def initialize(self):
-    """Initialize CarInterface, once controls are ready"""
-    self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
+    # card is driven by can recv, expected at 100Hz
+    self.rk = Ratekeeper(100, print_delay_threshold=None)
 
   def state_update(self) -> car.CarState:
     """carState update loop, driven by can"""
@@ -106,11 +106,6 @@ def state_update(self) -> car.CarState:
     if can_rcv_valid and REPLAY:
       self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
 
-    self.update_events(CS)
-    self.state_publish(CS)
-
-    CS = CS.as_reader()
-    self.CS_prev = CS
     return CS
 
   def update_events(self, CS: car.CarState) -> car.CarState:
@@ -129,12 +124,6 @@ def update_events(self, CS: car.CarState) -> car.CarState:
   def state_publish(self, CS: car.CarState):
     """carState and carParams publish loop"""
 
-    # carState
-    cs_send = messaging.new_message('carState')
-    cs_send.valid = CS.canValid
-    cs_send.carState = CS
-    self.pm.send('carState', cs_send)
-
     # carParams - logged every 50 seconds (> 1 per segment)
     if self.sm.frame % int(50. / DT_CTRL) == 0:
       cp_send = messaging.new_message('carParams')
@@ -144,17 +133,60 @@ def state_publish(self, CS: car.CarState):
 
     # publish new carOutput
     co_send = messaging.new_message('carOutput')
-    co_send.valid = True
-    if self.last_actuators is not None:
-      co_send.carOutput.actuatorsOutput = self.last_actuators
+    co_send.valid = self.sm.all_checks(['carControl'])
+    co_send.carOutput.actuatorsOutput = self.last_actuators_output
     self.pm.send('carOutput', co_send)
 
+    # kick off controlsd step while we actuate the latest carControl packet
+    cs_send = messaging.new_message('carState')
+    cs_send.valid = CS.canValid
+    cs_send.carState = CS
+    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.
+    self.pm.send('carState', cs_send)
+
   def controls_update(self, CS: car.CarState, CC: car.CarControl):
     """control update loop, driven by carControl"""
 
-    # send car controls over can
-    now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
-    self.last_actuators, can_sends = self.CI.apply(CC, now_nanos)
-    self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
+    if not self.initialized_prev:
+      # Initialize CarInterface, once controls are ready
+      self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
+
+    if self.sm.all_alive(['carControl']):
+      # send car controls over can
+      now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
+      self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos)
+      self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
+
+      self.CC_prev = CC
+
+  def step(self):
+    CS = self.state_update()
+
+    self.update_events(CS)
+
+    self.state_publish(CS)
+
+    initialized = (not any(e.name == EventName.controlsInitializing for e in self.sm['onroadEvents']) and
+                   self.sm.seen['onroadEvents'])
+    if not self.CP.passive and initialized:
+      self.controls_update(CS, self.sm['carControl'])
+
+    self.initialized_prev = initialized
+    self.CS_prev = CS.as_reader()
+
+  def card_thread(self):
+    while True:
+      self.step()
+      self.rk.monitor_time()
+
+
+def main():
+  config_realtime_process(4, Priority.CTRL_HIGH)
+  car = Car()
+  car.card_thread()
+
 
-    self.CC_prev = CC
+if __name__ == "__main__":
+  main()

selfdrive/car/chrysler/carcontroller.py

@@ -78,7 +78,7 @@ def update(self, CC, CS, now_nanos):
 
     self.frame += 1
 
-    new_actuators = CC.actuators.copy()
+    new_actuators = CC.actuators.as_builder()
     new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
     new_actuators.steerOutputCan = self.apply_steer_last
 

selfdrive/car/ford/carcontroller.py

@@ -113,7 +113,7 @@ def update(self, CC, CS, now_nanos):
     self.steer_alert_last = steer_alert
     self.lead_distance_bars_last = hud_control.leadDistanceBars
 
-    new_actuators = actuators.copy()
+    new_actuators = actuators.as_builder()
     new_actuators.curvature = self.apply_curvature_last
 
     self.frame += 1

selfdrive/car/gm/carcontroller.py

@@ -155,7 +155,7 @@ def update(self, CC, CS, now_nanos):
       if self.frame % 10 == 0:
         can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status))
 
-    new_actuators = actuators.copy()
+    new_actuators = actuators.as_builder()
     new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
     new_actuators.steerOutputCan = self.apply_steer_last
     new_actuators.gas = self.apply_gas

selfdrive/car/honda/carcontroller.py

@@ -244,7 +244,7 @@ def update(self, CC, CS, now_nanos):
         self.speed = pcm_speed
         self.gas = pcm_accel / self.params.NIDEC_GAS_MAX
 
-    new_actuators = actuators.copy()
+    new_actuators = actuators.as_builder()
     new_actuators.speed = self.speed
     new_actuators.accel = self.accel
     new_actuators.gas = self.gas

selfdrive/car/hyundai/carcontroller.py

@@ -163,7 +163,7 @@ def update(self, CC, CS, now_nanos):
       if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
         can_sends.append(hyundaican.create_frt_radar_opt(self.packer))
 
-    new_actuators = actuators.copy()
+    new_actuators = actuators.as_builder()
     new_actuators.steer = apply_steer / self.params.STEER_MAX
     new_actuators.steerOutputCan = apply_steer
     new_actuators.accel = accel

selfdrive/car/mazda/carcontroller.py

@@ -58,7 +58,7 @@ def update(self, CC, CS, now_nanos):
     can_sends.append(mazdacan.create_steering_control(self.packer, self.CP,
                                                       self.frame, apply_steer, CS.cam_lkas))
 
-    new_actuators = CC.actuators.copy()
+    new_actuators = CC.actuators.as_builder()
     new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
     new_actuators.steerOutputCan = apply_steer
 

selfdrive/car/mock/carcontroller.py

@@ -2,4 +2,4 @@
 
 class CarController(CarControllerBase):
   def update(self, CC, CS, now_nanos):
-    return CC.actuators.copy(), []
+    return CC.actuators.as_builder(), []

selfdrive/car/nissan/carcontroller.py

@@ -75,7 +75,7 @@ def update(self, CC, CS, now_nanos):
           self.packer, CS.lkas_hud_info_msg, steer_hud_alert
         ))
 
-    new_actuators = actuators.copy()
+    new_actuators = actuators.as_builder()
     new_actuators.steeringAngleDeg = apply_angle
 
     self.frame += 1

selfdrive/car/subaru/carcontroller.py

@@ -136,7 +136,7 @@ def update(self, CC, CS, now_nanos):
         if self.frame % 2 == 0:
           can_sends.append(subarucan.create_es_static_2(self.packer))
 
-    new_actuators = actuators.copy()
+    new_actuators = actuators.as_builder()
     new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
     new_actuators.steerOutputCan = self.apply_steer_last
 

selfdrive/car/tesla/carcontroller.py

@@ -60,7 +60,7 @@ def update(self, CC, CS, now_nanos):
 
     # TODO: HUD control
 
-    new_actuators = actuators.copy()
+    new_actuators = actuators.as_builder()
     new_actuators.steeringAngleDeg = self.apply_angle_last
 
     self.frame += 1

selfdrive/car/tests/test_car_interfaces.py

@@ -91,14 +91,14 @@ def test_car_interfaces(self, car_name, data):
     CC = car.CarControl.new_message(**cc_msg)
     for _ in range(10):
       car_interface.update(CC, [])
-      car_interface.apply(CC, now_nanos)
+      car_interface.apply(CC.as_reader(), now_nanos)
       now_nanos += DT_CTRL * 1e9  # 10 ms
 
     CC = car.CarControl.new_message(**cc_msg)
     CC.enabled = True
     for _ in range(10):
       car_interface.update(CC, [])
-      car_interface.apply(CC, now_nanos)
+      car_interface.apply(CC.as_reader(), now_nanos)
       now_nanos += DT_CTRL * 1e9  # 10ms
 
     # Test controller initialization

selfdrive/car/tests/test_models.py

@@ -15,7 +15,7 @@
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.car import gen_empty_fingerprint
-from openpilot.selfdrive.car.card import CarD
+from openpilot.selfdrive.car.card import Car
 from openpilot.selfdrive.car.fingerprints import all_known_cars, MIGRATION
 from openpilot.selfdrive.car.car_helpers import FRAME_FINGERPRINT, interfaces
 from openpilot.selfdrive.car.honda.values import CAR as HONDA, HondaFlags
@@ -215,7 +215,7 @@ def test_car_interface(self):
     # TODO: also check for checksum violations from can parser
     can_invalid_cnt = 0
     can_valid = False
-    CC = car.CarControl.new_message()
+    CC = car.CarControl.new_message().as_reader()
 
     for i, msg in enumerate(self.can_msgs):
       CS = self.CI.update(CC, (msg.as_builder().to_bytes(),))
@@ -308,17 +308,17 @@ def test_car_controller(car_control):
 
     # Make sure we can send all messages while inactive
     CC = car.CarControl.new_message()
-    test_car_controller(CC)
+    test_car_controller(CC.as_reader())
 
     # Test cancel + general messages (controls_allowed=False & cruise_engaged=True)
     self.safety.set_cruise_engaged_prev(True)
     CC = car.CarControl.new_message(cruiseControl={'cancel': True})
-    test_car_controller(CC)
+    test_car_controller(CC.as_reader())
 
     # Test resume + general messages (controls_allowed=True & cruise_engaged=True)
     self.safety.set_controls_allowed(True)
     CC = car.CarControl.new_message(cruiseControl={'resume': True})
-    test_car_controller(CC)
+    test_car_controller(CC.as_reader())
 
   # Skip stdout/stderr capture with pytest, causes elevated memory usage
   @pytest.mark.nocapture
@@ -406,7 +406,7 @@ def test_panda_safety_carstate(self):
     controls_allowed_prev = False
     CS_prev = car.CarState.new_message()
     checks = defaultdict(int)
-    card = CarD(CI=self.CI)
+    card = Car(CI=self.CI)
     for idx, can in enumerate(self.can_msgs):
       CS = self.CI.update(CC, (can.as_builder().to_bytes(), ))
       for msg in filter(lambda m: m.src in range(64), can.can):

selfdrive/car/toyota/carcontroller.py

@@ -168,7 +168,7 @@ def update(self, CC, CS, now_nanos):
     if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
       can_sends.append([0x750, 0, b"\x0F\x02\x3E\x00\x00\x00\x00\x00", 0])
 
-    new_actuators = actuators.copy()
+    new_actuators = actuators.as_builder()
     new_actuators.steer = apply_steer / self.params.STEER_MAX
     new_actuators.steerOutputCan = apply_steer
     new_actuators.steeringAngleDeg = self.last_angle

selfdrive/car/volkswagen/carcontroller.py

@@ -112,7 +112,7 @@ def update(self, CC, CS, now_nanos):
       can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, self.ext_bus, CS.gra_stock_values,
                                                            cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume))
 
-    new_actuators = actuators.copy()
+    new_actuators = actuators.as_builder()
     new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX
     new_actuators.steerOutputCan = self.apply_steer_last