diff --git a/openpilot/system/camerad/cameras/camera_qcom2.cc b/openpilot/system/camerad/cameras/camera_qcom2.cc index 63640920db9b23..6c5aaea5a02cf3 100644 --- a/openpilot/system/camerad/cameras/camera_qcom2.cc +++ b/openpilot/system/camerad/cameras/camera_qcom2.cc @@ -294,7 +294,13 @@ void camerad_thread() { for (auto &cam : cams) { if (event_data->session_hdl == cam->camera.session_handle) { if (cam->camera.handle_camera_event(event_data)) { + assert(cam->camera.buf.cur_frame_data.request_id + 1 == cam->camera.next_request_id - cam->camera.requests_in_flight); cam->sendState(); + if (cam->camera.held_buf_idx >= 0) { + assert(cam->camera.next_request_id % cam->camera.ife_buf_depth == cam->camera.held_buf_idx); + cam->camera.enqueue_frame(); + } + cam->camera.held_buf_idx = cam->camera.buf.cur_buf_idx; } break; } diff --git a/openpilot/system/camerad/cameras/spectra.cc b/openpilot/system/camerad/cameras/spectra.cc index f2d849ae28264c..9c3fc25b25555c 100644 --- a/openpilot/system/camerad/cameras/spectra.cc +++ b/openpilot/system/camerad/cameras/spectra.cc @@ -297,7 +297,7 @@ void SpectraCamera::camera_open(VisionIpcServer *v) { LOGD("camera init %d", cc.camera_num); buf.init(this, v, ife_buf_depth, cc.stream_type); camera_map_bufs(); - clearAndRequeue(1); + clearAndRequeue(); } void SpectraCamera::sensors_start() { @@ -942,7 +942,11 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) { assert(ret == 0); } -void SpectraCamera::enqueue_frame(uint64_t request_id) { +void SpectraCamera::enqueue_frame() { + // The kernel reports only requests newer than reported_req_id, which a flush does not reset. + // Always allocate a new ID so recovery never reuses one the kernel has already seen. + // https://github.com/commaai/agnos-kernel-sdm845/blob/93ddd472ce522ab8669456b11bf3924ad32e9882/drivers/media/platform/msm/camera/cam_isp/cam_isp_context.c#L609-L613 + uint64_t request_id = next_request_id++; int i = request_id % ife_buf_depth; assert(sync_objs_ife[i] == 0); @@ -981,6 +985,7 @@ void SpectraCamera::enqueue_frame(uint64_t request_id) { // submit request to IFE and BPS config_ife(i, request_id); if (cc.output_type == ISP_BPS_PROCESSED) config_bps(i, request_id); + requests_in_flight++; } void SpectraCamera::destroySyncObjectAt(int index) { @@ -1416,26 +1421,25 @@ bool SpectraCamera::handle_camera_event(const cam_req_mgr_message *event_data) { if (!validateEvent(request_id, ife_frame_id)) { return false; } + assert(requests_in_flight > 0); + requests_in_flight--; // Update tracking variables - if (request_id == last_valid_request_id + 1) { - skip_expected = false; - } last_valid_ife_frame_id = ife_frame_id; - last_valid_request_id = request_id; // Wait until frame's fully read out and processed if (!waitForFrameReady(request_id)) { // Reset queue on sync failure to prevent frame tearing LOGE("camera %d sync failure %ld %ld ", cc.camera_num, request_id, ife_frame_id); - clearAndRequeue(request_id + 1); + clearAndRequeue(); return false; } int buf_idx = request_id % ife_buf_depth; bool ret = processFrame(buf_idx, request_id, ife_frame_id, timestamp); destroySyncObjectAt(buf_idx); - enqueue_frame(request_id + ife_buf_depth); // request next frame for this slot + if (!ret) enqueue_frame(); + if (stress_test("publish delay")) util::sleep_for(1200); return ret; } @@ -1445,39 +1449,42 @@ bool SpectraCamera::validateEvent(uint64_t request_id, uint64_t ife_frame_id) { if (request_id == 0) { if (invalid_request_count++ > ife_buf_depth+2) { LOGE("camera %d reset after half second of invalid requests", cc.camera_num); - clearAndRequeue(last_valid_request_id + 1); + clearAndRequeue(); invalid_request_count = 0; } return false; } invalid_request_count = 0; - // check for skips in frame_id or request_id - if (!skip_expected) { - if (ife_frame_id != last_valid_ife_frame_id + 1) { - LOGE("camera %d frame ID skipped, %lu -> %lu", cc.camera_num, last_valid_ife_frame_id, ife_frame_id); - clearAndRequeue(request_id + 1); - return false; - } + uint64_t expected_request_id = next_request_id - requests_in_flight; + if (request_id != expected_request_id) { + LOGE("camera %d request ID mismatch, expected %lu, got %lu", cc.camera_num, expected_request_id, request_id); + clearAndRequeue(); + return false; + } - if (request_id != last_valid_request_id + 1) { - LOGE("camera %d requests skipped %ld -> %ld", cc.camera_num, last_valid_request_id, request_id); - clearAndRequeue(request_id + 1); - return false; - } + if (last_valid_ife_frame_id != 0 && ife_frame_id != last_valid_ife_frame_id + 1) { + LOGE("camera %d frame ID skipped, %lu -> %lu", cc.camera_num, last_valid_ife_frame_id, ife_frame_id); + clearAndRequeue(); + return false; } return true; } -void SpectraCamera::clearAndRequeue(uint64_t from_request_id) { +void SpectraCamera::clearAndRequeue() { // clear everything, then queue up a fresh set of frames - LOGW("clearing and requeuing camera %d from %lu", cc.camera_num, from_request_id); + LOGW("clearing and requeuing camera %d from %lu", cc.camera_num, next_request_id); clear_req_queue(); + requests_in_flight = 0; last_requeue_ts = nanos_since_boot(); - for (uint64_t id = from_request_id; id < from_request_id + ife_buf_depth; ++id) { - enqueue_frame(id); + if (held_buf_idx >= 0) { + assert(next_request_id % ife_buf_depth == held_buf_idx); + next_request_id++; // keep the last published buffer out of the hardware queue + } + for (int i = 0; i < ife_buf_depth - (held_buf_idx >= 0); ++i) { + enqueue_frame(); } - skip_expected = true; + last_valid_ife_frame_id = 0; // accept any IFE frame ID after requeue } bool SpectraCamera::waitForFrameReady(uint64_t request_id) { @@ -1490,10 +1497,11 @@ bool SpectraCamera::waitForFrameReady(uint64_t request_id) { } auto waitForSync = [&](uint32_t sync_obj, int timeout_ms, const char *sync_type) { + if (stress_test(sync_type)) return false; double st = millis_since_boot(); struct cam_sync_wait sync_wait = {}; sync_wait.sync_obj = sync_obj; - sync_wait.timeout_ms = stress_test(sync_type) ? 1 : timeout_ms; + sync_wait.timeout_ms = timeout_ms; bool ret = do_sync_control(m->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)) == 0; double et = millis_since_boot(); if (!ret) LOGE("camera %d %s failed after %.2fms", cc.camera_num, sync_type, et-st); diff --git a/openpilot/system/camerad/cameras/spectra.h b/openpilot/system/camerad/cameras/spectra.h index acb20003135cb6..aa12b0e78ca2aa 100644 --- a/openpilot/system/camerad/cameras/spectra.h +++ b/openpilot/system/camerad/cameras/spectra.h @@ -144,7 +144,7 @@ class SpectraCamera { void config_ife(int idx, int request_id, bool init=false); int clear_req_queue(); - void enqueue_frame(uint64_t request_id); + void enqueue_frame(); int sensors_init(); void sensors_start(); @@ -205,17 +205,18 @@ class SpectraCamera { int buf_handle_raw[MAX_IFE_BUFS] = {}; int sync_objs_ife[MAX_IFE_BUFS] = {}; int sync_objs_bps[MAX_IFE_BUFS] = {}; - uint64_t last_valid_request_id = 0; + uint64_t next_request_id = 1; + int requests_in_flight = 0; + int held_buf_idx = -1; uint64_t last_requeue_ts = 0; uint64_t last_valid_ife_frame_id = 0; int invalid_request_count = 0; - bool skip_expected = true; CameraBuf buf; SpectraMaster *m; private: - void clearAndRequeue(uint64_t from_request_id); + void clearAndRequeue(); bool validateEvent(uint64_t request_id, uint64_t ife_frame_id); bool waitForFrameReady(uint64_t request_id); bool processFrame(int buf_idx, uint64_t request_id, uint64_t ife_frame_id, uint64_t timestamp); @@ -230,10 +231,12 @@ class SpectraCamera { // a mode for stressing edge cases: realignment, sync failures, etc. inline bool stress_test(std::string log) { - static double last_trigger = 0; + static double last_trigger = millis_since_boot(); static double prob = std::stod(util::getenv("SPECTRA_ERROR_PROB", "-1")); static double dt = std::stod(util::getenv("SPECTRA_ERROR_DT", "1")); - bool triggered = (prob > 0) && \ + static std::string filter = util::getenv("SPECTRA_ERROR_FILTER"); + static int camera = std::stoi(util::getenv("SPECTRA_ERROR_CAMERA", "-1")); + bool triggered = (camera < 0 || camera == cc.camera_num) && (filter.empty() || filter == log) && (prob > 0) && \ ((static_cast(rand()) / RAND_MAX) < prob) && \ (millis_since_boot() - last_trigger) > dt; if (triggered) { diff --git a/openpilot/system/camerad/sensors/os04c10.cc b/openpilot/system/camerad/sensors/os04c10.cc index d14c288b919c9b..c19fea1b66c17a 100644 --- a/openpilot/system/camerad/sensors/os04c10.cc +++ b/openpilot/system/camerad/sensors/os04c10.cc @@ -1,4 +1,5 @@ #include +#include #include "system/camerad/sensors/sensor.h" #include @@ -37,6 +38,9 @@ OS04C10::OS04C10() { start_reg_array.assign(std::begin(start_reg_array_os04c10), std::end(start_reg_array_os04c10)); init_reg_array.assign(std::begin(init_array_os04c10), std::end(init_array_os04c10)); + if (std::getenv("SPECTRA_TEST_PATTERN")) { + init_reg_array.push_back({0x5080, 0xc4}); + } probe_reg_addr = 0x300a; probe_expected_data = 0x5304; bits_per_pixel = 12; diff --git a/openpilot/system/camerad/sensors/ox03c10.cc b/openpilot/system/camerad/sensors/ox03c10.cc index e0e0dec2ffe948..ea6eb374506ed4 100644 --- a/openpilot/system/camerad/sensors/ox03c10.cc +++ b/openpilot/system/camerad/sensors/ox03c10.cc @@ -1,4 +1,5 @@ #include +#include #include "system/camerad/sensors/sensor.h" #include @@ -37,6 +38,12 @@ OX03C10::OX03C10() { start_reg_array.assign(std::begin(start_reg_array_ox03c10), std::end(start_reg_array_ox03c10)); init_reg_array.assign(std::begin(init_array_ox03c10), std::end(init_array_ox03c10)); + if (std::getenv("SPECTRA_TEST_PATTERN")) { + init_reg_array.insert(init_reg_array.end(), { + {0x5004, 0x1f}, {0x5005, 0x1f}, {0x5006, 0x1f}, {0x5007, 0x1f}, + {0x5240, 0x03}, {0x5440, 0x03}, {0x5640, 0x03}, {0x5840, 0x03}, + }); + } probe_reg_addr = 0x300a; probe_expected_data = 0x5803; bits_per_pixel = 12; diff --git a/openpilot/system/camerad/test/test_camerad.py b/openpilot/system/camerad/test/test_camerad.py index 0d66f0e16b9f68..a8ca4d3c490366 100755 --- a/openpilot/system/camerad/test/test_camerad.py +++ b/openpilot/system/camerad/test/test_camerad.py @@ -3,8 +3,10 @@ import os import time import unittest +from unittest.mock import patch import numpy as np +from msgq.visionipc import VisionIpcClient, VisionStreamType from openpilot.common.parameterized import parameterized from openpilot.common.test import OpenpilotTestCase from openpilot.cereal.services import SERVICE_LIST @@ -17,6 +19,12 @@ EXPOSURE_STABLE_COUNT = 3 EXPOSURE_RANGE = (0.15, 0.35) MAX_TEST_TIME = 25 +STRESS_ERRORS = ( + ("skip_sof", "skipping SOF event"), + ("processing_delay", "sync sleep time"), + ("ife_timeout", "IFE sync"), + ("bps_timeout", "BPS sync"), +) def _numpy_rgb2gray(im): @@ -37,6 +45,39 @@ def _exposure_stable(results): for v in results.values() ) +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:4, :buf.width:4] + profile = y.mean(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)) + if residual[position] <= 10: + return None + return client.timestamp_sof, client.frame_id, position / len(profile) + +def _sanity_checks(ts): + for camera in CAMERAS: + assert camera in ts + assert len(ts[camera]['t']) > 20 + assert 0 not in ts[camera]['requestId'] + + frame_steps = np.diff(ts[camera]['frameId']) + assert np.all(frame_steps > 0) + assert np.all(np.diff(ts[camera]['requestId']) > 0) + # Skipped frame IDs must account for the same number of frame periods in SOF time. + expected_sof_steps = frame_steps * 1e9 / SERVICE_LIST[camera].frequency + sof_step_errors = np.diff(ts[camera]['timestampSof']) - expected_sof_steps + assert np.all(np.abs(sof_step_errors) < 2e6), f"{camera} frame/SOF steps disagree: {sof_step_errors[np.abs(sof_step_errors) >= 2e6]}" + + assert np.all((ts[camera]['timestampEof'] - ts[camera]['timestampSof']) > 0) + assert np.all((ts[camera]['t'] - ts[camera]['timestampSof']/1e9) > 1e-7) + assert np.mean((ts[camera]['t'] - ts[camera]['timestampEof']/1e9) > 1e-7) > 0.7 + assert np.all((ts[camera]['t'] - ts[camera]['timestampEof']/1e9) > -0.10) + def run_and_log(procs, services, duration): with processes_context(procs): @@ -124,43 +165,59 @@ def test_frame_sync(self): assert 20 < offset_ms < 30, f"driver camera stagger out of range at frame {i}: {offset_ms:.1f}ms (expected ~25ms)" def test_sanity_checks(self): - self._sanity_checks(self.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'] + _sanity_checks(self.logs) - # 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 +class TestCameradStress(OpenpilotTestCase): + TICI_TEST = True - def test_stress_test(self): - os.environ['SPECTRA_ERROR_PROB'] = '0.008' - try: - logs = run_and_log(["camerad", ], CAMERAS, 10) - finally: - del os.environ['SPECTRA_ERROR_PROB'] + @parameterized.expand(STRESS_ERRORS, ids=lambda name, _: name) + def test_stress_test(self, _, error): + env = {'SPECTRA_ERROR_FILTER': error, 'SPECTRA_ERROR_PROB': '1', 'SPECTRA_ERROR_DT': '2000'} + with patch.dict(os.environ, env): + logs = run_and_log(["camerad"], CAMERAS, 6) 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) + assert max(np.max(np.diff(ts[c]['frameId'])) for c in CAMERAS) > 1 + assert max(np.max(np.diff(ts[c]['requestId'])) for c in CAMERAS) > 1 + _sanity_checks(ts) + + def test_frame_data_alignment(self): + env = {'SPECTRA_TEST_PATTERN': '1', 'SPECTRA_ERROR_FILTER': 'publish delay', 'SPECTRA_ERROR_CAMERA': '1', + 'SPECTRA_ERROR_PROB': '1', 'SPECTRA_ERROR_DT': '6000'} + samples = {'road': [], 'wide': []} + with patch.dict(os.environ, env), processes_context(["camerad"]), log_collector(CAMERAS) as (raw_logs, lock): + clients = { + 'road': VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True), + 'wide': VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, True), + } + for client in clients.values(): + client.connect(True) + end = time.monotonic() + 10 + while time.monotonic() < end: + for camera, client in clients.items(): + sample = _pattern_sample(client) + if sample is not None: + samples[camera].append(sample) + + with lock: + ts = msgs_to_time_series(raw_logs) + assert all(samples.values()) + offsets = [] + for timestamp, frame_id, phase in samples['road']: + wide = min(samples['wide'], key=lambda sample: abs(sample[0] - timestamp)) + if abs(wide[0] - timestamp) < 1.1e6: + offsets.append((frame_id, (phase - wide[2]) % 1)) + assert len(offsets) > 20 + expected_offsets = {offset for _, offset in offsets[:len(offsets) // 2]} + # Allow small sensor-specific phase jitter while rejecting a substituted frame. + tolerance = 1 / 32 + unexpected = {frame_id: offset for frame_id, offset in offsets[len(offsets) // 2:] + if all(abs((offset - expected + .5) % 1 - .5) > tolerance for expected in expected_offsets)} + assert not unexpected, f"road/wide pixels disagree for synchronized frames: {unexpected}" + + assert max(np.max(np.diff(ts[c]['frameId'])) for c in CAMERAS) > 1 + _sanity_checks(ts) if __name__ == "__main__":