mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-11 18:53:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Glycogen Supercompensation

Browse Source
This commit is contained in:
firestar5683
2026-09-10 17:12:08 -05:00
parent 340d225039
commit 14b15022eb
4 changed files with 117 additions and 201 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/subaru/carcontroller.py
+17 -146
View File
@@ -16,19 +16,8 @@ _SNG_ACC_MIN_DIST = 3
_SNG_ACC_MAX_DIST = 4.5
_LEGACY_2025_MADS_MIN_SPEED = 0.44704
_LEGACY_2025_MADS_MAX_STEER_ANGLE = 120.0
_LEGACY_2025_OVERRIDE_HOLD_FRAMES = 10
_LEGACY_2025_REENGAGE_SETTLE_FRAMES = 8
_LEGACY_2025_REENGAGE_MAX_STEER_RATE = 2.0
_LEGACY_2025_REENGAGE_MAX_ANGLE_DELTA = 1.0
_LEGACY_2025_RECLAIM_FRAMES = 36
_LEGACY_2025_RECLAIM_EXPONENT = 2.5
_ANGLE_OVERRIDE_CONFIRM_FRAMES = 2
_ANGLE_OVERRIDE_HOLD_FRAMES = 10
_ANGLE_REENGAGE_SETTLE_FRAMES = 8
_ANGLE_REENGAGE_MAX_STEER_RATE = 2.0
_ANGLE_REENGAGE_MAX_ANGLE_DELTA = 1.0
_ANGLE_RECLAIM_FRAMES = 36
_ANGLE_RECLAIM_EXPONENT = 2.5
_ANGLE_MADS_MIN_SPEED = 0.44704
_ANGLE_MADS_MAX_STEER_ANGLE = 120.0
_STOP_START_STARTUP_DELAY_FRAMES = 100
@@ -53,20 +42,8 @@ class CarController(CarControllerBase):
self.apply_steer_last = 0
self.driver_override = False
self.angle_override_confirm_frames = 0
self.legacy_2025_lkas_active = False
self.legacy_2025_handoff_active = False
self.legacy_2025_override_hold_frames = 0
self.legacy_2025_reengage_settle_frames = 0
self.legacy_2025_reengage_reference_angle = 0.0
self.legacy_2025_reclaim_frames = 0
self.legacy_2025_reclaim_start_angle = 0.0
self.angle_lkas_active = False
self.angle_handoff_active = False
self.angle_override_hold_frames = 0
self.angle_reengage_settle_frames = 0
self.angle_reengage_reference_angle = 0.0
self.angle_reclaim_frames = 0
self.angle_reclaim_start_angle = 0.0
self.cruise_button_prev = 0
self.steer_rate_counter = 0
@@ -146,81 +123,10 @@ class CarController(CarControllerBase):
self.stop_start_counter = (self.stop_start_counter + 1) % 0x10
return msg
def _reset_legacy_2025_handoff(self):
self.driver_override = False
self.angle_override_confirm_frames = 0
self.legacy_2025_handoff_active = False
self.legacy_2025_override_hold_frames = 0
self.legacy_2025_reengage_settle_frames = 0
self.legacy_2025_reengage_reference_angle = 0.0
self.legacy_2025_reclaim_frames = 0
self.legacy_2025_reclaim_start_angle = 0.0
def _legacy_2025_manual_handoff(self, CS, lkas_available):
if not lkas_available:
self._reset_legacy_2025_handoff()
return False
driver_override = self._update_angle_driver_override(CS)
if driver_override:
self.legacy_2025_handoff_active = True
self.legacy_2025_override_hold_frames = _LEGACY_2025_OVERRIDE_HOLD_FRAMES
self.legacy_2025_reengage_settle_frames = 0
self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
self.legacy_2025_reclaim_frames = 0
return True
if not self.legacy_2025_handoff_active and not self.legacy_2025_lkas_active and \
abs(getattr(CS.out, "steeringRateDeg", 0.0)) > _LEGACY_2025_REENGAGE_MAX_STEER_RATE:
self.legacy_2025_handoff_active = True
self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
if not self.legacy_2025_handoff_active:
return False
if self.legacy_2025_override_hold_frames > 0:
self.legacy_2025_override_hold_frames -= 1
if self.legacy_2025_override_hold_frames == 0:
self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
return True
wheel_stable = abs(getattr(CS.out, "steeringRateDeg", 0.0)) <= _LEGACY_2025_REENGAGE_MAX_STEER_RATE and \
abs(CS.out.steeringAngleDeg - self.legacy_2025_reengage_reference_angle) <= _LEGACY_2025_REENGAGE_MAX_ANGLE_DELTA
if wheel_stable:
self.legacy_2025_reengage_settle_frames += 1
else:
self.legacy_2025_reengage_settle_frames = 0
self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
if self.legacy_2025_reengage_settle_frames < _LEGACY_2025_REENGAGE_SETTLE_FRAMES:
return True
self.legacy_2025_handoff_active = False
self.legacy_2025_reengage_settle_frames = 0
self.legacy_2025_reclaim_frames = _LEGACY_2025_RECLAIM_FRAMES
self.legacy_2025_reclaim_start_angle = CS.out.steeringAngleDeg
return True
def _legacy_2025_reclaim_target(self, target_angle):
if self.legacy_2025_reclaim_frames <= 0:
return target_angle
progress = (_LEGACY_2025_RECLAIM_FRAMES - self.legacy_2025_reclaim_frames + 1) / _LEGACY_2025_RECLAIM_FRAMES
eased_progress = progress ** _LEGACY_2025_RECLAIM_EXPONENT
target_angle = self.legacy_2025_reclaim_start_angle + eased_progress * \
(target_angle - self.legacy_2025_reclaim_start_angle)
self.legacy_2025_reclaim_frames -= 1
return target_angle
def _reset_angle_handoff(self):
self.driver_override = False
self.angle_override_confirm_frames = 0
self.angle_handoff_active = False
self.angle_override_hold_frames = 0
self.angle_reengage_settle_frames = 0
self.angle_reengage_reference_angle = 0.0
self.angle_reclaim_frames = 0
self.angle_reclaim_start_angle = 0.0
def _angle_manual_handoff(self, CS, lat_active, use_steering_pressed=False):
if not lat_active:
@@ -230,44 +136,23 @@ class CarController(CarControllerBase):
driver_override = self._update_angle_driver_override(CS)
if use_steering_pressed:
driver_override = driver_override or getattr(CS.out, "steeringPressed", False)
steering_rate = abs(getattr(CS.out, "steeringRateDeg", 0.0))
if driver_override:
self.angle_handoff_active = True
self.angle_override_hold_frames = _ANGLE_OVERRIDE_HOLD_FRAMES
self.angle_reengage_settle_frames = 0
self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
self.angle_reclaim_frames = 0
return True
if not self.angle_handoff_active and not self.angle_lkas_active and \
abs(getattr(CS.out, "steeringRateDeg", 0.0)) > _ANGLE_REENGAGE_MAX_STEER_RATE:
if self.angle_handoff_active:
if steering_rate > _ANGLE_REENGAGE_MAX_STEER_RATE:
return True
self.angle_handoff_active = False
return True
if not self.angle_lkas_active and steering_rate > _ANGLE_REENGAGE_MAX_STEER_RATE:
self.angle_handoff_active = True
self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
if not self.angle_handoff_active:
return False
if self.angle_override_hold_frames > 0:
self.angle_override_hold_frames -= 1
if self.angle_override_hold_frames == 0:
self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
return True
wheel_stable = abs(getattr(CS.out, "steeringRateDeg", 0.0)) <= _ANGLE_REENGAGE_MAX_STEER_RATE and \
abs(CS.out.steeringAngleDeg - self.angle_reengage_reference_angle) <= _ANGLE_REENGAGE_MAX_ANGLE_DELTA
if wheel_stable:
self.angle_reengage_settle_frames += 1
else:
self.angle_reengage_settle_frames = 0
self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
if self.angle_reengage_settle_frames < _ANGLE_REENGAGE_SETTLE_FRAMES:
return True
self.angle_handoff_active = False
self.angle_reengage_settle_frames = 0
self.angle_reclaim_frames = _ANGLE_RECLAIM_FRAMES
self.angle_reclaim_start_angle = CS.out.steeringAngleDeg
return True
return False
def _update_angle_driver_override(self, CS):
"""Debounce the higher-confidence raw torque override signal for angle cars."""
@@ -285,17 +170,6 @@ class CarController(CarControllerBase):
return self.driver_override
def _angle_reclaim_target(self, target_angle):
if self.angle_reclaim_frames <= 0:
return target_angle
progress = (_ANGLE_RECLAIM_FRAMES - self.angle_reclaim_frames + 1) / _ANGLE_RECLAIM_FRAMES
eased_progress = progress ** _ANGLE_RECLAIM_EXPONENT
target_angle = self.angle_reclaim_start_angle + eased_progress * \
(target_angle - self.angle_reclaim_start_angle)
self.angle_reclaim_frames -= 1
return target_angle
def lateral_angle(self, CC, CS):
if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
mads_only = CC.latActive and not CC.enabled
@@ -304,12 +178,11 @@ class CarController(CarControllerBase):
lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill
manual_handoff = self._legacy_2025_manual_handoff(CS, CC.latActive)
manual_handoff = self._angle_manual_handoff(CS, lkas_available)
lkas_active = lkas_available and not manual_handoff
steer_target = self._legacy_2025_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg
apply_steer = apply_std_steer_angle_limits(
steer_target,
CC.actuators.steeringAngleDeg,
self.apply_steer_last,
CS.out.vEgoRaw,
CS.out.steeringAngleDeg,
@@ -317,7 +190,7 @@ class CarController(CarControllerBase):
self.p.LEGACY_2025_ANGLE_LIMITS,
)
self.apply_steer_last = apply_steer
self.legacy_2025_lkas_active = lkas_active
self.angle_lkas_active = lkas_active
return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus)
if self.CP.carFingerprint in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
@@ -328,17 +201,16 @@ class CarController(CarControllerBase):
CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill
manual_handoff = self._angle_manual_handoff(
CS, CC.latActive, use_steering_pressed=self.CP.carFingerprint == CAR.SUBARU_OUTBACK_2023,
CS, lkas_available, use_steering_pressed=self.CP.carFingerprint == CAR.SUBARU_OUTBACK_2023,
)
lkas_active = lkas_available and not manual_handoff
if lkas_active and not self.angle_lkas_active:
self.apply_steer_last = CS.out.steeringAngleDeg
steer_target = self._angle_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg
if self.CP.carFingerprint in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
apply_steer = apply_std_steer_angle_limits(
steer_target,
CC.actuators.steeringAngleDeg,
self.apply_steer_last,
CS.out.vEgoRaw,
CS.out.steeringAngleDeg,
@@ -365,13 +237,12 @@ class CarController(CarControllerBase):
lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
getattr(CS.out, "gearShifter", structs.CarState.GearShifter.drive) == structs.CarState.GearShifter.drive and \
not getattr(CS.out, "standstill", False)
manual_handoff = self._angle_manual_handoff(CS, CC.latActive)
manual_handoff = self._angle_manual_handoff(CS, lkas_available)
lat_active = lkas_available and not self.driver_override and not manual_handoff
if lat_active and not self.angle_lkas_active:
self.apply_steer_last = CS.out.steeringAngleDeg
steer_target = self._angle_reclaim_target(CC.actuators.steeringAngleDeg) if lat_active else CC.actuators.steeringAngleDeg
apply_steer = apply_steer_angle_limits_vm(
steer_target,
CC.actuators.steeringAngleDeg,
self.apply_steer_last,
CS.out.vEgoRaw,
CS.out.steeringAngleDeg,
opendbc_repo/opendbc/car/subaru/tests/test_subaru.py
+25 -54
View File
@@ -414,7 +414,7 @@ def test_legacy_2025_engagement_continues_from_last_sent_angle():
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(0.47)
def test_legacy_2025_waits_for_manual_steering_to_settle_before_reengaging():
def test_legacy_2025_reengages_immediately_after_manual_steering_stops():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
controller = CarController({}, CP)
CC = SimpleNamespace(
@@ -444,42 +444,20 @@ def test_legacy_2025_waits_for_manual_steering_to_settle_before_reengaging():
CS.out.steeringPressed = False
CS.out.steeringTorque = 0.0
CS.out.steeringAngleDeg = -113.78
CS.out.steeringRateDeg = 0.0
msg = controller.lateral_angle(CC, CS)
parser.update([(2, [msg])])
parser.update([(3, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
for i in range(9):
CS.out.steeringAngleDeg += 0.5
CS.out.steeringRateDeg = 20.0
msg = controller.lateral_angle(CC, CS)
parser.update([(3 + i, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
for i in range(6):
if i % 2:
CS.out.steeringAngleDeg += 0.5
CS.out.steeringRateDeg = 0.0 if i % 2 == 0 else 20.0
msg = controller.lateral_angle(CC, CS)
parser.update([(12 + i, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
CS.out.steeringRateDeg = 0.0
for i in range(8):
msg = controller.lateral_angle(CC, CS)
parser.update([(18 + i, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
measured_angle = CS.out.steeringAngleDeg
msg = controller.lateral_angle(CC, CS)
parser.update([(26, [msg])])
parser.update([(4, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
assert abs(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] - measured_angle) < 0.1
assert -113.78 < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < -100.0
def test_legacy_2025_manual_handoff_reclaim_is_gradual():
def test_legacy_2025_manual_handoff_reentry_uses_normal_angle_limits():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
controller = CarController({}, CP)
CC = SimpleNamespace(
@@ -508,22 +486,24 @@ def test_legacy_2025_manual_handoff_reclaim_is_gradual():
CS.out.steeringPressed = False
CS.out.steeringTorque = 0.0
CS.out.steeringRateDeg = 0.0
for i in range(19):
msg = controller.lateral_angle(CC, CS)
parser.update([(2 + i, [msg])])
msg = controller.lateral_angle(CC, CS)
parser.update([(3, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
msg = controller.lateral_angle(CC, CS)
parser.update([(4, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
first_reclaim_angle = parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"]
assert first_reclaim_angle == pytest.approx(CS.out.steeringAngleDeg, abs=0.1)
first_reentry_angle = parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"]
assert CC.actuators.steeringAngleDeg < first_reentry_angle < CS.out.steeringAngleDeg
reclaim_angles = []
reentry_angles = []
for i in range(6):
msg = controller.lateral_angle(CC, CS)
parser.update([(20 + i, [msg])])
reclaim_angles.append(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"])
reentry_angles.append(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"])
assert all(reclaim_angles[i] >= reclaim_angles[i + 1] for i in range(len(reclaim_angles) - 1))
assert reclaim_angles[-1] > CC.actuators.steeringAngleDeg
assert all(reentry_angles[i] >= reentry_angles[i + 1] for i in range(len(reentry_angles) - 1))
assert reentry_angles[-1] > CC.actuators.steeringAngleDeg
def test_ascent_2023_uses_gen2_angle_bus_layout():
@@ -664,7 +644,7 @@ def test_angle_controller_uses_fixed_angle_rate_limits(platform):
@pytest.mark.parametrize("platform", (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023))
def test_angle_controller_yields_until_manual_steering_settles(platform):
def test_angle_controller_reengages_immediately_after_manual_steering_stops(platform):
CP = CarInterface.get_non_essential_params(platform)
controller = CarController({}, CP)
CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-10.0))
@@ -691,18 +671,18 @@ def test_angle_controller_yields_until_manual_steering_settles(platform):
CS.out.steeringTorque = 0.0
CS.out.steeringAngleDeg = -17.91
CS.out.steeringRateDeg = 0.0
for i in range(18):
msg = controller.lateral_angle(CC, CS)
parser.update([(2 + i, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
msg = controller.lateral_angle(CC, CS)
parser.update([(3, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
msg = controller.lateral_angle(CC, CS)
parser.update([(20, [msg])])
parser.update([(4, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg, abs=0.1)
assert CS.out.steeringAngleDeg < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < CC.actuators.steeringAngleDeg
def test_ascent_angle_controller_blocks_parking_lot_aol_engagement():
def test_ascent_angle_controller_waits_for_parking_lot_safety_envelope():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
controller = CarController({}, CP)
CC = SimpleNamespace(enabled=False, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-206.12))
@@ -726,15 +706,6 @@ def test_ascent_angle_controller_blocks_parking_lot_aol_engagement():
CS.out.steeringRateDeg = 0.0
msg = controller.lateral_angle(CC, CS)
parser.update([(2, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
for i in range(8):
msg = controller.lateral_angle(CC, CS)
parser.update([(3 + i, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
msg = controller.lateral_angle(CC, CS)
parser.update([(11, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
CS.out.gearShifter = structs.CarState.GearShifter.reverse
starpilot/car/ford/lateral.py
+34 -1
View File
@@ -34,6 +34,10 @@ MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - ACCELERATION_DUE_TO_GRAVITY * 0.06
STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL
CURVATURE_LOOKAHEAD_MIN = 0.20
CURVATURE_LOOKAHEAD_MAX = 0.40
MACH_E_TURN_IN_LOOKAHEAD_EXTRA = 0.40
MACH_E_TURN_IN_MIN_CURVATURE = 0.006
MACH_E_TURN_IN_FULL_CURVATURE = 0.009
MACH_E_TURN_IN_LAG_CURVATURE = 0.006
FORD_CURVATURE_LOOKAHEAD = {
CAR.FORD_EXPLORER_MK6: 0.20,
}
@@ -114,6 +118,7 @@ class FordLateralController:
self.manual_turn_recovery_timer = 0.0
self.curvature_samples = deque(maxlen=max(2, round(0.3 / STEER_DT)))
self.curvature_last = 0.0
self.desired_curvature_last = 0.0
self._frame = 0
self._update_params()
@@ -179,6 +184,25 @@ class FordLateralController:
precision = 0
return requested, precision
def _turn_in_preview_weight(self, desired: float, predicted: float, current: float) -> float:
if self.CP.carFingerprint not in FORD_CONSERVATIVE_PREVIEW_CARS:
return 0.0
if desired * predicted <= 0.0 or desired * self.desired_curvature_last < 0.0:
return 0.0
if abs(desired) <= abs(self.desired_curvature_last) or abs(current) >= abs(desired):
return 0.0
curvature_weight = float(np.interp(
abs(desired),
[MACH_E_TURN_IN_MIN_CURVATURE, MACH_E_TURN_IN_FULL_CURVATURE],
[0.0, 1.0],
))
lag_weight = float(np.clip(
(abs(desired) - abs(current)) / MACH_E_TURN_IN_LAG_CURVATURE,
0.0, 1.0,
))
return curvature_weight * lag_weight
def _manual_turn(self, CC, CS) -> bool:
if not CC.latActive:
self.human_turn.reset()
@@ -227,18 +251,27 @@ class FordLateralController:
self.manual_turn_recovery_timer = 0.0
self.curvature_samples.clear()
self.curvature_last = 0.0
self.desired_curvature_last = 0.0
return FordLateralResult()
manual_turn = self._manual_turn(CC, CS)
if manual_turn or CS.out.vEgoRaw < 0.1:
self.curvature_samples.clear()
self.curvature_last = 0.0
self.desired_curvature_last = 0.0
return FordLateralResult(active=not (
manual_turn and self.CP.carFingerprint in FORD_MANUAL_TURN_LATCH_CARS))
v_ego = float(CS.out.vEgoRaw)
predicted = self._predicted_curvature(v_ego, self._curvature_lookahead())
requested, precision = self._blend_and_scale(float(actuators.curvature), predicted, v_ego, current)
desired = float(actuators.curvature)
turn_in_weight = self._turn_in_preview_weight(desired, predicted, current)
if turn_in_weight > 0.0:
turn_in_predicted = self._predicted_curvature(
v_ego, self._curvature_lookahead() + MACH_E_TURN_IN_LOOKAHEAD_EXTRA)
predicted = float(np.interp(turn_in_weight, [0.0, 1.0], [predicted, turn_in_predicted]))
requested, precision = self._blend_and_scale(desired, predicted, v_ego, current)
self.desired_curvature_last = desired
if v_ego > 9.0:
requested = float(np.clip(requested, current - CarControllerParams.CURVATURE_ERROR,
starpilot/car/ford/tests/test_lateral.py
+41
View File
@@ -142,6 +142,47 @@ def test_mach_e_preview_remains_available_on_curve_entry(controller):
assert requested == pytest.approx(0.0011)
def test_mach_e_turn_in_preview_leads_when_path_lags(controller):
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
controller.desired_curvature_last = 0.007
weight = controller._turn_in_preview_weight(
desired=0.009, predicted=0.007, current=0.006)
assert weight == pytest.approx(0.5)
def test_mach_e_turn_in_preview_is_not_carried_into_unwind(controller):
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
controller.desired_curvature_last = 0.010
assert controller._turn_in_preview_weight(
desired=0.008, predicted=0.009, current=0.004) == 0.0
def test_mach_e_turn_in_preview_uses_extra_model_horizon(controller, monkeypatch):
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
controller.sm["liveDelay"].lateralDelay = 0.4
controller.desired_curvature_last = 0.007
lookaheads = []
monkeypatch.setattr(controller, "_predicted_curvature",
lambda _v_ego, lookahead: lookaheads.append(lookahead) or 0.007)
controller.update(
SimpleNamespace(latActive=True), car_state(speed=8.0, curvature=0.002),
SimpleNamespace(curvature=0.010),
)
assert lookaheads == [pytest.approx(0.4), pytest.approx(0.8)]
def test_non_mach_e_turn_in_preview_is_unchanged(controller):
controller.desired_curvature_last = 0.007
assert controller._turn_in_preview_weight(
desired=0.010, predicted=0.007, current=0.004) == 0.0
def test_non_mach_e_preview_blend_is_unchanged(controller):
requested, _ = controller._blend_and_scale(-0.0001, 0.002, 20.0, current=0.0015)