Keep Turn Assist helping through braking turns

openpilot/starpilot/lateral/model_turn_assist.py

@@ -236,7 +236,8 @@ class ModelTurnAssist:
     if (CC.latActive and (blinker_dir != 0.0) and (model_v2.meta.laneChangeState == 0) and
         (TURN_LEAD_MIN_SPEED <= CS.vEgo < TURN_LEAD_MAX_SPEED) and (new_desired_curvature * blinker_dir > -TURN_LEAD_MODEL_OPPOSE)):
       d_near = max(min(TURN_LEAD_T * CS.vEgo, TURN_LEAD_MAX_M), TURN_LEAD_MIN_M)
-      stopping_short = CS.aEgo < TURN_LEAD_DECEL_GATE and CS.vEgo ** 2 / (2.0 * -CS.aEgo) < TURN_LEAD_STOP_MARGIN * d_near
+      turning = self.curvature * blinker_dir > CURVATURE_HOLD_CONFIRM_MIN
+      stopping_short = not turning and CS.aEgo < TURN_LEAD_DECEL_GATE and CS.vEgo ** 2 / (2.0 * -CS.aEgo) < TURN_LEAD_STOP_MARGIN * d_near
       lead_curvature = 0.0 if stopping_short else _plan_dual_probe(model_v2, d_near, d_near + 3.0) * TURN_LEAD_SCALE
       lead_curvature = max(min(lead_curvature, TURN_LEAD_CAP), -TURN_LEAD_CAP)
       if lead_curvature * blinker_dir > 0.0:

openpilot/starpilot/lateral/tests/test_model_turn_assist_braking.py

@@ -0,0 +1,62 @@
+import unittest
+from types import SimpleNamespace
+
+from openpilot.starpilot.lateral.model_turn_assist import (
+  CURVATURE_HOLD_CONFIRM_MIN, ModelTurnAssist, TURN_LEAD_CAP,
+)
+
+
+def turn_sample(direction=1, curvature=.004, acceleration=-3., active=True, speed=6., lane_change=0):
+  xs = list(range(31))
+  model = SimpleNamespace(position=SimpleNamespace(x=xs, y=[direction * .025 * x*x for x in xs]),
+                          meta=SimpleNamespace(laneChangeState=lane_change))
+  cs = SimpleNamespace(vEgo=speed, aEgo=acceleration, rightBlinker=direction == 1,
+                       leftBlinker=direction == -1, steeringPressed=False, steeringTorque=0.)
+  assist = ModelTurnAssist()
+  assist.curvature = direction * curvature
+  return assist.update(cs, model, SimpleNamespace(latActive=active), direction * .005)
+
+
+class TestTurnAssistBraking(unittest.TestCase):
+  def test_braking_does_not_remove_assistance_during_established_turn(self):
+    for direction in (-1, 1):
+      for acceleration in (-1., -3., -6.):
+        with self.subTest(direction=direction, acceleration=acceleration):
+          output = turn_sample(direction, acceleration=acceleration)
+          self.assertGreater(output * direction, .005)
+          self.assertEqual(output, turn_sample(direction, acceleration=0.))
+          self.assertLessEqual(abs(output), TURN_LEAD_CAP)
+
+  def test_stopping_before_turn_keeps_original_request(self):
+    for direction in (-1, 1):
+      for curvature in (0., CURVATURE_HOLD_CONFIRM_MIN, -.004):
+        with self.subTest(direction=direction, curvature=curvature):
+          self.assertEqual(turn_sample(direction, curvature=curvature), direction * .005)
+      self.assertGreater(turn_sample(direction, curvature=0., acceleration=0.) * direction, .005)
+
+  def test_lateral_inactive_or_lane_change_never_adds_turn_lead(self):
+    for direction in (-1, 1):
+      self.assertEqual(turn_sample(direction, active=False), direction * .005)
+      self.assertEqual(turn_sample(direction, lane_change=1), direction * .005)
+
+  def test_turn_lead_speed_bounds_are_unchanged(self):
+    for direction in (-1, 1):
+      for speed in (3., 7., 8.):
+        with self.subTest(direction=direction, speed=speed):
+          self.assertEqual(turn_sample(direction, speed=speed), direction * .005)
+
+  def test_opposing_model_and_no_signal_keep_request(self):
+    for direction in (-1, 1):
+      for signal in (False, True):
+        assist = ModelTurnAssist()
+        assist.curvature = direction * .01
+        cs = SimpleNamespace(vEgo=6., aEgo=-3., leftBlinker=signal and direction == -1,
+                             rightBlinker=signal and direction == 1, steeringPressed=False, steeringTorque=0.)
+        model = SimpleNamespace(position=SimpleNamespace(x=[0., 10.], y=[0., -direction * 3.]),
+                                meta=SimpleNamespace(laneChangeState=0))
+        desired = -direction * .01
+        self.assertEqual(assist.update(cs, model, SimpleNamespace(latActive=True), desired), desired)
+
+
+if __name__ == '__main__':
+  unittest.main()

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "a00625907878fe5b0b96eeb5982df585ae89bdf0c0e734f2461448fb1dbd83ee", "schema_version": 1}
+{"protected_inventory_sha256": "ccfa43cb7752bbb1f157d40b458817f1641998199b936bc39453e84c5fc26c3e", "schema_version": 1}