achievements: community log, privacy filter and a dry-run preview

Phase 0 of the community plan: the car works out what it would share and
writes it to files. Nothing is uploaded.

- community.py keeps community.db next to state.json: 6 km map tiles (H3
  resolution 6) with the day first seen, the tile where each drive starts and
  ends, engaged distance and time per day, and engaged distance per region per
  day. Positions only ever become tiles.
- The privacy filter follows the build plan: a tile where drives start or end
  on 3+ days is a frequent stop and it and its 6 neighbors never leave the car;
  other start/end tiles wait 14 days; hidden tiles never leave; tile batches
  are sorted with no dates; tiles that become private after being sent turn
  into delete requests; home region is where you drove most on most of the
  last 90 days, with detected changes sent at most every 30 days.
- preview.py writes the daily reports, the tile batch, delete requests and a
  map page (your 6 km map, the 45 km blocks others see, what stays on the car)
  to achievements/preview/ after every drive.
- h3lite.py ports the H3 functions we need to pure Python, since sunnypilot
  doesn't add packages. Checked against h3-py on 2.8M points: identical cells,
  centers, parents and rings, including pentagons, poles and the antimeridian.
- regions.json.gz (207 KB) maps tiles to ISO 3166-2 codes for US, CA, MX, BR,
  AU, DE and country codes elsewhere, from Natural Earth (public domain).
  tools/build_regions.py rebuilds it.
- replay.py runs everything over qlogs already on the car, into its own folder.
- achievementsd feeds the log each sample; any community error turns it off
  for the drive without touching achievements.

92 tests pass (engine, store, H3 port, privacy filter, replay feeder). The
daemon itself was only compiled and linted here; cereal and msgq are not built
in this environment.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01ATDgLQKGkv2ToqFZNWSF8j

openpilot/common/params_keys.h

@@ -259,9 +259,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}},
     {"Blendful", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"PathSteering", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"PathSteeringGeometry", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"PathSteeringPursuit", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
 
     // mapd

openpilot/selfdrive/modeld/models/driving_supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:de9f536d899fa451d75a28fb5766ecb996189c559aa9c1841a6b8ccc8453cf8d
-size 84567106
+oid sha256:b043d3bbc556e3a872c0edde4081f99b5ba92cbfd98eaea2ee2cbb87e9f277ec
+size 82539558

openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py

@@ -99,27 +99,8 @@ class ModelsLayoutMici(NavScroller):
                                            description=tr("Adds a gentle nudge toward lane center on top of the model's own steering, stronger when " +
                                                           "lane lines are clear. Yields around obstacles. Not available with the Default model."))
 
-    # path steering (openpilot/sunnypilot/modeld_v2/recent_paths.py); left out while the key is unknown to the build
-    self.path_steering_toggle = None
-    try:
-      ui_state.params.get_bool("PathSteering")
-      self.path_steering_toggle = BigParamControl(tr("path steering (alpha)"), "PathSteering",
-                                                  description=tr("In town, steers from an average of the model's recent paths: while a turn is signalled the paths lead, otherwise they only smooth the model's own steering. A calmer wheel at low speed. Not available with the Default model."))
-    except Exception:
-      pass
-    # its two switches (recent_paths.py), shown the same way
-    self.path_extra_toggles = []
-    for key, title, about in (
-        ("PathSteeringPursuit", "path steering: pursuit", "With path steering on, signalled turns steer at a point a few metres ahead on each recent path, as seen from where the car is now. Pulls the car back when it has drifted wide or tight in a turn."),
-        ("PathSteeringGeometry", "path steering: shape", "With path steering on, reads how hard the path turns from its shape instead of its yaw rate and planned speed. Steadier when the plan expects to creep through a corner.")):
-      try:
-        ui_state.params.get_bool(key)
-        self.path_extra_toggles.append(BigParamControl(tr(title), key, description=tr(about)))
-      except Exception:
-        pass
-
     self.main_items = [self.current_model_info, self.select_model_btn, self.cancel_download_btn, self.refresh_btn, self.clear_cache_btn,
-                       self.blendful_toggle] + ([self.path_steering_toggle] if self.path_steering_toggle is not None else []) + self.path_extra_toggles
+                       self.blendful_toggle]
     self._scroller.add_widgets(self.main_items)
 
   @property
@@ -232,10 +213,6 @@ class ModelsLayoutMici(NavScroller):
   def show_event(self):
     super().show_event()
     self.blendful_toggle.refresh()
-    if self.path_steering_toggle is not None:
-      self.path_steering_toggle.refresh()
-    for toggle in self.path_extra_toggles:
-      toggle.refresh()
 
   def _update_state(self):
     super()._update_state()
@@ -255,14 +232,6 @@ class ModelsLayoutMici(NavScroller):
     blendful_value = "" if blendful_allowed else tr("custom model only")
     if self.blendful_toggle.value != blendful_value:
       self.blendful_toggle.set_value(blendful_value)
-    if self.path_steering_toggle is not None:
-      self.path_steering_toggle.set_checked(blendful_allowed and ui_state.params.get_bool("PathSteering"))
-      self.path_steering_toggle.set_enabled(blendful_allowed)
-      if self.path_steering_toggle.value != blendful_value:
-        self.path_steering_toggle.set_value(blendful_value)
-    for toggle in self.path_extra_toggles:
-      toggle.set_checked(blendful_allowed and ui_state.params.get_bool(toggle.param))
-      toggle.set_enabled(blendful_allowed)
 
     manager = self.model_manager
     self._download_frame += 1

openpilot/sunnypilot/modeld_v2/compile_warp.py

@@ -18,28 +18,17 @@ def main():
   parser.add_argument("--warp-to", required=True, help="model input WxH")
   parser.add_argument("--output", required=True)
   parser.add_argument("--light", action="store_true")
-  parser.add_argument("--turn", action="store_true")
-  parser.add_argument("--split-frames", action="store_true", help="one input per camera (frame, big_frame), bound to the camera buffers by pointer")
   args = parser.parse_args()
 
   from examples.openpilot.compile_warp import make_frame_prepare, parse_frame, parse_size
   from tinygrad import Device, Tensor, TinyJit
   frame = parse_frame(args.frame)
   warp = make_frame_prepare(frame, *parse_size(args.warp_to))
-  n = 1 if args.light or args.turn else 2
-  cams = [1] if args.turn else list(range(n))
+  n = 1 if args.light else 2
+  run = TinyJit(lambda input_frame, M_inv: Tensor.stack(*(warp(input_frame[i], M_inv[i]) for i in range(n))), prune=True)
   rng = np.random.default_rng(42)
