Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

ADD FORD EDGE #212

Open
wants to merge 3 commits into
base: MAKE-PRS-HERE
Choose a base branch
from
Open

ADD FORD EDGE #212

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8dceb49
UPDATE CARSTAT EDGE
gustavoclimaco Nov 11, 2024
2940cf1
UPDATE VALUE EDGE.py
gustavoclimaco Nov 11, 2024
b3430d7
UPDATE FINGER EDGE
gustavoclimaco Nov 11, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
43 changes: 35 additions & 8 deletions selfdrive/car/ford/carstate.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,13 +9,15 @@
GearShifter = car.CarState.GearShifter
TransmissionType = car.CarParams.TransmissionType


class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
if CP.transmissionType == TransmissionType.automatic:
self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"]
if CP.flags & FordFlags.ALT_STEER_ANGLE:
self.shifter_values = can_define.dv["TransGearData"]["GearLvrPos_D_Actl"]
else:
self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"]

self.vehicle_sensors_valid = False

Expand All @@ -26,9 +28,16 @@ def update(self, cp, cp_cam, frogpilot_toggles):
ret = car.CarState.new_message()
fp_ret = custom.FrogPilotCarState.new_message()

if self.CP.flags & FordFlags.ALT_STEER_ANGLE:
self.vehicle_sensors_valid = (
int((cp.vl["ParkAid_Data"]["ExtSteeringAngleReq2"] + 1000) * 10) not in (32766, 32767)
and cp.vl["ParkAid_Data"]["EPASExtAngleStatReq"] == 0
and cp.vl["ParkAid_Data"]["ApaSys_D_Stat"] in (0, 1)
)
else:
# Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement
# The vehicle usually recovers out of this state within a minute of normal driving
self.vehicle_sensors_valid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] == 3
self.vehicle_sensors_valid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] == 3

# car speed
ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS
Expand All @@ -46,7 +55,14 @@ def update(self, cp, cp_cam, frogpilot_toggles):
ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2)

# steering wheel
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
if self.CP.flags & FordFlags.ALT_STEER_ANGLE:
steering_angle_init = cp.vl["SteeringPinion_Data_Alt"]["StePinRelInit_An_Sns"]
if self.vehicle_sensors_valid:
steering_angle_est = cp.vl["ParkAid_Data"]["ExtSteeringAngleReq2"]
self.steering_angle_offset_deg = steering_angle_est - steering_angle_init
ret.steeringAngleDeg = steering_angle_init + self.steering_angle_offset_deg
else:
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1
Expand All @@ -70,8 +86,11 @@ def update(self, cp, cp_cam, frogpilot_toggles):

# gear
if self.CP.transmissionType == TransmissionType.automatic:
gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"])
ret.gearShifter = self.parse_gear_shifter(gear)
if self.CP.flags & FordFlags.ALT_STEER_ANGLE:
gear = self.shifter_values.get(cp.vl["TransGearData"]["GearLvrPos_D_Actl"])
else:
gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"])
ret.gearShifter = self.parse_gear_shifter(gear)
elif self.CP.transmissionType == TransmissionType.manual:
ret.clutchPressed = cp.vl["Engine_Clutch_Data"]["CluPdlPos_Pc_Meas"] > 0
if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]):
Expand Down Expand Up @@ -126,13 +145,21 @@ def get_can_parser(CP):
("BrakeSnData_4", 50),
("EngBrakeData", 10),
("Cluster_Info1_FD1", 10),
("SteeringPinion_Data", 100),
("EPAS_INFO", 50),
("Steering_Data_FD1", 10),
("BodyInfo_3_FD1", 2),
("RCMStatusMessage2_FD1", 10),
]

