Revert "Stops on curves false positives"

This reverts commit f22bcfee36.

starpilot/common/starpilot_utilities.py

@@ -124,24 +124,6 @@ def calculate_lane_width(lane_line1, lane_line2, road_edge=None):
   return float(distance_to_lane)
 
 
-def calculate_model_path_length(model_position):
-  # Distance the model plans to TRAVEL, measured along the path.
-  #
-  # position.x[-1] is only the forward projection of the plan's endpoint in the car's
-  # frame, so it collapses as soon as the path bends: on a highway sweeper it reads
-  # ~0.85x the real distance, on an off-ramp ~0.30x. Every "is the model planning a
-  # stop" test compares this against v_ego * T, so pure geometry reads as a stop.
-  # Measured on route 78511c37de32c375--9c33d63ad6 (32 min): 546 of 1149 false stop
-  # detections came from the chord collapse alone, with zero true stops depending on it
-  # (on a straight approach the two are identical to within 0.3%).
-  x = np.asarray(model_position.x, dtype=np.float64)
-  y = np.asarray(model_position.y, dtype=np.float64)
-  if x.size < 2 or y.size != x.size:
-    return float(x[-1]) if x.size else 0.0
-
-  return float(np.sum(np.hypot(np.diff(x), np.diff(y))))
-
-
 # Credit goes to Pfeiferj!
 def calculate_road_curvature(modelData, v_ego):
   orientation_rate = np.array(modelData.orientationRate.z)

starpilot/controls/lib/starpilot_vcruise.py

@@ -63,14 +63,6 @@ LEAD_VETO_M_OVERRIDES = {
 FORCE_STOP_APPROACH_DECEL = 0.65  # m/s^2 — speed ceiling before commit. LOWER = more early
                           # braking; don't go under FORCE_STOP_MODEL_APPROACH_DECEL
 ADAS_MAX_MS = 17.88       # 40 mph — cross-street ADAS guard
-CEM_MAX_MS = 17.88        # 40 mph — same envelope for the model path. Committing at
-                          # ACTIVATION_M from above this needs >2.4 m/s^2, past the
-                          # 2.3 m/s^2 line where stops start failing, and a hard
-                          # highway-speed decel is what a driver reads as a phantom
-                          # stop. Every real force stop in route
-                          # 78511c37de32c375--9c33d63ad6 armed at <=39 mph; the one
-                          # false commit (off-ramp, 5.2 s of Force Stop) armed at 48.
-                          # The far-approach envelope bleeds speed under this first.
 DASH_SEED_M = 27.0        # ~88 ft — typical ADAS detection distance, used to snap
                           # tracked length closer when dashboard confirms a sign
 DASH_MODEL_AGREE_M = 50.0 # m — dash arm/snap needs model_length under this; a lone dash bit
@@ -380,16 +372,9 @@ class StarPilotVCruise:
     lead_present = (bool(getattr(lead, "status", False))
                     and float(getattr(lead, "dRel", float("inf"))) < lead_veto_m
                     and float(getattr(lead, "vLead", float("inf"))) < v_ego + 2.0)
-    # stop_then_turn exists so a wheel already pre-wound at a stop line can't veto a
-    # legit Force Stop. That scene only exists at pre-wind speeds. Unscoped, the override
-    # made this veto unreachable: stop_then_turn is armed by stop_light_detected on the
-    # same frame the veto is evaluated, so it was True on 100% of the frames the veto
-    # was meant to gate (route 78511c37de32c375--9c33d63ad6: 1130 curved detection
-    # frames, curved_approach_scene fired on 0 of them).
-    prewind_scene = stop_then_turn and v_ego <= FORCE_STOP_TURN_VETO_MAX_SPEED
     curved_approach_scene = (
       abs(float(getattr(self.starpilot_planner, "road_curvature", 0.0))) >= FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE
-      and not prewind_scene
+      and not stop_then_turn
     )
 
     # CEM/model path: model predicted stop within ACTIVATION_M.
@@ -409,7 +394,6 @@ class StarPilotVCruise:
     cem_path = (stop_light_detected
                 and controls_enabled and starpilot_toggles.force_stops
                 and model_length_active
-                and v_ego < CEM_MAX_MS
                 and self.override_force_stop_timer <= 0
                 and not self.starpilot_planner.driving_in_curve
                 and not curved_approach_scene

starpilot/controls/starpilot_planner.py

@@ -20,12 +20,7 @@ from openpilot.selfdrive.controls.lib.lead_behavior import (
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE
 from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_lead_follow_jerk_scale
 
-from openpilot.starpilot.common.starpilot_utilities import (
-  calculate_lane_width,
-  calculate_model_path_length,
-  calculate_road_curvature,
-  extract_curve_profile,
-)
+from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature, extract_curve_profile
 from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD
 from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode
 from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
@@ -210,8 +205,8 @@ class StarPilotPlanner:
     self.CS_prev_left_blinker = CS.leftBlinker
     self.CS_prev_right_blinker = CS.rightBlinker
 
+    self.model_length = sm["modelV2"].position.x[-1]
     model_position = sm["modelV2"].position
-    self.model_length = calculate_model_path_length(model_position)
     model_path_y = getattr(model_position, "y", [])
     if len(model_path_y) == len(model_position.x):
       self.lead_path_y = float(np.interp(self.lead_one.dRel, model_position.x, model_path_y))