-  frames = rng.integers(0, 256, (2, frame.size), dtype=np.uint8)
-  m_inv = Tensor((rng.standard_normal((n, 3, 3)) * 8).astype(np.float32), device=Device.DEFAULT).realize()
-  if args.split_frames:
-    names = [("frame", "big_frame")[c] for c in cams]
-    run = TinyJit(lambda M_inv, **f: Tensor.stack(*(warp(f[nm], M_inv[i]) for i, nm in enumerate(names))), prune=True)
-    inputs = {nm: Tensor(frames[c], device=Device.DEFAULT).realize() for nm, c in zip(names, cams, strict=True)} | {"M_inv": m_inv}
-    specs = {nm: ((frame.size,), "|u1", Device.DEFAULT) for nm in names} | {"M_inv": ((n, 3, 3), "<f4", Device.DEFAULT)}  # noqa: C420
-  else:
-    run = TinyJit(lambda input_frame, M_inv: Tensor.stack(*(warp(input_frame[c], M_inv[i]) for i, c in enumerate(cams))), prune=True)
-    inputs = {"input_frame": Tensor(frames, device=Device.DEFAULT).realize(), "M_inv": m_inv}
-    specs = {"input_frame": ((2, frame.size), "|u1", Device.DEFAULT), "M_inv": ((n, 3, 3), "<f4", Device.DEFAULT)}
+  inputs = {"input_frame": Tensor(rng.integers(0, 256, (2, frame.size), dtype=np.uint8), device=Device.DEFAULT).realize(),
+            "M_inv": Tensor((rng.standard_normal((n, 3, 3)) * 8).astype(np.float32), device=Device.DEFAULT).realize()}
   with record_kernel_linear() as recorded:
     run(**inputs)
     run(**inputs)
@@ -47,6 +36,7 @@ def main():
   assert len(recorded) == 1, f"expected one captured kernel list, got {len(recorded)}"
   part = serialize_jit(run, recorded[0], check)
   PlainJit(*part)
+  specs = {"input_frame": ((2, frame.size), "|u1", Device.DEFAULT), "M_inv": ((n, 3, 3), "<f4", Device.DEFAULT)}
   write_bundle(args.output, {"run": part}, {"input_specs": specs, "metadata": {}})
   print(f"Saved warp to {args.output} ({len(part[0]['calls'])} kernels)")
 

openpilot/sunnypilot/modeld_v2/frame_fusion.py

