IQ.Pilot Release Commit @ 5bc9cd3

system/camerad/test/.gitignore

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

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

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

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

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

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;
+}

system/camerad/test/test_camerad.py

@@ -0,0 +1,101 @@
+import os
+import time
+import pytest
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal.services import SERVICE_LIST
+from openpilot.system.manager.process_config import managed_processes
+from openpilot.tools.lib.log_time_series import msgs_to_time_series
+
+TEST_TIMESPAN = 10
+CAMERAS = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState')
+
+
+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)

system/camerad/test/test_exposure.py

@@ -0,0 +1,51 @@
+import time
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.test.helpers import with_processes
+from openpilot.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