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
Split 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 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))
# 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
new_steer = actuators.steeringAngleDeg
apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo)
### 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

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.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),

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.
@ -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):
@ -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]
"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
"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)

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

@ -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):

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

@ -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),
}

2
selfdrive/car/fw_versions.py
Unescape View File

@ -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],
),
]

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("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),

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

@ -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]
###

Write Preview
Loading…
Cancel
Save