From ed251bfb85d1fd5a73522f6c9b61a3968f47f472 Mon Sep 17 00:00:00 2001 From: Praeuner Date: Tue, 25 Aug 2026 09:05:44 -0500 Subject: [PATCH 1/2] Update curvature_factor interpolation Add "high_gain_boundary" interpolation based on speed. Modify curvature_factor interpolation range from .0005 to the new high_gain_boundary. --- .../opendbc/sunnypilot/car/ford/lateral_angle_ext.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 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..6519606b88 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -508,12 +508,18 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # Speed-interpolated gain: at low speed both curves use 1.0; at high speed the params take effect. self.low_gain_calc = interp( - v_ego, [13.5, 26.82], [1.0, (self.path_angle_gain_lowC_highV * self.user_dampening_factor)] + v_ego, [11.18, 31.29], [1.00, (self.path_angle_gain_lowC_highV * self.user_dampening_factor)] ) - self.high_gain_calc = interp(v_ego, [13.5, 26.82], [(1.30 * self.low_speed_curv_factor), (self.path_angle_gain_highC_highV * self.high_speed_curv_factor)]) + self.high_gain_calc = interp( + v_ego, [11.18, 31.29], [(1.30 * self.low_speed_curv_factor), (self.path_angle_gain_highC_highV * self.high_speed_curv_factor)] + ) + + # Speed-interpolated curve-radius: at low speed, dont need full gain until a much tighter curve + high_gain_boundary = interp(v_ego, [11.18, 31.29], [0.02, 0.0045]) # As the curve gets bigger, we will need a little boost to the signal to to not understeer - self.curvature_factor = interp(abs(kappa_cmd), [0.0007, 0.001], [self.low_gain_calc, self.high_gain_calc]) + #self.curvature_factor = interp(abs(self.kappa_gain_filt), [0.0005, high_gain_boundary], [self.low_gain_calc, self.high_gain_calc]) + self.curvature_factor = interp(abs(kappa_cmd), [0.0005, high_gain_boundary], [self.low_gain_calc, self.high_gain_calc]) path_angle_calc = kappa_cmd * v_ego * self.curvature_factor path_angle = path_angle_calc From a15672fb15fc01b74ad55e517060d84617baef22 Mon Sep 17 00:00:00 2001 From: Praeuner Date: Tue, 1 Sep 2026 10:49:24 -0500 Subject: [PATCH 2/2] Decrease high gain boundaries Bring down high curve gain boundary for both high and low speed to better match real world max curves. Previous boundaries were theoretically possible, but would need an unrealistically gentle curve increase for the wheel to keep up with required angle. --- opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py | 3 +-- 1 file changed, 1 insertion(+), 2 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 6519606b88..aca3b1038c 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -515,10 +515,9 @@ def update_angle_strategy(self, CC, CS, actuators, CP): ) # Speed-interpolated curve-radius: at low speed, dont need full gain until a much tighter curve - high_gain_boundary = interp(v_ego, [11.18, 31.29], [0.02, 0.0045]) + high_gain_boundary = interp(v_ego, [11.18, 31.29], [0.015, 0.0035]) # As the curve gets bigger, we will need a little boost to the signal to to not understeer - #self.curvature_factor = interp(abs(self.kappa_gain_filt), [0.0005, high_gain_boundary], [self.low_gain_calc, self.high_gain_calc]) self.curvature_factor = interp(abs(kappa_cmd), [0.0005, high_gain_boundary], [self.low_gain_calc, self.high_gain_calc]) path_angle_calc = kappa_cmd * v_ego * self.curvature_factor