Ford: Support for LCA vehicles (#23331)
* 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: acd455ed3ae0ffbb82fe36dd6d9a1f4d16279823
This commit is contained in:
Cameron Clough
@@ -565,8 +565,8 @@ opendbc/gm_global_a_powertrain_generated.dbc
|
||||
opendbc/gm_global_a_object.dbc
|
||||
opendbc/gm_global_a_chassis.dbc
|
||||
|
||||
opendbc/ford_fusion_2018_pt.dbc
|
||||
opendbc/ford_fusion_2018_adas.dbc
|
||||
opendbc/ford_lincoln_base_pt.dbc
|
||||
|
||||
opendbc/honda_accord_2018_can_generated.dbc
|
||||
opendbc/acura_ilx_2016_can_generated.dbc
|
||||
|
||||
@@ -1,86 +1,90 @@
|
||||
import math
|
||||
from cereal import car
|
||||
from selfdrive.car import make_can_msg
|
||||
from selfdrive.car.ford.fordcan import create_steer_command, create_lkas_ui, spam_cancel_button
|
||||
from common.numpy_fast import clip, interp
|
||||
from selfdrive.car.ford import fordcan
|
||||
from selfdrive.car.ford.values import CarControllerParams
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
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():
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.VM = VM
|
||||
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.vehicle_model = VM
|
||||
self.generic_toggle_last = 0
|
||||
self.lkas_enabled_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 = []
|
||||
steer_alert = visual_alert in (VisualAlert.steerRequired, VisualAlert.ldw)
|
||||
|
||||
apply_steer = actuators.steer
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
|
||||
if pcm_cancel:
|
||||
#print "CANCELING!!!!"
|
||||
can_sends.append(spam_cancel_button(self.packer))
|
||||
main_on = CS.out.cruiseState.available
|
||||
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
|
||||
|
||||
if (frame % 3) == 0:
|
||||
if CC.cruiseControl.cancel:
|
||||
# cancel stock ACC
|
||||
can_sends.append(fordcan.spam_cancel_button(self.packer))
|
||||
|
||||
curvature = self.vehicle_model.calc_curvature(math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0)
|
||||
# apply rate limits
|
||||
new_steer = actuators.steeringAngleDeg
|
||||
apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
# The use of the toggle below is handy for trying out the various LKAS modes
|
||||
if TOGGLE_DEBUG:
|
||||
self.lkas_action += int(CS.out.genericToggle and not self.generic_toggle_last)
|
||||
self.lkas_action &= 0xf
|
||||
else:
|
||||
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy
|
||||
# send steering commands at 20Hz
|
||||
if (frame % CarControllerParams.LKAS_STEER_STEP) == 0:
|
||||
lca_rq = 1 if CC.latActive else 0
|
||||
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, enabled,
|
||||
CS.lkas_state, CS.out.steeringAngleDeg, curvature, self.lkas_action))
|
||||
self.generic_toggle_last = CS.out.genericToggle
|
||||
# use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented
|
||||
path_angle = apply_steer
|
||||
|
||||
if (frame % 100) == 0:
|
||||
# convert steer angle to curvature
|
||||
curvature = self.VM.calc_curvature(apply_steer, CS.out.vEgo, 0.0)
|
||||
|
||||
can_sends.append(make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0))
|
||||
#can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0))
|
||||
# TODO: get other actuators
|
||||
curvature_rate = 0
|
||||
path_offset = 0
|
||||
|
||||
if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \
|
||||
(self.steer_alert_last != steer_alert):
|
||||
can_sends.append(create_lkas_ui(self.packer, CS.out.cruiseState.available, enabled, steer_alert))
|
||||
ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately
|
||||
precision = 0 # 0=Comfortable, 1=Precise
|
||||
|
||||
if (frame % 200) == 0:
|
||||
can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1))
|
||||
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))
|
||||
|
||||
if (frame % 10) == 0:
|
||||
|
||||
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))
|
||||
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(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1))
|
||||
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))
|
||||
# send lkas ui command at 1Hz or if ui state changes
|
||||
if (frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
|
||||
can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values))
|
||||
|
||||
can_sends.append(make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1))
|
||||
can_sends.append(make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1))
|
||||
can_sends.append(make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1))
|
||||
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))
|
||||
# send acc ui command at 20Hz or if ui state changes
|
||||
if (frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
|
||||
can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, CS.acc_tja_status_stock_values))
|
||||
|
||||
static_msgs = range(1653, 1658)
|
||||
for addr in static_msgs:
|
||||
cnt = (frame % 10) + 1
|
||||
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 = CS.out.cruiseState.available
|
||||
self.main_on_last = main_on
|
||||
self.lkas_enabled_last = CC.latActive
|
||||
self.steer_alert_last = steer_alert
|
||||
|
||||
return actuators, can_sends
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steeringAngleDeg = apply_steer
|
||||
|
||||
return new_actuators, can_sends
|
||||
|
||||
@@ -1,58 +1,221 @@
|
||||
from typing import Dict
|
||||
|
||||
from cereal import car
|
||||
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 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):
|
||||
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.wheelSpeeds = self.get_wheel_speeds(
|
||||
cp.vl["WheelSpeed_CG1"]["WhlFl_W_Meas"],
|
||||
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])
|
||||
# car speed
|
||||
ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = not ret.vEgoRaw > 0.001
|
||||
ret.steeringAngleDeg = cp.vl["Steering_Wheel_Data_CG1"]["SteWhlRelInit_An_Sns"]
|
||||
ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]["LaHandsOff_B_Actl"]
|
||||
ret.steerFaultPermanent = cp.vl["Lane_Keep_Assist_Status"]["LaActDeny_B_Actl"] == 1
|
||||
ret.cruiseState.speed = cp.vl["Cruise_Status"]["Set_Speed"] * CV.MPH_TO_MS
|
||||
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.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] * CV.RAD_TO_DEG
|
||||
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
|
||||
ret.brakePressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"])
|
||||
ret.genericToggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"])
|
||||
# TODO: we also need raw driver torque, needed for Assisted Lane Change
|
||||
self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]["LaActAvail_D_Actl"]
|
||||
|
||||
# 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 = 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
|
||||
|
||||
@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
|
||||
def get_can_parser(CP):
|
||||
signals = [
|
||||
# sig_name, sig_address
|
||||
("WhlRr_W_Meas", "WheelSpeed_CG1"),
|
||||
("WhlRl_W_Meas", "WheelSpeed_CG1"),
|
||||
("WhlFr_W_Meas", "WheelSpeed_CG1"),
|
||||
("WhlFl_W_Meas", "WheelSpeed_CG1"),
|
||||
("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1"),
|
||||
("Cruise_State", "Cruise_Status"),
|
||||
("Set_Speed", "Cruise_Status"),
|
||||
("LaActAvail_D_Actl", "Lane_Keep_Assist_Status"),
|
||||
("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status"),
|
||||
("LaActDeny_B_Actl", "Lane_Keep_Assist_Status"),
|
||||
("ApedPosScal_Pc_Actl", "EngineData_14"),
|
||||
("Dist_Incr", "Steering_Buttons"),
|
||||
("Brake_Drv_Appl", "Cruise_Status"),
|
||||
("Veh_V_ActlEng", "EngVehicleSpThrottle2"), # ABS vehicle speed (kph)
|
||||
("VehYaw_W_Actl", "Yaw_Data_FD1"), # ABS vehicle yaw rate (rad/s)
|
||||
("VehStop_D_Stat", "DesiredTorqBrk"), # ABS vehicle stopped
|
||||
("PrkBrkStatus", "DesiredTorqBrk"), # ABS park brake status
|
||||
("ApedPos_Pc_ActlArb", "EngVehicleSpThrottle"), # PCM throttle (pct)
|
||||
("BrkTot_Tq_Actl", "BrakeSnData_4"), # ABS brake torque (Nm)
|
||||
("BpedDrvAppl_D_Actl", "EngBrakeData"), # PCM driver brake pedal pressed
|
||||
("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph)
|
||||
# The units might change with IPC settings?
|
||||
("CcStat_D_Actl", "EngBrakeData"), # PCM ACC status
|
||||
("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg)
|
||||
# Calculates steering angle (and offset) from pinion
|
||||
# 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)
|
||||
|
||||
@@ -1,50 +1,144 @@
|
||||
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):
|
||||
"""Creates a CAN message for the Ford Steer Command."""
|
||||
def create_lkas_command(packer, angle_deg: float, curvature: float):
|
||||
"""
|
||||
Creates a CAN message for the Ford LKAS Command.
|
||||
|
||||
#if enabled and lkas available:
|
||||
if enabled and lkas_state in (2, 3): # and (frame % 500) >= 3:
|
||||
action = lkas_action
|
||||
This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the
|
||||
PSCM lockout.
|
||||
|
||||
Frequency is 20Hz.
|
||||
"""
|
||||
|
||||
values = {
|
||||
"LkaDrvOvrrd_D_Rq": 0, # driver override level? [0|3]
|
||||
"LkaActvStats_D2_Req": 0, # action [0|7]
|
||||
"LaRefAng_No_Req": angle_deg, # angle [-102.4|102.3] degrees
|
||||
"LaRampType_B_Req": 0, # Ramp speed: 0=Smooth, 1=Quick
|
||||
"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_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.
|
||||
|
||||
Stock functionality is maintained by passing through unmodified signals.
|
||||
|
||||
Frequency is 1Hz.
|
||||
"""
|
||||
|
||||
# LaActvStats_D_Dsply
|
||||
# TODO: get LDW state from OP
|
||||
if enabled:
|
||||
lines = 6
|
||||
elif main_on:
|
||||
lines = 0
|
||||
else:
|
||||
action = 0xf
|
||||
angle_cmd = angle_steers/MAX_ANGLE
|
||||
|
||||
angle_cmd = clip(angle_cmd * MAX_ANGLE, - MAX_ANGLE, MAX_ANGLE)
|
||||
lines = 30
|
||||
|
||||
values = {
|
||||
"Lkas_Action": action,
|
||||
"Lkas_Alert": 0xf, # no alerts
|
||||
"Lane_Curvature": clip(curvature, -0.01, 0.01), # is it just for debug?
|
||||
#"Lane_Curvature": 0, # is it just for debug?
|
||||
"Steer_Angle_Req": angle_cmd
|
||||
"FeatConfigIpmaActl": stock_values["FeatConfigIpmaActl"], # [0|65535]
|
||||
"FeatNoIpmaActl": stock_values["FeatNoIpmaActl"], # [0|65535]
|
||||
"PersIndexIpma_D_Actl": stock_values["PersIndexIpma_D_Actl"], # [0|7]
|
||||
"AhbcRampingV_D_Rq": stock_values["AhbcRampingV_D_Rq"], # AHB ramping [0|3]
|
||||
"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_Control", 0, values)
|
||||
return packer.make_can_msg("IPMA_Data", 0, values)
|
||||
|
||||
|
||||
def create_lkas_ui(packer, main_on, enabled, steer_alert):
|
||||
"""Creates a CAN message for the Ford Steer Ui."""
|
||||
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.
|
||||
|
||||
if not main_on:
|
||||
lines = 0xf
|
||||
elif enabled:
|
||||
lines = 0x3
|
||||
else:
|
||||
lines = 0x6
|
||||
Stock functionality is maintained by passing through unmodified signals.
|
||||
|
||||
Frequency is 20Hz.
|
||||
"""
|
||||
|
||||
values = {
|
||||
"Set_Me_X80": 0x80,
|
||||
"Set_Me_X45": 0x45,
|
||||
"Set_Me_X30": 0x30,
|
||||
"Lines_Hud": lines,
|
||||
"Hands_Warning_W_Chime": steer_alert,
|
||||
"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("Lane_Keep_Assist_Ui", 0, values)
|
||||
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 = {
|
||||
"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
Executable file → Normal file
83
selfdrive/car/ford/interface.py
Executable file → Normal file
@@ -1,65 +1,84 @@
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
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, get_safety_config
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
|
||||
from selfdrive.car.ford.values import TransmissionType, CAR
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
|
||||
EventName = car.CarEvent.EventName
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
|
||||
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.wheelbase = 2.85
|
||||
ret.steerRatio = 14.8
|
||||
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
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
|
||||
# Angle-based steering
|
||||
# TODO: use curvature control when ready
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle
|
||||
ret.steerActuatorDelay = 0.1
|
||||
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.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)
|
||||
|
||||
# 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,
|
||||
tire_stiffness_factor=tire_stiffness_factor)
|
||||
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle
|
||||
|
||||
return ret
|
||||
|
||||
def _update(self, c):
|
||||
ret = self.CS.update(self.cp)
|
||||
ret = self.CS.update(self.cp, self.cp_cam)
|
||||
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
|
||||
# events
|
||||
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)
|
||||
|
||||
ret.events = events.to_msg()
|
||||
|
||||
return ret
|
||||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c):
|
||||
|
||||
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.hudControl.visualAlert, c.cruiseControl.cancel)
|
||||
|
||||
ret = self.CC.update(c, self.CS, self.frame)
|
||||
self.frame += 1
|
||||
return ret
|
||||
|
||||
4
selfdrive/car/ford/radar_interface.py
Executable file → Normal file
4
selfdrive/car/ford/radar_interface.py
Executable file → Normal file
@@ -2,7 +2,7 @@
|
||||
from cereal import car
|
||||
from common.conversions import Conversions as CV
|
||||
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
|
||||
|
||||
RADAR_MSGS = list(range(0x500, 0x540))
|
||||
@@ -14,7 +14,7 @@ def _create_radar_can_parser(car_fingerprint):
|
||||
RADAR_MSGS * 3))
|
||||
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):
|
||||
def __init__(self, CP):
|
||||
|
||||
@@ -1,21 +1,82 @@
|
||||
from collections import namedtuple
|
||||
from typing import Dict, List, Union
|
||||
|
||||
from cereal import car
|
||||
from selfdrive.car import dbc_dict
|
||||
from selfdrive.car.docs_definitions import CarInfo
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
MAX_ANGLE = 87. # make sure we never command the extremes (0xfff) which cause latching fault
|
||||
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])
|
||||
|
||||
|
||||
class CANBUS:
|
||||
main = 0
|
||||
radar = 1
|
||||
camera = 2
|
||||
|
||||
|
||||
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.FUSION: CarInfo("Ford Fusion 2018", "All")
|
||||
}
|
||||
|
||||
DBC = {
|
||||
CAR.FUSION: dbc_dict('ford_fusion_2018_pt', 'ford_fusion_2018_adas'),
|
||||
|
||||
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 = {
|
||||
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,6 +3,7 @@ from collections import namedtuple
|
||||
|
||||
from selfdrive.car.chrysler.values import CAR as CHRYSLER
|
||||
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.hyundai.values import CAR as HYUNDAI
|
||||
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
|
||||
non_tested_cars = [
|
||||
FORD.ESCAPE_MK4,
|
||||
FORD.FOCUS_MK4,
|
||||
GM.CADILLAC_ATS,
|
||||
GM.HOLDEN_ASTRA,
|
||||
GM.MALIBU,
|
||||
|
||||
Reference in New Issue
Block a user