Ford: keep the angle-mode shadow curvature truthful during overrides

opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py

@@ -228,7 +228,15 @@ def update_angle_strategy(self, CC, CS, actuators, CP):
       self.bp_curvature_rate_limited = False
       self.bp_curvature_deviation_limited = False
       self.sim_curvature_last = 0.0
-      self.bp_kappa_cmd = 0.0
+      # Publish the shadow curvature from the measured curvature while inactive. LKA keeps
+      # carrying angle_mode_engaged whenever angle mode is configured (independent of
+      # latActive), and ford.h latches the shadow from every LKA frame -- so the latched
+      # value must track reality here, not sit at a stale zero. Otherwise the first enabled
+      # LMC frame after (re-)engage races LKA's 33Hz latch against LMC's 20Hz enable bit and
+      # ford.h's deviation check compares a zero shadow against real measured curvature.
+      # (ford.h skips the check while steer_control_enabled is 0, so the value is free to
+      # follow the measurement during the inactive period itself.)
+      self.bp_kappa_cmd = self.get_current_curvature(CS)
       self.human_turn_detector.reset()
       self.angle_human_turn_active = False
       self.stall_blip_hold_s = 0.0
@@ -265,9 +273,10 @@ def update_angle_strategy(self, CC, CS, actuators, CP):
       self.bp_curvature_rate_limited = False
       self.bp_curvature_deviation_limited = False
       self.sim_curvature_last = 0.0
-      # Zero the shadow curvature on the wire during the override (mirrors the inactive path);
-      # ford.h skips the deviation check while steer_control_enabled is 0 either way.
-      self.bp_kappa_cmd = 0.0
+      # Truthful shadow during the override (mirrors the inactive path -- see the comment
+      # there): the driver is steering, so the honest command is the car's actual curvature,
+      # and the panda-latched shadow stays current for the re-engage frame.
+      self.bp_kappa_cmd = self.get_current_curvature(CS)
       # Keep exit detection current so resume doesn't compare against a stale pre-turn value.
       self._desired_curvature_last = float(actuators.curvature)
       # A human turn ends any stall episode -- its own mode 0 does the PSCM reset job. That also
@@ -317,7 +326,8 @@ def update_angle_strategy(self, CC, CS, actuators, CP):
       self.bp_curvature_rate_limited = False
       self.bp_curvature_deviation_limited = False
       self.sim_curvature_last = 0.0
-      self.bp_kappa_cmd = 0.0
+      # Truthful shadow during the blip (see the inactive-path comment).
+      self.bp_kappa_cmd = self.get_current_curvature(CS)
       self._desired_curvature_last = float(actuators.curvature)
       self.precision_type = 1
       if self.stall_blip_frames_left <= 0:
@@ -427,7 +437,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP):
     # routinely, not just on genuine pothole/override divergence. Curvature mode has always clipped
     # here; this brings angle mode's actual steering intent in line with that proven behavior rather
     # than only clipping the value reported to panda (which would make the check a no-op).
-    current_curvature = -CS.out.yawRate / max(v_ego, 0.1)
+    current_curvature = self.get_current_curvature(CS)
     self.bp_curvature_deviation_limited = False
     if v_ego > 9:
       _kappa_cmd_pre_error_clip = kappa_cmd
@@ -505,7 +515,13 @@ def update_angle_strategy(self, CC, CS, actuators, CP):
     # BluePilot: the error-clipped kappa path_angle was derived from -- carcontroller.py reads this
     # as shadow_curvature for ford.h's angle-mode deviation check (see fordcan_ext.create_lka_msg).
     # Not just telemetry: an actively-consumed value, unlike the removed *_kappa_cmd_raw stubs.
