From 2c3fc143a3a5c4e933c4a05ba285a402e73d7241 Mon Sep 17 00:00:00 2001
From: firestar5683 <168790843+firestar5683@users.noreply.github.com>
Date: Sun, 29 Mar 2026 23:40:08 -0500
Subject: [PATCH] plot grading

---
 starpilot/system/the_pond/the_pond.py | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/starpilot/system/the_pond/the_pond.py b/starpilot/system/the_pond/the_pond.py
index 118c1efe..5da6db0f 100644
--- a/starpilot/system/the_pond/the_pond.py
+++ b/starpilot/system/the_pond/the_pond.py
@@ -727,6 +727,17 @@ def _is_plots_boot_stabilizing():
     return False
   return _get_system_uptime_seconds() < _PLOTS_BOOT_STABILIZATION_WINDOW_S
 
+def _extract_plots_speed_mps(controls_state, live_pose):
+  try:
+    velocity_device = getattr(live_pose, "velocityDevice", None)
+    if velocity_device and getattr(velocity_device, "valid", False):
+      # Use forward device-frame velocity for plot gating without adding any new subscriptions.
+      return abs(_safe_float(getattr(velocity_device, "x", 0.0), 0.0))
+  except Exception:
+    pass
+
+  return abs(_safe_float(getattr(controls_state, "vPid", 0.0), 0.0))
+
 def _extract_lateral_accel_values(controls_state, speed_mps):
   v_ego = max(0.0, _safe_float(speed_mps))
   speed_sq = v_ego * v_ego
@@ -833,7 +844,7 @@ def _plots_worker():
 
       controls_state = sm["controlsState"]
       live_pose = sm["livePose"]
-      speed = _safe_float(getattr(controls_state, "vPid", 0.0))
+      speed = _extract_plots_speed_mps(controls_state, live_pose)
       controls_active = bool(getattr(controls_state, "active", False))
       long_control_state = int(_safe_float(getattr(controls_state, "longControlState", 0)))
       longitudinal_control_active = controls_active and long_control_state != 0