Fun Dip

common/time_helpers.py

@@ -2,6 +2,7 @@ import datetime
 from pathlib import Path
 
 MIN_DATE = datetime.datetime(year=2025, month=2, day=21)
+MAX_DATE = datetime.datetime(year=2035, month=1, day=1)
 
 def min_date():
   # on systemd systems, the default time is the systemd build time
@@ -12,4 +13,4 @@ def min_date():
   return MIN_DATE
 
 def system_time_valid():
-  return datetime.datetime.now() > min_date()
+  return min_date() < datetime.datetime.now() < MAX_DATE

scripts/model_compiler.py

@@ -4,6 +4,7 @@ import codecs
 import hashlib
 import json
 import os
+import platform
 import pickle
 import shutil
 import subprocess
@@ -45,6 +46,8 @@ MODEL_CONTEXT_FREQ = 5
 REPOSITORY_FILE_LIMIT = 100_000_000
 DEFAULT_MULTIPART_SIZE = 95 * 1024 * 1024
 USBGPU_PROBE_ATTEMPTS = 10
+
+
 def build_compile_env(*, supercombo: bool = False) -> dict[str, str]:
   env = os.environ.copy()
   existing_pythonpath = env.get("PYTHONPATH", "")
@@ -81,6 +84,13 @@ def wait_for_external_gpu() -> None:
   raise RuntimeError("Chestnut not ready; external GPU PCIe link did not come up")
 
 
+def external_gpu_compile_command(command: list[str]) -> list[str]:
+  """Pin USB-GPU compilation to AGNOS' isolated CPU without changing host builds."""
+  if sys.platform == "linux" and platform.machine() == "aarch64":
+    return ["taskset", "-c", "7", *command]
+  return command
+
+
 def parse_args() -> argparse.Namespace:
   parser = argparse.ArgumentParser(
     description="Compile staged ONNX models into StarPilot's unified tinygrad artifact format.",
@@ -534,6 +544,7 @@ def compile_driving(
     })
     command.append("--out-of-band")
     wait_for_external_gpu()
+    command = external_gpu_compile_command(command)
   subprocess.run(command, cwd=REPO_ROOT, env=compile_env, check=True)
   return output_path
 

selfdrive/controls/lib/latcontrol_torque.py

@@ -108,6 +108,7 @@ class LatControlTorque(LatControl):
     self.is_palisade = CP.carFingerprint in PALISADE_CARS
     self.is_prius = CP.carFingerprint in PRIUS_CARS
     self.is_camry = CP.carFingerprint in CAMRY_CARS
+    self.is_rav4_tss2 = CP.carFingerprint in RAV4_TSS2_CARS
     self.is_rav4_prime = CP.carFingerprint in RAV4_PRIME_CARS
     self.is_sienna_4th_gen = CP.carFingerprint in SIENNA_4TH_GEN_CARS
     self.is_lexus_is = CP.carFingerprint in LEXUS_IS_CARS
@@ -301,6 +302,7 @@ class LatControlTorque(LatControl):
       genesis_g70_active = self.is_genesis_g70
       prius_active = self.is_prius
       camry_active = self.is_camry
+      rav4_tss2_active = self.is_rav4_tss2
       rav4_prime_active = self.is_rav4_prime
       sienna_4th_gen_active = self.is_sienna_4th_gen
       lexus_is_active = self.is_lexus_is
@@ -381,6 +383,8 @@ class LatControlTorque(LatControl):
       elif camry_active:
         ff *= get_camry_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
         friction_threshold = get_camry_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
+      elif rav4_tss2_active:
+        friction_threshold = get_rav4_tss2_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
       elif rav4_prime_active:
         ff *= get_rav4_prime_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
         friction_threshold = get_rav4_prime_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
@@ -555,6 +559,8 @@ class LatControlTorque(LatControl):
         rapid_reversal = setpoint * desired_lateral_jerk < 0.0
         if output_torque * setpoint > 0.0 or rapid_reversal:
           output_torque *= get_kona_non_scc_highway_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
