[RFC PATCH 2/3] libcamera: clock: Add ClockRecovery class to help generate wallclock timestamps
Kieran Bingham
kieran.bingham at ideasonboard.com
Thu Dec 5 12:39:29 CET 2024
Hi David,
Thanks for discussing this in real-time. I'm looking forward to merging
these.
Quoting David Plowman (2024-11-26 12:17:05)
> 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. */
// */ needs fixing.
> + 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
Small nit here
} /* namespace libcamera */
Some line wrapping needed throughout to aim for 80 chars could help too,
- but that's all I've got - so with that:
Reviewed-by: Kieran Bingham <kieran.bingham at ideasonboard.com>
> 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
>
More information about the libcamera-devel
mailing list