[v3,2/7] libcamera: Add ClockRecovery class to generate wallclock timestamps

22501 diff mbox series

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@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

Quoting David Plowman (2025-01-09 14:32:06)
> 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@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

Clock ?

Aside from that, as a standalone component, I would suspect this is a
good candidate for some unit tests in the future - but as we're going
to see this hooked into the pipeline already that's going to provide the
real use case testing for the moment, so I won't insist on unit-tests.

Bonus points to anyone who wants to add something though.


Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com>


> + */
> +#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
>