+      elif rav4_tss2_active:
+        output_torque *= get_rav4_tss2_center_output_scale(setpoint, CS.vEgo)
       elif rav4_prime_active:
         output_torque *= get_rav4_prime_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo)
       elif sienna_4th_gen_active:

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -1088,6 +1088,14 @@ RAV4_TSS2_PID_CENTER_ANGLE = 14.0
 RAV4_TSS2_PID_CENTER_ANGLE_WIDTH = 3.0
 RAV4_TSS2_PID_OUTPUT_SCALE_MIN = 0.62
 RAV4_TSS2_PID_OUTPUT_ALPHA_MIN = 0.28
+RAV4_TSS2_CENTER_FRICTION_THRESHOLD_GAIN = 0.14
+RAV4_TSS2_CENTER_FRICTION_LAT = 0.30
+RAV4_TSS2_CENTER_FRICTION_LAT_WIDTH = 0.08
+RAV4_TSS2_CENTER_SPEED = 13.0
+RAV4_TSS2_CENTER_SPEED_WIDTH = 3.0
+RAV4_TSS2_CENTER_OUTPUT_TAPER_MAX = 0.12
+RAV4_TSS2_CENTER_OUTPUT_LAT = 0.30
+RAV4_TSS2_CENTER_OUTPUT_LAT_WIDTH = 0.08
 
 RAM_1500_TRANSITION_TAPER_MAX = 0.34
 RAM_1500_TRANSITION_SPEED_ONSET = 10.0
@@ -1588,6 +1596,30 @@ def get_rav4_tss2_pid_output(output_torque: float, prev_output_torque: float,
   return float(prev_output_torque + output_alpha * (limited_output - prev_output_torque))
 
 
+def _rav4_tss2_center_envelope(desired_lateral_accel: float, v_ego: float) -> float:
+  speed_weight = _sigmoid((RAV4_TSS2_CENTER_SPEED - max(v_ego, 0.0)) /
+                          RAV4_TSS2_CENTER_SPEED_WIDTH)
+  center_weight = _sigmoid((RAV4_TSS2_CENTER_FRICTION_LAT - abs(desired_lateral_accel)) /
+                           RAV4_TSS2_CENTER_FRICTION_LAT_WIDTH)
+  return speed_weight * center_weight
+
+
+def get_rav4_tss2_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
+                                     desired_lateral_jerk: float = 0.0) -> float:
+  del desired_lateral_jerk
+  gain = _flm_vehicle_knob("toyota_rav4_tss2.center_friction_threshold_gain",
+                           RAV4_TSS2_CENTER_FRICTION_THRESHOLD_GAIN)
+  return get_standard_friction_threshold(v_ego) * (
+    1.0 + gain * _rav4_tss2_center_envelope(desired_lateral_accel, v_ego)
+  )
+
+
+def get_rav4_tss2_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
+  reduction = _flm_vehicle_knob("toyota_rav4_tss2.center_output_taper_max",
+                                RAV4_TSS2_CENTER_OUTPUT_TAPER_MAX)
+  return 1.0 - reduction * _rav4_tss2_center_envelope(desired_lateral_accel, v_ego)
+
+
 def get_ram_1500_transition_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
   speed_weight = float(np.interp(v_ego, [RAM_1500_TRANSITION_SPEED_ONSET, RAM_1500_TRANSITION_SPEED_FULL], [0.0, 1.0]))
   jerk_weight = float(np.interp(abs(desired_lateral_jerk),

selfdrive/controls/tests/test_latcontrol.py

@@ -99,6 +99,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
   get_rav4_prime_friction_scale,
   get_rav4_prime_friction_threshold,
   get_rav4_prime_output_taper_scale,
+  get_rav4_tss2_center_output_scale,
+  get_rav4_tss2_friction_threshold,
   get_sienna_4th_gen_center_taper_scale,
   get_sienna_4th_gen_ff_scale,
   get_sienna_4th_gen_friction_threshold,
@@ -1655,6 +1657,29 @@ class TestLatControl:
     assert abs(large_turn) > abs(low_speed)
     assert highway > low_speed
 
+  def test_rav4_tss2_torque_center_tune_fades_before_real_turns(self):
+    low_speed_center = get_rav4_tss2_center_output_scale(0.05, 8.0)
+    low_speed_turn = get_rav4_tss2_center_output_scale(1.0, 8.0)
+    highway_center = get_rav4_tss2_center_output_scale(0.05, 22.0)
+
+    assert low_speed_center < low_speed_turn
+    assert low_speed_center < highway_center
+    assert get_rav4_tss2_friction_threshold(8.0, 0.05) > get_standard_friction_threshold(8.0)
+    assert get_rav4_tss2_friction_threshold(8.0, 1.0) == pytest.approx(get_standard_friction_threshold(8.0), rel=0.02)
+
+  def test_rav4_tss2_torque_update_path_applies_center_taper(self, monkeypatch):
+    tuned_controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(TOYOTA.TOYOTA_RAV4_TSS2, force_torque=True)
+    CS.vEgo = 8.0
+    tuned_output, _, lac_log = tuned_controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, starpilot_toggles)
+
+    monkeypatch.setattr(latcontrol_torque, "get_rav4_tss2_center_output_scale", lambda *_args: 1.0)
+    base_controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(TOYOTA.TOYOTA_RAV4_TSS2, force_torque=True)
+    CS.vEgo = 8.0
+    base_output, _, _ = base_controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, starpilot_toggles)
+
+    assert lac_log.active
+    assert abs(tuned_output) < abs(base_output)
+
   def test_rav4_tss2_pid_output_update_path(self, monkeypatch):
     controller, VM, CS, params, starpilot_toggles = self._build_pid_controller(TOYOTA.TOYOTA_RAV4_TSS2)
     CS.vEgo = 6.0 * 0.44704