@@ -19,12 +19,10 @@ def pack_layout(shapes: dict[str, tuple], frame_size: int) -> tuple[dict[str, in
     offsets[name], offset = offset, offset + -(-math.prod(shape) * 4 // 128) * 128
   return offsets | {"frames": offset}, offset + 2 * frame_size
 
-EXTRA_WARPS = {"light_img": "light_tfm", "turn_img": "turn_tfm"}
-SPLIT_FRAMES = ("big_frame", "frame")
 
 def host_shapes(input_shapes: dict[str, tuple], states) -> dict[str, tuple]:
-  shapes = {name: shape for name, shape in input_shapes.items() if name not in states and name != "new_img" and name not in EXTRA_WARPS}
-  return shapes | {tfm: (1, 3, 3) for img, tfm in EXTRA_WARPS.items() if img in input_shapes}
+  shapes = {name: shape for name, shape in input_shapes.items() if name not in states and name not in ("new_img", "light_img")}
+  return shapes | ({"light_tfm": (1, 3, 3)} if "light_img" in input_shapes else {})
 
 
 class _Blobs:  # the model's and the warp's blobs as one address space, for a part whose ranges span both files
@@ -37,35 +35,28 @@ class _Blobs:  # the model's and the warp's blobs as one address space, for a pa
 
 
 def fused_with_warp(cam_w: int, cam_h: int, chestnut: bool):  # load_model's extend: a native model runs upload, warp and model as one submit
-  models, big = f"{BASEDIR}/openpilot/sunnypilot/modeld_v2/models", 'big_' if chestnut else ''
-  return partial(frame_part, f"{models}/{big}driving_warp_{cam_w}x{cam_h}_tinygrad.pkl",
-                 light_warp_path=f"{models}/{big}driving_lightwarp_{cam_w}x{cam_h}_tinygrad.pkl",
-                 turn_warp_path=f"{models}/{big}driving_turnwarp_{cam_w}x{cam_h}_tinygrad.pkl")
+  models = f"{BASEDIR}/openpilot/sunnypilot/modeld_v2/models"
+  return partial(frame_part, f"{models}/{'big_' if chestnut else ''}driving_warp_{cam_w}x{cam_h}_tinygrad.pkl",
+                 light_warp_path=f"{models}/driving_lightwarp_{cam_w}x{cam_h}_tinygrad.pkl")
 
 
-def frame_part(warp_path: str, parts: dict, extras: dict, light_warp_path: str | None = None, turn_warp_path: str | None = None) -> dict:
+def frame_part(warp_path: str, parts: dict, extras: dict, light_warp_path: str | None = None) -> dict:
   if "input_specs" not in extras:  # a legacy model warps in its own parts
     return {}
   (model, model_blob), (warp, warp_blob) = parts["run"], read_parts(warp_path)[0]["run"]
   states = sorted(name for name in extras["input_specs"] if f"next_{name}" in extras["metadata"]["output_shapes"])
   host = host_shapes({name: shape for name, (shape, _, _) in extras["input_specs"].items()}, states)
-  paths = {"light_img": light_warp_path, "turn_img": turn_warp_path}
-  extra = [(img, tfm, *read_parts(paths[img])[0]["run"]) for img, tfm in EXTRA_WARPS.items() if img in extras["input_specs"]]
-  split = any(s["name"] in SPLIT_FRAMES for s in warp["inputs"])
-  assert all(any(s["name"] in SPLIT_FRAMES for s in part["inputs"]) == split for _, _, part, _ in extra), "camera and extra warps must share a frame layout"
-  offsets, packed = pack_layout(host, 0 if split else next(s["numel"] for s in warp["inputs"] if s["name"] == "input_frame") // 2)
-  shift = len(model_blob)
-  buf_base, blob_base = [len(model["buffers"]) + len(warp["buffers"])], [len(model_blob) + len(warp_blob)]
-  for _, _, part, blob in extra:
-    buf_base.append(buf_base[-1] + len(part["buffers"])); blob_base.append(blob_base[-1] + len(blob))
-  upload, u8 = buf_base[-1], repr(_dtype("dtypes.uint8"))
-  extra_at = {img: (part, buf_base[i], blob_base[i], tfm) for i, (img, tfm, part, _) in enumerate(extra)}
+  light = "light_img" in extras["input_specs"]
+  light_warp, light_blob = read_parts(light_warp_path)[0]["run"] if light else ({"buffers": [], "calls": [], "inputs": []}, memoryview(b""))
+  frame_size = next(s["numel"] for s in warp["inputs"] if s["name"] == "input_frame") // 2
+  offsets, packed = pack_layout(host, frame_size)
+  shift, light_shift = len(model_blob), len(model_blob) + len(warp_blob)
+  upload, u8 = len(model["buffers"]) + len(warp["buffers"]) + len(light_warp["buffers"]), repr(_dtype("dtypes.uint8"))
 
   def view(name: str, nbytes: int, dtype: str) -> dict:
     return {"buffer": upload, "offset": offsets[name], "nbytes": nbytes, "dtype": dtype}
   outputs = sorted(s["slot"] for s in model["inputs"] if s["name"] is None)
-  frame_specs = {s["name"]: s for _, _, part, _ in [(None, None, warp, None), *extra] for s in part["inputs"] if s["name"] in SPLIT_FRAMES}
-  names = sorted(["host", *states, *frame_specs])
+  names = sorted(["host", *states])
   slots = {s["slot"]: names.index(s["name"]) for s in model["inputs"] if s["name"] in states} | \
           {slot: len(names) + i for i, slot in enumerate(outputs)}
   def warped(part: dict, base: int) -> dict:
@@ -73,24 +64,22 @@ def frame_part(warp_path: str, parts: dict, extras: dict, light_warp_path: str |
     return {k: v for k, v in returned.items() if k != "shape"} | {"buffer": returned["buffer"] + base}
   model_inputs = {s["slot"]: {"input": slots[s["slot"]]} if s["slot"] in slots else
                   warped(warp, len(model["buffers"])) if s["name"] == "new_img" else
-                  warped(extra_at[s["name"]][0], extra_at[s["name"]][1]) if s["name"] in extra_at else
+                  warped(light_warp, len(model["buffers"]) + len(warp["buffers"])) if s["name"] == "light_img" else
                   view(s["name"], s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"]) for s in model["inputs"]}
   def warp_inputs(part: dict, tfm: str) -> dict:
-    return {s["slot"]: {"input": names.index(s["name"])} if s["name"] in frame_specs else
-            view("frames" if s["name"] == "input_frame" else tfm, s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"]) for s in part["inputs"]}
+    return {s["slot"]: view("frames" if s["name"] == "input_frame" else tfm, s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"]) for s in part["inputs"]}
 
   def remap(calls: list, inputs: dict, buffers: int, lib: int) -> list:
     return [call | ({"lib": [call["lib"][0] + lib, call["lib"][1] + lib]} if "lib" in call else {}) |
             {"args": [dict(inputs[a["input"]]) if "input" in a else a | {"buffer": a["buffer"] + buffers} for a in call["args"]]} for call in calls]
   header = {"input_names": names, "returns": {"kind": "NoneType", "tensors": []}, "check": None,
             "inputs": [{"slot": names.index("host"), "name": "host", "dtype": u8, "numel": packed, "device": "NPY"}] +
-                      [s | {"slot": slots[s["slot"]]} for s in model["inputs"] if s["slot"] in slots] +
-                      [s | {"slot": names.index(name)} for name, s in frame_specs.items()],
+                      [s | {"slot": slots[s["slot"]]} for s in model["inputs"] if s["slot"] in slots],
             "buffers": model["buffers"] + [b | ({"init": [b["init"][0] + d, b["init"][1] + d]} if "init" in b else {})
-                                           for part, d in ((warp, shift), *((p, bb) for p, _, bb, _ in extra_at.values())) for b in part["buffers"]] +
+                                           for part, d in ((warp, shift), (light_warp, light_shift)) for b in part["buffers"]] +
                        [{"nbytes": packed, "device": model["inputs"][0]["device"], "dtype": u8}],
             "calls": [{"kind": "copy", "args": [{"buffer": upload, "offset": 0, "nbytes": packed, "dtype": u8}, {"input": names.index("host")}]}] +
                      remap(warp["calls"], warp_inputs(warp, "tfm"), len(model["buffers"]), shift) +
-                     [c for p, b0, bb, tfm in extra_at.values() for c in remap(p["calls"], warp_inputs(p, tfm), b0, bb)] +
+                     remap(light_warp["calls"], warp_inputs(light_warp, "light_tfm"), len(model["buffers"]) + len(warp["buffers"]), light_shift) +
                      remap(model["calls"], model_inputs, 0, 0)}
-  return {"run": (header, _Blobs(model_blob, warp_blob, *(blob for *_, blob in extra)))}
+  return {"run": (header, _Blobs(model_blob, warp_blob, light_blob) if light else _Blobs(model_blob, warp_blob))}

openpilot/sunnypilot/modeld_v2/helpers.py

@@ -270,32 +270,15 @@ def _program_uop(call: dict, lib: bytes):
                                UOp(Ops.SOURCE, arg=""), UOp(Ops.BINARY, arg=lib)), arg=info)
 
 
-def _input_tensors(kwargs: dict) -> list:  # in _prepare_jit_inputs' order
-  from tinygrad import Tensor
-  tensors = [t for _, t in sorted(kwargs.items()) if t.__class__ is Tensor]
-  for value in kwargs.values():
-    tensors += [t for t in (value.values() if isinstance(value, dict) else value if isinstance(value, (tuple, list)) else ())
-                if t.__class__ is Tensor and not any(t is y for y in tensors)]
-  return tensors
-
-
 class _Jit:
   def __init__(self, captured, input_names: list, buffers: list):
     self.captured, self.input_names, self.buffers = captured, input_names, buffers
 
   def __call__(self, *args, **kwargs):
     import tinygrad.engine.jit as jit
-    from tinygrad.uop.ops import Ops
-    # same names, no variables: the binding is the tensors' own uops, without _prepare_jit_inputs' graph rewrites
-    if not args and self.__dict__.get("_names") == sorted(kwargs):
-      tensors = _input_tensors(kwargs)
-      if all(t.uop.is_realized for t in tensors):
-        return self.captured([t.uop.src[0] if t.uop.op is Ops.UNSHARD else t.uop for t in tensors], {})
     input_uops, var_vals, names, _ = jit._prepare_jit_inputs(args, kwargs)
     if names != self.input_names:
       raise ValueError(f"expected inputs {self.input_names}, got {names}")
-    if not args and not var_vals:
-      self._names = sorted(kwargs)
     return self.captured(input_uops, var_vals)
 
 

openpilot/sunnypilot/modeld_v2/model_adapters.py

@@ -11,7 +11,7 @@ import pickle
 import numpy as np
 
 from openpilot.common.basedir import BASEDIR
-from openpilot.sunnypilot.modeld_v2.frame_fusion import host_shapes, pack_layout, SPLIT_FRAMES
+from openpilot.sunnypilot.modeld_v2.frame_fusion import host_shapes, pack_layout
 from openpilot.sunnypilot.modeld_v2.helpers import load_model
 from openpilot.sunnypilot.modeld_v2.compile_modeld import (POLICY_INPUTS, derive_frame_skip,
                                                            make_split_input_queues, make_supercombo_input_queues)
@@ -169,18 +169,16 @@ class NativeTinygradAdapter(BaseModelAdapter):
     self.warp_frame_size = self.frame_copy_size
     self._init_common()
     self.run_model = self.jits['run']
-    self.split = [name for name in SPLIT_FRAMES if name in self.run_model.input_names]
-    self.frame_inputs: dict[str, Tensor] = {}
 
     self.input_queues = {name: Tensor(np.zeros(shape, dtype=dtype), device=self.model_device).realize()
                          for name, (shape, dtype) in self.input_shapes.items() if name in self.state_pairs}
     shapes = host_shapes({name: shape for name, (shape, _) in self.input_shapes.items()}, self.state_pairs)
-    offsets, packed = pack_layout(shapes, 0 if self.split else self.warp_frame_size)
+    offsets, packed = pack_layout(shapes, self.warp_frame_size)
     self.packed_input = np.zeros(packed, dtype=np.uint8)
     self.host = Tensor(self.packed_input, device='NPY')
     self.numpy_inputs = {name: np.ndarray(shape, dtype=np.float32, buffer=self.packed_input, offset=offsets[name])
                          for name, shape in ({'tfm': (2, 3, 3)} | shapes).items()}
-    self.frames = self.packed_input[offsets['frames']:].reshape(2, -1)
+    self.frames = self.packed_input[offsets['frames']:].reshape(2, self.warp_frame_size)
 
     self.outputs = {name: Tensor(np.zeros(shape, dtype=dtype), device=device).realize() for name, (shape, dtype, device) in self.jits['output_specs'].items()}
     for name, next_name in self.state_pairs.items():
@@ -189,29 +187,17 @@ class NativeTinygradAdapter(BaseModelAdapter):
 
   def copy_frames(self, bufs):
     for i, key in enumerate(self._vision_input_names):
-      if key not in bufs:
-        continue
-      if not self.split:
+      if key in bufs:
         data = bufs[key].data if hasattr(bufs[key], 'data') else bufs[key]
         np.copyto(self.frames[i], np.frombuffer(data, dtype=np.uint8, count=self.warp_frame_size))
-      elif (name := ('frame', 'big_frame')[i]) in self.split:
-        if isinstance(bufs[key], np.ndarray):  # cached memory: the GPU would read stale lines in place
-          self.frame_inputs[name] = Tensor(np.ascontiguousarray(bufs[key][:self.warp_frame_size]), device=self.model_device).realize()
-          continue
-        ptr = np.frombuffer(bufs[key].data, dtype=np.uint8).ctypes.data  # visionipc buffers are coherent
-        if (key, ptr) not in self._blob_cache:
-          self._blob_cache[(key, ptr)] = Tensor.from_blob(ptr, (self.warp_frame_size,), dtype='uint8', device=self.model_device)
-        self.frame_inputs[name] = self._blob_cache[(key, ptr)]
 
   def reset_warmup_buffers(self) -> None:
     self.packed_input[:] = 0
-    self.frame_inputs.clear()
-    self._blob_cache.clear()
     for key in self.state_pairs:
       self.input_queues[key].assign(0).realize()
 
   def run(self):
-    self.run_model(host=self.host, **self.input_queues, **self.frame_inputs, output_buffers=self.outputs)
+    self.run_model(host=self.host, **self.input_queues, output_buffers=self.outputs)
     return self.outputs['outputs']
 
 

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -41,12 +41,10 @@ from openpilot.selfdrive.modeld.modeld import ChestnutGpuState
 from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
 from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser
 from openpilot.sunnypilot.modeld_v2.blendful import Blendful
-from openpilot.sunnypilot.modeld_v2 import recent_paths
 from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
 from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
 from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
 from openpilot.sunnypilot.modeld_v2.light_frame import get_light_warp_matrix
-from openpilot.sunnypilot.modeld_v2.turn_frame import get_turn_warp_matrix
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.modeld_v2.helpers import load_model
@@ -143,9 +141,6 @@ class ModelState(ModelStateBase):
       self.constants = ModelConstants()
 
     self.parser = Parser()
-    self.recent_paths = recent_paths.SignalSteer(self.constants.T_IDXS)  # path steering (the PathSteering toggle)
-    self.turn_pulse_age = 1e3  # s since the model was last given a turn pulse
-    self.path_share = 0.0      # how far the steering has moved over to path steering
     self.prev_desire = np.zeros(self.constants.DESIRE_LEN, dtype=np.float32)
 
   def warmup(self) -> None:
@@ -173,10 +168,8 @@ class ModelState(ModelStateBase):
 
     desire_key = self.desire_key
     inputs[desire_key][0] = 0
-    pulse = np.where(inputs[desire_key] - self.prev_desire > .99, inputs[desire_key], 0)
-    (self.numpy_inputs[desire_key].flat if self.adapter.is_native else self.numpy_inputs[desire_key])[:] = pulse
-    # a turn counts as signalled while its pulse is still in the model's own cue history (recent_paths.py)
-    self.turn_pulse_age = 0. if float(np.max(np.ravel(pulse)[1:3])) > 0. else self.turn_pulse_age + DT_MDL
+    (self.numpy_inputs[desire_key].flat if self.adapter.is_native else self.numpy_inputs[desire_key])[:] = \
+      np.where(inputs[desire_key] - self.prev_desire > .99, inputs[desire_key], 0)
     self.prev_desire[:] = inputs[desire_key]
 
     for key in ('traffic_convention', 'lateral_control_params', 'action_t', 'speed_limit'):
@@ -189,8 +182,6 @@ class ModelState(ModelStateBase):
           self.numpy_inputs['tfm'][i] = transforms[key].reshape(3, 3)
       if 'light_tfm' in self.numpy_inputs and 'light' in transforms:
         self.numpy_inputs['light_tfm'][0] = transforms['light'].reshape(3, 3)
-      if 'turn_tfm' in self.numpy_inputs and 'turn' in transforms:
-        self.numpy_inputs['turn_tfm'][0] = transforms['turn'].reshape(3, 3)
     else:
       self.numpy_inputs['tfm'][:, :] = transforms[self._road_key].reshape(3, 3)
       self.numpy_inputs['big_tfm'][:, :] = transforms[self._wide_key].reshape(3, 3)
@@ -243,8 +234,7 @@ class ModelState(ModelStateBase):
 
   def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
                             lat_action_t: float, long_action_t: float, v_ego: float,
-                            blendful: Callable[[float], float] | None = None,
-                            path_steering: bool = False, car_curvature: float = 0.0) -> log.ModelDataV2.Action:
+                            blendful: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
     if 'action' not in model_output:
       plan = model_output['plan'][0]
       desired_accel = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
@@ -258,17 +248,6 @@ class ModelState(ModelStateBase):
       desired_curvature = model_output['action'][0, 0] / (max(2.5, v_ego))**2
     if blendful is not None:
       desired_curvature += blendful(desired_curvature)
-    if 'action' in model_output and 'plan' in model_output:
-      # path steering (recent_paths.py): kept up to date on every frame, so the toggle can come on mid-drive, and
-      # moved over to gradually when it does
-      path_plan = model_output['plan'][0]
-      from_paths = self.recent_paths.update(path_plan[:, Plan.ORIENTATION_RATE][:, 2], path_plan[:, Plan.VELOCITY][:, 0],
-                                            desired_curvature, self.turn_pulse_age < recent_paths.CUE_S,
-                                            v_ego, lat_action_t, DT_MDL, path_plan[:, Plan.POSITION],
-                                            path_plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], car_curvature)
-      most = DT_MDL / recent_paths.FADE
-      self.path_share += float(np.clip((1. if path_steering else 0.) - self.path_share, -most, most))
-      desired_curvature = self.path_share * from_paths + (1. - self.path_share) * desired_curvature
 
     stop = v_ego < 0.3 and desired_accel < 0.1
     desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
@@ -376,7 +355,6 @@ def main(demo=False):
   model_transform_main = np.zeros((3, 3), dtype=np.float32)
   model_transform_extra = np.zeros((3, 3), dtype=np.float32)
   model_transform_light = np.zeros((3, 3), dtype=np.float32)
-  model_transform_turn = np.zeros((3, 3), dtype=np.float32)
   live_calib_seen = False
   buf_main, buf_extra = None, None
   meta_main = FrameMeta()
@@ -402,7 +380,6 @@ def main(demo=False):
   RELC = RoadEdgeLaneChangeController()
   blendful = Blendful()
   blendful_enabled = False
-  path_steering = False
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -447,15 +424,6 @@ def main(demo=False):
       model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True)
       camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True))
       blendful_enabled = params.get_bool("Blendful")
-      try:
-        path_steering = params.get_bool("PathSteering")
-      except Exception:  # the key is unknown until the build that adds it has run
-        path_steering = False
-      for key, attr in (("PathSteeringPursuit", "pursuit"), ("PathSteeringGeometry", "geometric")):
-        try:
-          setattr(model.recent_paths, attr, params.get_bool(key))
-        except Exception:  # as above
-          setattr(model.recent_paths, attr, False)
     lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS
     if sm.updated["extrinsicsCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["extrinsicsCalibration"].rpyCalib, dtype=np.float32)
@@ -465,7 +433,6 @@ def main(demo=False):
       model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.wide_road.intrinsics, True).astype(np.float32)
       model_transform_main, model_transform_extra = camera_offset_helper.update(model_transform_main, model_transform_extra, sm, main_wide_camera)
       model_transform_light = get_light_warp_matrix(device_from_calib_euler, main_intrinsics)
-      model_transform_turn = get_turn_warp_matrix(device_from_calib_euler, dc.wide_road.intrinsics)
       live_calib_seen = True
 
     traffic_convention = np.zeros(2)
@@ -488,7 +455,6 @@ def main(demo=False):
     bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names}
     transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names}
     transforms['light'] = model_transform_light
-    transforms['turn'] = model_transform_turn
 
     frame_delay = DT_MDL # compensate for time passed since the frame was captured: current_time - timestamp_eof is 50ms on average
     action_delay = DT_MDL / 2 # middle of the interval between model output (current state) and next frame (expected state)
@@ -539,8 +505,7 @@ def main(demo=False):
         prev_action.desiredCurvature = sm['controlsState'].curvature
       blendful_active = desire == log.Desire.none and DH.lane_change_state == log.LaneChangeState.off
       action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego,
-                                           partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled),
-                                           path_steering=path_steering, car_curvature=sm['controlsState'].curvature)
+                                           partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled))
       prev_action = action
       fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
                      publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,

