{"id":22213,"url":"https://patchwork.libcamera.org/api/patches/22213/?format=json","web_url":"https://patchwork.libcamera.org/patch/22213/","project":{"id":1,"url":"https://patchwork.libcamera.org/api/projects/1/?format=json","name":"libcamera","link_name":"libcamera","list_id":"libcamera_core","list_email":"libcamera-devel@lists.libcamera.org","web_url":"","scm_url":"","webscm_url":""},"msgid":"<20241206142742.7931-3-david.plowman@raspberrypi.com>","date":"2024-12-06T14:27:39","name":"[2/5] libcamera: Add ClockRecovery class to generate wallclock timestamps","commit_ref":null,"pull_url":null,"state":"superseded","archived":false,"hash":"37fba9a8cd6c2e83d94383cd20f6e433e5b2a5e3","submitter":{"id":42,"url":"https://patchwork.libcamera.org/api/people/42/?format=json","name":"David Plowman","email":"david.plowman@raspberrypi.com"},"delegate":null,"mbox":"https://patchwork.libcamera.org/patch/22213/mbox/","series":[{"id":4856,"url":"https://patchwork.libcamera.org/api/series/4856/?format=json","web_url":"https://patchwork.libcamera.org/project/libcamera/list/?series=4856","date":"2024-12-06T14:27:37","name":"Implement wallclock timestamps for frames","version":1,"mbox":"https://patchwork.libcamera.org/series/4856/mbox/"}],"comments":"https://patchwork.libcamera.org/api/patches/22213/comments/","check":"pending","checks":"https://patchwork.libcamera.org/api/patches/22213/checks/","tags":{},"headers":{"Return-Path":"","X-Original-To":"parsemail@patchwork.libcamera.org","Delivered-To":"parsemail@patchwork.libcamera.org","Received":["from lancelot.ideasonboard.com (lancelot.ideasonboard.com\n\t[92.243.16.209])\n\tby patchwork.libcamera.org (Postfix) with ESMTPS id AA77FBE173\n\tfor ;\n\tFri, 6 Dec 2024 14:27:56 +0000 (UTC)","from lancelot.ideasonboard.com (localhost [IPv6:::1])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTP id 8987566164;\n\tFri, 6 Dec 2024 15:27:51 +0100 (CET)","from mail-wm1-x331.google.com (mail-wm1-x331.google.com\n\t[IPv6:2a00:1450:4864:20::331])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTPS id 2BC7B66159\n\tfor ;\n\tFri, 6 Dec 2024 15:27:47 +0100 (CET)","by mail-wm1-x331.google.com with SMTP id\n\t5b1f17b1804b1-434a0fd9778so21152865e9.0\n\tfor ;\n\tFri, 06 Dec 2024 06:27:47 -0800 (PST)","from localhost.localdomain ([88.202.252.90])\n\tby smtp.gmail.com with ESMTPSA id\n\tffacd0b85a97d-386220b071dsm4608219f8f.101.2024.12.06.06.27.45\n\t(version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256);\n\tFri, 06 Dec 2024 06:27:46 -0800 (PST)"],"Authentication-Results":"lancelot.ideasonboard.com; dkim=pass (2048-bit key;\n\tunprotected) header.d=raspberrypi.com header.i=@raspberrypi.com\n\theader.b=\"UTq9lmfL\"; dkim-atps=neutral","DKIM-Signature":"v=1; a=rsa-sha256; c=relaxed/relaxed;\n\td=raspberrypi.com; s=google; t=1733495266; x=1734100066;\n\tdarn=lists.libcamera.org; \n\th=content-transfer-encoding:mime-version:references:in-reply-to\n\t:message-id:date:subject:cc:to:from:from:to:cc:subject:date\n\t:message-id:reply-to;\n\tbh=8WZjyOLa+u+y5Ber4pcSec/2+A8y3D3TjO9Bb/8OAx0=;\n\tb=UTq9lmfLPGaSnkgn6w7TafDdJsIvWgJetPfTlURHkdp6Y4l/wBbGIdbVcdQrQI2QAx\n\tqpvBJgkmeiyx8YnCPgS5y2YwjlbXJfXW5QfICujq64wSLvv3x1PrJ2aAZLitQnBuXr15\n\tCqZ3pS8EJ1JFhTfJv+yIKYKk5620C0ubZh70fx1LBy4Ejri3I2vR/ifl/BUUDl4XXdi8\n\tjleLWplzM44RAlyyyKrq+RRo7HElJqCBRyERLArVn5z85BU/3BEbyyEr1nviyZMiPwqo\n\tGkjrzaCKL/bU32QSqi33TZoHH02ucwHmqMX+OhtmnyliNKce4lZUalhYjAjL5lgIlUhE\n\t8ONA==","X-Google-DKIM-Signature":"v=1; a=rsa-sha256; c=relaxed/relaxed;\n\td=1e100.net; s=20230601; t=1733495266; x=1734100066;\n\th=content-transfer-encoding:mime-version:references:in-reply-to\n\t:message-id:date:subject:cc:to:from:x-gm-message-state:from:to:cc\n\t:subject:date:message-id:reply-to;\n\tbh=8WZjyOLa+u+y5Ber4pcSec/2+A8y3D3TjO9Bb/8OAx0=;\n\tb=GhbuNgL2+iCoSMnYDz1D+Qpg9PLtCkDvoepBLKjSKLqiD6el4jGqO4r/XJRNNMOTFW\n\tMhCe806rIph576gjLO8aZpgzaiN4XUQ/bfNVpalwPe4znAge2vcbK6rt7sAyynPsx1DB\n\tt/GTuGndgzWLqoZqBcx5nQxvSSspUMjWCSrY8JiMHDVRPWKWyQiH0NBhTO7EoNTHuRSm\n\t/LZjqLXWI3YNzbx+jTHV+U+vdFjDb0VdcNoXfyskV7eRLPsOQbHKTLfgzaTLAuC7BiPm\n\tVrU3hPxm7qIvnvGO+igfsfxHWxFJLsD6v1nUBzVcY+U5O6FpTB2Yog20QwxDSM1sx9se\n\tRjhQ==","X-Gm-Message-State":"AOJu0Yxdu5OZkd4VLGYGFyhse2Y3eZPtGnqGB2htF3JsLGHLIgZix1dF\n\tORSMnLj88LZ4Oh/y95A8vfuQ88pTPOa42iFKyN4zMaW0MxTkO5Nljx42/SHyG9vkZ+dbbW1AUF9\n\ts","X-Gm-Gg":"ASbGncuqf4oLewveWOxCl7MccmSK94ZZKMhg1lUmJEBYXzpKrBWVW0P8HLFozXNltD1\n\tWKgz3gqO0eTdvLr3rOu0bO6cbOqSSg32xadUThpL9ThJQLewYFybbOmG72v2VcBQjabWiL5z04p\n\tX5DI4aVo/YTRPWrlb+17jqqlr5+H8VCwa+J+RwIzB9hr7d3/5eFfA7xXae+nikoGmcGQbWe+kfy\n\tAs7tOsRWB4k87LS55jZ4Y9EOyhT1bb2+/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\n\t5b1f17b1804b1-434ddeb8c74mr29804615e9.18.1733495266410; \n\tFri, 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\n\ttimestamps","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","Content-Transfer-Encoding":"8bit","X-BeenThere":"libcamera-devel@lists.libcamera.org","X-Mailman-Version":"2.1.29","Precedence":"list","List-Id":"","List-Unsubscribe":",\n\t","List-Archive":"","List-Post":"","List-Help":"","List-Subscribe":",\n\t","Errors-To":"libcamera-devel-bounces@lists.libcamera.org","Sender":"\"libcamera-devel\" "},"content":"The ClockRecovery class takes pairs of timestamps from two different\nclocks, and models the second (\"output\") clock from the first (\"input\")\nclock.\n\nWe can use it, in particular, to get a good wallclock estimate for a\nframe's SensorTimestamp.\n\nSigned-off-by: David Plowman \n---\n include/libcamera/internal/clock_recovery.h | 72 +++++++\n include/libcamera/internal/meson.build | 1 +\n src/libcamera/clock_recovery.cpp | 207 ++++++++++++++++++++\n src/libcamera/meson.build | 1 +\n 4 files changed, 281 insertions(+)\n create mode 100644 include/libcamera/internal/clock_recovery.h\n create mode 100644 src/libcamera/clock_recovery.cpp","diff":"diff --git a/include/libcamera/internal/clock_recovery.h b/include/libcamera/internal/clock_recovery.h\nnew file mode 100644\nindex 00000000..c874574e\n--- /dev/null\n+++ b/include/libcamera/internal/clock_recovery.h\n@@ -0,0 +1,72 @@\n+/* SPDX-License-Identifier: BSD-2-Clause */\n+/*\n+ * Copyright (C) 2024, Raspberry Pi Ltd\n+ *\n+ * Camera recovery algorithm\n+ */\n+#pragma once\n+\n+#include \n+\n+namespace libcamera {\n+\n+class ClockRecovery\n+{\n+public:\n+\tClockRecovery();\n+\n+\t/* Set configuration parameters. */\n+\tvoid configure(unsigned int numPts = 100, unsigned int maxJitter = 2000, unsigned int minPts = 10,\n+\t\t unsigned int errorThreshold = 50000);\n+\t/* Erase all history and restart the fitting process. */\n+\tvoid reset();\n+\n+\t/*\n+\t * Add a new input clock / output clock sample, taking the input from the Linux\n+\t * CLOCK_BOOTTIME and the output from the CLOCK_REALTIME.\n+\t */\n+\tvoid addSample();\n+\t/*\n+\t * Add a new input clock / output clock sample, specifying the clock times exactly. Use this\n+\t * when you want to use clocks other than the ones described above.\n+\t */\n+\tvoid addSample(uint64_t input, uint64_t output);\n+\t/* Calculate the output clock value for this input. */\n+\tuint64_t getOutput(uint64_t input);\n+\n+private:\n+\tunsigned int numPts_; /* how many samples contribute to the history */\n+\tunsigned int maxJitter_; /* smooth out any jitter larger than this immediately */\n+\tunsigned int minPts_; /* number of samples below which we treat clocks as 1:1 */\n+\tunsigned int errorThreshold_; /* reset everything when the error exceeds this */\n+\n+\tunsigned int count_; /* how many samples seen (up to numPts_) */\n+\tuint64_t inputBase_; /* subtract this from all input values, just to make the numbers easier */\n+\tuint64_t outputBase_; /* as above, for the output */\n+\n+\tuint64_t lastInput_; /* the previous input sample */\n+\tuint64_t lastOutput_; /* the previous output sample */\n+\n+\t/*\n+\t * We do a linear regression of y against x, where:\n+\t * x is the value input - inputBase_, and\n+\t * y is the value output - outputBase_ - x.\n+\t * We additionally subtract x from y so that y \"should\" be zero, again making the numnbers easier.\n+\t */\n+\tdouble xAve_; /* average x value seen so far */\n+\tdouble yAve_; /* average y value seen so far */\n+\tdouble x2Ave_; /* average x^2 value seen so far */\n+\tdouble xyAve_; /* average x*y value seen so far */\n+\n+\t/*\n+\t * Once we've seen more than minPts_ samples, we recalculate the slope and offset according\n+\t * to the linear regression normal equations.\n+\t */\n+\tdouble slope_; /* latest slope value */\n+\tdouble offset_; /* latest offset value */\n+\n+\t/* We use this cumulative error to monitor spontaneous system clock updates. */\n+\tdouble error_;\n+};\n+\n+} /* namespace libcamera */\ndiff --git a/include/libcamera/internal/meson.build b/include/libcamera/internal/meson.build\nindex 7d6aa8b7..41500636 100644\n--- a/include/libcamera/internal/meson.build\n+++ b/include/libcamera/internal/meson.build\n@@ -11,6 +11,7 @@ libcamera_internal_headers = files([\n 'camera_manager.h',\n 'camera_sensor.h',\n 'camera_sensor_properties.h',\n+ 'clock_recovery.h',\n 'control_serializer.h',\n 'control_validator.h',\n 'converter.h',\ndiff --git a/src/libcamera/clock_recovery.cpp b/src/libcamera/clock_recovery.cpp\nnew file mode 100644\nindex 00000000..966599ee\n--- /dev/null\n+++ b/src/libcamera/clock_recovery.cpp\n@@ -0,0 +1,207 @@\n+/* SPDX-License-Identifier: BSD-2-Clause */\n+/*\n+ * Copyright (C) 2024, Raspberry Pi Ltd\n+ *\n+ * Clock recovery algorithm\n+ */\n+\n+#include \"libcamera/internal/clock_recovery.h\"\n+\n+#include \n+\n+#include \n+\n+/**\n+ * \\file clock_recovery.h\n+ * \\brief Clock recovery - deriving one clock from another independent clock\n+ */\n+\n+namespace libcamera {\n+\n+LOG_DEFINE_CATEGORY(ClockRec)\n+\n+/**\n+ * \\class ClockRecovery\n+ * \\brief Recover an output clock from an input clock\n+ *\n+ * The ClockRecovery class derives an output clock from an input clock,\n+ * modelling the output clock as being linearly related to the input clock.\n+ * For example, we may use it to derive wall clock timestamps from timestamps\n+ * measured by the internal system clock which counts local time since boot.\n+ *\n+ * When pairs of corresponding input and output timestamps are available,\n+ * they should be submitted to the model with addSample(). The model will\n+ * update, and output clock values for known input clock values can be\n+ * obtained using getOutput().\n+ *\n+ * As a convenience, if the input clock is indeed the time since boot, and the\n+ * output clock represents a real wallclock time, then addSample() can be\n+ * called with no arguments, and a pair of timestamps will be captured at\n+ * that moment.\n+ *\n+ * The configure() function accepts some configuration parameters to control\n+ * the linear fitting process.\n+ */\n+\n+/**\n+ * \\brief Construct a ClockRecovery\n+ */\n+ClockRecovery::ClockRecovery()\n+{\n+\tconfigure();\n+\treset();\n+}\n+\n+/**\n+ * \\brief Set configuration parameters\n+ * \\param[in] numPts The approximate duration for which the state of the model\n+ * is persistent, measured in samples\n+ * \\param[in] maxJitter New output samples are clamped to no more than this\n+ * amount of jitter, to prevent sudden swings from having a large effect\n+ * \\param[in] minPts The fitted clock model is not used to generate outputs\n+ * until this many samples have been received\n+ * \\param[in] errorThreshold If the accumulated differences between input and\n+ * output clocks reaches this amount over a few frames, the model is reset\n+ */\n+void ClockRecovery::configure(unsigned int numPts, unsigned int maxJitter, unsigned int minPts,\n+\t\t\t unsigned int errorThreshold)\n+{\n+\tLOG(ClockRec, Debug)\n+\t\t<< \"configure \" << numPts << \" \" << maxJitter << \" \" << minPts << \" \" << errorThreshold;\n+\n+\tnumPts_ = numPts;\n+\tmaxJitter_ = maxJitter;\n+\tminPts_ = minPts;\n+\terrorThreshold_ = errorThreshold;\n+}\n+\n+/**\n+ * \\brief Reset the clock recovery model and start again from scratch\n+ */\n+void ClockRecovery::reset()\n+{\n+\tLOG(ClockRec, Debug) << \"reset\";\n+\n+\tlastInput_ = 0;\n+\tlastOutput_ = 0;\n+\txAve_ = 0;\n+\tyAve_ = 0;\n+\tx2Ave_ = 0;\n+\txyAve_ = 0;\n+\tcount_ = 0;\n+\tslope_ = 0.0;\n+\toffset_ = 0.0;\n+\terror_ = 0.0;\n+}\n+\n+/**\n+ * \\brief Add a sample point to the clock recovery model, for recovering a wall\n+ * clock value from the internal system time since boot\n+ *\n+ * This is a convenience function to make it easy to derive a wall clock value\n+ * (using the Linux CLOCK_REALTIME) from the time since the system started\n+ * (measured by CLOCK_BOOTTIME).\n+ */\n+void ClockRecovery::addSample()\n+{\n+\tLOG(ClockRec, Debug) << \"addSample\";\n+\n+\tstruct timespec bootTime;\n+\tstruct timespec wallTime;\n+\n+\t/* Get boot and wall clocks in microseconds. */\n+\tclock_gettime(CLOCK_BOOTTIME, &bootTime);\n+\tclock_gettime(CLOCK_REALTIME, &wallTime);\n+\tuint64_t boot = bootTime.tv_sec * 1000000ULL + bootTime.tv_nsec / 1000;\n+\tuint64_t wall = wallTime.tv_sec * 1000000ULL + wallTime.tv_nsec / 1000;\n+\n+\taddSample(boot, wall);\n+}\n+\n+/**\n+ * \\brief Add a sample point to the clock recovery model, specifying the exact\n+ * input and output clock values\n+ *\n+ * This function should be used for corresponding clocks other than the Linux\n+ * BOOTTIME and REALTIME clocks.\n+ */\n+void ClockRecovery::addSample(uint64_t input, uint64_t output)\n+{\n+\tLOG(ClockRec, Debug) << \"addSample \" << input << \" \" << output;\n+\n+\tif (count_ == 0) {\n+\t\tinputBase_ = input;\n+\t\toutputBase_ = output;\n+\t}\n+\n+\t/*\n+\t * We keep an eye on cumulative drift over the last several frames. If this exceeds a\n+\t * threshold, then probably the system clock has been updated and we're going to have to\n+\t * reset everything and start over.\n+\t */\n+\tif (lastOutput_) {\n+\t\tint64_t inputDiff = getOutput(input) - getOutput(lastInput_);\n+\t\tint64_t outputDiff = output - lastOutput_;\n+\t\terror_ = error_ * 0.95 + (outputDiff - inputDiff);\n+\t\tif (std::abs(error_) > errorThreshold_) {\n+\t\t\treset();\n+\t\t\tinputBase_ = input;\n+\t\t\toutputBase_ = output;\n+\t\t}\n+\t}\n+\tlastInput_ = input;\n+\tlastOutput_ = output;\n+\n+\t/*\n+\t * Never let the new output value be more than maxJitter_ away from what we would have expected.\n+\t * This is just to reduce the effect of sudden large delays in the measured output.\n+\t */\n+\tuint64_t expectedOutput = getOutput(input);\n+\toutput = std::clamp(output, expectedOutput - maxJitter_, expectedOutput + maxJitter_);\n+\n+\t/*\n+\t * We use x, y, x^2 and x*y sums to calculate the best fit line. Here we update them by\n+\t * pretending we have count_ samples at the previous fit, and now one new one. Gradually\n+\t * the effect of the older values gets lost. This is a very simple way of updating the\n+\t * fit (there are much more complicated ones!), but it works well enough. Using averages\n+\t * instead of sums makes the relative effect of old values and the new sample clearer.\n+\t */\n+\tdouble x = static_cast(input - inputBase_);\n+\tdouble y = static_cast(output - outputBase_) - x;\n+\tunsigned int count1 = count_ + 1;\n+\txAve_ = (count_ * xAve_ + x) / count1;\n+\tyAve_ = (count_ * yAve_ + y) / count1;\n+\tx2Ave_ = (count_ * x2Ave_ + x * x) / count1;\n+\txyAve_ = (count_ * xyAve_ + x * y) / count1;\n+\n+\t/* Don't update slope and offset until we've seen \"enough\" sample points. */\n+\tif (count_ > minPts_) {\n+\t\t/* These are the standard equations for least squares linear regression. */\n+\t\tslope_ = (count1 * count1 * xyAve_ - count1 * xAve_ * count1 * yAve_) /\n+\t\t\t (count1 * count1 * x2Ave_ - count1 * xAve_ * count1 * xAve_);\n+\t\toffset_ = yAve_ - slope_ * xAve_;\n+\t}\n+\n+\t/* Don't increase count_ above numPts_, as this controls the long-term amount of the residual fit. */\n+\tif (count1 < numPts_)\n+\t\tcount_++;\n+}\n+\n+/**\n+ * \\brief Calculate the output clock value according to the model from an input\n+ * clock value\n+ *\n+ * \\return Output clock value\n+ */\n+uint64_t ClockRecovery::getOutput(uint64_t input)\n+{\n+\tdouble x = static_cast(input - inputBase_);\n+\tdouble y = slope_ * x + offset_;\n+\tuint64_t output = y + x + outputBase_;\n+\n+\tLOG(ClockRec, Debug) << \"getOutput \" << input << \" \" << output;\n+\n+\treturn output;\n+}\n+\n+} /* namespace libcamera */\ndiff --git a/src/libcamera/meson.build b/src/libcamera/meson.build\nindex 57fde8a8..4eaa1c8e 100644\n--- a/src/libcamera/meson.build\n+++ b/src/libcamera/meson.build\n@@ -21,6 +21,7 @@ libcamera_internal_sources = files([\n 'byte_stream_buffer.cpp',\n 'camera_controls.cpp',\n 'camera_lens.cpp',\n+ 'clock_recovery.cpp',\n 'control_serializer.cpp',\n 'control_validator.cpp',\n 'converter.cpp',\n","prefixes":["2/5"]}