|
|
@ -44,11 +44,11 @@ class CarController(): |
|
|
|
self.apply_steer_last = 0 |
|
|
|
self.apply_steer_last = 0 |
|
|
|
self.lka_icon_status_last = (False, False) |
|
|
|
self.lka_icon_status_last = (False, False) |
|
|
|
self.steer_rate_limited = False |
|
|
|
self.steer_rate_limited = False |
|
|
|
self.fcw_frames = 0 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
self.params = CarControllerParams() |
|
|
|
self.params = CarControllerParams() |
|
|
|
|
|
|
|
|
|
|
|
self.packer_pt = CANPacker(DBC[CP.carFingerprint]['pt']) |
|
|
|
self.packer_pt = CANPacker(DBC[CP.carFingerprint]['pt']) |
|
|
|
|
|
|
|
self.packer_obj = CANPacker(DBC[CP.carFingerprint]['radar']) |
|
|
|
self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis']) |
|
|
|
self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis']) |
|
|
|
|
|
|
|
|
|
|
|
def update(self, enabled, CS, frame, actuators, |
|
|
|
def update(self, enabled, CS, frame, actuators, |
|
|
@ -59,10 +59,6 @@ class CarController(): |
|
|
|
# Send CAN commands. |
|
|
|
# Send CAN commands. |
|
|
|
can_sends = [] |
|
|
|
can_sends = [] |
|
|
|
|
|
|
|
|
|
|
|
# FCW: trigger FCWAlert for 100 frames (4 seconds) |
|
|
|
|
|
|
|
if hud_alert == VisualAlert.fcw: |
|
|
|
|
|
|
|
self.fcw_frames = 100 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# STEER |
|
|
|
# STEER |
|
|
|
if (frame % P.STEER_STEP) == 0: |
|
|
|
if (frame % P.STEER_STEP) == 0: |
|
|
|
lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED |
|
|
|
lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED |
|
|
@ -102,12 +98,7 @@ class CarController(): |
|
|
|
|
|
|
|
|
|
|
|
# Send dashboard UI commands (ACC status), 25hz |
|
|
|
# Send dashboard UI commands (ACC status), 25hz |
|
|
|
if (frame % 4) == 0: |
|
|
|
if (frame % 4) == 0: |
|
|
|
# Send FCW if applicable |
|
|
|
send_fcw = hud_alert == VisualAlert.fcw |
|
|
|
send_fcw = False |
|
|
|
|
|
|
|
if self.fcw_frames > 0: |
|
|
|
|
|
|
|
send_fcw = True |
|
|
|
|
|
|
|
self.fcw_frames -= 1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw)) |
|
|
|
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw)) |
|
|
|
|
|
|
|
|
|
|
|
# Radar needs to know current speed and yaw rate (50hz), |
|
|
|
# Radar needs to know current speed and yaw rate (50hz), |
|
|
@ -118,7 +109,7 @@ class CarController(): |
|
|
|
if frame % time_and_headlights_step == 0: |
|
|
|
if frame % time_and_headlights_step == 0: |
|
|
|
idx = (frame // time_and_headlights_step) % 4 |
|
|
|
idx = (frame // time_and_headlights_step) % 4 |
|
|
|
can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) |
|
|
|
can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) |
|
|
|
can_sends.append(gmcan.create_adas_headlights_status(self.packer_ch, CanBus.OBSTACLE)) |
|
|
|
can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE)) |
|
|
|
|
|
|
|
|
|
|
|
speed_and_accelerometer_step = 2 |
|
|
|
speed_and_accelerometer_step = 2 |
|
|
|
if frame % speed_and_accelerometer_step == 0: |
|
|
|
if frame % speed_and_accelerometer_step == 0: |
|
|
|