openpilot/sunnypilot/modeld_v2/models/big_driving_lightwarp_1344x760_tinygrad.pkl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:1baac6f02902df5689e9e275633c122a9955b7c57de2681e0137e1724c1f32dd
-size 3463278

openpilot/sunnypilot/modeld_v2/models/big_driving_lightwarp_1928x1208_tinygrad.pkl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:0478837491c584b2212e31a91c494a908b8fb048205700f05a02a13ee7fc9b90
-size 7690094

openpilot/sunnypilot/modeld_v2/models/driving_lightwarp_1344x760_tinygrad.pkl

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:580fadf61ded34b4b19f0ba1f5e4b61c0ea489ca34a0952ccf2878593e3f68c4
-size 1851239
+oid sha256:c08cffa27671d6773e316fdf4dcab80ffce2c4cc6fa28775615bc6726a63d7ba
+size 3473263

openpilot/sunnypilot/modeld_v2/models/driving_lightwarp_1928x1208_tinygrad.pkl

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:44700f3533abb3e65232c8aca46b48384a59218545ed6bb505c5d4762204359b
-size 3964263
+oid sha256:6cddff2e3320057735fd4812d1ef9cc7d6d9fe405e3b6e7c4df85ee654a0e886
+size 7699823

openpilot/sunnypilot/modeld_v2/models/driving_turnwarp_1344x760_tinygrad.pkl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:7b1416dec9a7e1f1a6f70592056da60c1ce4fba5b5e2929b4d8a0acb437e010b
-size 1851243

