|
|
@ -49,25 +49,26 @@ class CarController: |
|
|
|
can_sends = [] |
|
|
|
can_sends = [] |
|
|
|
|
|
|
|
|
|
|
|
# Steering (Active: 50Hz, inactive: 10Hz) |
|
|
|
# Steering (Active: 50Hz, inactive: 10Hz) |
|
|
|
# Attempt to sync with camera on startup at 50Hz, first few msgs are blocked |
|
|
|
steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP |
|
|
|
init_lka_counter = not self.sent_lka_steering_cmd |
|
|
|
|
|
|
|
# Also send at 50Hz until we're in sync with camera so counters align when relay closes, preventing a fault |
|
|
|
if self.CP.networkLocation == NetworkLocation.fwdCamera: |
|
|
|
|
|
|
|
# Also send at 50Hz: |
|
|
|
|
|
|
|
# - on startup, first few msgs are blocked |
|
|
|
|
|
|
|
# - until we're in sync with camera so counters align when relay closes, preventing a fault. |
|
|
|
# openpilot can subtly drift, so this is activated throughout a drive to stay synced |
|
|
|
# openpilot can subtly drift, so this is activated throughout a drive to stay synced |
|
|
|
out_of_sync = self.lka_steering_cmd_counter % 4 != (CS.camera_lka_steering_cmd_counter + 1) % 4 |
|
|
|
out_of_sync = self.lka_steering_cmd_counter % 4 != (CS.camera_lka_steering_cmd_counter + 1) % 4 |
|
|
|
sync_steer = (init_lka_counter or out_of_sync) and self.CP.networkLocation == NetworkLocation.fwdCamera |
|
|
|
if not self.sent_lka_steering_cmd or out_of_sync: |
|
|
|
|
|
|
|
|
|
|
|
steer_step = self.params.INACTIVE_STEER_STEP |
|
|
|
|
|
|
|
if CC.latActive or sync_steer: |
|
|
|
|
|
|
|
steer_step = self.params.STEER_STEP |
|
|
|
steer_step = self.params.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: |
|
|
|
if CS.loopback_lka_steering_cmd_updated: |
|
|
|
self.lka_steering_cmd_counter += 1 |
|
|
|
self.lka_steering_cmd_counter += 1 |
|
|
|
self.sent_lka_steering_cmd = True |
|
|
|
self.sent_lka_steering_cmd = True |
|
|
|
elif (self.frame - self.last_steer_frame) >= steer_step: |
|
|
|
|
|
|
|
|
|
|
|
# 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: |
|
|
|
# Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus |
|
|
|
# Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus |
|
|
|
if init_lka_counter: |
|
|
|
if not self.sent_lka_steering_cmd: |
|
|
|
self.lka_steering_cmd_counter = CS.pt_lka_steering_cmd_counter + 1 |
|
|
|
self.lka_steering_cmd_counter = CS.pt_lka_steering_cmd_counter + 1 |
|
|
|
|
|
|
|
|
|
|
|
if CC.latActive: |
|
|
|
if CC.latActive: |
|
|
|