sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Ford Explorer 2020-21 (#25614)

Browse Source 
* 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>
pull/25686/head
Adeeb Shihadeh 3 years ago committed by GitHub
parent d9ca45ed1b
commit 9241de2210
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 236 additions and 83 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      panda
  2. 70
      selfdrive/car/ford/carcontroller.py
  3. 49
      selfdrive/car/ford/carstate.py
  4. 88
      selfdrive/car/ford/fordcan.py
  5. 40
      selfdrive/car/ford/interface.py
  6. 56
      selfdrive/car/ford/values.py
  7. 2
      selfdrive/car/fw_versions.py
  8. 1
      selfdrive/car/tests/routes.py
  9. 1
      selfdrive/car/torque_data/override.yaml

2
panda

@ -1 +1 @@
Subproject commit 0ca23b67786d0fa2a3162371aadeca52d24a8958 Subproject commit 8f13ca3f66bcc72e3ac9028df7ce04851e7e3a48

70
selfdrive/car/ford/carcontroller.py
Unescape View File

@ -1,3 +1,4 @@
import math
from cereal import car from cereal import car
from common.numpy_fast import clip, interp from common.numpy_fast import clip, interp
from opendbc.can.packer import CANPacker from opendbc.can.packer import CANPacker
@ -7,14 +8,19 @@ from selfdrive.car.ford.values import CarControllerParams
VisualAlert = car.CarControl.HUDControl.VisualAlert 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 # rate limit
steer_up = apply_steer * apply_steer_last > 0. and abs(apply_steer) > abs(apply_steer_last) steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last)
rate_limit = CarControllerParams.STEER_RATE_LIMIT_UP if steer_up else CarControllerParams.STEER_RATE_LIMIT_DOWN 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) 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: class CarController:
@ -24,7 +30,7 @@ class CarController:
self.packer = CANPacker(dbc_name) self.packer = CANPacker(dbc_name)
self.frame = 0 self.frame = 0
self.apply_steer_last = 0 self.apply_angle_last = 0
self.main_on_last = False self.main_on_last = False
self.lkas_enabled_last = False self.lkas_enabled_last = False
self.steer_alert_last = False self.steer_alert_last = False
@ -38,52 +44,62 @@ class CarController:
main_on = CS.out.cruiseState.available main_on = CS.out.cruiseState.available
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
### acc buttons ###
if CC.cruiseControl.cancel: if CC.cruiseControl.cancel:
# cancel stock ACC can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True))
can_sends.append(fordcan.spam_cancel_button(self.packer)) elif CC.cruiseControl.resume:
can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True))
# 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))
# apply rate limits ### lateral control ###
new_steer = actuators.steeringAngleDeg if CC.latActive:
apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo) 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 # send steering commands at 20Hz
if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0: if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0:
lca_rq = 1 if CC.latActive else 0 lca_rq = 1 if CC.latActive else 0
# use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented # use LatCtlPath_An_Actl to actuate steering
path_angle = apply_steer # 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 # ramp rate: 0=Slow, 1=Medium, 2=Fast, 3=Immediately
curvature = self.VM.calc_curvature(apply_steer, CS.out.vEgo, 0.0) # 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 offset_roll_compensation_curvature = clip(self.VM.calc_curvature(0, CS.out.vEgo, -CS.yaw_data["VehYaw_W_Actl"]), -0.02, 0.02094)
curvature_rate = 0
path_offset = 0
ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately self.apply_angle_last = apply_angle
precision = 0 # 0=Comfortable, 1=Precise can_sends.append(fordcan.create_lka_command(self.packer, apply_angle, 0))
self.apply_steer_last = apply_steer
can_sends.append(fordcan.create_lkas_command(self.packer, apply_steer, curvature))
can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision, 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_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 # send lkas ui command at 1Hz or if ui state changes
if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: 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 # send acc ui command at 20Hz or if ui state changes
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: 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.main_on_last = main_on
self.lkas_enabled_last = CC.latActive self.lkas_enabled_last = CC.latActive
self.steer_alert_last = steer_alert self.steer_alert_last = steer_alert
new_actuators = actuators.copy() new_actuators = actuators.copy()
new_actuators.steeringAngleDeg = apply_steer new_actuators.steeringAngleDeg = apply_angle
self.frame += 1 self.frame += 1
return new_actuators, can_sends return new_actuators, can_sends

