diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 7ce2ded6e3..f55e14632c 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,6 +1,7 @@ from cereal import car from opendbc.can.packer import CANPacker from openpilot.common.numpy_fast import clip +from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_std_steer_angle_limits from openpilot.selfdrive.car.ford import fordcan from openpilot.selfdrive.car.ford.values import CarControllerParams, FordFlags @@ -35,6 +36,8 @@ class CarController(CarControllerBase): self.main_on_last = False self.lkas_enabled_last = False self.steer_alert_last = False + self.lead_distance_bars_last = None + self.lead_distance_bars_changed = 0 def update(self, CC, CS, now_nanos): can_sends = [] @@ -98,11 +101,18 @@ class CarController(CarControllerBase): # send lkas ui msg at 1Hz or if ui state changes if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: can_sends.append(fordcan.create_lkas_ui_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values)) + # send acc ui msg at 5Hz or if ui state changes + if self.lead_distance_bars_last and hud_control.leadDistanceBars != self.lead_distance_bars_last: + self.lead_distance_bars_changed = self.frame + send_ui = True + self.lead_distance_bars_last = hud_control.leadDistanceBars + if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: - can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive, - fcw_alert, CS.out.cruiseState.standstill, hud_control, - CS.acc_tja_status_stock_values)) + # display lead distance bars for 2 seconds after they change + lead_distance_bars_dsply = (self.frame - self.lead_distance_bars_changed) * DT_CTRL < 2.0 + can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive, fcw_alert, CS.out.cruiseState.standstill, + lead_distance_bars_dsply, hud_control, CS.acc_tja_status_stock_values)) self.main_on_last = main_on self.lkas_enabled_last = CC.latActive diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index 939084c4a0..5fb6bd0854 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -143,7 +143,7 @@ def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: fl return packer.make_can_msg("ACCDATA", CAN.main, values) -def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, standstill: bool, +def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, standstill: bool, lead_distance_bars_dsply: bool, hud_control, stock_values: dict): """ Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam @@ -208,7 +208,7 @@ def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw values.update({ "AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text "AccMsgTxt_D2_Rq": 0, # ACC text - "AccTGap_B_Dsply": 0, # Show time gap control UI + "AccTGap_B_Dsply": 1 if lead_distance_bars_dsply else 0, # Show time gap control UI "AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0, # Lead indicator "AccStopMde_B_Dsply": 1 if standstill else 0, "AccWarn_D_Dsply": 0, # ACC warning