From patchwork Fri Oct 4 11:55:57 2024 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: David Plowman X-Patchwork-Id: 21516 Return-Path: X-Original-To: parsemail@patchwork.libcamera.org Delivered-To: parsemail@patchwork.libcamera.org Received: from lancelot.ideasonboard.com (lancelot.ideasonboard.com [92.243.16.209]) by patchwork.libcamera.org (Postfix) with ESMTPS id EF1ACBD80A for ; Fri, 4 Oct 2024 11:56:21 +0000 (UTC) Received: from lancelot.ideasonboard.com (localhost [IPv6:::1]) by lancelot.ideasonboard.com (Postfix) with ESMTP id 272206352E; Fri, 4 Oct 2024 13:56:21 +0200 (CEST) Authentication-Results: lancelot.ideasonboard.com; dkim=pass (2048-bit key; unprotected) header.d=raspberrypi.com header.i=@raspberrypi.com header.b="BobiUisv"; dkim-atps=neutral Received: from mail-wr1-x42e.google.com (mail-wr1-x42e.google.com [IPv6:2a00:1450:4864:20::42e]) by lancelot.ideasonboard.com (Postfix) with ESMTPS id 88B8B62C92 for ; Fri, 4 Oct 2024 13:56:06 +0200 (CEST) Received: by mail-wr1-x42e.google.com with SMTP id ffacd0b85a97d-37cc84c12c2so1134770f8f.3 for ; Fri, 04 Oct 2024 04:56:06 -0700 (PDT) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=raspberrypi.com; s=google; t=1728042966; x=1728647766; darn=lists.libcamera.org; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:cc:to:from:from:to:cc:subject:date :message-id:reply-to; bh=obcW8TllxKY4racC770ZVBJlg3ef+KFaFjujkzVUGIA=; b=BobiUisvPdaXruLLn5OdPd1qknjJMoFA+DEqITTSamFgRoygIA+YPidzeYZk1979Au hr9R0kRJGgQU8cGS8LUbXd4xnnFRJhHB/GaWT1movUJ8KENQxPM+KSeB/dcyJs42Mc6N 6J5UQlSWJjZnOsqZuwkyAvRW0Ulk3ta+xgEiNIyg+lz4eolHLwmByb36K4enlq01Nhku 7aDAML+T2SMsuo/ouMczogSB9oWFyFVuPlxyM4j7ffUttvP7viOpvYsmty1nzdRMdlyH Xmw1kO1AhqebKlJTJDBwU0ZllwiIpL/zfkd05bck8S/RFoEuLNJ2lJnMXeKY3vEK3huB Rlrg== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20230601; t=1728042966; x=1728647766; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:cc:to:from:x-gm-message-state:from:to:cc :subject:date:message-id:reply-to; bh=obcW8TllxKY4racC770ZVBJlg3ef+KFaFjujkzVUGIA=; b=A094ZGMPlYwp8CW4bfLfScQs3c6ZvM4W3N6w76tc3ZuAIRmB4GpIR1ScTAd0QlsjLW aODZua+Y7zHbCJGryk8pj/6SjeCNWK1gXc04aOtp4pm1U7VTpJEo1jRyHFSMX4R1L+X8 Tz6TDfCqEhAiqUdMBY7blfS1rN4RMO7cz0Jb+b1BqlPiQaKfrrmldN+H4XoA1Vu4b718 mLWbWtcKjw4IOQr2iuAiHgmlt3uJrMZPp5z6Me1/ZPZ4n2ftwEfSSxVlwLguqjBccrhO 9XtJL6+fKnnQZwwGmUypKGrTvppY/+uTotQKkQFYi2BcIP5vVsvj3ifXR35cXS+S8HL7 OXlQ== X-Gm-Message-State: AOJu0YzVEyE0PKxyHa1artPw4XhFd7sNABVn2HvkT8UGvr9RtDbzLfiF +b1+kK7OGy3lt/iCJG2bi1ZkHAfPbdeeNRzGwZdIyY1qb/uGqYrY41BOOQAbwKUaaX/635mATOV W X-Google-Smtp-Source: AGHT+IFfVtQZ1tC5ApzCGAa1QQ6y8mfMrURmjQUNfW1wEarklQHspQn0wpXJIfvZPkAyzmTHjZ25yA== X-Received: by 2002:adf:ce12:0:b0:371:9121:5643 with SMTP id ffacd0b85a97d-37d0e6f247cmr1557654f8f.12.1728042965359; Fri, 04 Oct 2024 04:56:05 -0700 (PDT) Received: from raspberrypi.pitowers.org ([2a00:1098:3142:1f:daa2:371b:a97:3e3e]) by smtp.gmail.com with ESMTPSA id ffacd0b85a97d-37d081f743esm3107147f8f.21.2024.10.04.04.56.04 (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Fri, 04 Oct 2024 04:56:05 -0700 (PDT) From: David Plowman To: libcamera-devel@lists.libcamera.org Cc: David Plowman , Arsen Mikovic , Naushir Patuck Subject: [PATCH 5/6] ipa: rpi: sync: Add an implementation of the camera sync algorithm Date: Fri, 4 Oct 2024 12:55:57 +0100 Message-Id: <20241004115558.9166-6-david.plowman@raspberrypi.com> X-Mailer: git-send-email 2.39.5 In-Reply-To: <20241004115558.9166-1-david.plowman@raspberrypi.com> References: <20241004115558.9166-1-david.plowman@raspberrypi.com> MIME-Version: 1.0 X-BeenThere: libcamera-devel@lists.libcamera.org X-Mailman-Version: 2.1.29 Precedence: list List-Id: List-Unsubscribe: , List-Archive: List-Post: List-Help: List-Subscribe: , Errors-To: libcamera-devel-bounces@lists.libcamera.org Sender: "libcamera-devel" In this implementation, the server sends data packets out onto the network every 30 frames or so. Clients listening for this packet will send frame length deltas back to the pipeline handler to match the synchronisation of the server. We can use wallclock timestamps so that the process will actually work across networked Pis, but it does really on those wallclocks being properly synchronised. We de-jitter our wallclock measurements (as they're made in userspace) to match the more accurate kernel SensorTimestamp value. When the server's advertised "ready time" is reached, both client and server will signal this through metadata back to their respective controlling applications. Signed-off-by: David Plowman Signed-off-by: Arsen Mikovic Signed-off-by: Naushir Patuck --- src/ipa/rpi/controller/meson.build | 2 + src/ipa/rpi/controller/rpi/clock_recovery.cpp | 87 ++++ src/ipa/rpi/controller/rpi/clock_recovery.h | 55 +++ src/ipa/rpi/controller/rpi/sync.cpp | 384 ++++++++++++++++++ src/ipa/rpi/controller/rpi/sync.h | 71 ++++ 5 files changed, 599 insertions(+) create mode 100644 src/ipa/rpi/controller/rpi/clock_recovery.cpp create mode 100644 src/ipa/rpi/controller/rpi/clock_recovery.h create mode 100644 src/ipa/rpi/controller/rpi/sync.cpp create mode 100644 src/ipa/rpi/controller/rpi/sync.h diff --git a/src/ipa/rpi/controller/meson.build b/src/ipa/rpi/controller/meson.build index 74b74888..8df38a0c 100644 --- a/src/ipa/rpi/controller/meson.build +++ b/src/ipa/rpi/controller/meson.build @@ -13,6 +13,7 @@ rpi_ipa_controller_sources = files([ 'rpi/black_level.cpp', 'rpi/cac.cpp', 'rpi/ccm.cpp', + 'rpi/clock_recovery.cpp', 'rpi/contrast.cpp', 'rpi/denoise.cpp', 'rpi/dpc.cpp', @@ -23,6 +24,7 @@ rpi_ipa_controller_sources = files([ 'rpi/saturation.cpp', 'rpi/sdn.cpp', 'rpi/sharpen.cpp', + 'rpi/sync.cpp', 'rpi/tonemap.cpp', ]) diff --git a/src/ipa/rpi/controller/rpi/clock_recovery.cpp b/src/ipa/rpi/controller/rpi/clock_recovery.cpp new file mode 100644 index 00000000..1ccbf9e9 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/clock_recovery.cpp @@ -0,0 +1,87 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2024, Raspberry Pi Ltd + * + * Camera sync control algorithm + */ +#include "clock_recovery.h" + +#include + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiClockRec) + +ClockRecovery::ClockRecovery() +{ + initialise(); +} + +void ClockRecovery::initialise(unsigned int numPts, unsigned int maxJitter, unsigned int minPts) +{ + numPts_ = numPts; + maxJitter_ = maxJitter; + minPts_ = minPts; + reset(); +} + +void ClockRecovery::reset() +{ + xAve_ = 0; + yAve_ = 0; + x2Ave_ = 0; + xyAve_ = 0; + count_ = 0; + slope_ = 0.0; + offset_ = 0.0; +} + +void ClockRecovery::addSample(uint64_t input, uint64_t output) +{ + if (count_ == 0) { + inputBase_ = input; + outputBase_ = output; + } + + /* + * Never let the new output value be more than maxJitter_ away from what we would have expected. + * This is just to filter out any rare but really crazy values. + */ + uint64_t expectedOutput = getOutput(input); + output = std::clamp(output, expectedOutput - maxJitter_, expectedOutput + maxJitter_); + double x = input - inputBase_; + double y = output - outputBase_ - x; + + /* + * 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. + */ + 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 regressions. */ + 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/ipa/rpi/controller/rpi/clock_recovery.h b/src/ipa/rpi/controller/rpi/clock_recovery.h new file mode 100644 index 00000000..dd05dd97 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/clock_recovery.h @@ -0,0 +1,55 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2024, Raspberry Pi Ltd + * + * Camera sync control algorithm + */ +#pragma once + +#include + +namespace RPiController { + +class ClockRecovery +{ +public: + ClockRecovery(); + + /* Initialise with configuration parameters and restart the fitting process. */ + void initialise(unsigned int numPts = 100, unsigned int maxJitter = 100000, unsigned int minPts = 10); + /* Erase all history and restart the fitting process. */ + void reset(); + + // Add a new input clock / output clock sample. */ + 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 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 */ + + /* + * 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 */ +}; + +} //namespace RPiController diff --git a/src/ipa/rpi/controller/rpi/sync.cpp b/src/ipa/rpi/controller/rpi/sync.cpp new file mode 100644 index 00000000..9e76d879 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/sync.cpp @@ -0,0 +1,384 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2024, Raspberry Pi Ltd + * + * sync.cpp - sync algorithm + */ +#include "sync.h" + +#include +#include +#include +#include +#include + +#include + +#include + +#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 = 1000; +constexpr unsigned int kDefaultMinAdjustment = 50; +constexpr unsigned int kDefaultFitNumPts = 100; +constexpr unsigned int kDefaultFitMaxJitter = 100000; +constexpr unsigned int kDefaultFitMinPts = 10; + +/* Returns IP address of the device we are on. */ +static std::string local_address_IP() +{ + const char *google_dns_server = "8.8.8.8"; + int dns_port = 53; + + struct sockaddr_in serv; + int sock = socket(AF_INET, SOCK_DGRAM, 0); + + if (sock < 0) + LOG(RPiSync, Error) << "Socket error"; + + memset(&serv, 0, sizeof(serv)); + serv.sin_family = AF_INET; + serv.sin_addr.s_addr = inet_addr(google_dns_server); + serv.sin_port = htons(dns_port); + + int err = connect(sock, (const struct sockaddr *)&serv, sizeof(serv)); + if (err < 0) + LOG(RPiSync, Error) << "Socket connect error"; + + struct sockaddr_in name; + socklen_t namelen = sizeof(name); + err = getsockname(sock, (struct sockaddr *)&name, &namelen); + + char buffer[80]; + (void)inet_ntop(AF_INET, &name.sin_addr, buffer, 80); + close(sock); + return buffer; +} + +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(kDefaultGroup); + port_ = params["port"].get(kDefaultPort); + /* Send a sync message every this many frames. */ + syncPeriod_ = params["sync_period"].get(kDefaultSyncPeriod); + /* Application will be told we're ready after this many frames. */ + readyFrame_ = params["ready_frame"].get(kDefaultReadyFrame); + /* Don't change client frame length unless the change exceeds this amount (microseconds). */ + minAdjustment_ = params["min_adjustment"].get(kDefaultMinAdjustment); + + /* Parameters controlling the clock fitting. */ + uint32_t fitNumPts = params["fit_num_pts"].get(kDefaultFitNumPts); + uint32_t fitMaxJitter = params["fit_max_jitter"].get(kDefaultFitMaxJitter); + uint32_t fitMinPts = params["fit_min_pts"].get(kDefaultFitMinPts); + systemToWallClock_.initialise(fitNumPts, fitMaxJitter, fitMinPts); + + 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) +{ + syncReady_ = false; + frameCount_ = 0; + firstFrame_ = true; + lag_ = 0; + serverFrameCountPeriod_ = 0; + clientServerReadyTime_ = 0; + clientSeenPacket_ = false; +} + +/* + * 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 lagKnown = true; + + imageMetadata->get("sync.params", local); + + if (!frameDuration_) { + LOG(RPiSync, Error) << "Sync frame duration not set!"; + return; + } + + if (mode_ == Mode::Off) + return; + + if (socket_ < 0) + initialiseSocket(); + + /* The local wallclock for the very first frame can be a bit off, so ignore it. */ + if (firstFrame_) { + firstFrame_ = false; + + /* + * 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); + } + + return; + } + + /* + * It might be possible for a frame not to have a valid wallclock, in which case don't let it + * get into the clock recovery as it would totally throw it off. + */ + if (local.wallClock == 0) { + LOG(RPiSync, Debug) << "Zero-valued wallclock - ignoring"; + return; + } + + /* Derive a de-jittered version of wall clock. sensorTimestamp needs converting from ns to us. */ + uint64_t systemFrameTimestamp = local.sensorTimestamp / 1000; + systemToWallClock_.addSample(systemFrameTimestamp, local.wallClock); + uint64_t wallClockFrameTimestamp = systemToWallClock_.getOutput(systemFrameTimestamp); + + /* + * 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(); + + /* 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(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 = (systemFrameTimestamp - lastSystemFrameTimestamp_) / (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 systemReadyTime = systemFrameTimestamp + (int64_t)framesRemaining * frameDurationEstimated_; + uint64_t wallClockReadyTime = systemToWallClock_.getOutput(systemReadyTime); + + if (serverFrameCountPeriod_ >= syncPeriod_) { + serverFrameCountPeriod_ = 0; + + payload.frameDuration = frameDurationEstimated_ + .5; /* round to nearest */ + payload.systemFrameTimestamp = systemFrameTimestamp; + payload.wallClockFrameTimestamp = wallClockFrameTimestamp; + payload.systemReadyTime = systemReadyTime; + payload.wallClockReadyTime = wallClockReadyTime; + + LOG(RPiSync, Debug) << "Send packet (frameNumber " << frameCount_ << "):"; + LOG(RPiSync, Debug) << " frameDuration " << payload.frameDuration; + LOG(RPiSync, Debug) << " systemFrameTimestamp " << systemFrameTimestamp + << " (" << systemFrameTimestamp - lastSystemFrameTimestamp_ << ")"; + LOG(RPiSync, Debug) << " wallClockFrameTimestamp " << wallClockFrameTimestamp + << " (" << wallClockFrameTimestamp - lastWallClockFrameTimestamp_ << ")"; + LOG(RPiSync, Debug) << " systemReadyTime " << systemReadyTime; + 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); + } + + lag_ = (int64_t)wallClockFrameTimestamp - (int64_t)wallClockReadyTime; + if (!syncReady_ && wallClockFrameTimestamp + frameDurationEstimated_ / 2 > wallClockReadyTime) { + syncReady_ = true; + LOG(RPiSync, Info) << "*** Sync achieved! Lag " << lag_; + } + + 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; + + if (!IPCheck_) { + IPCheck_ = true; + char srcIP[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &(addr_.sin_addr), srcIP, INET_ADDRSTRLEN); + clientSamePi_ = (local_address_IP() == srcIP); + LOG(RPiSync, Debug) << "Server is " << (clientSamePi_ ? "same" : "different"); + } + + frameDurationEstimated_ = payload.frameDuration; + if (clientSamePi_) { + serverFrameTimestamp = payload.systemFrameTimestamp; + clientServerReadyTime_ = payload.systemReadyTime; + } else { + serverFrameTimestamp = payload.wallClockFrameTimestamp; + clientServerReadyTime_ = payload.wallClockReadyTime; + } + } + + if (packetReceived) { + uint64_t clientFrameTimestamp = clientSamePi_ ? systemFrameTimestamp : 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 " << clientServerReadyTime_; + LOG(RPiSync, Debug) << " clientFrameTimestamp " << clientFrameTimestamp; + LOG(RPiSync, Debug) << " clientFrameTimestampNearest " << clientFrameTimestampNearest + << " (" << framePeriodErrors << ")"; + LOG(RPiSync, Debug) << " systemFrameTimestamp " << systemFrameTimestamp + << " (" << systemFrameTimestamp - lastSystemFrameTimestamp_ << ")"; + LOG(RPiSync, Debug) << " correction " << correction; + + status.frameDurationOffset = correction * 1us; + } + + uint64_t clientFrameTimestamp = clientSamePi_ ? systemFrameTimestamp : wallClockFrameTimestamp; + lag_ = (int64_t)clientFrameTimestamp - (int64_t)clientServerReadyTime_; + lagKnown = clientSeenPacket_; /* client must receive a packet before the lag is correct */ + if (clientSeenPacket_ && !syncReady_ && clientFrameTimestamp + frameDurationEstimated_ / 2 > clientServerReadyTime_) { + syncReady_ = true; + LOG(RPiSync, Info) << "*** Sync achieved! Lag " << lag_; + } + } + + lastSystemFrameTimestamp_ = systemFrameTimestamp; + lastWallClockFrameTimestamp_ = wallClockFrameTimestamp; + + status.ready = syncReady_; + status.lag = lag_; + status.lagKnown = lagKnown; + imageMetadata->set("sync.status", status); + frameCount_++; +} + +void Sync::setFrameDuration(libcamera::utils::Duration frameDuration) +{ + frameDuration_ = frameDuration; +}; + +/* Register algorithm with the system. */ +static Algorithm *create(Controller *controller) +{ + return (Algorithm *)new Sync(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/rpi/controller/rpi/sync.h b/src/ipa/rpi/controller/rpi/sync.h new file mode 100644 index 00000000..15427adb --- /dev/null +++ b/src/ipa/rpi/controller/rpi/sync.h @@ -0,0 +1,71 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2024, Raspberry Pi Ltd + * + * sync.h - sync algorithm + */ +#pragma once + +#include + +#include "../sync_algorithm.h" +#include "clock_recovery.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 { mode_ = mode; } + void initialiseSocket(); + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override; + void process(StatisticsPtr &stats, Metadata *imageMetadata) override; + void setFrameDuration(libcamera::utils::Duration frameDuration) override; + +private: + 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 lag_ = 0; + bool IPCheck_ = false; + bool firstFrame_ = true; + + double frameDurationEstimated_ = 0; /* estimate the true frame duration of the sensor */ + ClockRecovery systemToWallClock_; /* for deriving a de-jittered wall clock time */ + uint64_t lastSystemFrameTimestamp_; /* system timestamp of previous frame */ + 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 */ + bool clientSamePi_ = false; /* whether server running on the same Pi as client */ + uint64_t clientServerReadyTime_ = 0; /* the client's latest value for when the server will be "ready" */ +}; + +} /* namespace RPiController */