From patchwork Fri Dec 6 14:27:39 2024 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: David Plowman X-Patchwork-Id: 22213 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 AA77FBE173 for ; Fri, 6 Dec 2024 14:27:56 +0000 (UTC) Received: from lancelot.ideasonboard.com (localhost [IPv6:::1]) by lancelot.ideasonboard.com (Postfix) with ESMTP id 8987566164; Fri, 6 Dec 2024 15:27:51 +0100 (CET) Authentication-Results: lancelot.ideasonboard.com; dkim=pass (2048-bit key; unprotected) header.d=raspberrypi.com header.i=@raspberrypi.com header.b="UTq9lmfL"; dkim-atps=neutral Received: from mail-wm1-x331.google.com (mail-wm1-x331.google.com [IPv6:2a00:1450:4864:20::331]) by lancelot.ideasonboard.com (Postfix) with ESMTPS id 2BC7B66159 for ; Fri, 6 Dec 2024 15:27:47 +0100 (CET) Received: by mail-wm1-x331.google.com with SMTP id 5b1f17b1804b1-434a0fd9778so21152865e9.0 for ; Fri, 06 Dec 2024 06:27:47 -0800 (PST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=raspberrypi.com; s=google; t=1733495266; x=1734100066; 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=8WZjyOLa+u+y5Ber4pcSec/2+A8y3D3TjO9Bb/8OAx0=; b=UTq9lmfLPGaSnkgn6w7TafDdJsIvWgJetPfTlURHkdp6Y4l/wBbGIdbVcdQrQI2QAx qpvBJgkmeiyx8YnCPgS5y2YwjlbXJfXW5QfICujq64wSLvv3x1PrJ2aAZLitQnBuXr15 CqZ3pS8EJ1JFhTfJv+yIKYKk5620C0ubZh70fx1LBy4Ejri3I2vR/ifl/BUUDl4XXdi8 jleLWplzM44RAlyyyKrq+RRo7HElJqCBRyERLArVn5z85BU/3BEbyyEr1nviyZMiPwqo GkjrzaCKL/bU32QSqi33TZoHH02ucwHmqMX+OhtmnyliNKce4lZUalhYjAjL5lgIlUhE 8ONA== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20230601; t=1733495266; x=1734100066; 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=8WZjyOLa+u+y5Ber4pcSec/2+A8y3D3TjO9Bb/8OAx0=; b=GhbuNgL2+iCoSMnYDz1D+Qpg9PLtCkDvoepBLKjSKLqiD6el4jGqO4r/XJRNNMOTFW MhCe806rIph576gjLO8aZpgzaiN4XUQ/bfNVpalwPe4znAge2vcbK6rt7sAyynPsx1DB t/GTuGndgzWLqoZqBcx5nQxvSSspUMjWCSrY8JiMHDVRPWKWyQiH0NBhTO7EoNTHuRSm /LZjqLXWI3YNzbx+jTHV+U+vdFjDb0VdcNoXfyskV7eRLPsOQbHKTLfgzaTLAuC7BiPm VrU3hPxm7qIvnvGO+igfsfxHWxFJLsD6v1nUBzVcY+U5O6FpTB2Yog20QwxDSM1sx9se RjhQ== X-Gm-Message-State: AOJu0Yxdu5OZkd4VLGYGFyhse2Y3eZPtGnqGB2htF3JsLGHLIgZix1dF ORSMnLj88LZ4Oh/y95A8vfuQ88pTPOa42iFKyN4zMaW0MxTkO5Nljx42/SHyG9vkZ+dbbW1AUF9 s X-Gm-Gg: ASbGncuqf4oLewveWOxCl7MccmSK94ZZKMhg1lUmJEBYXzpKrBWVW0P8HLFozXNltD1 WKgz3gqO0eTdvLr3rOu0bO6cbOqSSg32xadUThpL9ThJQLewYFybbOmG72v2VcBQjabWiL5z04p X5DI4aVo/YTRPWrlb+17jqqlr5+H8VCwa+J+RwIzB9hr7d3/5eFfA7xXae+nikoGmcGQbWe+kfy As7tOsRWB4k87LS55jZ4Y9EOyhT1bb2+/3wlHHAtMgC2pNe+eDMoQvKsh4UtZOBE+Z+96tz7ys= X-Google-Smtp-Source: AGHT+IGeUUymyuM5VgCWS2WsDoPLK/kQA4ky10VF7TVFC70fyerYZ3AP+FMnIuT6yxeoDorN3WD7jA== X-Received: by 2002:a05:600c:1d85:b0:434:a902:4168 with SMTP id 5b1f17b1804b1-434ddeb8c74mr29804615e9.18.1733495266410; Fri, 06 Dec 2024 06:27:46 -0800 (PST) Received: from localhost.localdomain ([88.202.252.90]) by smtp.gmail.com with ESMTPSA id ffacd0b85a97d-386220b071dsm4608219f8f.101.2024.12.06.06.27.45 (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Fri, 06 Dec 2024 06:27:46 -0800 (PST) From: David Plowman To: libcamera-devel@lists.libcamera.org Cc: David Plowman Subject: [PATCH 2/5] libcamera: Add ClockRecovery class to generate wallclock timestamps Date: Fri, 6 Dec 2024 14:27:39 +0000 Message-Id: <20241206142742.7931-3-david.plowman@raspberrypi.com> X-Mailer: git-send-email 2.39.5 In-Reply-To: <20241206142742.7931-1-david.plowman@raspberrypi.com> References: <20241206142742.7931-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" The ClockRecovery class takes pairs of timestamps from two different clocks, and models the second ("output") clock from the first ("input") clock. We can use it, in particular, to get a good wallclock estimate for a frame's SensorTimestamp. Signed-off-by: David Plowman --- include/libcamera/internal/clock_recovery.h | 72 +++++++ include/libcamera/internal/meson.build | 1 + src/libcamera/clock_recovery.cpp | 207 ++++++++++++++++++++ src/libcamera/meson.build | 1 + 4 files changed, 281 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..c874574e --- /dev/null +++ b/include/libcamera/internal/clock_recovery.h @@ -0,0 +1,72 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2024, Raspberry Pi Ltd + * + * Camera recovery algorithm + */ +#pragma once + +#include + +namespace libcamera { + +class ClockRecovery +{ +public: + ClockRecovery(); + + /* Set configuration parameters. */ + void configure(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, taking the input from the Linux + * CLOCK_BOOTTIME and the output from the CLOCK_REALTIME. + */ + void addSample(); + /* + * Add a new input clock / output clock sample, specifying the clock times exactly. Use this + * when you want to use clocks other than the ones described above. + */ + 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 7d6aa8b7..41500636 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..966599ee --- /dev/null +++ b/src/libcamera/clock_recovery.cpp @@ -0,0 +1,207 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2024, Raspberry Pi Ltd + * + * Clock recovery algorithm + */ + +#include "libcamera/internal/clock_recovery.h" + +#include + +#include + +/** + * \file clock_recovery.h + * \brief Clock recovery - deriving one clock from another independent clock + */ + +namespace libcamera { + +LOG_DEFINE_CATEGORY(ClockRec) + +/** + * \class ClockRecovery + * \brief Recover an output clock from an input clock + * + * The ClockRecovery class derives an output clock from an input clock, + * modelling the output clock as being linearly related to the input clock. + * For example, we may use it to derive wall clock timestamps from timestamps + * measured by the internal system clock which counts local time since boot. + * + * When pairs of corresponding input and output timestamps are available, + * they should be submitted to the model with addSample(). The model will + * update, and output clock values for known input clock values can be + * obtained using getOutput(). + * + * As a convenience, if the input clock is indeed the time since boot, and the + * output clock represents a real wallclock time, then addSample() can be + * called with no arguments, and a pair of timestamps will be captured at + * that moment. + * + * The configure() function accepts some configuration parameters to control + * the linear fitting process. + */ + +/** + * \brief Construct a ClockRecovery + */ +ClockRecovery::ClockRecovery() +{ + configure(); + reset(); +} + +/** + * \brief Set configuration parameters + * \param[in] numPts The approximate duration for which the state of the model + * is persistent, measured in samples + * \param[in] maxJitter New output samples are clamped to no more than this + * amount of jitter, to prevent sudden swings from having a large effect + * \param[in] minPts The fitted clock model is not used to generate outputs + * until this many samples have been received + * \param[in] errorThreshold If the accumulated differences between input and + * output clocks reaches this amount over a few frames, the model is reset + */ +void ClockRecovery::configure(unsigned int numPts, unsigned int maxJitter, unsigned int minPts, + unsigned int errorThreshold) +{ + LOG(ClockRec, Debug) + << "configure " << numPts << " " << maxJitter << " " << minPts << " " << errorThreshold; + + numPts_ = numPts; + maxJitter_ = maxJitter; + minPts_ = minPts; + errorThreshold_ = errorThreshold; +} + +/** + * \brief Reset the clock recovery model and start again from scratch + */ +void ClockRecovery::reset() +{ + LOG(ClockRec, Debug) << "reset"; + + lastInput_ = 0; + lastOutput_ = 0; + xAve_ = 0; + yAve_ = 0; + x2Ave_ = 0; + xyAve_ = 0; + count_ = 0; + slope_ = 0.0; + offset_ = 0.0; + error_ = 0.0; +} + +/** + * \brief Add a sample point to the clock recovery model, for recovering a wall + * clock value from the internal system time since boot + * + * This is a convenience function to make it easy to derive a wall clock value + * (using the Linux CLOCK_REALTIME) from the time since the system started + * (measured by CLOCK_BOOTTIME). + */ +void ClockRecovery::addSample() +{ + LOG(ClockRec, Debug) << "addSample"; + + struct timespec bootTime; + struct timespec wallTime; + + /* Get boot and wall clocks in microseconds. */ + clock_gettime(CLOCK_BOOTTIME, &bootTime); + clock_gettime(CLOCK_REALTIME, &wallTime); + uint64_t boot = bootTime.tv_sec * 1000000ULL + bootTime.tv_nsec / 1000; + uint64_t wall = wallTime.tv_sec * 1000000ULL + wallTime.tv_nsec / 1000; + + addSample(boot, wall); +} + +/** + * \brief Add a sample point to the clock recovery model, specifying the exact + * input and output clock values + * + * This function should be used for corresponding clocks other than the Linux + * BOOTTIME and REALTIME clocks. + */ +void ClockRecovery::addSample(uint64_t input, uint64_t output) +{ + LOG(ClockRec, Debug) << "addSample " << input << " " << 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 = static_cast(input - inputBase_); + double y = static_cast(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_++; +} + +/** + * \brief Calculate the output clock value according to the model from an input + * clock value + * + * \return Output clock value + */ +uint64_t ClockRecovery::getOutput(uint64_t input) +{ + double x = static_cast(input - inputBase_); + double y = slope_ * x + offset_; + uint64_t output = y + x + outputBase_; + + LOG(ClockRec, Debug) << "getOutput " << input << " " << output; + + return output; +} + +} /* namespace libcamera */ diff --git a/src/libcamera/meson.build b/src/libcamera/meson.build index 57fde8a8..4eaa1c8e 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',