[PATCH v3 2/7] libcamera: Add ClockRecovery class to generate wallclock timestamps
Naushir Patuck
naush at raspberrypi.com
Fri Jan 17 10:03:47 CET 2025
Hi David,
On Thu, 9 Jan 2025 at 14:32, David Plowman
<david.plowman at raspberrypi.com> wrote:
>
> 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 <david.plowman at raspberrypi.com>
Reviewed-by: Naushir Patuck <naush at raspberrypi.com>
> ---
> include/libcamera/internal/clock_recovery.h | 68 ++++++
> include/libcamera/internal/meson.build | 1 +
> src/libcamera/clock_recovery.cpp | 230 ++++++++++++++++++++
> src/libcamera/meson.build | 1 +
> 4 files changed, 300 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..43e46b7d
> --- /dev/null
> +++ b/include/libcamera/internal/clock_recovery.h
> @@ -0,0 +1,68 @@
> +/* SPDX-License-Identifier: LGPL-2.1-or-later */
> +/*
> + * Copyright (C) 2024, Raspberry Pi Ltd
> + *
> + * Camera recovery algorithm
> + */
> +#pragma once
> +
> +#include <stdint.h>
> +
> +namespace libcamera {
> +
> +class ClockRecovery
> +{
> +public:
> + ClockRecovery();
> +
> + void configure(unsigned int numSamples = 100, unsigned int maxJitter = 2000,
> + unsigned int minSamples = 10, unsigned int errorThreshold = 50000);
> + void reset();
> +
> + void addSample();
> + void addSample(uint64_t input, uint64_t output);
> +
> + uint64_t getOutput(uint64_t input);
> +
> +private:
> + /* Approximate number of samples over which the model state persists. */
> + unsigned int numSamples_;
> + /* Remove any output jitter larger than this immediately. */
> + unsigned int maxJitter_;
> + /* Number of samples required before we start to use model estimates. */
> + unsigned int minSamples_;
> + /* Threshold above which we assume the wallclock has been reset. */
> + unsigned int errorThreshold_;
> +
> + /* How many samples seen (up to numSamples_). */
> + unsigned int count_;
> + /* This gets subtracted from all input values, just to make the numbers easier. */
> + uint64_t inputBase_;
> + /* As above, for the output. */
> + uint64_t outputBase_;
> + /* The previous input sample. */
> + uint64_t lastInput_;
> + /* The previous output sample. */
> + uint64_t lastOutput_;
> +
> + /* Average x value seen so far. */
> + double xAve_;
> + /* Average y value seen so far */
> + double yAve_;
> + /* Average x^2 value seen so far. */
> + double x2Ave_;
> + /* Average x*y value seen so far. */
> + double xyAve_;
> +
> + /*
> + * The latest estimate of linear parameters to derive the output clock
> + * from the input.
> + */
> + double slope_;
> + double offset_;
> +
> + /* Use this cumulative error to monitor for spontaneous 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..abacf444
> --- /dev/null
> +++ b/src/libcamera/clock_recovery.cpp
> @@ -0,0 +1,230 @@
> +/* SPDX-License-Identifier: LGPL-2.1-or-later */
> +/*
> + * Copyright (C) 2024, Raspberry Pi Ltd
> + *
> + * Clock recovery algorithm
> + */
> +
> +#include "libcamera/internal/clock_recovery.h"
> +
> +#include <time.h>
> +
> +#include <libcamera/base/log.h>
> +
> +/**
> + * \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] numSamples The approximate duration for which the state of the model
> + * is persistent
> + * \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] minSamples 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 numSamples, unsigned int maxJitter,
> + unsigned int minSamples, unsigned int errorThreshold)
> +{
> + LOG(ClockRec, Debug)
> + << "configure " << numSamples << " " << maxJitter << " " << minSamples << " " << errorThreshold;
> +
> + numSamples_ = numSamples;
> + maxJitter_ = maxJitter;
> + minSamples_ = minSamples;
> + 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;
> + error_ = 0.0;
> + /*
> + * Setting slope_ and offset_ to zero initially means that the clocks
> + * advance at exactly the same rate.
> + */
> + slope_ = 0.0;
> + offset_ = 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 bootTime1;
> + struct timespec bootTime2;
> + struct timespec wallTime;
> +
> + /* Get boot and wall clocks in microseconds. */
> + clock_gettime(CLOCK_BOOTTIME, &bootTime1);
> + clock_gettime(CLOCK_REALTIME, &wallTime);
> + clock_gettime(CLOCK_BOOTTIME, &bootTime2);
> + uint64_t boot1 = bootTime1.tv_sec * 1000000ULL + bootTime1.tv_nsec / 1000;
> + uint64_t boot2 = bootTime2.tv_sec * 1000000ULL + bootTime2.tv_nsec / 1000;
> + uint64_t boot = (boot1 + boot2) / 2;
> + 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
> + * \param[in] input The input clock value
> + * \param[in] output The value of the output clock at the same moment, as far
> + * as possible, that the input clock was sampled
> + *
> + * 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<int64_t>(input - inputBase_);
> + double y = static_cast<int64_t>(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. Note that the initial settings for slope_ and offset_
> + * ensures that the wallclock advances at the same rate as the realtime
> + * clock (but with their respective initial offsets).
> + */
> + if (count_ > minSamples_) {
> + /* 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 numSamples_, as this controls the long-term
> + * amount of the residual fit.
> + */
> + if (count1 < numSamples_)
> + count_++;
> +}
> +
> +/**
> + * \brief Calculate the output clock value according to the model from an input
> + * clock value
> + * \param[in] input The input clock value
> + *
> + * \return Output clock value
> + */
> +uint64_t ClockRecovery::getOutput(uint64_t input)
> +{
> + double x = static_cast<int64_t>(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',
> --
> 2.39.5
>
More information about the libcamera-devel
mailing list