IQ.Pilot Prebuilt Release @ 27f668a

iqpilot/system/camerad/cameras/nv12_info.py

@@ -0,0 +1,21 @@
+# Python version of system/camerad/cameras/nv12_info.h
+# Calculations from third_party/linux/include/msm_media_info.h (VENUS_BUFFER_SIZE)
+
+def align(val: int, alignment: int) -> int:
+  return ((val + alignment - 1) // alignment) * alignment
+
+def get_nv12_info(width: int, height: int) -> tuple[int, int, int, int]:
+  """Returns (stride, y_height, uv_height, buffer_size) for NV12 frame dimensions."""
+  stride = align(width, 128)
+  y_height = align(height, 32)
+  uv_height = align(height // 2, 16)
+
+  # VENUS_BUFFER_SIZE for NV12
+  y_plane = stride * y_height
+  uv_plane = stride * uv_height + 4096
+  size = y_plane + uv_plane + max(16 * 1024, 8 * stride)
+  size = align(size, 4096)
+  size += align(width, 512) * 512  # kernel padding for non-aligned frames
+  size = align(size, 4096)
+
+  return stride, y_height, uv_height, size

iqpilot/system/camerad/snapshot.py

@@ -0,0 +1,132 @@
+#!/usr/bin/env python3
+import subprocess
+import time
+
+import numpy as np
+from PIL import Image
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal.visionipc import VisionStreamType
+from msgq.visionipc import VisionIpcClient
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.system.hardware import PC
+from iqpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
+from iqpilot.system.manager.process_config import managed_processes
+
+
+VISION_STREAMS = {
+  "roadCameraState": VisionStreamType.VISION_STREAM_ROAD,
+  "driverCameraState": VisionStreamType.VISION_STREAM_DRIVER,
+  "wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD,
+}
+
+
+def jpeg_write(fn, dat):
+  img = Image.fromarray(dat)
+  img.save(fn, "JPEG")
+
+
+def yuv_to_rgb(y, u, v):
+  ul = np.repeat(np.repeat(u, 2).reshape(u.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape)
+  vl = np.repeat(np.repeat(v, 2).reshape(v.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape)
+
+  yuv = np.dstack((y, ul, vl)).astype(np.int16)
+  yuv[:, :, 1:] -= 128
+
+  m = np.array([
+    [1.00000,  1.00000, 1.00000],
+    [0.00000, -0.39465, 2.03211],
+    [1.13983, -0.58060, 0.00000],
+  ])
+  rgb = np.dot(yuv, m).clip(0, 255)
+  return rgb.astype(np.uint8)
+
+
+def extract_image(buf):
+  # NV12 format: Y plane followed by interleaved UV plane
+  # UV plane size is stride * uv_height, where uv_height = align(height/2, 16)
+  uv_height = ((buf.height // 2) + 15) // 16 * 16
+  uv_plane_size = buf.stride * uv_height
+
+  y = np.array(buf.data[:buf.uv_offset], dtype=np.uint8).reshape((-1, buf.stride))[:buf.height, :buf.width]
+  uv_data = buf.data[buf.uv_offset:buf.uv_offset + uv_plane_size]
+  u = np.array(uv_data[::2], dtype=np.uint8).reshape((-1, buf.stride//2))[:buf.height//2, :buf.width//2]
+  v = np.array(uv_data[1::2], dtype=np.uint8).reshape((-1, buf.stride//2))[:buf.height//2, :buf.width//2]
+
+  return yuv_to_rgb(y, u, v)
+
+
+def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"):
+  sockets = [s for s in (frame, front_frame) if s is not None]
+  sm = messaging.SubMaster(sockets)
+  vipc_clients = {s: VisionIpcClient("camerad", VISION_STREAMS[s], True) for s in sockets}
+
+  # wait 4 sec from camerad startup for focus and exposure
+  while sm[sockets[0]].frameId < int(4. / DT_MDL):
+    sm.update()
+
+  for client in vipc_clients.values():
+    client.connect(True)
+
+  # grab images
+  rear, front = None, None
+  if frame is not None:
+    c = vipc_clients[frame]
+    rear = extract_image(c.recv())
+  if front_frame is not None:
+    c = vipc_clients[front_frame]
+    front = extract_image(c.recv())
+  return rear, front
+
+
+def snapshot():
+  params = Params()
+
+  if (not params.get_bool("IsOffroad")) or params.get_bool("IsTakingSnapshot"):
+    print("Already taking snapshot")
+    return None, None
+
+  front_camera_allowed = params.get_bool("RecordFront")
+  params.put_bool("IsTakingSnapshot", True)
+  set_offroad_alert("Offroad_IsTakingSnapshot", True)
+  time.sleep(2.0)  # Give hardwared time to read the param, or if just started give camerad time to start
+
+  # Check if camerad is already started
+  try:
+    subprocess.check_call(["pgrep", "camerad"])
+    print("Camerad already running")
+    params.put_bool("IsTakingSnapshot", False)
+    params.remove("Offroad_IsTakingSnapshot")
+    return None, None
+  except subprocess.CalledProcessError:
+    pass
+
+  try:
+    # Allow testing on replay on PC
+    if not PC:
+      managed_processes['camerad'].start()
+
+    frame = "wideRoadCameraState"
+    front_frame = "driverCameraState" if front_camera_allowed else None
+    rear, front = get_snapshots(frame, front_frame)
+  finally:
+    managed_processes['camerad'].stop()
+    params.put_bool("IsTakingSnapshot", False)
+    set_offroad_alert("Offroad_IsTakingSnapshot", False)
+
+  if not front_camera_allowed:
+    front = None
+
+  return rear, front
+
+
+if __name__ == "__main__":
+  pic, fpic = snapshot()
+  if pic is not None:
+    print(pic.shape)
+    jpeg_write("/tmp/back.jpg", pic)
+    if fpic is not None:
+      jpeg_write("/tmp/front.jpg", fpic)
+  else:
+    print("Error taking snapshot")

iqpilot/system/camerad/test/.gitignore

@@ -0,0 +1,2 @@
+jpegs/
+test_ae_gray

iqpilot/system/camerad/test/debug.sh

@@ -0,0 +1,16 @@
+#!/usr/bin/env bash
+set -e
+
+#echo 4294967295 | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl
+
+# no CCI and UTIL, very spammy
+echo 0xfffdbfff | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl
+#echo 0 | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl
+
+sudo dmesg -C
+scons -u -j8 --minimal .
+export DEBUG_FRAMES=1
+export DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1
+#export DISABLE_DRIVER=1
+export LOGPRINT=debug
+./camerad

iqpilot/system/camerad/test/icp_debug.sh

@@ -0,0 +1,13 @@
+#!/usr/bin/env bash
+set -e
+
+cd /sys/kernel/debug/tracing
+echo "" > trace
+echo 1 > tracing_on
+#echo Y > /sys/kernel/debug/camera_icp/a5_debug_q
+echo 0x1 > /sys/kernel/debug/camera_icp/a5_debug_type
+echo 1 > /sys/kernel/debug/tracing/events/camera/enable
+echo 0xffffffff > /sys/kernel/debug/camera_icp/a5_debug_lvl
+echo 1 > /sys/kernel/debug/tracing/events/camera/cam_icp_fw_dbg/enable
+
+cat /sys/kernel/debug/tracing/trace_pipe

iqpilot/system/camerad/test/intercept.sh

@@ -0,0 +1,2 @@
+#!/usr/bin/env bash
+DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1 DEBUG_FRAMES=1 LOGPRINT=debug LD_PRELOAD=/data/tici_test_scripts/isp/interceptor/tmpioctl.so ./camerad

iqpilot/system/camerad/test/stress_restart.sh

@@ -0,0 +1,9 @@
+#!/bin/sh
+cd ..
+while :; do
+  ./camerad &
+  pid="$!"
+  sleep 2
+  kill -2 $pid
+  wait $pid
+done

iqpilot/system/camerad/test/test_camerad.py

@@ -0,0 +1,217 @@
+import os
+import time
+
+import pytest
+import numpy as np
+from msgq.visionipc import VisionIpcClient
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal.services import SERVICE_LIST
+from iqpilot.selfdrive.test.helpers import processes_context
+from iqpilot.system.camerad.snapshot import VISION_STREAMS
+from iqpilot.system.manager.process_config import managed_processes
+from iqpilot.tools.lib.logreader import msgs_to_time_series
+
+TEST_TIMESPAN = 10
+CAMERAS = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState')
+TEST_PATTERN_FRAMES = 200
+TEST_PATTERN_MIN_CONFIDENCE = 10
+TEST_PATTERN_CONNECT_TIMEOUT = 15
+TEST_PATTERN_CONFIGS = {
+  'ox03c10': (41, 4),
+  'os04c10': (97, 4),
+}
+
+
+def _pattern_sample(client):
+  buf = client.recv(1000)
+  if buf is None:
+    return None
+
+  y = np.asarray(buf.data[:buf.uv_offset], dtype=np.uint8).reshape((-1, buf.stride))[:buf.height, :buf.width]
+  profile = y[:, ::8].mean(axis=1)
+  padded = np.pad(profile, (4, 4), mode='edge')
+  neighbors = [padded[i:i + len(profile)] for i in range(9) if i != 4]
+  residual = profile - np.median(neighbors, axis=0)
+  position = int(np.argmax(residual))
+  return client.frame_id, client.timestamp_sof, position, residual[position], buf.height
+
+
+def _test_pattern_session():
+  samples = {camera: [] for camera in CAMERAS}
+  sockets = {camera: messaging.sub_sock(camera, conflate=False, timeout=100) for camera in CAMERAS}
+  logs = []
+  with pytest.MonkeyPatch.context() as monkeypatch:
+    monkeypatch.setenv('SPECTRA_TEST_PATTERN', '1')
+    monkeypatch.setenv('SPECTRA_ERROR_PROB', '-1')
+    with processes_context(['camerad']) as processes:
+      clients = {camera: VisionIpcClient('camerad', VISION_STREAMS[camera], False) for camera in CAMERAS}
+      pending = set(clients)
+      deadline = time.monotonic() + TEST_PATTERN_CONNECT_TIMEOUT
+      while pending and time.monotonic() < deadline:
+        assert processes[0].proc is not None and processes[0].proc.exitcode is None
+        pending = {camera for camera in pending if not clients[camera].connect(False)}
+        if pending:
+          time.sleep(0.1)
+      assert not pending, f'VisionIPC connection timeout: {sorted(pending)}'
+
+      for _ in range(TEST_PATTERN_FRAMES):
+        for camera, client in clients.items():
+          sample = _pattern_sample(client)
+          if sample is not None:
+            samples[camera].append(sample)
+        for sock in sockets.values():
+          logs.extend(messaging.drain_sock(sock))
+
+  return msgs_to_time_series(logs), samples
+
+
+def run_and_log(procs, services, duration):
+  logs = []
+
+  try:
+    for p in procs:
+      managed_processes[p].start()
+    socks = [messaging.sub_sock(s, conflate=False, timeout=100) for s in services]
+
+    start_time = time.monotonic()
+    while time.monotonic() - start_time < duration:
+      for s in socks:
+        logs.extend(messaging.drain_sock(s))
+    for p in procs:
+      assert managed_processes[p].proc.is_alive()
+  finally:
+    for p in procs:
+      managed_processes[p].stop()
+
+  return logs
+
+@pytest.fixture(scope="module")
+def logs():
+  logs = run_and_log(["camerad", ], CAMERAS, TEST_TIMESPAN)
+  ts = msgs_to_time_series(logs)
+
+  for cam in CAMERAS:
+    expected_frames = SERVICE_LIST[cam].frequency * TEST_TIMESPAN
+    cnt = len(ts[cam]['t'])
+    assert expected_frames*0.8 < cnt < expected_frames*1.2, f"unexpected frame count {cam}: {expected_frames=}, got {cnt}"
+
+    dts = np.abs(np.diff([ts[cam]['timestampSof']/1e6]) - 1000/SERVICE_LIST[cam].frequency)
+    assert (dts < 1.0).all(), f"{cam} dts(ms) out of spec: max diff {dts.max()}, 99 percentile {np.percentile(dts, 99)}"
+  return ts
+
+@pytest.mark.tici
+class TestCamerad:
+  def test_frame_skips(self, logs):
+    for c in CAMERAS:
+      assert set(np.diff(logs[c]['frameId'])) == {1, }, f"{c} has frame skips"
+
+  def test_frame_sync(self, logs):
+    n = range(len(logs['roadCameraState']['t'][:-10]))
+
+    frame_ids = {i: [logs[cam]['frameId'][i] for cam in CAMERAS] for i in n}
+    assert all(len(set(v)) == 1 for v in frame_ids.values()), "frame IDs not aligned"
+
+    frame_times = {i: [logs[cam]['timestampSof'][i] for cam in CAMERAS] for i in n}
+    diffs = {i: (max(ts) - min(ts))/1e6 for i, ts in frame_times.items()}
+
+    laggy_frames = {k: v for k, v in diffs.items() if v > 1.1}
+    assert len(laggy_frames) == 0, f"Frames not synced properly: {laggy_frames=}"
+
+  def test_sanity_checks(self, logs):
+    self._sanity_checks(logs)
+
+  def _sanity_checks(self, ts):
+    for c in CAMERAS:
+      assert c in ts
+      assert len(ts[c]['t']) > 20
+
+      # not a valid request id
+      assert 0 not in ts[c]['requestId']
+
+      # should monotonically increase
+      assert np.all(np.diff(ts[c]['frameId']) >= 1)
+      assert np.all(np.diff(ts[c]['requestId']) >= 1)
+
+      # EOF > SOF
+      assert np.all((ts[c]['timestampEof'] - ts[c]['timestampSof']) > 0)
+
+      # logMonoTime > SOF
+      assert np.all((ts[c]['t'] - ts[c]['timestampSof']/1e9) > 1e-7)
+
+      # logMonoTime > EOF, needs some tolerance since EOF is (SOF + readout time) but there is noise in the SOF timestamping (done via IRQ)
+      assert np.mean((ts[c]['t'] - ts[c]['timestampEof']/1e9) > 1e-7) > 0.7  # should be mostly logMonoTime > EOF
+      assert np.all((ts[c]['t'] - ts[c]['timestampEof']/1e9) > -0.10)        # when EOF > logMonoTime, it should never be more than two frames
+
+  def test_stress_test(self):
+    os.environ['SPECTRA_ERROR_PROB'] = '0.008'
+    logs = run_and_log(["camerad", ], CAMERAS, 10)
+    ts = msgs_to_time_series(logs)
+
+    # we should see some jumps from introduced errors
+    assert np.max([ np.max(np.diff(ts[c]['frameId'])) for c in CAMERAS ]) > 1
+    assert np.max([ np.max(np.diff(ts[c]['requestId'])) for c in CAMERAS ]) > 1
+
+    self._sanity_checks(ts)
+
+
+@pytest.fixture(scope="module")
+def test_pattern_data():
+  return _test_pattern_session()
+
+
+@pytest.mark.tici
+@pytest.mark.xdist_group("camerad_test_pattern")
+class TestCameradTestPattern:
+  def test_frame_delivery(self, test_pattern_data):
+    logs, samples_by_camera = test_pattern_data
+    for camera in CAMERAS:
+      assert camera in logs
+      samples = samples_by_camera[camera]
+      assert len(samples) > TEST_PATTERN_FRAMES * 0.9
+
+      state_frame_ids = logs[camera]['frameId']
+      state_request_ids = logs[camera]['requestId']
+      vipc_frame_ids = np.array([sample[0] for sample in samples])
+      for source, frame_ids in (('camera state', state_frame_ids), ('VisionIPC', vipc_frame_ids)):
+        frame_steps = np.diff(frame_ids)
+        skipped = frame_ids[1:][frame_steps != 1]
+        assert len(skipped) == 0, f'{camera} {source} skipped frames before {skipped}'
+
+      expected_sof_step = 1e9 / SERVICE_LIST[camera].frequency
+      sof_step_errors = np.diff(logs[camera]['timestampSof']) - expected_sof_step
+      assert np.all(np.abs(sof_step_errors) < 1e6), f'{camera} SOF cadence errors: {sof_step_errors[np.abs(sof_step_errors) >= 1e6]}'
+
+      request_steps = np.diff(state_request_ids)
+      skipped_requests = state_request_ids[1:][request_steps != 1]
+      assert len(skipped_requests) == 0, f'{camera} skipped requests before {skipped_requests}'
+
+      state_sofs = dict(zip(state_frame_ids, logs[camera]['timestampSof'], strict=True))
+      matched_samples = [sample for sample in samples if sample[0] in state_sofs]
+      assert len(matched_samples) > len(samples) * 0.8
+      mismatched_sofs = {
+        frame_id: (timestamp_sof, state_sofs[frame_id]) for frame_id, timestamp_sof, *_ in matched_samples if timestamp_sof != state_sofs[frame_id]
+      }
+      assert not mismatched_sofs, f'{camera} VisionIPC/camera state SOFs disagree: {mismatched_sofs}'
+
+  def test_pattern(self, test_pattern_data):
+    logs, samples_by_camera = test_pattern_data
+    for camera in CAMERAS:
+      sensors = set(logs[camera]['sensor'])
+      assert len(sensors) == 1
+      sensor = sensors.pop()
+      assert sensor in TEST_PATTERN_CONFIGS, f'unsupported test pattern sensor: {sensor}'
+      cycle_frames, position_tolerance = TEST_PATTERN_CONFIGS[sensor]
+
+      samples = samples_by_camera[camera]
+      confident = [sample for sample in samples if sample[3] > TEST_PATTERN_MIN_CONFIDENCE]
+      positions = np.array([sample[2] for sample in confident])
+      assert len(confident) > len(samples) * 0.7, f'{camera} test pattern confidence too low'
+      assert len(np.unique(positions)) > 20, f'{camera} test pattern is not moving'
+      assert np.ptp(positions) > confident[0][4] * 0.75, f'{camera} test pattern does not span the frame'
+
+      samples_by_frame = {sample[0]: sample for sample in confident}
+      repeating_pairs = [(sample, samples_by_frame[sample[0] + cycle_frames]) for sample in confident if sample[0] + cycle_frames in samples_by_frame]
+      assert len(repeating_pairs) > 20
+      unexpected = [(first[0], first[2], second[2]) for first, second in repeating_pairs if abs(second[2] - first[2]) > position_tolerance]
+      assert len(unexpected) < len(repeating_pairs) * 0.3, f'{camera} test pattern cycle mismatches: {unexpected}'

iqpilot/system/camerad/test/test_exposure.py

@@ -0,0 +1,51 @@
+import time
+import numpy as np
+import pytest
+
+from iqpilot.selfdrive.test.helpers import with_processes
+from iqpilot.system.camerad.snapshot import get_snapshots
+
+TEST_TIME = 45
+REPEAT = 5
+
+@pytest.mark.tici
+class TestCamerad:
+  @classmethod
+  def setup_class(cls):
+    pass
+
+  def _numpy_rgb2gray(self, im):
+    ret = np.clip(im[:,:,2] * 0.114 + im[:,:,1] * 0.587 + im[:,:,0] * 0.299, 0, 255).astype(np.uint8)
+    return ret
+
+  def _is_exposure_okay(self, i, med_mean=None):
+    if med_mean is None:
+      med_mean = np.array([[0.18,0.3],[0.18,0.3]])
+    h, w = i.shape[:2]
+    i = i[h//10:9*h//10,w//10:9*w//10]
+    med_ex, mean_ex = med_mean
+    i = self._numpy_rgb2gray(i)
+    i_median = np.median(i) / 255.
+    i_mean = np.mean(i) / 255.
+    print([i_median, i_mean])
+    return med_ex[0] < i_median < med_ex[1] and mean_ex[0] < i_mean < mean_ex[1]
+
+  @with_processes(['camerad'])
+  def test_camera_operation(self):
+    passed = 0
+    start = time.monotonic()
+    while time.monotonic() - start < TEST_TIME and passed < REPEAT:
+      rpic, dpic = get_snapshots(frame="roadCameraState", front_frame="driverCameraState")
+      wpic, _ = get_snapshots(frame="wideRoadCameraState")
+
+      res = self._is_exposure_okay(rpic)
+      res = res and self._is_exposure_okay(dpic)
+      res = res and self._is_exposure_okay(wpic)
+
+      if passed > 0 and not res:
+        passed = -passed # fails test if any failure after first sus
+        break
+
+      passed += int(res)
+      time.sleep(2)
+    assert passed >= REPEAT