-    self.bp_kappa_cmd = kappa_cmd
+    # While the driver is pressing (before the human-turn override latches), the clipped planner
+    # kappa can't follow the wheel: the driver moves the measured curvature faster than the
+    # deviation clip tracks it, so the shadow can exit ford.h's error band mid-curve -- the one
+    # in-drive lateral safety block observed across ~3h of replayed road-test routes was exactly
+    # this (driver fighting a sustained curve with the mode still enabled). The honest command
+    # during a press is the driver's actual curvature.
+    self.bp_kappa_cmd = self.get_current_curvature(CS) if CS.out.steeringPressed else kappa_cmd
 
     # BluePilot: would the equivalent curvature (kappa_cmd) have been rate-limited by curvature-mode's
     # ROC (apply_std_steer_angle_limits)? kappa_cmd is already error-clipped above (same clip

opendbc_repo/opendbc/sunnypilot/car/ford/lateral_curv_ext.py

@@ -230,6 +230,17 @@ def _ensure_lateral_curv_initialized(self, CP):
     # branch LateralCurvExt state is initialized eagerly in __init__, so nothing to do here.
     pass
 
+  def get_current_curvature(self, CS):
+    """Measured curvature of the car right now (OP sign convention).
+
+    The single measurement source for every BluePilot lateral consumer: the deviation
+    clip, the stall detector, and the shadow curvature published to ford.h's angle-mode
+    deviation check. Sourced from the RCM yaw rate -- the same family ford.h derives its
+    angle_meas from. The shadow value judged against that check must always come from
+    the same measurement as the check's own reference, so route all reads through here.
+    """
+    return -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
+
   def update_sm(self):
     """Update SubMaster and vehicle model. Called each frame before lateral/long update."""
     self.sm.update(0)
@@ -285,7 +296,7 @@ def update(self, CC, CS, actuators, apply_curvature_last, CP):
       self.pc_blend_ratio_v = [self.pc_blend_ratio_low_C, self.pc_blend_ratio_high_C]
 
       # Current and desired curvature
-      current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
+      current_curvature = self.get_current_curvature(CS)
       desired_curvature = actuators.curvature
 
       # Extract predicted curvature from modelV2

opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py

