diff --git a/common/time_helpers.py b/common/time_helpers.py index 8564e270c..c709182d4 100644 --- a/common/time_helpers.py +++ b/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 diff --git a/scripts/model_compiler.py b/scripts/model_compiler.py index 229f555d9..c91f4859a 100644 --- a/scripts/model_compiler.py +++ b/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 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 2f04e899c..2ff55192d 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/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: diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 2bb83b635..d767762fe 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/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), diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index e19eee148..0a417ad26 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/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 diff --git a/selfdrive/ui/lib/starpilot_state.py b/selfdrive/ui/lib/starpilot_state.py index c03aa1257..7c1d4837f 100644 --- a/selfdrive/ui/lib/starpilot_state.py +++ b/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)) diff --git a/starpilot/assets/tests/test_model_pipeline.py b/starpilot/assets/tests/test_model_pipeline.py index 95bdfc903..429fff041 100644 --- a/starpilot/assets/tests/test_model_pipeline.py +++ b/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) diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index ea094e1de..9d51d6947 100644 --- a/starpilot/common/starpilot_variables.py +++ b/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): diff --git a/system/hardware/chestnut/flash.py b/system/hardware/chestnut/flash.py index a603a9948..a5aa44e3c 100755 --- a/system/hardware/chestnut/flash.py +++ b/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) - diff --git a/system/hardware/tests/test_chestnut_flash.py b/system/hardware/tests/test_chestnut_flash.py new file mode 100644 index 000000000..997029f63 --- /dev/null +++ b/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)] diff --git a/system/timed.py b/system/timed.py index ba256c9f7..64fd59252 100644 --- a/system/timed.py +++ b/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)