Release 260111
This commit is contained in:
Comma Device
0
opendbc_repo/opendbc/car/ford/__init__.py
Normal file
0
opendbc_repo/opendbc/car/ford/__init__.py
Normal file
179
opendbc_repo/opendbc/car/ford/carcontroller.py
Normal file
179
opendbc_repo/opendbc/car/ford/carcontroller.py
Normal file
@@ -0,0 +1,179 @@
|
||||
import math
|
||||
import numpy as np
|
||||
from opendbc.can.packer import CANPacker
|
||||
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, apply_std_steer_angle_limits, structs
|
||||
from opendbc.car.ford import fordcan
|
||||
from opendbc.car.ford.values import CarControllerParams, FordFlags
|
||||
from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX
|
||||
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||
|
||||
# ISO 11270
|
||||
ISO_LATERAL_ACCEL = 3.0 # m/s^2 # TODO: import from test lateral limits file?
|
||||
|
||||
# Limit to average banked road since safety doesn't have the roll
|
||||
EARTH_G = 9.81
|
||||
AVERAGE_ROAD_ROLL = 0.06 # ~3.4 degrees, 6% superelevation
|
||||
MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - (EARTH_G * AVERAGE_ROAD_ROLL) # ~2.4 m/s^2
|
||||
|
||||
|
||||
def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw, steering_angle, lat_active, CP):
|
||||
# No blending at low speed due to lack of torque wind-up and inaccurate current curvature
|
||||
if v_ego_raw > 9:
|
||||
apply_curvature = np.clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR,
|
||||
current_curvature + CarControllerParams.CURVATURE_ERROR)
|
||||
|
||||
# Curvature rate limit after driver torque limit
|
||||
apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS)
|
||||
|
||||
# Ford Q4/CAN FD has more torque available compared to Q3/CAN so we limit it based on lateral acceleration.
|
||||
# Safety is not aware of the road roll so we subtract a conservative amount at all times
|
||||
if CP.flags & FordFlags.CANFD:
|
||||
# Limit curvature to conservative max lateral acceleration
|
||||
curvature_accel_limit = MAX_LATERAL_ACCEL / (max(v_ego_raw, 1) ** 2)
|
||||
apply_curvature = float(np.clip(apply_curvature, -curvature_accel_limit, curvature_accel_limit))
|
||||
|
||||
return apply_curvature
|
||||
|
||||
|
||||
def apply_creep_compensation(accel: float, v_ego: float) -> float:
|
||||
creep_accel = np.interp(v_ego, [1., 3.], [0.6, 0.])
|
||||
creep_accel = np.interp(accel, [0., 0.2], [creep_accel, 0.])
|
||||
accel -= creep_accel
|
||||
return float(accel)
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_names, CP):
|
||||
super().__init__(dbc_names, CP)
|
||||
self.packer = CANPacker(dbc_names[Bus.pt])
|
||||
self.CAN = fordcan.CanBus(CP)
|
||||
|
||||
self.apply_curvature_last = 0
|
||||
self.accel = 0.0
|
||||
self.gas = 0.0
|
||||
self.brake_request = False
|
||||
self.main_on_last = False
|
||||
self.lkas_enabled_last = False
|
||||
self.steer_alert_last = False
|
||||
self.lead_distance_bars_last = None
|
||||
self.distance_bar_frame = 0
|
||||
|
||||
def update(self, CC, CS, now_nanos):
|
||||
can_sends = []
|
||||
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
|
||||
main_on = CS.out.cruiseState.available
|
||||
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
|
||||
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
|
||||
|
||||
### acc buttons ###
|
||||
if CC.cruiseControl.cancel:
|
||||
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True))
|
||||
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, cancel=True))
|
||||
elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
|
||||
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, resume=True))
|
||||
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, resume=True))
|
||||
# if stock lane centering isn't off, send a button press to toggle it off
|
||||
# the stock system checks for steering pressed, and eventually disengages cruise control
|
||||
elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0:
|
||||
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, tja_toggle=True))
|
||||
|
||||
### lateral control ###
|
||||
# send steer msg at 20Hz
|
||||
if (self.frame % CarControllerParams.STEER_STEP) == 0:
|
||||
# apply rate limits, curvature error limit, and clip to signal range
|
||||
current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
|
||||
self.apply_curvature_last = apply_ford_curvature_limits(actuators.curvature, self.apply_curvature_last, current_curvature,
|
||||
CS.out.vEgoRaw, 0., CC.latActive, self.CP)
|
||||
|
||||
if self.CP.flags & FordFlags.CANFD:
|
||||
# TODO: extended mode
|
||||
# Ford uses four individual signals to dictate how to drive to the car. Curvature alone (limited to 0.02m/s^2)
|
||||
# can actuate the steering for a large portion of any lateral movements. However, in order to get further control on
|
||||
# steer actuation, the other three signals are necessary. Ford controls vehicles differently than most other makes.
|
||||
# A detailed explanation on ford control can be found here:
|
||||
# https://www.f150gen14.com/forum/threads/introducing-bluepilot-a-ford-specific-fork-for-comma3x-openpilot.24241/#post-457706
|
||||
mode = 1 if CC.latActive else 0
|
||||
counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10
|
||||
can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -self.apply_curvature_last, 0., counter))
|
||||
else:
|
||||
can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -self.apply_curvature_last, 0.))
|
||||
|
||||
# send lka msg at 33Hz
|
||||
if (self.frame % CarControllerParams.LKA_STEP) == 0:
|
||||
can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN))
|
||||
|
||||
### longitudinal control ###
|
||||
# send acc msg at 50Hz
|
||||
if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
|
||||
accel = actuators.accel
|
||||
gas = accel
|
||||
|
||||
if CC.longActive:
|
||||
# Compensate for engine creep at low speed.
|
||||
# Either the ABS does not account for engine creep, or the correction is very slow
|
||||
# TODO: verify this applies to EV/hybrid
|
||||
accel = apply_creep_compensation(accel, CS.out.vEgo)
|
||||
|
||||
# The stock system has been seen rate limiting the brake accel to 5 m/s^3,
|
||||
# however even 3.5 m/s^3 causes some overshoot with a step response.
|
||||
accel = max(accel, self.accel - (3.5 * CarControllerParams.ACC_CONTROL_STEP * DT_CTRL))
|
||||
|
||||
accel = float(np.clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
gas = float(np.clip(gas, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
|
||||
# Both gas and accel are in m/s^2, accel is used solely for braking
|
||||
if not CC.longActive or gas < CarControllerParams.MIN_GAS:
|
||||
gas = CarControllerParams.INACTIVE_GAS
|
||||
|
||||
# PCM applies pitch compensation to gas/accel, but we need to compensate for the brake/pre-charge bits
|
||||
accel_due_to_pitch = 0.0
|
||||
if len(CC.orientationNED) == 3:
|
||||
accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
|
||||
|
||||
accel_pitch_compensated = accel + accel_due_to_pitch
|
||||
if accel_pitch_compensated > 0.3 or not CC.longActive:
|
||||
self.brake_request = False
|
||||
elif accel_pitch_compensated < 0.0:
|
||||
self.brake_request = True
|
||||
|
||||
stopping = CC.actuators.longControlState == LongCtrlState.stopping
|
||||
# TODO: look into using the actuators packet to send the desired speed
|
||||
can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX))
|
||||
|
||||
self.accel = accel
|
||||
self.gas = gas
|
||||
|
||||
### 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 msg 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_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
|
||||
|
||||
# send acc ui msg at 5Hz or if ui state changes
|
||||
if hud_control.leadDistanceBars != self.lead_distance_bars_last:
|
||||
send_ui = True
|
||||
self.distance_bar_frame = self.frame
|
||||
|
||||
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
|
||||
show_distance_bars = self.frame - self.distance_bar_frame < 400
|
||||
can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive,
|
||||
fcw_alert, CS.out.cruiseState.standstill, show_distance_bars,
|
||||
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
|
||||
self.lead_distance_bars_last = hud_control.leadDistanceBars
|
||||
|
||||
new_actuators = actuators.as_builder()
|
||||
new_actuators.curvature = self.apply_curvature_last
|
||||
new_actuators.accel = self.accel
|
||||
new_actuators.gas = self.gas
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
127
opendbc_repo/opendbc/car/ford/carstate.py
Normal file
127
opendbc_repo/opendbc/car/ford/carstate.py
Normal file
@@ -0,0 +1,127 @@
|
||||
from opendbc.can import CANDefine, CANParser
|
||||
from opendbc.car import Bus, create_button_events, structs
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.ford.fordcan import CanBus
|
||||
from opendbc.car.ford.values import DBC, CarControllerParams, FordFlags
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
GearShifter = structs.CarState.GearShifter
|
||||
TransmissionType = structs.CarParams.TransmissionType
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
|
||||
if CP.transmissionType == TransmissionType.automatic:
|
||||
self.shifter_values = can_define.dv["PowertrainData_10"]["TrnRng_D_Rq"]
|
||||
|
||||
self.distance_button = 0
|
||||
self.lc_button = 0
|
||||
|
||||
def update(self, can_parsers) -> structs.CarState:
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
|
||||
ret = structs.CarState()
|
||||
|
||||
# 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
|
||||
ret.vehicleSensorsInvalid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] != 3
|
||||
|
||||
# car speed
|
||||
ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"]
|
||||
ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1
|
||||
|
||||
# gas pedal
|
||||
ret.gas = cp.vl["EngVehicleSpThrottle"]["ApedPos_Pc_ActlArb"] / 100.
|
||||
ret.gasPressed = ret.gas > 1e-6
|
||||
|
||||
# brake pedal
|
||||
ret.brake = cp.vl["BrakeSnData_4"]["BrkTot_Tq_Actl"] / 32756. # torque in Nm
|
||||
ret.brakePressed = cp.vl["EngBrakeData"]["BpedDrvAppl_D_Actl"] == 2
|
||||
ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2)
|
||||
|
||||
# steering wheel
|
||||
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
|
||||
ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3)
|
||||
ret.espDisabled = cp.vl["Cluster_Info1_FD1"]["DrvSlipCtlMde_D_Rq"] != 0 # 0 is default mode
|
||||
|
||||
if self.CP.flags & FordFlags.CANFD:
|
||||
# this signal is always 0 on non-CAN FD cars
|
||||
ret.steerFaultTemporary |= cp.vl["Lane_Assist_Data3_FD1"]["LatCtlSte_D_Stat"] not in (1, 2, 3)
|
||||
|
||||
# cruise state
|
||||
is_metric = cp.vl["INSTRUMENT_PANEL"]["METRIC_UNITS"] == 1 if not self.CP.flags & FordFlags.CANFD else False
|
||||
ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * (CV.KPH_TO_MS if is_metric else 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
|
||||
ret.accFaulted = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (1, 2)
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
ret.accFaulted = ret.accFaulted or cp_cam.vl["ACCDATA"]["CmbbDeny_B_Actl"] == 1
|
||||
|
||||
# gear
|
||||
if self.CP.transmissionType == TransmissionType.automatic:
|
||||
gear = self.shifter_values.get(cp.vl["PowertrainData_10"]["TrnRng_D_Rq"])
|
||||
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"]):
|
||||
ret.gearShifter = GearShifter.reverse
|
||||
else:
|
||||
ret.gearShifter = GearShifter.drive
|
||||
|
||||
ret.engineRpm = cp.vl["EngVehicleSpThrottle"]["EngAout_N_Actl"]
|
||||
|
||||
# safety
|
||||
ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"])
|
||||
ret.stockAeb = bool(cp_cam.vl["ACCDATA_2"]["CmbbBrkDecel_B_Rq"])
|
||||
|
||||
# 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"])
|
||||
prev_distance_button = self.distance_button
|
||||
prev_lc_button = self.lc_button
|
||||
self.distance_button = cp.vl["Steering_Data_FD1"]["AccButtnGapTogglePress"]
|
||||
self.lc_button = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
|
||||
|
||||
# lock info
|
||||
ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"],
|
||||
cp.vl["BodyInfo_3_FD1"]["DrStatRl_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatRr_B_Actl"]])
|
||||
ret.seatbeltUnlatched = cp.vl["RCMStatusMessage2_FD1"]["FirstRowBuckleDriver"] == 2
|
||||
|
||||
# blindspot sensors
|
||||
if self.CP.enableBsm:
|
||||
cp_bsm = cp_cam if self.CP.flags & FordFlags.CANFD else cp
|
||||
ret.leftBlindspot = cp_bsm.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0
|
||||
ret.rightBlindspot = cp_bsm.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"]
|
||||
|
||||
ret.buttonEvents = [
|
||||
*create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}),
|
||||
*create_button_events(self.lc_button, prev_lc_button, {1: ButtonType.lkas}),
|
||||
]
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers(CP):
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).main),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).camera),
|
||||
}
|
||||
223
opendbc_repo/opendbc/car/ford/fingerprints.py
Normal file
223
opendbc_repo/opendbc/car/ford/fingerprints.py
Normal file
@@ -0,0 +1,223 @@
|
||||
""" AUTO-FORMATTED USING opendbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.ford.values import CAR
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.FORD_BRONCO_SPORT_MK1: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'LX6C-2D053-RD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-2D053-RE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-2D053-RF\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'M1PT-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'M1PT-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_ESCAPE_MK4: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'LX6C-14D003-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'LX6C-2D053-NS\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-2D053-NT\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-2D053-NY\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-2D053-SA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-2D053-SD\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'LJ6T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LJ6T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LV4T-14F397-GG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_ESCAPE_MK4_5: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'PZ11-14D003-EA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'PZ1C-2D053-EJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'PJ6T-14H102-ABL\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_EXPLORER_MK6: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
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',
|
||||
b'M1MC-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'M1MC-14D003-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'P1MC-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'L1MC-2D053-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-BD\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',
|
||||
b'L1MC-2D053-BJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-KB\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-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
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',
|
||||
b'LC5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LC5T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_EXPEDITION_MK4: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'NL14-14D003-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'RL14-2D053-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'ML3T-14H102-ABT\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_F_150_MK14: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'ML3V-14D003-BC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'ML3V-14D003-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'NL34-2D053-CA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PL34-2D053-CA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PL34-2D053-CC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PL3V-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PL3V-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'ML3T-14D049-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'ML3T-14H102-ABR\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'ML3T-14H102-ABS\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'ML3T-14H102-ABT\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PJ6T-14H102-ABS\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RJ6T-14H102-ACJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RJ6T-14H102-BBC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_F_150_LIGHTNING_MK1: {
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'PL38-2D053-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RL38-2D053-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'ML3T-14H102-ABT\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RJ6T-14H102-ACJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RJ6T-14H102-BBC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'RL38-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_MUSTANG_MACH_E_MK1: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'LJ9C-14D003-AM\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LJ9C-14D003-CC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LJ9C-14D003-FA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LJ9C-14D003-GA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LJ9C-14D003-HA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'LK9C-2D053-CK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LK9C-2D053-CN\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'ML3T-14H102-ABS\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RJ6T-14H102-BAE\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_FOCUS_MK4: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'JX61-2D053-CJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'JX7T-14D049-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'JX7T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_MAVERICK_MK1: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'NZ6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'NZ6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'NZ6C-2D053-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'NZ6C-2D053-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'NZ6C-2D053-AG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PZ6C-2D053-ED\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PZ6C-2D053-EE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PZ6C-2D053-EF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'NZ6T-14D049-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'NZ6T-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_RANGER_MK2: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'NB3C-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'NL14-14D003-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RB3C-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'PB3C-2D053-ZD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PB3C-2D053-ZG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PB3C-2D053-ZJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RJ6T-14H102-BBB\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
}
|
||||
342
opendbc_repo/opendbc/car/ford/fordcan.py
Normal file
342
opendbc_repo/opendbc/car/ford/fordcan.py
Normal file
@@ -0,0 +1,342 @@
|
||||
from opendbc.car import CanBusBase, structs
|
||||
|
||||
HUDControl = structs.CarControl.HUDControl
|
||||
|
||||
|
||||
class CanBus(CanBusBase):
|
||||
def __init__(self, CP=None, fingerprint=None) -> None:
|
||||
super().__init__(CP, fingerprint)
|
||||
|
||||
@property
|
||||
def main(self) -> int:
|
||||
return self.offset
|
||||
|
||||
@property
|
||||
def radar(self) -> int:
|
||||
return self.offset + 1
|
||||
|
||||
@property
|
||||
def camera(self) -> int:
|
||||
return self.offset + 2
|
||||
|
||||
|
||||
def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray) -> int:
|
||||
curvature = (dat[2] << 3) | ((dat[3]) >> 5)
|
||||
curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5)
|
||||
path_angle = ((dat[3] & 0x1F) << 6) | ((dat[4]) >> 2)
|
||||
path_offset = ((dat[4] & 0x3) << 8) | dat[5]
|
||||
|
||||
checksum = mode + counter
|
||||
for sig_val in (curvature, curvature_rate, path_angle, path_offset):
|
||||
checksum += sig_val + (sig_val >> 8)
|
||||
|
||||
return 0xFF - (checksum & 0xFF)
|
||||
|
||||
|
||||
def create_lka_msg(packer, CAN: CanBus):
|
||||
"""
|
||||
Creates an empty CAN message for the Ford LKA Command.
|
||||
|
||||
This command can apply "Lane Keeping Aid" maneuvers, which are subject to the PSCM lockout.
|
||||
|
||||
Frequency is 33Hz.
|
||||
"""
|
||||
|
||||
return packer.make_can_msg("Lane_Assist_Data1", CAN.main, {})
|
||||
|
||||
|
||||
def create_lat_ctl_msg(packer, CAN: CanBus, lat_active: bool, path_offset: float, path_angle: float, curvature: float,
|
||||
curvature_rate: float):
|
||||
"""
|
||||
Creates a CAN message for the Ford TJA/LCA Command.
|
||||
|
||||
This command can apply "Lane Centering" maneuvers: continuous lane centering for traffic jam assist and highway
|
||||
driving. It is not subject to the PSCM lockout.
|
||||
|
||||
Ford lane centering command uses a third order polynomial to describe the road centerline. The polynomial is defined
|
||||
by the following coefficients:
|
||||
c0: lateral offset between the vehicle and the centerline (positive is right)
|
||||
c1: heading angle between the vehicle and the centerline (positive is right)
|
||||
c2: curvature of the centerline (positive is left)
|
||||
c3: rate of change of curvature of the centerline
|
||||
As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and speed, the steering
|
||||
angle cannot be easily controlled.
|
||||
|
||||
The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. This can be done
|
||||
using tools such as Forscan.
|
||||
|
||||
Frequency is 20Hz.
|
||||
"""
|
||||
|
||||
values = {
|
||||
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter
|
||||
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1]
|
||||
"LatCtl_D_Rq": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft,
|
||||
# 3=InterventionRight, 4-7=NotUsed [0|7]
|
||||
"LatCtlRampType_D_Rq": 0, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
|
||||
# Makes no difference with curvature control
|
||||
"LatCtlPrecision_D_Rq": 1, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
|
||||
# The stock system always uses comfortable
|
||||
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
|
||||
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians
|
||||
"LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2
|
||||
"LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter
|
||||
}
|
||||
return packer.make_can_msg("LateralMotionControl", CAN.main, values)
|
||||
|
||||
|
||||
def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, path_offset: float, path_angle: float, curvature: float,
|
||||
curvature_rate: float, counter: int):
|
||||
"""
|
||||
Create a CAN message for the new Ford Lane Centering command.
|
||||
|
||||
This message is used on the CAN FD platform and replaces the old LateralMotionControl message. It is similar but has
|
||||
additional signals for a counter and checksum.
|
||||
|
||||
Frequency is 20Hz.
|
||||
"""
|
||||
|
||||
values = {
|
||||
"LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode,
|
||||
# 3=SafeRampOut, 4-7=NotUsed [0|7]
|
||||
"LatCtlRampType_D_Rq": 0, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
|
||||
"LatCtlPrecision_D_Rq": 1, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
|
||||
"LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter
|
||||
"LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians
|
||||
"LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter
|
||||
"LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2
|
||||
"HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1]
|
||||
"LatCtlPath_No_Cnt": counter, # [0|15]
|
||||
"LatCtlPath_No_Cs": 0, # [0|255]
|
||||
}
|
||||
|
||||
# calculate checksum
|
||||
dat = packer.make_can_msg("LateralMotionControl2", 0, values)[1]
|
||||
values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat)
|
||||
|
||||
return packer.make_can_msg("LateralMotionControl2", CAN.main, values)
|
||||
|
||||
|
||||
def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool, brake_request, v_ego_kph: float):
|
||||
"""
|
||||
Creates a CAN message for the Ford ACC Command.
|
||||
|
||||
This command can be used to enable ACC, to set the ACC gas/brake/decel values
|
||||
and to disable ACC.
|
||||
|
||||
Frequency is 50Hz.
|
||||
"""
|
||||
values = {
|
||||
"AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2
|
||||
"Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes
|
||||
"AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2
|
||||
# No observed acceleration seen from this signal alone. During stock system operation, it appears to
|
||||
# be the raw acceleration request (AccPrpl_A_Rq when positive, AccBrkTot_A_Rq when negative)
|
||||
"AccPrpl_A_Pred": -5.0, # Acceleration request: [-5|5.23] m/s^2
|
||||
"AccResumEnbl_B_Rq": 1 if long_active else 0,
|
||||
# No observed acceleration seen from this signal alone
|
||||
"AccVeh_V_Trg": v_ego_kph, # Target speed: [0|255] km/h
|
||||
# TODO: we may be able to improve braking response by utilizing pre-charging better
|
||||
# When setting these two bits without AccBrkTot_A_Rq, an initial jerk is observed and car may be able to brake temporarily with AccPrpl_A_Rq
|
||||
"AccBrkPrchg_B_Rq": 1 if brake_request else 0, # Pre-charge brake request: 0=No, 1=Yes
|
||||
"AccBrkDecel_B_Rq": 1 if brake_request else 0, # Deceleration request: 0=Inactive, 1=Active
|
||||
"AccStopStat_B_Rq": 1 if stopping else 0,
|
||||
}
|
||||
return packer.make_can_msg("ACCDATA", CAN.main, values)
|
||||
|
||||
|
||||
def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, standstill: bool,
|
||||
show_distance_bars: bool, hud_control, stock_values: dict):
|
||||
"""
|
||||
Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam
|
||||
assist status.
|
||||
|
||||
Stock functionality is maintained by passing through unmodified signals.
|
||||
|
||||
Frequency is 5Hz.
|
||||
"""
|
||||
|
||||
# Tja_D_Stat
|
||||
if enabled:
|
||||
if hud_control.leftLaneDepart:
|
||||
status = 3 # ActiveInterventionLeft
|
||||
elif hud_control.rightLaneDepart:
|
||||
status = 4 # ActiveInterventionRight
|
||||
else:
|
||||
status = 2 # Active
|
||||
elif main_on:
|
||||
if hud_control.leftLaneDepart:
|
||||
status = 5 # ActiveWarningLeft
|
||||
elif hud_control.rightLaneDepart:
|
||||
status = 6 # ActiveWarningRight
|
||||
else:
|
||||
status = 1 # Standby
|
||||
else:
|
||||
status = 0 # Off
|
||||
|
||||
values = {s: stock_values[s] for s in [
|
||||
"HaDsply_No_Cs",
|
||||
"HaDsply_No_Cnt",
|
||||
"AccStopStat_D_Dsply", # ACC stopped status message
|
||||
"AccTrgDist2_D_Dsply", # ACC target distance
|
||||
"AccStopRes_B_Dsply",
|
||||
"TjaWarn_D_Rq", # TJA warning
|
||||
"TjaMsgTxt_D_Dsply", # TJA text
|
||||
"IaccLamp_D_Rq", # iACC status icon
|
||||
"AccMsgTxt_D2_Rq", # ACC text
|
||||
"FcwDeny_B_Dsply", # FCW disabled
|
||||
"FcwMemStat_B_Actl", # FCW enabled setting
|
||||
"AccTGap_B_Dsply", # ACC time gap display setting
|
||||
"CadsAlignIncplt_B_Actl",
|
||||
"AccFllwMde_B_Dsply", # ACC follow mode display setting
|
||||
"CadsRadrBlck_B_Actl",
|
||||
"CmbbPostEvnt_B_Dsply", # AEB event status
|
||||
"AccStopMde_B_Dsply", # ACC stop mode display setting
|
||||
"FcwMemSens_D_Actl", # FCW sensitivity setting
|
||||
"FcwMsgTxt_D_Rq", # FCW text
|
||||
"AccWarn_D_Dsply", # ACC warning
|
||||
"FcwVisblWarn_B_Rq", # FCW visible alert
|
||||
"FcwAudioWarn_B_Rq", # FCW audio alert
|
||||
"AccTGap_D_Dsply", # ACC time gap
|
||||
"AccMemEnbl_B_RqDrv", # ACC adaptive/normal setting
|
||||
"FdaMem_B_Stat", # FDA enabled setting
|
||||
]}
|
||||
|
||||
values.update({
|
||||
"Tja_D_Stat": status, # TJA status
|
||||
})
|
||||
|
||||
if CP.openpilotLongitudinalControl:
|
||||
values.update({
|
||||
"AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text
|
||||
"AccMsgTxt_D2_Rq": 0, # ACC text
|
||||
"AccTGap_B_Dsply": 1 if show_distance_bars else 0, # Show time gap control UI
|
||||
"AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0, # Lead indicator
|
||||
"AccStopMde_B_Dsply": 1 if standstill else 0,
|
||||
"AccWarn_D_Dsply": 0, # ACC warning
|
||||
"AccTGap_D_Dsply": hud_control.leadDistanceBars, # Time gap
|
||||
})
|
||||
|
||||
# Forwards FCW alert from IPMA
|
||||
if fcw_alert:
|
||||
values["FcwVisblWarn_B_Rq"] = 1 # FCW visible alert
|
||||
|
||||
return packer.make_can_msg("ACCDATA_3", CAN.main, values)
|
||||
|
||||
|
||||
def create_lkas_ui_msg(packer, CAN: CanBus, main_on: bool, enabled: bool, steer_alert: bool, hud_control,
|
||||
stock_values: dict):
|
||||
"""
|
||||
Creates a CAN message for the Ford IPC IPMA/LKAS status.
|
||||
|
||||
Show the LKAS status with the "driver assist" lines in the IPC.
|
||||
|
||||
Stock functionality is maintained by passing through unmodified signals.
|
||||
|
||||
Frequency is 1Hz.
|
||||
"""
|
||||
|
||||
# LaActvStats_D_Dsply
|
||||
# R Intvn Warn Supprs Avail No
|
||||
# L
|
||||
# Intvn 24 19 14 9 4
|
||||
# Warn 23 18 13 8 3
|
||||
# Supprs 22 17 12 7 2
|
||||
# Avail 21 16 11 6 1
|
||||
# No 20 15 10 5 0
|
||||
#
|
||||
# TODO: test suppress state
|
||||
if enabled:
|
||||
lines = 0 # NoLeft_NoRight
|
||||
if hud_control.leftLaneDepart:
|
||||
lines += 4
|
||||
elif hud_control.leftLaneVisible:
|
||||
lines += 1
|
||||
if hud_control.rightLaneDepart:
|
||||
lines += 20
|
||||
elif hud_control.rightLaneVisible:
|
||||
lines += 5
|
||||
elif main_on:
|
||||
lines = 0
|
||||
else:
|
||||
if hud_control.leftLaneDepart:
|
||||
lines = 3 # WarnLeft_NoRight
|
||||
elif hud_control.rightLaneDepart:
|
||||
lines = 15 # NoLeft_WarnRight
|
||||
else:
|
||||
lines = 30 # LA_Off
|
||||
|
||||
hands_on_wheel_dsply = 1 if steer_alert else 0
|
||||
|
||||
values = {s: stock_values[s] for s in [
|
||||
"FeatConfigIpmaActl",
|
||||
"FeatNoIpmaActl",
|
||||
"PersIndexIpma_D_Actl",
|
||||
"AhbcRampingV_D_Rq", # AHB ramping
|
||||
"LaDenyStats_B_Dsply", # LKAS error
|
||||
"CamraDefog_B_Req", # Windshield heater?
|
||||
"CamraStats_D_Dsply", # Camera status
|
||||
"DasAlrtLvl_D_Dsply", # DAS alert level
|
||||
"DasStats_D_Dsply", # DAS status
|
||||
"DasWarn_D_Dsply", # DAS warning
|
||||
"AhbHiBeam_D_Rq", # AHB status
|
||||
"Passthru_63",
|
||||
"Passthru_48",
|
||||
]}
|
||||
|
||||
values.update({
|
||||
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
|
||||
"LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
|
||||
})
|
||||
return packer.make_can_msg("IPMA_Data", CAN.main, values)
|
||||
|
||||
|
||||
def create_button_msg(packer, bus: int, stock_values: dict, cancel=False, resume=False, tja_toggle=False):
|
||||
"""
|
||||
Creates a CAN message for the Ford SCCM buttons/switches.
|
||||
|
||||
Includes cruise control buttons, turn lights and more.
|
||||
|
||||
Frequency is 10Hz.
|
||||
"""
|
||||
|
||||
values = {s: stock_values[s] for s in [
|
||||
"HeadLghtHiFlash_D_Stat", # SCCM Passthrough the remaining buttons
|
||||
"TurnLghtSwtch_D_Stat", # SCCM Turn signal switch
|
||||
"WiprFront_D_Stat",
|
||||
"LghtAmb_D_Sns",
|
||||
"AccButtnGapDecPress",
|
||||
"AccButtnGapIncPress",
|
||||
"AslButtnOnOffCnclPress",
|
||||
"AslButtnOnOffPress",
|
||||
"LaSwtchPos_D_Stat",
|
||||
"CcAslButtnCnclResPress",
|
||||
"CcAslButtnDeny_B_Actl",
|
||||
"CcAslButtnIndxDecPress",
|
||||
"CcAslButtnIndxIncPress",
|
||||
"CcAslButtnOffCnclPress",
|
||||
"CcAslButtnOnOffCncl",
|
||||
"CcAslButtnOnPress",
|
||||
"CcAslButtnResDecPress",
|
||||
"CcAslButtnResIncPress",
|
||||
"CcAslButtnSetDecPress",
|
||||
"CcAslButtnSetIncPress",
|
||||
"CcAslButtnSetPress",
|
||||
"CcButtnOffPress",
|
||||
"CcButtnOnOffCnclPress",
|
||||
"CcButtnOnOffPress",
|
||||
"CcButtnOnPress",
|
||||
"HeadLghtHiFlash_D_Actl",
|
||||
"HeadLghtHiOn_B_StatAhb",
|
||||
"AhbStat_B_Dsply",
|
||||
"AccButtnGapTogglePress",
|
||||
"WiprFrontSwtch_D_Stat",
|
||||
"HeadLghtHiCtrl_D_RqAhb",
|
||||
]}
|
||||
|
||||
values.update({
|
||||
"CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button
|
||||
"CcAsllButtnResPress": 1 if resume else 0, # CC resume button
|
||||
"TjaButtnOnOffPress": 1 if tja_toggle else 0, # LCA/TJA toggle button
|
||||
})
|
||||
return packer.make_can_msg("Steering_Data_FD1", bus, values)
|
||||
98
opendbc_repo/opendbc/car/ford/interface.py
Normal file
98
opendbc_repo/opendbc/car/ford/interface.py
Normal file
@@ -0,0 +1,98 @@
|
||||
import numpy as np
|
||||
from opendbc.car import Bus, get_safety_config, structs
|
||||
from opendbc.car.carlog import carlog
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.ford.carcontroller import CarController
|
||||
from opendbc.car.ford.carstate import CarState
|
||||
from opendbc.car.ford.fordcan import CanBus
|
||||
from opendbc.car.ford.radar_interface import RadarInterface
|
||||
from opendbc.car.ford.values import CarControllerParams, DBC, Ecu, FordFlags, RADAR, FordSafetyFlags
|
||||
from opendbc.car.interfaces import CarInterfaceBase
|
||||
|
||||
TransmissionType = structs.CarParams.TransmissionType
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
CarState = CarState
|
||||
CarController = CarController
|
||||
RadarInterface = RadarInterface
|
||||
|
||||
@staticmethod
|
||||
def get_pid_accel_limits(CP, current_speed, cruise_speed):
|
||||
# PCM doesn't allow acceleration near cruise_speed,
|
||||
# so limit limits of pid to prevent windup
|
||||
ACCEL_MAX_VALS = [CarControllerParams.ACCEL_MAX, 0.2]
|
||||
ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .4]
|
||||
return CarControllerParams.ACCEL_MIN, np.interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS)
|
||||
|
||||
@staticmethod
|
||||
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
|
||||
ret.brand = "ford"
|
||||
|
||||
ret.radarUnavailable = Bus.radar not in DBC[candidate]
|
||||
ret.steerControlType = structs.CarParams.SteerControlType.angle
|
||||
ret.steerActuatorDelay = 0.2
|
||||
ret.steerLimitTimer = 1.0
|
||||
ret.steerAtStandstill = True
|
||||
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiV = [0.5]
|
||||
|
||||
if not ret.radarUnavailable and DBC[candidate][Bus.radar] == RADAR.DELPHI_MRR:
|
||||
# average of 33.3 Hz radar timestep / 4 scan modes = 60 ms
|
||||
# MRR_Header_Timestamps->CAN_DET_TIME_SINCE_MEAS reports 61.3 ms
|
||||
ret.radarDelay = 0.06
|
||||
|
||||
CAN = CanBus(fingerprint=fingerprint)
|
||||
cfgs = [get_safety_config(structs.CarParams.SafetyModel.ford)]
|
||||
if CAN.main >= 4:
|
||||
cfgs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput))
|
||||
ret.safetyConfigs = cfgs
|
||||
|
||||
ret.alphaLongitudinalAvailable = ret.radarUnavailable
|
||||
if alpha_long or not ret.radarUnavailable:
|
||||
ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.LONG_CONTROL.value
|
||||
ret.openpilotLongitudinalControl = True
|
||||
|
||||
if ret.flags & FordFlags.CANFD:
|
||||
ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.CANFD.value
|
||||
|
||||
# TRON (SecOC) platforms are not supported
|
||||
# LateralMotionControl2, ACCDATA are 16 bytes on these platforms
|
||||
if len(fingerprint[CAN.camera]):
|
||||
if fingerprint[CAN.camera].get(0x3d6) != 8 or fingerprint[CAN.camera].get(0x186) != 8:
|
||||
carlog.error('dashcamOnly: SecOC is unsupported')
|
||||
ret.dashcamOnly = True
|
||||
else:
|
||||
# Lock out if the car does not have needed lateral and longitudinal control APIs.
|
||||
# Note that we also check CAN for adaptive cruise, but no known signal for LCA exists
|
||||
pscm_config = next((fw for fw in car_fw if fw.ecu == Ecu.eps and b'\x22\xDE\x01' in fw.request), None)
|
||||
if pscm_config:
|
||||
if len(pscm_config.fwVersion) != 24:
|
||||
carlog.error('dashcamOnly: Invalid EPS FW version')
|
||||
ret.dashcamOnly = True
|
||||
else:
|
||||
config_tja = pscm_config.fwVersion[7] # Traffic Jam Assist
|
||||
config_lca = pscm_config.fwVersion[8] # Lane Centering Assist
|
||||
if config_tja != 0xFF or config_lca != 0xFF:
|
||||
carlog.error('dashcamOnly: Car lacks required lateral control APIs')
|
||||
ret.dashcamOnly = True
|
||||
|
||||
# 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[CAN.main] or docs:
|
||||
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[CAN.main] and 0x3A7 in fingerprint[CAN.main]
|
||||
|
||||
# LCA can steer down to zero
|
||||
ret.minSteerSpeed = 0.
|
||||
|
||||
ret.autoResumeSng = ret.minEnableSpeed == -1.
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
return ret
|
||||
274
opendbc_repo/opendbc/car/ford/radar_interface.py
Normal file
274
opendbc_repo/opendbc/car/ford/radar_interface.py
Normal file
@@ -0,0 +1,274 @@
|
||||
import numpy as np
|
||||
from typing import cast
|
||||
from collections import defaultdict
|
||||
from math import cos, sin
|
||||
from dataclasses import dataclass
|
||||
from opendbc.can import CANParser
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.ford.fordcan import CanBus
|
||||
from opendbc.car.ford.values import DBC, RADAR
|
||||
from opendbc.car.interfaces import RadarInterfaceBase
|
||||
|
||||
DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540))
|
||||
|
||||
DELPHI_MRR_RADAR_START_ADDR = 0x120
|
||||
DELPHI_MRR_RADAR_HEADER_ADDR = 0x174 # MRR_Header_SensorCoverage
|
||||
DELPHI_MRR_RADAR_MSG_COUNT = 64
|
||||
|
||||
DELPHI_MRR_RADAR_RANGE_COVERAGE = {0: 42, 1: 164, 2: 45, 3: 175} # scan index to detection range (m)
|
||||
DELPHI_MRR_MIN_LONG_RANGE_DIST = 30 # meters
|
||||
DELPHI_MRR_CLUSTER_THRESHOLD = 5 # meters, lateral distance and relative velocity are weighted
|
||||
|
||||
|
||||
@dataclass
|
||||
class Cluster:
|
||||
dRel: float = 0.0
|
||||
yRel: float = 0.0
|
||||
vRel: float = 0.0
|
||||
trackId: int = 0
|
||||
|
||||
|
||||
def cluster_points(pts_l: list[list[float]], pts2_l: list[list[float]], max_dist: float) -> list[int]:
|
||||
"""
|
||||
Clusters a collection of points based on another collection of points. This is useful for correlating clusters through time.
|
||||
Points in pts2 not close enough to any point in pts are assigned -1.
|
||||
Args:
|
||||
pts_l: List of points to base the new clusters on
|
||||
pts2_l: List of points to cluster using pts
|
||||
max_dist: Max distance from cluster center to candidate point
|
||||
|
||||
Returns:
|
||||
List of cluster indices for pts2 that correspond to pts
|
||||
"""
|
||||
|
||||
if not len(pts2_l):
|
||||
return []
|
||||
|
||||
if not len(pts_l):
|
||||
return [-1] * len(pts2_l)
|
||||
|
||||
max_dist_sq = max_dist ** 2
|
||||
pts = np.array(pts_l)
|
||||
pts2 = np.array(pts2_l)
|
||||
|
||||
# Compute squared norms
|
||||
pts_norm_sq = np.sum(pts ** 2, axis=1)
|
||||
pts2_norm_sq = np.sum(pts2 ** 2, axis=1)
|
||||
|
||||
# Compute squared Euclidean distances using the identity
|
||||
# dist_sq[i, j] = ||pts2[i]||^2 + ||pts[j]||^2 - 2 * pts2[i] . pts[j]
|
||||
dist_sq = pts2_norm_sq[:, np.newaxis] + pts_norm_sq[np.newaxis, :] - 2 * np.dot(pts2, pts.T)
|
||||
dist_sq = np.maximum(dist_sq, 0.0)
|
||||
|
||||
# Find the closest cluster for each point and assign its index
|
||||
closest_clusters = np.argmin(dist_sq, axis=1)
|
||||
closest_dist_sq = dist_sq[np.arange(len(pts2)), closest_clusters]
|
||||
cluster_idxs = np.where(closest_dist_sq < max_dist_sq, closest_clusters, -1)
|
||||
|
||||
return cast(list[int], cluster_idxs.tolist())
|
||||
|
||||
|
||||
def _create_delphi_esr_radar_can_parser(CP) -> CANParser:
|
||||
msg_n = len(DELPHI_ESR_RADAR_MSGS)
|
||||
messages = list(zip(DELPHI_ESR_RADAR_MSGS, [20] * msg_n, strict=True))
|
||||
|
||||
return CANParser(RADAR.DELPHI_ESR, messages, CanBus(CP).radar)
|
||||
|
||||
|
||||
def _create_delphi_mrr_radar_can_parser(CP) -> CANParser:
|
||||
messages = [
|
||||
("MRR_Header_InformationDetections", 33),
|
||||
("MRR_Header_SensorCoverage", 33),
|
||||
]
|
||||
|
||||
for i in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
|
||||
msg = f"MRR_Detection_{i:03d}"
|
||||
messages += [(msg, 33)]
|
||||
|
||||
return CANParser(RADAR.DELPHI_MRR, messages, CanBus(CP).radar)
|
||||
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
|
||||
self.points: list[list[float]] = []
|
||||
self.clusters: list[Cluster] = []
|
||||
|
||||
self.updated_messages = set()
|
||||
self.track_id = 0
|
||||
self.radar = DBC[CP.carFingerprint].get(Bus.radar)
|
||||
self.scan_index_invalid_cnt = 0
|
||||
self.radar_unavailable_cnt = 0
|
||||
self.prev_headerScanIndex = 0
|
||||
if CP.radarUnavailable:
|
||||
self.rcp = None
|
||||
elif self.radar == RADAR.DELPHI_ESR:
|
||||
self.rcp = _create_delphi_esr_radar_can_parser(CP)
|
||||
self.trigger_msg = DELPHI_ESR_RADAR_MSGS[-1]
|
||||
self.valid_cnt = {key: 0 for key in DELPHI_ESR_RADAR_MSGS}
|
||||
elif self.radar == RADAR.DELPHI_MRR:
|
||||
self.rcp = _create_delphi_mrr_radar_can_parser(CP)
|
||||
self.trigger_msg = DELPHI_MRR_RADAR_HEADER_ADDR
|
||||
else:
|
||||
raise ValueError(f"Unsupported radar: {self.radar}")
|
||||
|
||||
def update(self, can_strings):
|
||||
if self.rcp is None:
|
||||
return super().update(None)
|
||||
|
||||
vls = self.rcp.update(can_strings)
|
||||
self.updated_messages.update(vls)
|
||||
|
||||
if self.trigger_msg not in self.updated_messages:
|
||||
return None
|
||||
self.updated_messages.clear()
|
||||
|
||||
ret = structs.RadarData()
|
||||
if not self.rcp.can_valid:
|
||||
ret.errors.canError = True
|
||||
|
||||
if self.radar == RADAR.DELPHI_ESR:
|
||||
self._update_delphi_esr()
|
||||
elif self.radar == RADAR.DELPHI_MRR:
|
||||
_update = self._update_delphi_mrr(ret)
|
||||
if not _update:
|
||||
return None
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
return ret
|
||||
|
||||
def _update_delphi_esr(self):
|
||||
for ii in sorted(self.updated_messages):
|
||||
cpt = self.rcp.vl[ii]
|
||||
|
||||
if cpt['X_Rel'] > 0.00001:
|
||||
self.valid_cnt[ii] = 0 # reset counter
|
||||
|
||||
if cpt['X_Rel'] > 0.00001:
|
||||
self.valid_cnt[ii] += 1
|
||||
else:
|
||||
self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
|
||||
#print ii, self.valid_cnt[ii], cpt['VALID'], cpt['X_Rel'], cpt['Angle']
|
||||
|
||||
# radar point only valid if there have been enough valid measurements
|
||||
if self.valid_cnt[ii] > 0:
|
||||
if ii not in self.pts:
|
||||
self.pts[ii] = structs.RadarData.RadarPoint()
|
||||
self.pts[ii].trackId = self.track_id
|
||||
self.track_id += 1
|
||||
self.pts[ii].dRel = cpt['X_Rel'] # from front of car
|
||||
self.pts[ii].yRel = cpt['X_Rel'] * cpt['Angle'] * CV.DEG_TO_RAD # in car frame's y axis, left is positive
|
||||
self.pts[ii].vRel = cpt['V_Rel']
|
||||
self.pts[ii].vLead = self.pts[ii].vRel + self.v_ego
|
||||
self.pts[ii].aRel = float('nan')
|
||||
self.pts[ii].yvRel = 0# float('nan')
|
||||
self.pts[ii].measured = True
|
||||
else:
|
||||
if ii in self.pts:
|
||||
del self.pts[ii]
|
||||
|
||||
def _update_delphi_mrr(self, ret: structs.RadarData):
|
||||
headerScanIndex = int(self.rcp.vl["MRR_Header_InformationDetections"]['CAN_SCAN_INDEX']) & 0b11
|
||||
|
||||
# In reverse, the radar continually sends the last messages. Mark this as invalid
|
||||
if (self.prev_headerScanIndex + 1) % 4 != headerScanIndex:
|
||||
self.radar_unavailable_cnt += 1
|
||||
else:
|
||||
self.radar_unavailable_cnt = 0
|
||||
self.prev_headerScanIndex = headerScanIndex
|
||||
|
||||
if self.radar_unavailable_cnt >= 5:
|
||||
self.pts.clear()
|
||||
self.points.clear()
|
||||
self.clusters.clear()
|
||||
ret.errors.radarUnavailableTemporary = True
|
||||
return True
|
||||
|
||||
# Use points with Doppler coverage of +-60 m/s, reduces similar points
|
||||
if headerScanIndex not in (2, 3):
|
||||
return False
|
||||
|
||||
if DELPHI_MRR_RADAR_RANGE_COVERAGE[headerScanIndex] != int(self.rcp.vl["MRR_Header_SensorCoverage"]["CAN_RANGE_COVERAGE"]):
|
||||
self.scan_index_invalid_cnt += 1
|
||||
else:
|
||||
self.scan_index_invalid_cnt = 0
|
||||
|
||||
# Rarely MRR_Header_InformationDetections can fail to send a message. The scan index is skipped in this case
|
||||
if self.scan_index_invalid_cnt >= 5:
|
||||
ret.errors.wrongConfig = True
|
||||
|
||||
for ii in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
|
||||
msg = self.rcp.vl[f"MRR_Detection_{ii:03d}"]
|
||||
|
||||
# SCAN_INDEX rotates through 0..3 on each message for different measurement modes
|
||||
# Indexes 0 and 2 have a max range of ~40m, 1 and 3 are ~170m (MRR_Header_SensorCoverage->CAN_RANGE_COVERAGE)
|
||||
# Indexes 0 and 1 have a Doppler coverage of +-71 m/s, 2 and 3 have +-60 m/s
|
||||
scanIndex = msg[f"CAN_SCAN_INDEX_2LSB_{ii:02d}"]
|
||||
|
||||
# Throw out old measurements. Very unlikely to happen, but is proper behavior
|
||||
if scanIndex != headerScanIndex:
|
||||
continue
|
||||
|
||||
valid = bool(msg[f"CAN_DET_VALID_LEVEL_{ii:02d}"])
|
||||
|
||||
# Long range measurement mode is more sensitive and can detect the road surface
|
||||
dist = msg[f"CAN_DET_RANGE_{ii:02d}"] # m [0|255.984]
|
||||
if scanIndex in (1, 3) and dist < DELPHI_MRR_MIN_LONG_RANGE_DIST:
|
||||
valid = False
|
||||
|
||||
if valid:
|
||||
azimuth = msg[f"CAN_DET_AZIMUTH_{ii:02d}"] # rad [-3.1416|3.13964]
|
||||
distRate = msg[f"CAN_DET_RANGE_RATE_{ii:02d}"] # m/s [-128|127.984]
|
||||
dRel = cos(azimuth) * dist # m from front of car
|
||||
yRel = -sin(azimuth) * dist # in car frame's y axis, left is positive
|
||||
|
||||
self.points.append([dRel, yRel * 2, distRate * 2])
|
||||
|
||||
# Cluster and publish using stored points once we've cycled through all 4 scan modes
|
||||
if headerScanIndex != 3:
|
||||
return False
|
||||
|
||||
# Cluster points from this cycle against the centroids from the previous cycle
|
||||
prev_keys = [[p.dRel, p.yRel * 2, p.vRel * 2] for p in self.clusters]
|
||||
labels = cluster_points(prev_keys, self.points, DELPHI_MRR_CLUSTER_THRESHOLD)
|
||||
|
||||
points_by_track_id = defaultdict(list)
|
||||
for idx, label in enumerate(labels):
|
||||
if label != -1:
|
||||
points_by_track_id[self.clusters[label].trackId].append(self.points[idx])
|
||||
else:
|
||||
points_by_track_id[self.track_id].append(self.points[idx])
|
||||
self.track_id += 1
|
||||
|
||||
self.clusters = []
|
||||
for idx, (track_id, pts) in enumerate(points_by_track_id.items()):
|
||||
dRel = [p[0] for p in pts]
|
||||
min_dRel = min(dRel)
|
||||
dRel = sum(dRel) / len(dRel)
|
||||
|
||||
yRel = [p[1] for p in pts]
|
||||
yRel = sum(yRel) / len(yRel) / 2
|
||||
|
||||
vRel = [p[2] for p in pts]
|
||||
vRel = sum(vRel) / len(vRel) / 2
|
||||
|
||||
# FIXME: creating capnp RadarPoint and accessing attributes are both expensive, so we store a dataclass and reuse the RadarPoint
|
||||
self.clusters.append(Cluster(dRel=dRel, yRel=yRel, vRel=vRel, trackId=track_id))
|
||||
|
||||
if idx not in self.pts:
|
||||
self.pts[idx] = structs.RadarData.RadarPoint(measured=True, aRel=float('nan'), yvRel=0)
|
||||
|
||||
self.pts[idx].dRel = min_dRel
|
||||
self.pts[idx].yRel = yRel
|
||||
self.pts[idx].vRel = vRel
|
||||
self.pts[idx].vLead = vRel + self.v_ego
|
||||
self.pts[idx].trackId = track_id
|
||||
|
||||
for idx in range(len(points_by_track_id), len(self.pts)):
|
||||
del self.pts[idx]
|
||||
|
||||
self.points = []
|
||||
|
||||
return True
|
||||
0
opendbc_repo/opendbc/car/ford/tests/__init__.py
Normal file
0
opendbc_repo/opendbc/car/ford/tests/__init__.py
Normal file
28
opendbc_repo/opendbc/car/ford/tests/print_platform_codes.py
Executable file
28
opendbc_repo/opendbc/car/ford/tests/print_platform_codes.py
Executable file
@@ -0,0 +1,28 @@
|
||||
#!/usr/bin/env python3
|
||||
from collections import defaultdict
|
||||
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.ford.values import get_platform_codes
|
||||
from opendbc.car.ford.fingerprints import FW_VERSIONS
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
if __name__ == "__main__":
|
||||
cars_for_code: defaultdict = defaultdict(lambda: defaultdict(set))
|
||||
|
||||
for car_model, ecus in FW_VERSIONS.items():
|
||||
print(car_model)
|
||||
for ecu in sorted(ecus):
|
||||
platform_codes = get_platform_codes(ecus[ecu])
|
||||
for code in platform_codes:
|
||||
cars_for_code[ecu][code].add(car_model)
|
||||
|
||||
print(f' (Ecu.{ecu[0]}, {hex(ecu[1])}, {ecu[2]}):')
|
||||
print(f' Codes: {sorted(platform_codes)}')
|
||||
print()
|
||||
|
||||
print('\nCar models vs. platform codes:')
|
||||
for ecu, codes in cars_for_code.items():
|
||||
print(f' (Ecu.{ecu[0]}, {hex(ecu[1])}, {ecu[2]}):')
|
||||
for code, cars in codes.items():
|
||||
print(f' {code!r}: {sorted(map(str, cars))}')
|
||||
142
opendbc_repo/opendbc/car/ford/tests/test_ford.py
Normal file
142
opendbc_repo/opendbc/car/ford/tests/test_ford.py
Normal file
@@ -0,0 +1,142 @@
|
||||
import random
|
||||
from collections.abc import Iterable
|
||||
|
||||
from hypothesis import settings, given, strategies as st
|
||||
from parameterized import parameterized
|
||||
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.fw_versions import build_fw_dict
|
||||
from opendbc.car.ford.values import CAR, FW_QUERY_CONFIG, FW_PATTERN, get_platform_codes
|
||||
from opendbc.car.ford.fingerprints import FW_VERSIONS
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
|
||||
ECU_ADDRESSES = {
|
||||
Ecu.eps: 0x730, # Power Steering Control Module (PSCM)
|
||||
Ecu.abs: 0x760, # Anti-Lock Brake System (ABS)
|
||||
Ecu.fwdRadar: 0x764, # Cruise Control Module (CCM)
|
||||
Ecu.fwdCamera: 0x706, # Image Processing Module A (IPMA)
|
||||
Ecu.engine: 0x7E0, # Powertrain Control Module (PCM)
|
||||
Ecu.shiftByWire: 0x732, # Gear Shift Module (GSM)
|
||||
Ecu.debug: 0x7D0, # Accessory Protocol Interface Module (APIM)
|
||||
}
|
||||
|
||||
|
||||
ECU_PART_NUMBER = {
|
||||
Ecu.eps: [
|
||||
b"14D003",
|
||||
],
|
||||
Ecu.abs: [
|
||||
b"2D053",
|
||||
],
|
||||
Ecu.fwdRadar: [
|
||||
b"14D049",
|
||||
],
|
||||
Ecu.fwdCamera: [
|
||||
b"14F397", # Ford Q3
|
||||
b"14H102", # Ford Q4
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
class TestFordFW:
|
||||
def test_fw_query_config(self):
|
||||
for (ecu, addr, subaddr) in FW_QUERY_CONFIG.extra_ecus:
|
||||
assert ecu in ECU_ADDRESSES, "Unknown ECU"
|
||||
assert addr == ECU_ADDRESSES[ecu], "ECU address mismatch"
|
||||
assert subaddr is None, "Unexpected ECU subaddress"
|
||||
|
||||
@parameterized.expand(FW_VERSIONS.items())
|
||||
def test_fw_versions(self, car_model: str, fw_versions: dict[tuple[int, int, int | None], Iterable[bytes]]):
|
||||
for (ecu, addr, subaddr), fws in fw_versions.items():
|
||||
assert ecu in ECU_PART_NUMBER, "Unexpected ECU"
|
||||
assert addr == ECU_ADDRESSES[ecu], "ECU address mismatch"
|
||||
assert subaddr is None, "Unexpected ECU subaddress"
|
||||
|
||||
for fw in fws:
|
||||
assert len(fw) == 24, "Expected ECU response to be 24 bytes"
|
||||
|
||||
match = FW_PATTERN.match(fw)
|
||||
assert match is not None, f"Unable to parse FW: {fw!r}"
|
||||
if match:
|
||||
part_number = match.group("part_number")
|
||||
assert part_number in ECU_PART_NUMBER[ecu], f"Unexpected part number for {fw!r}"
|
||||
|
||||
codes = get_platform_codes([fw])
|
||||
assert 1 == len(codes), f"Unable to parse FW: {fw!r}"
|
||||
|
||||
@settings(max_examples=100)
|
||||
@given(data=st.data())
|
||||
def test_platform_codes_fuzzy_fw(self, data):
|
||||
"""Ensure function doesn't raise an exception"""
|
||||
fw_strategy = st.lists(st.binary())
|
||||
fws = data.draw(fw_strategy)
|
||||
get_platform_codes(fws)
|
||||
|
||||
def test_platform_codes_spot_check(self):
|
||||
# Asserts basic platform code parsing behavior for a few cases
|
||||
results = get_platform_codes([
|
||||
b"JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
b"NZ6T-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
b"PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
b"LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
])
|
||||
assert results == {(b"X6A", b"J"), (b"Z6T", b"N"), (b"J6T", b"P"), (b"B5A", b"L")}
|
||||
|
||||
def test_fuzzy_match(self):
|
||||
for platform, fw_by_addr in FW_VERSIONS.items():
|
||||
# Ensure there's no overlaps in platform codes
|
||||
for _ in range(20):
|
||||
car_fw = []
|
||||
for ecu, fw_versions in fw_by_addr.items():
|
||||
ecu_name, addr, sub_addr = ecu
|
||||
fw = random.choice(fw_versions)
|
||||
car_fw.append(CarParams.CarFw(ecu=ecu_name, fwVersion=fw, address=addr,
|
||||
subAddress=0 if sub_addr is None else sub_addr))
|
||||
|
||||
CP = CarParams(carFw=car_fw)
|
||||
matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), CP.carVin, FW_VERSIONS)
|
||||
assert matches == {platform}
|
||||
|
||||
def test_match_fw_fuzzy(self):
|
||||
offline_fw = {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b"L1MC-14D003-AJ\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-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
b"L1MC-2D053-BD\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",
|
||||
b"LB5T-14D049-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
],
|
||||
# We consider all model year hints for ECU, even with different platform codes
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b"LB5T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
b"NC5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
],
|
||||
}
|
||||
expected_fingerprint = CAR.FORD_EXPLORER_MK6
|
||||
|
||||
# ensure that we fuzzy match on all non-exact FW with changed revisions
|
||||
live_fw = {
|
||||
(0x730, None): {b"L1MC-14D003-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"},
|
||||
(0x760, None): {b"L1MC-2D053-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"},
|
||||
(0x764, None): {b"LB5T-14D049-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"},
|
||||
(0x706, None): {b"LB5T-14F397-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"},
|
||||
}
|
||||
candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw})
|
||||
assert candidates == {expected_fingerprint}
|
||||
|
||||
# model year hint in between the range should match
|
||||
live_fw[(0x706, None)] = {b"MB5T-14F397-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}
|
||||
candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw,})
|
||||
assert candidates == {expected_fingerprint}
|
||||
|
||||
# unseen model year hint should not match
|
||||
live_fw[(0x760, None)] = {b"M1MC-2D053-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}
|
||||
candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw})
|
||||
assert len(candidates) == 0, "Should not match new model year hint"
|
||||
316
opendbc_repo/opendbc/car/ford/values.py
Normal file
316
opendbc_repo/opendbc/car/ford/values.py
Normal file
@@ -0,0 +1,316 @@
|
||||
import copy
|
||||
import re
|
||||
from dataclasses import dataclass, field, replace
|
||||
from enum import Enum, IntFlag
|
||||
|
||||
from opendbc.car import AngleSteeringLimits, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, \
|
||||
Device
|
||||
from opendbc.car.fw_query_definitions import FwQueryConfig, LiveFwVersions, OfflineFwVersions, Request, StdQueries, p16
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
STEER_STEP = 5 # LateralMotionControl, 20Hz
|
||||
LKA_STEP = 3 # Lane_Assist_Data1, 33Hz
|
||||
ACC_CONTROL_STEP = 2 # ACCDATA, 50Hz
|
||||
LKAS_UI_STEP = 100 # IPMA_Data, 1Hz
|
||||
ACC_UI_STEP = 20 # ACCDATA_3, 5Hz
|
||||
BUTTONS_STEP = 5 # Steering_Data_FD1, 10Hz, but send twice as fast
|
||||
|
||||
STEER_DRIVER_ALLOWANCE = 1.0 # Driver intervention threshold, Nm
|
||||
|
||||
ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
|
||||
0.02, # Max curvature for steering command, m^-1
|
||||
# Curvature rate limits
|
||||
# Max curvature is limited by the EPS to an equivalent of ~2.0 m/s^2 at all speeds,
|
||||
# however max curvature rate linearly decreases as speed increases:
|
||||
# ~0.009 m^-1/sec at 7 m/s, ~0.002 m^-1/sec at 35 m/s
|
||||
# Limit to ~2 m/s^3 up, ~3.3 m/s^3 down at 75 mph and match EPS limit at low speed
|
||||
([5, 25], [0.00045, 0.0001]),
|
||||
([5, 25], [0.00045, 0.00015])
|
||||
)
|
||||
CURVATURE_ERROR = 0.002 # ~6 degrees at 10 m/s, ~10 degrees at 35 m/s
|
||||
|
||||
ACCEL_MAX = 2.0 # m/s^2 max acceleration
|
||||
ACCEL_MIN = -3.5 # m/s^2 max deceleration
|
||||
MIN_GAS = -0.5
|
||||
INACTIVE_GAS = -5.0
|
||||
|
||||
def __init__(self, CP):
|
||||
pass
|
||||
|
||||
|
||||
class FordSafetyFlags(IntFlag):
|
||||
LONG_CONTROL = 1
|
||||
CANFD = 2
|
||||
|
||||
|
||||
class FordFlags(IntFlag):
|
||||
# Static flags
|
||||
CANFD = 1
|
||||
|
||||
|
||||
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+"
|
||||
hybrid: bool = False
|
||||
plug_in_hybrid: bool = False
|
||||
|
||||
def init_make(self, CP: CarParams):
|
||||
harness = CarHarness.ford_q4 if CP.flags & FordFlags.CANFD else CarHarness.ford_q3
|
||||
if CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_MAVERICK_MK1, CAR.FORD_F_150_MK14, CAR.FORD_F_150_LIGHTNING_MK1):
|
||||
self.car_parts = CarParts([Device.threex_angled_mount, harness])
|
||||
else:
|
||||
self.car_parts = CarParts([Device.threex, harness])
|
||||
|
||||
if harness == CarHarness.ford_q4:
|
||||
self.setup_video = "https://www.youtube.com/watch?v=uUGkH6C_EQU"
|
||||
|
||||
if CP.carFingerprint in (CAR.FORD_F_150_MK14, CAR.FORD_F_150_LIGHTNING_MK1, CAR.FORD_EXPEDITION_MK4):
|
||||
self.setup_video = "https://www.youtube.com/watch?v=MewJc9LYp9M"
|
||||
|
||||
@dataclass
|
||||
class FordPlatformConfig(PlatformConfig):
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: {
|
||||
Bus.pt: 'ford_lincoln_base_pt',
|
||||
Bus.radar: RADAR.DELPHI_MRR,
|
||||
})
|
||||
|
||||
def init(self):
|
||||
for car_docs in list(self.car_docs):
|
||||
if car_docs.hybrid:
|
||||
name = f"{car_docs.make} {car_docs.model} Hybrid {car_docs.years}"
|
||||
self.car_docs.append(replace(copy.deepcopy(car_docs), name=name))
|
||||
if car_docs.plug_in_hybrid:
|
||||
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: {
|
||||
Bus.pt: 'ford_lincoln_base_pt',
|
||||
})
|
||||
|
||||
def init(self):
|
||||
super().init()
|
||||
self.flags |= FordFlags.CANFD
|
||||
|
||||
@dataclass
|
||||
class FordF150LightningPlatform(FordCANFDPlatformConfig):
|
||||
def init(self):
|
||||
super().init()
|
||||
|
||||
# Don't show in docs until this issue is resolved. See https://github.com/commaai/openpilot/issues/30302
|
||||
self.car_docs = []
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
FORD_BRONCO_SPORT_MK1 = FordPlatformConfig(
|
||||
[FordCarDocs("Ford Bronco Sport 2021-24")],
|
||||
CarSpecs(mass=1625, wheelbase=2.67, steerRatio=17.7),
|
||||
)
|
||||
FORD_ESCAPE_MK4 = FordPlatformConfig(
|
||||
[
|
||||
FordCarDocs("Ford Escape 2020-22", hybrid=True, plug_in_hybrid=True),
|
||||
FordCarDocs("Ford Kuga 2020-23", "Adaptive Cruise Control with Lane Centering", hybrid=True, plug_in_hybrid=True),
|
||||
],
|
||||
CarSpecs(mass=1750, wheelbase=2.71, steerRatio=16.7),
|
||||
)
|
||||
FORD_ESCAPE_MK4_5 = FordCANFDPlatformConfig(
|
||||
[
|
||||
FordCarDocs("Ford Escape 2023-24", hybrid=True, plug_in_hybrid=True, setup_video="https://www.youtube.com/watch?v=M6uXf4b2SHM"),
|
||||
FordCarDocs("Ford Kuga Hybrid 2024", "All"),
|
||||
FordCarDocs("Ford Kuga Plug-in Hybrid 2024", "All"),
|
||||
],
|
||||
CarSpecs(mass=1750, wheelbase=2.71, steerRatio=16.7),
|
||||
)
|
||||
FORD_EXPLORER_MK6 = FordPlatformConfig(
|
||||
[
|
||||
FordCarDocs("Ford Explorer 2020-24", hybrid=True), # Hybrid: Limited and Platinum only
|
||||
FordCarDocs("Lincoln Aviator 2020-24", "Co-Pilot360 Plus", plug_in_hybrid=True), # Hybrid: Grand Touring only
|
||||
],
|
||||
CarSpecs(mass=2050, wheelbase=3.025, steerRatio=16.8),
|
||||
)
|
||||
FORD_EXPEDITION_MK4 = FordCANFDPlatformConfig(
|
||||
[FordCarDocs("Ford Expedition 2022-24", "Co-Pilot360 Assist 2.0", hybrid=False)],
|
||||
CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0),
|
||||
)
|
||||
FORD_F_150_MK14 = FordCANFDPlatformConfig(
|
||||
[FordCarDocs("Ford F-150 2021-23", "Co-Pilot360 Assist 2.0", hybrid=True)],
|
||||
CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0),
|
||||
)
|
||||
FORD_F_150_LIGHTNING_MK1 = FordF150LightningPlatform(
|
||||
[FordCarDocs("Ford F-150 Lightning 2022-23", "Co-Pilot360 Assist 2.0")],
|
||||
CarSpecs(mass=2948, wheelbase=3.70, steerRatio=16.9),
|
||||
)
|
||||
FORD_FOCUS_MK4 = FordPlatformConfig(
|
||||
[FordCarDocs("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS], hybrid=True)], # mHEV only
|
||||
CarSpecs(mass=1350, wheelbase=2.7, steerRatio=15.0),
|
||||
)
|
||||
FORD_MAVERICK_MK1 = FordPlatformConfig(
|
||||
[
|
||||
FordCarDocs("Ford Maverick 2022", "LARIAT Luxury", hybrid=True),
|
||||
FordCarDocs("Ford Maverick 2023-24", "Co-Pilot360 Assist", hybrid=True),
|
||||
],
|
||||
CarSpecs(mass=1650, wheelbase=3.076, steerRatio=17.0),
|
||||
)
|
||||
FORD_MUSTANG_MACH_E_MK1 = FordCANFDPlatformConfig(
|
||||
[FordCarDocs("Ford Mustang Mach-E 2021-24", "All", setup_video="https://www.youtube.com/watch?v=AR4_eTF3b_A")],
|
||||
CarSpecs(mass=2200, wheelbase=2.984, steerRatio=17.0), # TODO: check steer ratio
|
||||
)
|
||||
FORD_RANGER_MK2 = FordCANFDPlatformConfig(
|
||||
[FordCarDocs("Ford Ranger 2024", "Adaptive Cruise Control with Lane Centering", setup_video="https://www.youtube.com/watch?v=2oJlXCKYOy0")],
|
||||
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
|
||||
# 1222-333333-444
|
||||
# 1 = Model year hint (approximates model year/generation)
|
||||
# 2 = Platform hint
|
||||
# 3 = Part number
|
||||
# 4 = Software version
|
||||
FW_ALPHABET = b'A-HJ-NP-VX-Z'
|
||||
FW_PATTERN = re.compile(b'^(?P<model_year_hint>[' + FW_ALPHABET + b'])' +
|
||||
b'(?P<platform_hint>[0-9' + FW_ALPHABET + b']{3})-' +
|
||||
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:
|
||||
match = FW_PATTERN.match(fw)
|
||||
if match is not None:
|
||||
codes.add((match.group('platform_hint'), match.group('model_year_hint')))
|
||||
|
||||
return codes
|
||||
|
||||
|
||||
def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, vin: str, offline_fw_versions: OfflineFwVersions) -> set[str]:
|
||||
candidates: set[str] = set()
|
||||
|
||||
for candidate, fws in offline_fw_versions.items():
|
||||
# Keep track of ECUs which pass all checks (platform hint, within model year hint range)
|
||||
valid_found_ecus = set()
|
||||
valid_expected_ecus = {ecu[1:] for ecu in fws if ecu[0] in PLATFORM_CODE_ECUS}
|
||||
for ecu, expected_versions in fws.items():
|
||||
addr = ecu[1:]
|
||||
# Only check ECUs expected to have platform codes
|
||||
if ecu[0] not in PLATFORM_CODE_ECUS:
|
||||
continue
|
||||
|
||||
# Expected platform codes & model year hints
|
||||
codes = get_platform_codes(expected_versions)
|
||||
expected_platform_codes = {code for code, _ in codes}
|
||||
expected_model_year_hints = {model_year_hint for _, model_year_hint in codes}
|
||||
|
||||
# Found platform codes & model year hints
|
||||
codes = get_platform_codes(live_fw_versions.get(addr, set()))
|
||||
found_platform_codes = {code for code, _ in codes}
|
||||
found_model_year_hints = {model_year_hint for _, model_year_hint in codes}
|
||||
|
||||
# Check platform code matches for any found versions
|
||||
if not any(found_platform_code in expected_platform_codes for found_platform_code in found_platform_codes):
|
||||
break
|
||||
|
||||
# Check any model year hint within range in the database. Note that some models have more than one
|
||||
# platform code per ECU which we don't consider as separate ranges
|
||||
if not any(min(expected_model_year_hints) <= found_model_year_hint <= max(expected_model_year_hints) for
|
||||
found_model_year_hint in found_model_year_hints):
|
||||
break
|
||||
|
||||
valid_found_ecus.add(addr)
|
||||
|
||||
# If all live ECUs pass all checks for candidate, add it as a match
|
||||
if valid_expected_ecus.issubset(valid_found_ecus):
|
||||
candidates.add(candidate)
|
||||
|
||||
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)
|
||||
|
||||
DATA_IDENTIFIER_FORD_ASBUILT = 0xDE00
|
||||
|
||||
ASBUILT_BLOCKS: list[tuple[int, list]] = [
|
||||
(1, [Ecu.debug, Ecu.fwdCamera, Ecu.eps]),
|
||||
(2, [Ecu.abs, Ecu.debug, Ecu.eps]),
|
||||
(3, [Ecu.abs, Ecu.debug, Ecu.eps]),
|
||||
(4, [Ecu.debug, Ecu.fwdCamera]),
|
||||
(5, [Ecu.debug]),
|
||||
(6, [Ecu.debug]),
|
||||
(7, [Ecu.debug]),
|
||||
(8, [Ecu.debug]),
|
||||
(9, [Ecu.debug]),
|
||||
(16, [Ecu.debug, Ecu.fwdCamera]),
|
||||
(18, [Ecu.fwdCamera]),
|
||||
(20, [Ecu.fwdCamera]),
|
||||
(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.
|
||||
# FIXME: For CAN FD, ECUs respond with frames larger than 8 bytes on the powertrain bus
|
||||
Request(
|
||||
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
|
||||
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire],
|
||||
logging=True,
|
||||
),
|
||||
Request(
|
||||
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
|
||||
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire],
|
||||
bus=0,
|
||||
auxiliary=True,
|
||||
),
|
||||
*[Request(
|
||||
[StdQueries.TESTER_PRESENT_REQUEST, ford_asbuilt_block_request(block_id)],
|
||||
[StdQueries.TESTER_PRESENT_RESPONSE, ford_asbuilt_block_response(block_id)],
|
||||
whitelist_ecus=ecus,
|
||||
bus=0,
|
||||
logging=True,
|
||||
) for block_id, ecus in ASBUILT_BLOCKS],
|
||||
],
|
||||
extra_ecus=[
|
||||
(Ecu.engine, 0x7e0, None), # Powertrain Control Module
|
||||
# Note: We are unlikely to get a response from behind the gateway
|
||||
(Ecu.shiftByWire, 0x732, None), # Gear Shift Module
|
||||
(Ecu.debug, 0x7d0, None), # Accessory Protocol Interface Module
|
||||
],
|
||||
# Custom fuzzy fingerprinting function using platform and model year hints
|
||||
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
|
||||
)
|
||||
|
||||
DBC = CAR.create_dbc_map()
|
||||
Reference in New Issue
Block a user