IQ.Pilot Release Commit @ f2a861c

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_ae_gray.cc

@@ -0,0 +1,84 @@
+#define CATCH_CONFIG_MAIN
+#include "catch2/catch.hpp"
+
+#include <cassert>
+
+#include <cmath>
+#include <cstring>
+
+#include "common/util.h"
+#include "system/camerad/cameras/camera_common.h"
+
+#define W 240
+#define H 160
+
+
+#define TONE_SPLITS 3
+
+float gts[TONE_SPLITS * TONE_SPLITS * TONE_SPLITS * TONE_SPLITS] = {
+  0.917969, 0.917969, 0.375000, 0.917969, 0.375000, 0.375000, 0.187500, 0.187500, 0.187500, 0.917969,
+  0.375000, 0.375000, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.093750, 0.093750,
+  0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.917969, 0.375000, 0.375000,
+  0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.093750, 0.093750, 0.093750, 0.093750,
+  0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750,
+  0.093750, 0.093750, 0.093750, 0.093750, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
+  0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
+  0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
+  0.000000};
+
+
+TEST_CASE("camera.test_calculate_exposure_value") {
+  // set up fake camerabuf
+  CameraBuf cb = {};
+  VisionBuf vb = {};
+  uint8_t * fb_y = new uint8_t[W*H];
+  vb.y = fb_y;
+  cb.cur_yuv_buf = &vb;
+  cb.out_img_width = W;
+  cb.out_img_height = H;
+  Rect rect = {0, 0, W-1, H-1};
+
+  printf("AE test patterns %dx%d\n", cb.out_img_width, cb.out_img_height);
+
+  // mix of 5 tones
+  uint8_t l[5] = {0, 24, 48, 96, 235}; // 235 is yuv max
+
+  bool passed = true;
+  float rtol = 0.05;
+  // generate pattern and calculate EV
+  int cnt = 0;
+  for (int i_0=0; i_0<TONE_SPLITS; i_0++) {
+    for (int i_1=0; i_1<TONE_SPLITS; i_1++) {
+      for (int i_2=0; i_2<TONE_SPLITS; i_2++) {
+        for (int i_3=0; i_3<TONE_SPLITS; i_3++) {
+          int h_0 = i_0 * H / TONE_SPLITS;
+          int h_1 = i_1 * (H - h_0) / TONE_SPLITS;
+          int h_2 = i_2 * (H - h_0 - h_1) / TONE_SPLITS;
+          int h_3 = i_3 * (H - h_0 - h_1 - h_2) / TONE_SPLITS;
+          int h_4 = H - h_0 - h_1 - h_2 - h_3;
+          memset(&fb_y[0], l[0], h_0*W);
+          memset(&fb_y[h_0*W], l[1], h_1*W);
+          memset(&fb_y[h_0*W+h_1*W], l[2], h_2*W);
+          memset(&fb_y[h_0*W+h_1*W+h_2*W], l[3], h_3*W);
+          memset(&fb_y[h_0*W+h_1*W+h_2*W+h_3*W], l[4], h_4*W);
+          float ev = calculate_exposure_value((const CameraBuf*) &cb, rect, 1, 1);
+          // printf("%d/%d/%d/%d/%d ev is %f\n", h_0, h_1, h_2, h_3, h_4, ev);
+          // printf("%f\n", ev);
+
+          // compare to gt
+          float evgt = gts[cnt];
+          if (fabs(ev - evgt) > rtol*evgt) {
+            passed = false;
+          }
+
+          // report
+          printf("%d/%d/%d/%d/%d: ev %f, gt %f, err %f\n", h_0, h_1, h_2, h_3, h_4, ev, evgt, fabs(ev - evgt) / (evgt != 0 ? evgt : 0.00001f));
+          cnt++;
+        }
+      }
+    }
+  }
+  assert(passed);
+
+  delete[] fb_y;
+}

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