Add traffic border color to C4

launch_env.sh

@@ -21,7 +21,7 @@ fi
 export QCOM_PRIORITY=12
 
 if [ -z "$AGNOS_VERSION" ]; then
-  export AGNOS_VERSION="19.6.2"
+  export AGNOS_VERSION="19.6.1"
 fi
 
 if [ -z "$AGNOS_ACCEPTED_VERSIONS" ]; then

selfdrive/controls/lib/desire_helper.py

@@ -224,12 +224,12 @@ class DesireHelper:
         if desired_lane_width >= starpilot_toggles.lane_detection_width and self._nav_torque_applied(carstate, lane_change_direction):
           return log.Desire.keepRight
     elif modifier in ("left", "sharpLeft"):
-      turn_allowed = carstate.leftBlinker and not carstate.rightBlinker and not carstate.leftBlindspot
+      turn_allowed = not carstate.rightBlinker and not carstate.leftBlindspot
       turn_allowed &= carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill
       if turn_allowed and self._nav_turn_is_imminent(carstate, maneuver_distance):
         return log.Desire.turnLeft
     elif modifier in ("right", "sharpRight"):
-      turn_allowed = carstate.rightBlinker and not carstate.leftBlinker and not carstate.rightBlindspot
+      turn_allowed = not carstate.leftBlinker and not carstate.rightBlindspot
       turn_allowed &= carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill
       if turn_allowed and self._nav_turn_is_imminent(carstate, maneuver_distance):
         return log.Desire.turnRight

selfdrive/controls/lib/latcontrol_torque.py

@@ -111,7 +111,6 @@ class LatControlTorque(LatControl):
     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_toyota_highlander_tss2 = CP.carFingerprint in TOYOTA_HIGHLANDER_TSS2_CARS
     self.is_toyota_corolla_tss2 = CP.carFingerprint in TOYOTA_COROLLA_TSS2_CARS
     self.is_lexus_is = CP.carFingerprint in LEXUS_IS_CARS
     self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS
@@ -311,7 +310,6 @@ class LatControlTorque(LatControl):
       rav4_tss2_active = self.is_rav4_tss2
       rav4_prime_active = self.is_rav4_prime
       sienna_4th_gen_active = self.is_sienna_4th_gen
-      toyota_highlander_tss2_active = self.is_toyota_highlander_tss2
       toyota_corolla_tss2_active = self.is_toyota_corolla_tss2
       lexus_is_active = self.is_lexus_is
       ioniq_5_active = self.is_ioniq_5
@@ -400,10 +398,6 @@ class LatControlTorque(LatControl):
       elif sienna_4th_gen_active:
         ff *= get_sienna_4th_gen_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
         friction_threshold = get_sienna_4th_gen_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
-      elif toyota_highlander_tss2_active:
-        ff *= get_toyota_highlander_tss2_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
-        friction_threshold = get_toyota_highlander_tss2_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
-        friction_scale = get_toyota_highlander_tss2_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
       elif toyota_corolla_tss2_active:
         ff *= get_toyota_corolla_tss2_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
       elif lexus_is_active:

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -184,10 +184,6 @@ SIENNA_4TH_GEN_CARS = (
   TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN,
 )
 
-TOYOTA_HIGHLANDER_TSS2_CARS = (
-  TOYOTA_CAR.TOYOTA_HIGHLANDER_TSS2,
-)
-
 TOYOTA_COROLLA_TSS2_CARS = (
   TOYOTA_CAR.TOYOTA_COROLLA_TSS2,
 )
@@ -239,7 +235,7 @@ GENESIS_G70_FRICTION_CENTER_LAT = 0.28
 GENESIS_G70_FRICTION_CENTER_LAT_WIDTH = 0.10
 GENESIS_G70_FRICTION_CALM_JERK = 0.35
 GENESIS_G70_FRICTION_CALM_JERK_WIDTH = 0.10
-GENESIS_G70_FRICTION_JERK_DEADZONE_MAX = 0.30
+GENESIS_G70_FRICTION_JERK_DEADZONE_MAX = 0.24
 GENESIS_G70_FRICTION_JERK_DEADZONE_LAT = 0.30
 GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08
 GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED = 12.0
@@ -267,14 +263,14 @@ GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT = 0.14
 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT_WIDTH = 0.05
 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED = 6.0
 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 1.5
-GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST = 0.00
+GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST = 0.02
 GENESIS_G70_CURVE_UNWIND_SPEED = 18.0
 GENESIS_G70_CURVE_UNWIND_SPEED_WIDTH = 3.0
 GENESIS_G70_CURVE_UNWIND_LAT = 0.25
 GENESIS_G70_CURVE_UNWIND_LAT_WIDTH = 0.12
 GENESIS_G70_CURVE_UNWIND_JERK = 0.08
 GENESIS_G70_CURVE_UNWIND_JERK_WIDTH = 0.08
-GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.28
+GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.20
 GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.12
 GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.12
 GENESIS_G70_UNWIND_FF_JERK = 0.10
@@ -373,7 +369,7 @@ BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED = 2.5
 BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_WIDTH = 0.7
 BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX = 7.2
 BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX_WIDTH = 0.5
-BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP = 0.080
+BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP = 0.050
 BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT = 0.18
 BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.06
 BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED = 6.7
@@ -1112,17 +1108,6 @@ TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.08
 TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED = 4.5
 TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 1.5
 
-TOYOTA_HIGHLANDER_TSS2_PHASE_SCALE = 0.12
-TOYOTA_HIGHLANDER_TSS2_UNWIND_FF_REDUCTION = 0.10
-TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_THRESHOLD_GAIN = 0.16
-TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_SCALE_REDUCTION = 0.10
-TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_ONSET = 0.20
-TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_WIDTH = 0.08
-TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_ONSET = 3.0
-TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_WIDTH = 1.5
-TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX = 15.0
-TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX_WIDTH = 2.0
-
 LEXUS_IS_PHASE_SCALE = 0.10
 LEXUS_IS_TURN_IN_FF_BOOST_LEFT = 0.06
 LEXUS_IS_TURN_IN_FF_BOOST_RIGHT = 0.06
@@ -1660,52 +1645,6 @@ def get_toyota_corolla_tss2_center_output_scale(desired_lateral_accel: float, v_
   return max(1.0 - reduction, 0.65)
 
 
-def _toyota_highlander_tss2_unwind_weight(desired_lateral_accel: float,
-                                           desired_lateral_jerk: float,
-                                           v_ego: float) -> float:
-  phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) /
-                    TOYOTA_HIGHLANDER_TSS2_PHASE_SCALE)
-  curve_weight = _sigmoid((abs(desired_lateral_accel) - TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_ONSET) /
-                          TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_WIDTH)
-  speed_weight = (_sigmoid((v_ego - TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_ONSET) /
-                           TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_WIDTH) *
-                  _sigmoid((TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX - v_ego) /
-                           TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX_WIDTH))
-  return max(-phase, 0.0) * curve_weight * speed_weight
-
-
-def get_toyota_highlander_tss2_ff_scale(desired_lateral_accel: float,
-                                         desired_lateral_jerk: float,
-                                         v_ego: float) -> float:
-  reduction = _flm_vehicle_knob("toyota_highlander_tss2.unwind_ff_reduction",
-                                TOYOTA_HIGHLANDER_TSS2_UNWIND_FF_REDUCTION)
-  return 1.0 - reduction * _toyota_highlander_tss2_unwind_weight(
-    desired_lateral_accel, desired_lateral_jerk, v_ego,
-  )
-
-
-def get_toyota_highlander_tss2_friction_threshold(v_ego: float,
-                                                  desired_lateral_accel: float = 0.0,
-                                                  desired_lateral_jerk: float = 0.0) -> float:
-  gain = _flm_vehicle_knob("toyota_highlander_tss2.unwind_friction_threshold_gain",
-                           TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_THRESHOLD_GAIN)
-  return get_standard_friction_threshold(v_ego) * (
-    1.0 + gain * _toyota_highlander_tss2_unwind_weight(
-      desired_lateral_accel, desired_lateral_jerk, v_ego,
-    )
-  )
-
-
-def get_toyota_highlander_tss2_friction_scale(v_ego: float,
-                                              desired_lateral_accel: float,
-                                              desired_lateral_jerk: float) -> float:
-  reduction = _flm_vehicle_knob("toyota_highlander_tss2.unwind_friction_scale_reduction",
-                                TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_SCALE_REDUCTION)
-  return 1.0 - reduction * _toyota_highlander_tss2_unwind_weight(
-    desired_lateral_accel, desired_lateral_jerk, v_ego,
-  )
-
-
 def get_lexus_is_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
   if desired_lateral_accel == 0.0:
     return 1.0

selfdrive/controls/tests/test_latcontrol.py

@@ -109,9 +109,6 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
   get_sienna_4th_gen_ff_scale,
   get_sienna_4th_gen_friction_threshold,
   get_sienna_4th_gen_high_speed_output_taper_scale,
-  get_toyota_highlander_tss2_ff_scale,
-  get_toyota_highlander_tss2_friction_scale,
-  get_toyota_highlander_tss2_friction_threshold,
   get_toyota_corolla_tss2_center_output_scale,
   get_toyota_corolla_tss2_ff_scale,
   get_lexus_is_ff_scale,
