path steering: pursuit and shape switches, each behind its own toggle

PathSteeringPursuit: in signalled turns, steer at a point a few metres ahead
on each recent path as seen from where the car is now (dead reckoned from
controlsState.curvature), newer paths weighted more. Pulls a car that has
drifted wide or tight back to the line.

PathSteeringGeometry: read each path's curvature from its heading change per
metre instead of yaw rate over planned speed.

Both default off; with both off the steering is unchanged.

openpilot/common/params_keys.h

@@ -259,6 +259,9 @@ 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:b043d3bbc556e3a872c0edde4081f99b5ba92cbfd98eaea2ee2cbb87e9f277ec
-size 82539558
+oid sha256:de9f536d899fa451d75a28fb5766ecb996189c559aa9c1841a6b8ccc8453cf8d
+size 84567106

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

@@ -99,8 +99,27 @@ 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.blendful_toggle] + ([self.path_steering_toggle] if self.path_steering_toggle is not None else []) + self.path_extra_toggles
     self._scroller.add_widgets(self.main_items)
 
   @property
@@ -213,6 +232,10 @@ 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()
@@ -232,6 +255,14 @@ 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,17 +18,28 @@ 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 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)
+  n = 1 if args.light or args.turn else 2
+  cams = [1] if args.turn else list(range(n))
   rng = np.random.default_rng(42)
-  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()}
+  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)}
   with record_kernel_linear() as recorded:
     run(**inputs)
     run(**inputs)
@@ -36,7 +47,6 @@ 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,10 +19,12 @@ 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 not in ("new_img", "light_img")}
-  return shapes | ({"light_tfm": (1, 3, 3)} if "light_img" in input_shapes else {})
+  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}
 
 
 class _Blobs:  # the model's and the warp's blobs as one address space, for a part whose ranges span both files
@@ -35,28 +37,35 @@ 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 = 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")
+  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")
 
 
-def frame_part(warp_path: str, parts: dict, extras: dict, light_warp_path: str | None = None) -> dict:
+def frame_part(warp_path: str, parts: dict, extras: dict, light_warp_path: str | None = None, turn_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)
-  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"))
+  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)}
 
   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)
-  names = sorted(["host", *states])
+  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])
   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:
@@ -64,22 +73,24 @@ 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(light_warp, len(model["buffers"]) + len(warp["buffers"])) if s["name"] == "light_img" else
+                  warped(extra_at[s["name"]][0], extra_at[s["name"]][1]) if s["name"] in extra_at 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"]: 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"]: {"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"]}
 
   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": 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()],
             "buffers": model["buffers"] + [b | ({"init": [b["init"][0] + d, b["init"][1] + d]} if "init" in b else {})
-                                           for part, d in ((warp, shift), (light_warp, light_shift)) for b in part["buffers"]] +
+                                           for part, d in ((warp, shift), *((p, bb) for p, _, bb, _ in extra_at.values())) 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) +
-                     remap(light_warp["calls"], warp_inputs(light_warp, "light_tfm"), len(model["buffers"]) + len(warp["buffers"]), light_shift) +
+                     [c for p, b0, bb, tfm in extra_at.values() for c in remap(p["calls"], warp_inputs(p, tfm), b0, bb)] +
                      remap(model["calls"], model_inputs, 0, 0)}
-  return {"run": (header, _Blobs(model_blob, warp_blob, light_blob) if light else _Blobs(model_blob, warp_blob))}
+  return {"run": (header, _Blobs(model_blob, warp_blob, *(blob for *_, blob in extra)))}

openpilot/sunnypilot/modeld_v2/helpers.py

@@ -270,15 +270,32 @@ 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
+from openpilot.sunnypilot.modeld_v2.frame_fusion import host_shapes, pack_layout, SPLIT_FRAMES
 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,16 +169,18 @@ 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, self.warp_frame_size)
+    offsets, packed = pack_layout(shapes, 0 if self.split else 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, self.warp_frame_size)
+    self.frames = self.packed_input[offsets['frames']:].reshape(2, -1)
 
     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():
@@ -187,17 +189,29 @@ class NativeTinygradAdapter(BaseModelAdapter):
 
   def copy_frames(self, bufs):
     for i, key in enumerate(self._vision_input_names):
-      if key in bufs:
+      if key not in bufs:
+        continue
+      if not self.split:
         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, output_buffers=self.outputs)
+    self.run_model(host=self.host, **self.input_queues, **self.frame_inputs, output_buffers=self.outputs)
     return self.outputs['outputs']
 
 

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -41,10 +41,12 @@ 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
@@ -141,6 +143,9 @@ 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:
@@ -168,8 +173,10 @@ class ModelState(ModelStateBase):
 
     desire_key = self.desire_key
     inputs[desire_key][0] = 0
-    (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)
+    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.prev_desire[:] = inputs[desire_key]
 
     for key in ('traffic_convention', 'lateral_control_params', 'action_t', 'speed_limit'):
@@ -182,6 +189,8 @@ 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)
@@ -234,7 +243,8 @@ 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) -> log.ModelDataV2.Action:
+                            blendful: Callable[[float], float] | None = None,
+                            path_steering: bool = False, car_curvature: float = 0.0) -> 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,
@@ -248,6 +258,17 @@ 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)
@@ -355,6 +376,7 @@ 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()
@@ -380,6 +402,7 @@ 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
@@ -424,6 +447,15 @@ 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)
@@ -433,6 +465,7 @@ 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)
@@ -455,6 +488,7 @@ 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)
@@ -505,7 +539,8 @@ 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))
+                                           partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled),
+                                           path_steering=path_steering, car_curvature=sm['controlsState'].curvature)
       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

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

openpilot/sunnypilot/modeld_v2/models/big_driving_lightwarp_1928x1208_tinygrad.pkl

@@ -0,0 +1,3 @@
+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:c08cffa27671d6773e316fdf4dcab80ffce2c4cc6fa28775615bc6726a63d7ba
-size 3473263
+oid sha256:580fadf61ded34b4b19f0ba1f5e4b61c0ea489ca34a0952ccf2878593e3f68c4
+size 1851239

openpilot/sunnypilot/modeld_v2/models/driving_lightwarp_1928x1208_tinygrad.pkl

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

openpilot/sunnypilot/modeld_v2/models/driving_turnwarp_1344x760_tinygrad.pkl

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

openpilot/sunnypilot/modeld_v2/models/driving_turnwarp_1928x1208_tinygrad.pkl

@@ -0,0 +1,3 @@
+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:4b22fd6bcb3a3e8a642e6866449dd9c30c12e694ff0741cad9e34e4b00d8c5fa
-size 3698799
+oid sha256:1f5dd8855de07d0e8b9ed836934e1dcbe77aa27c3b5c77eee1f21888f974ba8b
+size 3699072

openpilot/sunnypilot/modeld_v2/models/driving_warp_1928x1208_tinygrad.pkl

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

openpilot/sunnypilot/modeld_v2/recent_paths.py

@@ -0,0 +1,163 @@
+"""
+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

@@ -0,0 +1,159 @@
+"""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

@@ -0,0 +1,22 @@
+"""
+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)