openpilot/sunnypilot/modeld_v2/models/driving_turnwarp_1928x1208_tinygrad.pkl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:86197ec6b944ec962b8e7e1ed82836e593b78ddc8009c7c2852ac69e7e9750ad
-size 3964267

openpilot/sunnypilot/modeld_v2/models/driving_warp_1344x760_tinygrad.pkl

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:1f5dd8855de07d0e8b9ed836934e1dcbe77aa27c3b5c77eee1f21888f974ba8b
-size 3699072
+oid sha256:4b22fd6bcb3a3e8a642e6866449dd9c30c12e694ff0741cad9e34e4b00d8c5fa
+size 3698799

openpilot/sunnypilot/modeld_v2/models/driving_warp_1928x1208_tinygrad.pkl

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:226d40112a292077b7eca26b97273c56ab3a84feee8ddf9b6d02d46c2a7b8403
-size 7924864
+oid sha256:9a17fd6acece4fab23784ef0f3f8bba28a049e9599e5e9e5ee7b36e4b1d24a6f
+size 7924847

openpilot/sunnypilot/modeld_v2/recent_paths.py

@@ -1,163 +0,0 @@
-"""
-Steering from the model's recent paths.
-
-A big model with an action output steers from that output: a lateral acceleration, divided by the speed squared. At
-town speeds a small error in it is a large one at the wheel (0.1 m/s2 is 22 degrees at 7 mph), and the wheel twitches.
-The model's path does not have that problem, and every path it made in the last second also says how hard the road
-turns where the car will be when the steering asked now arrives. Paths that agree about the road give that with no
-delay, unlike an average of past steering; one frame's own error counts for one in twenty.
-
-Each path is read by distance - where the car has really got to since the path was made, along the path's own
-length - so a driver who goes slower or faster than the model expected does not shift the turn.
-
-Two asks come out of the recent paths:
-  own       the mean of what the paths say. Used while a turn is signalled: the car commits to the turn its paths show.
-  anchored  each frame's action, moved on by the change its own path expects since, averaged: the action's level, the
-            paths' shape. Used with no signal: the action decides and the paths only smooth it, because a path alone,
-            at a junction nobody signalled for, can talk itself into a turn.
-On faster roads the action steers alone, as it always has.
-
-A model trained on the driver's own lines (the closed-loop fine-tunes) places its turns with its action, and the paths'
-own ask under-steers them; for such a model the anchored ask is used in signalled turns too, over a shorter window.
-That is switched by the file ANCHOR_SWITCH, read when a drive starts.
-
-Two more switches, both off unless their toggles are on:
-  pursuit    (PathSteeringPursuit) the paths' own ask above knows the road's shape but not where the car is now: each
-             path was planned from where the car was when it was made, so drift since is never pulled back. With
-             pursuit, every recent path is placed in the car's frame of now (by dead reckoning the car's own
-             curvature since the path was made) and steered at like a driver looking ahead: the curvature of the arc
-             from the car to a point on the path a short way ahead (pure pursuit). Averaged over the recent paths as
-             before, so still smooth, but a car that has drifted wide or tight is steered back. Used in place of the
-             paths' own ask in signalled turns.
-  geometric  (PathSteeringGeometry) read each path's curvature from its shape, the change in heading per metre over a
-             few metres, and measure distance along it from its points, instead of yaw rate over planned speed. A plan
-             that expects to creep through a corner has a small planned speed and so a noisy, too large yaw-rate
-             curvature; its shape does not.
-"""
-
-import os
-
-import numpy as np
-
-MIN_SPEED = 1.0       # m/s, as openpilot's steer-from-plan formula
-WINDOW = 1.0          # s of recent paths that count
-TOWN = (9.0, 13.0)    # m/s: below, the recent paths take part; above, the action steers alone; a mix in between
-FADE = 0.6            # s to move over between the anchored ask and the paths' own when the signal changes
-CUE_S = 6.6           # s a turn pulse stays in the model's own cue history: that long, a turn counts as signalled
-ANCHOR_SWITCH = '/data/recent_paths_anchor'  # exists: the anchored ask in signalled turns too (a trained model)
-ANCHOR_WINDOW = 0.6   # s of recent paths that count with the anchored ask in turns
-PURSUIT_MIN = 4.0     # m: the nearest the pursuit point is ever looked at
-PURSUIT_LEAD = 0.5    # s of driving, past the steering delay, to the pursuit point
-PURSUIT_TAU = 0.3     # s: the pursuit ask weighs each recent path by exp(-age / this), as its dead reckoning ages
-GEOM_SPAN = 2.0       # m either side over which the geometric curvature measures the change in heading
-GEOM_MIN_LEN = 3.0    # m: a path shorter than this (the car about to stop) is read by yaw rate, as without the switch
-
-
-def _geometric_curvature(along, yaws):
-  """Curvature (1/m) at each of a path's points, from the change in heading over GEOM_SPAN either side of it."""
-  lo = np.maximum(along - GEOM_SPAN, along[0])
-  hi = np.minimum(along + GEOM_SPAN, along[-1])
-  return (np.interp(hi, along, yaws) - np.interp(lo, along, yaws)) / np.maximum(hi - lo, 1e-3)
-
-
-class RecentPaths:
-  def __init__(self, t_idxs, window: float = WINDOW):
-    self.t = np.asarray(t_idxs, dtype=np.float64)
-    self.window = float(window)
-    self.held: list[dict] = []
-    self.pursuit = 0.0  # the pursuit ask of the last update, averaged over the recent paths (1/m)
-
-  def reset(self) -> None:
-    self.held = []
-    self.pursuit = 0.0
-
-  def _curvature(self, rates, speeds, at: float) -> float:
-    return float(np.interp(at, self.t, rates)) / max(float(np.interp(at, self.t, speeds)), MIN_SPEED)
-
-  def _curvature_at(self, path: dict, distance: float) -> float:
-    if path['geo'] is not None:
-      return float(np.interp(distance, path['along'], path['geo']))
-    return self._curvature(path['rates'], path['speeds'], float(np.interp(distance, path['along'], self.t)))
-
-  def update(self, yaw_rates, speeds, action_curvature: float, v_ego: float, action_t: float, dt: float,
-             positions=None, yaws=None, car_curvature: float = 0.0, geometric: bool = False) -> tuple[float, float]:
-    """(own, anchored) curvature asks for this frame, and self.pursuit. yaw_rates, speeds: this frame's path at the
-    model's time steps (rad/s, m/s); action_curvature: what the action alone asks (1/m); v_ego: the car's speed;
-    action_t: s until the steering asked now arrives; dt: s since the frame before. positions (x, y per time step, m,
-    the car's frame at this frame), yaws (rad) and car_curvature (what the car is really steering, 1/m) are needed for
-    the pursuit ask and the geometric curvature; without positions pursuit is the own ask."""
-    v = max(float(v_ego), 0.0)
-    for path in self.held:
-      path['age'] += dt
-      path['come'] += v * dt
-      turn = v * float(car_curvature) * dt
-      mid = path['pose'][2] + 0.5 * turn
-      path['pose'][0] += v * dt * np.cos(mid)
-      path['pose'][1] += v * dt * np.sin(mid)
-      path['pose'][2] += turn
-    self.held = [path for path in self.held if path['age'] <= self.window + 1e-9]
-
-    speeds = np.maximum(np.asarray(speeds, dtype=np.float64), 0.0)
-    xy = None if positions is None else np.asarray(positions, dtype=np.float64)[:, :2]
-    geo, along = None, None
-    if xy is not None:
-      geo_along = np.concatenate([[0.0], np.cumsum(np.hypot(*np.diff(xy, axis=0).T))]) + 1e-6 * np.arange(len(xy))
-      if geometric and yaws is not None and geo_along[-1] >= GEOM_MIN_LEN:
-        along = geo_along
-        geo = _geometric_curvature(along, np.unwrap(np.asarray(yaws, dtype=np.float64)))
-    if along is None:
-      along = np.concatenate([[0.0], np.cumsum(0.5 * (speeds[1:] + speeds[:-1]) * np.diff(self.t))])
-      along = along + 1e-6 * np.arange(len(along))
-    path = {'age': 0.0, 'come': 0.0, 'level': float(action_curvature), 'was': v * action_t,
-            'rates': np.array(yaw_rates, dtype=np.float64), 'speeds': speeds, 'along': along, 'geo': geo,
-            'xy': xy, 'xy_along': None if xy is None else geo_along, 'pose': [0.0, 0.0, 0.0]}
-    self.held.append(path)
-
-    own, anchored, pursuit = [], [], []
-    look = max(PURSUIT_MIN, v * (action_t + PURSUIT_LEAD))
-    for path in self.held:
-      there = self._curvature_at(path, path['come'] + v * action_t)
-      meant = self._curvature_at(path, path['was'])
-      own.append(there)
-      anchored.append(path['level'] + there - meant)
-      if path['xy'] is not None:
-        px, py, pyaw = path['pose']
-        s = path['come'] + look
-        dx = float(np.interp(s, path['xy_along'], path['xy'][:, 0])) - px
-        dy = float(np.interp(s, path['xy_along'], path['xy'][:, 1])) - py
-        ahead = np.cos(pyaw) * dx + np.sin(pyaw) * dy
-        side = -np.sin(pyaw) * dx + np.cos(pyaw) * dy
-        pursuit.append(2.0 * side / max(ahead * ahead + side * side, 1.0))
-      else:
-        pursuit.append(there)
-    weights = np.exp(-np.array([path['age'] for path in self.held]) / PURSUIT_TAU)
-    self.pursuit = float(np.dot(weights, pursuit) / weights.sum())
-    return float(np.mean(own)), float(np.mean(anchored))
-
-
-class SignalSteer:
-  """The steering ask of a frame from its action and the recent paths, by whether a turn is signalled."""
-
-  def __init__(self, t_idxs, window: float | None = None, fade: float = FADE, anchor_turns: bool | None = None):
-    self.anchor_turns = os.path.exists(ANCHOR_SWITCH) if anchor_turns is None else anchor_turns
-    self.paths = RecentPaths(t_idxs, (ANCHOR_WINDOW if self.anchor_turns else WINDOW) if window is None else window)
-    self.fade = float(fade)
-    self.signalled = 0.0  # how far the steering has moved over from the anchored ask to the paths' own
-    self.pursuit = False    # the PathSteeringPursuit toggle
-    self.geometric = False  # the PathSteeringGeometry toggle
-
-  def reset(self) -> None:
-    self.paths.reset()
-    self.signalled = 0.0
-
-  def update(self, yaw_rates, speeds, action_curvature: float, cued: bool, v_ego: float, action_t: float, dt: float,
-             positions=None, yaws=None, car_curvature: float = 0.0) -> float:
-    own, anchored = self.paths.update(yaw_rates, speeds, action_curvature, v_ego, action_t, dt,
-                                      positions, yaws, car_curvature, self.geometric)
-    if self.pursuit and positions is not None:
-      own = self.paths.pursuit
-    most = dt / self.fade
-    self.signalled += float(np.clip((1.0 if cued and not self.anchor_turns else 0.0) - self.signalled, -most, most))
-    ask = self.signalled * own + (1.0 - self.signalled) * anchored
-    fast = float(np.clip((v_ego - TOWN[0]) / (TOWN[1] - TOWN[0]), 0.0, 1.0))
-    return (1.0 - fast) * ask + fast * float(action_curvature)

