From patchwork Wed Jan 22 14:53:48 2025 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: David Plowman X-Patchwork-Id: 22687 Return-Path: X-Original-To: parsemail@patchwork.libcamera.org Delivered-To: parsemail@patchwork.libcamera.org Received: from perceval.ideasonboard.com (perceval.ideasonboard.com [213.167.242.64]) by patchwork.libcamera.org (Postfix) with ESMTPS id 09E9DBD808 for ; Thu, 30 Jan 2025 11:53:04 +0000 (UTC) Received: from pendragon.ideasonboard.com (cpc89244-aztw30-2-0-cust6594.18-1.cable.virginm.net [86.31.185.195]) by perceval.ideasonboard.com (Postfix) with ESMTPSA id 8B0AB886 for ; Thu, 30 Jan 2025 12:51:55 +0100 (CET) Authentication-Results: perceval.ideasonboard.com; dkim=permerror header.d=raspberrypi.com header.i=@raspberrypi.com header.a=rsa-sha1 header.s=google header.b=OxUYv8dI; dkim-atps=neutral Delivered-To: kbingham@ideasonboard.com Received: from perceval.ideasonboard.com by perceval.ideasonboard.com with LMTP id 2AU9DVUGkWe3RjQA4E0KoQ (envelope-from ) for ; Wed, 22 Jan 2025 15:53:09 +0100 Received: from lancelot.ideasonboard.com (lancelot.ideasonboard.com [92.243.16.209]) by perceval.ideasonboard.com (Postfix) with ESMTPS id 0B0CC1081; Wed, 22 Jan 2025 15:53:09 +0100 (CET) Received: from lancelot.ideasonboard.com (localhost [IPv6:::1]) by lancelot.ideasonboard.com (Postfix) with ESMTP id 8ED4C6856A; Wed, 22 Jan 2025 15:54:10 +0100 (CET) Authentication-Results: lancelot.ideasonboard.com; dkim=pass (2048-bit key; unprotected) header.d=raspberrypi.com header.i=@raspberrypi.com header.b="OxUYv8dI"; dkim-atps=neutral Received: from mail-wm1-x332.google.com (mail-wm1-x332.google.com [IPv6:2a00:1450:4864:20::332]) by lancelot.ideasonboard.com (Postfix) with ESMTPS id 9D6C06855F for ; Wed, 22 Jan 2025 15:53:57 +0100 (CET) Received: by mail-wm1-x332.google.com with SMTP id 5b1f17b1804b1-4361f664af5so80664165e9.1 for ; Wed, 22 Jan 2025 06:53:57 -0800 (PST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=raspberrypi.com; s=google; t=1737557637; x=1738162437; 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=FDvnN7ryORqQHcoAh5tIXt4yh5I/D3MEokFNcVWq958=; b=OxUYv8dIe59obmcvhMfm6epzWw7rYCiwo4UKK20Bmu8mdwPHxt3+W/E86N/WYIQbRb MTRfGpYx39reSGL5YZQQ6bjCLJcDF6lLdcefr8a9SECLVR/NgOenhd96ByPdzPxLpHxg Sh6gB1OEXkJ+OrBGtUnyKHB9+Su0SOb2B6N+NAyKJWWbBM6p4Oxto11qQh48cIYGfQKK GgmOkmSWwySHYnZv9d+4KGEqSBiVS693Cub2kmwfhkIxOJ3OEqAH8tZ7cS+xYakv8Ch0 fwaGLjwChgIhKspcf7KYFnU8jJR2/NX/AkfyYVvnKDF8oJElzoGTfhd1kQXH6aLGZheM CfPQ== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20230601; t=1737557637; x=1738162437; 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=FDvnN7ryORqQHcoAh5tIXt4yh5I/D3MEokFNcVWq958=; b=Qh00Y/xUl5W5BJFfvHxgft/ALp3O5LVmW/LXey6NDPCRm/MMrqbn2FfoNnFm/13mlV 63NkWDAz/4AglAMbiK1NBQoCkBZl4I+9kdSqpjRLrshgfhCGre01hfrL1Ni3HhR8SVwY wAVRlKHPoWOBJHXXD+fgCHze98OPRPc0HE1OG0vD707syWcWurhC04DOJ7T0J6w1y2rB +cP9HIoZy4iz9n3SD9jpCd7BRvnaqkZ6mGhWvaMyyp7/7hhN40KhCGSdpCCuw7F+77AE f0DTZ0bHq2lk7FP7veMJkuf2Vl74lKn9i1Duf7emD4CAqNrC29MOYUbS1Ws+zvTFvIqr DG6A== X-Gm-Message-State: AOJu0YyZl7C8pQThPAzazB34715zjZoAz/3R/wvMjshHq0PUKa1KQ1Hm uBDFkKXpRiD5Ura3P1FFA/ytiDqdTJWImEf/d6L2hR/AioQhJk/Z4cja4vFOUzl/5D5D9u04nuo b X-Gm-Gg: ASbGnctWS5ixcS45lAoah1zRj+n37+MyfboIFYnptdJZVEtn7VO01um+LvPZ2ryCIs4 qe9v1VWjrUR8qzZPcAVZC37wCNNX9vaJrj1Gstyvp7eFj2mvIx2CxFD5V8EF0qO3U+Nrxwvl6xC hkvUhOaHlLVnEurWECsq7wr+P849Fysyr80luuwWLgtTT4A6vMG6Y/jLndv+XfF+i/lW9PskmOO o+t7Kc35NcbDVE36W+Uq7Ydd/MTpCuGV8cR902wwm8g3i186OCz9J1gPFsU4+uCaUzbaUSx1lfY 76OR5BJn+rwvCcYocRQF03PpTw== X-Google-Smtp-Source: =?utf-8?q?AGHT+IHAr4swNtEMo57x1rOuUCjo0oK5q7/nE2ALz?= =?utf-8?q?JyesrRVQu159QuWxGSD/Pv8XjUUly/1aOQewg=3D=3D?= X-Received: by 2002:a05:600c:3b02:b0:434:a781:f5d5 with SMTP id 5b1f17b1804b1-4389143ba53mr213022325e9.30.1737557636728; Wed, 22 Jan 2025 06:53:56 -0800 (PST) Received: from raspberrypi.pitowers.org ([2a00:1098:3142:1f:ffc9:aff6:7f7f:893b]) by smtp.gmail.com with ESMTPSA id 5b1f17b1804b1-438b66dc08bsm11551395e9.37.2025.01.22.06.53.56 (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Wed, 22 Jan 2025 06:53:56 -0800 (PST) From: David Plowman To: libcamera-devel@lists.libcamera.org Cc: David Plowman , Arsen Mikovic , Naushir Patuck Subject: [PATCH v4 6/7] ipa: rpi: sync: Add an implementation of the camera sync algorithm Date: Wed, 22 Jan 2025 14:53:48 +0000 Message-Id: <20250122145349.7220-7-david.plowman@raspberrypi.com> X-Mailer: git-send-email 2.39.5 In-Reply-To: <20250122145349.7220-1-david.plowman@raspberrypi.com> References: <20250122145349.7220-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" X-TUID: aDncQKUh8YKg Resent-From: Kieran Bingham Resent-To: parsemail@patchwork.libcamera.org 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 --- src/ipa/rpi/controller/meson.build | 1 + src/ipa/rpi/controller/rpi/sync.cpp | 329 ++++++++++++++++++++++++++++ src/ipa/rpi/controller/rpi/sync.h | 68 ++++++ 3 files changed, 398 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..39bf5de7 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/sync.cpp @@ -0,0 +1,329 @@ +/* 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" + +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) +{ + 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; + + /* 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 */