Skip to content

Predicted_Curvature Weight Adjustments #191

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Praeuner wants to merge 2 commits into
base: bp-dev
Choose a base branch
from
Open

Predicted_Curvature Weight Adjustments #191

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ba20937
Predicted_Curvature Weight Blending
Praeuner Aug 25, 2026
77ec55c
Taper predicted_curvature weight at speed
Praeuner Sep 1, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 16 additions & 6 deletions opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -141,6 +141,7 @@ class LateralAngleExt:
def __init__(self, CP=None, CP_SP=None):
# Predicted-curvature blend for path_angle: pred * b + desired * (1-b); b from ``FordPathAngleBlendRatio``
self.path_angle_blend_ratio = _FORD_PATH_ANGLE_BLEND_RATIO_DEFAULT
self.b_blend = self.path_angle_blend_ratio / 2 #initialize halfway between low speed and high speed
# Max extra VLT above t_base; from ``FordVLTExtraMax`` param
self.vlt_extra_max = _VLT_T_EXTRA_MAX
# Telemetry: final path_angle (rad) after limits (see bp_card_publisher)
Expand Down Expand Up @@ -407,7 +408,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP):
if self.model is not None and len(self.model.orientationRate.z) >= 17:
_curvatures_ref = np.array(self.model.orientationRate.z) / max(0.01, v_ego)
_kappa_at_t_base = abs(float(interp(_t_base, ModelConstants.T_IDXS, _curvatures_ref)))
_kappa_entering = _kappa_at_t_base > abs(desired_curvature)
_kappa_entering = _kappa_at_t_base > abs(desired_curvature) * 1.1
if _kappa_entering:
_kappa_factor = 1.0 # curve deepening ahead: full extra lookahead for gradual entry
else:
Expand All @@ -422,8 +423,8 @@ def update_angle_strategy(self, CC, CS, actuators, CP):
interp(curvature_lookup_time, ModelConstants.T_IDXS, curvatures)
)

b = float(self.path_angle_blend_ratio)
b = float(clip(b, 0.0, 1.0))
b = float(clip(self.path_angle_blend_ratio, 0.0, 1.0))
b = interp(v_ego, [_VLT_V_LOW_MS, _VLT_V_HIGH_MS], [b, 0.0])

# Exit-biased blend: near the PSCM authority limit or while the planner is actively
# reducing curvature (exit detected), drop model prediction weight from 60% → ~15%.
Expand All @@ -447,10 +448,19 @@ def update_angle_strategy(self, CC, CS, actuators, CP):
# that same real-world trigger rate on this branch's actual 20Hz cadence; unscaled it fired at
# 0.04 (1/m)/s, collapsing the model blend on mild straightening instead of genuine exits.
# Same bug class and fix as _PSCM_SAT_UNWIND_RATE and _soft_roc above.
_desired_falling = abs(desired_curvature) < abs(self._desired_curvature_last) - 0.010
_desired_falling = (
abs(desired_curvature) < abs(self._desired_curvature_last) * 0.9
)
_on_exit_near_limit = not _kappa_entering and (_pscm_lim >= 1 or _in_hard_sat or _desired_falling)
b_blend = float(clip(b * 0.25, 0.0, 1.0)) if _on_exit_near_limit else b
requested_curvature = predicted_curvature * b_blend + desired_curvature * (1.0 - b_blend)

# Low pass filter for b_blend. Prevents instant jumps between .5 and .125 predicted_curvature weight
target_b_blend = b * 0.25 if _on_exit_near_limit else b
self.b_blend = (
0.75 * self.b_blend +
0.25 * target_b_blend
)

requested_curvature = predicted_curvature * self.b_blend + desired_curvature * (1.0 - self.b_blend)
self._desired_curvature_last = desired_curvature

if self.model is not None:
Expand Down