@@ -884,10 +881,9 @@ class TestLatControl:
     assert get_genesis_g70_low_speed_output_limit(0.0, 2.0) < 0.30
     assert get_genesis_g70_low_speed_angle_damping(0.0, -20.0, 0.0, 2.0) < 0.0
     assert get_genesis_g70_low_speed_angle_damping(0.0, 20.0, 0.0, 2.0) > 0.0
-    assert get_genesis_g70_curve_unwind_output_scale(0.7, -0.5, 25.0) == pytest.approx(1.0)
+    assert get_genesis_g70_curve_unwind_output_scale(0.7, -0.5, 25.0) > 1.0
     assert get_genesis_g70_curve_unwind_output_scale(0.7, 0.5, 25.0) == 1.0
-    assert get_genesis_g70_friction_jerk_deadzone(25.0, 0.0) > 0.25
-    assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, 0.5, 25.0) < 0.90
+    assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, 0.5, 25.0) < 1.0
     assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, -0.5, 25.0) == 1.0
     assert get_genesis_g70_unwind_ff_scale(-0.7, 0.2, 0.5, 25.0) == 1.0
 
@@ -1013,26 +1009,6 @@ class TestLatControl:
     assert get_sienna_4th_gen_high_speed_output_taper_scale(10.0) == pytest.approx(1.0, abs=0.002)
     assert get_sienna_4th_gen_high_speed_output_taper_scale(22.0) < 1.0
 
-  def test_toyota_highlander_tss2_unwind_shaping_is_low_speed_only(self):
-    base = get_standard_friction_threshold(9.0)
-    steady = get_toyota_highlander_tss2_ff_scale(0.8, 0.0, 9.0)
-    unwind = get_toyota_highlander_tss2_ff_scale(0.8, -0.8, 9.0)
-    highway_unwind = get_toyota_highlander_tss2_ff_scale(0.8, -0.8, 28.0)
-
-    assert steady == pytest.approx(1.0)
-    assert 0.90 < unwind < 1.0
-    assert highway_unwind > unwind
-
-    unwind_threshold = get_toyota_highlander_tss2_friction_threshold(9.0, 0.8, -0.8)
-    turn_threshold = get_toyota_highlander_tss2_friction_threshold(9.0, 0.8, 0.8)
-    assert unwind_threshold > base
-    assert turn_threshold == pytest.approx(base, rel=0.01)
-
-    unwind_scale = get_toyota_highlander_tss2_friction_scale(9.0, 0.8, -0.8)
-    turn_scale = get_toyota_highlander_tss2_friction_scale(9.0, 0.8, 0.8)
-    assert 0.90 < unwind_scale < 1.0
-    assert turn_scale == pytest.approx(1.0)
-
   def test_rav4_prime_forced_torque_update_path(self, monkeypatch):
     controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(TOYOTA.TOYOTA_RAV4_PRIME, force_torque=True)
     CS.vEgo = 13.0

selfdrive/modeld/dmonitoringmodeld.py

@@ -16,8 +16,7 @@ from cereal import messaging
 from cereal.messaging import PubMaster, SubMaster
 from msgq.visionipc import VisionBuf, VisionIpcClient, VisionStreamType
 from openpilot.common.file_chunker import read_file_chunked
-from openpilot.common.params import Params
-from openpilot.common.realtime import config_realtime_process, set_core_affinity
+from openpilot.common.realtime import config_realtime_process
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
 from openpilot.common.transformations.model import dmonitoringmodel_intrinsics
@@ -30,19 +29,6 @@ SEND_RAW_PRED = os.getenv("SEND_RAW_PRED")
 MODELS_DIR = Path(__file__).parent / "models"
 MODEL_PKL_PATH = MODELS_DIR / "dmonitoring_model_tinygrad.pkl"
 METADATA_PATH = MODELS_DIR / "dmonitoring_model_metadata.pkl"
-AFFINITY_CHECK_INTERVAL_SECONDS = 0.5
-DEFAULT_AFFINITY_CORES = [7]
-EXTERNAL_GPU_AFFINITY_CORES = [6, 7]
-
-
-def update_external_gpu_affinity(params: Params, external_gpu_affinity: bool) -> bool:
-  """Let driver monitoring use the otherwise-idle camera core only while the big model is active."""
-  external_gpu_active = params.get_bool("UsbGpuActive")
-  if external_gpu_active != external_gpu_affinity:
-    cores = EXTERNAL_GPU_AFFINITY_CORES if external_gpu_active else DEFAULT_AFFINITY_CORES
-    set_core_affinity(cores)
-    cloudlog.warning(f"dmonitoringmodeld affinity set to {cores}; external GPU active: {external_gpu_active}")
-  return external_gpu_active
 
 
 class ModelState:
