From 8f5057ff2dbea4ebc85bc8f55bc7a37e4d5969b6 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 9 Feb 2023 15:37:39 -0800 Subject: [PATCH] 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 --- selfdrive/car/body/carcontroller.py | 2 +- selfdrive/car/body/interface.py | 4 ++-- selfdrive/car/chrysler/carcontroller.py | 2 +- selfdrive/car/chrysler/interface.py | 4 ++-- selfdrive/car/ford/carcontroller.py | 2 +- selfdrive/car/ford/interface.py | 4 ++-- selfdrive/car/gm/carcontroller.py | 11 +++++++---- selfdrive/car/gm/carstate.py | 2 ++ selfdrive/car/gm/interface.py | 4 ++-- selfdrive/car/honda/carcontroller.py | 2 +- selfdrive/car/honda/interface.py | 4 ++-- selfdrive/car/hyundai/carcontroller.py | 2 +- selfdrive/car/hyundai/interface.py | 4 ++-- selfdrive/car/interfaces.py | 2 +- selfdrive/car/mazda/carcontroller.py | 2 +- selfdrive/car/mazda/interface.py | 4 ++-- selfdrive/car/mock/interface.py | 2 +- selfdrive/car/nissan/carcontroller.py | 2 +- selfdrive/car/nissan/interface.py | 4 ++-- selfdrive/car/subaru/carcontroller.py | 2 +- selfdrive/car/subaru/interface.py | 4 ++-- selfdrive/car/tesla/carcontroller.py | 2 +- selfdrive/car/tesla/interface.py | 4 ++-- selfdrive/car/tests/test_car_interfaces.py | 8 ++++---- selfdrive/car/tests/test_models.py | 2 +- selfdrive/car/toyota/carcontroller.py | 2 +- selfdrive/car/toyota/interface.py | 4 ++-- selfdrive/car/volkswagen/carcontroller.py | 2 +- selfdrive/car/volkswagen/interface.py | 4 ++-- selfdrive/controls/controlsd.py | 6 +++++- selfdrive/test/process_replay/ref_commit | 2 +- 31 files changed, 57 insertions(+), 48 deletions(-) diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 00673a7e2..bcaf6f6f9 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 a90c4cfb8..850a3538a 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 ba6aaf825..20a44bce2 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 2b2fde8dc..657b018c0 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 00fb46122..99072ae97 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 8ce30e5d6..9e1366618 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 97f8b60e5..73085d30b 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 b73b7e705..3ddec13ce 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 f7473d88f..5ad1776f9 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 ab944a30a..4dc1dc813 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 98243d81d..66c0ce427 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 6f043fddb..7c31a48ba 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 d3a29dc0c..b74668093 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 e75067da7..249818369 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 027822cc3..524a02a37 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 65444ff6e..2930b002d 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 3ac487dbb..13210c86d 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 ff1381239..45c3dd720 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 2e769cf66..074cd1cc5 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 a56e63408..24d85877d 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 f94333baa..733482ef8 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 6e2869d1c..1f18c3d0e 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 6a7347210..70d49896c 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 24c995a2d..78ecbe425 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 4627b9a57..6fbe1436f 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 222cf7061..2e0d7009c 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 438a9c11f..a09b0e526 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 4b19f4d13..5d00b5a52 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 1e620b4f7..521c68184 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 fff6bcf57..1bb5be9a5 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 8a75a7248..d334792c4 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -35a3dbcbcd8504388ea2a70965be0b4e0869b06a +9bfd30202a9e70d9d7459cc02f86c3e55d7c864c