@@ -0,0 +1,155 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+# Unit tests for angle-mode shadow-curvature publishing (bp_kappa_cmd).
+#
+# The shadow value is consumed by carcontroller as the input to ford.h's angle-mode
+# deviation check (Lane_Assist_Data1 bytes 5-6, judged against angle_meas). These tests
+# pin the truthfulness contract: whenever the planner kappa cannot honestly describe the
+# car's steering -- inactive, human-turn override, stall blip, driver pressing -- the
+# published shadow must equal the measured curvature, so the panda-latched value always
+# stays inside the check's band and re-engage frames never compare a stale zero against
+# real measured curvature.
+
+import unittest
+from dataclasses import dataclass
+from unittest import mock
+
+from opendbc.car import structs
+from opendbc.car.ford.values import CarControllerParams
+from opendbc.car.interfaces import scale_tire_stiffness
+from opendbc.sunnypilot.car.ford import lateral_curv_ext
+from opendbc.sunnypilot.car.ford.lateral_curv_ext import LateralCurvExt
+from opendbc.sunnypilot.car.ford.lateral_angle_ext import LateralAngleExt
+
+
+def _explorer_cp():
+  CP = structs.CarParams()
+  CP.mass = 2050.
+  CP.wheelbase = 3.025
+  CP.steerRatio = 16.8
+  CP.centerToFront = CP.wheelbase * 0.44
+  CP.tireStiffnessFactor = 0.82
+  CP.tireStiffnessFront, CP.tireStiffnessRear = scale_tire_stiffness(
+    CP.mass, CP.wheelbase, CP.centerToFront, CP.tireStiffnessFactor)
+  return CP
+
+
+class _FakeLiveDelay:
+  lateralDelay = 0.2
+
+
+class _FakeSubMaster:
+  def __init__(self, *args, **kwargs):
+    self.updated = {s: False for s in ('modelV2', 'liveParameters', 'selfdriveState', 'radarState', 'liveDelay')}
+
+  def update(self, timeout=0):
+    pass
+
+  def __getitem__(self, key):
+    if key == 'liveDelay':
+      return _FakeLiveDelay()
+    raise KeyError(key)
+
+
+class _ForcedDetector:
+  def __init__(self, active):
+    self.active = active
+
+  def update(self, *_args):
+    return self.active
+
+  def reset(self):
+    pass
+
+
+@dataclass
+class _CSOut:
+  vEgoRaw: float = 15.0
+  vEgo: float = 15.0
+  steeringPressed: bool = False
+  steeringAngleDeg: float = 0.0
+  yawRate: float = 0.0
+
+
+class _CS:
+  def __init__(self, **kwargs):
+    self.out = _CSOut(**kwargs)
+    self.lat_ctl_lim_stat = 0
+
+
+@dataclass
+class _CC:
+  latActive: bool = True
+
+
+@dataclass
+class _Actuators:
+  curvature: float = 0.0
+
+
+class _Harness(LateralCurvExt, LateralAngleExt):
+  """Mirrors CarController's mixin composition (see carcontroller.py)."""
+
+  def __init__(self, CP):
+    with mock.patch.object(lateral_curv_ext.messaging, 'SubMaster', _FakeSubMaster):
+      LateralCurvExt.__init__(self, CP, None)
+    LateralAngleExt.__init__(self, CP, None)
+
+
+class TestShadowCurvaturePublishing(unittest.TestCase):
+  V_EGO = 15.0
+  YAW_RATE = 0.75  # rad/s -> measured curvature = -0.75 / 15 = -0.05 (OP convention)
+
+  def setUp(self):
+    self.CP = _explorer_cp()
+    self.ext = _Harness(self.CP)
+    self.ext.human_turn_detector = _ForcedDetector(False)
+    self.cs = _CS(vEgoRaw=self.V_EGO, vEgo=self.V_EGO, yawRate=self.YAW_RATE)
+    self.measured = -self.YAW_RATE / self.V_EGO
+
+  def _update(self, lat_active=True):
+    return self.ext.update_angle_strategy(_CC(latActive=lat_active), self.cs, _Actuators(curvature=0.01), self.CP)
+
+  def test_inactive_publishes_measured(self):
+    result = self._update(lat_active=False)
+    self.assertEqual(result.path_angle, 0.0)
+    self.assertAlmostEqual(self.ext.bp_kappa_cmd, self.measured)
+
+  def test_human_turn_override_publishes_measured(self):
+    self.ext.human_turn_detector = _ForcedDetector(True)
+    result = self._update()
+    self.assertTrue(self.ext.angle_human_turn_active)
+    self.assertEqual(result.path_angle, 0.0)
+    self.assertAlmostEqual(self.ext.bp_kappa_cmd, self.measured)
+
+  def test_stall_blip_publishes_measured(self):
+    self.ext.stall_blip_frames_left = 3
+    result = self._update()
+    self.assertTrue(self.ext.angle_stall_blip_active)
+    self.assertEqual(result.path_angle, 0.0)
+    self.assertAlmostEqual(self.ext.bp_kappa_cmd, self.measured)
+
+  def test_pressed_publishes_measured(self):
+    self.cs.out.steeringPressed = True
+    self._update()
+    self.assertFalse(self.ext.angle_human_turn_active)
+    self.assertAlmostEqual(self.ext.bp_kappa_cmd, self.measured)
+
+  def test_hands_off_publishes_clipped_planner_kappa(self):
+    # planner wants +0.01 while measured is -0.05: the deviation clip (active above 9 m/s)
+    # bounds the shadow to measured + CURVATURE_ERROR, not measured itself -- hands-off
+    # behavior is unchanged by the truthful-shadow sites.
+    self._update()
+    expected = self.measured + CarControllerParams.CURVATURE_ERROR
+    self.assertAlmostEqual(self.ext.bp_kappa_cmd, expected)
+    self.assertNotAlmostEqual(self.ext.bp_kappa_cmd, self.measured)
+    self.assertTrue(self.ext.bp_curvature_deviation_limited)
+
+
+if __name__ == '__main__':
+  unittest.main()