openpilot_comma/selfdrive/car/ford/fordcan.py

from common.numpy_fast import clip


def create_lkas_command(packer, angle_deg: float, curvature: float):
  """
  Creates a CAN message for the Ford LKAS Command.

  This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the
  PSCM lockout.

  Frequency is 20Hz.
  """

  values = {
    "LkaDrvOvrrd_D_Rq": 0,              # driver override level? [0|3]
    "LkaActvStats_D2_Req": 0,           # action [0|7]
    "LaRefAng_No_Req": angle_deg,       # angle [-102.4|102.3] degrees
    "LaRampType_B_Req": 0,              # Ramp speed: 0=Smooth, 1=Quick
    "LaCurvature_No_Calc": curvature,   # curvature [-0.01024|0.01023] 1/meter
    "LdwActvStats_D_Req": 0,            # LDW status [0|7]
    "LdwActvIntns_D_Req": 0,            # LDW intensity [0|3], shake alert strength
  }
  return packer.make_can_msg("Lane_Assist_Data1", 0, values)


def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float, curvature_rate: float, curvature: float):
  """
  Creates a CAN message for the Ford TJA/LCA Command.

  This command can apply "Lane Centering" manoeuvres: continuous lane centering
  for traffic jam assist and highway driving. It is not subject to the PSCM
  lockout.

  The PSCM should be configured to accept TJA/LCA commands before these
  commands will be processed. This can be done using tools such as Forscan.

  Frequency is 20Hz.
  """

  values = {
    "LatCtlRng_L_Max": 0,                                                   # Unknown [0|126] meter
    "HandsOffCnfm_B_Rq": 0,                                                 # Unknown: 0=Inactive, 1=Active [0|1]
    "LatCtl_D_Rq": lca_rq,                                                  # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight, 4-7=NotUsed [0|7]
    "LatCtlRampType_D_Rq": ramp_type,                                       # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
    "LatCtlPrecision_D_Rq": precision,                                      # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
    "LatCtlPathOffst_L_Actl": clip(path_offset, -5.12, 5.11),               # Path offset [-5.12|5.11] meter
    "LatCtlPath_An_Actl": clip(path_angle, -0.5, 0.5235),                   # Path angle [-0.5|0.5235] radians
    "LatCtlCurv_NoRate_Actl": clip(curvature_rate, -0.001024, 0.00102375),  # Curvature rate [-0.001024|0.00102375] 1/meter^2
    "LatCtlCurv_No_Actl": clip(curvature, -0.02, 0.02094),                  # Curvature [-0.02|0.02094] 1/meter
  }
  return packer.make_can_msg("LateralMotionControl", 0, values)


def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, stock_values):
  """
  Creates a CAN message for the Ford IPC IPMA/LKAS status.

  Show the LKAS status with the "driver assist" lines in the IPC.

  Stock functionality is maintained by passing through unmodified signals.

  Frequency is 1Hz.
  """

  # LaActvStats_D_Dsply
  # TODO: get LDW state from OP
  if enabled:
    lines = 6
  elif main_on:
    lines = 0
  else:
    lines = 30

  values = {
    "FeatConfigIpmaActl": stock_values["FeatConfigIpmaActl"],       # [0|65535]
    "FeatNoIpmaActl": stock_values["FeatNoIpmaActl"],               # [0|65535]
    "PersIndexIpma_D_Actl": stock_values["PersIndexIpma_D_Actl"],   # [0|7]
    "AhbcRampingV_D_Rq": stock_values["AhbcRampingV_D_Rq"],         # AHB ramping [0|3]
    "LaActvStats_D_Dsply": lines,                                   # LKAS status (lines) [0|31]
    "LaDenyStats_B_Dsply": stock_values["LaDenyStats_B_Dsply"],     # LKAS error [0|1]
    "LaHandsOff_D_Dsply": 2 if steer_alert else 0,                  # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
    "CamraDefog_B_Req": stock_values["CamraDefog_B_Req"],           # Windshield heater? [0|1]
    "CamraStats_D_Dsply": stock_values["CamraStats_D_Dsply"],       # Camera status [0|3]
    "DasAlrtLvl_D_Dsply": stock_values["DasAlrtLvl_D_Dsply"],       # DAS alert level [0|7]
    "DasStats_D_Dsply": stock_values["DasStats_D_Dsply"],           # DAS status [0|3]
    "DasWarn_D_Dsply": stock_values["DasWarn_D_Dsply"],             # DAS warning [0|3]
    "AhbHiBeam_D_Rq": stock_values["AhbHiBeam_D_Rq"],               # AHB status [0|3]
    "Set_Me_X1": stock_values["Set_Me_X1"],                         # [0|15]
  }
  return packer.make_can_msg("IPMA_Data", 0, values)