selfdrive/ui/lib/starpilot_state.py

@@ -8,6 +8,7 @@ from openpilot.selfdrive.ui.ui_state import ui_state
 from cereal import car, log, custom, messaging
 from opendbc.car.gm.values import GMFlags
 from opendbc.car.hyundai.values import HyundaiFlags
+from opendbc.car.toyota.values import TSS2_CAR
 from openpilot.starpilot.common.lateral_delay import full_lateral_delay
 
 @dataclass
@@ -175,7 +176,8 @@ class StarPilotState:
             self.car_state.hasModeStarButtons = car_make == "hyundai" and bool(cp_flags & HyundaiFlags.CANFD)
             self.car_state.lkasAllowedForAOL = (
                 (car_make == "hyundai" and (bool(cp_flags & HyundaiFlags.CANFD) or starpilot_toggles.get("lkas_allowed_for_aol", False))) or
-                car_make == "honda"
+                car_make == "honda" or
+                (car_make == "toyota" and car_fingerprint in TSS2_CAR)
             )
             self.car_state.longitudinalActuatorDelay = float(self._safe_get(CP, "longitudinalActuatorDelay", self.car_state.longitudinalActuatorDelay))
             self.car_state.startAccel = float(self._safe_get(CP, "startAccel", self.car_state.startAccel))

starpilot/assets/tests/test_model_pipeline.py

@@ -70,7 +70,7 @@ def test_external_gpu_compilation_is_opt_in(tmp_path, monkeypatch):
     "DEV": "QCOM", "IMAGE": "2", "NOLOCALS": "1", "OPENPILOT_HACKS": "1",
   })
   monkeypatch.setattr(model_compiler.subprocess, "run", lambda command, **kwargs: invocations.append((command, kwargs)))
-  monkeypatch.setattr(model_compiler, "wait_for_external_gpu", lambda _: None)
+  monkeypatch.setattr(model_compiler, "wait_for_external_gpu", lambda: None)
   files = {"driving_supercombo": tmp_path / "model.onnx"}
 
   model_compiler.compile_driving("normal", files, "supercombo", "v15", tmp_path, "policy")
@@ -87,6 +87,22 @@ def test_external_gpu_compilation_is_opt_in(tmp_path, monkeypatch):
   assert all(flag not in external_kwargs["env"] for flag in ("IMAGE", "NOLOCALS", "OPENPILOT_HACKS"))
 
 
