diff --git a/release/files_common b/release/files_common index 67e4c0e31cd0365..9a234bc2755f68e 100644 --- a/release/files_common +++ b/release/files_common @@ -560,8 +560,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 diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 389d3d8d8bffb67..c2f87a48068bd1c 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -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 - if pcm_cancel: - #print "CANCELING!!!!" - can_sends.append(spam_cancel_button(self.packer)) + actuators = CC.actuators + hud_control = CC.hudControl - 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 - 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 + # 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 - 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 + # send steering commands at 20Hz + if (frame % CarControllerParams.LKAS_STEER_STEP) == 0: + 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)) - #can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0)) + # convert steer angle to curvature + 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 \ - (self.steer_alert_last != steer_alert): - can_sends.append(create_lkas_ui(self.packer, CS.out.cruiseState.available, enabled, steer_alert)) + # TODO: get other actuators + curvature_rate = 0 + path_offset = 0 - if (frame % 200) == 0: - can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1)) + ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately + 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)) - 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_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(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 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)) - 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)) + # 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)) - 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 diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index eba623f5ce215a2..21013e94d09974e 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -1,58 +1,221 @@ +from typing import Dict + from cereal import car +from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser -from common.numpy_fast import mean from selfdrive.config import Conversions as CV 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.steerError = 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 + self.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.steerWarning = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1 + ret.steerError = 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) diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index 5a8b0b2dec5d79b..29fec1f2dedc36b 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -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 - else: - action = 0xf - angle_cmd = angle_steers/MAX_ANGLE + This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the + PSCM lockout. - angle_cmd = clip(angle_cmd * MAX_ANGLE, - MAX_ANGLE, MAX_ANGLE) + Frequency is 20Hz. + """ 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 + "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_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): - """Creates a CAN message for the Ford Steer Ui.""" + Stock functionality is maintained by passing through unmodified signals. - if not main_on: - lines = 0xf - elif enabled: - lines = 0x3 + Frequency is 1Hz. + """ + + # LaActvStats_D_Dsply + # TODO: get LDW state from OP + if enabled: + lines = 6 + elif main_on: + lines = 0 else: - lines = 0x6 + lines = 30 values = { - "Set_Me_X80": 0x80, - "Set_Me_X45": 0x45, - "Set_Me_X30": 0x30, - "Lines_Hud": lines, - "Hands_Warning_W_Chime": steer_alert, + "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_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 = { - "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) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py old mode 100755 new mode 100644 index 0faaa3f6698481a..69582a57c6111a8 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,70 +1,96 @@ #!/usr/bin/env python3 from cereal import car from selfdrive.config 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): 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: 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 # returns a car.CarState def update(self, c, can_strings): # ******************* do can recv ******************* self.cp.update_strings(can_strings) + self.cp_cam.update_strings(can_strings) - ret = self.CS.update(self.cp) + ret = self.CS.update(self.cp, self.cp_cam) + ret.canValid = self.cp.can_valid and self.cp_cam.can_valid + ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - ret.canValid = self.cp.can_valid - - # 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) + if ret.vEgo < self.CP.minEnableSpeed: + events.add(EventName.belowEngageSpeed) + if self.CP.parkingBrake: + events.add(EventName.parkBrake) ret.events = events.to_msg() self.CS.out = ret.as_reader() return self.CS.out - # 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 diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py old mode 100755 new mode 100644 index 94eb8bb0cc71b16..cc301c446b711e4 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 from cereal import car from opendbc.can.parser import CANParser -from selfdrive.car.ford.values import DBC from selfdrive.config import Conversions as CV +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): diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index e1e2206472f0206..8926a6f481d1719 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,12 +1,75 @@ -from selfdrive.car import dbc_dict +from collections import namedtuple + from cereal import car +from selfdrive.car import dbc_dict + 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 -MAX_ANGLE = 87. # make sure we never command the extremes (0xfff) which cause latching fault + 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" + + +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.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'), } diff --git a/selfdrive/test/test_routes.py b/selfdrive/test/test_routes.py index 02bcd36b195ccfd..73ca3c32f3d01ed 100644 --- a/selfdrive/test/test_routes.py +++ b/selfdrive/test/test_routes.py @@ -3,6 +3,7 @@ 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 @@ -14,6 +15,8 @@ # TODO: add routes for these cars non_tested_cars = [ + FORD.ESCAPE_MK4, + FORD.FOCUS_MK4, GM.CADILLAC_ATS, GM.HOLDEN_ASTRA, GM.MALIBU,