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

Ford: Support for LCA vehicles (#23331)

Browse Source 
* Ford: add Focus Mk4

Also removes support for the Ford Fusion.

* Ford: LKAS/LCA steering and UI CAN commands

* Ford: implement CarController w/ steering and lanes ui

* Ford: FPv2 firmware request

* Ford: Add FW for 2018 Ford Focus

* Ford: add Escape Mk4

* bump panda

* cleanup

* add that back

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: acd455ed3a
taco
Cameron Clough 3 years ago committed by GitHub
parent ebeadd9b41
commit 0830dc0277
8 changed files with 510 additions and 166 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      release/files_common
  2. 116
      selfdrive/car/ford/carcontroller.py
  3. 241
      selfdrive/car/ford/carstate.py
  4. 158
      selfdrive/car/ford/fordcan.py
  5. 83
      selfdrive/car/ford/interface.py
  6. 4
      selfdrive/car/ford/radar_interface.py
  7. 69
      selfdrive/car/ford/values.py
  8. 3
      selfdrive/car/tests/routes.py

2
release/files_common
Unescape View File

@ -565,8 +565,8 @@ opendbc/gm_global_a_powertrain_generated.dbc
opendbc/gm_global_a_object.dbc opendbc/gm_global_a_object.dbc
opendbc/gm_global_a_chassis.dbc opendbc/gm_global_a_chassis.dbc
opendbc/ford_fusion_2018_pt.dbc
opendbc/ford_fusion_2018_adas.dbc opendbc/ford_fusion_2018_adas.dbc
opendbc/ford_lincoln_base_pt.dbc
opendbc/honda_accord_2018_can_generated.dbc opendbc/honda_accord_2018_can_generated.dbc
opendbc/acura_ilx_2016_can_generated.dbc opendbc/acura_ilx_2016_can_generated.dbc

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

@ -1,86 +1,90 @@
import math
from cereal import car from cereal import car
from selfdrive.car import make_can_msg from common.numpy_fast import clip, interp
from selfdrive.car.ford.fordcan import create_steer_command, create_lkas_ui, spam_cancel_button from selfdrive.car.ford import fordcan
from selfdrive.car.ford.values import CarControllerParams
from opendbc.can.packer import CANPacker from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert VisualAlert = car.CarControl.HUDControl.VisualAlert
MAX_STEER_DELTA = 1
TOGGLE_DEBUG = False def apply_ford_steer_angle_limits(apply_steer, apply_steer_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
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))
return apply_steer
class CarController(): class CarController():
def __init__(self, dbc_name, CP, VM): def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.VM = VM
self.packer = CANPacker(dbc_name) self.packer = CANPacker(dbc_name)
self.enabled_last = False
self.apply_steer_last = 0
self.steer_rate_limited = False
self.main_on_last = False self.main_on_last = False
self.vehicle_model = VM self.lkas_enabled_last = False
self.generic_toggle_last = 0
self.steer_alert_last = False self.steer_alert_last = False
self.lkas_action = 0
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):
def update(self, CC, CS, frame):
can_sends = [] can_sends = []
steer_alert = visual_alert in (VisualAlert.steerRequired, VisualAlert.ldw)
apply_steer = actuators.steer
if pcm_cancel: actuators = CC.actuators
#print "CANCELING!!!!" hud_control = CC.hudControl
can_sends.append(spam_cancel_button(self.packer))
if (frame % 3) == 0: main_on = CS.out.cruiseState.available
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
curvature = self.vehicle_model.calc_curvature(math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0) if CC.cruiseControl.cancel:
# cancel stock ACC
can_sends.append(fordcan.spam_cancel_button(self.packer))
# The use of the toggle below is handy for trying out the various LKAS modes # apply rate limits
if TOGGLE_DEBUG: new_steer = actuators.steeringAngleDeg
self.lkas_action += int(CS.out.genericToggle and not self.generic_toggle_last) apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo)
self.lkas_action &= 0xf self.steer_rate_limited = new_steer != apply_steer
else:
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy
can_sends.append(create_steer_command(self.packer, apply_steer, enabled, # send steering commands at 20Hz
CS.lkas_state, CS.out.steeringAngleDeg, curvature, self.lkas_action)) if (frame % CarControllerParams.LKAS_STEER_STEP) == 0:
self.generic_toggle_last = CS.out.genericToggle lca_rq = 1 if CC.latActive else 0
if (frame % 100) == 0: # use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented
path_angle = apply_steer
can_sends.append(make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0)) # convert steer angle to curvature
#can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0)) curvature = self.VM.calc_curvature(apply_steer, CS.out.vEgo, 0.0)
if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \ # TODO: get other actuators
(self.steer_alert_last != steer_alert): curvature_rate = 0
can_sends.append(create_lkas_ui(self.packer, CS.out.cruiseState.available, enabled, steer_alert)) path_offset = 0
if (frame % 200) == 0: ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately
can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1)) precision = 0 # 0=Comfortable, 1=Precise
if (frame % 10) == 0: self.apply_steer_last = apply_steer
can_sends.append(fordcan.create_lkas_command(self.packer, apply_steer, curvature))
can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision,
path_offset, path_angle, curvature_rate, curvature))
can_sends.append(make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1))
can_sends.append(make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1))
can_sends.append(make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1)) send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
can_sends.append(make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1))
can_sends.append(make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1))
can_sends.append(make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1))
can_sends.append(make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1)) # send lkas ui command at 1Hz or if ui state changes
can_sends.append(make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1)) if (frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
can_sends.append(make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1)) can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values))
can_sends.append(make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1))
can_sends.append(make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1))
can_sends.append(make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1))
static_msgs = range(1653, 1658) # send acc ui command at 20Hz or if ui state changes
for addr in static_msgs: if (frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
cnt = (frame % 10) + 1 can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, CS.acc_tja_status_stock_values))
can_sends.append(make_can_msg(addr, (cnt << 4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1))
self.enabled_last = enabled self.main_on_last = main_on
self.main_on_last = CS.out.cruiseState.available self.lkas_enabled_last = CC.latActive
self.steer_alert_last = steer_alert self.steer_alert_last = steer_alert
return actuators, can_sends new_actuators = actuators.copy()
new_actuators.steeringAngleDeg = apply_steer
return new_actuators, can_sends

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

@ -1,58 +1,221 @@
from typing import Dict
from cereal import car from cereal import car
from common.conversions import Conversions as CV from common.conversions import Conversions as CV
from common.numpy_fast import mean from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import CarStateBase from selfdrive.car.interfaces import CarStateBase
from selfdrive.car.ford.values import DBC from selfdrive.car.ford.values import CANBUS, DBC
GearShifter = car.CarState.GearShifter
TransmissionType = car.CarParams.TransmissionType
WHEEL_RADIUS = 0.33
class CarState(CarStateBase): class CarState(CarStateBase):
def update(self, cp): 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"]["TrnGear_D_RqDrv"]
def update(self, cp, cp_cam):
ret = car.CarState.new_message() ret = car.CarState.new_message()
ret.wheelSpeeds = self.get_wheel_speeds( # car speed
cp.vl["WheelSpeed_CG1"]["WhlFl_W_Meas"], ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS
cp.vl["WheelSpeed_CG1"]["WhlFr_W_Meas"],
cp.vl["WheelSpeed_CG1"]["WhlRl_W_Meas"],
cp.vl["WheelSpeed_CG1"]["WhlRr_W_Meas"],
unit=WHEEL_RADIUS,
)
ret.vEgoRaw = mean([ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr, ret.wheelSpeeds.fl])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = not ret.vEgoRaw > 0.001 ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] * CV.RAD_TO_DEG
ret.steeringAngleDeg = cp.vl["Steering_Wheel_Data_CG1"]["SteWhlRelInit_An_Sns"] ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1
ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]["LaHandsOff_B_Actl"]
ret.steerFaultPermanent = cp.vl["Lane_Keep_Assist_Status"]["LaActDeny_B_Actl"] == 1 # gas pedal
ret.cruiseState.speed = cp.vl["Cruise_Status"]["Set_Speed"] * CV.MPH_TO_MS ret.gas = cp.vl["EngVehicleSpThrottle"]["ApedPos_Pc_ActlArb"] / 100.
ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]["Cruise_State"] in (0, 3))
ret.cruiseState.available = cp.vl["Cruise_Status"]["Cruise_State"] != 0
ret.gas = cp.vl["EngineData_14"]["ApedPosScal_Pc_Actl"] / 100.
ret.gasPressed = ret.gas > 1e-6 ret.gasPressed = ret.gas > 1e-6
ret.brakePressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"])
ret.genericToggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"]) # brake pedal
# TODO: we also need raw driver torque, needed for Assisted Lane Change ret.brake = cp.vl["BrakeSnData_4"]["BrkTot_Tq_Actl"] / 32756. # torque in Nm
self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]["LaActAvail_D_Actl"] 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 = cp.vl["Lane_Assist_Data3_FD1"]["LaHandsOff_B_Actl"] == 0
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
# cruise state
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)
# gear
if self.CP.transmissionType == TransmissionType.automatic:
gear = int(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
elif self.CP.transmissionType == TransmissionType.manual:
ret.clutchPressed = cp.vl["Engine_Clutch_Data"]["CluPdlPos_Pc_Meas"] > 0
# TODO: find reverse light signal
ret.gearShifter = GearShifter.drive
# safety
ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"])
ret.stockAeb = ret.stockFcw and ret.cruiseState.enabled
# button presses
ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1
ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2
ret.genericToggle = 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:
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 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"]
return ret return ret
@staticmethod
def parse_gear_shifter(gear: str) -> car.CarState.GearShifter:
d: Dict[str, car.CarState.GearShifter] = {
'Park': GearShifter.park, 'Reverse': GearShifter.reverse, 'Neutral': GearShifter.neutral,
'Manual': GearShifter.manumatic, 'Drive': GearShifter.drive,
}
return d.get(gear, GearShifter.unknown)
@staticmethod @staticmethod
def get_can_parser(CP): def get_can_parser(CP):
signals = [ signals = [
# sig_name, sig_address # sig_name, sig_address
("WhlRr_W_Meas", "WheelSpeed_CG1"), ("Veh_V_ActlEng", "EngVehicleSpThrottle2"), # ABS vehicle speed (kph)
("WhlRl_W_Meas", "WheelSpeed_CG1"), ("VehYaw_W_Actl", "Yaw_Data_FD1"), # ABS vehicle yaw rate (rad/s)
("WhlFr_W_Meas", "WheelSpeed_CG1"), ("VehStop_D_Stat", "DesiredTorqBrk"), # ABS vehicle stopped
("WhlFl_W_Meas", "WheelSpeed_CG1"), ("PrkBrkStatus", "DesiredTorqBrk"), # ABS park brake status
("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1"), ("ApedPos_Pc_ActlArb", "EngVehicleSpThrottle"), # PCM throttle (pct)
("Cruise_State", "Cruise_Status"), ("BrkTot_Tq_Actl", "BrakeSnData_4"), # ABS brake torque (Nm)
("Set_Speed", "Cruise_Status"), ("BpedDrvAppl_D_Actl", "EngBrakeData"), # PCM driver brake pedal pressed
("LaActAvail_D_Actl", "Lane_Keep_Assist_Status"), ("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph)
("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status"), # The units might change with IPC settings?
("LaActDeny_B_Actl", "Lane_Keep_Assist_Status"), ("CcStat_D_Actl", "EngBrakeData"), # PCM ACC status
("ApedPosScal_Pc_Actl", "EngineData_14"), ("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg)
("Dist_Incr", "Steering_Buttons"), # Calculates steering angle (and offset) from pinion
("Brake_Drv_Appl", "Cruise_Status"), # angle and driving measurements.
# StePinRelInit_An_Sns is the pinion angle, initialised
# 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
("DrStatPsngr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, passenger
("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
] ]
checks = []
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0, enforce_checks=False) checks = [
# sig_address, frequency
("EngVehicleSpThrottle2", 50),
("Yaw_Data_FD1", 100),
("DesiredTorqBrk", 50),
("EngVehicleSpThrottle", 100),
("BrakeSnData_4", 50),
("EngBrakeData", 10),
("SteeringPinion_Data", 100),
("EPAS_INFO", 50),
("Lane_Assist_Data3_FD1", 33),
("Steering_Data_FD1", 10),
("BodyInfo_3_FD1", 2),
("RCMStatusMessage2_FD1", 10),
]
if CP.transmissionType == TransmissionType.automatic:
signals += [
("TrnGear_D_RqDrv", "Gear_Shift_by_Wire_FD1"), # GWM transmission gear position
]
checks += [
("Gear_Shift_by_Wire_FD1", 10),
]
elif CP.transmissionType == TransmissionType.manual:
signals += [
("CluPdlPos_Pc_Meas", "Engine_Clutch_Data"), # PCM clutch (pct)
]
checks += [
("Engine_Clutch_Data", 33),
]
if CP.enableBsm:
signals += [
("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"), # Blindspot sensor, left
("SodDetctRight_D_Stat", "Side_Detect_R_Stat"), # Blindspot sensor, right
]
checks += [
("Side_Detect_L_Stat", 5),
("Side_Detect_R_Stat", 5),
]
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.main)
@staticmethod
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address
("HaDsply_No_Cs", "ACCDATA_3"),
("HaDsply_No_Cnt", "ACCDATA_3"),
("AccStopStat_D_Dsply", "ACCDATA_3"), # ACC stopped status message
("AccTrgDist2_D_Dsply", "ACCDATA_3"), # ACC target distance
("AccStopRes_B_Dsply", "ACCDATA_3"),
("TjaWarn_D_Rq", "ACCDATA_3"), # TJA warning
("Tja_D_Stat", "ACCDATA_3"), # TJA status
("TjaMsgTxt_D_Dsply", "ACCDATA_3"), # TJA text
("IaccLamp_D_Rq", "ACCDATA_3"), # iACC status icon
("AccMsgTxt_D2_Rq", "ACCDATA_3"), # ACC text
("FcwDeny_B_Dsply", "ACCDATA_3"), # FCW disabled
("FcwMemStat_B_Actl", "ACCDATA_3"), # FCW enabled setting
("AccTGap_B_Dsply", "ACCDATA_3"), # ACC time gap display setting
("CadsAlignIncplt_B_Actl", "ACCDATA_3"),
("AccFllwMde_B_Dsply", "ACCDATA_3"), # ACC follow mode display setting
("CadsRadrBlck_B_Actl", "ACCDATA_3"),
("CmbbPostEvnt_B_Dsply", "ACCDATA_3"), # AEB event status
("AccStopMde_B_Dsply", "ACCDATA_3"), # ACC stop mode display setting
("FcwMemSens_D_Actl", "ACCDATA_3"), # FCW sensitivity setting
("FcwMsgTxt_D_Rq", "ACCDATA_3"), # FCW text
("AccWarn_D_Dsply", "ACCDATA_3"), # ACC warning
("FcwVisblWarn_B_Rq", "ACCDATA_3"), # FCW visible alert
("FcwAudioWarn_B_Rq", "ACCDATA_3"), # FCW audio alert
("AccTGap_D_Dsply", "ACCDATA_3"), # ACC time gap
("AccMemEnbl_B_RqDrv", "ACCDATA_3"), # ACC adaptive/normal setting
("FdaMem_B_Stat", "ACCDATA_3"), # FDA enabled setting
("FeatConfigIpmaActl", "IPMA_Data"),
("FeatNoIpmaActl", "IPMA_Data"),
("PersIndexIpma_D_Actl", "IPMA_Data"),
("AhbcRampingV_D_Rq", "IPMA_Data"), # AHB ramping
("LaActvStats_D_Dsply", "IPMA_Data"), # LKAS status (lines)
("LaDenyStats_B_Dsply", "IPMA_Data"), # LKAS error
("LaHandsOff_D_Dsply", "IPMA_Data"), # LKAS hands on chime
("CamraDefog_B_Req", "IPMA_Data"), # Windshield heater?
("CamraStats_D_Dsply", "IPMA_Data"), # Camera status
("DasAlrtLvl_D_Dsply", "IPMA_Data"), # DAS alert level
("DasStats_D_Dsply", "IPMA_Data"), # DAS status
("DasWarn_D_Dsply", "IPMA_Data"), # DAS warning
("AhbHiBeam_D_Rq", "IPMA_Data"), # AHB status
("Set_Me_X1", "IPMA_Data"),
]
checks = [
# sig_address, frequency
("ACCDATA_3", 5),
("IPMA_Data", 1),
]
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.camera)

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

@ -1,50 +1,144 @@
from common.numpy_fast import clip from common.numpy_fast import clip
from selfdrive.car.ford.values import MAX_ANGLE
def create_steer_command(packer, angle_cmd, enabled, lkas_state, angle_steers, curvature, lkas_action): def create_lkas_command(packer, angle_deg: float, curvature: float):
"""Creates a CAN message for the Ford Steer Command.""" """
Creates a CAN message for the Ford LKAS Command.
#if enabled and lkas available: This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the
if enabled and lkas_state in (2, 3): # and (frame % 500) >= 3: PSCM lockout.
action = lkas_action
else:
action = 0xf
angle_cmd = angle_steers/MAX_ANGLE
angle_cmd = clip(angle_cmd * MAX_ANGLE, - MAX_ANGLE, MAX_ANGLE) Frequency is 20Hz.
"""
values = { values = {
"Lkas_Action": action, "LkaDrvOvrrd_D_Rq": 0, # driver override level? [0|3]
"Lkas_Alert": 0xf, # no alerts "LkaActvStats_D2_Req": 0, # action [0|7]
"Lane_Curvature": clip(curvature, -0.01, 0.01), # is it just for debug? "LaRefAng_No_Req": angle_deg, # angle [-102.4|102.3] degrees
#"Lane_Curvature": 0, # is it just for debug? "LaRampType_B_Req": 0, # Ramp speed: 0=Smooth, 1=Quick
"Steer_Angle_Req": angle_cmd "LaCurvature_No_Calc": curvature, # curvature [-0.01024|0.01023] 1/meter
"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_Keep_Assist_Control", 0, values) return packer.make_can_msg("Lane_Assist_Data1", 0, 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):
"""
Creates a CAN message for the Ford TJA/LCA Command.
This command can apply "Lane Centering" manoeuvres: continuous lane centering
for traffic jam assist and highway driving. It is not subject to the PSCM
lockout.
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": 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
}
return packer.make_can_msg("LateralMotionControl", 0, values)
def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, stock_values):
"""
Creates a CAN message for the Ford IPC IPMA/LKAS status.
Show the LKAS status with the "driver assist" lines in the IPC.
def create_lkas_ui(packer, main_on, enabled, steer_alert): Stock functionality is maintained by passing through unmodified signals.
"""Creates a CAN message for the Ford Steer Ui."""
if not main_on: Frequency is 1Hz.
lines = 0xf """
elif enabled:
lines = 0x3 # LaActvStats_D_Dsply
# TODO: get LDW state from OP
if enabled:
lines = 6
elif main_on:
lines = 0
else: else:
lines = 0x6 lines = 30
values = { values = {
"Set_Me_X80": 0x80, "FeatConfigIpmaActl": stock_values["FeatConfigIpmaActl"], # [0|65535]
"Set_Me_X45": 0x45, "FeatNoIpmaActl": stock_values["FeatNoIpmaActl"], # [0|65535]
"Set_Me_X30": 0x30, "PersIndexIpma_D_Actl": stock_values["PersIndexIpma_D_Actl"], # [0|7]
"Lines_Hud": lines, "AhbcRampingV_D_Rq": stock_values["AhbcRampingV_D_Rq"], # AHB ramping [0|3]
"Hands_Warning_W_Chime": steer_alert, "LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
"LaDenyStats_B_Dsply": stock_values["LaDenyStats_B_Dsply"], # LKAS error [0|1]
"LaHandsOff_D_Dsply": 2 if steer_alert else 0, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
"CamraDefog_B_Req": stock_values["CamraDefog_B_Req"], # Windshield heater? [0|1]
"CamraStats_D_Dsply": stock_values["CamraStats_D_Dsply"], # Camera status [0|3]
"DasAlrtLvl_D_Dsply": stock_values["DasAlrtLvl_D_Dsply"], # DAS alert level [0|7]
"DasStats_D_Dsply": stock_values["DasStats_D_Dsply"], # DAS status [0|3]
"DasWarn_D_Dsply": stock_values["DasWarn_D_Dsply"], # DAS warning [0|3]
"AhbHiBeam_D_Rq": stock_values["AhbHiBeam_D_Rq"], # AHB status [0|3]
"Set_Me_X1": stock_values["Set_Me_X1"], # [0|15]
} }
return packer.make_can_msg("Lane_Keep_Assist_Ui", 0, values) return packer.make_can_msg("IPMA_Data", 0, values)
def create_acc_ui_command(packer, main_on: bool, enabled: bool, stock_values):
"""
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 20Hz.
"""
values = {
"HaDsply_No_Cs": stock_values["HaDsply_No_Cs"], # [0|255]
"HaDsply_No_Cnt": stock_values["HaDsply_No_Cnt"], # [0|15]
"AccStopStat_D_Dsply": stock_values["AccStopStat_D_Dsply"], # ACC stopped status message: 0=NoDisplay, 1=ResumeReady, 2=Stopped, 3=PressResume [0|3]
"AccTrgDist2_D_Dsply": stock_values["AccTrgDist2_D_Dsply"], # ACC target distance [0|15]
"AccStopRes_B_Dsply": stock_values["AccStopRes_B_Dsply"], # [0|1]
"TjaWarn_D_Rq": stock_values["TjaWarn_D_Rq"], # TJA warning: 0=NoWarning, 1=Cancel, 2=HardTakeOverLevel1, 3=HardTakeOverLevel2 [0|7]
"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]
"TjaMsgTxt_D_Dsply": stock_values["TjaMsgTxt_D_Dsply"], # TJA text [0|7]
"IaccLamp_D_Rq": stock_values["IaccLamp_D_Rq"], # iACC status icon [0|3]
"AccMsgTxt_D2_Rq": stock_values["AccMsgTxt_D2_Rq"], # ACC text [0|15]
"FcwDeny_B_Dsply": stock_values["FcwDeny_B_Dsply"], # FCW disabled [0|1]
"FcwMemStat_B_Actl": stock_values["FcwMemStat_B_Actl"], # FCW enabled setting [0|1]
"AccTGap_B_Dsply": stock_values["AccTGap_B_Dsply"], # ACC time gap display setting [0|1]
"CadsAlignIncplt_B_Actl": stock_values["CadsAlignIncplt_B_Actl"], # Radar alignment? [0|1]
"AccFllwMde_B_Dsply": stock_values["AccFllwMde_B_Dsply"], # ACC follow mode display setting [0|1]
"CadsRadrBlck_B_Actl": stock_values["CadsRadrBlck_B_Actl"], # Radar blocked? [0|1]
"CmbbPostEvnt_B_Dsply": stock_values["CmbbPostEvnt_B_Dsply"], # AEB event status [0|1]
"AccStopMde_B_Dsply": stock_values["AccStopMde_B_Dsply"], # ACC stop mode display setting [0|1]
"FcwMemSens_D_Actl": stock_values["FcwMemSens_D_Actl"], # FCW sensitivity setting [0|3]
"FcwMsgTxt_D_Rq": stock_values["FcwMsgTxt_D_Rq"], # FCW text [0|7]
"AccWarn_D_Dsply": stock_values["AccWarn_D_Dsply"], # ACC warning [0|3]
"FcwVisblWarn_B_Rq": stock_values["FcwVisblWarn_B_Rq"], # FCW alert: 0=Off, 1=On [0|1]
"FcwAudioWarn_B_Rq": stock_values["FcwAudioWarn_B_Rq"], # FCW audio: 0=Off, 1=On [0|1]
"AccTGap_D_Dsply": stock_values["AccTGap_D_Dsply"], # ACC time gap: 1=Time_Gap_1, 2=Time_Gap_2, ..., 5=Time_Gap_5 [0|7]
"AccMemEnbl_B_RqDrv": stock_values["AccMemEnbl_B_RqDrv"], # ACC setting: 0=NormalCruise, 1=AdaptiveCruise [0|1]
"FdaMem_B_Stat": stock_values["FdaMem_B_Stat"], # FDA enabled setting [0|1]
}
return packer.make_can_msg("ACCDATA_3", 0, values)
def spam_cancel_button(packer, cancel=1):
"""
Creates a CAN message for the Ford SCCM buttons/switches.
Includes cruise control buttons, turn lights and more.
"""
def spam_cancel_button(packer):
values = { values = {
"Cancel": 1 "CcAslButtnCnclPress": cancel, # CC cancel button
} }
return packer.make_can_msg("Steering_Buttons", 0, values) return packer.make_can_msg("Steering_Data_FD1", 0, values)

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

@ -1,65 +1,84 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
from cereal import car from cereal import car
from common.conversions import Conversions as CV from common.conversions import Conversions as CV
from selfdrive.car.ford.values import MAX_ANGLE from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.ford.values import TransmissionType, CAR
from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.interfaces import CarInterfaceBase
EventName = car.CarEvent.EventName
class CarInterface(CarInterfaceBase): class CarInterface(CarInterfaceBase):
@staticmethod @staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
ret.carName = "ford" ret.carName = "ford"
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] #ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
ret.dashcamOnly = True ret.dashcamOnly = True
ret.wheelbase = 2.85 # Angle-based steering
ret.steerRatio = 14.8 # TODO: use curvature control when ready
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG ret.steerControlType = car.CarParams.SteerControlType.angle
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.steerActuatorDelay = 0.1
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]] # TODO: tune this
ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
ret.steerLimitTimer = 1.0 ret.steerLimitTimer = 1.0
# TODO: detect stop-and-go vehicles
stop_and_go = False
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.FOCUS_MK4:
ret.wheelbase = 2.7
ret.steerRatio = 14.3
tire_stiffness_factor = 0.5328
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]:
ret.transmissionType = TransmissionType.automatic
else:
ret.transmissionType = TransmissionType.manual
# 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.steerRateCost = 1.0 ret.steerRateCost = 1.0
ret.centerToFront = ret.wheelbase * 0.44 ret.centerToFront = ret.wheelbase * 0.44
tire_stiffness_factor = 0.5328
# TODO: add minSteerSpeed
ret.minEnableSpeed = 12. * CV.MPH_TO_MS
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor) tire_stiffness_factor=tire_stiffness_factor)
ret.steerControlType = car.CarParams.SteerControlType.angle
return ret return ret
def _update(self, c): def _update(self, c):
ret = self.CS.update(self.cp) ret = self.CS.update(self.cp, self.cp_cam)
# events ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
events = self.create_common_events(ret)
if self.CS.lkas_state not in (2, 3) and ret.vEgo > 13. * CV.MPH_TO_MS and ret.cruiseState.enabled:
events.add(car.CarEvent.EventName.steerTempUnavailable)
events = self.create_common_events(ret)
ret.events = events.to_msg() ret.events = events.to_msg()
return ret return ret
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c): def apply(self, c):
ret = self.CC.update(c, self.CS, self.frame)
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.hudControl.visualAlert, c.cruiseControl.cancel)
self.frame += 1 self.frame += 1
return ret return ret

4
selfdrive/car/ford/radar_interface.py
Unescape View File

@ -2,7 +2,7 @@
from cereal import car from cereal import car
from common.conversions import Conversions as CV from common.conversions import Conversions as CV
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from selfdrive.car.ford.values import DBC from selfdrive.car.ford.values import CANBUS, DBC
from selfdrive.car.interfaces import RadarInterfaceBase from selfdrive.car.interfaces import RadarInterfaceBase
RADAR_MSGS = list(range(0x500, 0x540)) RADAR_MSGS = list(range(0x500, 0x540))
@ -14,7 +14,7 @@ def _create_radar_can_parser(car_fingerprint):
RADAR_MSGS * 3)) RADAR_MSGS * 3))
checks = list(zip(RADAR_MSGS, [20] * msg_n)) checks = list(zip(RADAR_MSGS, [20] * msg_n))
return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1) return CANParser(DBC[car_fingerprint]['radar'], signals, checks, CANBUS.radar)
class RadarInterface(RadarInterfaceBase): class RadarInterface(RadarInterfaceBase):
def __init__(self, CP): def __init__(self, CP):

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

@ -1,21 +1,82 @@
from collections import namedtuple
from typing import Dict, List, Union from typing import Dict, List, Union
from cereal import car from cereal import car
from selfdrive.car import dbc_dict from selfdrive.car import dbc_dict
from selfdrive.car.docs_definitions import CarInfo from selfdrive.car.docs_definitions import CarInfo
Ecu = car.CarParams.Ecu Ecu = car.CarParams.Ecu
TransmissionType = car.CarParams.TransmissionType
AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points'])
class CarControllerParams:
# Messages: Lane_Assist_Data1, LateralMotionControl
LKAS_STEER_STEP = 5
# Message: IPMA_Data
LKAS_UI_STEP = 100
# 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])
MAX_ANGLE = 87. # make sure we never command the extremes (0xfff) which cause latching fault
class CANBUS:
main = 0
radar = 1
camera = 2
class CAR: class CAR:
FUSION = "FORD FUSION 2018" ESCAPE_MK4 = "FORD ESCAPE 4TH GEN"
FOCUS_MK4 = "FORD FOCUS 4TH GEN"
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
CAR.FUSION: CarInfo("Ford Fusion 2018", "All")
} }
FW_VERSIONS = {
CAR.ESCAPE_MK4: {
(Ecu.eps, 0x730, None): [
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 0x760, None): [
b'LX6C-2D053-NS\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',
],
(Ecu.engine, 0x7E0, None): [
b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FOCUS_MK4: {
(Ecu.eps, 0x730, None): [
b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 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',
],
(Ecu.engine, 0x7E0, None): [
b'JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
}
DBC = { DBC = {
CAR.FUSION: dbc_dict('ford_fusion_2018_pt', 'ford_fusion_2018_adas'), CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', 'ford_fusion_2018_adas'),
CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', 'ford_fusion_2018_adas'),
} }

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

@ -3,6 +3,7 @@ from collections import namedtuple
from selfdrive.car.chrysler.values import CAR as CHRYSLER from selfdrive.car.chrysler.values import CAR as CHRYSLER
from selfdrive.car.gm.values import CAR as GM from selfdrive.car.gm.values import CAR as GM
from selfdrive.car.ford.values import CAR as FORD
from selfdrive.car.honda.values import CAR as HONDA from selfdrive.car.honda.values import CAR as HONDA
from selfdrive.car.hyundai.values import CAR as HYUNDAI from selfdrive.car.hyundai.values import CAR as HYUNDAI
from selfdrive.car.nissan.values import CAR as NISSAN from selfdrive.car.nissan.values import CAR as NISSAN
@ -15,6 +16,8 @@ from selfdrive.car.body.values import CAR as COMMA
# TODO: add routes for these cars # TODO: add routes for these cars
non_tested_cars = [ non_tested_cars = [
FORD.ESCAPE_MK4,
FORD.FOCUS_MK4,
GM.CADILLAC_ATS, GM.CADILLAC_ATS,
GM.HOLDEN_ASTRA, GM.HOLDEN_ASTRA,
GM.MALIBU, GM.MALIBU,

Write Preview
Loading…
Cancel
Save