From patchwork Tue Nov 26 12:17:05 2024 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: David Plowman X-Patchwork-Id: 22098 Return-Path: X-Original-To: parsemail@patchwork.libcamera.org Delivered-To: parsemail@patchwork.libcamera.org Received: from lancelot.ideasonboard.com (lancelot.ideasonboard.com [92.243.16.209]) by patchwork.libcamera.org (Postfix) with ESMTPS id DB681C0DA4 for ; Tue, 26 Nov 2024 12:17:18 +0000 (UTC) Received: from lancelot.ideasonboard.com (localhost [IPv6:::1]) by lancelot.ideasonboard.com (Postfix) with ESMTP id ECC636606D; Tue, 26 Nov 2024 13:17:15 +0100 (CET) Authentication-Results: lancelot.ideasonboard.com; dkim=pass (2048-bit key; unprotected) header.d=raspberrypi.com header.i=@raspberrypi.com header.b="RYEupJZA"; dkim-atps=neutral Received: from mail-wm1-x32e.google.com (mail-wm1-x32e.google.com [IPv6:2a00:1450:4864:20::32e]) by lancelot.ideasonboard.com (Postfix) with ESMTPS id 5648C65FBA for ; Tue, 26 Nov 2024 13:17:11 +0100 (CET) Received: by mail-wm1-x32e.google.com with SMTP id 5b1f17b1804b1-4315e9e9642so51086395e9.0 for ; Tue, 26 Nov 2024 04:17:11 -0800 (PST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=raspberrypi.com; s=google; t=1732623431; x=1733228231; darn=lists.libcamera.org; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:cc:to:from:from:to:cc:subject:date :message-id:reply-to; bh=W9JLKZInM78lcIWFvLsDR1/OYxWXBHffYFHvcJoTyBc=; b=RYEupJZAuwms8/j7eRWBVKWHsECiJ9fFQMBqZIBWfAtZKRRi/dDSiL29c5aUE/33j3 lpXMGQfHNQ8ZXRXu9q0aHj6zsC/phZhooW+JVh3nBLMleOj7g1SZOahgiv7EONvgcK2J cF48/g3fZMxLbZ4G4lOFlpzbwzb0nfB/IcuBi+GniIhS1ZvjROj7qTwqjoembruZDBGH dQPY6QtkmQzAySDf7bpZx8tSyHwZ3mwnjPD0PN1FnCOrvUHXvFPPEMtIAsmVlIljrEDH kvnv9Yk6aSl05PIN19d5jHkwhoTESjZSksmXD+klC7VEQ0YE0DzYOFvcC2gdhItMdaQW RfcQ== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20230601; t=1732623431; x=1733228231; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:cc:to:from:x-gm-message-state:from:to:cc :subject:date:message-id:reply-to; bh=W9JLKZInM78lcIWFvLsDR1/OYxWXBHffYFHvcJoTyBc=; b=n2WIl1aL94GvHgpj486xM4EuSuzGEH6bfxWD6ZRt6h2xg5hpEuN9RRM8jbh18q7vXj JXsEDOuAWHsPTi4W2+YBCPh8lrw5msQoTonc03XvhqpvnhF1CMZRc7RS/whiBw4t0b/M YEQqoeMX5iGIe08EFy9JXLaIuP5WHqOwDr08AtUQPGdAqQ4vE6GH0luJv8uTci+Jg4O7 Ed/wXDsPLSC9Ucsg2wjKrflv/owc1muzNJ10f/hW6i/fs8GT2b32ZB4b5DuS+LzlZ/te sicq4xVlV2LGH/ccxcXswKZQJKi3nBu/54idisjn+dz2IM6xyhQqA76AaTYZth4uGSNe Gtvg== X-Gm-Message-State: AOJu0YxFAldcQIQV9/aGtStLM1igR3SxGBzBsu+FeRU5utioh5ecd9mk W4qHMWxmlpBMtn5TiTB2ZxVP7urbifBS733TL+O8M/d7+YzcZf1vxoc0F3El30z1GSGQJGbR2sX 8 X-Gm-Gg: ASbGncueHchzolrWpFwcuN+DkVkOCavAlw+02Q/Hb/20zTStL43bhMYZuCtpRbDGTCY RNu02qA+O3hPNAp30jCS6rWI/gUOyMW0U/+A9N3U4QXSwV6FFOBy2spG6cBjwU43DQHbXofCr7I d09F/CsyK1Wxd3XPXoA/zefdX84E2MhXhZCK6s7wcuagqswKtLf7ZoRI3V956qK1sj2LsU0euF9 DPfvCoscTZual/+I/oSpPhWOepp0IQf3jSFZjNhsv2jwa3lrjEd7lqHT86ZvdzSE7FnpxUBTWi1 P8+CJA== X-Google-Smtp-Source: AGHT+IEjkPgkaxXgjAINVcD1BiADVPpjUMrvQUtubDs2dS0VsKAZhOtK11KYiUikSFPWsf5tVVTTlA== X-Received: by 2002:a05:600c:5251:b0:434:a75b:5f6c with SMTP id 5b1f17b1804b1-434a75b6173mr17365145e9.10.1732623430660; Tue, 26 Nov 2024 04:17:10 -0800 (PST) Received: from raspberrypi.pitowers.org ([2a00:1098:3142:1f:c68a:6be1:5ba3:eddd]) by smtp.gmail.com with ESMTPSA id 5b1f17b1804b1-434a15d86a4sm51070325e9.36.2024.11.26.04.17.10 (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Tue, 26 Nov 2024 04:17:10 -0800 (PST) From: David Plowman To: libcamera-devel@lists.libcamera.org Cc: David Plowman Subject: [RFC PATCH 2/3] libcamera: clock: Add ClockRecovery class to help generate wallclock timestamps Date: Tue, 26 Nov 2024 12:17:05 +0000 Message-Id: <20241126121706.4350-3-david.plowman@raspberrypi.com> X-Mailer: git-send-email 2.39.5 In-Reply-To: <20241126121706.4350-1-david.plowman@raspberrypi.com> References: <20241126121706.4350-1-david.plowman@raspberrypi.com> MIME-Version: 1.0 X-BeenThere: libcamera-devel@lists.libcamera.org X-Mailman-Version: 2.1.29 Precedence: list List-Id: List-Unsubscribe: , List-Archive: List-Post: List-Help: List-Subscribe: , Errors-To: libcamera-devel-bounces@lists.libcamera.org Sender: "libcamera-devel" Sampling the system clock is susceptible to many milliseconds of jitter, dependent on system load and other factors. The ClockRecovery class takes pairs of kernel timestamps (which exhitbit much less jitter) and wallclock readings, and returns a smoother version of the wallclock timestamps. Signed-off-by: David Plowman Reviewed-by: Kieran Bingham --- include/libcamera/internal/clock_recovery.h | 64 ++++++++++++ include/libcamera/internal/meson.build | 1 + src/libcamera/clock_recovery.cpp | 110 ++++++++++++++++++++ src/libcamera/meson.build | 1 + 4 files changed, 176 insertions(+) create mode 100644 include/libcamera/internal/clock_recovery.h create mode 100644 src/libcamera/clock_recovery.cpp diff --git a/include/libcamera/internal/clock_recovery.h b/include/libcamera/internal/clock_recovery.h new file mode 100644 index 00000000..49747747 --- /dev/null +++ b/include/libcamera/internal/clock_recovery.h @@ -0,0 +1,64 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2024, Raspberry Pi Ltd + * + * Camera sync control algorithm + */ +#pragma once + +#include + +namespace libcamera { + +class ClockRecovery +{ +public: + ClockRecovery(); + + /* Initialise with configuration parameters and restart the fitting process. */ + void initialise(unsigned int numPts = 100, unsigned int maxJitter = 2000, unsigned int minPts = 10, + unsigned int errorThreshold = 50000); + /* Erase all history and restart the fitting process. */ + void reset(); + + // Add a new input clock / output clock sample. */ + void addSample(uint64_t input, uint64_t output); + /* Calculate the output clock value for this input. */ + uint64_t getOutput(uint64_t input); + +private: + unsigned int numPts_; /* how many samples contribute to the history */ + unsigned int maxJitter_; /* smooth out any jitter larger than this immediately */ + unsigned int minPts_; /* number of samples below which we treat clocks as 1:1 */ + unsigned int errorThreshold_; /* reset everything when the error exceeds this */ + + unsigned int count_; /* how many samples seen (up to numPts_) */ + uint64_t inputBase_; /* subtract this from all input values, just to make the numbers easier */ + uint64_t outputBase_; /* as above, for the output */ + + uint64_t lastInput_; /* the previous input sample */ + uint64_t lastOutput_; /* the previous output sample */ + + /* + * We do a linear regression of y against x, where: + * x is the value input - inputBase_, and + * y is the value output - outputBase_ - x. + * We additionally subtract x from y so that y "should" be zero, again making the numnbers easier. + */ + double xAve_; /* average x value seen so far */ + double yAve_; /* average y value seen so far */ + double x2Ave_; /* average x^2 value seen so far */ + double xyAve_; /* average x*y value seen so far */ + + /* + * Once we've seen more than minPts_ samples, we recalculate the slope and offset according + * to the linear regression normal equations. + */ + double slope_; /* latest slope value */ + double offset_; /* latest offset value */ + + /* We use this cumulative error to monitor spontaneous system clock updates. */ + double error_; +}; + +} //namespace libcamera diff --git a/include/libcamera/internal/meson.build b/include/libcamera/internal/meson.build index 1dddcd50..b6271ee1 100644 --- a/include/libcamera/internal/meson.build +++ b/include/libcamera/internal/meson.build @@ -11,6 +11,7 @@ libcamera_internal_headers = files([ 'camera_manager.h', 'camera_sensor.h', 'camera_sensor_properties.h', + 'clock_recovery.h', 'control_serializer.h', 'control_validator.h', 'converter.h', diff --git a/src/libcamera/clock_recovery.cpp b/src/libcamera/clock_recovery.cpp new file mode 100644 index 00000000..6dec8cb3 --- /dev/null +++ b/src/libcamera/clock_recovery.cpp @@ -0,0 +1,110 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2024, Raspberry Pi Ltd + * + * Camera sync control algorithm + */ + +#include "libcamera/internal/clock_recovery.h" + +#include + +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiClockRec) + +ClockRecovery::ClockRecovery() +{ + initialise(); +} + +void ClockRecovery::initialise(unsigned int numPts, unsigned int maxJitter, unsigned int minPts, + unsigned int errorThreshold) +{ + numPts_ = numPts; + maxJitter_ = maxJitter; + minPts_ = minPts; + errorThreshold_ = errorThreshold; + reset(); +} + +void ClockRecovery::reset() +{ + lastInput_ = 0; + lastOutput_ = 0; + xAve_ = 0; + yAve_ = 0; + x2Ave_ = 0; + xyAve_ = 0; + count_ = 0; + slope_ = 0.0; + offset_ = 0.0; + error_ = 0.0; +} + +void ClockRecovery::addSample(uint64_t input, uint64_t output) +{ + if (count_ == 0) { + inputBase_ = input; + outputBase_ = output; + } + + /* + * We keep an eye on cumulative drift over the last several frames. If this exceeds a + * threshold, then probably the system clock has been updated and we're going to have to + * reset everything and start over. + */ + if (lastOutput_) { + int64_t inputDiff = getOutput(input) - getOutput(lastInput_); + int64_t outputDiff = output - lastOutput_; + error_ = error_ * 0.95 + (outputDiff - inputDiff); + if (std::abs(error_) > errorThreshold_) { + reset(); + inputBase_ = input; + outputBase_ = output; + } + } + lastInput_ = input; + lastOutput_ = output; + + /* + * Never let the new output value be more than maxJitter_ away from what we would have expected. + * This is just to reduce the effect of sudden large delays in the measured output. + */ + uint64_t expectedOutput = getOutput(input); + output = std::clamp(output, expectedOutput - maxJitter_, expectedOutput + maxJitter_); + + /* + * We use x, y, x^2 and x*y sums to calculate the best fit line. Here we update them by + * pretending we have count_ samples at the previous fit, and now one new one. Gradually + * the effect of the older values gets lost. This is a very simple way of updating the + * fit (there are much more complicated ones!), but it works well enough. Using averages + * instead of sums makes the relative effect of old values and the new sample clearer. + */ + double x = input - inputBase_; + double y = output - outputBase_ - x; + unsigned int count1 = count_ + 1; + xAve_ = (count_ * xAve_ + x) / count1; + yAve_ = (count_ * yAve_ + y) / count1; + x2Ave_ = (count_ * x2Ave_ + x * x) / count1; + xyAve_ = (count_ * xyAve_ + x * y) / count1; + + /* Don't update slope and offset until we've seen "enough" sample points. */ + if (count_ > minPts_) { + /* These are the standard equations for least squares linear regression. */ + slope_ = (count1 * count1 * xyAve_ - count1 * xAve_ * count1 * yAve_) / + (count1 * count1 * x2Ave_ - count1 * xAve_ * count1 * xAve_); + offset_ = yAve_ - slope_ * xAve_; + } + + /* Don't increase count_ above numPts_, as this controls the long-term amount of the residual fit. */ + if (count1 < numPts_) + count_++; +} + +uint64_t ClockRecovery::getOutput(uint64_t input) +{ + double x = input - inputBase_; + double y = slope_ * x + offset_; + return y + x + outputBase_; +} diff --git a/src/libcamera/meson.build b/src/libcamera/meson.build index 21cae117..f221590c 100644 --- a/src/libcamera/meson.build +++ b/src/libcamera/meson.build @@ -21,6 +21,7 @@ libcamera_internal_sources = files([ 'byte_stream_buffer.cpp', 'camera_controls.cpp', 'camera_lens.cpp', + 'clock_recovery.cpp', 'control_serializer.cpp', 'control_validator.cpp', 'converter.cpp',