From patchwork Thu Jan 9 14:32:10 2025 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: David Plowman X-Patchwork-Id: 22504 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 62ABEC32EA for ; Thu, 9 Jan 2025 14:32:32 +0000 (UTC) Received: from lancelot.ideasonboard.com (localhost [IPv6:::1]) by lancelot.ideasonboard.com (Postfix) with ESMTP id DBA4F684CD; Thu, 9 Jan 2025 15:32:31 +0100 (CET) Authentication-Results: lancelot.ideasonboard.com; dkim=pass (2048-bit key; unprotected) header.d=raspberrypi.com header.i=@raspberrypi.com header.b="b3bQWcYd"; dkim-atps=neutral Received: from mail-wm1-x335.google.com (mail-wm1-x335.google.com [IPv6:2a00:1450:4864:20::335]) by lancelot.ideasonboard.com (Postfix) with ESMTPS id 7DB4668506 for ; Thu, 9 Jan 2025 15:32:21 +0100 (CET) Received: by mail-wm1-x335.google.com with SMTP id 5b1f17b1804b1-4361c705434so8093165e9.3 for ; Thu, 09 Jan 2025 06:32:21 -0800 (PST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=raspberrypi.com; s=google; t=1736433141; x=1737037941; 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=sDULZN4NqTcgv4C9d2RSWDOhuj4WFpuSUlo+qOGh2Q0=; b=b3bQWcYdR/DLnLuxED2LIRpEdGT+nsg8ThLZ4EycZubpwKzYLBlBOS2E0jZ9bxKNTa YZ8oHHRJtBPQlG0hMnle9krZkfcEbdvhUUtuq/riNnAiZwBk3uW8KwkmbnEz2+8+Yemc H/RowZRY/rI6NfRJOlSV26preybpl9bwW1ecAh2tupnTlyZbXsmARLe+LXumlqRI+THI tFCGYcxmxPymwtoHcUMVP5PToRWNaBdusu3+aRe1+SlfgNOJJdnqc5LbAGBrTRG8c50M jKweKZlacIn4MKjPOWOkHd5gBK1l/aDMkN5nazBZQ2N7lF8Ha0YYiD4LxKw00NvEkO9+ lP+Q== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20230601; t=1736433141; x=1737037941; 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=sDULZN4NqTcgv4C9d2RSWDOhuj4WFpuSUlo+qOGh2Q0=; b=S1GD3RNwvBdeqhzqUPIv+WzsJ+qUGomMxW6xPosL8/EX6V5ZH6jY64bcMEvJISaRtk aD+AxIA/VwSfp7aueXJ1404PYMLPIPxizeD87p7e9MuKdgZJiA5Zq2t4MZ1/iR4f77kt gNWn07KJ5i8G/xMqEVfQyf7fvlm0MqYe6iGwOTbzI37iVPedCGQllYvXv/eX6sDtX/40 bR3yRlljUlpWi39kSq0skUzDeD4STjzBiUlkiXOdw/D049PnNHu43gXqFX7qPR/hHOcM dtMUB5iaM6YQ0MUa3b+Y7BsjpvYB0g7vmpZwe70KfY59m8KaXJPxAhZtAe0mbMLgKr7d q+ZA== X-Gm-Message-State: AOJu0YzzUBGIIhlmrrP9wMkHZMY2l6JvWUVituA/R1cRZVcVNgqd6jtH mh7pZ1gc8AAh61V/bHmFVhR/ax5QPlHtKg0rUhrE0a3nPyy2PX7IBPO1Lr9DrsUmDkHucTeMefV 8 X-Gm-Gg: ASbGncutqnw5XpkzGElGeFHq0GRPDTKQy8iJmXDUJNencf4jDTauMCwJXEEhAgKGurE qGDt1Dmmuv1d2oyMs94z1tAho9KUJFBRaGluYuXT7vobAMAAa6H391VEqoq6Uhqg549xlLkCtdr ymQZsIHGVCO8HUq58imDRBq1FuMZRaX43jtJG934MC8+ongAO1OZQvd7KpAqgb9MIxeOkKFRlKd j70i4eGZpjMz2Ones5RDpliQ3evYANI+vTcsTN9JAWSEWBGKPNLFEEa0x5w12bjZd3i3oPkNmUZ QcD9pleX6/y8 X-Google-Smtp-Source: AGHT+IG9HGOJEJ5VvNjG6tS7QNzc4aJZsMKTpJgSZXTtU2nb+R7Hha9yN5k78dvc8u5qBoxsEekEFg== X-Received: by 2002:a05:6000:1789:b0:38a:5122:5a07 with SMTP id ffacd0b85a97d-38a8730394cmr6842668f8f.15.1736433140644; Thu, 09 Jan 2025 06:32:20 -0800 (PST) Received: from raspberrypi.pitowers.org ([2a00:1098:3142:1f:ffc9:aff6:7f7f:893b]) by smtp.gmail.com with ESMTPSA id ffacd0b85a97d-38a8e4b80b2sm1952569f8f.80.2025.01.09.06.32.19 (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Thu, 09 Jan 2025 06:32:20 -0800 (PST) From: David Plowman To: libcamera-devel@lists.libcamera.org Cc: David Plowman , Arsen Mikovic , Naushir Patuck Subject: [PATCH v3 6/7] ipa: rpi: sync: Add an implementation of the camera sync algorithm Date: Thu, 9 Jan 2025 14:32:10 +0000 Message-Id: <20250109143211.11939-7-david.plowman@raspberrypi.com> X-Mailer: git-send-email 2.39.5 In-Reply-To: <20250109143211.11939-1-david.plowman@raspberrypi.com> References: <20250109143211.11939-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 use wallclock timestamps, passed to us from the pipeline handler, that have been de-jittered appropriately, meaning that the synchronisation will actually work across networked devices. 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 Reviewed-by: Naushir Patuck --- src/ipa/rpi/controller/meson.build | 1 + src/ipa/rpi/controller/rpi/sync.cpp | 330 ++++++++++++++++++++++++++++ src/ipa/rpi/controller/rpi/sync.h | 68 ++++++ 3 files changed, 399 insertions(+) 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..dde4ac12 100644 --- a/src/ipa/rpi/controller/meson.build +++ b/src/ipa/rpi/controller/meson.build @@ -23,6 +23,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/sync.cpp b/src/ipa/rpi/controller/rpi/sync.cpp new file mode 100644 index 00000000..43a8cbe6 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/sync.cpp @@ -0,0 +1,330 @@ +/* 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 = 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(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); + + 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(); + + /* 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 = (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(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(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); diff --git a/src/ipa/rpi/controller/rpi/sync.h b/src/ipa/rpi/controller/rpi/sync.h new file mode 100644 index 00000000..d3c79b7a --- /dev/null +++ b/src/ipa/rpi/controller/rpi/sync.h @@ -0,0 +1,68 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2024, Raspberry Pi Ltd + * + * sync.h - sync algorithm + */ +#pragma once + +#include + +#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 */