GM: enforce steering command message timing (#27250)

* draft

* bump opendbc

* still draft

* that's not right

* superset of the changes, 33hz

* cleanup

* this should work

* remove line

* pass it in again

* actually no need to check updated now

* now_nanos

* consistent name

* fix replay

* one line isn't that bad

switch

switch

* fix CarController tests

* Update ref_commit
old-commit-hash: 8f5057ff2d

selfdrive/car/body/carcontroller.py

@@ -35,7 +35,7 @@ class CarController:
       torque -= deadband
     return torque
 
-  def update(self, CC, CS):
+  def update(self, CC, CS, now_nanos):
 
     torque_l = 0
     torque_r = 0

selfdrive/car/body/interface.py

@@ -43,5 +43,5 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
-  def apply(self, c):
-    return self.CC.update(c, self.CS)
+  def apply(self, c, now_nanos):
+    return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/chrysler/carcontroller.py

@@ -19,7 +19,7 @@ class CarController:
     self.packer = CANPacker(dbc_name)
     self.params = CarControllerParams(CP)
 
-  def update(self, CC, CS):
+  def update(self, CC, CS, now_nanos):
     can_sends = []
 
     lkas_active = CC.latActive and self.lkas_control_bit_prev

selfdrive/car/chrysler/interface.py

@@ -106,5 +106,5 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
-  def apply(self, c):
-    return self.CC.update(c, self.CS)
+  def apply(self, c, now_nanos):
+    return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/ford/carcontroller.py

@@ -21,7 +21,7 @@ class CarController:
     self.lkas_enabled_last = False
     self.steer_alert_last = False
 
-  def update(self, CC, CS):
+  def update(self, CC, CS, now_nanos):
     can_sends = []
 
     actuators = CC.actuators

selfdrive/car/ford/interface.py

@@ -78,5 +78,5 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
-  def apply(self, c):
-    return self.CC.update(c, self.CS)
+  def apply(self, c, now_nanos):
+    return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/gm/carcontroller.py

@@ -13,6 +13,8 @@ LongCtrlState = car.CarControl.Actuators.LongControlState
 
 # Camera cancels up to 0.1s after brake is pressed, ECM allows 0.5s
 CAMERA_CANCEL_DELAY_FRAMES = 10
+# Enforce a minimum interval between steering messages to avoid a fault
+MIN_STEER_MSG_INTERVAL_MS = 15
 
 
 class CarController:
@@ -37,7 +39,7 @@ class CarController:
     self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar'])
     self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis'])
 
-  def update(self, CC, CS):
+  def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
     hud_control = CC.hudControl
     hud_alert = hud_control.visualAlert
@@ -64,9 +66,10 @@ class CarController:
       self.lka_steering_cmd_counter += 1
       self.sent_lka_steering_cmd = True
 
-    # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just
-    # received the ASCMLKASteeringCmd loopback confirmation in the current CS frame
-    if (self.frame - self.last_steer_frame) >= steer_step and not CS.loopback_lka_steering_cmd_updated:
+    # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we
+    # received the ASCMLKASteeringCmd loopback confirmation too recently
+    last_lka_steer_msg_ms = (now_nanos - CS.loopback_lka_steering_cmd_ts_nanos) * 1e-6
+    if (self.frame - self.last_steer_frame) >= steer_step and last_lka_steer_msg_ms > MIN_STEER_MSG_INTERVAL_MS:
       # Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus
       if not self.sent_lka_steering_cmd:
         self.lka_steering_cmd_counter = CS.pt_lka_steering_cmd_counter + 1

selfdrive/car/gm/carstate.py

@@ -18,6 +18,7 @@ class CarState(CarStateBase):
     can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
     self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"]
     self.loopback_lka_steering_cmd_updated = False
+    self.loopback_lka_steering_cmd_ts_nanos = 0
     self.pt_lka_steering_cmd_counter = 0
     self.cam_lka_steering_cmd_counter = 0
     self.buttons_counter = 0
@@ -33,6 +34,7 @@ class CarState(CarStateBase):
 
     # Variables used for avoiding LKAS faults
     self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0
+    self.loopback_lka_steering_cmd_ts_nanos = loopback_cp.ts_nanos["ASCMLKASteeringCmd"]["RollingCounter"]
     if self.CP.networkLocation == NetworkLocation.fwdCamera:
       self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
       self.cam_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]

