mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-22 09:42:10 +08:00
Code Issues Packages Projects Releases Wiki Activity

Refactor

Browse Source
This commit is contained in:
firestar5683
2026-07-20 12:23:38 -05:00
parent f265aa3f7b
commit 4ac7fcaf5c
10 changed files with 1239 additions and 286 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/speed_limit_vision/build_localized_bookmark_review_queue.py
+11 -1
View File
@@ -95,6 +95,14 @@ def queue_row(row: dict[str, str]) -> dict[str, str]:
read_sources = "model"
if row.get("full_detection", ""):
read_sources += ";full_detection"
if row.get("event_type", "") == "visionPublish":
read_sources += ";logged_vision_publish"
review_reasons = ["corrected_source_timing"]
if row.get("event_type", "") == "visionPublish":
review_reasons.extend(("route_vision_publish", f"published_{row.get('published_speed', '')}"))
else:
review_reasons.append("route_bookmark")
item = dict.fromkeys(FIELDNAMES, "")
item.update({
@@ -121,8 +129,10 @@ def queue_row(row: dict[str, str]) -> dict[str, str]:
"read_sources": read_sources,
"read_support_count": "1",
"is_regulatory": row.get("is_regulatory", ""),
"map_current_speed_limit_mph": row.get("map_speed", ""),
"map_next_speed_limit_mph": row.get("next_speed", ""),
"review_priority": row.get("score", ""),
"review_reasons": "route_bookmark;corrected_source_timing",
"review_reasons": ";".join(review_reasons),
})
return item
scripts/speed_limit_vision/localize_bookmark_signs.py
+20 -9
View File
@@ -56,19 +56,23 @@ def configure_models(models_dir: Path | None):
slv.US_CLASSIFIER_MODEL_PATH = classifier_path
def iter_video_samples(clip_path: Path, start_s: float, end_s: float, sample_every: float):
def iter_video_samples(clip_path: Path, start_s: float, end_s: float, sample_every: float, seek: bool = False):
capture = cv2.VideoCapture(str(clip_path))
fps = common.source_video_fps(clip_path, capture.get(cv2.CAP_PROP_FPS))
start_frame = max(int(start_s * fps), 0)
end_frame = max(int(end_s * fps), start_frame)
frame_index = 0
while frame_index < start_frame:
ok, _ = capture.read()
if not ok:
capture.release()
return
frame_index += 1
if seek:
capture.set(cv2.CAP_PROP_POS_FRAMES, start_frame)
frame_index = max(round(capture.get(cv2.CAP_PROP_POS_FRAMES)), 0)
else:
while frame_index < start_frame:
ok, _ = capture.read()
if not ok:
capture.release()
return
frame_index += 1
next_sample_s = start_s
while frame_index <= end_frame:
@@ -86,7 +90,14 @@ def iter_video_samples(clip_path: Path, start_s: float, end_s: float, sample_eve
capture.release()
def iter_context_frames(clip_root: Path, window: ebl.BookmarkWindow, search_before: float, search_after: float, sample_every: float):
def iter_context_frames(
clip_root: Path,
window: ebl.BookmarkWindow,
search_before: float,
search_after: float,
sample_every: float,
seek: bool = False,
):
ranges: list[tuple[Path, float, float]] = []
start_s = window.segment_offset_s - search_before
end_s = window.segment_offset_s + search_after
@@ -102,7 +113,7 @@ def iter_context_frames(clip_root: Path, window: ebl.BookmarkWindow, search_befo
ranges.append((current_clip, max(start_s, 0.0), min(end_s, 60.0)))
for clip_path, range_start_s, range_end_s in ranges:
for source_time_s, frame_bgr in iter_video_samples(clip_path, range_start_s, range_end_s, sample_every):
for source_time_s, frame_bgr in iter_video_samples(clip_path, range_start_s, range_end_s, sample_every, seek=seek):
if clip_path.parent.name.endswith(f"--{window.segment - 1}"):
relative_time_s = source_time_s - 60.0
else:
scripts/speed_limit_vision/mine_connect_route_bookmarks.py
+102 -37
View File
@@ -30,6 +30,7 @@ else:
DEFAULT_WORKSPACE = Path("/Volumes/T5/starpilot_speed_limit/workspace/speed_limit_training_clean")
ROUTE_ID_RE = re.compile(r"([0-9a-f]{16})/([^/]+)")
BOOKMARK_TYPES = ("bookmarkButton", "userBookmark")
MS_TO_MPH = 2.2369362920544
def parse_args() -> argparse.Namespace:
@@ -38,6 +39,12 @@ def parse_args() -> argparse.Namespace:
parser.add_argument("--workspace", type=Path, default=DEFAULT_WORKSPACE, help="Training workspace root.")
parser.add_argument("--clip-root", type=Path, default=preferred_clip_root(), help="Downloaded route clip root.")
parser.add_argument("--output-dir", type=Path, help="Output directory. Defaults to <workspace>/review/connect_route_bookmarks.")
parser.add_argument(
"--event-types",
choices=("bookmark", "vision", "both"),
default="bookmark",
help="Mine user bookmarks, logged Vision publications, or both.",
)
parser.add_argument("--models-dir", type=Path, help="Optional directory containing speed_limit_us_detector.onnx and speed_limit_us_value_classifier.onnx.")
parser.add_argument("--lead-in", type=float, default=7.0, help="Seconds before each bookmark to sample into review sheets.")
parser.add_argument("--sample-every", type=float, default=0.5, help="Seconds between sampled lead-in frames.")
@@ -47,6 +54,8 @@ def parse_args() -> argparse.Namespace:
parser.add_argument("--localize-sample-every", type=float, default=0.25, help="Seconds between frames while searching for the best sign candidate.")
parser.add_argument("--top-k", type=int, default=1, help="Number of localized candidates to keep per bookmark.")
parser.add_argument("--model-only", action="store_true", help="Match the production detector/classifier path without crop OCR.")
parser.add_argument("--seek-sampling", action="store_true", help="Seek directly to each context window instead of decoding from clip start.")
parser.add_argument("--skip-contact-sheets", action="store_true", help="Skip lead-in frames and contact sheets when only a review queue is needed.")
parser.add_argument("--overwrite", action="store_true", help="Overwrite any existing outputs.")
return parser.parse_args()
@@ -67,15 +76,20 @@ def read_log_bytes(path: Path) -> bytes:
return path.read_bytes()
def load_route_bookmarks(clip_root: Path, log_id: str) -> list[dict]:
def load_route_bookmarks(clip_root: Path, log_id: str, event_types: str = "bookmark") -> list[dict]:
segment_dirs = sorted(clip_root.glob(f"{log_id}--*"), key=lambda path: int(path.name.rsplit("--", 1)[-1]))
if not segment_dirs:
raise FileNotFoundError(f"No downloaded segments found for {log_id} under {clip_root}")
route_start_monotime = None
raw_events: list[tuple[float, str]] = []
raw_events: list[dict] = []
last_vision_speed: int | None = None
for segment_dir in segment_dirs:
log_path = next((segment_dir / name for name in ("rlog.zst", "rlog.bz2", "qlog.zst", "qlog.bz2") if (segment_dir / name).exists()), None)
log_names = (
("qlog.zst", "qlog.bz2", "rlog.zst", "rlog.bz2")
if event_types == "vision"
else ("rlog.zst", "rlog.bz2", "qlog.zst", "qlog.bz2")
)
log_path = next((segment_dir / name for name in log_names if (segment_dir / name).exists()), None)
if log_path is None:
continue
@@ -86,35 +100,66 @@ def load_route_bookmarks(clip_root: Path, log_id: str) -> list[dict]:
continue
if not events:
continue
if route_start_monotime is None:
route_start_monotime = events[0].logMonoTime
segment = int(segment_dir.name.rsplit("--", 1)[-1])
road_camera_times = [event.logMonoTime for event in events if event.which() == "roadCameraState"]
fallback_times = [event.logMonoTime for event in events if event.which() != "initData"]
segment_start_monotime = min(road_camera_times or fallback_times or [events[0].logMonoTime])
for event in events:
event_type = event.which()
if event_type not in BOOKMARK_TYPES:
if event_types in ("bookmark", "both") and event_type in BOOKMARK_TYPES:
segment_offset_s = max((event.logMonoTime - segment_start_monotime) / 1e9, 0.0)
route_time_s = segment * 60.0 + segment_offset_s
raw_events.append({
"event_type": event_type,
"route_time_s": route_time_s,
"segment": segment,
"segment_offset_s": segment_offset_s,
"published_speed": "",
"map_speed": "",
"mapbox_speed": "",
"next_speed": "",
})
if event_types not in ("vision", "both") or event_type != "starpilotPlan":
continue
plan = event.starpilotPlan
source = str(plan.slcSpeedLimitSource)
if source != "Vision":
last_vision_speed = None
continue
route_time_s = (event.logMonoTime - route_start_monotime) / 1e9
raw_events.append((route_time_s, event_type))
raw_events.sort(key=lambda item: item[0])
published_speed = round(float(plan.slcSpeedLimit) * MS_TO_MPH)
if published_speed <= 0 or published_speed == last_vision_speed:
continue
last_vision_speed = published_speed
segment_offset_s = max((event.logMonoTime - segment_start_monotime) / 1e9, 0.0)
raw_events.append({
"event_type": "visionPublish",
"route_time_s": segment * 60.0 + segment_offset_s,
"segment": segment,
"segment_offset_s": segment_offset_s,
"published_speed": published_speed,
"map_speed": round(float(plan.slcMapSpeedLimit) * MS_TO_MPH),
"mapbox_speed": round(float(plan.slcMapboxSpeedLimit) * MS_TO_MPH),
"next_speed": round(float(plan.slcNextSpeedLimit) * MS_TO_MPH),
})
raw_events.sort(key=lambda item: item["route_time_s"])
deduped: list[dict] = []
for route_time_s, event_type in raw_events:
if deduped and abs(route_time_s - deduped[-1]["route_time_s"]) <= 0.5:
if event_type == "userBookmark":
deduped[-1]["event_type"] = event_type
deduped[-1]["route_time_s"] = route_time_s
deduped[-1]["segment"] = max(int(route_time_s // 60.0), 0)
deduped[-1]["segment_offset_s"] = route_time_s - deduped[-1]["segment"] * 60.0
for raw_event in raw_events:
if deduped and abs(raw_event["route_time_s"] - deduped[-1]["route_time_s"]) <= 0.5:
if raw_event["event_type"] == "userBookmark":
deduped[-1].update(raw_event)
elif raw_event["event_type"] == "visionPublish":
deduped[-1].update({
key: raw_event[key]
for key in ("published_speed", "map_speed", "mapbox_speed", "next_speed")
})
if deduped[-1]["event_type"] not in BOOKMARK_TYPES:
deduped[-1]["event_type"] = raw_event["event_type"]
continue
segment = max(int(route_time_s // 60.0), 0)
segment_offset_s = route_time_s - segment * 60.0
deduped.append({
"event_type": event_type,
"route_time_s": route_time_s,
"segment": segment,
"segment_offset_s": segment_offset_s,
})
deduped.append(raw_event)
return deduped
@@ -140,6 +185,11 @@ def write_localized_manifest(path: Path, rows: list[dict]) -> None:
"frame_path",
"crop_path",
"box",
"event_type",
"published_speed",
"map_speed",
"mapbox_speed",
"next_speed",
])
writer.writeheader()
writer.writerows(rows)
@@ -151,6 +201,13 @@ def fmt_detection(result) -> str:
return f"{result[0]}@{result[1]:.3f}"
def manifest_path(path: Path, workspace: Path) -> str:
try:
return str(path.relative_to(workspace))
except ValueError:
return str(path)
def main() -> int:
args = parse_args()
workspace = resolve_workspace(args.workspace)
@@ -170,12 +227,12 @@ def main() -> int:
for raw_route in args.routes:
dongle_id, log_id = parse_route_id(raw_route)
session_id = f"connect_{dongle_id}_{log_id}"
bookmarks = load_route_bookmarks(clip_root, log_id)
bookmarks = load_route_bookmarks(clip_root, log_id, args.event_types)
if not bookmarks:
print(f"{raw_route}: no bookmark events found in downloaded rlogs")
continue
print(f"{raw_route}: found {len(bookmarks)} bookmark(s)")
print(f"{raw_route}: found {len(bookmarks)} event(s)")
for bookmark_number, bookmark in enumerate(bookmarks, start=1):
window = BookmarkWindow(
bookmark_number=bookmark_number,
@@ -186,13 +243,15 @@ def main() -> int:
spans_previous_segment=float(bookmark["segment_offset_s"]) - args.lead_in < 0.0,
)
sampled_frames = extract_window_frames({
"route": log_id,
"segment": window.segment,
"segmentOffsetS": window.segment_offset_s,
"leadinStartS": window.leadin_start_s,
"spansPreviousSegment": window.spans_previous_segment,
}, clip_root, args.sample_every, args.max_samples)
sampled_frames = []
if not args.skip_contact_sheets:
sampled_frames = extract_window_frames({
"route": log_id,
"segment": window.segment,
"segmentOffsetS": window.segment_offset_s,
"leadinStartS": window.leadin_start_s,
"spansPreviousSegment": window.spans_previous_segment,
}, clip_root, args.sample_every, args.max_samples)
contact_sheet_frames = []
contact_sheet_labels = []
@@ -213,8 +272,8 @@ def main() -> int:
"segment": window.segment,
"segment_offset_s": f"{window.segment_offset_s:.3f}",
"sample_offset_s": f"{sample['relative_offset_s']:.3f}",
"frame_path": str(frame_path.relative_to(workspace)),
"contact_sheet_path": str(contact_sheet_path.relative_to(workspace)),
"frame_path": manifest_path(frame_path, workspace),
"contact_sheet_path": manifest_path(contact_sheet_path, workspace),
"source_video_path": str(sample["source_video"]),
"event_type": bookmark["event_type"],
"route_time_s": f"{bookmark['route_time_s']:.3f}",
@@ -230,6 +289,7 @@ def main() -> int:
args.search_before,
args.search_after,
args.localize_sample_every,
seek=args.seek_sampling,
):
scored = score_frame(daemon, frame_bgr, use_ocr=not args.model_only)
if scored is None:
@@ -271,6 +331,11 @@ def main() -> int:
"frame_path": str(frame_path),
"crop_path": str(crop_path),
"box": ",".join(str(value) for value in scored["box"]),
"event_type": bookmark["event_type"],
"published_speed": bookmark["published_speed"],
"map_speed": bookmark["map_speed"],
"mapbox_speed": bookmark["mapbox_speed"],
"next_speed": bookmark["next_speed"],
})
ensure_dir(leadin_manifest_path.parent)
selfdrive/controls/lib/longcontrol.py
+26 -203
View File
@@ -5,8 +5,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
from openpilot.common.pid import PIDController
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.common.filter_simple import FirstOrderFilter
from opendbc.car.gm.values import CAR, CarControllerParams, GMFlags
from openpilot.starpilot.common.testing_grounds import testing_ground
from openpilot.selfdrive.controls.lib.longcontrol_vehicle_tunes import LongControlVehicleTuning
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
clip = np.clip
@@ -17,8 +16,7 @@ STOPPING_RELEASE_STRONG_ACCEL = 0.45
MOVING_STOP_FOLLOW_MIN_GAP = 0.25
NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35
NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40
BOLT_ACC_PEDAL_REGEN_LIMIT_BP = [0.0, 1.5, 4.0, 8.0, 15.0, 30.0]
BOLT_ACC_PEDAL_REGEN_LIMIT_V = [-0.93, -1.28, -1.98, -2.58, -2.86, -2.95]
MODE_TRANSITION_MAX_DECEL = 4.0
LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -32,55 +30,6 @@ def apply_deadzone(error, deadzone):
return error
def get_bolt_acc_pedal_friction_bias(output_accel, a_target, v_ego):
if output_accel >= -0.05 or a_target >= -0.80 or v_ego <= 5.0:
return 0.0
authority_gap = max(0.0, abs(a_target) - abs(output_accel))
if authority_gap <= 0.25:
return 0.0
speed_factor = interp(v_ego, [5.0, 10.0, 15.0, 25.0], [0.0, 0.55, 0.85, 1.0])
max_bias = interp(abs(a_target), [0.8, 1.4, 2.2, 3.5], [0.0, 0.14, 0.42, 0.70])
return float(min(authority_gap * 0.30, max_bias) * speed_factor)
def get_bolt_acc_pedal_friction_floor(a_target, v_ego, pedal_regen_limit):
if v_ego <= 5.0 or a_target >= (pedal_regen_limit - 0.05):
return None
friction_request = max(0.0, pedal_regen_limit - a_target)
if friction_request <= 0.10:
return None
speed_factor = interp(v_ego, [5.0, 8.0, 12.0, 18.0, 25.0], [0.0, 0.45, 0.75, 0.90, 1.0])
demand_factor = interp(friction_request, [0.10, 0.25, 0.50, 0.90, 1.30], [0.0, 0.22, 0.50, 0.78, 1.0])
floor_fraction = float(clip(speed_factor * demand_factor, 0.0, 1.0))
return float(pedal_regen_limit - (friction_request * floor_fraction))
def get_bolt_acc_pedal_feedforward_gain(feedforward_gain, a_target, v_ego, pedal_regen_limit, last_output_accel):
effective_gain = feedforward_gain
if a_target >= 0.0:
return effective_gain
restore = 0.0
if a_target < pedal_regen_limit:
friction_gap = pedal_regen_limit - a_target
restore = float(interp(friction_gap, [0.0, 0.25, 0.75], [0.0, 0.6, 1.0]))
if v_ego > 5.0 and a_target < -1.10:
authority_gap = max(0.0, abs(a_target) - abs(min(last_output_accel, 0.0)))
target_restore = float(interp(abs(a_target), [1.1, 1.6, 2.2, 3.0], [0.0, 0.25, 0.55, 1.0]))
gap_restore = float(interp(authority_gap, [0.2, 0.6, 1.0, 1.6], [0.0, 0.25, 0.60, 1.0]))
speed_factor = float(interp(v_ego, [5.0, 8.0, 12.0, 18.0], [0.0, 0.35, 0.75, 1.0]))
restore = max(restore, max(target_restore, gap_restore) * speed_factor)
return float(feedforward_gain + ((1.0 - feedforward_gain) * clip(restore, 0.0, 1.0)))
def long_control_state_trans(CP, active, long_control_state, v_ego,
should_stop, brake_pressed, cruise_standstill, starpilot_toggles,
allow_stopping_release=True):
@@ -174,26 +123,8 @@ class LongControl:
self.v_pid = 0.0
self._mode_setup()
self.last_output_accel = 0.0
self.last_a_target = 0.0
self.integrator_hold_frames = 0
self.stop_release_counter = 0
self.is_gm_pedal_long = bool(
CP.brand == "gm" and CP.enableGasInterceptorDEPRECATED and (CP.flags & GMFlags.PEDAL_LONG.value)
)
self.is_volt = bool(
CP.brand == "gm" and str(CP.carFingerprint).startswith("CHEVROLET_VOLT")
)
self.is_gm_stock_truck = bool(
CP.brand == "gm" and
getattr(CP, "carFingerprint", None) in (CAR.CHEVROLET_SILVERADO, CAR.CHEVROLET_SILVERADO_CC) and
not CP.enableGasInterceptorDEPRECATED
)
self.is_bolt_acc_pedal_friction_car = bool(
CP.brand == "gm" and
CP.enableGasInterceptorDEPRECATED and
getattr(CP, "carFingerprint", None) == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL and
(CP.flags & GMFlags.PEDAL_LONG.value)
)
self.vehicle_tuning = LongControlVehicleTuning(CP)
def update_mpc_mode(self, experimental_mode):
new_mode = 'blended' if experimental_mode else 'acc'
@@ -224,8 +155,7 @@ class LongControl:
def reset(self, preserve_stop_release=False):
self.pid.reset()
self.last_a_target = 0.0
self.integrator_hold_frames = 0
self.vehicle_tuning.reset()
if not preserve_stop_release:
self.stop_release_counter = 0
@@ -269,45 +199,6 @@ class LongControl:
follow_step = interp(CS.vEgo, [follow_min_speed, 3.0, 6.0, 10.0], [0.02, 0.03, 0.05, 0.07])
return max(float(a_target), output_accel - float(follow_step))
def _get_pedal_long_freeze(self, a_target, error, v_ego, accel_limits):
volt_test_tune_handoff = self.is_volt and testing_ground.use_2
if not self.is_gm_pedal_long and not volt_test_tune_handoff:
self.last_a_target = a_target
self.integrator_hold_frames = 0
return False
if self.is_gm_pedal_long:
handoff_threshold = interp(v_ego, [0.0, 4.0, 12.0, 25.0], [0.35, 0.45, 0.55, 0.70])
hold_frames = int(round(interp(v_ego, [0.0, 4.0, 12.0, 25.0], [25.0, 20.0, 14.0, 10.0])))
else:
handoff_threshold = interp(v_ego, [0.0, 4.0, 12.0, 25.0], [0.24, 0.30, 0.38, 0.48])
hold_frames = int(round(interp(v_ego, [0.0, 4.0, 12.0, 25.0], [12.0, 10.0, 8.0, 6.0])))
if abs(a_target - self.last_a_target) > handoff_threshold:
self.integrator_hold_frames = max(self.integrator_hold_frames, hold_frames)
self.last_a_target = a_target
if self.integrator_hold_frames > 0:
self.integrator_hold_frames -= 1
sat_buffer = 0.03
at_neg_sat = self.last_output_accel <= (accel_limits[0] + sat_buffer)
at_pos_sat = self.last_output_accel >= (accel_limits[1] - sat_buffer)
sat_pushing_lower = at_neg_sat and error < -0.05
sat_pushing_upper = at_pos_sat and error > 0.05
return self.integrator_hold_frames > 0 or sat_pushing_lower or sat_pushing_upper
def _shape_volt_test_tune_integrator(self, error, v_ego):
if not (self.is_volt and testing_ground.use_2):
return
# Bleed stale I quickly when the target reverses against stored integrator.
if self.pid.i * error < 0.0 and abs(error) > 0.05:
bleed = interp(v_ego, [0.0, 4.0, 12.0, 25.0], [0.82, 0.86, 0.90, 0.94])
self.pid.i *= bleed
def _trim_positive_overshoot_integrator(self, a_target, error, CS):
if self.pid.i <= 0.0:
return
@@ -322,74 +213,6 @@ class LongControl:
bleed = interp(abs(error), [0.25, 0.75, 1.5], [0.55, 0.25, 0.0])
self.pid.i *= bleed
def _trim_gm_truck_positive_hold_integrator(self, a_target, error, CS):
if not self.is_gm_stock_truck or self.pid.i <= 0.0:
return
if self.last_output_accel <= 0.10:
return
light_accel_threshold = float(interp(CS.vEgo, [8.0, 15.0, 25.0], [0.03, 0.06, 0.10]))
if a_target > light_accel_threshold:
return
if CS.vEgo <= NEGATIVE_TARGET_CREEP_GUARD_SPEED and a_target > -NEGATIVE_TARGET_CREEP_GUARD_DECEL:
return
# Stock-ACC trucks are especially sensitive to hanging onto light positive
# torque after the planner has already crossed back to coast or mild decel.
# Bleed stale positive I faster in that narrow window so the command can
# settle instead of wobbling the converter lock state.
authority_mismatch = self.last_output_accel - max(a_target, 0.0)
if authority_mismatch <= 0.08 and error > -0.08:
return
target_factor = float(interp(a_target, [-0.30, -0.10, -0.02, light_accel_threshold], [0.20, 0.35, 0.60, 0.98]))
if error < -0.20:
target_factor *= 0.75
self.pid.i *= target_factor
def _trim_gm_truck_negative_hold_integrator(self, a_target, error, CS):
if not self.is_gm_stock_truck or self.pid.i >= -0.02:
return
if CS.vEgo < 12.0 or a_target <= -0.85:
return
if error <= 0.04:
return
authority_mismatch = float(a_target) - float(self.last_output_accel)
if authority_mismatch <= 0.10:
return
release = float(interp(
max(authority_mismatch, error),
[0.10, 0.25, 0.50],
[0.0008, 0.0020, 0.0040],
))
self.pid.i = min(0.0, self.pid.i + release)
def _apply_pedal_long_brake_bias(self, output_accel, a_target, CS):
if not self.is_gm_pedal_long:
return output_accel
if output_accel >= -0.05 or a_target >= -0.80:
return output_accel
if CS.vEgo <= 5.0:
return output_accel
authority_gap = max(0.0, abs(a_target) - abs(output_accel))
if self.is_bolt_acc_pedal_friction_car:
pedal_regen_limit = float(interp(CS.vEgo, BOLT_ACC_PEDAL_REGEN_LIMIT_BP, BOLT_ACC_PEDAL_REGEN_LIMIT_V))
bias = get_bolt_acc_pedal_friction_bias(output_accel, a_target, CS.vEgo)
floor = get_bolt_acc_pedal_friction_floor(a_target, CS.vEgo, pedal_regen_limit)
if floor is not None:
bias = max(bias, output_accel - floor)
return output_accel - float(max(bias, 0.0))
if authority_gap <= 0.40:
return output_accel
speed_factor = interp(CS.vEgo, [5.0, 12.0, 25.0], [0.0, 0.7, 1.0])
max_bias = interp(abs(a_target), [0.8, 2.0, 3.5], [0.0, 0.10, 0.20])
bias = min(authority_gap * 0.12, max_bias) * speed_factor
return output_accel - float(bias)
@staticmethod
def _cap_positive_output_on_negative_target(output_accel, a_target, error, CS):
if output_accel <= 0.0:
@@ -404,17 +227,6 @@ class LongControl:
positive_cap = interp(a_target, [-1.5, -0.6, -0.1], [0.0, 0.0, 0.05])
return min(output_accel, float(positive_cap))
def _get_longitudinal_feedforward(self, a_target, v_ego):
feedforward = a_target * self.feedforward_gain
if not self.is_bolt_acc_pedal_friction_car or a_target >= 0.0:
return feedforward
pedal_regen_limit = float(interp(v_ego, BOLT_ACC_PEDAL_REGEN_LIMIT_BP, BOLT_ACC_PEDAL_REGEN_LIMIT_V))
effective_gain = get_bolt_acc_pedal_feedforward_gain(
self.feedforward_gain, a_target, v_ego, pedal_regen_limit, self.last_output_accel,
)
return a_target * effective_gain
def update(self, active, CS, a_target, should_stop, accel_limits, starpilot_toggles, has_lead=False):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
self.pid.neg_limit = accel_limits[0]
@@ -447,16 +259,24 @@ class LongControl:
else: # LongCtrlState.pid
error = a_target - CS.aEgo
self.update_mpc_mode(self.experimental_mode)
self._shape_volt_test_tune_integrator(error, CS.vEgo)
self.vehicle_tuning.shape_volt_test_tune_integrator(self.pid, error, CS.vEgo)
self._trim_positive_overshoot_integrator(a_target, error, CS)
self._trim_gm_truck_positive_hold_integrator(a_target, error, CS)
self._trim_gm_truck_negative_hold_integrator(a_target, error, CS)
feedforward = self._get_longitudinal_feedforward(a_target, CS.vEgo)
freeze_integrator = self._get_pedal_long_freeze(a_target, error, CS.vEgo, accel_limits)
self.vehicle_tuning.trim_gm_truck_positive_hold_integrator(
self.pid, self.last_output_accel, a_target, error, CS,
)
self.vehicle_tuning.trim_gm_truck_negative_hold_integrator(
self.pid, self.last_output_accel, a_target, error, CS,
)
feedforward = self.vehicle_tuning.get_longitudinal_feedforward(
self.feedforward_gain, self.last_output_accel, a_target, CS.vEgo,
)
freeze_integrator = self.vehicle_tuning.get_integrator_freeze(
self.last_output_accel, a_target, error, CS.vEgo, accel_limits,
)
raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=feedforward,
freeze_integrator=freeze_integrator)
raw_output_accel = self._cap_positive_output_on_negative_target(raw_output_accel, a_target, error, CS)
raw_output_accel = self._apply_pedal_long_brake_bias(raw_output_accel, a_target, CS)
raw_output_accel = self.vehicle_tuning.apply_pedal_long_brake_bias(raw_output_accel, a_target, CS)
if self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended':
@@ -464,7 +284,7 @@ class LongControl:
progress = min(1.0, self.mode_transition_timer / self.mode_transition_duration)
# Soften transition at low urgency, but keep sharp for high decel
# 20% smoother for chill decel (lower exponent)
urgency = abs(raw_output_accel / CarControllerParams.ACCEL_MIN)
urgency = abs(raw_output_accel / -MODE_TRANSITION_MAX_DECEL)
urgency_smooth = min(1.0, urgency ** 0.4) # 20% smoother for chill decel
blend_factor = 1.0 - (1.0 - progress) * (1.0 - urgency_smooth)
output_accel = self.last_output_accel + (raw_output_accel - self.last_output_accel) * blend_factor
@@ -480,8 +300,7 @@ class LongControl:
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
self.last_a_target = 0.0
self.integrator_hold_frames = 0
self.vehicle_tuning.reset()
def update_old_long(self, active, CS, long_plan, accel_limits, t_since_plan, starpilot_toggles):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
@@ -536,8 +355,12 @@ class LongControl:
deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBP, self.CP.longitudinalTuning.deadzoneV)
error = self.v_pid - CS.vEgo
error_deadzone = apply_deadzone(error, deadzone)
freeze_integrator = prevent_overshoot or self._get_pedal_long_freeze(a_target, error_deadzone, CS.vEgo, accel_limits)
feedforward = self._get_longitudinal_feedforward(a_target, CS.vEgo)
freeze_integrator = prevent_overshoot or self.vehicle_tuning.get_integrator_freeze(
self.last_output_accel, a_target, error_deadzone, CS.vEgo, accel_limits,
)
feedforward = self.vehicle_tuning.get_longitudinal_feedforward(
self.feedforward_gain, self.last_output_accel, a_target, CS.vEgo,
)
output_accel = self.pid.update(error_deadzone, speed=CS.vEgo,
feedforward=feedforward,
freeze_integrator=freeze_integrator)
selfdrive/controls/lib/longcontrol_vehicle_tunes.py
+201
View File
@@ -0,0 +1,201 @@
import numpy as np
from opendbc.car.gm.values import CAR, GMFlags
from openpilot.starpilot.common.testing_grounds import testing_ground
clip = np.clip
interp = np.interp
BOLT_ACC_PEDAL_REGEN_LIMIT_BP = [0.0, 1.5, 4.0, 8.0, 15.0, 30.0]
BOLT_ACC_PEDAL_REGEN_LIMIT_V = [-0.93, -1.28, -1.98, -2.58, -2.86, -2.95]
NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35
NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40
def get_bolt_acc_pedal_friction_bias(output_accel, a_target, v_ego):
if output_accel >= -0.05 or a_target >= -0.80 or v_ego <= 5.0:
return 0.0
authority_gap = max(0.0, abs(a_target) - abs(output_accel))
if authority_gap <= 0.25:
return 0.0
speed_factor = interp(v_ego, [5.0, 10.0, 15.0, 25.0], [0.0, 0.55, 0.85, 1.0])
max_bias = interp(abs(a_target), [0.8, 1.4, 2.2, 3.5], [0.0, 0.14, 0.42, 0.70])
return float(min(authority_gap * 0.30, max_bias) * speed_factor)
def get_bolt_acc_pedal_friction_floor(a_target, v_ego, pedal_regen_limit):
if v_ego <= 5.0 or a_target >= (pedal_regen_limit - 0.05):
return None
friction_request = max(0.0, pedal_regen_limit - a_target)
if friction_request <= 0.10:
return None
speed_factor = interp(v_ego, [5.0, 8.0, 12.0, 18.0, 25.0], [0.0, 0.45, 0.75, 0.90, 1.0])
demand_factor = interp(friction_request, [0.10, 0.25, 0.50, 0.90, 1.30], [0.0, 0.22, 0.50, 0.78, 1.0])
floor_fraction = float(clip(speed_factor * demand_factor, 0.0, 1.0))
return float(pedal_regen_limit - (friction_request * floor_fraction))
def get_bolt_acc_pedal_feedforward_gain(feedforward_gain, a_target, v_ego, pedal_regen_limit, last_output_accel):
effective_gain = feedforward_gain
if a_target >= 0.0:
return effective_gain
restore = 0.0
if a_target < pedal_regen_limit:
friction_gap = pedal_regen_limit - a_target
restore = float(interp(friction_gap, [0.0, 0.25, 0.75], [0.0, 0.6, 1.0]))
if v_ego > 5.0 and a_target < -1.10:
authority_gap = max(0.0, abs(a_target) - abs(min(last_output_accel, 0.0)))
target_restore = float(interp(abs(a_target), [1.1, 1.6, 2.2, 3.0], [0.0, 0.25, 0.55, 1.0]))
gap_restore = float(interp(authority_gap, [0.2, 0.6, 1.0, 1.6], [0.0, 0.25, 0.60, 1.0]))
speed_factor = float(interp(v_ego, [5.0, 8.0, 12.0, 18.0], [0.0, 0.35, 0.75, 1.0]))
restore = max(restore, max(target_restore, gap_restore) * speed_factor)
return float(feedforward_gain + ((1.0 - feedforward_gain) * clip(restore, 0.0, 1.0)))
class LongControlVehicleTuning:
def __init__(self, CP):
self.is_gm_pedal_long = bool(
CP.brand == "gm" and CP.enableGasInterceptorDEPRECATED and (CP.flags & GMFlags.PEDAL_LONG.value)
)
self.is_volt = bool(
CP.brand == "gm" and str(CP.carFingerprint).startswith("CHEVROLET_VOLT")
)
self.is_gm_stock_truck = bool(
CP.brand == "gm" and
getattr(CP, "carFingerprint", None) in (CAR.CHEVROLET_SILVERADO, CAR.CHEVROLET_SILVERADO_CC) and
not CP.enableGasInterceptorDEPRECATED
)
self.is_bolt_acc_pedal_friction_car = bool(
CP.brand == "gm" and
CP.enableGasInterceptorDEPRECATED and
getattr(CP, "carFingerprint", None) == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL and
(CP.flags & GMFlags.PEDAL_LONG.value)
)
self.reset()
def reset(self):
self.last_a_target = 0.0
self.integrator_hold_frames = 0
def get_integrator_freeze(self, last_output_accel, a_target, error, v_ego, accel_limits):
volt_test_tune_handoff = self.is_volt and testing_ground.use_2
if not self.is_gm_pedal_long and not volt_test_tune_handoff:
self.last_a_target = a_target
self.integrator_hold_frames = 0
return False
if self.is_gm_pedal_long:
handoff_threshold = interp(v_ego, [0.0, 4.0, 12.0, 25.0], [0.35, 0.45, 0.55, 0.70])
hold_frames = int(round(interp(v_ego, [0.0, 4.0, 12.0, 25.0], [25.0, 20.0, 14.0, 10.0])))
else:
handoff_threshold = interp(v_ego, [0.0, 4.0, 12.0, 25.0], [0.24, 0.30, 0.38, 0.48])
hold_frames = int(round(interp(v_ego, [0.0, 4.0, 12.0, 25.0], [12.0, 10.0, 8.0, 6.0])))
if abs(a_target - self.last_a_target) > handoff_threshold:
self.integrator_hold_frames = max(self.integrator_hold_frames, hold_frames)
self.last_a_target = a_target
if self.integrator_hold_frames > 0:
self.integrator_hold_frames -= 1
sat_buffer = 0.03
at_neg_sat = last_output_accel <= (accel_limits[0] + sat_buffer)
at_pos_sat = last_output_accel >= (accel_limits[1] - sat_buffer)
sat_pushing_lower = at_neg_sat and error < -0.05
sat_pushing_upper = at_pos_sat and error > 0.05
return self.integrator_hold_frames > 0 or sat_pushing_lower or sat_pushing_upper
def shape_volt_test_tune_integrator(self, pid, error, v_ego):
if not (self.is_volt and testing_ground.use_2):
return
if pid.i * error < 0.0 and abs(error) > 0.05:
bleed = interp(v_ego, [0.0, 4.0, 12.0, 25.0], [0.82, 0.86, 0.90, 0.94])
pid.i *= bleed
def trim_gm_truck_positive_hold_integrator(self, pid, last_output_accel, a_target, error, CS):
if not self.is_gm_stock_truck or pid.i <= 0.0:
return
if last_output_accel <= 0.10:
return
light_accel_threshold = float(interp(CS.vEgo, [8.0, 15.0, 25.0], [0.03, 0.06, 0.10]))
if a_target > light_accel_threshold:
return
if CS.vEgo <= NEGATIVE_TARGET_CREEP_GUARD_SPEED and a_target > -NEGATIVE_TARGET_CREEP_GUARD_DECEL:
return
authority_mismatch = last_output_accel - max(a_target, 0.0)
if authority_mismatch <= 0.08 and error > -0.08:
return
target_factor = float(interp(a_target, [-0.30, -0.10, -0.02, light_accel_threshold], [0.20, 0.35, 0.60, 0.98]))
if error < -0.20:
target_factor *= 0.75
pid.i *= target_factor
def trim_gm_truck_negative_hold_integrator(self, pid, last_output_accel, a_target, error, CS):
if not self.is_gm_stock_truck or pid.i >= -0.02:
return
if CS.vEgo < 12.0 or a_target <= -0.85:
return
if error <= 0.04:
return
authority_mismatch = float(a_target) - float(last_output_accel)
if authority_mismatch <= 0.10:
return
release = float(interp(
max(authority_mismatch, error),
[0.10, 0.25, 0.50],
[0.0008, 0.0020, 0.0040],
))
pid.i = min(0.0, pid.i + release)
def apply_pedal_long_brake_bias(self, output_accel, a_target, CS):
if not self.is_gm_pedal_long:
return output_accel
if output_accel >= -0.05 or a_target >= -0.80:
return output_accel
if CS.vEgo <= 5.0:
return output_accel
authority_gap = max(0.0, abs(a_target) - abs(output_accel))
if self.is_bolt_acc_pedal_friction_car:
pedal_regen_limit = float(interp(CS.vEgo, BOLT_ACC_PEDAL_REGEN_LIMIT_BP, BOLT_ACC_PEDAL_REGEN_LIMIT_V))
bias = get_bolt_acc_pedal_friction_bias(output_accel, a_target, CS.vEgo)
floor = get_bolt_acc_pedal_friction_floor(a_target, CS.vEgo, pedal_regen_limit)
if floor is not None:
bias = max(bias, output_accel - floor)
return output_accel - float(max(bias, 0.0))
if authority_gap <= 0.40:
return output_accel
speed_factor = interp(CS.vEgo, [5.0, 12.0, 25.0], [0.0, 0.7, 1.0])
max_bias = interp(abs(a_target), [0.8, 2.0, 3.5], [0.0, 0.10, 0.20])
bias = min(authority_gap * 0.12, max_bias) * speed_factor
return output_accel - float(bias)
def get_longitudinal_feedforward(self, feedforward_gain, last_output_accel, a_target, v_ego):
feedforward = a_target * feedforward_gain
if not self.is_bolt_acc_pedal_friction_car or a_target >= 0.0:
return feedforward
pedal_regen_limit = float(interp(v_ego, BOLT_ACC_PEDAL_REGEN_LIMIT_BP, BOLT_ACC_PEDAL_REGEN_LIMIT_V))
effective_gain = get_bolt_acc_pedal_feedforward_gain(
feedforward_gain, a_target, v_ego, pedal_regen_limit, last_output_accel,
)
return a_target * effective_gain
selfdrive/controls/tests/test_longcontrol.py
+64 -35
View File
@@ -4,6 +4,7 @@ from cereal import car
import pytest
import openpilot.selfdrive.controls.lib.longcontrol as longcontrol
import openpilot.selfdrive.controls.lib.longcontrol_vehicle_tunes as vehicle_tunes
from opendbc.car.gm.values import CAR, GMFlags
from openpilot.selfdrive.controls.lib.longcontrol import (
LongControl,
@@ -396,13 +397,15 @@ def test_volt_testing_ground_handoff_freezes_integrator(monkeypatch):
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
monkeypatch.setattr(longcontrol, "testing_ground", SimpleNamespace(use_2=True))
monkeypatch.setattr(vehicle_tunes, "testing_ground", SimpleNamespace(use_2=True))
lc = LongControl(CP)
freeze = lc._get_pedal_long_freeze(a_target=0.7, error=0.7, v_ego=8.0, accel_limits=(-3.0, 2.0))
freeze = lc.vehicle_tuning.get_integrator_freeze(
lc.last_output_accel, a_target=0.7, error=0.7, v_ego=8.0, accel_limits=(-3.0, 2.0),
)
assert freeze
assert lc.integrator_hold_frames > 0
assert lc.vehicle_tuning.integrator_hold_frames > 0
def test_non_interceptor_volt_testing_ground_handoff_freezes_integrator(monkeypatch):
@@ -415,13 +418,15 @@ def test_non_interceptor_volt_testing_ground_handoff_freezes_integrator(monkeypa
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.03]
monkeypatch.setattr(longcontrol, "testing_ground", SimpleNamespace(use_2=True))
monkeypatch.setattr(vehicle_tunes, "testing_ground", SimpleNamespace(use_2=True))
lc = LongControl(CP)
freeze = lc._get_pedal_long_freeze(a_target=0.7, error=0.7, v_ego=8.0, accel_limits=(-3.0, 2.0))
freeze = lc.vehicle_tuning.get_integrator_freeze(
lc.last_output_accel, a_target=0.7, error=0.7, v_ego=8.0, accel_limits=(-3.0, 2.0),
)
assert freeze
assert lc.integrator_hold_frames > 0
assert lc.vehicle_tuning.integrator_hold_frames > 0
def test_negative_target_unwinds_positive_accel_command_after_sign_flip():
@@ -502,7 +507,7 @@ def test_pedal_long_brake_bias_adds_small_negative_nudge_for_strong_decel_reques
lc = LongControl(CP)
CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False)
biased = lc._apply_pedal_long_brake_bias(-1.0, -3.0, CS)
biased = lc.vehicle_tuning.apply_pedal_long_brake_bias(-1.0, -3.0, CS)
assert biased < -1.0
assert biased == pytest.approx(-1.15, abs=0.03)
@@ -521,8 +526,8 @@ def test_pedal_long_brake_bias_does_not_touch_non_pedal_or_mild_decel():
lc = LongControl(CP)
CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False)
assert lc._apply_pedal_long_brake_bias(-1.0, -3.0, CS) == -1.0
assert lc._apply_pedal_long_brake_bias(-0.4, -0.6, CS) == -0.4
assert lc.vehicle_tuning.apply_pedal_long_brake_bias(-1.0, -3.0, CS) == -1.0
assert lc.vehicle_tuning.apply_pedal_long_brake_bias(-0.4, -0.6, CS) == -0.4
def test_bolt_acc_pedal_friction_feedforward_preserves_regen_scaling_within_envelope():
@@ -536,7 +541,9 @@ def test_bolt_acc_pedal_friction_feedforward_preserves_regen_scaling_within_enve
lc = LongControl(CP)
assert lc._get_longitudinal_feedforward(-1.8, 4.73) == pytest.approx(-0.36)
assert lc.vehicle_tuning.get_longitudinal_feedforward(
lc.feedforward_gain, lc.last_output_accel, -1.8, 4.73,
) == pytest.approx(-0.36)
def test_bolt_acc_pedal_friction_feedforward_restores_full_gain_beyond_regen_envelope():
@@ -550,7 +557,9 @@ def test_bolt_acc_pedal_friction_feedforward_restores_full_gain_beyond_regen_env
lc = LongControl(CP)
assert lc._get_longitudinal_feedforward(-3.22, 4.73) == pytest.approx(-3.22)
assert lc.vehicle_tuning.get_longitudinal_feedforward(
lc.feedforward_gain, lc.last_output_accel, -3.22, 4.73,
) == pytest.approx(-3.22)
def test_bolt_acc_pedal_friction_feedforward_blends_back_in_for_small_friction_request():
@@ -563,19 +572,21 @@ def test_bolt_acc_pedal_friction_feedforward_blends_back_in_for_small_friction_r
CP.longitudinalTuning.kfDEPRECATED = 0.20
lc = LongControl(CP)
pedal_regen_limit = float(longcontrol.interp(20.0, longcontrol.BOLT_ACC_PEDAL_REGEN_LIMIT_BP,
longcontrol.BOLT_ACC_PEDAL_REGEN_LIMIT_V))
pedal_regen_limit = float(vehicle_tunes.interp(20.0, vehicle_tunes.BOLT_ACC_PEDAL_REGEN_LIMIT_BP,
vehicle_tunes.BOLT_ACC_PEDAL_REGEN_LIMIT_V))
a_target = pedal_regen_limit - 0.10
expected_gain = longcontrol.get_bolt_acc_pedal_feedforward_gain(0.20, a_target, 20.0, pedal_regen_limit, 0.0)
expected_gain = vehicle_tunes.get_bolt_acc_pedal_feedforward_gain(0.20, a_target, 20.0, pedal_regen_limit, 0.0)
expected = a_target * expected_gain
assert lc._get_longitudinal_feedforward(a_target, 20.0) == pytest.approx(expected)
assert lc.vehicle_tuning.get_longitudinal_feedforward(
lc.feedforward_gain, lc.last_output_accel, a_target, 20.0,
) == pytest.approx(expected)
def test_bolt_acc_pedal_friction_floor_holds_friction_only_authority():
pedal_regen_limit = float(longcontrol.interp(9.85, longcontrol.BOLT_ACC_PEDAL_REGEN_LIMIT_BP,
longcontrol.BOLT_ACC_PEDAL_REGEN_LIMIT_V))
floor = longcontrol.get_bolt_acc_pedal_friction_floor(-3.47, 9.85, pedal_regen_limit)
pedal_regen_limit = float(vehicle_tunes.interp(9.85, vehicle_tunes.BOLT_ACC_PEDAL_REGEN_LIMIT_BP,
vehicle_tunes.BOLT_ACC_PEDAL_REGEN_LIMIT_V))
floor = vehicle_tunes.get_bolt_acc_pedal_friction_floor(-3.47, 9.85, pedal_regen_limit)
assert floor is not None
assert floor < pedal_regen_limit
@@ -600,32 +611,32 @@ def test_bolt_acc_pedal_friction_bias_applies_floor_only_on_experimental_fingerp
bolt_cc_lc = LongControl(bolt_cc_cp)
CS = car.CarState.new_message(vEgo=9.85, aEgo=-2.0, brakePressed=False)
pedal_regen_limit = float(longcontrol.interp(CS.vEgo, longcontrol.BOLT_ACC_PEDAL_REGEN_LIMIT_BP,
longcontrol.BOLT_ACC_PEDAL_REGEN_LIMIT_V))
floor = longcontrol.get_bolt_acc_pedal_friction_floor(-3.47, CS.vEgo, pedal_regen_limit)
pedal_regen_limit = float(vehicle_tunes.interp(CS.vEgo, vehicle_tunes.BOLT_ACC_PEDAL_REGEN_LIMIT_BP,
vehicle_tunes.BOLT_ACC_PEDAL_REGEN_LIMIT_V))
floor = vehicle_tunes.get_bolt_acc_pedal_friction_floor(-3.47, CS.vEgo, pedal_regen_limit)
assert floor is not None
pedal_biased = pedal_lc._apply_pedal_long_brake_bias(-1.85, -3.47, CS)
bolt_cc_biased = bolt_cc_lc._apply_pedal_long_brake_bias(-1.85, -3.47, CS)
pedal_biased = pedal_lc.vehicle_tuning.apply_pedal_long_brake_bias(-1.85, -3.47, CS)
bolt_cc_biased = bolt_cc_lc.vehicle_tuning.apply_pedal_long_brake_bias(-1.85, -3.47, CS)
assert pedal_biased == pytest.approx(floor)
assert bolt_cc_biased > pedal_biased + 0.5
def test_bolt_acc_pedal_feedforward_gain_stays_base_for_mild_regen():
gain = longcontrol.get_bolt_acc_pedal_feedforward_gain(0.2, -1.0, 10.0, -2.75, -0.4)
gain = vehicle_tunes.get_bolt_acc_pedal_feedforward_gain(0.2, -1.0, 10.0, -2.75, -0.4)
assert gain == pytest.approx(0.2)
def test_bolt_acc_pedal_feedforward_gain_restores_for_authority_gap():
gain = longcontrol.get_bolt_acc_pedal_feedforward_gain(0.2, -1.83, 12.38, -2.79, -0.70)
gain = vehicle_tunes.get_bolt_acc_pedal_feedforward_gain(0.2, -1.83, 12.38, -2.79, -0.70)
assert gain > 0.55
def test_bolt_acc_pedal_feedforward_gain_restores_near_friction_handoff():
gain = longcontrol.get_bolt_acc_pedal_feedforward_gain(0.2, -2.63, 9.35, -2.69, -1.30)
gain = vehicle_tunes.get_bolt_acc_pedal_feedforward_gain(0.2, -2.63, 9.35, -2.69, -1.30)
assert gain > 0.45
@@ -641,7 +652,9 @@ def test_bolt_cc_pedal_friction_feedforward_remains_fully_scaled_by_kf():
lc = LongControl(CP)
assert lc._get_longitudinal_feedforward(-3.22, 4.73) == pytest.approx(-0.644)
assert lc.vehicle_tuning.get_longitudinal_feedforward(
lc.feedforward_gain, lc.last_output_accel, -3.22, 4.73,
) == pytest.approx(-0.644)
def test_gm_stock_truck_positive_i_bleeds_on_coast_request():
@@ -660,7 +673,9 @@ def test_gm_stock_truck_positive_i_bleeds_on_coast_request():
CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
lc._trim_gm_truck_positive_hold_integrator(-0.02, -0.02, CS)
lc.vehicle_tuning.trim_gm_truck_positive_hold_integrator(
lc.pid, lc.last_output_accel, -0.02, -0.02, CS,
)
assert lc.pid.i < 0.25
@@ -681,7 +696,9 @@ def test_gm_stock_truck_positive_i_bleeds_during_light_highway_accel_request():
CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
lc._trim_gm_truck_positive_hold_integrator(0.05, 0.05, CS)
lc.vehicle_tuning.trim_gm_truck_positive_hold_integrator(
lc.pid, lc.last_output_accel, 0.05, 0.05, CS,
)
assert lc.pid.i < 0.25
@@ -702,7 +719,9 @@ def test_gm_stock_truck_positive_i_trim_keeps_meaningful_accel_request():
CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
lc._trim_gm_truck_positive_hold_integrator(0.12, 0.12, CS)
lc.vehicle_tuning.trim_gm_truck_positive_hold_integrator(
lc.pid, lc.last_output_accel, 0.12, 0.12, CS,
)
assert lc.pid.i == pytest.approx(0.25, abs=1e-9)
@@ -723,7 +742,9 @@ def test_gm_stock_truck_positive_i_trim_preserves_low_speed_launch():
CS = car.CarState.new_message(vEgo=5.0, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
lc._trim_gm_truck_positive_hold_integrator(0.05, 0.05, CS)
lc.vehicle_tuning.trim_gm_truck_positive_hold_integrator(
lc.pid, lc.last_output_accel, 0.05, 0.05, CS,
)
assert lc.pid.i == pytest.approx(0.25, abs=1e-9)
@@ -739,7 +760,9 @@ def test_gm_stock_truck_negative_i_unwinds_when_already_overbraking():
lc.last_output_accel = -0.66
CS = car.CarState.new_message(vEgo=29.6, aEgo=-0.49, brakePressed=False)
lc._trim_gm_truck_negative_hold_integrator(-0.44, 0.05, CS)
lc.vehicle_tuning.trim_gm_truck_negative_hold_integrator(
lc.pid, lc.last_output_accel, -0.44, 0.05, CS,
)
assert -0.22 < lc.pid.i < 0.0
@@ -755,7 +778,9 @@ def test_gm_stock_truck_negative_i_stays_when_decel_is_not_achieved():
lc.last_output_accel = -0.66
CS = car.CarState.new_message(vEgo=29.6, aEgo=0.05, brakePressed=False)
lc._trim_gm_truck_negative_hold_integrator(-0.44, -0.49, CS)
lc.vehicle_tuning.trim_gm_truck_negative_hold_integrator(
lc.pid, lc.last_output_accel, -0.44, -0.49, CS,
)
assert lc.pid.i == pytest.approx(-0.22, abs=1e-9)
@@ -771,7 +796,9 @@ def test_gm_stock_truck_negative_i_stays_for_urgent_braking():
lc.last_output_accel = -1.30
CS = car.CarState.new_message(vEgo=29.6, aEgo=-1.20, brakePressed=False)
lc._trim_gm_truck_negative_hold_integrator(-1.00, 0.20, CS)
lc.vehicle_tuning.trim_gm_truck_negative_hold_integrator(
lc.pid, lc.last_output_accel, -1.00, 0.20, CS,
)
assert lc.pid.i == pytest.approx(-0.22, abs=1e-9)
@@ -787,7 +814,9 @@ def test_gm_stock_truck_negative_i_trim_does_not_affect_other_gm_cars():
lc.last_output_accel = -0.66
CS = car.CarState.new_message(vEgo=29.6, aEgo=-0.49, brakePressed=False)
lc._trim_gm_truck_negative_hold_integrator(-0.44, 0.05, CS)
lc.vehicle_tuning.trim_gm_truck_negative_hold_integrator(
lc.pid, lc.last_output_accel, -0.44, 0.05, CS,
)
assert lc.pid.i == pytest.approx(-0.22, abs=1e-9)
system/manager/launch_param_migrations.py
+25 -1
View File
@@ -7,6 +7,7 @@ from pathlib import Path
from typing import Protocol
LONG_PITCH_KEY = "LongPitch"
LANE_CHANGE_SMOOTHING_KEY = "LaneChangeSmoothing"
STEER_KP_KEY = "SteerKP"
STEER_KP_STOCK_KEY = "SteerKPStock"
USE_OLD_UI_KEY = "UseOldUI"
@@ -31,6 +32,7 @@ USE_OLD_UI_MIGRATION_MARKER = ".starpilot_use_old_ui_migration_v2"
LATERAL_METHOD_REBRAND_MIGRATION_MARKER = ".starpilot_lateral_method_rebrand_v1"
VISION_SPEED_LIMIT_DETECTION_MIGRATION_MARKER = ".starpilot_vision_speed_limit_detection_v1"
DEVELOPER_METRIC_DISPLAY_MIGRATION_MARKER = ".starpilot_developer_metric_display_off_v1"
LANE_CHANGE_SMOOTHING_MIGRATION_MARKER = ".starpilot_lane_change_smoothing_default_v1"
MARKER_DIRNAME = ".starpilot_param_migrations"
LATERAL_METHOD_PARAM_SUFFIXES = (
@@ -48,8 +50,10 @@ LEGACY_RELAXED_FOLLOW_DEFAULT = 1.75
LEGACY_RELAXED_FOLLOW_HIGH_DEFAULT = 1.75
LEGACY_JERK_DEFAULT = 50.0
LEGACY_ACCELERATION_PROFILE_DEFAULT = 2
LEGACY_LANE_CHANGE_SMOOTHING_DEFAULT = 10
STANDARD_ACCELERATION_PROFILE = 0
DEFAULT_LANE_CHANGE_SMOOTHING = 5
BRANCH_BOOL_MIGRATIONS = {
"CEStoppedLead": (LEGACY_CE_STOPPED_LEAD_DEFAULT, False),
@@ -118,6 +122,10 @@ def _developer_metric_display_marker_path(params: ParamsLike) -> Path:
return _marker_dir_path(params) / DEVELOPER_METRIC_DISPLAY_MIGRATION_MARKER
def _lane_change_smoothing_marker_path(params: ParamsLike) -> Path:
return _marker_dir_path(params) / LANE_CHANGE_SMOOTHING_MIGRATION_MARKER
def _marker_dir_path(params: ParamsLike) -> Path:
params_path = Path(params.get_param_path())
# Params.clear_all() removes unknown files inside the params directory, so
@@ -250,13 +258,26 @@ def _apply_developer_metric_display_migration(params: ParamsLike, marker: Path)
marker.touch()
def _apply_lane_change_smoothing_default_migration(params: ParamsLike, marker: Path) -> None:
if marker.exists():
return
marker.parent.mkdir(parents=True, exist_ok=True)
if params.get_int(LANE_CHANGE_SMOOTHING_KEY) == LEGACY_LANE_CHANGE_SMOOTHING_DEFAULT:
params.put_int(LANE_CHANGE_SMOOTHING_KEY, DEFAULT_LANE_CHANGE_SMOOTHING)
marker.touch()
def apply_launch_param_migrations(params: ParamsLike, marker_path: Path | None = None,
branch_defaults_marker_path: Path | None = None,
acceleration_profile_marker_path: Path | None = None,
use_old_ui_marker_path: Path | None = None,
lateral_method_rebrand_marker_path: Path | None = None,
vision_speed_limit_detection_marker_path: Path | None = None,
developer_metric_display_marker_path: Path | None = None) -> None:
developer_metric_display_marker_path: Path | None = None,
lane_change_smoothing_marker_path: Path | None = None) -> None:
_apply_legacy_launch_param_migrations(params, marker_path or _default_marker_path(params))
# Keep branch-default rollout on its own marker so older installs that already
# have the legacy marker still receive this one-time param reset.
@@ -274,6 +295,9 @@ def apply_launch_param_migrations(params: ParamsLike, marker_path: Path | None =
_apply_developer_metric_display_migration(
params, developer_metric_display_marker_path or _developer_metric_display_marker_path(params)
)
_apply_lane_change_smoothing_default_migration(
params, lane_change_smoothing_marker_path or _lane_change_smoothing_marker_path(params)
)
def main() -> int:
system/manager/test/test_launch_param_migrations.py
+32
View File
@@ -5,7 +5,9 @@ from openpilot.system.manager.launch_param_migrations import (
BRANCH_DEFAULTS_MIGRATION_MARKER,
DEVELOPER_METRIC_DISPLAY_KEYS,
DEVELOPER_METRIC_DISPLAY_MIGRATION_MARKER,
DEFAULT_LANE_CHANGE_SMOOTHING,
DEFAULT_STEER_KP,
LANE_CHANGE_SMOOTHING_MIGRATION_MARKER,
LAUNCH_PARAM_MIGRATION_MARKER,
LATERAL_METHOD_REBRAND_MIGRATION_MARKER,
MARKER_DIRNAME,
@@ -309,3 +311,33 @@ def test_apply_launch_param_migrations_preserves_developer_metric_display_after_
for key in DEVELOPER_METRIC_DISPLAY_KEYS:
assert params.get_bool(key)
def test_apply_launch_param_migrations_updates_legacy_lane_change_smoothing_once(tmp_path):
params = FileBackedFakeParams(tmp_path / "params")
params.put_int("LaneChangeSmoothing", 10)
apply_launch_param_migrations(params)
assert params.get_int("LaneChangeSmoothing") == DEFAULT_LANE_CHANGE_SMOOTHING
assert marker_path(tmp_path, LANE_CHANGE_SMOOTHING_MIGRATION_MARKER).is_file()
def test_apply_launch_param_migrations_preserves_custom_lane_change_smoothing(tmp_path):
params = FileBackedFakeParams(tmp_path / "params")
params.put_int("LaneChangeSmoothing", 7)
apply_launch_param_migrations(params)
assert params.get_int("LaneChangeSmoothing") == 7
assert marker_path(tmp_path, LANE_CHANGE_SMOOTHING_MIGRATION_MARKER).is_file()
def test_apply_launch_param_migrations_does_not_reapply_lane_change_smoothing_after_marker(tmp_path):
params = FileBackedFakeParams(tmp_path / "params")
params.put_int("LaneChangeSmoothing", 10)
marker_path(tmp_path, LANE_CHANGE_SMOOTHING_MIGRATION_MARKER).touch()
apply_launch_param_migrations(params)
assert params.get_int("LaneChangeSmoothing") == 10
tools/longitudinal/analyze_route_longitudinal.py
+622
View File
@@ -0,0 +1,622 @@
#!/usr/bin/env python3
from __future__ import annotations
import argparse
import json
import math
from dataclasses import asdict, dataclass, field
from pathlib import Path
from typing import Any
import numpy as np
from openpilot.tools.lib.logreader import LogReader, ReadMode, parse_direct, parse_indirect
from openpilot.tools.lib.route import SegmentRange
ACTIVE_DEADBAND = 0.08
SOURCE_SWITCH_SCORE = 1.5
DEFAULT_BOOKMARK_BEFORE = 12.0
DEFAULT_BOOKMARK_AFTER = 3.0
@dataclass
class LongitudinalSample:
segment: int
time_s: float
mono_time: int
long_active: bool = False
v_ego: float = 0.0
a_ego: float = 0.0
gas_pressed: bool = False
brake_pressed: bool = False
plan_accel: float = 0.0
command_accel: float = 0.0
output_accel: float = 0.0
p_term: float = 0.0
i_term: float = 0.0
f_term: float = 0.0
long_state: str = "unknown"
source: str = "unknown"
should_stop: bool = False
allow_throttle: bool = True
allow_brake: bool = True
has_lead: bool = False
lead_status: bool = False
lead_distance: float | None = None
lead_relative_velocity: float = 0.0
lead_velocity: float = 0.0
lead_acceleration: float = 0.0
lead_probability: float = 0.0
lead_radar: bool = False
lead_track_id: int = -1
lead_two_status: bool = False
lead_two_distance: float | None = None
experimental_mode: bool = False
forcing_stop: bool = False
red_light: bool = False
tracking_lead: bool = False
curve_controller_active: bool = False
cruise_target: float = 0.0
desired_follow_distance: float = 0.0
@dataclass
class Finding:
kind: str
severity: str
score: float
evidence: list[str] = field(default_factory=list)
@dataclass
class WindowReport:
label: str
segment: int
event_time_s: float
start_time_s: float
end_time_s: float
sample_count: int
summary: dict[str, Any]
findings: list[Finding]
@dataclass
class SegmentData:
segment: int
source: str
samples: list[LongitudinalSample]
bookmarks: list[float]
car_params: dict[str, Any] | None
settings: dict[str, Any]
def safe_float(value: Any, default: float = 0.0) -> float:
try:
result = float(value)
except (TypeError, ValueError):
return default
return result if math.isfinite(result) else default
def safe_attr(obj: Any, name: str, default: Any = None) -> Any:
if obj is None:
return default
try:
return getattr(obj, name)
except (AttributeError, RuntimeError):
return default
def state_changes(values: list[Any]) -> int:
return sum(current != previous for previous, current in zip(values, values[1:], strict=False))
def threshold_sign_changes(values: list[float], deadband: float = ACTIVE_DEADBAND) -> int:
signs: list[int] = []
for value in values:
sign = 1 if value > deadband else -1 if value < -deadband else 0
if sign and (not signs or sign != signs[-1]):
signs.append(sign)
return max(0, len(signs) - 1)
def direction_reversals(values: list[float], minimum_step: float = 0.06) -> int:
directions: list[int] = []
for previous, current in zip(values, values[1:], strict=False):
delta = current - previous
direction = 1 if delta > minimum_step else -1 if delta < -minimum_step else 0
if direction and (not directions or direction != directions[-1]):
directions.append(direction)
return max(0, len(directions) - 1)
def maximum_rate(values: list[float], times: list[float]) -> float:
rates = [
abs((current - previous) / max(time - previous_time, 1e-3))
for previous, current, previous_time, time in zip(values, values[1:], times, times[1:], strict=False)
if 0.0 < time - previous_time < 0.5
]
return max(rates, default=0.0)
def percentile(values: list[float], quantile: float, default: float = 0.0) -> float:
return float(np.percentile(values, quantile)) if values else default
def severity_for(kind: str, score: float) -> str:
if kind == "unsafe_stop_release" and score >= 4.0:
return "critical"
if kind == "late_lead_response" and score >= 5.0:
return "high"
if score >= 7.0:
return "high"
if score >= 3.0:
return "medium"
return "low"
def analyze_samples(samples: list[LongitudinalSample], label: str, event_time_s: float) -> WindowReport:
if not samples:
raise ValueError("Cannot analyze an empty longitudinal window")
active_samples = [sample for sample in samples if sample.long_active]
relevant = active_samples if active_samples else samples
times = [sample.time_s for sample in relevant]
plans = [sample.plan_accel for sample in relevant]
commands = [sample.command_accel for sample in relevant]
actual = [sample.a_ego for sample in relevant]
integrals = [sample.i_term for sample in relevant]
sources = [sample.source for sample in relevant]
lead_statuses = [sample.lead_status for sample in relevant]
experiment_states = [sample.experimental_mode for sample in relevant]
stop_states = [sample.should_stop for sample in relevant]
tracking_states = [sample.tracking_lead for sample in relevant]
plan_sign_flips = threshold_sign_changes(plans)
command_sign_flips = threshold_sign_changes(commands)
plan_reversals = direction_reversals(plans)
command_reversals = direction_reversals(commands)
source_switches = state_changes(sources)
lead_status_flips = state_changes(lead_statuses)
experimental_flips = state_changes(experiment_states)
stop_flips = state_changes(stop_states)
tracking_flips = state_changes(tracking_states)
lead_track_ids = [sample.lead_track_id for sample in relevant if sample.lead_status and sample.lead_track_id >= 0]
lead_track_switches = state_changes(lead_track_ids)
lead_velocity_jumps = sum(
abs(current.lead_velocity - previous.lead_velocity) > 1.25
for previous, current in zip(relevant, relevant[1:], strict=False)
if previous.lead_status and current.lead_status and 0.0 < current.time_s - previous.time_s < 0.5
)
lead_distance_jumps = sum(
abs((current.lead_distance or 0.0) - (previous.lead_distance or 0.0)) > 6.0
for previous, current in zip(relevant, relevant[1:], strict=False)
if previous.lead_status and current.lead_status and 0.0 < current.time_s - previous.time_s < 0.5
)
stale_positive_i = [sample for sample in relevant if sample.plan_accel < -0.08 and sample.i_term > 0.08]
stale_negative_i = [
sample for sample in relevant
if sample.plan_accel > -0.05 and sample.i_term < -0.08 and sample.command_accel < sample.plan_accel - 0.10
]
opposite_commands = [
sample for sample in relevant
if abs(sample.plan_accel) > 0.10 and abs(sample.command_accel) > 0.10 and sample.plan_accel * sample.command_accel < 0.0
]
interface_adjustments = [abs(sample.output_accel - sample.command_accel) for sample in relevant]
response_errors = [abs(sample.a_ego - sample.output_accel) for sample in relevant]
late_response_shortfalls: list[float] = []
minimum_ttc: float | None = None
for sample in relevant:
if not sample.lead_status or sample.lead_distance is None:
continue
closing_speed = max(-sample.lead_relative_velocity, sample.v_ego - sample.lead_velocity, 0.0)
if closing_speed < 1.5:
continue
ttc = sample.lead_distance / max(closing_speed, 0.1)
minimum_ttc = ttc if minimum_ttc is None else min(minimum_ttc, ttc)
if ttc >= 4.0:
continue
usable_gap = max(sample.lead_distance - 4.0, 1.0)
required_accel = -(closing_speed ** 2) / (2.0 * usable_gap)
late_response_shortfalls.append(max(0.0, sample.command_accel - required_accel))
unsafe_stop_release = [
sample for sample in relevant
if (sample.should_stop and sample.command_accel > 0.10) or (
sample.lead_status and sample.lead_distance is not None and sample.lead_distance < 12.0 and
sample.lead_velocity < 0.5 and sample.v_ego < 5.0 and sample.command_accel > 0.15
)
]
max_plan_jerk = maximum_rate(plans, times)
max_command_jerk = maximum_rate(commands, times)
controller_gap_p95 = percentile([abs(command - plan) for command, plan in zip(commands, plans, strict=True)], 95)
response_gap_p95 = percentile(response_errors, 95)
interface_gap_p95 = percentile(interface_adjustments, 95)
stale_i_ratio = (len(stale_positive_i) + len(stale_negative_i)) / max(len(relevant), 1)
opposite_ratio = len(opposite_commands) / max(len(relevant), 1)
late_shortfall = max(late_response_shortfalls, default=0.0)
category_scores = {
"unsafe_stop_release": len(unsafe_stop_release) * 1.5,
"late_lead_response": late_shortfall * 2.0 + (1.0 if minimum_ttc is not None and minimum_ttc < 3.0 else 0.0),
"mode_arbitration": experimental_flips * 3.0 + stop_flips * 2.0 + tracking_flips,
"lead_instability": (
source_switches * SOURCE_SWITCH_SCORE + lead_status_flips + lead_track_switches * 2.0 +
lead_velocity_jumps * 0.75 + lead_distance_jumps
),
"controller_integrator": stale_i_ratio * 12.0 + opposite_ratio * 10.0 + max(0.0, controller_gap_p95 - 0.20) * 3.0,
"planner_chatter": (
plan_sign_flips * 1.5 + plan_reversals * 0.65 + command_sign_flips * 0.6 +
max(0.0, max_plan_jerk - 1.5) * 0.35
),
"vehicle_response": max(0.0, response_gap_p95 - 0.35) * 2.5 + max(0.0, interface_gap_p95 - 0.10) * 4.0,
}
evidence = {
"unsafe_stop_release": [
f"{len(unsafe_stop_release)} samples commanded positive acceleration while a stop hold or close stopped lead was active",
],
"late_lead_response": [
f"minimum TTC={minimum_ttc:.2f}s" if minimum_ttc is not None else "no closing lead TTC available",
f"maximum kinematic decel shortfall={late_shortfall:.2f} m/s^2",
],
"mode_arbitration": [
f"experimental flips={experimental_flips}, shouldStop flips={stop_flips}, trackingLead flips={tracking_flips}",
],
"lead_instability": [
f"source switches={source_switches}, lead status flips={lead_status_flips}, radar track switches={lead_track_switches}",
f"lead velocity jumps={lead_velocity_jumps}, lead distance jumps={lead_distance_jumps}",
],
"controller_integrator": [
f"stale-I samples={len(stale_positive_i) + len(stale_negative_i)}/{len(relevant)}, opposite command samples={len(opposite_commands)}",
f"planner-to-command gap p95={controller_gap_p95:.2f} m/s^2, I range={min(integrals):+.2f}..{max(integrals):+.2f}",
],
"planner_chatter": [
f"plan sign flips={plan_sign_flips}, direction reversals={plan_reversals}, max plan jerk={max_plan_jerk:.2f} m/s^3",
f"command sign flips={command_sign_flips}, direction reversals={command_reversals}, max command jerk={max_command_jerk:.2f} m/s^3",
],
"vehicle_response": [
f"command-to-applied gap p95={interface_gap_p95:.2f} m/s^2",
f"applied-to-measured acceleration gap p95={response_gap_p95:.2f} m/s^2",
],
}
findings = [
Finding(kind=kind, severity=severity_for(kind, score), score=round(score, 3), evidence=evidence[kind])
for kind, score in category_scores.items()
if score >= 1.0
]
findings.sort(key=lambda finding: finding.score, reverse=True)
lead_samples = [sample for sample in relevant if sample.lead_status and sample.lead_distance is not None]
summary = {
"activeSamples": len(active_samples),
"speedRangeMps": [round(min(sample.v_ego for sample in relevant), 3), round(max(sample.v_ego for sample in relevant), 3)],
"planAccelRange": [round(min(plans), 3), round(max(plans), 3)],
"commandAccelRange": [round(min(commands), 3), round(max(commands), 3)],
"actualAccelRange": [round(min(actual), 3), round(max(actual), 3)],
"leadDistanceRange": None if not lead_samples else [
round(min(sample.lead_distance for sample in lead_samples if sample.lead_distance is not None), 3),
round(max(sample.lead_distance for sample in lead_samples if sample.lead_distance is not None), 3),
],
"sources": sorted(set(sources)),
"minimumTtc": None if minimum_ttc is None else round(minimum_ttc, 3),
"maxPlanJerk": round(max_plan_jerk, 3),
"maxCommandJerk": round(max_command_jerk, 3),
}
return WindowReport(
label=label,
segment=samples[0].segment,
event_time_s=round(event_time_s, 3),
start_time_s=round(samples[0].time_s, 3),
end_time_s=round(samples[-1].time_s, 3),
sample_count=len(relevant),
summary=summary,
findings=findings,
)
def parse_settings(serialized: str) -> dict[str, Any]:
if not serialized:
return {}
try:
settings = json.loads(serialized)
except (TypeError, json.JSONDecodeError):
return {}
if not isinstance(settings, dict):
return {}
keywords = ("accel", "decel", "follow", "jerk", "longitudinal", "personality", "smooth", "truck")
return {
key: value for key, value in sorted(settings.items())
if any(keyword in key.lower() for keyword in keywords)
}
def snapshot_car_params(CP: Any) -> dict[str, Any]:
tuning = safe_attr(CP, "longitudinalTuning")
return {
"brand": str(safe_attr(CP, "brand", "unknown")),
"carFingerprint": str(safe_attr(CP, "carFingerprint", "unknown")),
"openpilotLongitudinalControl": bool(safe_attr(CP, "openpilotLongitudinalControl", False)),
"longitudinalActuatorDelay": safe_float(safe_attr(CP, "longitudinalActuatorDelay", 0.0)),
"kpBP": list(safe_attr(tuning, "kpBP", [])),
"kpV": list(safe_attr(tuning, "kpV", [])),
"kiBP": list(safe_attr(tuning, "kiBP", [])),
"kiV": list(safe_attr(tuning, "kiV", [])),
"kf": safe_float(safe_attr(tuning, "kfDEPRECATED", 0.0)),
}
def make_sample(segment: int, segment_start_ns: int, mono_time: int, latest: dict[str, Any]) -> LongitudinalSample | None:
required = ("carState", "carControl", "controlsState", "radarState", "starpilotPlan", "longitudinalPlan")
if not all(service in latest for service in required):
return None
car_state = latest["carState"]
car_control = latest["carControl"]
controls_state = latest["controlsState"]
radar_state = latest["radarState"]
starpilot_plan = latest["starpilotPlan"]
long_plan = latest["longitudinalPlan"]
car_output = latest.get("carOutput")
lead = radar_state.leadOne
lead_two = radar_state.leadTwo
command_accel = safe_float(safe_attr(safe_attr(car_control, "actuators"), "accel", 0.0))
output_accel = safe_float(safe_attr(safe_attr(car_output, "actuatorsOutput"), "accel", command_accel), command_accel)
lead_status = bool(safe_attr(lead, "status", False))
lead_two_status = bool(safe_attr(lead_two, "status", False))
return LongitudinalSample(
segment=segment,
time_s=(mono_time - segment_start_ns) / 1e9,
mono_time=mono_time,
long_active=bool(safe_attr(car_control, "longActive", False)),
v_ego=safe_float(safe_attr(car_state, "vEgo", 0.0)),
a_ego=safe_float(safe_attr(car_state, "aEgo", 0.0)),
gas_pressed=bool(safe_attr(car_state, "gasPressed", False)),
brake_pressed=bool(safe_attr(car_state, "brakePressed", False)),
plan_accel=safe_float(safe_attr(long_plan, "aTarget", 0.0)),
command_accel=command_accel,
output_accel=output_accel,
p_term=safe_float(safe_attr(controls_state, "upAccelCmd", 0.0)),
i_term=safe_float(safe_attr(controls_state, "uiAccelCmd", 0.0)),
f_term=safe_float(safe_attr(controls_state, "ufAccelCmd", 0.0)),
long_state=str(safe_attr(controls_state, "longControlState", "unknown")),
source=str(safe_attr(long_plan, "longitudinalPlanSource", "unknown")),
should_stop=bool(safe_attr(long_plan, "shouldStop", False)),
allow_throttle=bool(safe_attr(long_plan, "allowThrottle", True)),
allow_brake=bool(safe_attr(long_plan, "allowBrake", True)),
has_lead=bool(safe_attr(long_plan, "hasLead", False)),
lead_status=lead_status,
lead_distance=safe_float(safe_attr(lead, "dRel", 0.0)) if lead_status else None,
lead_relative_velocity=safe_float(safe_attr(lead, "vRel", 0.0)),
lead_velocity=safe_float(safe_attr(lead, "vLead", 0.0)),
lead_acceleration=safe_float(safe_attr(lead, "aLeadK", 0.0)),
lead_probability=safe_float(safe_attr(lead, "modelProb", 0.0)),
lead_radar=bool(safe_attr(lead, "radar", False)),
lead_track_id=int(safe_attr(lead, "radarTrackId", -1)),
lead_two_status=lead_two_status,
lead_two_distance=safe_float(safe_attr(lead_two, "dRel", 0.0)) if lead_two_status else None,
experimental_mode=bool(safe_attr(starpilot_plan, "experimentalMode", False)),
forcing_stop=bool(safe_attr(starpilot_plan, "forcingStop", False)),
red_light=bool(safe_attr(starpilot_plan, "redLight", False)),
tracking_lead=bool(safe_attr(starpilot_plan, "trackingLead", False)),
curve_controller_active=bool(safe_attr(starpilot_plan, "cscControllingSpeed", False)),
cruise_target=safe_float(safe_attr(starpilot_plan, "vCruise", 0.0)),
desired_follow_distance=safe_float(safe_attr(starpilot_plan, "desiredFollowDistance", 0.0)),
)
def analyze_segment(identifier: str, segment: int, mode: ReadMode) -> SegmentData:
reader = LogReader(identifier, default_mode=mode, sort_by_time=True)
source = ",".join(Path(path).name or path.rsplit("/", 1)[-1] for path in reader.logreader_identifiers)
latest: dict[str, Any] = {}
samples: list[LongitudinalSample] = []
bookmarks: list[float] = []
car_params = None
settings: dict[str, Any] = {}
segment_start_ns: int | None = None
for msg in reader:
mono_time = int(msg.logMonoTime)
if segment_start_ns is None:
segment_start_ns = mono_time
which = msg.which()
if which in ("userBookmark", "bookmarkButton"):
bookmark_time = (mono_time - segment_start_ns) / 1e9
if not bookmarks or bookmark_time - bookmarks[-1] > 0.5:
bookmarks.append(bookmark_time)
continue
if which == "carParams" and car_params is None:
car_params = snapshot_car_params(msg.carParams)
if which in ("carState", "carControl", "carOutput", "controlsState", "radarState", "starpilotPlan", "longitudinalPlan"):
latest[which] = getattr(msg, which)
if which == "starpilotPlan":
settings = parse_settings(str(safe_attr(msg.starpilotPlan, "starpilotToggles", ""))) or settings
if which == "longitudinalPlan" and segment_start_ns is not None:
sample = make_sample(segment, segment_start_ns, mono_time, latest)
if sample is not None:
samples.append(sample)
return SegmentData(
segment=segment,
source=source,
samples=samples,
bookmarks=bookmarks,
car_params=car_params,
settings=settings,
)
def point_anomaly_scores(samples: list[LongitudinalSample]) -> list[tuple[float, float]]:
scores: list[tuple[float, float]] = []
for previous, current in zip(samples, samples[1:], strict=False):
if not current.long_active:
continue
score = 0.0
score += max(0.0, abs(current.plan_accel - previous.plan_accel) - 0.12) * 4.0
score += max(0.0, abs(current.command_accel - previous.command_accel) - 0.15) * 3.0
score += SOURCE_SWITCH_SCORE if current.source != previous.source else 0.0
score += 1.0 if current.lead_status != previous.lead_status else 0.0
score += 2.0 if current.experimental_mode != previous.experimental_mode else 0.0
score += 1.5 if current.should_stop != previous.should_stop else 0.0
if current.plan_accel < -0.08 and current.i_term > 0.08:
score += 1.0
if current.plan_accel > -0.05 and current.i_term < -0.08 and current.command_accel < current.plan_accel - 0.10:
score += 1.0
if score >= 1.0:
scores.append((current.time_s, score))
return scores
def anomaly_episode_times(samples: list[LongitudinalSample], limit: int) -> list[float]:
points = point_anomaly_scores(samples)
if not points:
return []
episodes: list[list[tuple[float, float]]] = [[points[0]]]
for point in points[1:]:
if point[0] - episodes[-1][-1][0] <= 1.5:
episodes[-1].append(point)
else:
episodes.append([point])
ranked = sorted(
((max(episode, key=lambda item: item[1])[0], sum(item[1] for item in episode)) for episode in episodes),
key=lambda item: item[1],
reverse=True,
)
return [time_s for time_s, _ in ranked[:limit]]
def window_samples(samples: list[LongitudinalSample], event_time_s: float, before: float, after: float) -> list[LongitudinalSample]:
return [sample for sample in samples if event_time_s - before <= sample.time_s <= event_time_s + after]
def resolve_segments(identifier: str, mode: ReadMode) -> tuple[str, list[tuple[int, str]]]:
if parse_direct(identifier) is not None:
return identifier, [(0, identifier)]
normalized = parse_indirect(identifier)
segment_range = SegmentRange(normalized)
route = segment_range.route_name
requests = [(segment, f"{route}/{segment}") for segment in segment_range.seg_idxs]
return route, requests
def analyze_route(identifier: str, mode: ReadMode, before: float, after: float, top: int) -> dict[str, Any]:
route, segment_requests = resolve_segments(identifier, mode)
segments = [analyze_segment(request, segment, mode) for segment, request in segment_requests]
reports: list[WindowReport] = []
for segment_data in segments:
for bookmark_number, bookmark_time in enumerate(segment_data.bookmarks, start=1):
samples = window_samples(segment_data.samples, bookmark_time, before, after)
if samples:
reports.append(analyze_samples(samples, f"bookmark {bookmark_number}", bookmark_time))
episode_times = anomaly_episode_times(segment_data.samples, top)
for episode_number, event_time in enumerate(episode_times, start=1):
if any(abs(event_time - bookmark) <= before for bookmark in segment_data.bookmarks):
continue
samples = window_samples(segment_data.samples, event_time, min(before, 5.0), min(after, 2.0))
if samples:
reports.append(analyze_samples(samples, f"route anomaly {episode_number}", event_time))
reports.sort(key=lambda report: (report.segment, report.event_time_s, report.label))
car_params = next((segment.car_params for segment in segments if segment.car_params), None)
settings = next((segment.settings for segment in reversed(segments) if segment.settings), {})
return {
"route": route,
"carParams": car_params,
"settings": settings,
"segments": [
{
"segment": segment.segment,
"source": segment.source,
"samples": len(segment.samples),
"bookmarks": segment.bookmarks,
}
for segment in segments
],
"reports": [asdict(report) for report in reports],
}
def print_report(payload: dict[str, Any]) -> None:
print(f"route={payload['route']}")
car_params = payload.get("carParams")
if car_params:
vehicle_line = f"vehicle={car_params['carFingerprint']} brand={car_params['brand']}"
delay_line = f"longDelay={car_params['longitudinalActuatorDelay']:.3f}s openpilotLong={car_params['openpilotLongitudinalControl']}"
print(f"{vehicle_line} {delay_line}")
print(f"longTune kp={car_params['kpV']} ki={car_params['kiV']} kf={car_params['kf']}")
else:
print("vehicle=unknown (carParams unavailable in selected segments)")
for segment in payload["segments"]:
segment_line = f"segment={segment['segment']} source={segment['source']} samples={segment['samples']}"
print(f"{segment_line} bookmarks={len(segment['bookmarks'])}")
settings = payload.get("settings", {})
if settings:
print("settings=" + json.dumps(settings, sort_keys=True))
reports = payload["reports"]
if not reports:
print("No bookmark windows or route-wide anomalies were found.")
return
for report in reports:
primary = report["findings"][0] if report["findings"] else None
primary_text = "no deterministic fault" if primary is None else f"{primary['kind']} ({primary['severity']}, score={primary['score']:.2f})"
event_line = f"\nseg {report['segment']} {report['label']} @{report['event_time_s']:.2f}s: {primary_text}"
window_line = f" window={report['start_time_s']:.2f}..{report['end_time_s']:.2f}s samples={report['sample_count']}"
accel_line = (
f"speed={report['summary']['speedRangeMps']}m/s plan={report['summary']['planAccelRange']} "
+ f"cmd={report['summary']['commandAccelRange']} actual={report['summary']['actualAccelRange']}"
)
lead_line = f" leadRange={report['summary']['leadDistanceRange']}m sources={report['summary']['sources']}"
print(event_line)
print(f"{window_line} {accel_line}")
print(f"{lead_line} minTTC={report['summary']['minimumTtc']}s")
for finding in report["findings"][:3]:
print(f" {finding['kind']}: {finding['severity']} score={finding['score']:.2f}")
for item in finding["evidence"]:
print(f" {item}")
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser(description="Diagnose longitudinal behavior around route bookmarks and automatically detected anomalies.")
parser.add_argument("routes", nargs="+", help="Comma/Konik route, segment range, Connect URL, or local log file")
parser.add_argument("--mode", choices=("auto", "qlog", "rlog"), default="auto")
parser.add_argument("--before", type=float, default=DEFAULT_BOOKMARK_BEFORE, help="Seconds before each bookmark to inspect")
parser.add_argument("--after", type=float, default=DEFAULT_BOOKMARK_AFTER, help="Seconds after each bookmark to inspect")
parser.add_argument("--top", type=int, default=5, help="Maximum route-wide anomaly episodes per segment")
parser.add_argument("--json-out", type=Path, help="Optional JSON report path")
return parser.parse_args()
def main() -> None:
args = parse_args()
mode = {"auto": ReadMode.AUTO, "qlog": ReadMode.QLOG, "rlog": ReadMode.RLOG}[args.mode]
payloads = [analyze_route(route, mode, args.before, args.after, args.top) for route in args.routes]
for index, payload in enumerate(payloads):
if index:
print("\n" + "=" * 80 + "\n")
print_report(payload)
if args.json_out:
args.json_out.parent.mkdir(parents=True, exist_ok=True)
args.json_out.write_text(json.dumps(payloads, indent=2, sort_keys=True) + "\n", encoding="utf-8")
if __name__ == "__main__":
main()
tools/longitudinal/tests/test_analyze_route_longitudinal.py
+136
View File
@@ -0,0 +1,136 @@
from dataclasses import replace
import pytest
from openpilot.tools.longitudinal.analyze_route_longitudinal import (
LongitudinalSample,
analyze_samples,
anomaly_episode_times,
parse_settings,
threshold_sign_changes,
)
def make_samples(count: int = 20, **overrides) -> list[LongitudinalSample]:
base = LongitudinalSample(
segment=4,
time_s=0.0,
mono_time=0,
long_active=True,
v_ego=20.0,
a_ego=0.1,
plan_accel=0.1,
command_accel=0.1,
output_accel=0.1,
source="cruise",
)
return [
replace(base, time_s=index * 0.05, mono_time=index * 50_000_000, **overrides)
for index in range(count)
]
def primary_kind(samples: list[LongitudinalSample]) -> str | None:
report = analyze_samples(samples, "test", samples[-1].time_s)
return report.findings[0].kind if report.findings else None
def test_threshold_sign_changes_ignores_zero_crossing_noise():
assert threshold_sign_changes([0.2, 0.03, -0.02, 0.1, -0.2]) == 1
def test_planner_chatter_is_identified_from_repeated_target_reversals():
samples = make_samples()
for index, sample in enumerate(samples):
accel = 0.45 if index % 2 == 0 else -0.45
samples[index] = replace(sample, plan_accel=accel, command_accel=accel, output_accel=accel, a_ego=accel * 0.8)
assert primary_kind(samples) == "planner_chatter"
def test_controller_integrator_is_identified_when_command_opposes_plan():
samples = make_samples(plan_accel=0.25, command_accel=-0.25, output_accel=-0.25, a_ego=-0.2, i_term=-0.40)
assert primary_kind(samples) == "controller_integrator"
def test_lead_instability_is_identified_from_source_and_track_churn():
samples = make_samples(12, lead_status=True, lead_distance=45.0, lead_velocity=18.0, source="lead0", lead_track_id=10)
for index, sample in enumerate(samples):
samples[index] = replace(
sample,
source="lead0" if index % 2 == 0 else "lead1",
lead_track_id=10 if index % 2 == 0 else 11,
lead_velocity=18.0 if index % 2 == 0 else 20.0,
)
assert primary_kind(samples) == "lead_instability"
def test_unsafe_stop_release_is_critical():
samples = make_samples(
6,
v_ego=2.0,
plan_accel=0.3,
command_accel=0.4,
output_accel=0.4,
a_ego=0.2,
should_stop=True,
lead_status=True,
lead_distance=7.0,
lead_velocity=0.0,
)
report = analyze_samples(samples, "test", samples[-1].time_s)
assert report.findings[0].kind == "unsafe_stop_release"
assert report.findings[0].severity == "critical"
def test_late_lead_response_is_identified_from_ttc_and_decel_shortfall():
samples = make_samples(
10,
v_ego=20.0,
plan_accel=0.0,
command_accel=0.0,
output_accel=0.0,
a_ego=0.0,
lead_status=True,
lead_distance=18.0,
lead_relative_velocity=-8.0,
lead_velocity=12.0,
)
assert primary_kind(samples) == "late_lead_response"
def test_normal_steady_follow_has_no_deterministic_finding():
samples = make_samples(
lead_status=True,
lead_distance=40.0,
lead_relative_velocity=0.0,
lead_velocity=20.0,
source="lead0",
)
report = analyze_samples(samples, "test", samples[-1].time_s)
assert report.findings == []
def test_anomaly_episodes_group_nearby_points():
samples = make_samples(80)
samples[10] = replace(samples[10], plan_accel=-0.5)
samples[11] = replace(samples[11], plan_accel=0.5)
samples[60] = replace(samples[60], plan_accel=-0.5)
episodes = anomaly_episode_times(samples, limit=5)
assert len(episodes) == 2
assert episodes[0] == pytest.approx(0.55)
def test_parse_settings_keeps_only_longitudinal_context():
settings = parse_settings('{"FollowDistance": 1.5, "AccelerationProfile": "eco", "LaneWidth": 3.5}')
assert settings == {"AccelerationProfile": "eco", "FollowDistance": 1.5}