diff --git a/scripts/speed_limit_vision/build_localized_bookmark_review_queue.py b/scripts/speed_limit_vision/build_localized_bookmark_review_queue.py index e2d6821ad..9d9149baf 100644 --- a/scripts/speed_limit_vision/build_localized_bookmark_review_queue.py +++ b/scripts/speed_limit_vision/build_localized_bookmark_review_queue.py @@ -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 diff --git a/scripts/speed_limit_vision/localize_bookmark_signs.py b/scripts/speed_limit_vision/localize_bookmark_signs.py index ff7940345..3f502f6fa 100644 --- a/scripts/speed_limit_vision/localize_bookmark_signs.py +++ b/scripts/speed_limit_vision/localize_bookmark_signs.py @@ -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: diff --git a/scripts/speed_limit_vision/mine_connect_route_bookmarks.py b/scripts/speed_limit_vision/mine_connect_route_bookmarks.py index 66a2c6603..6497dced1 100644 --- a/scripts/speed_limit_vision/mine_connect_route_bookmarks.py +++ b/scripts/speed_limit_vision/mine_connect_route_bookmarks.py @@ -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 /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) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 8dfefd83a..a1b5e7fcf 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -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) diff --git a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py new file mode 100644 index 000000000..51bce7490 --- /dev/null +++ b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py @@ -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 diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index c9ae5f3fb..619d0ee80 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -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) diff --git a/system/manager/launch_param_migrations.py b/system/manager/launch_param_migrations.py index 12ee4a536..82b276767 100644 --- a/system/manager/launch_param_migrations.py +++ b/system/manager/launch_param_migrations.py @@ -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: diff --git a/system/manager/test/test_launch_param_migrations.py b/system/manager/test/test_launch_param_migrations.py index c26184cdf..a13a522e3 100644 --- a/system/manager/test/test_launch_param_migrations.py +++ b/system/manager/test/test_launch_param_migrations.py @@ -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 diff --git a/tools/longitudinal/analyze_route_longitudinal.py b/tools/longitudinal/analyze_route_longitudinal.py new file mode 100644 index 000000000..f61a0d698 --- /dev/null +++ b/tools/longitudinal/analyze_route_longitudinal.py @@ -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() diff --git a/tools/longitudinal/tests/test_analyze_route_longitudinal.py b/tools/longitudinal/tests/test_analyze_route_longitudinal.py new file mode 100644 index 000000000..039fb704c --- /dev/null +++ b/tools/longitudinal/tests/test_analyze_route_longitudinal.py @@ -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}