49
selfdrive/car/ford/carstate.py
Unescape View File

@ -3,7 +3,7 @@ from common.conversions import Conversions as CV
from opendbc.can.can_define import CANDefine from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import CarStateBase 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 GearShifter = car.CarState.GearShifter
TransmissionType = car.CarParams.TransmissionType TransmissionType = car.CarParams.TransmissionType
@ -22,7 +22,7 @@ class CarState(CarStateBase):
# car speed # car speed
ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) 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 ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1
# gas pedal # gas pedal
@ -37,7 +37,7 @@ class CarState(CarStateBase):
# steering wheel # steering wheel
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"] ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"] 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.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1
ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3) ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3)
# ret.espDisabled = False # TODO: find traction control signal # 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.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.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.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 # gear
if self.CP.transmissionType == TransmissionType.automatic: if self.CP.transmissionType == TransmissionType.automatic:
@ -65,6 +67,7 @@ class CarState(CarStateBase):
# button presses # button presses
ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1 ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1
ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2 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"]) ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
# lock info # lock info
@ -77,9 +80,13 @@ class CarState(CarStateBase):
ret.leftBlindspot = cp.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0 ret.leftBlindspot = cp.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0
ret.rightBlindspot = cp.vl["Side_Detect_R_Stat"]["SodDetctRight_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 # Stock values from IPMA so that we can retain some stock functionality
self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"] self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"] self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
# Use stock sensor values
self.yaw_data = cp.vl["Yaw_Data_FD1"]
return ret return ret
@ -97,6 +104,8 @@ class CarState(CarStateBase):
("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph) ("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph)
# The units might change with IPC settings? # The units might change with IPC settings?
("CcStat_D_Actl", "EngBrakeData"), # PCM ACC status ("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) ("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg)
# Calculates steering angle (and offset) from pinion # Calculates steering angle (and offset) from pinion
# angle and driving measurements. # angle and driving measurements.
@ -104,7 +113,6 @@ class CarState(CarStateBase):
# to zero at the beginning of the drive. # to zero at the beginning of the drive.
("SteeringColumnTorque", "EPAS_INFO"), # PSCM steering column torque (Nm) ("SteeringColumnTorque", "EPAS_INFO"), # PSCM steering column torque (Nm)
("EPAS_Failure", "EPAS_INFO"), # PSCM EPAS status ("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 ("TurnLghtSwtch_D_Stat", "Steering_Data_FD1"), # SCCM Turn signal switch
("TjaButtnOnOffPress", "Steering_Data_FD1"), # SCCM ACC button, lane-centering/traffic jam assist toggle ("TjaButtnOnOffPress", "Steering_Data_FD1"), # SCCM ACC button, lane-centering/traffic jam assist toggle
("DrStatDrv_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, driver ("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 ("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left
("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right ("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right
("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"), # RCM Seatbelt status, driver ("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 = [ checks = [
@ -122,6 +162,7 @@ class CarState(CarStateBase):
("EngVehicleSpThrottle", 100), ("EngVehicleSpThrottle", 100),
("BrakeSnData_4", 50), ("BrakeSnData_4", 50),
("EngBrakeData", 10), ("EngBrakeData", 10),
("Cluster_Info1_FD1", 10),
("SteeringPinion_Data", 100), ("SteeringPinion_Data", 100),
("EPAS_INFO", 50), ("EPAS_INFO", 50),
("Lane_Assist_Data3_FD1", 33), ("Lane_Assist_Data3_FD1", 33),

88
selfdrive/car/ford/fordcan.py
Unescape View File

@ -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. 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] "LdwActvStats_D_Req": 0, # LDW status [0|7]
"LdwActvIntns_D_Req": 0, # LDW intensity [0|3], shake alert strength "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): def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float, curvature_rate: float, curvature: float):
@ -43,15 +46,15 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path
"LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight, 4-7=NotUsed [0|7] "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] "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] "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 "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
"LatCtlPath_An_Actl": clip(path_angle, -0.5, 0.5235), # Path angle [-0.5|0.5235] radians "LatCtlPath_An_Actl": path_angle, # Path angle [-0.4995|0.5240] radians
"LatCtlCurv_NoRate_Actl": clip(curvature_rate, -0.001024, 0.00102375), # Curvature rate [-0.001024|0.00102375] 1/meter^2 "LatCtlCurv_NoRate_Actl": curvature_rate, # 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 "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. 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 # 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: 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: elif main_on:
lines = 0 lines = 0
else: 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 = { values = {
**stock_values, **stock_values,
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31] "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 Creates a CAN message for the Ford IPC adaptive cruise, forward collision
warning and traffic jam assist status. 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. 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 = { values = {
**stock_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. Creates a CAN message for the Ford SCCM buttons/switches.
@ -104,6 +149,9 @@ def spam_cancel_button(packer, cancel=1):
""" """
values = { 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)

40
selfdrive/car/ford/interface.py
Unescape View File

@ -1,8 +1,8 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
from cereal import car from cereal import car
from common.conversions import Conversions as CV 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 import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
from selfdrive.car.ford.values import CAR, TransmissionType, GearShifter from selfdrive.car.ford.values import CAR, Ecu, TransmissionType, GearShifter
from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.interfaces import CarInterfaceBase
@ -12,59 +12,59 @@ EventName = car.CarEvent.EventName
class CarInterface(CarInterfaceBase): class CarInterface(CarInterfaceBase):
@staticmethod @staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): 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 = CarInterfaceBase.get_std_params(candidate, fingerprint)
ret.carName = "ford" ret.carName = "ford"
#ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
ret.dashcamOnly = True ret.dashcamOnly = True
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
# Angle-based steering # Angle-based steering
# TODO: use curvature control when ready
ret.steerControlType = car.CarParams.SteerControlType.angle ret.steerControlType = car.CarParams.SteerControlType.angle
ret.steerActuatorDelay = 0.1 ret.steerActuatorDelay = 0.4
ret.steerLimitTimer = 1.0 ret.steerLimitTimer = 1.0
tire_stiffness_factor = 1.0
# TODO: detect stop-and-go vehicles
stop_and_go = False
if candidate == CAR.ESCAPE_MK4: if candidate == CAR.ESCAPE_MK4:
ret.wheelbase = 2.71 ret.wheelbase = 2.71
ret.steerRatio = 14.3 # Copied from Focus ret.steerRatio = 14.3 # Copied from Focus
tire_stiffness_factor = 0.5328 # Copied from Focus
ret.mass = 1750 + STD_CARGO_KG 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: elif candidate == CAR.FOCUS_MK4:
ret.wheelbase = 2.7 ret.wheelbase = 2.7
ret.steerRatio = 14.3 ret.steerRatio = 13.8 # learned
tire_stiffness_factor = 0.5328
ret.mass = 1350 + STD_CARGO_KG ret.mass = 1350 + STD_CARGO_KG
else: else:
raise ValueError(f"Unsupported car: ${candidate}") raise ValueError(f"Unsupported car: ${candidate}")
# Auto Transmission: Gear_Shift_by_Wire_FD1 # Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1
# TODO: detect transmission in car_fw? found_ecus = [fw.ecu for fw in car_fw]
if 0x5A in fingerprint[0]: if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[0]:
ret.transmissionType = TransmissionType.automatic ret.transmissionType = TransmissionType.automatic
else: else:
ret.transmissionType = TransmissionType.manual ret.transmissionType = TransmissionType.manual
ret.minEnableSpeed = 20.0 * CV.MPH_TO_MS
# BSM: Side_Detect_L_Stat, Side_Detect_R_Stat # BSM: Side_Detect_L_Stat, Side_Detect_R_Stat
# TODO: detect bsm in car_fw? # TODO: detect bsm in car_fw?
ret.enableBsm = 0x3A6 in fingerprint[0] and 0x3A7 in fingerprint[0] 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 # LCA can steer down to zero
ret.minSteerSpeed = 0. ret.minSteerSpeed = 0.
ret.centerToFront = ret.wheelbase * 0.44 ret.autoResumeSng = ret.minEnableSpeed == -1.
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) 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, ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor) tire_stiffness_factor=tire_stiffness_factor)
return ret return ret
def _update(self, c): def _update(self, c):