def create_acc_ui_command(packer, main_on: bool, enabled: bool, stock_values):
  """
  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.

  Frequency is 20Hz.
  """

  values = {
    "HaDsply_No_Cs": stock_values["HaDsply_No_Cs"],                     # [0|255]
    "HaDsply_No_Cnt": stock_values["HaDsply_No_Cnt"],                   # [0|15]
    "AccStopStat_D_Dsply": stock_values["AccStopStat_D_Dsply"],         # ACC stopped status message: 0=NoDisplay, 1=ResumeReady, 2=Stopped, 3=PressResume [0|3]
    "AccTrgDist2_D_Dsply": stock_values["AccTrgDist2_D_Dsply"],         # ACC target distance [0|15]
    "AccStopRes_B_Dsply": stock_values["AccStopRes_B_Dsply"],           # [0|1]
    "TjaWarn_D_Rq": stock_values["TjaWarn_D_Rq"],                       # TJA warning: 0=NoWarning, 1=Cancel, 2=HardTakeOverLevel1, 3=HardTakeOverLevel2 [0|7]
    "Tja_D_Stat": 2 if enabled else (1 if main_on else 0),              # TJA status: 0=Off, 1=Standby, 2=Active, 3=InterventionLeft, 4=InterventionRight, 5=WarningLeft, 6=WarningRight, 7=NotUsed [0|7]
    "TjaMsgTxt_D_Dsply": stock_values["TjaMsgTxt_D_Dsply"],             # TJA text [0|7]
    "IaccLamp_D_Rq": stock_values["IaccLamp_D_Rq"],                     # iACC status icon [0|3]
    "AccMsgTxt_D2_Rq": stock_values["AccMsgTxt_D2_Rq"],                 # ACC text [0|15]
    "FcwDeny_B_Dsply": stock_values["FcwDeny_B_Dsply"],                 # FCW disabled [0|1]
    "FcwMemStat_B_Actl": stock_values["FcwMemStat_B_Actl"],             # FCW enabled setting [0|1]
    "AccTGap_B_Dsply": stock_values["AccTGap_B_Dsply"],                 # ACC time gap display setting [0|1]
    "CadsAlignIncplt_B_Actl": stock_values["CadsAlignIncplt_B_Actl"],   # Radar alignment? [0|1]
    "AccFllwMde_B_Dsply": stock_values["AccFllwMde_B_Dsply"],           # ACC follow mode display setting [0|1]
    "CadsRadrBlck_B_Actl": stock_values["CadsRadrBlck_B_Actl"],         # Radar blocked? [0|1]
    "CmbbPostEvnt_B_Dsply": stock_values["CmbbPostEvnt_B_Dsply"],       # AEB event status [0|1]
    "AccStopMde_B_Dsply": stock_values["AccStopMde_B_Dsply"],           # ACC stop mode display setting [0|1]
    "FcwMemSens_D_Actl": stock_values["FcwMemSens_D_Actl"],             # FCW sensitivity setting [0|3]
    "FcwMsgTxt_D_Rq": stock_values["FcwMsgTxt_D_Rq"],                   # FCW text [0|7]
    "AccWarn_D_Dsply": stock_values["AccWarn_D_Dsply"],                 # ACC warning [0|3]
    "FcwVisblWarn_B_Rq": stock_values["FcwVisblWarn_B_Rq"],             # FCW alert: 0=Off, 1=On [0|1]
    "FcwAudioWarn_B_Rq": stock_values["FcwAudioWarn_B_Rq"],             # FCW audio: 0=Off, 1=On [0|1]
    "AccTGap_D_Dsply": stock_values["AccTGap_D_Dsply"],                 # ACC time gap: 1=Time_Gap_1, 2=Time_Gap_2, ..., 5=Time_Gap_5 [0|7]
    "AccMemEnbl_B_RqDrv": stock_values["AccMemEnbl_B_RqDrv"],           # ACC setting: 0=NormalCruise, 1=AdaptiveCruise [0|1]
    "FdaMem_B_Stat": stock_values["FdaMem_B_Stat"],                     # FDA enabled setting [0|1]
  }
  return packer.make_can_msg("ACCDATA_3", 0, values)


def spam_cancel_button(packer, cancel=1):
  """
  Creates a CAN message for the Ford SCCM buttons/switches.

  Includes cruise control buttons, turn lights and more.
  """

  values = {
    "CcAslButtnCnclPress": cancel,  # CC cancel button
  }
  return packer.make_can_msg("Steering_Data_FD1", 0, values)