car: use vEgoCluster pre-hysteresis for longitudinal planner

2026-06-11 02:24:44 +08:00 · 2025-06-10 13:39:40 -04:00
4 changed files with 5 additions and 3 deletions
--- a/cereal/custom.capnp
+++ b/cereal/custom.capnp
@@ -224,6 +224,7 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
 }

 struct CarStateSP @0xb86e6369214c01c8 {
+  vEgoCluster @0 :Float32;    # best estimate of speed shown on car's instrument cluster
 }

 struct LiveMapDataSP @0xf416ec09499d9d19 {
--- a/2
+++ b/2
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -103,7 +103,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
    else:
      accel_coast = ACCEL_MAX

-    v_ego = sm['carState'].vEgo
+    v_ego = sm['carStateSP'].vEgoCluster
    v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
    v_cruise = v_cruise_kph * CV.KPH_TO_MS
    v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET
--- a/selfdrive/controls/plannerd.py
+++ b/selfdrive/controls/plannerd.py
@@ -19,7 +19,8 @@ def main():
  ldw = LaneDepartureWarning()
  longitudinal_planner = LongitudinalPlanner(CP)
  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
-  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'],
+  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
+                            'carStateSP'],
                           poll='modelV2')

  while True: