You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
342 lines
13 KiB
342 lines
13 KiB
from opendbc.car import CanBusBase, structs
|
|
|
|
HUDControl = structs.CarControl.HUDControl
|
|
|
|
|
|
class CanBus(CanBusBase):
|
|
def __init__(self, CP=None, fingerprint=None) -> None:
|
|
super().__init__(CP, fingerprint)
|
|
|
|
@property
|
|
def main(self) -> int:
|
|
return self.offset
|
|
|
|
@property
|
|
def radar(self) -> int:
|
|
return self.offset + 1
|
|
|
|
@property
|
|
def camera(self) -> int:
|
|
return self.offset + 2
|
|
|
|
|
|
def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray) -> int:
|
|
curvature = (dat[2] << 3) | ((dat[3]) >> 5)
|
|
curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5)
|
|
path_angle = ((dat[3] & 0x1F) << 6) | ((dat[4]) >> 2)
|
|
path_offset = ((dat[4] & 0x3) << 8) | dat[5]
|
|
|
|
checksum = mode + counter
|
|
for sig_val in (curvature, curvature_rate, path_angle, path_offset):
|
|
checksum += sig_val + (sig_val >> 8)
|
|
|
|
return 0xFF - (checksum & 0xFF)
|
|
|
|
|
|
def create_lka_msg(packer, CAN: CanBus):
|
|
"""
|
|
Creates an empty CAN message for the Ford LKA Command.
|
|
|
|
This command can apply "Lane Keeping Aid" maneuvers, which are subject to the PSCM lockout.
|
|
|
|
Frequency is 33Hz.
|
|
"""
|
|
|
|
return packer.make_can_msg("Lane_Assist_Data1", CAN.main, {})
|
|
|
|
|
|
def create_lat_ctl_msg(packer, CAN: CanBus, lat_active: bool, path_offset: float, path_angle: float, curvature: float,
|
|
curvature_rate: float):
|
|
"""
|
|
Creates a CAN message for the Ford TJA/LCA Command.
|
|
|
|
This command can apply "Lane Centering" maneuvers: continuous lane centering for traffic jam assist and highway
|
|
driving. It is not subject to the PSCM lockout.
|
|
|
|
Ford lane centering command uses a third order polynomial to describe the road centerline. The polynomial is defined
|
|
by the following coefficients:
|
|
c0: lateral offset between the vehicle and the centerline (positive is right)
|
|
c1: heading angle between the vehicle and the centerline (positive is right)
|
|
c2: curvature of the centerline (positive is left)
|
|
c3: rate of change of curvature of the centerline
|
|
As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and speed, the steering
|
|
angle cannot be easily controlled.
|
|
|
|
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": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft,
|
|
# 3=InterventionRight, 4-7=NotUsed [0|7]
|
|
"LatCtlRampType_D_Rq": 0, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
|
|
# Makes no difference with curvature control
|
|
"LatCtlPrecision_D_Rq": 1, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
|
|
# The stock system always uses comfortable
|
|
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
|
|
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians
|
|
"LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2
|
|
"LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter
|
|
}
|
|
return packer.make_can_msg("LateralMotionControl", CAN.main, values)
|
|
|
|
|
|
def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, path_offset: float, path_angle: float, curvature: float,
|
|
curvature_rate: float, counter: int):
|
|
"""
|
|
Create a CAN message for the new Ford Lane Centering command.
|
|
|
|
This message is used on the CAN FD platform and replaces the old LateralMotionControl message. It is similar but has
|
|
additional signals for a counter and checksum.
|
|
|
|
Frequency is 20Hz.
|
|
"""
|
|
|
|
values = {
|
|
"LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode,
|
|
# 3=SafeRampOut, 4-7=NotUsed [0|7]
|
|
"LatCtlRampType_D_Rq": 0, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
|
|
"LatCtlPrecision_D_Rq": 1, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
|
|
"LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter
|
|
"LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians
|
|
"LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter
|
|
"LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2
|
|
"HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1]
|
|
"LatCtlPath_No_Cnt": counter, # [0|15]
|
|
"LatCtlPath_No_Cs": 0, # [0|255]
|
|
}
|
|
|
|
# calculate checksum
|
|
dat = packer.make_can_msg("LateralMotionControl2", 0, values)[1]
|
|
values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat)
|
|
|
|
return packer.make_can_msg("LateralMotionControl2", CAN.main, values)
|
|
|
|
|
|
def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool, brake_request, v_ego_kph: float):
|
|
"""
|
|
Creates a CAN message for the Ford ACC Command.
|
|
|
|
This command can be used to enable ACC, to set the ACC gas/brake/decel values
|
|
and to disable ACC.
|
|
|
|
Frequency is 50Hz.
|
|
"""
|
|
values = {
|
|
"AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2
|
|
"Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes
|
|
"AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2
|
|
# No observed acceleration seen from this signal alone. During stock system operation, it appears to
|
|
# be the raw acceleration request (AccPrpl_A_Rq when positive, AccBrkTot_A_Rq when negative)
|
|
"AccPrpl_A_Pred": -5.0, # Acceleration request: [-5|5.23] m/s^2
|
|
"AccResumEnbl_B_Rq": 1 if long_active else 0,
|
|
# No observed acceleration seen from this signal alone
|
|
"AccVeh_V_Trg": v_ego_kph, # Target speed: [0|255] km/h
|
|
# TODO: we may be able to improve braking response by utilizing pre-charging better
|
|
# When setting these two bits without AccBrkTot_A_Rq, an initial jerk is observed and car may be able to brake temporarily with AccPrpl_A_Rq
|
|
"AccBrkPrchg_B_Rq": 1 if brake_request else 0, # Pre-charge brake request: 0=No, 1=Yes
|
|
"AccBrkDecel_B_Rq": 1 if brake_request else 0, # Deceleration request: 0=Inactive, 1=Active
|
|
"AccStopStat_B_Rq": 1 if stopping else 0,
|
|
}
|
|
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,
|
|
show_distance_bars: bool, hud_control, stock_values: dict):
|
|
"""
|
|
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 5Hz.
|
|
"""
|
|
|
|
# Tja_D_Stat
|
|
if enabled:
|
|
if hud_control.leftLaneDepart:
|
|
status = 3 # ActiveInterventionLeft
|
|
elif hud_control.rightLaneDepart:
|
|
status = 4 # ActiveInterventionRight
|
|
else:
|
|
status = 2 # Active
|
|
elif main_on:
|
|
if hud_control.leftLaneDepart:
|
|
status = 5 # ActiveWarningLeft
|
|
elif hud_control.rightLaneDepart:
|
|
status = 6 # ActiveWarningRight
|
|
else:
|
|
status = 1 # Standby
|
|
else:
|
|
status = 0 # Off
|
|
|
|
values = {s: stock_values[s] for s in [
|
|
"HaDsply_No_Cs",
|
|
"HaDsply_No_Cnt",
|
|
"AccStopStat_D_Dsply", # ACC stopped status message
|
|
"AccTrgDist2_D_Dsply", # ACC target distance
|
|
"AccStopRes_B_Dsply",
|
|
"TjaWarn_D_Rq", # TJA warning
|
|
"TjaMsgTxt_D_Dsply", # TJA text
|
|
"IaccLamp_D_Rq", # iACC status icon
|
|
"AccMsgTxt_D2_Rq", # ACC text
|
|
"FcwDeny_B_Dsply", # FCW disabled
|
|
"FcwMemStat_B_Actl", # FCW enabled setting
|
|
"AccTGap_B_Dsply", # ACC time gap display setting
|
|
"CadsAlignIncplt_B_Actl",
|
|
"AccFllwMde_B_Dsply", # ACC follow mode display setting
|
|
"CadsRadrBlck_B_Actl",
|
|
"CmbbPostEvnt_B_Dsply", # AEB event status
|
|
"AccStopMde_B_Dsply", # ACC stop mode display setting
|
|
"FcwMemSens_D_Actl", # FCW sensitivity setting
|
|
"FcwMsgTxt_D_Rq", # FCW text
|
|
"AccWarn_D_Dsply", # ACC warning
|
|
"FcwVisblWarn_B_Rq", # FCW visible alert
|
|
"FcwAudioWarn_B_Rq", # FCW audio alert
|
|
"AccTGap_D_Dsply", # ACC time gap
|
|
"AccMemEnbl_B_RqDrv", # ACC adaptive/normal setting
|
|
"FdaMem_B_Stat", # FDA enabled setting
|
|
]}
|
|
|
|
values.update({
|
|
"Tja_D_Stat": status, # TJA status
|
|
})
|
|
|
|
if CP.openpilotLongitudinalControl:
|
|
values.update({
|
|
"AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text
|
|
"AccMsgTxt_D2_Rq": 0, # ACC text
|
|
"AccTGap_B_Dsply": 1 if show_distance_bars 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
|
|
"AccTGap_D_Dsply": hud_control.leadDistanceBars, # Time gap
|
|
})
|
|
|
|
# Forwards FCW alert from IPMA
|
|
if fcw_alert:
|
|
values["FcwVisblWarn_B_Rq"] = 1 # FCW visible alert
|
|
|
|
return packer.make_can_msg("ACCDATA_3", CAN.main, values)
|
|
|
|
|
|
def create_lkas_ui_msg(packer, CAN: CanBus, main_on: bool, enabled: bool, steer_alert: bool, hud_control,
|
|
stock_values: dict):
|
|
"""
|
|
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
|
|
# R Intvn Warn Supprs Avail No
|
|
# L
|
|
# Intvn 24 19 14 9 4
|
|
# Warn 23 18 13 8 3
|
|
# Supprs 22 17 12 7 2
|
|
# Avail 21 16 11 6 1
|
|
# No 20 15 10 5 0
|
|
#
|
|
# TODO: test suppress state
|
|
if enabled:
|
|
lines = 0 # NoLeft_NoRight
|
|
if hud_control.leftLaneDepart:
|
|
lines += 4
|
|
elif hud_control.leftLaneVisible:
|
|
lines += 1
|
|
if hud_control.rightLaneDepart:
|
|
lines += 20
|
|
elif hud_control.rightLaneVisible:
|
|
lines += 5
|
|
elif main_on:
|
|
lines = 0
|
|
else:
|
|
if hud_control.leftLaneDepart:
|
|
lines = 3 # WarnLeft_NoRight
|
|
elif hud_control.rightLaneDepart:
|
|
lines = 15 # NoLeft_WarnRight
|
|
else:
|
|
lines = 30 # LA_Off
|
|
|
|
hands_on_wheel_dsply = 1 if steer_alert else 0
|
|
|
|
values = {s: stock_values[s] for s in [
|
|
"FeatConfigIpmaActl",
|
|
"FeatNoIpmaActl",
|
|
"PersIndexIpma_D_Actl",
|
|
"AhbcRampingV_D_Rq", # AHB ramping
|
|
"LaDenyStats_B_Dsply", # LKAS error
|
|
"CamraDefog_B_Req", # Windshield heater?
|
|
"CamraStats_D_Dsply", # Camera status
|
|
"DasAlrtLvl_D_Dsply", # DAS alert level
|
|
"DasStats_D_Dsply", # DAS status
|
|
"DasWarn_D_Dsply", # DAS warning
|
|
"AhbHiBeam_D_Rq", # AHB status
|
|
"Passthru_63",
|
|
"Passthru_48",
|
|
]}
|
|
|
|
values.update({
|
|
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
|
|
"LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
|
|
})
|
|
return packer.make_can_msg("IPMA_Data", CAN.main, values)
|
|
|
|
|
|
def create_button_msg(packer, bus: int, stock_values: dict, cancel=False, resume=False, tja_toggle=False):
|
|
"""
|
|
Creates a CAN message for the Ford SCCM buttons/switches.
|
|
|
|
Includes cruise control buttons, turn lights and more.
|
|
|
|
Frequency is 10Hz.
|
|
"""
|
|
|
|
values = {s: stock_values[s] for s in [
|
|
"HeadLghtHiFlash_D_Stat", # SCCM Passthrough the remaining buttons
|
|
"TurnLghtSwtch_D_Stat", # SCCM Turn signal switch
|
|
"WiprFront_D_Stat",
|
|
"LghtAmb_D_Sns",
|
|
"AccButtnGapDecPress",
|
|
"AccButtnGapIncPress",
|
|
"AslButtnOnOffCnclPress",
|
|
"AslButtnOnOffPress",
|
|
"LaSwtchPos_D_Stat",
|
|
"CcAslButtnCnclResPress",
|
|
"CcAslButtnDeny_B_Actl",
|
|
"CcAslButtnIndxDecPress",
|
|
"CcAslButtnIndxIncPress",
|
|
"CcAslButtnOffCnclPress",
|
|
"CcAslButtnOnOffCncl",
|
|
"CcAslButtnOnPress",
|
|
"CcAslButtnResDecPress",
|
|
"CcAslButtnResIncPress",
|
|
"CcAslButtnSetDecPress",
|
|
"CcAslButtnSetIncPress",
|
|
"CcAslButtnSetPress",
|
|
"CcButtnOffPress",
|
|
"CcButtnOnOffCnclPress",
|
|
"CcButtnOnOffPress",
|
|
"CcButtnOnPress",
|
|
"HeadLghtHiFlash_D_Actl",
|
|
"HeadLghtHiOn_B_StatAhb",
|
|
"AhbStat_B_Dsply",
|
|
"AccButtnGapTogglePress",
|
|
"WiprFrontSwtch_D_Stat",
|
|
"HeadLghtHiCtrl_D_RqAhb",
|
|
]}
|
|
|
|
values.update({
|
|
"CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button
|
|
"CcAsllButtnResPress": 1 if resume else 0, # CC resume button
|
|
"TjaButtnOnOffPress": 1 if tja_toggle else 0, # LCA/TJA toggle button
|
|
})
|
|
return packer.make_can_msg("Steering_Data_FD1", bus, values)
|
|
|