@@ -23,6 +23,7 @@ rpi_ipa_controller_sources = files([
'rpi/saturation.cpp',
'rpi/sdn.cpp',
'rpi/sharpen.cpp',
+ 'rpi/sync.cpp',
'rpi/tonemap.cpp',
])
new file mode 100644
@@ -0,0 +1,330 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2024, Raspberry Pi Ltd
+ *
+ * sync.cpp - sync algorithm
+ */
+#include "sync.h"
+
+#include <chrono>
+#include <ctype.h>
+#include <fcntl.h>
+#include <strings.h>
+#include <unistd.h>
+
+#include <libcamera/base/log.h>
+
+#include <arpa/inet.h>
+
+#include "sync_status.h"
+
+using namespace std;
+using namespace std::chrono_literals;
+using namespace RPiController;
+using namespace libcamera;
+
+LOG_DEFINE_CATEGORY(RPiSync)
+
+#define NAME "rpi.sync"
+
+const char *kDefaultGroup = "239.255.255.250";
+constexpr unsigned int kDefaultPort = 10000;
+constexpr unsigned int kDefaultSyncPeriod = 30;
+constexpr unsigned int kDefaultReadyFrame = 100;
+constexpr unsigned int kDefaultMinAdjustment = 50;
+
+Sync::Sync(Controller *controller)
+ : SyncAlgorithm(controller), mode_(Mode::Off), socket_(-1), frameDuration_(0s), frameCount_(0)
+{
+}
+
+Sync::~Sync()
+{
+ if (socket_ >= 0)
+ close(socket_);
+}
+
+char const *Sync::name() const
+{
+ return NAME;
+}
+
+/* This reads from json file and intitiaises server and client */
+int Sync::read(const libcamera::YamlObject ¶ms)
+{
+ /* Socket on which to communicate. */
+ group_ = params["group"].get<std::string>(kDefaultGroup);
+ port_ = params["port"].get<uint16_t>(kDefaultPort);
+ /* Send a sync message every this many frames. */
+ syncPeriod_ = params["sync_period"].get<uint32_t>(kDefaultSyncPeriod);
+ /* Application will be told we're ready after this many frames. */
+ readyFrame_ = params["ready_frame"].get<uint32_t>(kDefaultReadyFrame);
+ /* Don't change client frame length unless the change exceeds this amount (microseconds). */
+ minAdjustment_ = params["min_adjustment"].get<uint32_t>(kDefaultMinAdjustment);
+
+ return 0;
+}
+
+void Sync::initialiseSocket()
+{
+ socket_ = socket(AF_INET, SOCK_DGRAM, 0);
+ if (socket_ < 0) {
+ LOG(RPiSync, Error) << "Unable to create socket";
+ return;
+ }
+
+ memset(&addr_, 0, sizeof(addr_));
+ addr_.sin_family = AF_INET;
+ addr_.sin_addr.s_addr = mode_ == Mode::Client ? htonl(INADDR_ANY) : inet_addr(group_.c_str());
+ addr_.sin_port = htons(port_);
+
+ if (mode_ == Mode::Client) {
+ /* Set to non-blocking. */
+ int flags = fcntl(socket_, F_GETFL, 0);
+ fcntl(socket_, F_SETFL, flags | O_NONBLOCK);
+
+ unsigned int en = 1;
+ if (setsockopt(socket_, SOL_SOCKET, SO_REUSEADDR, &en, sizeof(en)) < 0) {
+ LOG(RPiSync, Error) << "Unable to set socket options";
+ goto err;
+ }
+
+ struct ip_mreq mreq {
+ };
+ mreq.imr_multiaddr.s_addr = inet_addr(group_.c_str());
+ mreq.imr_interface.s_addr = htonl(INADDR_ANY);
+ if (setsockopt(socket_, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0) {
+ LOG(RPiSync, Error) << "Unable to set socket options";
+ goto err;
+ }
+
+ if (bind(socket_, (struct sockaddr *)&addr_, sizeof(addr_)) < 0) {
+ LOG(RPiSync, Error) << "Unable to bind client socket";
+ goto err;
+ }
+ }
+
+ return;
+
+err:
+ close(socket_);
+ socket_ = -1;
+}
+
+void Sync::switchMode([[maybe_unused]] CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata)
+{
+ /*
+ * A mode switch means the camera has stopped, so synchronisation will be lost.
+ * Reset all the internal state so that we start over.
+ */
+ reset();
+}
+
+/*
+ * Camera sync algorithm.
+ * Server - there is a single server that sends framerate timing information over the network to any
+ * clients that are listening. It also signals when it will send a "everything is synchronised, now go"
+ * message back to the algorithm.
+ * Client - there may be many clients, either on the same Pi or different ones. They match their
+ * framerates to the server, and indicate when to "go" at the same instant as the server.
+ */
+void Sync::process([[maybe_unused]] StatisticsPtr &stats, Metadata *imageMetadata)
+{
+ SyncPayload payload;
+ SyncParams local{};
+ SyncStatus status{};
+ bool timerKnown = true;
+
+ if (mode_ == Mode::Off)
+ return;
+
+ if (!frameDuration_) {
+ LOG(RPiSync, Error) << "Sync frame duration not set!";
+ return;
+ }
+
+ if (socket_ < 0) {
+ initialiseSocket();
+
+ if (socket_ < 0)
+ return;
+
+ /*
+ * For the client, flush anything in the socket. It might be stale from a previous sync run,
+ * or we might get another packet in a frame to two before the adjustment caused by this (old)
+ * packet, although correct, had taken effect. So this keeps things simpler.
+ */
+ if (mode_ == Mode::Client) {
+ socklen_t addrlen = sizeof(addr_);
+ int ret = 0;
+ while (ret >= 0)
+ ret = recvfrom(socket_, &payload, sizeof(payload), 0, (struct sockaddr *)&addr_, &addrlen);
+ }
+ }
+
+ imageMetadata->get("sync.params", local);
+
+ /* The wallclock has already been de-jittered for us. */
+ uint64_t wallClockFrameTimestamp = local.wallClock;
+
+ /*
+ * This is the headline frame duration in microseconds as programmed into the sensor. Strictly,
+ * the sensor might not quite match the system clock, but this shouldn't matter for the calculations
+ * we'll do with it, unless it's a very very long way out!
+ */
+ uint32_t frameDuration = frameDuration_.get<std::micro>();
+
+ /* Timestamps tell us if we've dropped any frames, but we still want to count them. */
+ int droppedFrames = 0;
+ if (frameCount_) {
+ /*
+ * Round down here, because frameCount_ gets incremented at the end of the function. Also
+ * ensure droppedFrames can't go negative. It shouldn't, but things would go badly wrong
+ * if it did.
+ */
+ wallClockFrameTimestamp = std::max<uint64_t>(wallClockFrameTimestamp, lastWallClockFrameTimestamp_ + frameDuration / 2);
+ droppedFrames = (wallClockFrameTimestamp - lastWallClockFrameTimestamp_ - frameDuration / 2) / frameDuration;
+ frameCount_ += droppedFrames;
+ }
+
+ if (mode_ == Mode::Server) {
+ /*
+ * Server sends a packet every syncPeriod_ frames, or as soon after as possible (if any
+ * frames were dropped).
+ */
+ serverFrameCountPeriod_ += droppedFrames;
+
+ /*
+ * The client may want a better idea of the true frame duration. Any error would feed straight
+ * into the correction term because of how it uses it to get the "nearest" frame.
+ */
+ if (frameCount_ == 0)
+ frameDurationEstimated_ = frameDuration;
+ else {
+ double diff = (wallClockFrameTimestamp - lastWallClockFrameTimestamp_) / (1 + droppedFrames);
+ int N = std::min(frameCount_, 99U);
+ frameDurationEstimated_ = frameCount_ == 1 ? diff : (N * frameDurationEstimated_ + diff) / (N + 1);
+ }
+
+ /* Calculate frames remaining, and therefore "time left until ready". */
+ int framesRemaining = readyFrame_ - frameCount_;
+ uint64_t wallClockReadyTime = wallClockFrameTimestamp + (int64_t)framesRemaining * frameDurationEstimated_;
+
+ if (serverFrameCountPeriod_ >= syncPeriod_) {
+ serverFrameCountPeriod_ = 0;
+
+ payload.frameDuration = frameDurationEstimated_ + .5; /* round to nearest */
+ payload.wallClockFrameTimestamp = wallClockFrameTimestamp;
+ payload.wallClockReadyTime = wallClockReadyTime;
+
+ LOG(RPiSync, Debug) << "Send packet (frameNumber " << frameCount_ << "):";
+ LOG(RPiSync, Debug) << " frameDuration " << payload.frameDuration;
+ LOG(RPiSync, Debug) << " wallClockFrameTimestamp " << wallClockFrameTimestamp
+ << " (" << wallClockFrameTimestamp - lastWallClockFrameTimestamp_ << ")";
+ LOG(RPiSync, Debug) << " wallClockReadyTime " << wallClockReadyTime;
+
+ if (sendto(socket_, &payload, sizeof(payload), 0, (const sockaddr *)&addr_, sizeof(addr_)) < 0)
+ LOG(RPiSync, Error) << "Send error! " << strerror(errno);
+ }
+
+ timerValue_ = static_cast<int64_t>(wallClockReadyTime - wallClockFrameTimestamp);
+ if (!syncReady_ && wallClockFrameTimestamp + frameDurationEstimated_ / 2 > wallClockReadyTime) {
+ syncReady_ = true;
+ LOG(RPiSync, Info) << "*** Sync achieved! Difference " << timerValue_ << "us";
+ }
+
+ serverFrameCountPeriod_ += 1;
+
+ } else if (mode_ == Mode::Client) {
+ uint64_t serverFrameTimestamp = 0;
+
+ bool packetReceived = false;
+ while (true) {
+ socklen_t addrlen = sizeof(addr_);
+ int ret = recvfrom(socket_, &payload, sizeof(payload), 0, (struct sockaddr *)&addr_, &addrlen);
+
+ if (ret < 0)
+ break;
+ packetReceived = (ret > 0);
+ clientSeenPacket_ = true;
+
+ frameDurationEstimated_ = payload.frameDuration;
+ serverFrameTimestamp = payload.wallClockFrameTimestamp;
+ serverReadyTime_ = payload.wallClockReadyTime;
+ }
+
+ if (packetReceived) {
+ uint64_t clientFrameTimestamp = wallClockFrameTimestamp;
+ int64_t clientServerDelta = clientFrameTimestamp - serverFrameTimestamp;
+ /* "A few frames ago" may have better matched the server's frame. Calculate when it was. */
+ int framePeriodErrors = (clientServerDelta + frameDurationEstimated_ / 2) / frameDurationEstimated_;
+ int64_t clientFrameTimestampNearest = clientFrameTimestamp - framePeriodErrors * frameDurationEstimated_;
+ /* We must shorten a single client frame by this amount if it exceeds the minimum: */
+ int32_t correction = clientFrameTimestampNearest - serverFrameTimestamp;
+ if (std::abs(correction) < minAdjustment_)
+ correction = 0;
+
+ LOG(RPiSync, Debug) << "Received packet (frameNumber " << frameCount_ << "):";
+ LOG(RPiSync, Debug) << " serverFrameTimestamp " << serverFrameTimestamp;
+ LOG(RPiSync, Debug) << " serverReadyTime " << serverReadyTime_;
+ LOG(RPiSync, Debug) << " clientFrameTimestamp " << clientFrameTimestamp;
+ LOG(RPiSync, Debug) << " clientFrameTimestampNearest " << clientFrameTimestampNearest
+ << " (" << framePeriodErrors << ")";
+ LOG(RPiSync, Debug) << " correction " << correction;
+
+ status.frameDurationOffset = correction * 1us;
+ }
+
+ timerValue_ = static_cast<int64_t>(serverReadyTime_ - wallClockFrameTimestamp);
+ timerKnown = clientSeenPacket_; /* client must receive a packet before the timer value is correct */
+ if (clientSeenPacket_ && !syncReady_ && wallClockFrameTimestamp + frameDurationEstimated_ / 2 > serverReadyTime_) {
+ syncReady_ = true;
+ LOG(RPiSync, Info) << "*** Sync achieved! Difference " << timerValue_ << "us";
+ }
+ }
+
+ lastWallClockFrameTimestamp_ = wallClockFrameTimestamp;
+
+ status.ready = syncReady_;
+ status.timerValue = timerValue_;
+ status.timerKnown = timerKnown;
+ imageMetadata->set("sync.status", status);
+ frameCount_++;
+}
+
+void Sync::reset()
+{
+ /* This resets the state so that the synchronisation procedure will start over. */
+ syncReady_ = false;
+ frameCount_ = 0;
+ timerValue_ = 0;
+ serverFrameCountPeriod_ = 0;
+ serverReadyTime_ = 0;
+ clientSeenPacket_ = false;
+}
+
+void Sync::setMode(Mode mode)
+{
+ mode_ = mode;
+
+ /* Another "sync session" can be started by turning it off and on again. */
+ if (mode == Mode::Off)
+ reset();
+}
+
+void Sync::setFrameDuration(libcamera::utils::Duration frameDuration)
+{
+ frameDuration_ = frameDuration;
+};
+
+void Sync::setReadyFrame(unsigned int frame)
+{
+ readyFrame_ = frame;
+};
+
+/* Register algorithm with the system. */
+static Algorithm *create(Controller *controller)
+{
+ return (Algorithm *)new Sync(controller);
+}
+static RegisterAlgorithm reg(NAME, &create);
new file mode 100644
@@ -0,0 +1,68 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2024, Raspberry Pi Ltd
+ *
+ * sync.h - sync algorithm
+ */
+#pragma once
+
+#include <netinet/ip.h>
+
+#include "../sync_algorithm.h"
+
+namespace RPiController {
+
+struct SyncPayload {
+ /* Frame duration in microseconds. */
+ uint32_t frameDuration;
+ /* Server system (kernel) frame timestamp. */
+ uint64_t systemFrameTimestamp;
+ /* Server wall clock version of the frame timestamp. */
+ uint64_t wallClockFrameTimestamp;
+ /* Server system (kernel) sync time (the time at which frames are marked ready). */
+ uint64_t systemReadyTime;
+ /* Server wall clock version of the sync time. */
+ uint64_t wallClockReadyTime;
+};
+
+class Sync : public SyncAlgorithm
+{
+public:
+ Sync(Controller *controller);
+ ~Sync();
+ char const *name() const override;
+ int read(const libcamera::YamlObject ¶ms) override;
+ void setMode(Mode mode) override;
+ void initialiseSocket();
+ void switchMode(CameraMode const &cameraMode, Metadata *metadata) override;
+ void process(StatisticsPtr &stats, Metadata *imageMetadata) override;
+ void setFrameDuration(libcamera::utils::Duration frameDuration) override;
+ void setReadyFrame(unsigned int frame) override;
+
+private:
+ void reset(); /* reset internal state and start over */
+
+ Mode mode_; /* server or client */
+ std::string group_; /* IP group address for sync messages */
+ uint16_t port_; /* port number for messages */
+ uint32_t syncPeriod_; /* send a sync message every this many frames */
+ uint32_t readyFrame_; /* tell the application we're ready after this many frames */
+ uint32_t minAdjustment_; /* don't adjust the client frame length by less than this */
+
+ struct sockaddr_in addr_;
+ int socket_ = -1;
+ libcamera::utils::Duration frameDuration_;
+ unsigned int frameCount_;
+ bool syncReady_;
+ int64_t timerValue_ = 0; /* time until "ready time" */
+
+ double frameDurationEstimated_ = 0; /* estimate the true frame duration of the sensor */
+ uint64_t lastWallClockFrameTimestamp_; /* wall clock timestamp of previous frame */
+
+ uint32_t serverFrameCountPeriod_ = 0; /* send the next packet when this reaches syncPeriod_ */
+
+ bool clientSeenPacket_ = false; /* whether the client has received a packet yet */
+ uint64_t serverReadyTime_ = 0; /* the client's latest value for when the server will be "ready" */
+};
+
+} /* namespace RPiController */