selfdrive/car/gm/interface.py

@@ -236,5 +236,5 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
-  def apply(self, c):
-    return self.CC.update(c, self.CS)
+  def apply(self, c, now_nanos):
+    return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/honda/carcontroller.py

@@ -124,7 +124,7 @@ class CarController:
     self.brake = 0.0
     self.last_steer = 0.0
 
-  def update(self, CC, CS):
+  def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
     hud_control = CC.hudControl
     hud_v_cruise = hud_control.setSpeed * CV.MS_TO_KPH if hud_control.speedVisible else 255

selfdrive/car/honda/interface.py

@@ -348,5 +348,5 @@ class CarInterface(CarInterfaceBase):
 
   # pass in a car.CarControl
   # to be called @ 100hz
-  def apply(self, c):
-    return self.CC.update(c, self.CS)
+  def apply(self, c, now_nanos):
+    return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/hyundai/carcontroller.py

@@ -54,7 +54,7 @@ class CarController:
     self.car_fingerprint = CP.carFingerprint
     self.last_button_frame = 0
 
-  def update(self, CC, CS):
+  def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
     hud_control = CC.hudControl
 

selfdrive/car/hyundai/interface.py

@@ -330,5 +330,5 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
-  def apply(self, c):
-    return self.CC.update(c, self.CS)
+  def apply(self, c, now_nanos):
+    return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/interfaces.py

@@ -233,7 +233,7 @@ class CarInterfaceBase(ABC):
     return reader
 
   @abstractmethod
-  def apply(self, c: car.CarControl) -> Tuple[car.CarControl.Actuators, List[bytes]]:
+  def apply(self, c: car.CarControl, now_nanos: int) -> Tuple[car.CarControl.Actuators, List[bytes]]:
     pass
 
   def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True,

selfdrive/car/mazda/carcontroller.py

@@ -15,7 +15,7 @@ class CarController:
     self.brake_counter = 0
     self.frame = 0
 
-  def update(self, CC, CS):
+  def update(self, CC, CS, now_nanos):
     can_sends = []
 
     apply_steer = 0

selfdrive/car/mazda/interface.py

@@ -69,5 +69,5 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
-  def apply(self, c):
-    return self.CC.update(c, self.CS)
+  def apply(self, c, now_nanos):
+    return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/mock/interface.py

@@ -57,7 +57,7 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
-  def apply(self, c):
+  def apply(self, c, now_nanos):
     # in mock no carcontrols
     actuators = car.CarControl.Actuators.new_message()
     return actuators, []

selfdrive/car/nissan/carcontroller.py

@@ -18,7 +18,7 @@ class CarController:
 
     self.packer = CANPacker(dbc_name)
 
-  def update(self, CC, CS):
+  def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel

selfdrive/car/nissan/interface.py

@@ -56,5 +56,5 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
-  def apply(self, c):
-    return self.CC.update(c, self.CS)
+  def apply(self, c, now_nanos):
+    return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/subaru/carcontroller.py

@@ -19,7 +19,7 @@ class CarController:
     self.p = CarControllerParams(CP)
     self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
 
-  def update(self, CC, CS):
+  def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel

selfdrive/car/subaru/interface.py

@@ -112,5 +112,5 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
-  def apply(self, c):
-    return self.CC.update(c, self.CS)
+  def apply(self, c, now_nanos):
+    return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/tesla/carcontroller.py

@@ -14,7 +14,7 @@ class CarController:
     self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt'])
     self.tesla_can = TeslaCAN(self.packer, self.pt_packer)
 