openpilot/sunnypilot/modeld_v2/tests/test_recent_paths.py

@@ -1,159 +0,0 @@
-"""Path steering (recent_paths.py). Run on the device with:
-  /usr/local/venv/bin/python -m unittest openpilot.sunnypilot.modeld_v2.tests.test_recent_paths"""
-import unittest
-
-import numpy as np
-
-from openpilot.sunnypilot.modeld_v2 import recent_paths
-from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
-
-T = ModelConstants.T_IDXS
-
-
-def path(yaw_rate, v, harder=0.0):
-  """A path's yaw rates and speeds at the model's time steps: turning at yaw_rate now, `harder` more every second."""
-  t = np.asarray(T)
-  return yaw_rate + harder * t, np.full(len(t), float(v))
-
-
-class TestRecentPaths(unittest.TestCase):
-  def run_frames(self, steer, frames, yaw_rate, v, action, cued):
-    asked = 0.0
-    for _ in range(frames):
-      asked = steer.update(*path(yaw_rate, v), action, cued, v, 0.4, 0.05)
-    return asked
-
-  def test_with_no_signal_the_action_decides(self):
-    steer = recent_paths.SignalSteer(T, anchor_turns=False)
-    self.assertAlmostEqual(self.run_frames(steer, 60, 0.08, 4.0, 0.005, False), 0.005, places=9)
-
-  def test_while_a_turn_is_signalled_the_paths_steer(self):
-    steer = recent_paths.SignalSteer(T, anchor_turns=False)
-    self.assertAlmostEqual(self.run_frames(steer, 60, 0.08, 4.0, 0.005, True), 0.08 / 4.0, places=9)
-
-  def test_the_move_over_to_the_paths_takes_the_fade_time_and_never_jumps(self):
-    steer = recent_paths.SignalSteer(T, anchor_turns=False)
-    self.run_frames(steer, 40, 0.08, 4.0, 0.005, False)
-    asked = [steer.update(*path(0.08, 4.0), 0.005, True, 4.0, 0.4, 0.05) for _ in range(20)]
-    steps = np.diff([0.005] + asked)
-    self.assertTrue((steps >= -1e-12).all())
-    self.assertLessEqual(steps.max(), (0.02 - 0.005) * 0.05 / recent_paths.FADE + 1e-9)
-    self.assertAlmostEqual(asked[11], 0.02, places=9)   # 0.6 s at 20 Hz
-
-  def test_on_faster_roads_the_action_steers_alone(self):
-    steer = recent_paths.SignalSteer(T, anchor_turns=False)
-    self.assertAlmostEqual(self.run_frames(steer, 60, 0.4, 20.0, 0.005, True), 0.005, places=9)
-    mixed = recent_paths.SignalSteer(T, anchor_turns=False)
-    self.assertAlmostEqual(self.run_frames(mixed, 60, 0.22, 11.0, 0.005, True), 0.5 * 0.02 + 0.5 * 0.005, places=9)
-
-  def test_paths_that_agree_about_the_road_give_no_delay(self):
-    # every path knows the road turns harder the further along it is; the car drives slower than the paths expect
-    paths = recent_paths.RecentPaths(T)
-    road = lambda s: 0.002 * s
-    odometer, t = 0.0, np.asarray(T)
-    for _ in range(60):
-      rates = 4.0 * np.array([road(odometer + 4.0 * x) for x in t])
-      own, _ = paths.update(rates, np.full(len(t), 4.0), 0.0, 2.0, 0.5, 0.05)
-      here = odometer
-      odometer += 2.0 * 0.05
-    self.assertAlmostEqual(own, road(here + 2.0 * 0.5), delta=2e-3 * road(here + 1.0))
-
-  def test_one_frames_error_counts_for_little(self):
-    rng = np.random.default_rng(0)
-    paths, alone, averaged = recent_paths.RecentPaths(T), [], []
-    for _ in range(400):
-      rates, speeds = path(0.1 + rng.normal(0, 0.02), 4.0)
-      own, _ = paths.update(rates, speeds, 0.0, 4.0, 0.4, 0.05)
-      alone.append(rates[0] / 4.0)
-      averaged.append(own)
-    self.assertLess(np.std(averaged[40:]), 0.35 * np.std(alone[40:]))
-
-  def test_a_reset_forgets_everything(self):
-    steer = recent_paths.SignalSteer(T, anchor_turns=False)
-    self.run_frames(steer, 30, 0.08, 4.0, 0.005, True)
-    steer.reset()
-    fresh = recent_paths.SignalSteer(T, anchor_turns=False)
-    self.assertAlmostEqual(steer.update(*path(0.0, 4.0), 0.01, False, 4.0, 0.4, 0.05),
-                           fresh.update(*path(0.0, 4.0), 0.01, False, 4.0, 0.4, 0.05), places=12)
-
-  # the two switches (PathSteeringPursuit, PathSteeringGeometry)
-
-  def straight_path(self, v, side=0.0):
-    """A straight path at speed v, `side` m to the left of the car and parallel to it, in the car's frame."""
-    t = np.asarray(T)
-    xy = np.stack([v * t, np.full(len(t), side)], 1)
-    xy[0, 1] = 0.0
-    return np.zeros(len(t)), np.full(len(t), float(v)), xy, np.zeros(len(t))
-
-  def test_switches_off_change_nothing(self):
-    plain, wired = recent_paths.SignalSteer(T, anchor_turns=False), recent_paths.SignalSteer(T, anchor_turns=False)
-    rng = np.random.default_rng(1)
-    for _ in range(40):
-      rates, speeds = path(0.1 + rng.normal(0, 0.02), 4.0)
-      xy = np.stack([4.0 * np.asarray(T), rng.normal(0, 0.3, len(T))], 1)
-      a = plain.update(rates, speeds, 0.01, True, 4.0, 0.4, 0.05)
-      b = wired.update(rates, speeds, 0.01, True, 4.0, 0.4, 0.05, xy, rng.normal(0, 0.1, len(T)), 0.02)
-      self.assertAlmostEqual(a, b, places=12)
-
-  def test_pursuit_steers_back_to_a_path_the_car_has_drifted_off(self):
-    # every path says: go straight, 0.5 m to the left. The paths' own ask (curvature only) never moves the car there
-    steer = recent_paths.SignalSteer(T, anchor_turns=False)
-    steer.pursuit = True
-    for _ in range(40):
-      rates, speeds, xy, yaws = self.straight_path(4.0, side=0.5)
-      asked = steer.update(rates, speeds, 0.0, True, 4.0, 0.4, 0.05, xy, yaws, 0.0)
-    look = max(recent_paths.PURSUIT_MIN, 4.0 * (0.4 + recent_paths.PURSUIT_LEAD))
-    self.assertGreater(asked, 0.5 * 2 * 0.5 / look ** 2)  # steers left, about as hard as an arc onto the path
-    self.assertLess(asked, 2.0 * 2 * 0.5 / look ** 2)
-
-  def test_pursuit_follows_a_steady_turn_exactly(self):
-    # on an arc the car is already on, pure pursuit asks for the arc's own curvature
-    k, v = 0.05, 4.0
-    t = np.asarray(T)
-    s = v * t
-    xy, yaws = np.stack([np.sin(k * s) / k, (1 - np.cos(k * s)) / k], 1), k * s
-    steer = recent_paths.SignalSteer(T, anchor_turns=False)
-    steer.pursuit = True
-    for _ in range(40):
-      asked = steer.update(np.full(len(t), k * v), np.full(len(t), v), k, True, v, 0.4, 0.05, xy, yaws, k)
-    self.assertAlmostEqual(asked, k, delta=0.02 * k)
-
-  def test_pursuit_knows_the_car_has_moved_since_an_old_path(self):
-    # one path to the left; then the car really moves over and the paths stop coming: it is no longer asked to turn
-    paths = recent_paths.RecentPaths(T)
-    rates, speeds, xy, yaws = self.straight_path(4.0, side=0.5)
-    paths.update(rates, speeds, 0.0, 4.0, 0.4, 0.05, xy, yaws, 0.0)
-    first = paths.pursuit
-    paths.held[0]['pose'][1] = 0.5  # as if dead reckoning had carried the car over onto that path
-    rates, speeds, xy, yaws = self.straight_path(4.0, side=0.0)  # and the newest path agrees: straight on from here
-    paths.update(rates, speeds, 0.0, 4.0, 0.4, 0.05, xy, yaws, 0.0)
-    self.assertGreater(first, 0.01)
-    self.assertLess(abs(paths.pursuit - 0.0), 0.5 * first)
-
-  def test_geometric_curvature_reads_a_path_by_its_shape(self):
-    # a path whose yaw rates are noisy at a creeping planned speed, but whose shape is a clean arc
-    k, v = 0.05, 1.0
-    t = np.asarray(T)
-    s = v * t
-    xy, yaws = np.stack([np.sin(k * s) / k, (1 - np.cos(k * s)) / k], 1), k * s
-    rng = np.random.default_rng(2)
-    by_rate, by_shape = recent_paths.RecentPaths(T), recent_paths.RecentPaths(T)
-    for _ in range(40):
-      rates = k * v + rng.normal(0, 0.05, len(t))
-      own_rate, _ = by_rate.update(rates, np.full(len(t), v), k, v, 0.4, 0.05)
-      own_shape, _ = by_shape.update(rates, np.full(len(t), v), k, v, 0.4, 0.05, xy, yaws, k, geometric=True)
-    self.assertAlmostEqual(own_shape, k, delta=0.05 * k)
-    self.assertLess(abs(own_shape - k), abs(own_rate - k) + 1e-9)
-
-  def test_geometric_curvature_falls_back_on_a_path_too_short_to_read(self):
-    t = np.asarray(T)
-    xy = np.stack([0.1 * t, np.zeros(len(t))], 1)  # 1 m in 10 s: about to stop
-    paths, plain = recent_paths.RecentPaths(T), recent_paths.RecentPaths(T)
-    rates, speeds = path(0.02, 0.1)
-    a, _ = paths.update(rates, speeds, 0.0, 0.5, 0.4, 0.05, xy, np.zeros(len(t)), 0.0, geometric=True)
-    b, _ = plain.update(rates, speeds, 0.0, 0.5, 0.4, 0.05)
-    self.assertAlmostEqual(a, b, places=12)
-
-
-if __name__ == "__main__":
-  unittest.main()

openpilot/sunnypilot/modeld_v2/turn_frame.py

@@ -1,22 +0,0 @@
-"""
-Copyright (c) 2021-, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import numpy as np
-
-from openpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame
-from openpilot.common.transformations.orientation import rot_from_euler
-
-TURN_INPUT_SIZE = (512, 256)
-turn_intrinsics = np.array([
-  [180.0, 0.0, 256.0],
-  [0.0, 440.0, 70.0],
-  [0.0, 0.0, 1.0]])
-calib_from_turn = np.linalg.inv((turn_intrinsics @ get_view_frame_from_calib_frame(0, 0, 0, 0))[:, :3])
-
-
-def get_turn_warp_matrix(device_from_calib_euler: np.ndarray, wide_intrinsics: np.ndarray) -> np.ndarray:
-  camera_from_calib = wide_intrinsics @ view_frame_from_device_frame @ rot_from_euler(device_from_calib_euler)
-  return (camera_from_calib @ calib_from_turn).astype(np.float32)