From 42d4a5b207e203fe5753beab0413ea7787f99321 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 30 Sep 2026 17:16:41 -0500 Subject: [PATCH] Jalisco's --- docs/CARS.md | 2 +- .../opendbc/car/hyundai/fingerprints.py | 1 + .../opendbc/car/hyundai/tests/test_hyundai.py | 18 +++ opendbc_repo/opendbc/car/hyundai/values.py | 2 +- .../opendbc/car/toyota/carcontroller.py | 9 +- opendbc_repo/opendbc/car/toyota/interface.py | 7 +- selfdrive/controls/controlsd.py | 23 ++- selfdrive/controls/lib/lane_centering.py | 14 +- .../controls/lib/latcontrol_vehicle_tunes.py | 50 ++++++- .../lib/longitudinal_mpc_lib/long_mpc.py | 13 +- .../controls/lib/longitudinal_planner.py | 3 +- .../lib/longitudinal_vehicle_tunes.py | 7 + .../tests/test_gv70_highway_stabilizer.py | 80 +++++++++++ .../controls/tests/test_lane_centering.py | 51 ++++++- starpilot/car/ford/lateral.py | 34 +++-- starpilot/car/ford/tests/test_lateral.py | 131 +++++++++++++++++- .../tests/test_fingerprint_catalog.py | 7 + 17 files changed, 407 insertions(+), 45 deletions(-) diff --git a/docs/CARS.md b/docs/CARS.md index 86ab8289e3..55801eaa27 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -264,7 +264,7 @@ A supported vehicle is one that just works when you install a comma device. All |Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|6 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Hyundai G connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| |Kia|Forte 2022-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai E connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| |Kia|K4 (with HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai R connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| -|Kia|K4 (without HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai A connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| +|Kia|K4 (without HDA II) 2025-26|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai A connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| |Kia|K5 2021-24|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai A connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| |Kia|K5 (without HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai M connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| |Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai A connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| diff --git a/opendbc_repo/opendbc/car/hyundai/fingerprints.py b/opendbc_repo/opendbc/car/hyundai/fingerprints.py index 773470f31b..261f0caaf9 100644 --- a/opendbc_repo/opendbc/car/hyundai/fingerprints.py +++ b/opendbc_repo/opendbc/car/hyundai/fingerprints.py @@ -1335,6 +1335,7 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00CL4 MFC AT CAN LHD 1.00 1.02 99210-GG000 240708', b'\xf1\x00CL4 MFC AT USA LHD 1.00 1.02 99210-GG000 240708', + b'\xf1\x00CL4 MFC AT USA LHD 1.00 1.04 99210-GG100 251205', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00CL4_ RDR ----- 1.00 1.01 99110-GG000 ', diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index d8046f5544..2cc658b90c 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -1796,6 +1796,24 @@ class TestHyundaiFingerprint: assert exact assert matches == {candidate} + @pytest.mark.parametrize("camera_fw", [ + b'\xf1\x00CL4 MFC AT CAN LHD 1.00 1.02 99210-GG000 240708', + b'\xf1\x00CL4 MFC AT USA LHD 1.00 1.02 99210-GG000 240708', + b'\xf1\x00CL4 MFC AT USA LHD 1.00 1.04 99210-GG100 251205', + ]) + @pytest.mark.parametrize("radar_fw", [ + b'\xf1\x00CL4_ RDR ----- 1.00 1.01 99110-GG000 ', + b'\xf1\x00CL4_ RDR ----- 1.00 1.01 99110-GG100 ', + ]) + def test_k4_2025_2026_fw_exact_matches(self, camera_fw, radar_fw): + car_fw = [ + CarParams.CarFw(ecu=Ecu.fwdCamera, fwVersion=camera_fw, address=0x7c4, brand="hyundai"), + CarParams.CarFw(ecu=Ecu.fwdRadar, fwVersion=radar_fw, address=0x7d0, brand="hyundai"), + ] + exact, matches = match_fw_to_car(car_fw, "", allow_exact=True, allow_fuzzy=False, log=False) + assert exact + assert matches == {CAR.KIA_K4_2025} + def test_staria_2023_australian_route_fw_exact_matches(self): route_fw = { (Ecu.fwdCamera, 0x7c4): b'\xf1\x00US4 MFC AT AUS RHD 1.00 1.04 99211-CG000 210819', diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index daceb4ddc1..fe4d7dcabb 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -605,7 +605,7 @@ class CAR(Platforms): ) KIA_K4_2025 = HyundaiCanFDPlatformConfig( [ - HyundaiCarDocs("Kia K4 (without HDA II) 2025", car_parts=CarParts.common([CarHarness.hyundai_a])), + HyundaiCarDocs("Kia K4 (without HDA II) 2025-26", car_parts=CarParts.common([CarHarness.hyundai_a])), HyundaiCarDocs("Kia K4 (with HDA II) 2025", car_parts=CarParts.common([CarHarness.hyundai_r])), ], CarSpecs(mass=2987 * CV.LB_TO_KG, wheelbase=2.72, steerRatio=13.4), diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 2d5b395470..3f5083093c 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -10,7 +10,7 @@ from opendbc.car.secoc import add_mac, build_sync_mac from opendbc.car.interfaces import CarControllerBase from opendbc.car.toyota import toyotacan from opendbc.car.toyota.values import CAR, MIN_ACC_SPEED, NO_STOP_TIMER_CAR, PEDAL_TRANSITION, TSS2_CAR, \ - CarControllerParams, ToyotaFlags, \ + CarControllerParams, ToyotaFlags, ToyotaSafetyFlags, \ UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, TOYOTA_AUTO_HOLD_AEB_CARS from opendbc.can import CANPacker @@ -68,6 +68,11 @@ def is_ths_hybrid(CP) -> bool: return CP.carFingerprint in LEGACY_PRIUS_CAR or is_camry_hybrid(CP) +def uses_rav4_hybrid_sdsu_longitudinal(CP) -> bool: + return bool(CP.carFingerprint == CAR.TOYOTA_RAV4H and CP.openpilotLongitudinalControl and + CP.safetyConfigs[0].safetyParam & ToyotaSafetyFlags.LONG_FILTER.value) + + def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool: highlander_sdsu = ( CP.carFingerprint == CAR.TOYOTA_HIGHLANDER and @@ -107,7 +112,7 @@ def get_long_tune(CP, params): kiV = [0.5, 0.25] k_f = 1.0 - if is_ths_hybrid(CP): + if is_ths_hybrid(CP) or uses_rav4_hybrid_sdsu_longitudinal(CP): k_f = 0.8 if CP.carFingerprint in LEGACY_PRIUS_CAR else 1.0 elif CP.carFingerprint not in TSS2_CAR: kiBP = [0., 5., 35.] diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 5c21640803..18134b7fa9 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -1,6 +1,6 @@ from opendbc.car import Bus, structs, get_safety_config, uds from opendbc.car.toyota.carstate import CarState -from opendbc.car.toyota.carcontroller import CarController +from opendbc.car.toyota.carcontroller import CarController, uses_rav4_hybrid_sdsu_longitudinal from opendbc.car.toyota.radar_interface import RadarInterface from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \ @@ -176,7 +176,8 @@ class CarInterface(CarInterfaceBase): # 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 or ret.enableGasInterceptorDEPRECATED) else MIN_ACC_SPEED + rav4_hybrid_sdsu_long_defaults = uses_rav4_hybrid_sdsu_longitudinal(ret) + ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptorDEPRECATED or rav4_hybrid_sdsu_long_defaults) else MIN_ACC_SPEED prius_long_defaults = candidate in LEGACY_PRIUS_CAR and ret.openpilotLongitudinalControl camry_hybrid_long_defaults = (candidate == CAR.TOYOTA_CAMRY and ret.openpilotLongitudinalControl and @@ -193,7 +194,7 @@ class CarInterface(CarInterfaceBase): if ret.flags & ToyotaFlags.HYBRID.value: ret.longitudinalActuatorDelay = 0.05 - if camry_hybrid_long_defaults: + if camry_hybrid_long_defaults or rav4_hybrid_sdsu_long_defaults: # The THS eCVT responds much faster than the legacy non-TSS2 ICE tune. ret.longitudinalActuatorDelay = 0.05 ret.vEgoStopping = 0.25 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 320eea60a9..07fda2315f 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -31,7 +31,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle from openpilot.selfdrive.controls.lib.steering_saturation import is_angle_steering_limited from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature -from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import GENESIS_GV70_CARS, GenesisGV70HighwayCommandStabilizer +from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import get_genesis_highway_command_stabilizer from openpilot.selfdrive.controls.lib.latcontrol_torque import ( BOLT_2018_2021_STEER_RATIO_TEST_SCALE, LatControlTorque, @@ -427,9 +427,8 @@ class Controls: self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL) elif self.CP.lateralTuning.which() == 'torque': self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL) - self.gv70_highway_stabilizer = (GenesisGV70HighwayCommandStabilizer() - if self.CP.carFingerprint in GENESIS_GV70_CARS and self.CP.lateralTuning.which() == 'torque' - else None) + self.genesis_highway_stabilizer = get_genesis_highway_command_stabilizer( + self.CP.carFingerprint, self.CP.lateralTuning.which() == 'torque') self.sm = self.sm.extend(['liveDelay', 'starpilotCarState', 'starpilotPlan']) @@ -751,14 +750,14 @@ class Controls: bool(CS.leftBlinker or CS.rightBlinker), bool(CS.steeringPressed)) - if self.gv70_highway_stabilizer is not None: - stabilize_gv70 = (CC.latActive and isinstance(self.LaC, LatControlTorque) and - not CS.steeringPressed and not CS.leftBlinker and not CS.rightBlinker and - not self.starpilot_toggles.lane_centering and - model_v2.meta.laneChangeState == LaneChangeState.off and - self.sm.all_checks(['modelV2'])) - new_desired_curvature = self.gv70_highway_stabilizer.update( - new_desired_curvature, CS.vEgo, stabilize_gv70, DT_CTRL) + if self.genesis_highway_stabilizer is not None: + stabilize_genesis = (CC.latActive and isinstance(self.LaC, LatControlTorque) and + not CS.steeringPressed and not CS.leftBlinker and not CS.rightBlinker and + not self.starpilot_toggles.lane_centering and + model_v2.meta.laneChangeState == LaneChangeState.off and + self.sm.all_checks(['modelV2'])) + new_desired_curvature = self.genesis_highway_stabilizer.update( + new_desired_curvature, CS.vEgo, stabilize_genesis, DT_CTRL) jerk_factor = 1.0 if self.starpilot_toggles.lane_change_pace < 10: diff --git a/selfdrive/controls/lib/lane_centering.py b/selfdrive/controls/lib/lane_centering.py index 5067c53007..45e70d2d76 100644 --- a/selfdrive/controls/lib/lane_centering.py +++ b/selfdrive/controls/lib/lane_centering.py @@ -23,6 +23,9 @@ _CENTER_ERROR_DEADBAND = 0.08 _E2E_MAX_PATH_STD = 0.35 _E2E_BREAK_IN_START = 0.15 _E2E_BREAK_IN_FULL = 0.50 +_E2E_MIN_LANE_AUTHORITY = 0.20 +_E2E_BOUNDARY_LANE_AUTHORITY = 0.50 +_E2E_BOUNDARY_MARGIN = 0.40 class LaneCenteringController: @@ -146,7 +149,16 @@ class LaneCenteringController: 0.0, 1.0, ) - error *= 1.0 - e2e_authority * float(break_in) + path_clearance = min(model_y - left, right - model_y) + boundary_weight = float(np.clip( + (_MIN_CENTER_TO_LINE + _E2E_BOUNDARY_MARGIN - path_clearance) / _E2E_BOUNDARY_MARGIN, + 0.0, + 1.0, + )) + lane_authority = _E2E_MIN_LANE_AUTHORITY + boundary_weight * ( + _E2E_BOUNDARY_LANE_AUTHORITY - _E2E_MIN_LANE_AUTHORITY + ) + error *= 1.0 - e2e_authority * float(break_in) * (1.0 - lane_authority) except (AttributeError, TypeError, ValueError): pass diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 38590535c1..c1e2f6f1dc 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -280,7 +280,9 @@ GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP = [0.75, 1.0] GENESIS_GV70_HIGHWAY_STABILIZER_BASELINE_RC = 0.85 GENESIS_GV70_HIGHWAY_STABILIZER_BLEND_RC = 0.35 GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_LAT = 0.06 -GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_WINDOW = 4.0 +GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_WINDOW = 6.0 +GENESIS_GV70_HIGHWAY_STABILIZER_RECOVERY_SECONDS = 4.0 +GENESIS_GV70_HIGHWAY_STABILIZER_DIRECTION_CHANGE_LAT = 0.35 GENESIS_GV70_HIGHWAY_STABILIZER_REDUCTION = 0.70 GENESIS_GV70_HIGHWAY_STABILIZER_MAX_DELTA = 0.20 @@ -362,6 +364,8 @@ GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC = 0.18 GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC = 0.10 GENESIS_G70_OUTPUT_SMOOTHING_RELEASE_RC = 0.03 GENESIS_G70_OUTPUT_SMOOTHING_OVERSHOOT = 0.08 +GENESIS_G70_HIGHWAY_STABILIZER_CENTER_LAT_BP = [1.0, 1.4] +GENESIS_G70_HIGHWAY_STABILIZER_CURVE_EXIT_LAT = 0.15 GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN = 0.45 GENESIS_G70_ANGLE_OUTPUT_TAPER_START = 70.0 GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH = 6.0 @@ -3305,8 +3309,10 @@ def get_genesis_gv70_stabilized_output(output_torque: float, prev_output_torque: return float(output_torque + speed_weight * (smoothed_output - output_torque)) -class GenesisGV70HighwayCommandStabilizer: - def __init__(self) -> None: +class GenesisHighwayCommandStabilizer: + def __init__(self, center_lat_bp: list[float], curve_exit_lat: float = 0.0) -> None: + self.center_lat_bp = tuple(center_lat_bp) + self.curve_exit_lat = curve_exit_lat self.reset() def reset(self) -> None: @@ -3315,6 +3321,8 @@ class GenesisGV70HighwayCommandStabilizer: self.reversals: deque[float] = deque() self.elapsed = 0.0 self.blend = 0.0 + self.active_until = 0.0 + self.curve_direction = 0 def update(self, curvature: float, v_ego: float, enabled: bool, dt: float) -> float: if not enabled or v_ego <= GENESIS_GV70_HIGHWAY_STABILIZER_SPEED_BP[0] or not math.isfinite(curvature): @@ -3325,13 +3333,20 @@ class GenesisGV70HighwayCommandStabilizer: lateral_accel = curvature * v_ego ** 2 if self.baseline is None: self.baseline = lateral_accel + if abs(self.baseline) >= GENESIS_GV70_HIGHWAY_STABILIZER_DIRECTION_CHANGE_LAT: + self.curve_direction = 1 if self.baseline > 0.0 else -1 + if self.curve_direction * lateral_accel < 0.0: + self.reset() + self.baseline = lateral_accel + return curvature self.baseline += dt / (GENESIS_GV70_HIGHWAY_STABILIZER_BASELINE_RC + dt) * (lateral_accel - self.baseline) residual = lateral_accel - self.baseline - if abs(lateral_accel) >= GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP[1]: + if abs(lateral_accel) >= self.center_lat_bp[1]: self.last_sign = 0 self.reversals.clear() self.blend = 0.0 + self.active_until = 0.0 return curvature sign = 0 @@ -3347,15 +3362,38 @@ class GenesisGV70HighwayCommandStabilizer: self.reversals.popleft() speed_weight = float(np.interp(v_ego, GENESIS_GV70_HIGHWAY_STABILIZER_SPEED_BP, [0.0, 1.0])) - target_blend = speed_weight if len(self.reversals) >= 3 else 0.0 + if len(self.reversals) >= 3: + self.active_until = self.elapsed + GENESIS_GV70_HIGHWAY_STABILIZER_RECOVERY_SECONDS + target_blend = speed_weight if self.elapsed < self.active_until else 0.0 self.blend += dt / (GENESIS_GV70_HIGHWAY_STABILIZER_BLEND_RC + dt) * (target_blend - self.blend) - center_weight = float(np.interp(abs(lateral_accel), GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP, [1.0, 0.0])) + center_weight = float(np.interp(abs(lateral_accel), self.center_lat_bp, [1.0, 0.0])) + if self.curve_direction and self.curve_exit_lat > 0.0: + center_weight *= float(np.interp(abs(lateral_accel), [0.0, self.curve_exit_lat], [0.0, 1.0])) correction = float(np.clip(GENESIS_GV70_HIGHWAY_STABILIZER_REDUCTION * residual, -GENESIS_GV70_HIGHWAY_STABILIZER_MAX_DELTA, GENESIS_GV70_HIGHWAY_STABILIZER_MAX_DELTA)) return float((lateral_accel - self.blend * center_weight * correction) / v_ego ** 2) +class GenesisGV70HighwayCommandStabilizer(GenesisHighwayCommandStabilizer): + def __init__(self) -> None: + super().__init__(GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP) + + +class GenesisG70HighwayCommandStabilizer(GenesisHighwayCommandStabilizer): + def __init__(self) -> None: + super().__init__(GENESIS_G70_HIGHWAY_STABILIZER_CENTER_LAT_BP, GENESIS_G70_HIGHWAY_STABILIZER_CURVE_EXIT_LAT) + + +def get_genesis_highway_command_stabilizer(car_fingerprint: str, torque_control: bool) -> GenesisHighwayCommandStabilizer | None: + if torque_control: + if car_fingerprint in GENESIS_G70_CARS: + return GenesisG70HighwayCommandStabilizer() + if car_fingerprint in GENESIS_GV70_CARS: + return GenesisGV70HighwayCommandStabilizer() + return None + + def get_genesis_g70_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: base_threshold = get_standard_friction_threshold(v_ego) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 8db19f206f..bd5f1ea332 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -428,9 +428,10 @@ def gen_long_ocp(): class LongitudinalMpc: - def __init__(self, mode='acc', dt=DT_MDL): + def __init__(self, mode='acc', dt=DT_MDL, *, hold_stopped_lead_position=False): self.mode = mode self.dt = dt + self.hold_stopped_lead_position = hold_stopped_lead_position self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.source = SOURCES[2] # Initialize smoothing filters with default time constants @@ -601,7 +602,7 @@ class LongitudinalMpc: self.solver.set(i, 'x', self.x0) @staticmethod - def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego=0.0): + def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego=0.0, *, hold_stopped_lead_position=False): speed_mph = v_ego * CV.MS_TO_MPH bp = [0, 20, 35] exp_weight = np.interp(speed_mph, bp, [1.0, 1.0, 0.0]) # Full exp at <20, blend to constant at 35 @@ -617,7 +618,10 @@ class LongitudinalMpc: # Constant acceleration component v_lead_traj_const = np.clip(v_lead + a_lead * T_IDXS, 0.0, 1e8) - x_lead_traj_const = x_lead + v_lead * T_IDXS + 0.5 * a_lead * T_IDXS**2 + position_time = T_IDXS + if hold_stopped_lead_position and a_lead < 0.0: + position_time = np.minimum(T_IDXS, max(v_lead, 0.0) / -a_lead) + x_lead_traj_const = x_lead + v_lead * position_time + 0.5 * a_lead * position_time**2 # Blend based on weight v_lead_traj = exp_weight * v_lead_traj_exp + (1 - exp_weight) * v_lead_traj_const @@ -688,7 +692,8 @@ class LongitudinalMpc: self.duplicate_lead_x_filters[lead_index].initialized = False self.duplicate_lead_a_filters[lead_index].initialized = False self.duplicate_lead_v_filters[lead_index].initialized = False - lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego) + lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego, + hold_stopped_lead_position=self.hold_stopped_lead_position) return lead_xv @staticmethod diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 321c34ba4f..3c25460110 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -36,6 +36,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( is_toyota_rav4_tss2_post_departure_tune, get_toyota_rav4_tss2_early_lead_cap, get_toyota_corolla_braking_lead_cap, + use_stopped_lead_position, is_toyota_rav4_tss2_radar_follow_lead, get_toyota_sienna_post_departure_restop_cap, get_untracked_slow_lead_decel_scale, @@ -579,7 +580,7 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05): class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP - self.mpc = LongitudinalMpc(dt=dt) + self.mpc = LongitudinalMpc(dt=dt, hold_stopped_lead_position=use_stopped_lead_position(CP)) self.fcw = False self.dt = dt self.model_allow_throttle = True diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index 84299cb1a7..5e250a529b 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -171,6 +171,13 @@ def get_toyota_prius_stopped_lead_obstacle_bias(CP, lead, v_ego): return float(min(bias, max(distance - 0.5, 0.0))) +def use_stopped_lead_position(CP): + return ( + getattr(CP, "brand", "") == "toyota" and + str(getattr(CP, "carFingerprint", "")) == "TOYOTA_COROLLA_TSS2" + ) + + def get_toyota_corolla_braking_lead_cap(CP, lead, v_ego, desired_gap, accel_min): if ( getattr(CP, "brand", "") != "toyota" or diff --git a/selfdrive/controls/tests/test_gv70_highway_stabilizer.py b/selfdrive/controls/tests/test_gv70_highway_stabilizer.py index 79eadb18ae..a79c95cdaf 100644 --- a/selfdrive/controls/tests/test_gv70_highway_stabilizer.py +++ b/selfdrive/controls/tests/test_gv70_highway_stabilizer.py @@ -73,3 +73,83 @@ def test_strong_turn_and_driver_input_reset_stabilizer(): assert update_accel(stabilizer, -0.3, enabled=False) == pytest.approx(-0.3) assert update_accel(stabilizer, 0.3) == pytest.approx(0.3) + + +def test_slow_highway_oscillation_stays_damped_between_reversals(): + stabilizer = GenesisGV70HighwayCommandStabilizer() + raw, shaped, blends = [], [], [] + for i in range(3000): + accel = 0.50 + 0.20 * math.sin(2.0 * math.pi * 0.28 * i * 0.01) + raw.append(accel) + shaped.append(update_accel(stabilizer, accel)) + blends.append(stabilizer.blend) + + assert min(blends[1500:]) > 0.99 + assert np.std(shaped[1500:]) < 0.70 * np.std(raw[1500:]) + assert np.mean(shaped[1500:]) == pytest.approx(np.mean(raw[1500:]), abs=0.015) + + +def test_stabilizer_recovers_after_oscillation_ends(): + stabilizer = GenesisGV70HighwayCommandStabilizer() + for i in range(1600): + update_accel(stabilizer, 0.50 + 0.20 * math.sin(2.0 * math.pi * 0.4 * i * 0.01)) + assert stabilizer.blend > 0.9 + + for _ in range(1500): + shaped = update_accel(stabilizer, 0.50) + assert stabilizer.blend < 0.001 + assert shaped == pytest.approx(0.50, abs=1e-6) + + +@pytest.mark.parametrize('direction', [-1.0, 1.0]) +def test_real_curve_direction_change_bypasses_recovery(direction): + stabilizer = GenesisGV70HighwayCommandStabilizer() + for i in range(1600): + update_accel(stabilizer, direction * (0.55 + 0.20 * math.sin(2.0 * math.pi * 0.4 * i * 0.01))) + assert stabilizer.blend > 0.9 + + assert update_accel(stabilizer, -direction * 0.60) == pytest.approx(-direction * 0.60) + assert stabilizer.blend == 0.0 + assert not stabilizer.reversals + assert stabilizer.active_until == 0.0 + assert stabilizer.curve_direction == 0 + + +@pytest.mark.parametrize('direction', [-1.0, 1.0]) +def test_gradual_s_curve_does_not_delay_direction_change(direction): + stabilizer = GenesisGV70HighwayCommandStabilizer() + for i in range(1600): + update_accel(stabilizer, direction * (0.55 + 0.20 * math.sin(2.0 * math.pi * 0.4 * i * 0.01))) + assert stabilizer.blend > 0.9 + + raw = direction * np.linspace(0.55, -0.65, 500) + shaped = np.array([update_accel(stabilizer, float(accel)) for accel in raw]) + raw_crossing = np.flatnonzero(direction * raw < 0.0)[0] + shaped_crossing = np.flatnonzero(direction * shaped < 0.0)[0] + assert shaped_crossing == raw_crossing + assert shaped[raw_crossing:] == pytest.approx(raw[raw_crossing:]) + + +def test_inactive_reset_clears_recovery_and_reengages_cleanly(): + stabilizer = GenesisGV70HighwayCommandStabilizer() + for i in range(1600): + update_accel(stabilizer, 0.50 + 0.20 * math.sin(2.0 * math.pi * 0.4 * i * 0.01)) + assert stabilizer.active_until > stabilizer.elapsed + + assert update_accel(stabilizer, 0.60, enabled=False) == pytest.approx(0.60) + assert stabilizer.active_until == 0.0 + assert stabilizer.baseline is None + assert update_accel(stabilizer, -0.60) == pytest.approx(-0.60) + + +@pytest.mark.parametrize('speed', [40.1, 45.0, 50.0, 65.0]) +def test_recovery_respects_speed_gate_and_correction_bound(speed): + stabilizer = GenesisGV70HighwayCommandStabilizer() + speed *= CV.MPH_TO_MS + max_delta = 0.0 + for i in range(1600): + accel = 0.3 * math.sin(2.0 * math.pi * 0.4 * i * 0.01) + shaped = update_accel(stabilizer, accel, speed) + max_delta = max(max_delta, abs(shaped-accel)) + speed_weight = np.interp(speed, [40.0*CV.MPH_TO_MS,50.0*CV.MPH_TO_MS], [0.0,1.0]) + assert 0.0 < max_delta <= 0.20*speed_weight+1e-6 diff --git a/selfdrive/controls/tests/test_lane_centering.py b/selfdrive/controls/tests/test_lane_centering.py index f933c16a44..44091f873a 100644 --- a/selfdrive/controls/tests/test_lane_centering.py +++ b/selfdrive/controls/tests/test_lane_centering.py @@ -139,18 +139,59 @@ def test_offset_is_reduced_in_narrow_lane(): assert np.isclose(at_safe_limit, above_safe_limit) -def test_confident_e2e_path_can_fully_break_in(): - model = _model(left=-1.0, right=2.6, model_y=0.0, path_std=0.1) +@pytest.mark.parametrize("direction", [-1.0, 1.0]) +def test_confident_e2e_path_retains_bounded_lane_correction(direction): + model = _model(left=-2.4, right=2.4, model_y=direction * 0.6, path_std=0.1) _, lane_authority = _converge(model, authority=0.0) _, e2e_authority = _converge(model, authority=1.0) - assert lane_authority > 0.0 - assert abs(e2e_authority) < 1e-9 + assert lane_authority * direction < 0.0 + assert e2e_authority == pytest.approx(0.2 * lane_authority) + + +@pytest.mark.parametrize("direction", [-1.0, 1.0]) +def test_e2e_retains_more_lane_correction_near_boundary(direction): + model = _model(model_y=direction * 0.8, path_std=0.1) + _, lane_authority = _converge(model, authority=0.0) + _, e2e_authority = _converge(model, authority=1.0) + assert e2e_authority == pytest.approx(0.5 * lane_authority) + + +def test_e2e_boundary_authority_blends_continuously(): + fractions = [] + for clearance in np.linspace(1.55, 1.05, 101): + model = _model(left=-2.4, right=2.4, model_y=-2.4 + clearance) + lane_valid, lane_authority = LaneCenteringController._raw_correction(model, _V_EGO, 0.0, 0.0) + e2e_valid, e2e_authority = LaneCenteringController._raw_correction(model, _V_EGO, 0.0, 1.0) + assert lane_valid and e2e_valid + fractions.append(e2e_authority / lane_authority) + assert fractions[0] == pytest.approx(0.2) + assert fractions[-1] == pytest.approx(0.5) + assert np.all(np.diff(fractions) >= -1e-9) + assert np.max(np.diff(fractions)) < 0.004 + + +@pytest.mark.parametrize("line", [1, 2]) +def test_e2e_boundary_correction_requires_both_lane_lines(line): + model = _model(model_y=-0.8) + assert _update(LaneCenteringController(), model) > 0.0 + model.laneLineProbs[line] = 0.59 + assert _update(LaneCenteringController(), model) == 0.0 + + +@pytest.mark.parametrize("direction", [-1.0, 1.0]) +def test_e2e_boundary_correction_remains_capped_and_yields_to_driver(direction): + model = _model(model_y=direction * 2.0) + controller, output = _converge(model) + assert output * direction < 0.0 + assert abs(output) <= 0.004 * 0.30 + assert _update(controller, model, driver_override=True) == 0.0 def test_uncertain_e2e_path_does_not_break_in(): model = _model(left=-1.0, right=2.6, model_y=0.0, path_std=0.6) + _, lane_only = _converge(model, authority=0.0) _, output = _converge(model, authority=1.0) - assert output > 0.0 + assert output == pytest.approx(lane_only) def test_e2e_authority_blends_lane_correction(): diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index 2af1a29de8..fde56d4593 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -87,6 +87,9 @@ MACH_E_PATH_ANGLE_FADE_START_SPEED = 8.0 MACH_E_PATH_ANGLE_MAX_SPEED = 8.8 MACH_E_PATH_ANGLE_TRACKING_FACTOR = 0.75 MACH_E_PATH_ANGLE_DRIVER_COOLDOWN = 0.75 +MACH_E_DRIVER_ASSIST_MIN_SPEED = 2.0 +MACH_E_DRIVER_ASSIST_MAX_SPEED = 15.0 +MACH_E_DRIVER_ASSIST_MAX_TORQUE = 3.5 FORD_CURVATURE_LOOKAHEAD = { CAR.FORD_EXPLORER_MK6: 0.20, } @@ -223,6 +226,19 @@ class FordLateralController: def _current_curvature(CS) -> float: return -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) + def _driver_assisting_curve(self, CS, desired: float) -> bool: + v_ego = float(CS.out.vEgoRaw) + if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or + not CS.out.steeringPressed or not MACH_E_DRIVER_ASSIST_MIN_SPEED <= v_ego < MACH_E_DRIVER_ASSIST_MAX_SPEED or + self._lane_change()[0] or abs(desired) < MACH_E_TURN_IN_MIN_CURVATURE or + desired * CS.out.steeringTorque >= 0.0 or abs(CS.out.steeringTorque) > MACH_E_DRIVER_ASSIST_MAX_TORQUE): + return False + current = self._current_curvature(CS) + preview = self._predicted_curvature(v_ego, self._curvature_lookahead() + MACH_E_TURN_IN_LOOKAHEAD_EXTRA) + return bool(desired * current > 0.0 and desired * preview > 0.0 and + abs(preview) >= MACH_E_TURN_IN_FULL_CURVATURE and + np.sign(desired) * (current - desired) <= CarControllerParams.CURVATURE_ERROR) + def _curvature_error_limit(self, requested: float, desired: float, current: float, v_ego: float, steering_pressed: bool, lane_change: bool) -> float: base = CarControllerParams.CURVATURE_ERROR @@ -246,7 +262,7 @@ class FordLateralController: not steering_pressed and self.path_angle_driver_cooldown == 0.0 and not lane_change and 3.0 <= v_ego < MACH_E_PATH_ANGLE_MAX_SPEED and requested * desired > 0.0 and requested * applied > 0.0 and - abs(requested) > 0.0198 and abs(desired) > 0.016 and abs(applied) >= 0.0195 and + abs(requested) > 0.0198 and abs(desired) > MACH_E_TURN_IN_FULL_CURVATURE and abs(applied) >= 0.0195 and np.sign(desired) * (desired - current) > 0.002): max_curvature = MAX_LATERAL_ACCEL / v_ego ** 2 residual = max(0.0, min(max(abs(requested), abs(desired)), max_curvature) - abs(applied)) @@ -427,7 +443,7 @@ class FordLateralController: )) return speed_weight * curvature_weight * preview_weight * acceleration_weight - def _manual_turn(self, CC, CS, desired: float) -> bool: + def _manual_turn(self, CC, CS, desired: float, driver_assisting: bool = False) -> bool: if not CC.latActive: self.human_turn.reset() self.manual_turn_latched = False @@ -435,7 +451,7 @@ class FordLateralController: self.manual_turn_direction = 0.0 return False detected = self.human_turn.update( - self.human_turn_enabled, CS.out.steeringPressed, CS.out.steeringAngleDeg) + self.human_turn_enabled and not driver_assisting, CS.out.steeringPressed, CS.out.steeringAngleDeg) if self.CP.carFingerprint not in FORD_MANUAL_TURN_LATCH_CARS: return detected @@ -447,7 +463,7 @@ class FordLateralController: blinker_direction = float(CS.out.rightBlinker) - float(CS.out.leftBlinker) driver_turning_with_signal = ( - CS.out.steeringPressed and abs(CS.out.steeringAngleDeg) >= MANUAL_TURN_ENTRY_ANGLE_DEG and + CS.out.steeringPressed and not driver_assisting and abs(CS.out.steeringAngleDeg) >= MANUAL_TURN_ENTRY_ANGLE_DEG and blinker_direction != 0.0 and not self._lane_change()[0] and CS.out.steeringTorque * blinker_direction < 0.0 ) @@ -494,7 +510,10 @@ class FordLateralController: self.desired_curvature_last = 0.0 return FordLateralResult() - manual_turn = self._manual_turn(CC, CS, float(actuators.curvature)) + desired = float(actuators.curvature) + driver_assisting = self._driver_assisting_curve(CS, desired) + driver_override = bool(CS.out.steeringPressed) and not driver_assisting + manual_turn = self._manual_turn(CC, CS, desired, driver_assisting) if manual_turn or CS.out.vEgoRaw < 0.1: if CS.out.steeringPressed: self.path_angle_driver_cooldown = MACH_E_PATH_ANGLE_DRIVER_COOLDOWN @@ -508,7 +527,6 @@ class FordLateralController: v_ego = float(CS.out.vEgoRaw) lookahead = self._curvature_lookahead() predicted = self._predicted_curvature(v_ego, lookahead) - desired = float(actuators.curvature) allow_opposite_preview = False if self.CP.carFingerprint in FORD_CONSERVATIVE_PREVIEW_CARS: turn_in_predicted = self._predicted_curvature(v_ego, lookahead + MACH_E_TURN_IN_LOOKAHEAD_EXTRA) @@ -565,7 +583,7 @@ class FordLateralController: if v_ego > 9.0: error_limit = self._curvature_error_limit( - requested, desired, current, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0]) + requested, desired, current, v_ego, driver_override, self._lane_change()[0]) requested = float(np.clip(requested, current - error_limit, current + error_limit)) applied = float(apply_std_steer_angle_limits( requested, self.curvature_last, v_ego, CS.out.steeringAngleDeg, True, FORD_CURVATURE_LIMITS)) @@ -573,7 +591,7 @@ class FordLateralController: max_curvature = MAX_LATERAL_ACCEL / max(v_ego, 1.0) ** 2 applied = float(np.clip(applied, -max_curvature, max_curvature)) path_angle = self._path_angle_assist( - requested, desired, applied, current, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0]) + requested, desired, applied, current, v_ego, driver_override, self._lane_change()[0]) self.curvature_samples.append(predicted) curvature_rate = 0.0 diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index fa31ca9a55..bb9c85f9a4 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -144,7 +144,7 @@ def test_mach_e_path_angle_assist_starts_at_saturation_and_releases_after_driver @pytest.mark.parametrize("speed,requested,desired,applied,driver,lane_change", ( (9.0, 0.04, 0.04, 0.02, False, False), (7.5, 0.0197, 0.04, 0.02, False, False), - (7.5, 0.04, 0.015, 0.02, False, False), + (7.5, 0.04, 0.007, 0.02, False, False), (7.5, 0.04, 0.04, 0.018, False, False), (7.5, 0.04, 0.04, 0.02, True, False), (7.5, 0.04, 0.04, 0.02, False, True), @@ -171,6 +171,135 @@ def test_mach_e_path_angle_assist_is_encoded_with_curvature(controller): assert encoded_angle == pytest.approx(-assist) +@pytest.mark.parametrize("sign", (-1, 1)) +@pytest.mark.parametrize("speed,expected", ((1.9, False), (2.0, True), (3.0, True), (7.0, True), + (11.0, True), (14.9, True), (15.0, False))) +def test_mach_e_driver_curve_assistance_scope(controller, monkeypatch, sign, speed, expected): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.02) + state = car_state(speed=speed, curvature=sign * 0.003, steering_pressed=True, + steering_torque=-sign * 2.0) + assert controller._driver_assisting_curve(state, sign * 0.004) is expected + + +@pytest.mark.parametrize("driver,torque,current,desired,preview,lane_change", ( + (False, -2.0, 0.008, 0.020, 0.025, False), + (True, 2.0, 0.008, 0.020, 0.025, False), + (True, -3.6, 0.008, 0.020, 0.025, False), + (True, -2.0, -0.008, 0.020, 0.025, False), + (True, -2.0, 0.023, 0.020, 0.025, False), + (True, -2.0, 0.001, 0.0019, 0.025, False), + (True, -2.0, 0.008, 0.020, -0.025, False), + (True, -2.0, 0.008, 0.020, 0.007, False), + (True, -2.0, 0.008, 0.020, 0.025, True), +)) +def test_mach_e_driver_curve_assistance_requires_path_agreement( + controller, monkeypatch, driver, torque, current, desired, preview, lane_change): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: preview) + monkeypatch.setattr(controller, "_lane_change", lambda: (lane_change, 0)) + state = car_state(speed=7.0, curvature=current, steering_pressed=driver, steering_torque=torque) + assert not controller._driver_assisting_curve(state, desired) + + +@pytest.mark.parametrize("fingerprint,flags", ((CAR.FORD_EDGE_MK2, FordFlags.CANFD), + (CAR.FORD_F_150_MK14, FordFlags.CANFD), + (CAR.FORD_MUSTANG_MACH_E_MK1, 0))) +def test_driver_curve_assistance_preserves_other_fords(controller, monkeypatch, fingerprint, flags): + controller.CP.carFingerprint = fingerprint + controller.CP.flags = flags + monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: 0.025) + state = car_state(speed=7.0, curvature=0.008, steering_pressed=True, steering_torque=-2.0) + assert not controller._driver_assisting_curve(state, 0.020) + + +@pytest.mark.parametrize("sign", (-1, 1)) +def test_mach_e_driver_assistance_handoff_and_takeover(controller, monkeypatch, sign): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + controller.curvature_last = sign * 0.020 + monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.030) + CC = SimpleNamespace(latActive=True) + actuators = SimpleNamespace(curvature=sign * 0.022) + helping = car_state(speed=7.0, curvature=sign * 0.008, steering_pressed=True, + steering_angle=-sign * 50.0, steering_torque=-sign * 2.0, + left_blinker=sign < 0, right_blinker=sign > 0) + for _ in range(round(3.5 / STEER_DT)): + result = controller.update(CC, helping, actuators) + assert result.active + assert result.curvature == pytest.approx(sign * 0.020) + assert sign * result.path_angle > 0.0 + assert not controller.manual_turn_latched + + helping.out.steeringPressed = False + helping.out.steeringTorque = 0.0 + result = controller.update(CC, helping, actuators) + assert result.active + assert sign * result.path_angle > 0.0 + assert controller.path_angle_driver_cooldown == 0.0 + + helping.out.steeringPressed = True + helping.out.steeringTorque = sign * 2.0 + result = controller.update(CC, helping, actuators) + assert result.path_angle == 0.0 + assert controller.path_angle_driver_cooldown > 0.0 + + helping.out.steeringTorque = -sign * 3.6 + result = controller.update(CC, helping, actuators) + assert not result.active + assert result.curvature == result.path_angle == 0.0 + assert controller.manual_turn_latched + helping.out.steeringTorque = -sign * 2.0 + assert not controller.update(CC, helping, actuators).active + + controller.update(SimpleNamespace(latActive=False), helping, actuators) + helping.out.steeringTorque = -sign * 2.0 + helping.out.yawRate = -sign * 0.025 * helping.out.vEgoRaw + result = controller.update(CC, helping, actuators) + assert not result.active + assert result.curvature == result.path_angle == 0.0 + assert controller.manual_turn_latched + + controller.update(SimpleNamespace(latActive=False), helping, actuators) + assert not controller.manual_turn_latched + assert controller.path_angle_driver_cooldown == 0.0 + + +@pytest.mark.parametrize("sign", (-1, 1)) +def test_mach_e_driver_help_at_early_curve_entry(controller, monkeypatch, sign): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.030) + state = car_state(speed=2.7, curvature=sign * 0.003, steering_pressed=True, + steering_torque=-sign * 2.0, steering_angle=-sign * 15.0, + left_blinker=sign < 0, right_blinker=sign > 0) + CC = SimpleNamespace(latActive=True) + assert controller.update(CC, state, SimpleNamespace(curvature=sign * 0.004)).active + state.out.vEgoRaw = 3.5 + state.out.yawRate = -sign * 0.004 * state.out.vEgoRaw + for _ in range(8): + result = controller.update(CC, state, SimpleNamespace(curvature=sign * 0.022)) + assert result.active + assert result.curvature == pytest.approx(sign * 0.020) + assert sign * result.path_angle > 0.0 + + +@pytest.mark.parametrize("sign", (-1, 1)) +def test_mach_e_driver_help_preserves_curvature_error_authority(controller, monkeypatch, sign): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + controller.curvature_last = sign * 0.010 + monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.018) + state = car_state(speed=11.0, curvature=sign * 0.006, steering_pressed=True, + steering_torque=-sign * 2.0, steering_angle=-sign * 20.0) + result = controller.update(SimpleNamespace(latActive=True), state, SimpleNamespace(curvature=sign * 0.012)) + assert result.active + assert sign * result.curvature > 0.010 + assert result.path_angle == 0.0 + + @pytest.mark.parametrize("sign", (-1, 1)) def test_mach_e_unwind_anticipates_opening_curve_before_current_request_is_met(controller, monkeypatch, sign): controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 diff --git a/starpilot/system/the_galaxy/tests/test_fingerprint_catalog.py b/starpilot/system/the_galaxy/tests/test_fingerprint_catalog.py index 5439c91454..8fbb718cdb 100644 --- a/starpilot/system/the_galaxy/tests/test_fingerprint_catalog.py +++ b/starpilot/system/the_galaxy/tests/test_fingerprint_catalog.py @@ -39,6 +39,13 @@ def test_galaxy_does_not_assign_a_regional_label_to_ambiguous_ev6_fingerprint(): assert catalog["model_to_label"]["KIA_EV6"] is None +def test_galaxy_lists_2026_k4_under_existing_non_hda2_platform(): + kia_models = the_galaxy._extract_fingerprint_models_for_make("kia") + assert {"value": "KIA_K4_2025", "label": "Kia K4 (without HDA II) 2025-26"} in kia_models + assert {"value": "KIA_K4_2025", "label": "Kia K4 (with HDA II) 2025"} in kia_models + assert {"value": "KIA_K4_2025", "label": "Kia K4 (with HDA II) 2025-26"} not in kia_models + + def test_manual_fingerprint_api_keeps_the_saved_value_and_label_consistent(monkeypatch): client, params = _params_client(monkeypatch, {}, "pc") monkeypatch.setattr(api_server, "_get_param_type_info", lambda: ({"CarModel"}, {"CarModel": str}))