-  def update(self, CC, CS):
+  def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
     pcm_cancel_cmd = CC.cruiseControl.cancel
 

selfdrive/car/tesla/interface.py

@@ -58,5 +58,5 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
-  def apply(self, c):
-    return self.CC.update(c, self.CS)
+  def apply(self, c, now_nanos):
+    return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/tests/test_car_interfaces.py

@@ -59,15 +59,15 @@ class TestCarInterfaces(unittest.TestCase):
     CC = car.CarControl.new_message()
     for _ in range(10):
       car_interface.update(CC, [])
-      car_interface.apply(CC)
-      car_interface.apply(CC)
+      car_interface.apply(CC, 0)
+      car_interface.apply(CC, 0)
 
     CC = car.CarControl.new_message()
     CC.enabled = True
     for _ in range(10):
       car_interface.update(CC, [])
-      car_interface.apply(CC)
-      car_interface.apply(CC)
+      car_interface.apply(CC, 0)
+      car_interface.apply(CC, 0)
 
     # Test radar interface
     RadarInterface = importlib.import_module(f'selfdrive.car.{car_params.carName}.radar_interface').RadarInterface

selfdrive/car/tests/test_models.py

@@ -149,7 +149,7 @@ class TestCarModelBase(unittest.TestCase):
 
     for i, msg in enumerate(self.can_msgs):
       CS = self.CI.update(CC, (msg.as_builder().to_bytes(),))
-      self.CI.apply(CC)
+      self.CI.apply(CC, msg.logMonoTime)
 
       if CS.canValid:
         can_valid = True

selfdrive/car/toyota/carcontroller.py

@@ -34,7 +34,7 @@ class CarController:
     self.gas = 0
     self.accel = 0
 
-  def update(self, CC, CS):
+  def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel

selfdrive/car/toyota/interface.py

@@ -263,5 +263,5 @@ class CarInterface(CarInterfaceBase):
 
   # pass in a car.CarControl
   # to be called @ 100hz
-  def apply(self, c):
-    return self.CC.update(c, self.CS)
+  def apply(self, c, now_nanos):
+    return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/volkswagen/carcontroller.py

@@ -24,7 +24,7 @@ class CarController:
     self.hcaSameTorqueCount = 0
     self.hcaEnabledFrameCount = 0
 
-  def update(self, CC, CS, ext_bus):
+  def update(self, CC, CS, ext_bus, now_nanos):
     actuators = CC.actuators
     hud_control = CC.hudControl
     can_sends = []

selfdrive/car/volkswagen/interface.py

@@ -244,5 +244,5 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
-  def apply(self, c):
-    return self.CC.update(c, self.CS, self.ext_bus)
+  def apply(self, c, now_nanos):
+    return self.CC.update(c, self.CS, self.ext_bus, now_nanos)

selfdrive/controls/controlsd.py

@@ -187,6 +187,7 @@ class Controls:
 
     # TODO: no longer necessary, aside from process replay
     self.sm['liveParameters'].valid = True
+    self.can_log_mono_time = 0
 
     self.startup_event = get_startup_event(car_recognized, controller_available, len(self.CP.carFw) > 0)
 
@@ -425,6 +426,8 @@ class Controls:
     # Update carState from CAN
     can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
     CS = self.CI.update(self.CC, can_strs)
+    if len(can_strs) and REPLAY:
+      self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
 
     self.sm.update(0)
 
@@ -731,7 +734,8 @@ class Controls:
 
     if not self.read_only and self.initialized:
       # send car controls over can
-      self.last_actuators, can_sends = self.CI.apply(CC)
+      now_nanos = self.can_log_mono_time if REPLAY else int(sec_since_boot() * 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))
       CC.actuatorsOutput = self.last_actuators
       if self.CP.steerControlType == car.CarParams.SteerControlType.angle:

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-35a3dbcbcd8504388ea2a70965be0b4e0869b06a
+9bfd30202a9e70d9d7459cc02f86c3e55d7c864c