56
selfdrive/car/ford/values.py
Unescape View File

@ -1,9 +1,11 @@
from collections import namedtuple from collections import namedtuple
from dataclasses import dataclass
from enum import Enum
from typing import Dict, List, Union from typing import Dict, List, Union
from cereal import car from cereal import car
from selfdrive.car import dbc_dict 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 Ecu = car.CarParams.Ecu
TransmissionType = car.CarParams.TransmissionType TransmissionType = car.CarParams.TransmissionType
@ -20,8 +22,11 @@ class CarControllerParams:
# Message: ACCDATA_3 # Message: ACCDATA_3
ACC_UI_STEP = 5 ACC_UI_STEP = 5
STEER_RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) STEER_RATIO = 2.75
STEER_RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) 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: class RADAR:
@ -37,12 +42,23 @@ class CANBUS:
class CAR: class CAR:
ESCAPE_MK4 = "FORD ESCAPE 4TH GEN" ESCAPE_MK4 = "FORD ESCAPE 4TH GEN"
EXPLORER_MK6 = "FORD EXPLORER 6TH GEN"
FOCUS_MK4 = "FORD FOCUS 4TH 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_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
CAR.ESCAPE_MK4: CarInfo("Ford Escape", "NA"), CAR.ESCAPE_MK4: [
CAR.FOCUS_MK4: CarInfo("Ford Focus", "NA"), 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): [ (Ecu.engine, 0x7E0, None): [
b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00', 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: { CAR.FOCUS_MK4: {
(Ecu.eps, 0x730, None): [ (Ecu.eps, 0x730, None): [
@ -80,11 +123,14 @@ FW_VERSIONS = {
(Ecu.engine, 0x7E0, None): [ (Ecu.engine, 0x7E0, None): [
b'JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00',
], ],
(Ecu.shiftByWire, 0x732, None): [
],
}, },
} }
DBC = { DBC = {
CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR), 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), CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR),
} }

