mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-04 13:24:13 +08:00
Jalisco's
This commit is contained in:
firestar5683
+1
-1
@@ -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[<sup>1</sup>](#footnotes)|6 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Forte 2019-21">Buy Here</a></sub></details>|||
|
||||
|Kia|Forte 2022-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Forte 2022-23">Buy Here</a></sub></details>|||
|
||||
|Kia|K4 (with HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai R connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (with HDA II) 2025">Buy Here</a></sub></details>|||
|
||||
|Kia|K4 (without HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (without HDA II) 2025">Buy Here</a></sub></details>|||
|
||||
|Kia|K4 (without HDA II) 2025-26|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (without HDA II) 2025-26">Buy Here</a></sub></details>|||
|
||||
|Kia|K5 2021-24|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 2021-24">Buy Here</a></sub></details>|||
|
||||
|Kia|K5 (without HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai M connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 (without HDA II) 2025">Buy Here</a></sub></details>|||
|
||||
|Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 Hybrid 2020-22">Buy Here</a></sub></details>|||
|
||||
|
||||
@@ -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 ',
|
||||
|
||||
@@ -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',
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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.]
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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():
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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}))
|
||||
|
||||
Reference in New Issue
Block a user