@@ -146,9 +132,6 @@ def get_driverstate_packet(model_output, frame_id: int, exec_time: float, gpu_ex
 
 def main():
   config_realtime_process(7, 5)
-  params = Params()
-  external_gpu_affinity = False
-  next_affinity_check = 0.0
   cloudlog.warning("connecting to driver stream")
   vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True)
   while not vipc_client.connect(False):
@@ -173,12 +156,6 @@ def main():
     buf = vipc_client.recv()
     if buf is None:
       continue
-
-    now = time.monotonic()
-    if now >= next_affinity_check:
-      external_gpu_affinity = update_external_gpu_affinity(params, external_gpu_affinity)
-      next_affinity_check = now + AFFINITY_CHECK_INTERVAL_SECONDS
-
     if model_transform is None:
       camera = _os_fisheye if buf.width == _os_fisheye.width else _ar_ox_fisheye
       model_transform = np.linalg.inv(

selfdrive/modeld/tests/test_dmonitoring_affinity.py

@@ -1,38 +0,0 @@
-from openpilot.selfdrive.modeld import dmonitoringmodeld
-
-
-class FakeParams:
-  def __init__(self, active: bool):
-    self.active = active
-
-  def get_bool(self, key: str) -> bool:
-    assert key == "UsbGpuActive"
-    return self.active
-
-
-def test_non_gpu_affinity_is_unchanged(monkeypatch):
-  calls = []
-  monkeypatch.setattr(dmonitoringmodeld, "set_core_affinity", calls.append)
-
-  assert not dmonitoringmodeld.update_external_gpu_affinity(FakeParams(False), False)
-  assert calls == []
-
-
-def test_external_gpu_adds_idle_camera_core(monkeypatch):
-  calls = []
-  monkeypatch.setattr(dmonitoringmodeld, "set_core_affinity", calls.append)
-
-  assert dmonitoringmodeld.update_external_gpu_affinity(FakeParams(True), False)
-  assert calls == [dmonitoringmodeld.EXTERNAL_GPU_AFFINITY_CORES]
-
-  calls.clear()
-  assert dmonitoringmodeld.update_external_gpu_affinity(FakeParams(True), True)
-  assert calls == []
-
-
-def test_affinity_returns_to_upstream_default_after_gpu_fallback(monkeypatch):
-  calls = []
-  monkeypatch.setattr(dmonitoringmodeld, "set_core_affinity", calls.append)
-
-  assert not dmonitoringmodeld.update_external_gpu_affinity(FakeParams(False), True)
-  assert calls == [dmonitoringmodeld.DEFAULT_AFFINITY_CORES]

selfdrive/pandad/spi.cc

@@ -350,13 +350,13 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
     SPILOG(LOGE, "SPI: failed to send header");
     goto fail;
   }
+  wait_for_spi_turnaround(nanos_since_boot());
 
   // Wait for (N)ACK
   ret = wait_for_ack(SPI_HACK, 0x11, timeout, 1);
   if (ret < 0) {
     goto fail;
   }
-  wait_for_spi_turnaround(nanos_since_boot());
 
   // Send data
   if (tx_data != NULL) {

system/hardware/tici/agnos.json

@@ -67,13 +67,13 @@
   },
   {
     "name": "system",
-    "url": "https://www.dropbox.com/scl/fi/pewhzpqzi3aewuiaffc6m/system10.img.xz?rlkey=olzrzulhs93zzghnjrskmdwxt&st=exnfk2oz&dl=1",
-    "hash": "ab395d4c963a908ab86709f1a6580a62dd24cb34ee71cf5fd5ec29d7d48d0e10",
-    "hash_raw": "ab395d4c963a908ab86709f1a6580a62dd24cb34ee71cf5fd5ec29d7d48d0e10",
+    "url": "https://www.dropbox.com/scl/fi/3gtw78kxn6xs6dil10fmc/system9.img.xz?rlkey=uwj6tss7lw3j1di09xvtdugcy&st=ghnm9teb&dl=1",
+    "hash": "b6f1e5ade7baa0078990e881b9f392b6a29164347df8f54be20c81e9c9dbabfe",
+    "hash_raw": "b6f1e5ade7baa0078990e881b9f392b6a29164347df8f54be20c81e9c9dbabfe",
     "size": 4718592000,
     "sparse": false,
     "full_check": false,
     "has_ab": true,
-    "ondevice_hash": "ab395d4c963a908ab86709f1a6580a62dd24cb34ee71cf5fd5ec29d7d48d0e10"
+    "ondevice_hash": "b6f1e5ade7baa0078990e881b9f392b6a29164347df8f54be20c81e9c9dbabfe"
   }
 ]