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

22098 diff mbox series

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

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