From 9241de221099f6c65fd7ed5ed87d6296c17f668d Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 8 Sep 2022 15:49:51 -0700 Subject: [PATCH] Ford Explorer 2020-21 (#25614) * add model years * update Focus steer ratio * Ford: add EU label to Focus Mk4 * add packages * add Ford Explorer 2020 Package: Co-Pilot 360 Assist+ Optional on XLT Standard on Limited, Limited Hybrid, ST and Platinum https://cdn.dealereprocess.org/cdn/brochures/ford/2020-explorer.pdf * Ford: steering control with path angle * Ford: add TJA toggle to buttons * add Ford Explorer 2021 `62241b0c7fea4589|2022-08-30--11-58-24--0` Package: Co-Pilot 360 Assist+ Optional on XLT Standard on Limited, Limited Hybrid, ST and Platinum (same as 2020) https://cdn.dealereprocess.org/cdn/brochures/ford/2021-explorer.pdf * Ford: add shiftByWire ECU fw * angle/steer refactor * try always stop and go for US models * no dashcam * car info * send resume button * skip explorer * escape and focus back in dashcam * passthru buttons * fordcan set bus for button message * toggle off stock traffic jam assist so camera does not enforce driver presence checks * not used * update ramp rate/precision notes * cleanup * bump steering pressed torque to 0.8 Nm * add standstill * bump steer ratio * try increasing delay? * fix docs * add kuga car info * maybe fix tja toggle? * compensate for ford roll compensation?? * oops * better ui * block non-adaptive * add note on ui warning for hands on wheel * try only checking/toggling TJA every 2 seconds * add car test route * dashcam only again * send buttons to camera * add process replay segment * cleanup * bump panda * add extra FW Co-authored-by: Cameron Clough --- panda | 2 +- selfdrive/car/ford/carcontroller.py | 70 +++++++++++------- selfdrive/car/ford/carstate.py | 49 ++++++++++++- selfdrive/car/ford/fordcan.py | 98 ++++++++++++++++++------- selfdrive/car/ford/interface.py | 40 +++++----- selfdrive/car/ford/values.py | 56 ++++++++++++-- selfdrive/car/fw_versions.py | 2 +- selfdrive/car/tests/routes.py | 1 + selfdrive/car/torque_data/override.yaml | 1 + 9 files changed, 236 insertions(+), 83 deletions(-) diff --git a/panda b/panda index 0ca23b677..8f13ca3f6 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 0ca23b67786d0fa2a3162371aadeca52d24a8958 +Subproject commit 8f13ca3f66bcc72e3ac9028df7ce04851e7e3a48 diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 11d9bb3c7..592d8586c 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,3 +1,4 @@ +import math from cereal import car from common.numpy_fast import clip, interp from opendbc.can.packer import CANPacker @@ -7,14 +8,19 @@ from selfdrive.car.ford.values import CarControllerParams VisualAlert = car.CarControl.HUDControl.VisualAlert -def apply_ford_steer_angle_limits(apply_steer, apply_steer_last, vEgo): +def apply_ford_steer_angle_limits(apply_angle, apply_angle_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 + steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last) + rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.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)) + apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff)) - return apply_steer + # absolute limit (LatCtlPath_An_Actl) + apply_path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO + apply_path_angle = clip(apply_path_angle, -0.4995, 0.5240) + apply_angle = math.degrees(apply_path_angle) * CarControllerParams.STEER_RATIO + + return apply_angle class CarController: @@ -24,7 +30,7 @@ class CarController: self.packer = CANPacker(dbc_name) self.frame = 0 - self.apply_steer_last = 0 + self.apply_angle_last = 0 self.main_on_last = False self.lkas_enabled_last = False self.steer_alert_last = False @@ -38,52 +44,62 @@ class CarController: main_on = CS.out.cruiseState.available steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) + ### acc buttons ### if CC.cruiseControl.cancel: - # cancel stock ACC - can_sends.append(fordcan.spam_cancel_button(self.packer)) + can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True)) + elif CC.cruiseControl.resume: + can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True)) + + # if stock lane centering is active or in standby, toggle it off + # the stock system checks for steering pressed, and eventually disengages cruise control + if (self.frame % 200) == 0 and CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0: + can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, tja_toggle=True)) + - # apply rate limits - new_steer = actuators.steeringAngleDeg - apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo) + ### lateral control ### + if CC.latActive: + apply_angle = apply_ford_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo) + else: + apply_angle = CS.out.steeringAngleDeg # send steering commands at 20Hz if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0: lca_rq = 1 if CC.latActive else 0 - # use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented - path_angle = apply_steer + # use LatCtlPath_An_Actl to actuate steering + # path angle is the car wheel angle, not the steering wheel angle + path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO - # convert steer angle to curvature - curvature = self.VM.calc_curvature(apply_steer, CS.out.vEgo, 0.0) + # ramp rate: 0=Slow, 1=Medium, 2=Fast, 3=Immediately + # TODO: try slower ramp speed when driver torque detected + ramp_type = 3 + precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable) - # TODO: get other actuators - curvature_rate = 0 - path_offset = 0 + offset_roll_compensation_curvature = clip(self.VM.calc_curvature(0, CS.out.vEgo, -CS.yaw_data["VehYaw_W_Actl"]), -0.02, 0.02094) - ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately - precision = 0 # 0=Comfortable, 1=Precise - - self.apply_steer_last = apply_steer - can_sends.append(fordcan.create_lkas_command(self.packer, apply_steer, curvature)) + self.apply_angle_last = apply_angle + can_sends.append(fordcan.create_lka_command(self.packer, apply_angle, 0)) can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision, - path_offset, path_angle, curvature_rate, curvature)) + 0, path_angle, 0, offset_roll_compensation_curvature)) + + ### ui ### send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) # send lkas ui command at 1Hz or if ui state changes if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: - can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values)) + can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values)) # send acc ui command at 20Hz or if ui state changes if (self.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)) + can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values)) self.main_on_last = main_on self.lkas_enabled_last = CC.latActive self.steer_alert_last = steer_alert new_actuators = actuators.copy() - new_actuators.steeringAngleDeg = apply_steer + new_actuators.steeringAngleDeg = apply_angle self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 3c9961e9e..a7ea19eff 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -3,7 +3,7 @@ from common.conversions import Conversions as CV 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 CANBUS, DBC +from selfdrive.car.ford.values import CANBUS, DBC, CarControllerParams GearShifter = car.CarState.GearShifter TransmissionType = car.CarParams.TransmissionType @@ -22,7 +22,7 @@ class CarState(CarStateBase): # 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.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] * CV.RAD_TO_DEG + ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1 # gas pedal @@ -37,7 +37,7 @@ class CarState(CarStateBase): # 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.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE 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 @@ -46,6 +46,8 @@ class CarState(CarStateBase): 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) + ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0 + ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3 # gear if self.CP.transmissionType == TransmissionType.automatic: @@ -65,6 +67,7 @@ class CarState(CarStateBase): # button presses ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1 ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2 + # TODO: block this going to the camera otherwise it will enable stock TJA ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"]) # lock info @@ -77,9 +80,13 @@ class CarState(CarStateBase): 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 steering buttons so that we can passthru blinkers etc. + self.buttons_stock_values = cp.vl["Steering_Data_FD1"] # Stock values from IPMA so that we can retain some stock functionality self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"] self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"] + # Use stock sensor values + self.yaw_data = cp.vl["Yaw_Data_FD1"] return ret @@ -97,6 +104,8 @@ class CarState(CarStateBase): ("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph) # The units might change with IPC settings? ("CcStat_D_Actl", "EngBrakeData"), # PCM ACC status + ("AccStopMde_D_Rq", "EngBrakeData"), # PCM ACC standstill + ("AccEnbl_B_RqDrv", "Cluster_Info1_FD1"), # PCM ACC enable ("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg) # Calculates steering angle (and offset) from pinion # angle and driving measurements. @@ -104,7 +113,6 @@ class CarState(CarStateBase): # 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 @@ -112,6 +120,38 @@ class CarState(CarStateBase): ("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 + ("HeadLghtHiFlash_D_Stat", "Steering_Data_FD1"), # SCCM Passthru the remaining buttons + ("WiprFront_D_Stat", "Steering_Data_FD1"), + ("LghtAmb_D_Sns", "Steering_Data_FD1"), + ("AccButtnGapDecPress", "Steering_Data_FD1"), + ("AccButtnGapIncPress", "Steering_Data_FD1"), + ("AslButtnOnOffCnclPress", "Steering_Data_FD1"), + ("AslButtnOnOffPress", "Steering_Data_FD1"), + ("CcAslButtnCnclPress", "Steering_Data_FD1"), + ("LaSwtchPos_D_Stat", "Steering_Data_FD1"), + ("CcAslButtnCnclResPress", "Steering_Data_FD1"), + ("CcAslButtnDeny_B_Actl", "Steering_Data_FD1"), + ("CcAslButtnIndxDecPress", "Steering_Data_FD1"), + ("CcAslButtnIndxIncPress", "Steering_Data_FD1"), + ("CcAslButtnOffCnclPress", "Steering_Data_FD1"), + ("CcAslButtnOnOffCncl", "Steering_Data_FD1"), + ("CcAslButtnOnPress", "Steering_Data_FD1"), + ("CcAslButtnResDecPress", "Steering_Data_FD1"), + ("CcAslButtnResIncPress", "Steering_Data_FD1"), + ("CcAslButtnSetDecPress", "Steering_Data_FD1"), + ("CcAslButtnSetIncPress", "Steering_Data_FD1"), + ("CcAslButtnSetPress", "Steering_Data_FD1"), + ("CcAsllButtnResPress", "Steering_Data_FD1"), + ("CcButtnOffPress", "Steering_Data_FD1"), + ("CcButtnOnOffCnclPress", "Steering_Data_FD1"), + ("CcButtnOnOffPress", "Steering_Data_FD1"), + ("CcButtnOnPress", "Steering_Data_FD1"), + ("HeadLghtHiFlash_D_Actl", "Steering_Data_FD1"), + ("HeadLghtHiOn_B_StatAhb", "Steering_Data_FD1"), + ("AhbStat_B_Dsply", "Steering_Data_FD1"), + ("AccButtnGapTogglePress", "Steering_Data_FD1"), + ("WiprFrontSwtch_D_Stat", "Steering_Data_FD1"), + ("HeadLghtHiCtrl_D_RqAhb", "Steering_Data_FD1"), ] checks = [ @@ -122,6 +162,7 @@ class CarState(CarStateBase): ("EngVehicleSpThrottle", 100), ("BrakeSnData_4", 50), ("EngBrakeData", 10), + ("Cluster_Info1_FD1", 10), ("SteeringPinion_Data", 100), ("EPAS_INFO", 50), ("Lane_Assist_Data3_FD1", 33), diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index bb104aaf3..b42561df2 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -1,7 +1,10 @@ -from common.numpy_fast import clip +from cereal import car +from selfdrive.car.ford.values import CANBUS +HUDControl = car.CarControl.HUDControl -def create_lkas_command(packer, angle_deg: float, curvature: float): + +def create_lka_command(packer, angle_deg: float, curvature: float): """ Creates a CAN message for the Ford LKAS Command. @@ -20,7 +23,7 @@ def create_lkas_command(packer, angle_deg: float, curvature: float): "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) + return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, 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): @@ -38,20 +41,20 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path """ 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 + "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": path_offset, # Path offset [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # Path angle [-0.4995|0.5240] radians + "LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2 + "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter } - return packer.make_can_msg("LateralMotionControl", 0, values) + return packer.make_can_msg("LateralMotionControl", CANBUS.main, values) -def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, stock_values: dict): +def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict): """ Creates a CAN message for the Ford IPC IPMA/LKAS status. @@ -63,23 +66,47 @@ def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bo """ # LaActvStats_D_Dsply - # TODO: get LDW state from OP + # R Intvn Warn Supprs Avail No + # L + # Intvn 24 19 14 9 4 + # Warn 23 18 13 8 3 + # Supprs 22 17 12 7 2 + # Avail 21 16 11 6 1 + # No 20 15 10 5 0 + # + # TODO: test suppress state if enabled: - lines = 6 + lines = 0 # NoLeft_NoRight + if hud_control.leftLaneDepart: + lines += 4 + elif hud_control.leftLaneVisible: + lines += 1 + if hud_control.rightLaneDepart: + lines += 20 + elif hud_control.rightLaneVisible: + lines += 5 elif main_on: lines = 0 else: - lines = 30 + if hud_control.leftLaneDepart: + lines = 3 # WarnLeft_NoRight + elif hud_control.rightLaneDepart: + lines = 15 # NoLeft_WarnRight + else: + lines = 30 # LA_Off + + # TODO: use level 1 for no sound when less severe? + hands_on_wheel_dsply = 2 if steer_alert else 0 values = { **stock_values, "LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31] - "LaHandsOff_D_Dsply": 2 if steer_alert else 0, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed + "LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed } - return packer.make_can_msg("IPMA_Data", 0, values) + return packer.make_can_msg("IPMA_Data", CANBUS.main, values) -def create_acc_ui_command(packer, main_on: bool, enabled: bool, stock_values: dict): +def create_acc_ui_command(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict): """ Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam assist status. @@ -89,14 +116,32 @@ def create_acc_ui_command(packer, main_on: bool, enabled: bool, stock_values: di Frequency is 20Hz. """ + # Tja_D_Stat + if enabled: + if hud_control.leftLaneDepart: + status = 3 # ActiveInterventionLeft + elif hud_control.rightLaneDepart: + status = 4 # ActiveInterventionRight + else: + status = 2 # Active + elif main_on: + if hud_control.leftLaneDepart: + status = 5 # ActiveWarningLeft + elif hud_control.rightLaneDepart: + status = 6 # ActiveWarningRight + else: + status = 1 # Standby + else: + status = 0 # Off + values = { **stock_values, - "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] + "Tja_D_Stat": status, } - return packer.make_can_msg("ACCDATA_3", 0, values) + return packer.make_can_msg("ACCDATA_3", CANBUS.main, values) -def spam_cancel_button(packer, cancel=1): +def create_button_command(packer, stock_values: dict, cancel = False, resume = False, tja_toggle = False, bus = CANBUS.camera): """ Creates a CAN message for the Ford SCCM buttons/switches. @@ -104,6 +149,9 @@ def spam_cancel_button(packer, cancel=1): """ values = { - "CcAslButtnCnclPress": cancel, # CC cancel button + **stock_values, + "CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button + "CcAsllButtnResPress": 1 if resume else 0, # CC resume button + "TjaButtnOnOffPress": 1 if tja_toggle else 0, # TJA toggle button } - return packer.make_can_msg("Steering_Data_FD1", 0, values) + return packer.make_can_msg("Steering_Data_FD1", bus, values) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 271b34714..7d4c9eb94 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 from cereal import car from common.conversions import Conversions as CV -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint -from selfdrive.car.ford.values import CAR, TransmissionType, GearShifter +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 CAR, Ecu, TransmissionType, GearShifter from selfdrive.car.interfaces import CarInterfaceBase @@ -12,59 +12,59 @@ EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): + if car_fw is None: + car_fw = [] + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "ford" - #ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] ret.dashcamOnly = True + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] # Angle-based steering - # TODO: use curvature control when ready ret.steerControlType = car.CarParams.SteerControlType.angle - ret.steerActuatorDelay = 0.1 + ret.steerActuatorDelay = 0.4 ret.steerLimitTimer = 1.0 - - # TODO: detect stop-and-go vehicles - stop_and_go = False + tire_stiffness_factor = 1.0 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.EXPLORER_MK6: + ret.wheelbase = 3.025 + ret.steerRatio = 16.8 # learned + ret.mass = 2050 + STD_CARGO_KG + elif candidate == CAR.FOCUS_MK4: ret.wheelbase = 2.7 - ret.steerRatio = 14.3 - tire_stiffness_factor = 0.5328 + ret.steerRatio = 13.8 # learned 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]: + # Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1 + found_ecus = [fw.ecu for fw in car_fw] + if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[0]: ret.transmissionType = TransmissionType.automatic else: ret.transmissionType = TransmissionType.manual + ret.minEnableSpeed = 20.0 * CV.MPH_TO_MS # BSM: Side_Detect_L_Stat, Side_Detect_R_Stat # TODO: detect bsm in car_fw? ret.enableBsm = 0x3A6 in fingerprint[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.centerToFront = ret.wheelbase * 0.44 - + ret.autoResumeSng = ret.minEnableSpeed == -1. ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + ret.centerToFront = ret.wheelbase * 0.44 ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - return ret def _update(self, c): diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 2624fd252..54ff043a3 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,9 +1,11 @@ from collections import namedtuple +from dataclasses import dataclass +from enum import Enum from typing import Dict, List, Union from cereal import car from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarInfo +from selfdrive.car.docs_definitions import CarInfo, Harness Ecu = car.CarParams.Ecu TransmissionType = car.CarParams.TransmissionType @@ -20,8 +22,11 @@ class CarControllerParams: # 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]) + STEER_RATIO = 2.75 + STEER_DRIVER_ALLOWANCE = 0.8 + + RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) + RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) class RADAR: @@ -37,12 +42,23 @@ class CANBUS: class CAR: ESCAPE_MK4 = "FORD ESCAPE 4TH GEN" + EXPLORER_MK6 = "FORD EXPLORER 6TH GEN" FOCUS_MK4 = "FORD FOCUS 4TH GEN" +@dataclass +class FordCarInfo(CarInfo): + package: str = "Co-Pilot360 Assist+" + harness: Enum = Harness.ford_q3 + + CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { - CAR.ESCAPE_MK4: CarInfo("Ford Escape", "NA"), - CAR.FOCUS_MK4: CarInfo("Ford Focus", "NA"), + CAR.ESCAPE_MK4: [ + FordCarInfo("Ford Escape 2020"), + FordCarInfo("Ford Kuga EU", "Driver Assistance Pack"), + ], + CAR.EXPLORER_MK6: FordCarInfo("Ford Explorer 2020-21"), + CAR.FOCUS_MK4: FordCarInfo("Ford Focus EU 2019", "Driver Assistance Pack"), } @@ -63,6 +79,33 @@ FW_VERSIONS = { (Ecu.engine, 0x7E0, None): [ b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], + (Ecu.shiftByWire, 0x732, None): [ + ], + }, + CAR.EXPLORER_MK6: { + (Ecu.eps, 0x730, None): [ + b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'L1MC-2D053-BF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'LB5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LB5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7E0, None): [ + b'LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'MB5A-14C204-MD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.shiftByWire, 0x732, None): [ + b'L1MP-14G395-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'L1MP-14G395-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], }, CAR.FOCUS_MK4: { (Ecu.eps, 0x730, None): [ @@ -80,11 +123,14 @@ FW_VERSIONS = { (Ecu.engine, 0x7E0, None): [ b'JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], + (Ecu.shiftByWire, 0x732, None): [ + ], }, } DBC = { CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR), + CAR.EXPLORER_MK6: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR), CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR), } diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index e6faa0e95..376a743df 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -240,7 +240,7 @@ REQUESTS: List[Request] = [ [TESTER_PRESENT_REQUEST, FORD_VERSION_REQUEST], [TESTER_PRESENT_RESPONSE, FORD_VERSION_RESPONSE], bus=0, - whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera], + whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire], ), ] diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index b769cbe5f..635f43cc8 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -41,6 +41,7 @@ routes = [ CarTestRoute("221c253375af4ee9|2022-06-15--18-38-24", CHRYSLER.RAM_1500), CarTestRoute("8fb5eabf914632ae|2022-08-04--17-28-53", CHRYSLER.RAM_HD, segment=6), + CarTestRoute("62241b0c7fea4589|2022-09-01--15-32-49", FORD.EXPLORER_MK6), #TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION), CarTestRoute("7cc2a8365b4dd8a9|2018-12-02--12-10-44", GM.ACADIA), diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index 14ee7e223..fcc762c2b 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -13,6 +13,7 @@ TESLA AP2 MODEL S: [.nan, 2.5, .nan] # Guess FORD ESCAPE 4TH GEN: [.nan, 1.5, .nan] +FORD EXPLORER 6TH GEN: [.nan, 1.5, .nan] FORD FOCUS 4TH GEN: [.nan, 1.5, .nan] ###