diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index b56b3bfca6..a75eb89f25 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -22,6 +22,7 @@ class CarController: self.apply_gas = 0 self.apply_brake = 0 self.frame = 0 + self.last_steer_frame = 0 self.last_button_frame = 0 self.cancel_counter = 0 @@ -46,15 +47,21 @@ class CarController: # Send CAN commands. can_sends = [] - # Steering (50Hz) + # Steering (Active: 50Hz, inactive: 10Hz) + # Attempt to sync with camera on startup at 50Hz, first few msgs are blocked + init_lka_counter = not self.sent_lka_steering_cmd and self.CP.networkLocation == NetworkLocation.fwdCamera + steer_step = self.params.INACTIVE_STEER_STEP + if CC.latActive or init_lka_counter: + steer_step = self.params.ACTIVE_STEER_STEP + # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the # next Panda loopback confirmation in the current CS frame. if CS.loopback_lka_steering_cmd_updated: self.lka_steering_cmd_counter += 1 self.sent_lka_steering_cmd = True - elif (self.frame % self.params.STEER_STEP) == 0: - # Initialize ASCMLKASteeringCmd counter using the camera - if not self.sent_lka_steering_cmd and self.CP.networkLocation == NetworkLocation.fwdCamera: + elif (self.frame - self.last_steer_frame) >= steer_step: + # Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus + if init_lka_counter: self.lka_steering_cmd_counter = CS.camera_lka_steering_cmd_counter + 1 if CC.latActive: @@ -63,6 +70,7 @@ class CarController: else: apply_steer = 0 + self.last_steer_frame = self.frame self.apply_steer_last = apply_steer idx = self.lka_steering_cmd_counter % 4 can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, CC.latActive)) diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index f8f9e7f043..85e291aaf6 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -11,7 +11,8 @@ Ecu = car.CarParams.Ecu class CarControllerParams: STEER_MAX = 300 # GM limit is 3Nm. Used by carcontroller to generate LKA output - STEER_STEP = 2 # Control frames per command (50hz) + ACTIVE_STEER_STEP = 2 # Active control frames per command (50hz) + INACTIVE_STEER_STEP = 10 # Inactive control frames per command (10hz) STEER_DELTA_UP = 7 # Delta rates require review due to observed EPS weakness STEER_DELTA_DOWN = 17 STEER_DRIVER_ALLOWANCE = 50 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index d03b897f9d..604abd666c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -a5c77655fba5563e8128fb655f69b1bbd02198aa +9d042d06f61c8f48e8c7a4e30001630c14712272 \ No newline at end of file