2
selfdrive/car/fw_versions.py
Unescape View File

@ -240,7 +240,7 @@ REQUESTS: List[Request] = [
[TESTER_PRESENT_REQUEST, FORD_VERSION_REQUEST], [TESTER_PRESENT_REQUEST, FORD_VERSION_REQUEST],
[TESTER_PRESENT_RESPONSE, FORD_VERSION_RESPONSE], [TESTER_PRESENT_RESPONSE, FORD_VERSION_RESPONSE],
bus=0, bus=0,
whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera], whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire],
), ),
] ]

1
selfdrive/car/tests/routes.py
Unescape View File

@ -41,6 +41,7 @@ routes = [
CarTestRoute("221c253375af4ee9|2022-06-15--18-38-24", CHRYSLER.RAM_1500), 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("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), #TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION),
CarTestRoute("7cc2a8365b4dd8a9|2018-12-02--12-10-44", GM.ACADIA), CarTestRoute("7cc2a8365b4dd8a9|2018-12-02--12-10-44", GM.ACADIA),

1
selfdrive/car/torque_data/override.yaml
Unescape View File

@ -13,6 +13,7 @@ TESLA AP2 MODEL S: [.nan, 2.5, .nan]
# Guess # Guess
FORD ESCAPE 4TH GEN: [.nan, 1.5, .nan] FORD ESCAPE 4TH GEN: [.nan, 1.5, .nan]
FORD EXPLORER 6TH GEN: [.nan, 1.5, .nan]
FORD FOCUS 4TH GEN: [.nan, 1.5, .nan] FORD FOCUS 4TH GEN: [.nan, 1.5, .nan]
### ###

Write Preview
Loading…
Cancel
Save