if CP.flags & FordFlags.ALT_STEER_ANGLE:
messages += [
("SteeringPinion_Data_Alt", 100),
("ParkAid_Data", 50),
("TransGearData",10),
]
else:
messages += [
("SteeringPinion_Data", 100),
]
if CP.flags & FordFlags.CANFD:
messages += [
("Lane_Assist_Data3_FD1", 33),
Expand Down
16 changes: 16 additions & 0 deletions selfdrive/car/ford/fingerprints.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,22 @@
Ecu = car.CarParams.Ecu

FW_VERSIONS = {

CAR.FORD_EDGE_MK2: {
(Ecu.eps, 0x730, None): [
b'K2GC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'HG9C-2D053-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'HG9C-2D053-MG\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'KT4T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_BRONCO_SPORT_MK1: {
(Ecu.eps, 0x730, None): [
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
Expand Down
24 changes: 9 additions & 15 deletions selfdrive/car/ford/values.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@

Ecu = car.CarParams.Ecu


class CarControllerParams:
STEER_STEP = 5 # LateralMotionControl, 20Hz
LKA_STEP = 3 # Lane_Assist_Data1, 33Hz
Expand Down Expand Up @@ -40,25 +39,22 @@ class CarControllerParams:
def __init__(self, CP):
pass


class FordFlags(IntFlag):
# Static flags
CANFD = 1

ALT_STEER_ANGLE = 2

class RADAR:
DELPHI_ESR = 'ford_fusion_2018_adas'
DELPHI_MRR = 'FORD_CADS'


class Footnote(Enum):
FOCUS = CarFootnote(
"Refers only to the Focus Mk4 (C519) available in Europe/China/Taiwan/Australasia, not the Focus Mk3 (C346) in " +
"North and South America/Southeast Asia.",
Column.MODEL,
)


@dataclass
class FordCarDocs(CarDocs):
package: str = "Co-Pilot360 Assist+"
Expand All @@ -72,7 +68,6 @@ def init_make(self, CP: car.CarParams):
else:
self.car_parts = CarParts([Device.threex, harness])


@dataclass
class FordPlatformConfig(PlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR))
Expand All @@ -86,7 +81,6 @@ def init(self):
name = f"{car_docs.make} {car_docs.model} Plug-in Hybrid {car_docs.years}"
self.car_docs.append(replace(copy.deepcopy(car_docs), name=name))


@dataclass
class FordCANFDPlatformConfig(FordPlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('ford_lincoln_base_pt', None))
Expand All @@ -95,12 +89,19 @@ def init(self):
super().init()
self.flags |= FordFlags.CANFD


class CAR(Platforms):
FORD_BRONCO_SPORT_MK1 = FordPlatformConfig(
[FordCarDocs("Ford Bronco Sport 2021-23")],
CarSpecs(mass=1625, wheelbase=2.67, steerRatio=17.7),
)
FORD_EDGE_MK2 = FordPlatformConfig(
[
FordCarDocs("Ford Edge 2022"),
FordCarDocs("Ford Fusion Retrofited 2013-2019"),
],
CarSpecs(mass=1691, steerRatio=15.3, wheelbase=2.824),
flags=FordFlags.ALT_STEER_ANGLE,
)
FORD_ESCAPE_MK4 = FordPlatformConfig(
[
FordCarDocs("Ford Escape 2020-22", hybrid=True, plug_in_hybrid=True),
Expand Down Expand Up @@ -143,7 +144,6 @@ class CAR(Platforms):
CarSpecs(mass=2000, wheelbase=3.27, steerRatio=17.0),
)


# FW response contains a combined software and part number
# A-Z except no I, O or W
# e.g. NZ6A-14C204-AAA
Expand All @@ -158,7 +158,6 @@ class CAR(Platforms):
b'(?P<part_number>[0-9' + FW_ALPHABET + b']{5,6})-' +
b'(?P<software_revision>[' + FW_ALPHABET + b']{2,})\x00*$')


def get_platform_codes(fw_versions: list[bytes] | set[bytes]) -> set[tuple[bytes, bytes]]:
codes = set()
for fw in fw_versions:
Expand All @@ -168,7 +167,6 @@ def get_platform_codes(fw_versions: list[bytes] | set[bytes]) -> set[tuple[bytes

return codes


def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, vin: str, offline_fw_versions: OfflineFwVersions) -> set[str]:
candidates: set[str] = set()

Expand Down Expand Up @@ -210,7 +208,6 @@ def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, vin: str, offline_fw

return candidates


# All of these ECUs must be present and are expected to have platform codes we can match
PLATFORM_CODE_ECUS = (Ecu.abs, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.eps)

Expand All @@ -232,15 +229,12 @@ def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, vin: str, offline_fw
(21, [Ecu.fwdCamera]),
]


def ford_asbuilt_block_request(block_id: int):
return bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + p16(DATA_IDENTIFIER_FORD_ASBUILT + block_id - 1)


def ford_asbuilt_block_response(block_id: int):
return bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + p16(DATA_IDENTIFIER_FORD_ASBUILT + block_id - 1)


FW_QUERY_CONFIG = FwQueryConfig(
requests=[
# CAN and CAN FD queries are combined.
Expand Down
Loading