diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index edce3418be..29b86f4ff6 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -99,7 +99,9 @@ class CarController: can_sends.append(create_lkas_ui_msg(self.packer, 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.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: - can_sends.append(create_acc_ui_msg(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values)) + can_sends.append(create_acc_ui_msg(self.packer, self.CP, main_on, CC.latActive, + CS.out.cruiseState.standstill, 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/carstate.py b/selfdrive/car/ford/carstate.py index 1281b65d1d..4ab968ea99 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -231,7 +231,7 @@ class CarState(CarStateBase): ("FcwMemStat_B_Actl", "ACCDATA_3"), # FCW enabled setting ("AccTGap_B_Dsply", "ACCDATA_3"), # ACC time gap display setting ("CadsAlignIncplt_B_Actl", "ACCDATA_3"), - ("AccFllwMde_B_Dsply", "ACCDATA_3"), # ACC follow mode display setting + ("AccFllwMde_B_Dsply", "ACCDATA_3"), # ACC lead indicator ("CadsRadrBlck_B_Actl", "ACCDATA_3"), ("CmbbPostEvnt_B_Dsply", "ACCDATA_3"), # AEB event status ("AccStopMde_B_Dsply", "ACCDATA_3"), # ACC stop mode display setting diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index f35a2d81a3..0202dc8845 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -123,9 +123,11 @@ def create_acc_msg(packer, long_active: bool, gas: float, accel: float, decel: b return packer.make_can_msg("ACCDATA", CANBUS.main, values) -def create_acc_ui_msg(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict): +def create_acc_ui_msg(packer, CP, main_on: bool, enabled: bool, standstill: bool, hud_control, + stock_values: dict): """ - Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam assist status. + Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam + assist status. Stock functionality is maintained by passing through unmodified signals. @@ -181,6 +183,16 @@ def create_acc_ui_msg(packer, main_on: bool, enabled: bool, hud_control, stock_v values.update({ "Tja_D_Stat": status, # TJA status }) + + if CP.openpilotLongitudinalControl: + values.update({ + "AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text + "AccTGap_B_Dsply": 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, + "AccTGap_D_Dsply": 4, # Fixed time gap in UI + }) + return packer.make_can_msg("ACCDATA_3", CANBUS.main, values)