+def test_external_gpu_compile_uses_agnos_isolated_cpu(monkeypatch):
+  command = ["python3", "compile_modeld.py"]
+  monkeypatch.setattr(model_compiler.sys, "platform", "linux")
+  monkeypatch.setattr(model_compiler.platform, "machine", lambda: "aarch64")
+
+  assert model_compiler.external_gpu_compile_command(command) == ["taskset", "-c", "7", *command]
+
+
+def test_external_gpu_compile_does_not_pin_other_platforms(monkeypatch):
+  command = ["python3", "compile_modeld.py"]
+  monkeypatch.setattr(model_compiler.sys, "platform", "darwin")
+  monkeypatch.setattr(model_compiler.platform, "machine", lambda: "arm64")
+
+  assert model_compiler.external_gpu_compile_command(command) is command
+
+
 def test_compile_clears_only_selected_model_outputs(tmp_path, monkeypatch):
   monkeypatch.setattr(model_compiler, "build_compile_env", lambda **_: {})
   monkeypatch.setattr(model_compiler.subprocess, "run", lambda *args, **kwargs: None)

starpilot/common/starpilot_variables.py

@@ -22,7 +22,7 @@ from opendbc.car.interfaces import TORQUE_SUBSTITUTE_PATH, CarInterfaceBase, Gea
 from opendbc.car.mock.values import CAR as MOCK
 from opendbc.car.subaru.values import SubaruFlags
 from opendbc.car.tesla.values import CAR as TESLA_CAR
-from opendbc.car.toyota.values import CAR as TOYOTA_CAR, ToyotaStarPilotFlags
+from opendbc.car.toyota.values import CAR as TOYOTA_CAR, TSS2_CAR, ToyotaStarPilotFlags
 from openpilot.common.basedir import BASEDIR
 from openpilot.common.constants import CV
 from openpilot.common.params import Params
@@ -615,7 +615,8 @@ class StarPilotVariables:
     hyundai_can_use_lkas_for_aol = toggle.car_make == "hyundai" and (
       bool(CP.flags & HyundaiFlags.CANFD) or hyundai_has_lda_button
     )
-    toggle.lkas_allowed_for_aol = hyundai_can_use_lkas_for_aol or toggle.car_make == "honda"
+    toyota_can_use_lkas_for_aol = toggle.car_make == "toyota" and CP.carFingerprint in TSS2_CAR
+    toggle.lkas_allowed_for_aol = hyundai_can_use_lkas_for_aol or toggle.car_make == "honda" or toyota_can_use_lkas_for_aol
     longitudinalActuatorDelay = CP.longitudinalActuatorDelay
     toggle.openpilot_longitudinal = CP.openpilotLongitudinalControl and not toggle.disable_openpilot_long
     if not toggle.redneck_cruise_available or (toggle.openpilot_longitudinal and FPCP.pcmCruiseSpeed):

system/hardware/chestnut/flash.py

@@ -33,6 +33,7 @@ USBDEVFS_SETCONFIGURATION = 0x80045505
 USBDEVFS_CLAIMINTERFACE = 0x8004550F
 USBDEVFS_RESET = 0x5514
 USBDEVFS_CLEAR_HALT = 0x80045515
+MAX_REGISTER_READ_SIZE = 255
 
 _deadline = float("inf")
 
@@ -146,6 +147,7 @@ def claim_interface(path, setup=False):
 class Flash:
   def __init__(self):
     self.fd = -1
+    self.max_register_read_size = MAX_REGISTER_READ_SIZE
 
   def close(self):
     if self.fd >= 0:
@@ -160,6 +162,9 @@ class Flash:
       if in_rom_bootloader(vid_pid, product):
         raise RomFallback("chestnut fell back to the ROM bootloader")
       if path is not None:
+        speed = int(Path(path, "speed").read_text())
+        # USB2 firmware truncates larger reads to one full packet without a terminating ZLP.
+        self.max_register_read_size = 64 if speed < 5000 else MAX_REGISTER_READ_SIZE
         self.fd = claim_interface(path)
         return
       time.sleep(0.1)
