[PATCH 2/5] libcamera: Add ClockRecovery class to generate wallclock timestamps
Kieran Bingham
kieran.bingham at ideasonboard.com
Fri Dec 6 17:43:43 CET 2024
Quoting David Plowman (2024-12-06 16:29:02)
> Hi Kieran
>
> Thanks for the review!
>
> On Fri, 6 Dec 2024 at 15:50, Kieran Bingham
> <kieran.bingham at ideasonboard.com> wrote:
> >
> > Quoting David Plowman (2024-12-06 14:27:39)
> > > 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>
> > > ---
> > > include/libcamera/internal/clock_recovery.h | 72 +++++++
> > > include/libcamera/internal/meson.build | 1 +
> > > src/libcamera/clock_recovery.cpp | 207 ++++++++++++++++++++
> > > src/libcamera/meson.build | 1 +
> > > 4 files changed, 281 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..c874574e
> > > --- /dev/null
> > > +++ b/include/libcamera/internal/clock_recovery.h
> > > @@ -0,0 +1,72 @@
> > > +/* SPDX-License-Identifier: BSD-2-Clause */
> > > +/*
> > > + * Copyright (C) 2024, Raspberry Pi Ltd
> > > + *
> > > + * Camera recovery algorithm
> >
> > Recovering cameras ? ;-)
>
> Hehe. I'll fix that!!
>
> >
> > > + */
> > > +#pragma once
> > > +
> > > +#include <stdint.h>
> > > +
> > > +namespace libcamera {
> > > +
> > > +class ClockRecovery
> > > +{
> > > +public:
> > > + ClockRecovery();
> > > +
> > > + /* Set configuration parameters. */
> > > + void configure(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, taking the input from the Linux
> > > + * CLOCK_BOOTTIME and the output from the CLOCK_REALTIME.
> > > + */
> > > + void addSample();
> > > + /*
> > > + * Add a new input clock / output clock sample, specifying the clock times exactly. Use this
> > > + * when you want to use clocks other than the ones described above.
> > > + */
> > > + 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 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..966599ee
> > > --- /dev/null
> > > +++ b/src/libcamera/clock_recovery.cpp
> > > @@ -0,0 +1,207 @@
> > > +/* SPDX-License-Identifier: BSD-2-Clause */
> > > +/*
> > > + * 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] numPts The approximate duration for which the state of the model
> > > + * is persistent, measured in samples
> > > + * \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] minPts 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 numPts, unsigned int maxJitter, unsigned int minPts,
> > > + unsigned int errorThreshold)
> > > +{
> > > + LOG(ClockRec, Debug)
> > > + << "configure " << numPts << " " << maxJitter << " " << minPts << " " << errorThreshold;
> > > +
> > > + numPts_ = numPts;
> > > + maxJitter_ = maxJitter;
> > > + minPts_ = minPts;
> > > + 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;
> > > + slope_ = 0.0;
> >
> > Should slope be initialised to 1.0 or anything 'normalised' for any
> > startup conditions?
>
> Actually, 0 is the value that ensures "output = input" (allowing for
> the different starting offset).
> Maybe worth adding a comment to that end?
aha - no - that's probably fine - it's just me misunderstanding. For
some reason I thought '1' would be the more expected slope for
undefined/uninitialised, but I see '0' just removes the slope which is
(until samples are ready...) undefined ;-)
>
> >
> > > + offset_ = 0.0;
> > > + error_ = 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 bootTime;
> > > + struct timespec wallTime;
> > > +
> > > + /* Get boot and wall clocks in microseconds. */
> > > + clock_gettime(CLOCK_BOOTTIME, &bootTime);
> > > + clock_gettime(CLOCK_REALTIME, &wallTime);
> > > + uint64_t boot = bootTime.tv_sec * 1000000ULL + bootTime.tv_nsec / 1000;
> > > + 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
> > > + *
> > > + * This function should be used for corresponding clocks other than the Linux
> > > + * BOOTTIME and REALTIME clocks.
> >
> > I like that this object has both an 'easy addSample()' and the paired
> > version.
> >
> > I think having the addSample() can define that it pairs BOOTTIME and
> > REALTIME clocks, but this addSample(uint64_t input, uint64_t output)
> > itself isn't actually tied to those clocks.
> >
> > If this ClockRecovery object is used elsewhere, the 'only' reason I
> > could imagine this function being used is if it were to use a clock
> > specifically different to the defaults - so I'd probably drop the text
> > here, and keep the specification of BOOTTIME/REALTIME to the addSample()
> > - or specific to the usage in V4L2VideoDevice.
> >
> > > +void ClockRecovery::addSample(uint64_t input, uint64_t output)
> >
> > As an example that I'm not sure if it's crazy or not yet - I could
> > imagine setting up a ClockRecovery object that maps sequence number and
> > timestamp so that you could determine the frame sequence number (output)
> > from timestamp (input) which would give you quantised sequence numbers
> > determined from the timestamp - so you could spot frame drops if a CSI
> > receiver always supplies monotonic sequence numbers. (which I think the
> > i.MX8MP does... ?)
> >
>
> Interesting thought, though I think I'd prefer to keep the basic
> ClockRecovery restricted to, well, clocks. If someone wants to spot
> frame drops... obviously they're welcome to do that, but maybe that
> gets implemented on top? Remember of course that frame times may be
> variable.
Oh absolutly - I'm not expecting anything here - I'm just speculating on
what crazy things we could match between input/output to get the linear
regression on ;-)
And that's why I think "ClockRecovery::addSample(uint64_t input, uint64_t
output)" as a function with two parameters isn't itself tied to a
specific clock. (but the usage in V4L2VideoDevice, and addSample() is).
>
> >
> >
> >
> > > +{
> > > + 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.
> > > + */
> >
> > Throughout the series, there's lots of text that could easily be wrapped
> > to 80 chars. It shows up a lot in my email because I have an 92 char
> > terminal for my email which shows lots of lines wrapping, but it's not a
> > hard requirement.
>
> I guess I'm not clear how strict we are about line lengths... mostly I
> don't worry if the style checker doesn't complain. But I'm happy to do
> a bit of reflowing if that helps, though some slightly more "official"
> guidance on when to do that would be helpful.
I think we normally aim for blocks of 80 - but the exceptions are up to
120 if it helps/makes sense in that instance.
It only shows up in this series, as I think everything is targetting
~100/120ish instead.
>
> >
> >
> > > + 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. */
> > > + if (count_ > minPts_) {
> >
> > What's the output from getOuput before this happens ? Anything
> > problematic? Or just things that can be ignored for the first few
> > frames?
>
> No, you just get "output = input" allowing for the different starting
> offsets. Again, maybe worth a comment?
Hrm, ok so maybe it might be useful to state 'somewhere' that the system
will produce output values matching the input value until sufficient
sampling points have been collected.
And following that back, I see minPts_ is configurable (which is a good
thing :D), so it's up to the use case to determine what the minimum
requirements are.
Seems pretty good to me.
>
> Thanks!
> David
>
> >
> > > + /* 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_++;
> > > +}
> > > +
> > > +/**
> > > + * \brief Calculate the output clock value according to the model from an 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