diff --git a/common/params_keys.h b/common/params_keys.h index e1a924167..0f91b612d 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -335,6 +335,16 @@ inline static std::unordered_map keys = { {"ForceStopDistanceOffset", {PERSISTENT, INT, "0", "0", 2, SETTINGS_SIMPLE}}, {"ForceStandstill", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}}, {"ForceTorqueController", {PERSISTENT, BOOL, "0", "0", 3}}, + {"FordAngleBlend", {PERSISTENT, FLOAT, "0.5", "0.5", 2}}, + {"FordAngleHighSpeedDamping", {PERSISTENT, FLOAT, "1.0", "1.0", 2}}, + {"FordAngleHighSpeedFactor", {PERSISTENT, FLOAT, "1.0", "1.0", 2}}, + {"FordAngleLaneChangeFactor", {PERSISTENT, FLOAT, "1.0", "1.0", 2}}, + {"FordAngleLowSpeedFactor", {PERSISTENT, FLOAT, "1.0", "1.0", 2}}, + {"FordCurvatureBlendHigh", {PERSISTENT, FLOAT, "0.4", "0.4", 2}}, + {"FordCurvatureBlendLow", {PERSISTENT, FLOAT, "0.4", "0.4", 2}}, + {"FordCurvatureLaneChangeFactor", {PERSISTENT, FLOAT, "0.85", "0.85", 2}}, + {"FordHumanTurnDetection", {PERSISTENT, BOOL, "1", "1", 2}}, + {"FordLateralMode", {PERSISTENT, INT, "1", "1", 2}}, {"FLMActiveOverrides", {PERSISTENT, JSON, "{}", "{}", 2}}, {"FLMActiveProfileId", {PERSISTENT, STRING, "", "", 2}}, {"FLMSubmittedTune", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}}, diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 19bcb2015..b07986b78 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -6,6 +6,8 @@ from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits from opendbc.car.ford import fordcan from opendbc.car.ford.values import CarControllerParams, FordFlags, CAR from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX +from openpilot.starpilot.car.ford import fordcan as starpilot_fordcan +from openpilot.starpilot.car.ford.lateral import FordLateralController, FordLateralMode, FordLateralResult LongCtrlState = structs.CarControl.Actuators.LongControlState VisualAlert = structs.CarControl.HUDControl.VisualAlert @@ -80,6 +82,9 @@ class CarController(CarControllerBase): self.steer_alert_last = False self.lead_distance_bars_last = None self.distance_bar_frame = 0 + self.ford_lateral = None if CP.flags & FordFlags.LKA_STEERING else FordLateralController(CP) + self.ford_shadow_curvature = 0.0 + self.ford_lateral_announced_mode = FordLateralMode.native def update(self, CC, CS, now_nanos, starpilot_toggles): can_sends = [] @@ -91,6 +96,9 @@ class CarController(CarControllerBase): steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) fcw_alert = hud_control.visualAlert == VisualAlert.fcw + if self.ford_lateral is not None: + self.ford_lateral.update_inputs() + ### acc buttons ### if CC.cruiseControl.cancel: can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True)) @@ -128,38 +136,70 @@ class CarController(CarControllerBase): can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN, active=lka_active, apply_angle=self.apply_angle_last, direction=direction, ramp_type=ramp_type, curvature=-self.apply_curvature_last)) else: - # send steer msg at 20Hz - if (self.frame % CarControllerParams.STEER_STEP) == 0: - # Bronco and some other cars consistently overshoot curv requests - # Apply some deadzone + smoothing convergence to avoid oscillations - if self.CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14): - self.anti_overshoot_curvature_last = anti_overshoot(actuators.curvature, self.anti_overshoot_curvature_last, CS.out.vEgoRaw) - apply_curvature = self.anti_overshoot_curvature_last + lateral_mode = self.ford_lateral.mode + lateral_mode_ready = lateral_mode == self.ford_lateral_announced_mode + + # Keep the original Ford path available without changing its command behavior. + if lateral_mode == FordLateralMode.native: + if (self.frame % CarControllerParams.STEER_STEP) == 0: + if not lateral_mode_ready: + self.apply_curvature_last = 0.0 + apply_curvature = 0.0 + elif self.CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14): + self.anti_overshoot_curvature_last = anti_overshoot( + actuators.curvature, self.anti_overshoot_curvature_last, CS.out.vEgoRaw) + apply_curvature = self.anti_overshoot_curvature_last + else: + apply_curvature = actuators.curvature + + current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) + self.apply_curvature_last = apply_ford_curvature_limits( + apply_curvature, self.apply_curvature_last, current_curvature, + CS.out.vEgoRaw, 0., CC.latActive and lateral_mode_ready, self.CP) + + if self.CP.flags & FordFlags.CANFD: + mode = 1 if CC.latActive and lateral_mode_ready else 0 + counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 + can_sends.append(fordcan.create_lat_ctl2_msg( + self.packer, self.CAN, mode, 0., 0., -self.apply_curvature_last, 0., counter)) + else: + can_sends.append(fordcan.create_lat_ctl_msg( + self.packer, self.CAN, CC.latActive and lateral_mode_ready, 0., 0., -self.apply_curvature_last, 0.)) + elif (self.frame % CarControllerParams.STEER_STEP) == 0: + if not lateral_mode_ready: + lateral = FordLateralResult(shadow_curvature=self.ford_lateral._current_curvature(CS)) + elif lateral_mode == FordLateralMode.angle: + lateral = self.ford_lateral.update_angle(CC, CS, actuators) else: - apply_curvature = actuators.curvature - - # apply rate limits, curvature error limit, and clip to signal range - current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) - - self.apply_curvature_last = apply_ford_curvature_limits(apply_curvature, self.apply_curvature_last, current_curvature, - CS.out.vEgoRaw, 0., CC.latActive, self.CP) + lateral = self.ford_lateral.update_curvature(CC, CS, actuators) + self.apply_curvature_last = lateral.curvature + self.ford_shadow_curvature = lateral.shadow_curvature if self.CP.flags & FordFlags.CANFD: - # TODO: extended mode - # Ford uses four individual signals to dictate how to drive to the car. Curvature alone (limited to 0.02m/s^2) - # can actuate the steering for a large portion of any lateral movements. However, in order to get further control on - # steer actuation, the other three signals are necessary. Ford controls vehicles differently than most other makes. - # A detailed explanation on ford control can be found here: - # https://www.f150gen14.com/forum/threads/introducing-bluepilot-a-ford-specific-fork-for-comma3x-openpilot.24241/#post-457706 - mode = 1 if CC.latActive else 0 counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 - can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -self.apply_curvature_last, 0., counter)) + can_sends.append(starpilot_fordcan.create_lat_ctl2_msg( + self.packer, self.CAN, 1 if lateral.active else 0, + lateral.ramp_type, lateral.precision_type, + -lateral.path_offset, -lateral.path_angle, + -lateral.curvature, -lateral.curvature_rate, counter)) else: - can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -self.apply_curvature_last, 0.)) + can_sends.append(starpilot_fordcan.create_lat_ctl_msg( + self.packer, self.CAN, lateral.active, + lateral.ramp_type, lateral.precision_type, + -lateral.path_offset, -lateral.path_angle, + -lateral.curvature, -lateral.curvature_rate)) - # send lka msg at 33Hz if (self.frame % CarControllerParams.LKA_STEP) == 0: - can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN)) + if lateral_mode == FordLateralMode.native: + can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN)) + else: + angle_mode = lateral_mode == FordLateralMode.angle + shadow_curvature = -self.ford_lateral._current_curvature(CS) + if angle_mode: + shadow_curvature = -self.ford_shadow_curvature + can_sends.append(starpilot_fordcan.create_lka_msg( + self.packer, self.CAN, angle_mode=angle_mode, shadow_curvature=shadow_curvature)) + self.ford_lateral_announced_mode = lateral_mode ### longitudinal control ### # send acc msg at 50Hz diff --git a/opendbc_repo/opendbc/car/ford/carstate.py b/opendbc_repo/opendbc/car/ford/carstate.py index f35a3b491..bb4cc0e48 100644 --- a/opendbc_repo/opendbc/car/ford/carstate.py +++ b/opendbc_repo/opendbc/car/ford/carstate.py @@ -16,12 +16,18 @@ class CarState(CarStateBase): super().__init__(CP, FPCP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) if CP.transmissionType == TransmissionType.automatic: - self.shifter_values = can_define.dv["PowertrainData_10"]["TrnRng_D_Rq"] + if CP.flags & FordFlags.CANFD: + self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"] + elif CP.flags & FordFlags.ALT_STEER_ANGLE: + self.shifter_values = can_define.dv["TransGearData"]["GearLvrPos_D_Actl"] + else: + self.shifter_values = can_define.dv["PowertrainData_10"]["TrnRng_D_Rq"] self.distance_button = 0 self.lc_button = 0 self.lkas_available = False self.lateral_motion_control = None + self.steering_angle_offset_deg = 0.0 def update(self, can_parsers, starpilot_toggles) -> structs.CarState: cp = can_parsers[Bus.pt] @@ -29,13 +35,24 @@ class CarState(CarStateBase): ret = structs.CarState() - # Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement - # The vehicle usually recovers out of this state within a minute of normal driving - ret.vehicleSensorsInvalid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] != 3 + if self.CP.flags & FordFlags.ALT_STEER_ANGLE: + sensors_valid = ( + int((cp.vl["ParkAid_Data"]["ExtSteeringAngleReq2"] + 1000) * 10) not in (32766, 32767) + and cp.vl["ParkAid_Data"]["EPASExtAngleStatReq"] == 0 + and cp.vl["ParkAid_Data"]["ApaSys_D_Stat"] in (0, 1) + ) + ret.vehicleSensorsInvalid = not sensors_valid + else: + # The ABS can recalibrate the steering pinion offset briefly after startup. + ret.vehicleSensorsInvalid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] != 3 # car speed ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + if self.CP.flags & FordFlags.CANFD: + ret.vEgoCluster = ((cp.vl["Cluster_Info_3_FD1"]["DISPLAY_SPEED_SCALING"] / 100) * + cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] + + cp.vl["Cluster_Info_3_FD1"]["DISPLAY_SPEED_OFFSET"]) * CV.KPH_TO_MS ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1 @@ -48,7 +65,14 @@ class CarState(CarStateBase): ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2) # steering wheel - ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"] + if self.CP.flags & FordFlags.ALT_STEER_ANGLE: + steering_angle_init = cp.vl["SteeringPinion_Data_Alt"]["StePinRelInit_An_Sns"] + if not ret.vehicleSensorsInvalid: + steering_angle_est = cp.vl["ParkAid_Data"]["ExtSteeringAngleReq2"] + self.steering_angle_offset_deg = steering_angle_est - steering_angle_init + ret.steeringAngleDeg = steering_angle_init + self.steering_angle_offset_deg + else: + ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"] ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"] ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5) ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1 @@ -60,7 +84,8 @@ class CarState(CarStateBase): ret.steerFaultTemporary |= cp.vl["Lane_Assist_Data3_FD1"]["LatCtlSte_D_Stat"] not in (1, 2, 3) # cruise state - is_metric = cp.vl["INSTRUMENT_PANEL"]["METRIC_UNITS"] == 1 if not self.CP.flags & FordFlags.CANFD else False + is_metric = cp.vl["INSTRUMENT_PANEL"]["METRIC_UNITS"] == 1 if not self.CP.flags & FordFlags.CANFD else \ + cp_cam.vl["IPMA_Data2"]["IsaVLimUnit_D_Rq"] == 1 ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS) ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5) ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5) @@ -72,7 +97,12 @@ class CarState(CarStateBase): # gear if self.CP.transmissionType == TransmissionType.automatic: - gear = self.shifter_values.get(cp.vl["PowertrainData_10"]["TrnRng_D_Rq"]) + if self.CP.flags & FordFlags.CANFD: + gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"]) + elif self.CP.flags & FordFlags.ALT_STEER_ANGLE: + gear = self.shifter_values.get(cp.vl["TransGearData"]["GearLvrPos_D_Actl"]) + else: + gear = self.shifter_values.get(cp.vl["PowertrainData_10"]["TrnRng_D_Rq"]) ret.gearShifter = self.parse_gear_shifter(gear) elif self.CP.transmissionType == TransmissionType.manual: if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]): @@ -126,6 +156,22 @@ class CarState(CarStateBase): ] fp_ret = custom.StarPilotCarState.new_message() + fp_ret.brakeLights = ret.brakePressed + try: + fp_ret.brakeLights = bool(cp.vl["BCM_Lamp_Stat_FD1"]["StopLghtOn_B_Stat"]) + except (KeyError, AttributeError): + try: + fp_ret.brakeLights = cp.vl["BrakeSysFeatures_2"]["BrkLamp_B_Rq"] == 1 + except (KeyError, AttributeError): + pass + + try: + speed_limit = cp_cam.vl["Traffic_RecognitnData"]["TsrVLim1MsgTxt_D_Rq"] + speed_limit_unit = cp_cam.vl["Traffic_RecognitnData"]["TsrVlUnitMsgTxt_D_Rq"] + speed_factor = CV.MPH_TO_MS if speed_limit_unit == 2 else CV.KPH_TO_MS if speed_limit_unit == 1 else 0.0 + fp_ret.dashboardSpeedLimit = speed_limit * speed_factor if speed_limit not in (0, 255) else 0.0 + except (KeyError, AttributeError): + fp_ret.dashboardSpeedLimit = 0.0 return ret, fp_ret diff --git a/opendbc_repo/opendbc/car/ford/fingerprints.py b/opendbc_repo/opendbc/car/ford/fingerprints.py index a48e70526..efe6326e4 100644 --- a/opendbc_repo/opendbc/car/ford/fingerprints.py +++ b/opendbc_repo/opendbc/car/ford/fingerprints.py @@ -12,6 +12,8 @@ FW_VERSIONS = { b'LX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x760, None): [ + b'LX6C-2D053-KF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LX6C-2D053-KG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LX6C-2D053-RD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LX6C-2D053-RE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LX6C-2D053-RF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -24,6 +26,21 @@ FW_VERSIONS = { b'M1PT-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, + CAR.FORD_EDGE_MK2: { + (Ecu.eps, 0x730, None): [ + b'M2GC-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'M2GC-2D053-CB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'M2GC-2D053-EA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'JX7T-14D049-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'KT4T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, CAR.FORD_ESCAPE_MK4: { (Ecu.eps, 0x730, None): [ b'LX6C-14D003-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -49,7 +66,9 @@ FW_VERSIONS = { }, CAR.FORD_ESCAPE_MK4_5: { (Ecu.eps, 0x730, None): [ + b'PZ11-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PZ11-14D003-EA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PZ11-14D003-FA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x760, None): [ b'PZ1C-2D053-EJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -58,11 +77,20 @@ FW_VERSIONS = { b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ + b'PJ6T-14H102-ABE\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PJ6T-14H102-ABL\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PJ6T-14H102-MDC\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PJ6T-14H102-MDF\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PJ6T-14H102-SCD\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PJ6T-14H102-SCG\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, CAR.FORD_EXPLORER_MK6: { (Ecu.eps, 0x730, None): [ + b'R1MC-14D003-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'R1MC-14D003-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'R1MC-14D003-AG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'R1MC-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -96,6 +124,7 @@ FW_VERSIONS = { ], (Ecu.abs, 0x760, None): [ b'RL14-2D053-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PL14-2D053-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -106,6 +135,7 @@ FW_VERSIONS = { }, CAR.FORD_F_150_MK14: { (Ecu.eps, 0x730, None): [ + b'MB3C-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'ML3V-14D003-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'ML3V-14D003-BC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'ML3V-14D003-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -136,8 +166,11 @@ FW_VERSIONS = { }, CAR.FORD_F_150_LIGHTNING_MK1: { (Ecu.abs, 0x760, None): [ + b'NL38-2D053-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PL38-2D053-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'RL38-2D053-BC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'RL38-2D053-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'TL38-2D053-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ b'ML3T-14H102-ABT\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -145,9 +178,13 @@ FW_VERSIONS = { b'RJ6T-14H102-BBC\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ + b'ML3T-14D049-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'RB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x730, None): [ + b'NL38-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'NL38-14D003-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'RL38-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, @@ -164,6 +201,8 @@ FW_VERSIONS = { b'LK9C-2D053-CN\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ + b'ML3T-14D049-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'ML3T-14D049-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ @@ -174,15 +213,42 @@ FW_VERSIONS = { CAR.FORD_FOCUS_MK4: { (Ecu.eps, 0x730, None): [ b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'JX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'JX6C-14D003-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'NX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x760, None): [ b'JX61-2D053-CJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LX61-2D053-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'NX61-2D053-MD\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', + b'JX7T-14D049-AD\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', + b'N1BT-14F397-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, + CAR.FORD_MONDEO_MK5: { + (Ecu.fwdCamera, 0x706, None): [ + b'KT4T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'KG9C-2D053-DF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x730, None): [ + b'K2GC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'JX7T-14D049-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'HS7A-14C204-CJD\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.debug, 0x7d0, None): [ + b'1U5T-14G374-DA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, CAR.FORD_MAVERICK_MK1: { @@ -207,17 +273,25 @@ FW_VERSIONS = { }, CAR.FORD_RANGER_MK2: { (Ecu.eps, 0x730, None): [ + b'JR3C-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'NB3C-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'NL14-14D003-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'NL14-14D003-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'RB3C-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x760, None): [ + b'MB3C-2D053-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'MB3C-2D053-ZJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PB3C-2D053-ZB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PB3C-2D053-ZC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PB3C-2D053-ZD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PB3C-2D053-ZG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PB3C-2D053-ZJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PB9C-2D053-ZG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'RB3C-2D053-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ + b'JX7T-14D049-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ diff --git a/opendbc_repo/opendbc/car/ford/interface.py b/opendbc_repo/opendbc/car/ford/interface.py index 17036e46f..ed336f027 100644 --- a/opendbc_repo/opendbc/car/ford/interface.py +++ b/opendbc_repo/opendbc/car/ford/interface.py @@ -31,12 +31,13 @@ class CarInterface(CarInterfaceBase): ret.radarUnavailable = Bus.radar not in DBC[candidate] ret.steerControlType = structs.CarParams.SteerControlType.angle - ret.steerActuatorDelay = 0.05 if ret.flags & FordFlags.LKA_STEERING else 0.2 + ret.steerActuatorDelay = 0.05 if ret.flags & FordFlags.LKA_STEERING else 0.22 ret.steerLimitTimer = 1.0 ret.steerAtStandstill = True ret.longitudinalTuning.kiBP = [0.] ret.longitudinalTuning.kiV = [0.5] + ret.longitudinalTuning.kpV = [0.] if not ret.radarUnavailable and DBC[candidate][Bus.radar] == RADAR.DELPHI_MRR: # average of 33.3 Hz radar timestep / 4 scan modes = 60 ms @@ -44,6 +45,10 @@ class CarInterface(CarInterfaceBase): ret.radarDelay = 0.06 CAN = CanBus(fingerprint=fingerprint) + if 0x365 in fingerprint[CAN.main]: + ret.flags |= int(FordFlags.HEV_CLUSTER_DATA) + if all(address in fingerprint[CAN.main] for address in (0x07A, 0x24B, 0x24C)): + ret.flags |= int(FordFlags.HEV_BATTERY_DATA) cfgs = [get_safety_config(structs.CarParams.SafetyModel.ford)] if CAN.main >= 4: cfgs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput)) diff --git a/opendbc_repo/opendbc/car/ford/radar_interface.py b/opendbc_repo/opendbc/car/ford/radar_interface.py index 87a8f2344..fa147a265 100644 --- a/opendbc_repo/opendbc/car/ford/radar_interface.py +++ b/opendbc_repo/opendbc/car/ford/radar_interface.py @@ -1,4 +1,5 @@ import numpy as np +from collections import deque from typing import cast from collections import defaultdict from math import cos, sin @@ -19,6 +20,7 @@ DELPHI_MRR_RADAR_MSG_COUNT = 64 DELPHI_MRR_RADAR_RANGE_COVERAGE = {0: 42, 1: 164, 2: 45, 3: 175} # scan index to detection range (m) DELPHI_MRR_MIN_LONG_RANGE_DIST = 30 # meters DELPHI_MRR_CLUSTER_THRESHOLD = 5 # meters, lateral distance and relative velocity are weighted +STEER_ASSIST_DATA_ADDR = 0x3D7 @dataclass @@ -89,12 +91,17 @@ def _create_delphi_mrr_radar_can_parser(CP) -> CANParser: return CANParser(RADAR.DELPHI_MRR, messages, CanBus(CP).radar) +def _create_steer_assist_radar_can_parser(CP) -> CANParser: + return CANParser(RADAR.STEER_ASSIST_DATA, [("Steer_Assist_Data", 20)], CanBus(CP).camera) + + class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) self.points: list[list[float]] = [] self.clusters: list[Cluster] = [] + self.v_rel_history = deque(maxlen=20) self.updated_messages = set() self.track_id = 0 @@ -111,6 +118,9 @@ class RadarInterface(RadarInterfaceBase): elif self.radar == RADAR.DELPHI_MRR: self.rcp = _create_delphi_mrr_radar_can_parser(CP) self.trigger_msg = DELPHI_MRR_RADAR_HEADER_ADDR + elif self.radar == RADAR.STEER_ASSIST_DATA: + self.rcp = _create_steer_assist_radar_can_parser(CP) + self.trigger_msg = STEER_ASSIST_DATA_ADDR else: raise ValueError(f"Unsupported radar: {self.radar}") @@ -135,10 +145,45 @@ class RadarInterface(RadarInterfaceBase): _update = self._update_delphi_mrr(ret) if not _update: return None + elif self.radar == RADAR.STEER_ASSIST_DATA: + self._update_steer_assist() ret.points = list(self.pts.values()) return ret + def _update_steer_assist(self): + msg = self.rcp.vl["Steer_Assist_Data"] + confidence = msg["CmbbObjConfdnc_D_Stat"] + if confidence <= 0: + self.pts.pop(0, None) + self.v_rel_history.clear() + return + + d_rel = msg["CmbbObjDistLong_L_Actl"] + v_rel = msg["CmbbObjRelLong_V_Actl"] + new_track = 0 not in self.pts + if new_track: + self.pts[0] = structs.RadarData.RadarPoint() + self.pts[0].trackId = self.track_id + self.track_id += 1 + elif abs(v_rel) < 1e-2: + self.v_rel_history.append(d_rel - self.pts[0].dRel) + v_rel = sum(self.v_rel_history) + else: + self.v_rel_history.clear() + + if not new_track and (abs(self.pts[0].vRel - v_rel) > 2.0 or abs(self.pts[0].dRel - d_rel) > 5.0): + self.pts[0].trackId = self.track_id + self.track_id += 1 + self.v_rel_history.clear() + + self.pts[0].dRel = d_rel + self.pts[0].yRel = msg["CmbbObjDistLat_L_Actl"] + self.pts[0].vRel = v_rel + self.pts[0].aRel = float('nan') + self.pts[0].yvRel = msg["CmbbObjRelLat_V_Actl"] + self.pts[0].measured = True + def _update_delphi_esr(self): for ii in sorted(self.updated_messages): cpt = self.rcp.vl[ii] diff --git a/opendbc_repo/opendbc/car/ford/tests/test_ford.py b/opendbc_repo/opendbc/car/ford/tests/test_ford.py index 5e3cc420b..5b709fffb 100644 --- a/opendbc_repo/opendbc/car/ford/tests/test_ford.py +++ b/opendbc_repo/opendbc/car/ford/tests/test_ford.py @@ -6,7 +6,7 @@ from parameterized import parameterized from opendbc.car.structs import CarParams from opendbc.car.fw_versions import build_fw_dict -from opendbc.car.ford.values import CAR, FW_QUERY_CONFIG, FW_PATTERN, get_platform_codes +from opendbc.car.ford.values import CAR, FW_QUERY_CONFIG, FW_PATTERN, get_platform_codes, match_vin_to_car from opendbc.car.ford.fingerprints import FW_VERSIONS Ecu = CarParams.Ecu @@ -20,6 +20,7 @@ ECU_ADDRESSES = { Ecu.engine: 0x7E0, # Powertrain Control Module (PCM) Ecu.shiftByWire: 0x732, # Gear Shift Module (GSM) Ecu.debug: 0x7D0, # Accessory Protocol Interface Module (APIM) + Ecu.hud: 0x720, # Instrument Cluster Module (ICM) } @@ -41,6 +42,16 @@ ECU_PART_NUMBER = { class TestFordFW: + def test_vin_fallback(self): + def vin(wmi, vds, powertrain, year): + return f"{wmi}{vds}{powertrain}0{year}1234567" + + assert match_vin_to_car(vin("2FM", "PK4A", "A", "N")) == {str(CAR.FORD_EDGE_MK2)} + assert match_vin_to_car(vin("3FM", "K1RA", "A", "M")) == {str(CAR.FORD_MUSTANG_MACH_E_MK1)} + assert match_vin_to_car(vin("1FT", "F1CA", "A", "M")) == {str(CAR.FORD_F_150_MK14)} + assert match_vin_to_car(vin("1FT", "F1CA", "L", "N")) == {str(CAR.FORD_F_150_LIGHTNING_MK1)} + assert match_vin_to_car("0" * 17) == set() + def test_fw_query_config(self): for (ecu, addr, subaddr) in FW_QUERY_CONFIG.extra_ecus: assert ecu in ECU_ADDRESSES, "Unknown ECU" @@ -50,10 +61,13 @@ class TestFordFW: @parameterized.expand(FW_VERSIONS.items()) def test_fw_versions(self, car_model: str, fw_versions: dict[tuple[int, int, int | None], Iterable[bytes]]): for (ecu, addr, subaddr), fws in fw_versions.items(): - assert ecu in ECU_PART_NUMBER, "Unexpected ECU" + assert ecu in ECU_ADDRESSES, "Unknown ECU" assert addr == ECU_ADDRESSES[ecu], "ECU address mismatch" assert subaddr is None, "Unexpected ECU subaddress" + if ecu not in ECU_PART_NUMBER: + continue + for fw in fws: assert len(fw) == 24, "Expected ECU response to be 24 bytes" diff --git a/opendbc_repo/opendbc/car/ford/values.py b/opendbc_repo/opendbc/car/ford/values.py index ef2b0e3ab..8d9eec2e8 100644 --- a/opendbc_repo/opendbc/car/ford/values.py +++ b/opendbc_repo/opendbc/car/ford/values.py @@ -8,6 +8,7 @@ from opendbc.car.lateral import AngleSteeringLimits from opendbc.car.structs import CarParams from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column from opendbc.car.fw_query_definitions import FwQueryConfig, LiveFwVersions, OfflineFwVersions, Request, StdQueries, p16 +from opendbc.car.vin import Vin, is_valid_vin Ecu = CarParams.Ecu @@ -53,11 +54,15 @@ class FordFlags(IntFlag): # Static flags CANFD = 1 LKA_STEERING = 2 + ALT_STEER_ANGLE = 4 + HEV_CLUSTER_DATA = 8 + HEV_BATTERY_DATA = 16 class RADAR: DELPHI_ESR = 'ford_fusion_2018_adas' DELPHI_MRR = 'FORD_CADS' + STEER_ASSIST_DATA = 'ford_lincoln_base_pt' class Footnote(Enum): @@ -87,6 +92,10 @@ class FordCarDocs(CarDocs): @dataclass class FordPlatformConfig(PlatformConfig): + wmis: set[str] = field(default_factory=set) + vds_codes: set[str] = field(default_factory=set) + years: set[str] = field(default_factory=set) + dbc_dict: DbcDict = field(default_factory=lambda: { Bus.pt: 'ford_lincoln_base_pt', Bus.radar: RADAR.DELPHI_MRR, @@ -106,6 +115,7 @@ class FordPlatformConfig(PlatformConfig): class FordCANFDPlatformConfig(FordPlatformConfig): dbc_dict: DbcDict = field(default_factory=lambda: { Bus.pt: 'ford_lincoln_base_pt', + Bus.radar: RADAR.STEER_ASSIST_DATA, }) def init(self): @@ -129,10 +139,23 @@ class FordF150LightningPlatform(FordCANFDPlatformConfig): self.car_docs = [] +MY_2020, MY_2021, MY_2022, MY_2023, MY_2024, MY_2025 = 'L', 'M', 'N', 'P', 'R', 'S' +F150_VDS_CODES = {'F1C', 'F1E', 'W1C', 'W1E', 'X1C', 'X1E', 'W1R', 'W1P', 'W1S', 'W1T'} +F150_ELECTRIC_CODES = {'L', 'V'} +MACH_E_VDS_CODES = {'K1R', 'K1S', 'K2S', 'K3R', 'K3S', 'K4S'} + + class CAR(Platforms): FORD_BRONCO_SPORT_MK1 = FordPlatformConfig( [FordCarDocs("Ford Bronco Sport 2021-24")], CarSpecs(mass=1625, wheelbase=2.67, steerRatio=17.7), + wmis={'3FM'}, vds_codes={'CR9'}, years={MY_2021, MY_2022, MY_2023, MY_2024}, + ) + FORD_EDGE_MK2 = FordPlatformConfig( + [FordCarDocs("Ford Edge 2022")], + CarSpecs(mass=1933, wheelbase=2.824, steerRatio=15.3), + flags=FordFlags.ALT_STEER_ANGLE, + wmis={'2FM'}, vds_codes={'PK4'}, years={MY_2022}, ) FORD_ESCAPE_MK4 = FordPlatformConfig( [ @@ -140,6 +163,7 @@ class CAR(Platforms): FordCarDocs("Ford Kuga 2020-23", "Adaptive Cruise Control with Lane Centering", hybrid=True, plug_in_hybrid=True), ], CarSpecs(mass=1750, wheelbase=2.71, steerRatio=16.7), + wmis={'1FM'}, vds_codes={'CU0', 'CU9'}, years={MY_2020, MY_2021, MY_2022}, ) FORD_ESCAPE_MK4_5 = FordCANFDPlatformConfig( [ @@ -148,6 +172,7 @@ class CAR(Platforms): FordCarDocs("Ford Kuga Plug-in Hybrid 2024", "All"), ], CarSpecs(mass=1750, wheelbase=2.71, steerRatio=16.7), + wmis={'1FM'}, vds_codes={'CU0', 'CU9'}, years={MY_2023, MY_2024}, ) FORD_EXPLORER_MK6 = FordPlatformConfig( [ @@ -155,37 +180,49 @@ class CAR(Platforms): FordCarDocs("Lincoln Aviator 2020-24", "Co-Pilot360 Plus", plug_in_hybrid=True), # Hybrid: Grand Touring only ], CarSpecs(mass=2050, wheelbase=3.025, steerRatio=16.8), + wmis={'1FM', '5LM'}, vds_codes={'5K7', '5K8', '5J7'}, + years={MY_2020, MY_2021, MY_2022, MY_2023, MY_2024}, ) FORD_EXPEDITION_MK4 = FordCANFDPlatformConfig( [FordCarDocs("Ford Expedition 2022-24", "Co-Pilot360 Assist 2.0", hybrid=False)], CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0), + wmis={'1FM'}, vds_codes={'JU1', 'JU2', 'JK1'}, years={MY_2022, MY_2023, MY_2024}, ) FORD_F_150_MK14 = FordCANFDPlatformConfig( [FordCarDocs("Ford F-150 2021-23", "Co-Pilot360 Assist 2.0", hybrid=True)], - CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0), + CarSpecs(mass=3334, wheelbase=3.99, steerRatio=17.0), + wmis={'1FT'}, vds_codes=F150_VDS_CODES, years={MY_2021, MY_2022, MY_2023}, ) FORD_F_150_LIGHTNING_MK1 = FordF150LightningPlatform( - [FordCarDocs("Ford F-150 Lightning 2022-23", "Co-Pilot360 Assist 2.0")], + [FordCarDocs("Ford F-150 Lightning 2022-25", "Co-Pilot360 Assist 2.0")], CarSpecs(mass=2948, wheelbase=3.70, steerRatio=16.9), + wmis={'1FT'}, vds_codes=F150_VDS_CODES, years={MY_2022, MY_2023, MY_2024, MY_2025}, ) FORD_FOCUS_MK4 = FordPlatformConfig( - [FordCarDocs("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS], hybrid=True)], # mHEV only + [FordCarDocs("Ford Focus 2018-22", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS], hybrid=True)], # mHEV only CarSpecs(mass=1350, wheelbase=2.7, steerRatio=15.0), ) + FORD_MONDEO_MK5 = FordCANFDPlatformConfig( + [FordCarDocs("Ford Mondeo 2014-22", "Adaptive Cruise Control with Lane Centering")], + CarSpecs(mass=1550, wheelbase=2.85, steerRatio=14.8), + ) FORD_MAVERICK_MK1 = FordPlatformConfig( [ FordCarDocs("Ford Maverick 2022", "LARIAT Luxury", hybrid=True), FordCarDocs("Ford Maverick 2023-24", "Co-Pilot360 Assist", hybrid=True), ], CarSpecs(mass=1650, wheelbase=3.076, steerRatio=17.0), + wmis={'3FT'}, vds_codes={'TW8'}, years={MY_2022, MY_2023, MY_2024}, ) FORD_MUSTANG_MACH_E_MK1 = FordCANFDPlatformConfig( [FordCarDocs("Ford Mustang Mach-E 2021-24", "All", setup_video="https://www.youtube.com/watch?v=AR4_eTF3b_A")], CarSpecs(mass=2200, wheelbase=2.984, steerRatio=17.0), # TODO: check steer ratio + wmis={'3FM'}, vds_codes=MACH_E_VDS_CODES, years={MY_2021, MY_2022, MY_2023, MY_2024}, ) FORD_RANGER_MK2 = FordCANFDPlatformConfig( [FordCarDocs("Ford Ranger 2024", "Adaptive Cruise Control with Lane Centering", setup_video="https://www.youtube.com/watch?v=2oJlXCKYOy0")], CarSpecs(mass=2000, wheelbase=3.27, steerRatio=17.0), + wmis={'1FT'}, vds_codes={'ER4'}, years={MY_2024}, ) FORD_TRANSIT_MK5 = FordLKASteeringPlatformConfig( [FordCarDocs("Ford Transit 2025", "Co-Pilot360 Assist+")], @@ -257,7 +294,25 @@ def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, vin: str, offline_fw if valid_expected_ecus.issubset(valid_found_ecus): candidates.add(candidate) - return candidates + return candidates or match_vin_to_car(vin) + + +def match_vin_to_car(vin: str) -> set[str]: + if not is_valid_vin(vin): + return set() + + vin_obj = Vin(vin) + vds = vin[3:7] + model_year = vin[9] + candidates = {platform for platform in CAR if vin_obj.wmi in platform.config.wmis and + model_year in platform.config.years and + any(code in vds for code in platform.config.vds_codes)} + + if {CAR.FORD_F_150_MK14, CAR.FORD_F_150_LIGHTNING_MK1} & candidates: + electric = vin[7] in F150_ELECTRIC_CODES + candidates.discard(CAR.FORD_F_150_MK14 if electric else CAR.FORD_F_150_LIGHTNING_MK1) + + return {str(candidate) for candidate in candidates} # All of these ECUs must be present and are expected to have platform codes we can match @@ -266,10 +321,10 @@ PLATFORM_CODE_ECUS = (Ecu.abs, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.eps) DATA_IDENTIFIER_FORD_ASBUILT = 0xDE00 ASBUILT_BLOCKS: list[tuple[int, list]] = [ - (1, [Ecu.debug, Ecu.fwdCamera, Ecu.eps]), - (2, [Ecu.abs, Ecu.debug, Ecu.eps]), - (3, [Ecu.abs, Ecu.debug, Ecu.eps]), - (4, [Ecu.debug, Ecu.fwdCamera]), + (1, [Ecu.debug, Ecu.fwdCamera, Ecu.eps, Ecu.hud]), + (2, [Ecu.abs, Ecu.debug, Ecu.eps, Ecu.hud]), + (3, [Ecu.abs, Ecu.debug, Ecu.eps, Ecu.hud]), + (4, [Ecu.debug, Ecu.fwdCamera, Ecu.hud]), (5, [Ecu.debug]), (6, [Ecu.debug]), (7, [Ecu.debug]), @@ -297,13 +352,13 @@ FW_QUERY_CONFIG = FwQueryConfig( Request( [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], - whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire], + whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire, Ecu.hud], logging=True, ), Request( [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], - whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire], + whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire, Ecu.hud], bus=0, auxiliary=True, ), @@ -320,7 +375,9 @@ FW_QUERY_CONFIG = FwQueryConfig( # Note: We are unlikely to get a response from behind the gateway (Ecu.shiftByWire, 0x732, None), # Gear Shift Module (Ecu.debug, 0x7d0, None), # Accessory Protocol Interface Module + (Ecu.hud, 0x720, None), # Instrument Cluster Module ], + non_essential_ecus={Ecu.eps: [CAR.FORD_F_150_LIGHTNING_MK1, CAR.FORD_EXPEDITION_MK4]}, # Custom fuzzy fingerprinting function using platform and model year hints match_fw_to_car_fuzzy=match_fw_to_car_fuzzy, ) diff --git a/opendbc_repo/opendbc/car/torque_data/override.toml b/opendbc_repo/opendbc/car/torque_data/override.toml index 61cd088e1..15fff7cad 100644 --- a/opendbc_repo/opendbc/car/torque_data/override.toml +++ b/opendbc_repo/opendbc/car/torque_data/override.toml @@ -31,6 +31,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "FORD_BRONCO_SPORT_MK1" = [nan, 1.5, nan] "FORD_ESCAPE_MK4" = [nan, 1.5, nan] "FORD_ESCAPE_MK4_5" = [nan, 1.5, nan] +"FORD_EDGE_MK2" = [nan, 1.5, nan] "FORD_EXPLORER_MK6" = [nan, 1.5, nan] "FORD_EXPEDITION_MK4" = [nan, 1.5, nan] "FORD_F_150_MK14" = [nan, 1.5, nan] @@ -39,6 +40,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "FORD_F_150_LIGHTNING_MK1" = [nan, 1.5, nan] "FORD_MUSTANG_MACH_E_MK1" = [nan, 1.5, nan] "FORD_RANGER_MK2" = [nan, 1.5, nan] +"FORD_MONDEO_MK5" = [nan, 1.5, nan] "FORD_TRANSIT_MK5" = [nan, 1.5, nan] ### diff --git a/opendbc_repo/opendbc/safety/modes/ford.h b/opendbc_repo/opendbc/safety/modes/ford.h index fb159e4d5..7f639c4ac 100644 --- a/opendbc_repo/opendbc/safety/modes/ford.h +++ b/opendbc_repo/opendbc/safety/modes/ford.h @@ -87,6 +87,9 @@ static bool ford_get_quality_flag_valid(const CANPacket_t *msg) { #define FORD_CANFD_INACTIVE_CURVATURE_RATE 1024U static bool ford_lka_steering = false; +static bool ford_extended_lateral = false; +static bool ford_angle_mode = false; +static int16_t ford_shadow_curvature = 0; // Curvature rate limits #define FORD_LIMITS(limit_lateral_acceleration) { \ @@ -112,6 +115,59 @@ static bool ford_lka_steering = false; static const AngleSteeringLimits FORD_STEERING_LIMITS = FORD_LIMITS(false); +#define FORD_EXTENDED_LIMITS(limit_lateral_acceleration) { \ + .max_angle = 1000, \ + .angle_deg_to_can = 50000, \ + .max_angle_error = 100, \ + .angle_rate_up_lookup = { \ + {5., 16., 25.}, \ + {0.0025, 0.0014, 0.00018} \ + }, \ + .angle_rate_down_lookup = { \ + {5., 16., 25.}, \ + {0.0025, 0.0014, 0.00018} \ + }, \ + .angle_error_min_speed = 10.0, \ + .frequency = 20U, \ + .angle_is_curvature = (limit_lateral_acceleration), \ + .enforce_angle_error = true, \ + .inactive_angle_is_zero = true, \ +} + +static const AngleSteeringLimits FORD_EXTENDED_STEERING_LIMITS = FORD_EXTENDED_LIMITS(false); + +static int ford_desired_path_angle_last = 0; + +static bool ford_path_angle_checks(int desired_path_angle, bool steer_control_enabled) { + bool violation = false; + if (steer_control_enabled) { + float speed = ((float)vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0; + const struct lookup_t path_angle_rate = { + .x = {10., 15., 25.}, + .y = {0.0561, 0.04335, 0.00918}, + }; + int max_delta = (safety_interpolate(path_angle_rate, speed) * 2000.0) + 1.0; + violation |= safety_max_limit_check(desired_path_angle, + ford_desired_path_angle_last + max_delta, + ford_desired_path_angle_last - max_delta); + } else { + violation |= desired_path_angle != 0; + } + ford_desired_path_angle_last = violation ? 0 : desired_path_angle; + return violation; +} + +static bool ford_shadow_curvature_check(int desired_curvature, bool steer_control_enabled, + const AngleSteeringLimits limits) { + if (steer_control_enabled && limits.enforce_angle_error && + ((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.angle_error_min_speed)) { + int lowest_allowed = angle_meas.min - limits.max_angle_error - 1; + int highest_allowed = angle_meas.max + limits.max_angle_error + 1; + return safety_max_limit_check(desired_curvature, highest_allowed, lowest_allowed); + } + return false; +} + static void ford_rx_hook(const CANPacket_t *msg) { if (msg->bus == FORD_MAIN_BUS) { // Update in motion state from standstill signal @@ -235,6 +291,15 @@ static bool ford_tx_hook(const CANPacket_t *msg) { if (!valid_lka_action) { tx = false; } + + if (!ford_lka_steering) { + ford_angle_mode = (msg->data[4] & 0x1U) != 0U; + ford_extended_lateral = (msg->data[4] & 0x2U) != 0U; + ford_shadow_curvature = (int16_t)((msg->data[5] << 8) | msg->data[6]); + if (ford_angle_mode && !ford_extended_lateral) { + tx = false; + } + } } // Safety check for LateralMotionControl action @@ -246,12 +311,38 @@ static bool ford_tx_hook(const CANPacket_t *msg) { unsigned int raw_path_angle = (msg->data[3] << 3) | (msg->data[4] >> 5); unsigned int raw_path_offset = (msg->data[5] << 2) | (msg->data[6] >> 6); - // These signals are not yet tested with the current safety limits - bool violation = (raw_curvature_rate != FORD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET); - - // Check angle error and steer_control_enabled int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature - violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS); + int desired_curvature_rate = raw_curvature_rate - FORD_INACTIVE_CURVATURE_RATE; + int desired_path_angle = raw_path_angle - FORD_INACTIVE_PATH_ANGLE; + int desired_path_offset = raw_path_offset - FORD_INACTIVE_PATH_OFFSET; + + bool violation = false; + if (ford_extended_lateral) { + violation |= desired_path_offset != 0; + violation |= (desired_curvature_rate < -4096) || (desired_curvature_rate > 4095); + violation |= ford_path_angle_checks(desired_path_angle, steer_control_enabled); + if (ford_angle_mode) { + violation |= (desired_path_angle < -1000) || (desired_path_angle > 1047); + violation |= desired_curvature != 0; + violation |= steer_control_enabled && !(aol_allowed || controls_allowed); + int shadow_curvature_can = ROUND((float)ford_shadow_curvature * 0.05); + violation |= ford_shadow_curvature_check(shadow_curvature_can, steer_control_enabled, + FORD_EXTENDED_STEERING_LIMITS); + desired_angle_last = 0; + } else { + violation |= desired_path_angle != 0; + violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, + FORD_EXTENDED_STEERING_LIMITS); + } + if (!steer_control_enabled) { + violation |= (desired_curvature != 0) || (desired_curvature_rate != 0); + } + } else { + violation |= (raw_curvature_rate != FORD_INACTIVE_CURVATURE_RATE) || + (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || + (raw_path_offset != FORD_INACTIVE_PATH_OFFSET); + violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS); + } if (violation) { tx = false; @@ -261,6 +352,7 @@ static bool ford_tx_hook(const CANPacket_t *msg) { // Safety check for LateralMotionControl2 action if (msg->addr == FORD_LateralMotionControl2) { static const AngleSteeringLimits FORD_CANFD_STEERING_LIMITS = FORD_LIMITS(true); + static const AngleSteeringLimits FORD_CANFD_EXTENDED_STEERING_LIMITS = FORD_EXTENDED_LIMITS(true); // Signal: LatCtl_D2_Rq bool steer_control_enabled = ((msg->data[0] >> 4) & 0x7U) != 0U; @@ -269,12 +361,39 @@ static bool ford_tx_hook(const CANPacket_t *msg) { unsigned int raw_path_angle = ((msg->data[3] & 0x1FU) << 6) | (msg->data[4] >> 2); unsigned int raw_path_offset = ((msg->data[4] & 0x3U) << 8) | msg->data[5]; - // These signals are not yet tested with the current safety limits - bool violation = (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET); - - // Check angle error and steer_control_enabled int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature - violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_CANFD_STEERING_LIMITS); + int desired_curvature_rate = raw_curvature_rate - FORD_CANFD_INACTIVE_CURVATURE_RATE; + int desired_path_angle = raw_path_angle - FORD_INACTIVE_PATH_ANGLE; + int desired_path_offset = raw_path_offset - FORD_INACTIVE_PATH_OFFSET; + + bool violation = false; + if (ford_extended_lateral) { + violation |= desired_path_offset != 0; + violation |= (desired_curvature_rate < -1024) || (desired_curvature_rate > 1023); + violation |= ford_path_angle_checks(desired_path_angle, steer_control_enabled); + if (ford_angle_mode) { + violation |= (desired_path_angle < -1000) || (desired_path_angle > 1047); + violation |= desired_curvature != 0; + violation |= steer_control_enabled && !(aol_allowed || controls_allowed); + int shadow_curvature_can = ROUND((float)ford_shadow_curvature * 0.05); + violation |= ford_shadow_curvature_check(shadow_curvature_can, steer_control_enabled, + FORD_CANFD_EXTENDED_STEERING_LIMITS); + desired_angle_last = 0; + } else { + violation |= desired_path_angle != 0; + violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, + FORD_CANFD_EXTENDED_STEERING_LIMITS); + } + if (!steer_control_enabled) { + violation |= (desired_curvature != 0) || (desired_curvature_rate != 0); + } + } else { + violation |= (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE) || + (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || + (raw_path_offset != FORD_INACTIVE_PATH_OFFSET); + violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, + FORD_CANFD_STEERING_LIMITS); + } if (violation) { tx = false; @@ -328,6 +447,10 @@ static safety_config ford_init(uint16_t param) { const uint16_t FORD_PARAM_LKA_STEERING = 4; const bool ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD); ford_lka_steering = GET_FLAG(param, FORD_PARAM_LKA_STEERING); + ford_extended_lateral = false; + ford_angle_mode = false; + ford_shadow_curvature = 0; + ford_desired_path_angle_last = 0; bool ford_longitudinal = false; diff --git a/opendbc_repo/opendbc/safety/tests/test_ford.py b/opendbc_repo/opendbc/safety/tests/test_ford.py index 2bfe71b6a..6289b1759 100755 --- a/opendbc_repo/opendbc/safety/tests/test_ford.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford.py @@ -410,6 +410,38 @@ class TestFordCANFDStockSafety(TestFordSafetyBase): self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.CANFD) self.safety.init_tests() + def _extended_lka_msg(self, angle_mode=False, shadow_curvature=0.0): + msg = self._lkas_command_msg(0) + raw_shadow = int(round(shadow_curvature / 1e-6)) & 0xFFFF + msg[0].data[4] |= 0x2 | int(angle_mode) + msg[0].data[5] = raw_shadow >> 8 + msg[0].data[6] = raw_shadow & 0xFF + return msg + + def test_extended_curvature_signals(self): + speed = 15.0 + self.safety.set_controls_allowed(True) + self._reset_curvature_measurement(0.0, speed) + self.assertTrue(self._tx(self._extended_lka_msg())) + self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, 0.001, 0.0005))) + + self.assertTrue(self._tx(self._extended_lka_msg())) + self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.1, 0.0, 0.001, 0.0005))) + + def test_extended_angle_signals(self): + speed = 15.0 + curvature = 0.005 + self.safety.set_controls_allowed(True) + self._reset_curvature_measurement(curvature, speed) + self.assertTrue(self._tx(self._extended_lka_msg(angle_mode=True, shadow_curvature=curvature))) + self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.02, 0.0, 0.0))) + + self.assertTrue(self._tx(self._extended_lka_msg(angle_mode=True, shadow_curvature=curvature))) + self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.2, 0.0, 0.0))) + + self.assertTrue(self._tx(self._extended_lka_msg(angle_mode=True, shadow_curvature=-curvature))) + self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.01, 0.0, 0.0))) + class TestFordLongitudinalSafetyBase(TestFordSafetyBase): MAX_ACCEL = 2.0 # accel is used for brakes, but openpilot can set positive values diff --git a/selfdrive/ui/layouts/settings/starpilot/lateral.py b/selfdrive/ui/layouts/settings/starpilot/lateral.py index 621bd39a6..c3409c9e1 100644 --- a/selfdrive/ui/layouts/settings/starpilot/lateral.py +++ b/selfdrive/ui/layouts/settings/starpilot/lateral.py @@ -5,6 +5,7 @@ from openpilot.selfdrive.ui.lib.starpilot_state import starpilot_state from openpilot.system.ui.lib.application import gui_app, FontWeight from openpilot.system.ui.lib.multilang import tr, tr_noop from openpilot.system.ui.widgets import DialogResult +from openpilot.system.ui.widgets.option_dialog import MultiOptionDialog from openpilot.selfdrive.ui.layouts.settings.starpilot.panel import _SettingsPage from openpilot.selfdrive.ui.layouts.settings.starpilot.aethergrid import ( @@ -52,7 +53,7 @@ class SteeringManagerView(CardHubManagerView): super().__init__(controller, [], **kwargs) def _build_cards(self): - return [ + cards = [ { "title": tr("Steering Behavior"), "desc": tr("Configure Always On Lateral (AOL), pause speed thresholds, and turn signal behaviors."), @@ -72,6 +73,14 @@ class SteeringManagerView(CardHubManagerView): "on_click": lambda: self._controller._navigate_to("advanced"), }, ] + if starpilot_state.car_state.isFord: + cards.append({ + "title": tr("Ford Lateral Tuning"), + "desc": tr("Select the Ford steering strategy and tune prediction, lane-change, and speed response."), + "icon": "steering", + "on_click": lambda: self._controller._navigate_to("ford"), + }) + return cards # ═══════════════════════════════════════════════════════════════ @@ -319,6 +328,88 @@ class StarPilotLateralLayout(_SettingsPage): ), ] + # ── 4. Ford Lateral Tuning ── + def ford_curvature_mode(): + return p.get_int("FordLateralMode") == 1 + + def ford_angle_mode(): + return p.get_int("FordLateralMode") == 2 + + def ford_enhanced_mode(): + return p.get_int("FordLateralMode") != 0 + + self._ford_rows = [ + SettingRow( + "FordLateralMode", "value", tr_noop("Steering Strategy"), + subtitle=tr_noop("Native keeps the existing Ford controls. Curvature and Angle enable the enhanced Ford strategies."), + get_value=self._get_ford_lateral_mode, + on_click=self._show_ford_lateral_mode, + ), + SettingRow( + "FordHumanTurnDetection", "toggle", tr_noop("Manual Turn Release"), + subtitle=tr_noop("Release lateral control during a sustained hands-on turn, then ramp back in smoothly."), + get_state=lambda: p.get_bool("FordHumanTurnDetection"), + set_state=lambda s: p.put_bool("FordHumanTurnDetection", s), + visible=ford_enhanced_mode, + ), + SettingRow( + "FordCurvatureBlendLow", "value", tr_noop("Small-Curve Prediction"), + subtitle=tr_noop("Blend model-predicted curvature into gentle turns."), + get_value=lambda: f"{p.get_float('FordCurvatureBlendLow') * 100:.0f}%", + on_click=lambda: self._show_slider("FordCurvatureBlendLow", 0.0, 1.0, step=0.05, unit="", value_type="float"), + visible=ford_curvature_mode, + ), + SettingRow( + "FordCurvatureBlendHigh", "value", tr_noop("Large-Curve Prediction"), + subtitle=tr_noop("Blend model-predicted curvature into tighter turns."), + get_value=lambda: f"{p.get_float('FordCurvatureBlendHigh') * 100:.0f}%", + on_click=lambda: self._show_slider("FordCurvatureBlendHigh", 0.0, 1.0, step=0.05, unit="", value_type="float"), + visible=ford_curvature_mode, + ), + SettingRow( + "FordCurvatureLaneChangeFactor", "value", tr_noop("Curvature Lane-Change Factor"), + subtitle=tr_noop("Scale steering during high-speed lane changes in Curvature mode."), + get_value=lambda: f"{p.get_float('FordCurvatureLaneChangeFactor'):.2f}x", + on_click=lambda: self._show_slider("FordCurvatureLaneChangeFactor", 0.5, 1.25, step=0.05, unit="x", value_type="float"), + visible=ford_curvature_mode, + ), + SettingRow( + "FordAngleBlend", "value", tr_noop("Angle Prediction Blend"), + subtitle=tr_noop("Blend model prediction into the path-angle command."), + get_value=lambda: f"{p.get_float('FordAngleBlend') * 100:.0f}%", + on_click=lambda: self._show_slider("FordAngleBlend", 0.0, 1.0, step=0.05, unit="", value_type="float"), + visible=ford_angle_mode, + ), + SettingRow( + "FordAngleLowSpeedFactor", "value", tr_noop("Low-Speed Angle Response"), + subtitle=tr_noop("Adjust path-angle strength at lower speeds and higher curvature."), + get_value=lambda: f"{p.get_float('FordAngleLowSpeedFactor'):.2f}x", + on_click=lambda: self._show_slider("FordAngleLowSpeedFactor", 0.5, 1.5, step=0.05, unit="x", value_type="float"), + visible=ford_angle_mode, + ), + SettingRow( + "FordAngleHighSpeedFactor", "value", tr_noop("High-Speed Angle Response"), + subtitle=tr_noop("Adjust path-angle strength through larger highway curves."), + get_value=lambda: f"{p.get_float('FordAngleHighSpeedFactor'):.2f}x", + on_click=lambda: self._show_slider("FordAngleHighSpeedFactor", 0.5, 1.5, step=0.05, unit="x", value_type="float"), + visible=ford_angle_mode, + ), + SettingRow( + "FordAngleHighSpeedDamping", "value", tr_noop("High-Speed Damping"), + subtitle=tr_noop("Dampen small steering corrections at highway speed."), + get_value=lambda: f"{p.get_float('FordAngleHighSpeedDamping'):.2f}x", + on_click=lambda: self._show_slider("FordAngleHighSpeedDamping", 0.25, 1.25, step=0.05, unit="x", value_type="float"), + visible=ford_angle_mode, + ), + SettingRow( + "FordAngleLaneChangeFactor", "value", tr_noop("Angle Lane-Change Factor"), + subtitle=tr_noop("Scale steering during high-speed lane changes in Angle mode."), + get_value=lambda: f"{p.get_float('FordAngleLaneChangeFactor'):.2f}x", + on_click=lambda: self._show_slider("FordAngleLaneChangeFactor", 0.5, 1.5, step=0.05, unit="x", value_type="float"), + visible=ford_angle_mode, + ), + ] + self._manager_view = SteeringManagerView( self, header_title=tr_noop("Steering"), @@ -362,6 +453,13 @@ class StarPilotLateralLayout(_SettingsPage): parent_toggle=pt_advanced, panel_style=PANEL_STYLE, ) + self._sub_panels["ford"] = AetherSettingsView( + self, + [SettingSection(title="", rows=self._ford_rows)], + header_title=tr_noop("Ford Lateral Tuning"), + header_subtitle=tr_noop("Tune Ford-specific polynomial steering while retaining the native strategy as a fallback."), + panel_style=PANEL_STYLE, + ) self._wire_sub_panels() def _on_pause_lateral_speed_clicked(self): @@ -402,3 +500,17 @@ class StarPilotLateralLayout(_SettingsPage): current = self._params.get_int("LaneChangeSmoothing") if self._params.get_int("LaneChangeSmoothing") > 0 else 5 gui_app.push_widget(AetherSliderDialog(tr("Lane Change Smoothing"), 1, 10, 1, current, on_close, color=self.SLIDER_COLOR)) + + def _get_ford_lateral_mode(self) -> str: + return tr(("Native", "Curvature", "Angle")[max(0, min(2, self._params.get_int("FordLateralMode")))]) + + def _show_ford_lateral_mode(self): + options = [tr("Native"), tr("Curvature"), tr("Angle")] + current = options[max(0, min(2, self._params.get_int("FordLateralMode")))] + + def on_select(res): + if res == DialogResult.CONFIRM and dialog.selection in options: + self._params.put_int("FordLateralMode", options.index(dialog.selection)) + + dialog = MultiOptionDialog(tr("Ford Steering Strategy"), options, current, callback=on_select) + gui_app.push_widget(dialog) diff --git a/selfdrive/ui/lib/starpilot_state.py b/selfdrive/ui/lib/starpilot_state.py index 7c1d4837f..ba15bd506 100644 --- a/selfdrive/ui/lib/starpilot_state.py +++ b/selfdrive/ui/lib/starpilot_state.py @@ -15,6 +15,7 @@ from openpilot.starpilot.common.lateral_delay import full_lateral_delay class StarPilotCarState: # ========== Car Type Detection ========== isGM: bool = False + isFord: bool = False isHKG: bool = False isJeep: bool = False isToyota: bool = False @@ -91,6 +92,7 @@ class StarPilotState: brand = FINGERPRINT_MAKE_TO_VALUES_DIR.get(fallback_make_lower, fallback_make_lower) fallback_model_str = fallback_model or "" self.car_state.isGM = brand == "gm" + self.car_state.isFord = brand == "ford" self.car_state.isHKG = brand == "hyundai" self.car_state.isJeep = brand == "chrysler" and fallback_model_str.startswith("JEEP_") self.car_state.isSubaru = brand == "subaru" @@ -164,6 +166,7 @@ class StarPilotState: self.car_state.isAngleCar = self._safe_get(CP, "steerControlType", None) == car.CarParams.SteerControlType.angle self.car_state.isBolt = car_fingerprint.startswith("CHEVROLET_BOLT") self.car_state.isGM = car_make == "gm" + self.car_state.isFord = car_make == "ford" self.car_state.isHKG = car_make == "hyundai" self.car_state.isHKGCanFd = self.car_state.isHKG and safety_model == car.CarParams.SafetyModel.hyundaiCanfd self.car_state.isJeep = car_make == "chrysler" and car_fingerprint.startswith("JEEP_") diff --git a/starpilot/car/__init__.py b/starpilot/car/__init__.py new file mode 100644 index 000000000..ab4ddbf92 --- /dev/null +++ b/starpilot/car/__init__.py @@ -0,0 +1 @@ +"""Vehicle-specific StarPilot extensions.""" diff --git a/starpilot/car/ford/__init__.py b/starpilot/car/ford/__init__.py new file mode 100644 index 000000000..72d1e4b5c --- /dev/null +++ b/starpilot/car/ford/__init__.py @@ -0,0 +1 @@ +"""Ford-specific control extensions.""" diff --git a/starpilot/car/ford/fordcan.py b/starpilot/car/ford/fordcan.py new file mode 100644 index 000000000..c9295d4a7 --- /dev/null +++ b/starpilot/car/ford/fordcan.py @@ -0,0 +1,53 @@ +from opendbc.car.ford.fordcan import CanBus, calculate_lat_ctl2_checksum + + +SHADOW_CURVATURE_SCALE = 1e-6 + + +def create_lka_msg(packer, CAN: CanBus, angle_mode: bool = False, shadow_curvature: float = 0.0, + extended_mode: bool = True): + addr, dat, bus = packer.make_can_msg("Lane_Assist_Data1", CAN.main, {}) + dat = bytearray(dat) + + shadow_raw = int(round(shadow_curvature / SHADOW_CURVATURE_SCALE)) + shadow_raw = max(-32768, min(32767, shadow_raw)) & 0xFFFF + dat[4] |= int(angle_mode) | (int(extended_mode) << 1) + dat[5] = shadow_raw >> 8 + dat[6] = shadow_raw & 0xFF + return addr, bytes(dat), bus + + +def create_lat_ctl_msg(packer, CAN: CanBus, active: bool, ramp_type: int, precision_type: int, + path_offset: float, path_angle: float, curvature: float, curvature_rate: float): + values = { + "LatCtlRng_L_Max": 0, + "HandsOffCnfm_B_Rq": 0, + "LatCtl_D_Rq": 1 if active else 0, + "LatCtlRampType_D_Rq": ramp_type, + "LatCtlPrecision_D_Rq": precision_type, + "LatCtlPathOffst_L_Actl": path_offset, + "LatCtlPath_An_Actl": path_angle, + "LatCtlCurv_NoRate_Actl": curvature_rate, + "LatCtlCurv_No_Actl": curvature, + } + return packer.make_can_msg("LateralMotionControl", CAN.main, values) + + +def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, ramp_type: int, precision_type: int, + path_offset: float, path_angle: float, curvature: float, + curvature_rate: float, counter: int): + values = { + "LatCtl_D2_Rq": mode, + "LatCtlRampType_D_Rq": ramp_type, + "LatCtlPrecision_D_Rq": precision_type, + "LatCtlPathOffst_L_Actl": path_offset, + "LatCtlPath_An_Actl": path_angle, + "LatCtlCurv_No_Actl": curvature, + "LatCtlCrv_NoRate2_Actl": curvature_rate, + "HandsOffCnfm_B_Rq": 0, + "LatCtlPath_No_Cnt": counter, + "LatCtlPath_No_Cs": 0, + } + dat = packer.make_can_msg("LateralMotionControl2", 0, values)[1] + values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat) + return packer.make_can_msg("LateralMotionControl2", CAN.main, values) diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py new file mode 100644 index 000000000..711cf1b10 --- /dev/null +++ b/starpilot/car/ford/lateral.py @@ -0,0 +1,275 @@ +from collections import deque +from dataclasses import dataclass +from enum import IntEnum + +import numpy as np + +from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, DT_CTRL +from opendbc.car.ford.values import CAR, CarControllerParams, FordFlags +from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL, apply_std_steer_angle_limits +from openpilot.common.params import Params +from openpilot.selfdrive.modeld.constants import ModelConstants + + +class FordLateralMode(IntEnum): + native = 0 + curvature = 1 + angle = 2 + + +FORD_ANGLE_LIMITS = AngleSteeringLimits( + 0.02, + ([5, 16, 25], [0.0025, 0.0012, 0.00008]), + ([5, 16, 25], [0.0025, 0.0014, 0.00018]), +) + +MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - ACCELERATION_DUE_TO_GRAVITY * 0.06 +PATH_ANGLE_MIN = -0.5 +PATH_ANGLE_MAX = 0.5235 +STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL + +CANFD_BODY_ON_FRAME = frozenset({ + CAR.FORD_F_150_MK14, + CAR.FORD_F_150_LIGHTNING_MK1, + CAR.FORD_EXPEDITION_MK4, + CAR.FORD_RANGER_MK2, +}) +CANFD_UNIBODY = frozenset({ + CAR.FORD_MUSTANG_MACH_E_MK1, + CAR.FORD_ESCAPE_MK4_5, +}) + + +@dataclass(frozen=True) +class FordLateralResult: + curvature: float = 0.0 + curvature_rate: float = 0.0 + path_offset: float = 0.0 + path_angle: float = 0.0 + ramp_type: int = 0 + precision_type: int = 1 + active: bool = False + shadow_curvature: float = 0.0 + + +class HumanTurnDetector: + ANGLE_DEG = 45.0 + HOLD_SECONDS = 1.5 + PRETURNED_HOLD_SECONDS = 3.0 + + def __init__(self): + self.timer = 0.0 + self.active = False + self._pressed_last = False + self._press_started_preturned = False + + def update(self, enabled: bool, steering_pressed: bool, steering_angle_deg: float) -> bool: + if steering_pressed and not self._pressed_last: + self._press_started_preturned = abs(steering_angle_deg) > self.ANGLE_DEG + self._pressed_last = steering_pressed + + if enabled and steering_pressed and abs(steering_angle_deg) > self.ANGLE_DEG: + self.timer += STEER_DT + else: + self.timer = 0.0 + + hold_time = self.PRETURNED_HOLD_SECONDS if self._press_started_preturned else self.HOLD_SECONDS + self.active = self.timer + 1e-9 >= hold_time + return self.active + + def reset(self): + self.timer = 0.0 + self.active = False + self._pressed_last = False + self._press_started_preturned = False + + +class FordLateralController: + """Ford polynomial lateral strategies kept outside the native car implementation.""" + + def __init__(self, CP): + self.CP = CP + self.params = Params(return_defaults=True) + try: + import cereal.messaging as messaging + self.sm = messaging.SubMaster(["modelV2", "liveDelay"]) + except ImportError: + # The host interface tests don't load the device messaging extension. + self.sm = None + self.model = None + + self.mode = FordLateralMode.curvature + self.human_turn_enabled = True + self.curvature_blend_low = 0.4 + self.curvature_blend_high = 0.4 + self.angle_blend = 0.5 + self.curvature_lane_change_factor = 0.85 + self.angle_lane_change_factor = 1.0 + self.angle_low_speed_factor = 1.0 + self.angle_high_speed_factor = 1.0 + self.angle_high_speed_damping = 1.0 + + self.human_turn = HumanTurnDetector() + self.curvature_samples = deque(maxlen=max(2, round(0.3 / STEER_DT))) + self.path_angle_last = 0.0 + self.curvature_last = 0.0 + self._frame = 0 + self._update_params() + + def _update_params(self): + try: + self.mode = FordLateralMode(int(np.clip(self.params.get_int("FordLateralMode", return_default=True), 0, 2))) + except ValueError: + self.mode = FordLateralMode.native + + self.human_turn_enabled = self.params.get_bool("FordHumanTurnDetection") + self.curvature_blend_low = float(np.clip(self.params.get_float("FordCurvatureBlendLow", return_default=True), 0.0, 1.0)) + self.curvature_blend_high = float(np.clip(self.params.get_float("FordCurvatureBlendHigh", return_default=True), 0.0, 1.0)) + self.angle_blend = float(np.clip(self.params.get_float("FordAngleBlend", return_default=True), 0.0, 1.0)) + self.curvature_lane_change_factor = float(np.clip( + self.params.get_float("FordCurvatureLaneChangeFactor", return_default=True), 0.5, 1.25)) + self.angle_lane_change_factor = float(np.clip( + self.params.get_float("FordAngleLaneChangeFactor", return_default=True), 0.5, 1.5)) + self.angle_low_speed_factor = float(np.clip( + self.params.get_float("FordAngleLowSpeedFactor", return_default=True), 0.5, 1.5)) + self.angle_high_speed_factor = float(np.clip( + self.params.get_float("FordAngleHighSpeedFactor", return_default=True), 0.5, 1.5)) + self.angle_high_speed_damping = float(np.clip( + self.params.get_float("FordAngleHighSpeedDamping", return_default=True), 0.25, 1.25)) + + def update_inputs(self): + if self.sm is not None: + self.sm.update(0) + if self.sm.updated["modelV2"]: + self.model = self.sm["modelV2"] + if self._frame % 100 == 0: + self._update_params() + self._frame += 1 + + def _predicted_curvature(self, v_ego: float, lookup_time: float) -> float: + if self.model is None or len(self.model.orientationRate.z) < 17: + return 0.0 + curvatures = np.asarray(self.model.orientationRate.z) / max(v_ego, 0.01) + return float(np.interp(lookup_time, ModelConstants.T_IDXS, curvatures)) + + def _lane_change(self) -> tuple[bool, int]: + if self.model is None: + return False, 0 + state = int(self.model.meta.laneChangeState) + return state in (1, 2, 3), int(self.model.meta.laneChangeDirection) + + @staticmethod + def _current_curvature(CS) -> float: + return -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) + + def _blend_and_scale(self, desired: float, predicted: float, v_ego: float, angle_mode: bool) -> tuple[float, int]: + if angle_mode: + blend = self.angle_blend + high_factor = self.angle_lane_change_factor + else: + blend = float(np.interp(abs(desired), [0.0, 0.001], [self.curvature_blend_low, self.curvature_blend_high])) + high_factor = self.curvature_lane_change_factor + + requested = predicted * blend + desired * (1.0 - blend) + lane_change, direction = self._lane_change() + precision = 1 + if lane_change: + factor = float(np.interp(v_ego, [4.4, 40.23], [0.95, high_factor])) + if (direction == 1 and requested < 0.0) or (direction == 2 and requested > 0.0): + requested *= factor + precision = 0 + return requested, precision + + def _manual_turn(self, CC, CS) -> bool: + if not CC.latActive: + self.human_turn.reset() + return False + return self.human_turn.update( + self.human_turn_enabled, CS.out.steeringPressed, CS.out.steeringAngleDeg) + + def update_curvature(self, CC, CS, actuators) -> FordLateralResult: + if not CC.latActive or self._manual_turn(CC, CS) or CS.out.vEgoRaw < 0.1: + self.curvature_samples.clear() + self.curvature_last = 0.0 + return FordLateralResult(shadow_curvature=self._current_curvature(CS)) + + v_ego = float(CS.out.vEgoRaw) + predicted = self._predicted_curvature(v_ego, 0.2) + requested, precision = self._blend_and_scale(float(actuators.curvature), predicted, v_ego, False) + current = self._current_curvature(CS) + + if v_ego > 9.0: + requested = float(np.clip(requested, current - CarControllerParams.CURVATURE_ERROR, + current + CarControllerParams.CURVATURE_ERROR)) + applied = float(apply_std_steer_angle_limits( + requested, self.curvature_last, v_ego, CS.out.steeringAngleDeg, True, FORD_ANGLE_LIMITS)) + if self.CP.flags & FordFlags.CANFD: + max_curvature = MAX_LATERAL_ACCEL / max(v_ego, 1.0) ** 2 + applied = float(np.clip(applied, -max_curvature, max_curvature)) + + self.curvature_samples.append(predicted) + curvature_rate = 0.0 + if len(self.curvature_samples) > 1: + sample_time = (len(self.curvature_samples) - 1) * STEER_DT + curvature_rate = (self.curvature_samples[-1] - self.curvature_samples[0]) / max(sample_time * v_ego, 0.01) + curvature_rate *= float(np.interp(abs(predicted), [0.0, 0.008, 0.01], [0.0, 0.0, 1.0])) + curvature_rate *= float(np.interp(v_ego, [0.0, 14.5, 15.5], [1.0, 1.0, 0.0])) + if self._lane_change()[0]: + curvature_rate = 0.0 + + self.curvature_last = float(np.clip(applied, -0.02, 0.02)) + curvature_rate = float(np.clip(curvature_rate, -0.001024, 0.001023)) + return FordLateralResult( + curvature=self.curvature_last, + curvature_rate=curvature_rate, + ramp_type=2, + precision_type=precision, + active=True, + ) + + def _platform_angle_gains(self) -> tuple[float, float]: + if self.CP.carFingerprint in CANFD_BODY_ON_FRAME: + return 0.95, 0.95 + if self.CP.carFingerprint in CANFD_UNIBODY: + return 1.0, 1.05 + return 1.0, 1.15 + + def update_angle(self, CC, CS, actuators) -> FordLateralResult: + current = self._current_curvature(CS) + if not CC.latActive or self._manual_turn(CC, CS): + self.path_angle_last = 0.0 + return FordLateralResult(shadow_curvature=current) + + v_ego = float(CS.out.vEgoRaw) + live_delay = 0.12 if self.sm is None else float(np.clip(self.sm["liveDelay"].lateralDelay, 0.1, 0.15)) + speed_factor = float(np.interp(v_ego, [11.176, 24.587], [1.0, 0.0])) + curvature_factor = float(np.interp(abs(actuators.curvature), [0.005, 0.02], [1.0, 0.0])) + lookup_time = live_delay + 0.05 + 0.10 * speed_factor * curvature_factor + predicted = self._predicted_curvature(v_ego, lookup_time) + requested, precision = self._blend_and_scale(float(actuators.curvature), predicted, v_ego, True) + + if v_ego > 9.0: + requested = float(np.clip(requested, current - CarControllerParams.CURVATURE_ERROR, + current + CarControllerParams.CURVATURE_ERROR)) + + low_gain_high_speed, high_gain_high_speed = self._platform_angle_gains() + low_gain = float(np.interp(v_ego, [13.5, 26.82], + [1.0, low_gain_high_speed * self.angle_high_speed_damping])) + high_gain = float(np.interp(v_ego, [13.5, 26.82], + [1.30 * self.angle_low_speed_factor, + high_gain_high_speed * self.angle_high_speed_factor])) + gain = float(np.interp(abs(requested), [0.0007, 0.001], [low_gain, high_gain])) + path_angle = float(np.clip(requested * v_ego * gain, PATH_ANGLE_MIN, PATH_ANGLE_MAX)) + + max_delta = float(np.interp(v_ego, [9.0, 10.0, 15.0, 25.0], [0.055, 0.055, 0.0425, 0.009])) + path_angle = float(np.clip(path_angle, self.path_angle_last - max_delta, self.path_angle_last + max_delta)) + self.path_angle_last = path_angle + + shadow = current if CS.out.steeringPressed else requested + return FordLateralResult( + path_angle=path_angle, + ramp_type=2, + precision_type=precision, + active=True, + shadow_curvature=shadow, + ) diff --git a/starpilot/car/ford/tests/__init__.py b/starpilot/car/ford/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py new file mode 100644 index 000000000..88b83a376 --- /dev/null +++ b/starpilot/car/ford/tests/test_lateral.py @@ -0,0 +1,69 @@ +import sys +from types import SimpleNamespace + +import pytest + +from ..lateral import FordLateralController, HumanTurnDetector + + +class FakeSubMaster(dict): + def __init__(self, services): + super().__init__({"liveDelay": SimpleNamespace(lateralDelay=0.12)}) + self.updated = dict.fromkeys(services, False) + + def update(self, timeout): + pass + + +@pytest.fixture +def controller(monkeypatch): + messaging = SimpleNamespace(SubMaster=FakeSubMaster) + monkeypatch.setitem(sys.modules, "cereal.messaging", messaging) + CP = SimpleNamespace(flags=0, carFingerprint="FORD_EDGE_MK2") + return FordLateralController(CP) + + +def car_state(speed=15.0, curvature=0.0, steering_pressed=False, steering_angle=0.0): + return SimpleNamespace(out=SimpleNamespace( + vEgoRaw=speed, + yawRate=-curvature * speed, + steeringPressed=steering_pressed, + steeringAngleDeg=steering_angle, + )) + + +def test_human_turn_requires_sustained_input(): + detector = HumanTurnDetector() + assert not detector.update(True, True, 0.0) + for _ in range(29): + assert not detector.update(True, True, 50.0) + assert detector.update(True, True, 50.0) + assert not detector.update(True, False, 50.0) + + +def test_curvature_strategy_uses_polynomial_signals(controller): + result = controller.update_curvature( + SimpleNamespace(latActive=True), car_state(), SimpleNamespace(curvature=0.001)) + assert result.active + assert 0.0 < result.curvature <= 0.001 + assert result.ramp_type == 2 + + +def test_angle_strategy_uses_path_angle_and_shadow(controller): + result = controller.update_angle( + SimpleNamespace(latActive=True), car_state(curvature=0.001), SimpleNamespace(curvature=0.001)) + assert result.active + assert result.curvature == 0.0 + assert result.path_angle > 0.0 + assert result.shadow_curvature == pytest.approx(0.001) + + +def test_manual_turn_releases_lateral(controller): + controller.human_turn_enabled = True + CC = SimpleNamespace(latActive=True) + CS = car_state(steering_pressed=True, steering_angle=50.0) + actuators = SimpleNamespace(curvature=0.001) + for _ in range(61): + result = controller.update_angle(CC, CS, actuators) + assert not result.active + assert result.path_angle == 0.0