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

22098 diff mbox series

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',