@@ -231,8 +236,8 @@ class Flash:
     while len(out) < length:
       n = min(4096, length - len(out))
       self.transaction(0x03, addr + len(out), max(4096, n))
-      for off in range(0, n, 255):
-        out += self.reg_read(0x7000 + off, min(255, n - off))
+      for off in range(0, n, self.max_register_read_size):
+        out += self.reg_read(0x7000 + off, min(self.max_register_read_size, n - off))
     return bytes(out)
 
   def erase_sector(self, addr):
@@ -574,4 +579,3 @@ if __name__ == "__main__":
   except Exception as e:
     print(f"FAIL: {type(e).__name__}: {e}", file=sys.stderr)
     sys.exit(1)
-

system/hardware/tests/test_chestnut_flash.py

@@ -0,0 +1,39 @@
+from openpilot.system.hardware.chestnut import flash as chestnut_flash
+
+
+def test_usb2_flash_reads_are_limited_to_ep0_packet_size(tmp_path, monkeypatch):
+  (tmp_path / "speed").write_text("480\n")
+  monkeypatch.setattr(chestnut_flash, "find_chestnut", lambda: (str(tmp_path), ("3801", "0001"), "custom test-CLEAN"))
+  monkeypatch.setattr(chestnut_flash, "claim_interface", lambda _: 123)
+  flash = chestnut_flash.Flash()
+
+  flash.connect()
+
+  assert flash.max_register_read_size == 64
+
+
+def test_superspeed_flash_keeps_full_register_reads(tmp_path, monkeypatch):
+  (tmp_path / "speed").write_text("5000\n")
+  monkeypatch.setattr(chestnut_flash, "find_chestnut", lambda: (str(tmp_path), ("3801", "0001"), "custom test-CLEAN"))
+  monkeypatch.setattr(chestnut_flash, "claim_interface", lambda _: 123)
+  flash = chestnut_flash.Flash()
+
+  flash.connect()
+
+  assert flash.max_register_read_size == chestnut_flash.MAX_REGISTER_READ_SIZE
+
+
+def test_flash_read_uses_negotiated_register_chunk_size(monkeypatch):
+  flash = chestnut_flash.Flash()
+  flash.max_register_read_size = 64
+  reads = []
+  monkeypatch.setattr(flash, "transaction", lambda *args, **kwargs: None)
+
+  def reg_read(addr, length=1):
+    reads.append((addr, length))
+    return bytes(length)
+
+  monkeypatch.setattr(flash, "reg_read", reg_read)
+
+  assert flash.read(0, 130) == bytes(130)
+  assert reads == [(0x7000, 64), (0x7040, 64), (0x7080, 2)]

system/timed.py

@@ -6,7 +6,7 @@ import time
 from typing import NoReturn
 
 import cereal.messaging as messaging
-from openpilot.common.time_helpers import min_date, system_time_valid
+from openpilot.common.time_helpers import min_date, MAX_DATE, system_time_valid
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.params import Params
 from openpilot.common.gps import get_gps_location_service
@@ -19,7 +19,7 @@ except Exception:
 
 
 def set_time(new_time):
-  diff = datetime.datetime.now() - new_time
+  diff = datetime.datetime.now(datetime.UTC).replace(tzinfo=None) - new_time
   if abs(diff) < datetime.timedelta(seconds=10):
     cloudlog.debug(f"Time diff too small: {diff}")
     return
@@ -82,12 +82,12 @@ def main() -> NoReturn:
     pm.send('clocks', msg)
 
     gps = sm[gps_location_service]
-    gps_time = datetime.datetime.fromtimestamp(gps.unixTimestampMillis / 1000.)
+    gps_time = datetime.datetime.fromtimestamp(gps.unixTimestampMillis / 1000., datetime.UTC).replace(tzinfo=None)
     if not sm.updated[gps_location_service] or (time.monotonic() - sm.logMonoTime[gps_location_service] / 1e9) > 2.0:
       continue
     if not gps.hasFix:
       continue
-    if gps_time < min_date():
+    if gps_time < min_date() or gps_time > MAX_DATE:
       continue
 
     set_time(gps_time)