[RFC PATCH 2/3] libcamera: clock: Add ClockRecovery class to help generate wallclock timestamps

[David Plowman]

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 <david.plowman at raspberrypi.com>
---
 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 <stdint.h>
+
+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 <libcamera/base/log.h>
+
+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',
-- 
2.39.5