mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-07-14 01:12:06 +08:00
Ford Explorer 2020-21 (#25614)
* add model years * update Focus steer ratio * Ford: add EU label to Focus Mk4 * add packages * add Ford Explorer 2020 Package: Co-Pilot 360 Assist+ Optional on XLT Standard on Limited, Limited Hybrid, ST and Platinum https://cdn.dealereprocess.org/cdn/brochures/ford/2020-explorer.pdf * Ford: steering control with path angle * Ford: add TJA toggle to buttons * add Ford Explorer 2021 `62241b0c7fea4589|2022-08-30--11-58-24--0` Package: Co-Pilot 360 Assist+ Optional on XLT Standard on Limited, Limited Hybrid, ST and Platinum (same as 2020) https://cdn.dealereprocess.org/cdn/brochures/ford/2021-explorer.pdf * Ford: add shiftByWire ECU fw * angle/steer refactor * try always stop and go for US models * no dashcam * car info * send resume button * skip explorer * escape and focus back in dashcam * passthru buttons * fordcan set bus for button message * toggle off stock traffic jam assist so camera does not enforce driver presence checks * not used * update ramp rate/precision notes * cleanup * bump steering pressed torque to 0.8 Nm * add standstill * bump steer ratio * try increasing delay? * fix docs * add kuga car info * maybe fix tja toggle? * compensate for ford roll compensation?? * oops * better ui * block non-adaptive * add note on ui warning for hands on wheel * try only checking/toggling TJA every 2 seconds * add car test route * dashcam only again * send buttons to camera * add process replay segment * cleanup * bump panda * add extra FW Co-authored-by: Cameron Clough <cameronjclough@gmail.com>
This commit is contained in:
Adeeb Shihadeh
+1
-1
Submodule panda updated: 0ca23b6778...8f13ca3f66
@@ -1,3 +1,4 @@
|
||||
import math
|
||||
from cereal import car
|
||||
from common.numpy_fast import clip, interp
|
||||
from opendbc.can.packer import CANPacker
|
||||
@@ -7,14 +8,19 @@ from selfdrive.car.ford.values import CarControllerParams
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
def apply_ford_steer_angle_limits(apply_steer, apply_steer_last, vEgo):
|
||||
def apply_ford_steer_angle_limits(apply_angle, apply_angle_last, vEgo):
|
||||
# rate limit
|
||||
steer_up = apply_steer * apply_steer_last > 0. and abs(apply_steer) > abs(apply_steer_last)
|
||||
rate_limit = CarControllerParams.STEER_RATE_LIMIT_UP if steer_up else CarControllerParams.STEER_RATE_LIMIT_DOWN
|
||||
steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last)
|
||||
rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN
|
||||
max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points)
|
||||
apply_steer = clip(apply_steer, (apply_steer_last - max_angle_diff), (apply_steer_last + max_angle_diff))
|
||||
apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff))
|
||||
|
||||
return apply_steer
|
||||
# absolute limit (LatCtlPath_An_Actl)
|
||||
apply_path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO
|
||||
apply_path_angle = clip(apply_path_angle, -0.4995, 0.5240)
|
||||
apply_angle = math.degrees(apply_path_angle) * CarControllerParams.STEER_RATIO
|
||||
|
||||
return apply_angle
|
||||
|
||||
|
||||
class CarController:
|
||||
@@ -24,7 +30,7 @@ class CarController:
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.frame = 0
|
||||
|
||||
self.apply_steer_last = 0
|
||||
self.apply_angle_last = 0
|
||||
self.main_on_last = False
|
||||
self.lkas_enabled_last = False
|
||||
self.steer_alert_last = False
|
||||
@@ -38,52 +44,62 @@ class CarController:
|
||||
main_on = CS.out.cruiseState.available
|
||||
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
|
||||
|
||||
### acc buttons ###
|
||||
if CC.cruiseControl.cancel:
|
||||
# cancel stock ACC
|
||||
can_sends.append(fordcan.spam_cancel_button(self.packer))
|
||||
can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True))
|
||||
elif CC.cruiseControl.resume:
|
||||
can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True))
|
||||
|
||||
# apply rate limits
|
||||
new_steer = actuators.steeringAngleDeg
|
||||
apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo)
|
||||
# if stock lane centering is active or in standby, toggle it off
|
||||
# the stock system checks for steering pressed, and eventually disengages cruise control
|
||||
if (self.frame % 200) == 0 and CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0:
|
||||
can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, tja_toggle=True))
|
||||
|
||||
|
||||
### lateral control ###
|
||||
if CC.latActive:
|
||||
apply_angle = apply_ford_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo)
|
||||
else:
|
||||
apply_angle = CS.out.steeringAngleDeg
|
||||
|
||||
# send steering commands at 20Hz
|
||||
if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0:
|
||||
lca_rq = 1 if CC.latActive else 0
|
||||
|
||||
# use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented
|
||||
path_angle = apply_steer
|
||||
# use LatCtlPath_An_Actl to actuate steering
|
||||
# path angle is the car wheel angle, not the steering wheel angle
|
||||
path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO
|
||||
|
||||
# convert steer angle to curvature
|
||||
curvature = self.VM.calc_curvature(apply_steer, CS.out.vEgo, 0.0)
|
||||
# ramp rate: 0=Slow, 1=Medium, 2=Fast, 3=Immediately
|
||||
# TODO: try slower ramp speed when driver torque detected
|
||||
ramp_type = 3
|
||||
precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable)
|
||||
|
||||
# TODO: get other actuators
|
||||
curvature_rate = 0
|
||||
path_offset = 0
|
||||
offset_roll_compensation_curvature = clip(self.VM.calc_curvature(0, CS.out.vEgo, -CS.yaw_data["VehYaw_W_Actl"]), -0.02, 0.02094)
|
||||
|
||||
ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately
|
||||
precision = 0 # 0=Comfortable, 1=Precise
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
can_sends.append(fordcan.create_lkas_command(self.packer, apply_steer, curvature))
|
||||
self.apply_angle_last = apply_angle
|
||||
can_sends.append(fordcan.create_lka_command(self.packer, apply_angle, 0))
|
||||
can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision,
|
||||
path_offset, path_angle, curvature_rate, curvature))
|
||||
0, path_angle, 0, offset_roll_compensation_curvature))
|
||||
|
||||
|
||||
### ui ###
|
||||
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
|
||||
|
||||
# send lkas ui command 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_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values))
|
||||
can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
|
||||
|
||||
# send acc ui command at 20Hz or if ui state changes
|
||||
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
|
||||
can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, CS.acc_tja_status_stock_values))
|
||||
can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values))
|
||||
|
||||
self.main_on_last = main_on
|
||||
self.lkas_enabled_last = CC.latActive
|
||||
self.steer_alert_last = steer_alert
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steeringAngleDeg = apply_steer
|
||||
new_actuators.steeringAngleDeg = apply_angle
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
@@ -3,7 +3,7 @@ from common.conversions import Conversions as CV
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from selfdrive.car.ford.values import CANBUS, DBC
|
||||
from selfdrive.car.ford.values import CANBUS, DBC, CarControllerParams
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
@@ -22,7 +22,7 @@ class CarState(CarStateBase):
|
||||
# car speed
|
||||
ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] * CV.RAD_TO_DEG
|
||||
ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"]
|
||||
ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1
|
||||
|
||||
# gas pedal
|
||||
@@ -37,7 +37,7 @@ class CarState(CarStateBase):
|
||||
# steering wheel
|
||||
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
|
||||
ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"]
|
||||
ret.steeringPressed = cp.vl["Lane_Assist_Data3_FD1"]["LaHandsOff_B_Actl"] == 0
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
|
||||
ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1
|
||||
ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3)
|
||||
# ret.espDisabled = False # TODO: find traction control signal
|
||||
@@ -46,6 +46,8 @@ class CarState(CarStateBase):
|
||||
ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * CV.MPH_TO_MS
|
||||
ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5)
|
||||
ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5)
|
||||
ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0
|
||||
ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3
|
||||
|
||||
# gear
|
||||
if self.CP.transmissionType == TransmissionType.automatic:
|
||||
@@ -65,6 +67,7 @@ class CarState(CarStateBase):
|
||||
# button presses
|
||||
ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1
|
||||
ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2
|
||||
# TODO: block this going to the camera otherwise it will enable stock TJA
|
||||
ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
|
||||
|
||||
# lock info
|
||||
@@ -77,9 +80,13 @@ class CarState(CarStateBase):
|
||||
ret.leftBlindspot = cp.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0
|
||||
ret.rightBlindspot = cp.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0
|
||||
|
||||
# Stock steering buttons so that we can passthru blinkers etc.
|
||||
self.buttons_stock_values = cp.vl["Steering_Data_FD1"]
|
||||
# Stock values from IPMA so that we can retain some stock functionality
|
||||
self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
|
||||
self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
|
||||
# Use stock sensor values
|
||||
self.yaw_data = cp.vl["Yaw_Data_FD1"]
|
||||
|
||||
return ret
|
||||
|
||||
@@ -97,6 +104,8 @@ class CarState(CarStateBase):
|
||||
("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph)
|
||||
# The units might change with IPC settings?
|
||||
("CcStat_D_Actl", "EngBrakeData"), # PCM ACC status
|
||||
("AccStopMde_D_Rq", "EngBrakeData"), # PCM ACC standstill
|
||||
("AccEnbl_B_RqDrv", "Cluster_Info1_FD1"), # PCM ACC enable
|
||||
("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg)
|
||||
# Calculates steering angle (and offset) from pinion
|
||||
# angle and driving measurements.
|
||||
@@ -104,7 +113,6 @@ class CarState(CarStateBase):
|
||||
# to zero at the beginning of the drive.
|
||||
("SteeringColumnTorque", "EPAS_INFO"), # PSCM steering column torque (Nm)
|
||||
("EPAS_Failure", "EPAS_INFO"), # PSCM EPAS status
|
||||
("LaHandsOff_B_Actl", "Lane_Assist_Data3_FD1"), # PSCM LKAS hands off wheel
|
||||
("TurnLghtSwtch_D_Stat", "Steering_Data_FD1"), # SCCM Turn signal switch
|
||||
("TjaButtnOnOffPress", "Steering_Data_FD1"), # SCCM ACC button, lane-centering/traffic jam assist toggle
|
||||
("DrStatDrv_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, driver
|
||||
@@ -112,6 +120,38 @@ class CarState(CarStateBase):
|
||||
("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left
|
||||
("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right
|
||||
("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"), # RCM Seatbelt status, driver
|
||||
("HeadLghtHiFlash_D_Stat", "Steering_Data_FD1"), # SCCM Passthru the remaining buttons
|
||||
("WiprFront_D_Stat", "Steering_Data_FD1"),
|
||||
("LghtAmb_D_Sns", "Steering_Data_FD1"),
|
||||
("AccButtnGapDecPress", "Steering_Data_FD1"),
|
||||
("AccButtnGapIncPress", "Steering_Data_FD1"),
|
||||
("AslButtnOnOffCnclPress", "Steering_Data_FD1"),
|
||||
("AslButtnOnOffPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnCnclPress", "Steering_Data_FD1"),
|
||||
("LaSwtchPos_D_Stat", "Steering_Data_FD1"),
|
||||
("CcAslButtnCnclResPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnDeny_B_Actl", "Steering_Data_FD1"),
|
||||
("CcAslButtnIndxDecPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnIndxIncPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnOffCnclPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnOnOffCncl", "Steering_Data_FD1"),
|
||||
("CcAslButtnOnPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnResDecPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnResIncPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnSetDecPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnSetIncPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnSetPress", "Steering_Data_FD1"),
|
||||
("CcAsllButtnResPress", "Steering_Data_FD1"),
|
||||
("CcButtnOffPress", "Steering_Data_FD1"),
|
||||
("CcButtnOnOffCnclPress", "Steering_Data_FD1"),
|
||||
("CcButtnOnOffPress", "Steering_Data_FD1"),
|
||||
("CcButtnOnPress", "Steering_Data_FD1"),
|
||||
("HeadLghtHiFlash_D_Actl", "Steering_Data_FD1"),
|
||||
("HeadLghtHiOn_B_StatAhb", "Steering_Data_FD1"),
|
||||
("AhbStat_B_Dsply", "Steering_Data_FD1"),
|
||||
("AccButtnGapTogglePress", "Steering_Data_FD1"),
|
||||
("WiprFrontSwtch_D_Stat", "Steering_Data_FD1"),
|
||||
("HeadLghtHiCtrl_D_RqAhb", "Steering_Data_FD1"),
|
||||
]
|
||||
|
||||
checks = [
|
||||
@@ -122,6 +162,7 @@ class CarState(CarStateBase):
|
||||
("EngVehicleSpThrottle", 100),
|
||||
("BrakeSnData_4", 50),
|
||||
("EngBrakeData", 10),
|
||||
("Cluster_Info1_FD1", 10),
|
||||
("SteeringPinion_Data", 100),
|
||||
("EPAS_INFO", 50),
|
||||
("Lane_Assist_Data3_FD1", 33),
|
||||
|
||||
@@ -1,7 +1,10 @@
|
||||
from common.numpy_fast import clip
|
||||
from cereal import car
|
||||
from selfdrive.car.ford.values import CANBUS
|
||||
|
||||
HUDControl = car.CarControl.HUDControl
|
||||
|
||||
|
||||
def create_lkas_command(packer, angle_deg: float, curvature: float):
|
||||
def create_lka_command(packer, angle_deg: float, curvature: float):
|
||||
"""
|
||||
Creates a CAN message for the Ford LKAS Command.
|
||||
|
||||
@@ -20,7 +23,7 @@ def create_lkas_command(packer, angle_deg: float, curvature: float):
|
||||
"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)
|
||||
return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, 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):
|
||||
@@ -38,20 +41,20 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path
|
||||
"""
|
||||
|
||||
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
|
||||
"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": path_offset, # Path offset [-5.12|5.11] meter
|
||||
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.4995|0.5240] 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", 0, values)
|
||||
return packer.make_can_msg("LateralMotionControl", CANBUS.main, values)
|
||||
|
||||
|
||||
def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, stock_values: dict):
|
||||
def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict):
|
||||
"""
|
||||
Creates a CAN message for the Ford IPC IPMA/LKAS status.
|
||||
|
||||
@@ -63,23 +66,47 @@ def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bo
|
||||
"""
|
||||
|
||||
# LaActvStats_D_Dsply
|
||||
# TODO: get LDW state from OP
|
||||
# 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 = 6
|
||||
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:
|
||||
lines = 30
|
||||
if hud_control.leftLaneDepart:
|
||||
lines = 3 # WarnLeft_NoRight
|
||||
elif hud_control.rightLaneDepart:
|
||||
lines = 15 # NoLeft_WarnRight
|
||||
else:
|
||||
lines = 30 # LA_Off
|
||||
|
||||
# TODO: use level 1 for no sound when less severe?
|
||||
hands_on_wheel_dsply = 2 if steer_alert else 0
|
||||
|
||||
values = {
|
||||
**stock_values,
|
||||
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
|
||||
"LaHandsOff_D_Dsply": 2 if steer_alert else 0, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
|
||||
"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", 0, values)
|
||||
return packer.make_can_msg("IPMA_Data", CANBUS.main, values)
|
||||
|
||||
|
||||
def create_acc_ui_command(packer, main_on: bool, enabled: bool, stock_values: dict):
|
||||
def create_acc_ui_command(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict):
|
||||
"""
|
||||
Creates a CAN message for the Ford IPC adaptive cruise, forward collision
|
||||
warning and traffic jam assist status.
|
||||
@@ -89,14 +116,32 @@ def create_acc_ui_command(packer, main_on: bool, enabled: bool, stock_values: di
|
||||
Frequency is 20Hz.
|
||||
"""
|
||||
|
||||
# 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 = {
|
||||
**stock_values,
|
||||
"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]
|
||||
"Tja_D_Stat": status,
|
||||
}
|
||||
return packer.make_can_msg("ACCDATA_3", 0, values)
|
||||
return packer.make_can_msg("ACCDATA_3", CANBUS.main, values)
|
||||
|
||||
|
||||
def spam_cancel_button(packer, cancel=1):
|
||||
def create_button_command(packer, stock_values: dict, cancel = False, resume = False, tja_toggle = False, bus = CANBUS.camera):
|
||||
"""
|
||||
Creates a CAN message for the Ford SCCM buttons/switches.
|
||||
|
||||
@@ -104,6 +149,9 @@ def spam_cancel_button(packer, cancel=1):
|
||||
"""
|
||||
|
||||
values = {
|
||||
"CcAslButtnCnclPress": cancel, # CC cancel button
|
||||
**stock_values,
|
||||
"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, # TJA toggle button
|
||||
}
|
||||
return packer.make_can_msg("Steering_Data_FD1", 0, values)
|
||||
return packer.make_can_msg("Steering_Data_FD1", bus, values)
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
|
||||
from selfdrive.car.ford.values import CAR, TransmissionType, GearShifter
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
|
||||
from selfdrive.car.ford.values import CAR, Ecu, TransmissionType, GearShifter
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
|
||||
@@ -12,59 +12,59 @@ EventName = car.CarEvent.EventName
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False):
|
||||
if car_fw is None:
|
||||
car_fw = []
|
||||
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
|
||||
ret.carName = "ford"
|
||||
#ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
|
||||
ret.dashcamOnly = True
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
|
||||
|
||||
# Angle-based steering
|
||||
# TODO: use curvature control when ready
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerActuatorDelay = 0.4
|
||||
ret.steerLimitTimer = 1.0
|
||||
|
||||
# TODO: detect stop-and-go vehicles
|
||||
stop_and_go = False
|
||||
tire_stiffness_factor = 1.0
|
||||
|
||||
if candidate == CAR.ESCAPE_MK4:
|
||||
ret.wheelbase = 2.71
|
||||
ret.steerRatio = 14.3 # Copied from Focus
|
||||
tire_stiffness_factor = 0.5328 # Copied from Focus
|
||||
ret.mass = 1750 + STD_CARGO_KG
|
||||
|
||||
elif candidate == CAR.EXPLORER_MK6:
|
||||
ret.wheelbase = 3.025
|
||||
ret.steerRatio = 16.8 # learned
|
||||
ret.mass = 2050 + STD_CARGO_KG
|
||||
|
||||
elif candidate == CAR.FOCUS_MK4:
|
||||
ret.wheelbase = 2.7
|
||||
ret.steerRatio = 14.3
|
||||
tire_stiffness_factor = 0.5328
|
||||
ret.steerRatio = 13.8 # learned
|
||||
ret.mass = 1350 + STD_CARGO_KG
|
||||
|
||||
else:
|
||||
raise ValueError(f"Unsupported car: ${candidate}")
|
||||
|
||||
# Auto Transmission: Gear_Shift_by_Wire_FD1
|
||||
# TODO: detect transmission in car_fw?
|
||||
if 0x5A in fingerprint[0]:
|
||||
# Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1
|
||||
found_ecus = [fw.ecu for fw in car_fw]
|
||||
if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[0]:
|
||||
ret.transmissionType = TransmissionType.automatic
|
||||
else:
|
||||
ret.transmissionType = TransmissionType.manual
|
||||
ret.minEnableSpeed = 20.0 * CV.MPH_TO_MS
|
||||
|
||||
# BSM: Side_Detect_L_Stat, Side_Detect_R_Stat
|
||||
# TODO: detect bsm in car_fw?
|
||||
ret.enableBsm = 0x3A6 in fingerprint[0] and 0x3A7 in fingerprint[0]
|
||||
|
||||
# min speed to enable ACC. if car can do stop and go, then set enabling speed
|
||||
# to a negative value, so it won't matter.
|
||||
ret.minEnableSpeed = -1. if (stop_and_go) else 20. * CV.MPH_TO_MS
|
||||
# LCA can steer down to zero
|
||||
ret.minSteerSpeed = 0.
|
||||
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
|
||||
ret.autoResumeSng = ret.minEnableSpeed == -1.
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
|
||||
tire_stiffness_factor=tire_stiffness_factor)
|
||||
|
||||
return ret
|
||||
|
||||
def _update(self, c):
|
||||
|
||||
@@ -1,9 +1,11 @@
|
||||
from collections import namedtuple
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
from typing import Dict, List, Union
|
||||
|
||||
from cereal import car
|
||||
from selfdrive.car import dbc_dict
|
||||
from selfdrive.car.docs_definitions import CarInfo
|
||||
from selfdrive.car.docs_definitions import CarInfo, Harness
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
@@ -20,8 +22,11 @@ class CarControllerParams:
|
||||
# Message: ACCDATA_3
|
||||
ACC_UI_STEP = 5
|
||||
|
||||
STEER_RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
|
||||
STEER_RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
|
||||
STEER_RATIO = 2.75
|
||||
STEER_DRIVER_ALLOWANCE = 0.8
|
||||
|
||||
RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
|
||||
RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
|
||||
|
||||
|
||||
class RADAR:
|
||||
@@ -37,12 +42,23 @@ class CANBUS:
|
||||
|
||||
class CAR:
|
||||
ESCAPE_MK4 = "FORD ESCAPE 4TH GEN"
|
||||
EXPLORER_MK6 = "FORD EXPLORER 6TH GEN"
|
||||
FOCUS_MK4 = "FORD FOCUS 4TH GEN"
|
||||
|
||||
|
||||
@dataclass
|
||||
class FordCarInfo(CarInfo):
|
||||
package: str = "Co-Pilot360 Assist+"
|
||||
harness: Enum = Harness.ford_q3
|
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
|
||||
CAR.ESCAPE_MK4: CarInfo("Ford Escape", "NA"),
|
||||
CAR.FOCUS_MK4: CarInfo("Ford Focus", "NA"),
|
||||
CAR.ESCAPE_MK4: [
|
||||
FordCarInfo("Ford Escape 2020"),
|
||||
FordCarInfo("Ford Kuga EU", "Driver Assistance Pack"),
|
||||
],
|
||||
CAR.EXPLORER_MK6: FordCarInfo("Ford Explorer 2020-21"),
|
||||
CAR.FOCUS_MK4: FordCarInfo("Ford Focus EU 2019", "Driver Assistance Pack"),
|
||||
}
|
||||
|
||||
|
||||
@@ -63,6 +79,33 @@ FW_VERSIONS = {
|
||||
(Ecu.engine, 0x7E0, None): [
|
||||
b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.shiftByWire, 0x732, None): [
|
||||
],
|
||||
},
|
||||
CAR.EXPLORER_MK6: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-BF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'LB5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LB5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7E0, None): [
|
||||
b'LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'MB5A-14C204-MD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.shiftByWire, 0x732, None): [
|
||||
b'L1MP-14G395-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MP-14G395-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FOCUS_MK4: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
@@ -80,11 +123,14 @@ FW_VERSIONS = {
|
||||
(Ecu.engine, 0x7E0, None): [
|
||||
b'JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.shiftByWire, 0x732, None): [
|
||||
],
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
DBC = {
|
||||
CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR),
|
||||
CAR.EXPLORER_MK6: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR),
|
||||
CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR),
|
||||
}
|
||||
|
||||
@@ -240,7 +240,7 @@ REQUESTS: List[Request] = [
|
||||
[TESTER_PRESENT_REQUEST, FORD_VERSION_REQUEST],
|
||||
[TESTER_PRESENT_RESPONSE, FORD_VERSION_RESPONSE],
|
||||
bus=0,
|
||||
whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera],
|
||||
whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire],
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
@@ -41,6 +41,7 @@ routes = [
|
||||
CarTestRoute("221c253375af4ee9|2022-06-15--18-38-24", CHRYSLER.RAM_1500),
|
||||
CarTestRoute("8fb5eabf914632ae|2022-08-04--17-28-53", CHRYSLER.RAM_HD, segment=6),
|
||||
|
||||
CarTestRoute("62241b0c7fea4589|2022-09-01--15-32-49", FORD.EXPLORER_MK6),
|
||||
#TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION),
|
||||
|
||||
CarTestRoute("7cc2a8365b4dd8a9|2018-12-02--12-10-44", GM.ACADIA),
|
||||
|
||||
@@ -13,6 +13,7 @@ TESLA AP2 MODEL S: [.nan, 2.5, .nan]
|
||||
|
||||
# Guess
|
||||
FORD ESCAPE 4TH GEN: [.nan, 1.5, .nan]
|
||||
FORD EXPLORER 6TH GEN: [.nan, 1.5, .nan]
|
||||
FORD FOCUS 4TH GEN: [.nan, 1.5, .nan]
|
||||
###
|
||||
|
||||
|
||||
Reference in New Issue
Block a user