diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 00673a7e28..bcaf6f6f93 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/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 diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index a90c4cfb87..850a3538ad 100644 --- a/selfdrive/car/body/interface.py +++ b/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) diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index ba6aaf8250..20a44bce21 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/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 diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 2b2fde8dc3..657b018c00 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/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) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 00fb461223..99072ae975 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/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 diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 8ce30e5d68..9e1366618c 100644 --- a/selfdrive/car/ford/interface.py +++ b/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) diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 97f8b60e59..73085d30b0 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/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 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index b73b7e7059..3ddec13ced 100644 --- a/selfdrive/car/gm/carstate.py +++ b/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"] diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index f7473d88fc..5ad1776f97 100755 --- a/selfdrive/car/gm/interface.py +++ b/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) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index ab944a30aa..4dc1dc8131 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/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 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 98243d81dd..66c0ce4275 100755 --- a/selfdrive/car/honda/interface.py +++ b/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) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 6f043fddbb..7c31a48ba5 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/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 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index d3a29dc0cb..b746680930 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/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) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index e75067da7e..249818369c 100644 --- a/selfdrive/car/interfaces.py +++ b/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, diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 027822cc3f..524a02a370 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/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 diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 65444ff6e0..2930b002d4 100755 --- a/selfdrive/car/mazda/interface.py +++ b/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) diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 3ac487dbb7..13210c86d5 100755 --- a/selfdrive/car/mock/interface.py +++ b/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, [] diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index ff13812398..45c3dd720c 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/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 diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 2e769cf662..074cd1cc57 100644 --- a/selfdrive/car/nissan/interface.py +++ b/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) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index a56e63408e..24d85877d7 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/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 diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index f94333baab..733482ef82 100644 --- a/selfdrive/car/subaru/interface.py +++ b/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) diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 6e2869d1c2..1f18c3d0ea 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/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 diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 6a73472108..70d49896cb 100755 --- a/selfdrive/car/tesla/interface.py +++ b/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) diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 24c995a2d0..78ecbe425e 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/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 diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 4627b9a570..6fbe1436f1 100755 --- a/selfdrive/car/tests/test_models.py +++ b/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 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 222cf70613..2e0d7009c8 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/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 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 438a9c11fc..a09b0e5265 100644 --- a/selfdrive/car/toyota/interface.py +++ b/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) diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 4b19f4d13c..5d00b5a52f 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/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 = [] diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 1e620b4f73..521c68184d 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/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) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index fff6bcf576..1bb5be9a56 100755 --- a/selfdrive/controls/controlsd.py +++ b/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: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 8a75a72483..d334792c48 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -35a3dbcbcd8504388ea2a70965be0b4e0869b06a +9bfd30202a9e70d9d7459cc02f86c3e55d7c864c