From ba20937aacf2b92c365cee8b6693dca3b57e1efc Mon Sep 17 00:00:00 2001 From: Praeuner Date: Tue, 25 Aug 2026 08:52:02 -0500 Subject: [PATCH 1/2] Predicted_Curvature Weight Blending Set the weight for predicted_curvature better throughout driving situations. --- .../sunnypilot/car/ford/lateral_angle_ext.py | 31 ++++++++++++++++--- 1 file changed, 27 insertions(+), 4 deletions(-) diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py index 9db162c866..8d4df18f7d 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -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 = _FORD_PATH_ANGLE_BLEND_RATIO_DEFAULT # 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) @@ -265,6 +266,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.path_angle_last = 0.0 self.bp_path_angle_final = 0.0 self.apply_curvature_last = 0.0 + self.b_blend = self.path_angle_blend_ratio self.bp_angle_rate_limited = False self.bp_curvature_rate_limited = False self.bp_curvature_deviation_limited = False @@ -311,6 +313,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.path_angle_last = 0.0 self.bp_path_angle_final = 0.0 self.apply_curvature_last = 0.0 + self.b_blend = self.path_angle_blend_ratio self.bp_angle_rate_limited = False self.bp_curvature_rate_limited = False self.bp_curvature_deviation_limited = False @@ -366,6 +369,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.path_angle_last = 0.0 self.bp_path_angle_final = 0.0 self.apply_curvature_last = 0.0 + self.b_blend = self.path_angle_blend_ratio self.bp_angle_rate_limited = False self.bp_curvature_rate_limited = False self.bp_curvature_deviation_limited = False @@ -407,7 +411,10 @@ 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 = ( + abs(desired_curvature) > 0.001 and + _kappa_at_t_base > abs(desired_curvature) * 1.25 + ) if _kappa_entering: _kappa_factor = 1.0 # curve deepening ahead: full extra lookahead for gradual entry else: @@ -447,10 +454,26 @@ 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(self._desired_curvature_last) > 0.001 and + abs(desired_curvature) < abs(self._desired_curvature_last) * 0.8 + ) _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) + _on_straightaway = (not _kappa_entering and not _desired_falling and abs(desired_curvature) < 0.00125) + # step function for b_blend. Prevents instant jumps between .5 and .125 predicted_curvature weight + if _on_exit_near_limit: + target_b_blend = b * 0.25 + elif _on_straightaway: + target_b_blend = b * 0.35 + else: + target_b_blend = b + b_step = 0.1 + if target_b_blend > self.b_blend: + self.b_blend = min(target_b_blend, self.b_blend + b_step) + else: + self.b_blend = max(target_b_blend, self.b_blend - b_step) + + 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: From 77ec55c73e3a6ace7182e6a446983ae57c9c25ce Mon Sep 17 00:00:00 2001 From: Praeuner Date: Tue, 1 Sep 2026 10:20:32 -0500 Subject: [PATCH 2/2] Taper predicted_curvature weight at speed Above 55mph, predicted_curvature is not needed because PSCM delay is within stock live_delay ranges. PSCM delay smoothly decreases as speed increases, so should predicted_curvature weight. Interpolate b based on speed: interp b from _FORD_PATH_ANGLE_BLEND_RATIO to 0.0 between 25mph and 55mph Few minor tweaks to b_blend logic: Initialize b_blend halfway between 25 and 55 mph so it can quickly raise or lower. Remove re-initializations on interruptions. Remove straightaway logic since no more blending at highway speeds. Replace b_blend step function with smoother low pass filter. --- .../sunnypilot/car/ford/lateral_angle_ext.py | 39 +++++++------------ 1 file changed, 13 insertions(+), 26 deletions(-) diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py index 8d4df18f7d..d8f4cf98da 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -141,7 +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 = _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) @@ -266,7 +266,6 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.path_angle_last = 0.0 self.bp_path_angle_final = 0.0 self.apply_curvature_last = 0.0 - self.b_blend = self.path_angle_blend_ratio self.bp_angle_rate_limited = False self.bp_curvature_rate_limited = False self.bp_curvature_deviation_limited = False @@ -313,7 +312,6 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.path_angle_last = 0.0 self.bp_path_angle_final = 0.0 self.apply_curvature_last = 0.0 - self.b_blend = self.path_angle_blend_ratio self.bp_angle_rate_limited = False self.bp_curvature_rate_limited = False self.bp_curvature_deviation_limited = False @@ -369,7 +367,6 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.path_angle_last = 0.0 self.bp_path_angle_final = 0.0 self.apply_curvature_last = 0.0 - self.b_blend = self.path_angle_blend_ratio self.bp_angle_rate_limited = False self.bp_curvature_rate_limited = False self.bp_curvature_deviation_limited = False @@ -411,10 +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 = ( - abs(desired_curvature) > 0.001 and - _kappa_at_t_base > abs(desired_curvature) * 1.25 - ) + _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: @@ -429,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%. @@ -455,24 +449,17 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # 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(self._desired_curvature_last) > 0.001 and - abs(desired_curvature) < abs(self._desired_curvature_last) * 0.8 + 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) - _on_straightaway = (not _kappa_entering and not _desired_falling and abs(desired_curvature) < 0.00125) - # step function for b_blend. Prevents instant jumps between .5 and .125 predicted_curvature weight - if _on_exit_near_limit: - target_b_blend = b * 0.25 - elif _on_straightaway: - target_b_blend = b * 0.35 - else: - target_b_blend = b - b_step = 0.1 - if target_b_blend > self.b_blend: - self.b_blend = min(target_b_blend, self.b_blend + b_step) - else: - self.b_blend = max(target_b_blend, self.b_blend - b_step) - + + # 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