From c82a1a383000f1ee06547ec5c8e35878449e9d1e Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 24 Mar 2026 21:03:38 -0500 Subject: [PATCH] long planner --- panda/board/obj/gitversion.h | 2 +- panda/board/obj/version | 2 +- .../controls/lib/longitudinal_planner.py | 10 +++---- selfdrive/controls/radard.py | 28 +++++++---------- .../test/longitudinal_maneuvers/plant.py | 30 +++++++++++++++++-- 5 files changed, 45 insertions(+), 27 deletions(-) diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index c3d5f18cd..691904b53 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,2 +1,2 @@ extern const uint8_t gitversion[19]; -const uint8_t gitversion[19] = "DEV-10948af7-DEBUG"; +const uint8_t gitversion[19] = "DEV-a2ac4341-DEBUG"; diff --git a/panda/board/obj/version b/panda/board/obj/version index 3ed22c9c0..bd0eb287a 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-10948af7-DEBUG \ No newline at end of file +DEV-a2ac4341-DEBUG \ No newline at end of file diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 3c8efd509..9d44a0f36 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -274,11 +274,11 @@ class LongitudinalPlanner: stop_distance=getattr(frogpilot_toggles, "stop_distance", 6.0)) self.mpc.set_accel_limits(accel_clip[0], accel_clip[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - try: - tracking_lead = bool(sm['frogpilotPlan'].trackingLead) - except AttributeError: - # Backward-compatible fallback for stale schema instances. - tracking_lead = sm['frogpilotPlan'].desiredFollowDistance > 0 + # Keep StarPilot behavior as the primary gate: desired follow distance implies + # lead tracking in ACC mode. trackingLead is treated as an additive hint. + tracking_lead = sm['frogpilotPlan'].desiredFollowDistance > 0 + if hasattr(sm['frogpilotPlan'], 'trackingLead'): + tracking_lead = tracking_lead or bool(sm['frogpilotPlan'].trackingLead) self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, sm['frogpilotPlan'].dangerFactor, sm['frogpilotPlan'].tFollow, personality=sm['selfdriveState'].personality, tracking_lead=tracking_lead) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 366cd2aa5..c29f2b824 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -18,7 +18,7 @@ from openpilot.frogpilot.common.frogpilot_variables import THRESHOLD, get_frogpi # Default lead acceleration decay set to 50% at 1s -_LEAD_ACCEL_TAU = 0.6 +_LEAD_ACCEL_TAU = 1.5 # radar tracks SPEED, ACCEL = 0, 1 # Kalman filter states enum @@ -68,7 +68,7 @@ class Track: self.leadTrackID = 0 - self.radarfulFilter = FirstOrderFilter(0, 0.5, self.K_A[0][1]) + self.radarfulFilter = FirstOrderFilter(0, 1, self.K_A[0][1]) def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float): # relative values, copy @@ -87,7 +87,7 @@ class Track: # Learn if constant acceleration if abs(self.aLeadK) < 0.5: - self.aLeadTau.x = min(max(self.aLeadTau.x, 1e-2) * 1.1, _LEAD_ACCEL_TAU) + self.aLeadTau.x = _LEAD_ACCEL_TAU else: self.aLeadTau.update(0.0) @@ -139,14 +139,11 @@ class Track: else: return self.leadRight - def potential_far_lead(self, standstill: bool, model_data: capnp._DynamicStructReader): - if standstill or self.vLead < 1 or abs(self.yRel) > 1: - return False - + def potential_far_lead(self, lead_msg: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader): left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y) right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y) - if left_lane < -self.yRel < right_lane: + if left_lane < -self.yRel < right_lane and self.dRel < model_data.position.x[-1] and self.vLeadK > 1: self.radarfulFilter.update(1) return True else: @@ -197,9 +194,6 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float): - prev_aLeadK = getattr(get_RadarState_from_vision, "prev_aLeadK", 0.0) - blended_aLeadK = 0.8 * float(lead_msg.a[0]) + 0.2 * prev_aLeadK - get_RadarState_from_vision.prev_aLeadK = blended_aLeadK lead_v_rel_pred = lead_msg.v[0] - model_v_ego return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), @@ -207,7 +201,7 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa "vRel": float(lead_v_rel_pred), "vLead": float(v_ego + lead_v_rel_pred), "vLeadK": float(v_ego + lead_v_rel_pred), - "aLeadK": blended_aLeadK, + "aLeadK": float(lead_msg.a[0]), "aLeadTau": 0.3, "fcw": False, "modelProb": float(lead_msg.prob), @@ -218,7 +212,7 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, - model_v_ego: float, model_data: capnp._DynamicStructReader, standstill: bool, + model_v_ego: float, model_data: capnp._DynamicStructReader, frogpilot_plan: capnp._DynamicStructReader, frogpilot_toggles: SimpleNamespace, low_speed_override: bool = True) -> dict[str, Any]: # Determine leads, this is where the essential logic happens @@ -243,7 +237,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn lead_dict = closest_track.get_RadarState() if low_speed_override and not lead_dict['status'] and len(tracks) > 0: - far_lead_tracks = [c for c in tracks.values() if c.potential_far_lead(standstill, model_data) and c.radarfulFilter.x >= THRESHOLD] + far_lead_tracks = [c for c in tracks.values() if c.potential_far_lead(lead_msg, model_data) and c.radarfulFilter.x >= THRESHOLD] if len(far_lead_tracks) > 0: closest_track = min(far_lead_tracks, key=lambda c: c.dRel) lead_dict = closest_track.get_RadarState() @@ -332,10 +326,8 @@ class RadarD: model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'], - sm['carState'].standstill, sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], - sm['carState'].standstill, sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=False) + self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'], sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=True) + self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=False) # FrogPilot variables if self.ready and (self.frogpilot_toggles.adjacent_lead_tracking or self.frogpilot_toggles.human_lane_changes): diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index b8c6adb43..19027b6e8 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -1,9 +1,11 @@ #!/usr/bin/env python3 import time import numpy as np +from types import SimpleNamespace from cereal import log import cereal.messaging as messaging +from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN from openpilot.common.realtime import Ratekeeper, DT_MDL from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.modeld.constants import ModelConstants @@ -52,6 +54,13 @@ class Plant: from opendbc.car.honda.interface import CarInterface self.planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.HONDA_CIVIC), init_v=self.speed) + self.frogpilot_toggles = SimpleNamespace( + taco_tune=False, + model_version=None, + stop_distance=6.0, + longitudinalActuatorDelay=0.2, + vEgoStopping=0.5, + ) @property def current_time(self): @@ -127,14 +136,31 @@ class Plant: car_control.carControl.orientationNED = [0., float(pitch), 0.] # ******** get controlsState messages for plotting *** + frogpilot_plan = SimpleNamespace( + vCruise=float(v_cruise), + minAcceleration=float(ACCEL_MIN), + maxAcceleration=float(ACCEL_MAX), + cscControllingSpeed=False, + disableThrottle=False, + accelerationJerk=5.0, + dangerJerk=5.0, + speedJerk=5.0, + trackingLead=bool(status), + desiredFollowDistance=float(d_rel), + dangerFactor=1.0, + tFollow=1.45, + forcingStopLength=2, + ) + sm = {'radarState': radar.radarState, 'carState': car_state.carState, 'carControl': car_control.carControl, 'controlsState': control.controlsState, 'selfdriveState': ss.selfdriveState, 'liveParameters': lp.liveParameters, - 'modelV2': model.modelV2} - self.planner.update(sm) + 'modelV2': model.modelV2, + 'frogpilotPlan': frogpilot_plan} + self.planner.update(sm, self.frogpilot_toggles) self.acceleration = self.planner.output_a_target self.speed = self.speed + self.acceleration * self.ts self.should_stop = self.planner.output_should_stop