From patchwork Fri Aug 25 06:37:06 2023 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Gabrielle George X-Patchwork-Id: 18961 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 CAC5AC3262 for ; Fri, 25 Aug 2023 06:37:32 +0000 (UTC) Received: from lancelot.ideasonboard.com (localhost [IPv6:::1]) by lancelot.ideasonboard.com (Postfix) with ESMTP id 8013E61F19; Fri, 25 Aug 2023 08:37:32 +0200 (CEST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/simple; d=libcamera.org; s=mail; t=1692945452; bh=CPhyN4E/4hqI8BZ3sYxX5MSPwp2ywtnMwIlrtJol5XY=; h=To:Date:In-Reply-To:References:Subject:List-Id:List-Unsubscribe: List-Archive:List-Post:List-Help:List-Subscribe:From:Reply-To: From; b=xWqg4nrw2aiGQiiirf+G5ooOkE2qBfv7O+h6BZokVuM9B61kpy2UFyN8ar1K4fzI4 eyCpvHMBXdIPPckDZt2mBf0mjZxTZboi6c7TFj5k/23eekkdtDsV+qYkTgDaRgB0q7 9v5AcJTy0r7+xG29AVwKG2D6hFvDCNwTQLwCgv94FtFO/0n3Bu0lSdOQVV3Db0108h waRJv/KS95BepoM3dL1NCpqj4XC93LErXxLkuDOPiNnG4j7GimU+UnUBF0VO+qxh+c 9BfVpaDrfTmK4yp3lGVuAH9XkaFTiOMcmUEN8S1j/I66A1OOOdLGnCmDEC1z9WUeJh 3dTcJG/le1CMQ== Received: from mail-oo1-xc2d.google.com (mail-oo1-xc2d.google.com [IPv6:2607:f8b0:4864:20::c2d]) by lancelot.ideasonboard.com (Postfix) with ESMTPS id AB19161E01 for ; Fri, 25 Aug 2023 08:37:28 +0200 (CEST) Authentication-Results: lancelot.ideasonboard.com; dkim=pass (2048-bit key; unprotected) header.d=gmail.com header.i=@gmail.com header.b="ET8powXy"; dkim-atps=neutral Received: by mail-oo1-xc2d.google.com with SMTP id 006d021491bc7-5732481b22eso421739eaf.3 for ; Thu, 24 Aug 2023 23:37:28 -0700 (PDT) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=gmail.com; s=20221208; t=1692945447; x=1693550247; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:to:from:from:to:cc:subject:date:message-id :reply-to; bh=2Onkgtntva6Jxzg6cR1B7ztmbjs3DRkoZm6St4PeQ3g=; b=ET8powXyYcxq/uYhpmkP22qFo7qOo79TBH/G+ZLBFDOmn/9LnOZT/P9RfcC/iepJ8K SirDRFcmbb+2aOwk942aagLXIRm2Ee5HWoB87odXLyZROKdW5TGXJNiUdm74X6Pi3zye ugFQyRbu5vXoZ1pK6qFOnwYC/j+LL21/EDTS3+9P50rkehbjn7OFydZeDcnca84qMfbp cr0nf+FtBeQjundYviVJntEjNZWq93cVTD5LFcf2z/cv/j/hdKHJubVcXfoDIADB8Csx lxw8jvu3g362CaFTPtERdlFyc5aVb7bIjBAbouGOb/ueY6gdksJN7JkX3dt3XMHFTRZy nTbQ== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20221208; t=1692945447; x=1693550247; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:to:from:x-gm-message-state:from:to:cc :subject:date:message-id:reply-to; bh=2Onkgtntva6Jxzg6cR1B7ztmbjs3DRkoZm6St4PeQ3g=; b=hx8eSE3xn7TfcvdN5elcDu2jcGu/HQGS2uJqUXGYR0dM9p4a9rsoPj0bmFpGwiBrTw TMJfckFgvxv966e2QxR72vCohO6BEKgafwePG2WLflt5RTvm53/rM7fWD/cyXbGAzrU9 ZiC6n0MqKXvd2ibDao3QlPZUu9Ut8wbNjSSsQZ4tgyLavFIEjsw+Myx+BK+A7ihFdCrD IowcL4H0p4zPoWxHn9UW9ne/RMLPAfICxJekesau8EqiMCVgdGBxY+77bl+A0dUzIadu bx3OWRTnyLvHZ+/bDgMyw6miEbkPceseQXNm8re1mXfTnqEQ0FmkLnNM7tKApXV6WfO3 eJTg== X-Gm-Message-State: AOJu0YzeWsPdkgsIBBltWdIDQF8sdcMr+ZSX2Si+IkjVJ8ugv14F//5s GBGTvdZihj8sp9xv/fu1JaScoju5OtI= X-Google-Smtp-Source: AGHT+IFp2crJPQ7H/yU9vR/4Y2xjdO9hZSbgZwxjKSTLkUlfctE6gdHJpxh2Yscv+63XrB8gXKPhFw== X-Received: by 2002:a4a:d2c8:0:b0:56d:2cbf:2315 with SMTP id j8-20020a4ad2c8000000b0056d2cbf2315mr4987011oos.9.1692945446948; Thu, 24 Aug 2023 23:37:26 -0700 (PDT) Received: from localhost.localdomain (97-115-76-16.ptld.qwest.net. [97.115.76.16]) by smtp.gmail.com with ESMTPSA id u41-20020a4a8c2c000000b0056688eea98csm623239ooj.27.2023.08.24.23.37.26 (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Thu, 24 Aug 2023 23:37:26 -0700 (PDT) To: libcamera-devel@lists.libcamera.org, kieran.bingham@ideasonboard.com, vedantparanjape160201@gmail.com, gabbymg94@gmail.com Date: Thu, 24 Aug 2023 23:37:06 -0700 Message-Id: <20230825063707.8734-2-gabbymg94@gmail.com> X-Mailer: git-send-email 2.34.1 In-Reply-To: <20230825063707.8734-1-gabbymg94@gmail.com> References: <20230825063707.8734-1-gabbymg94@gmail.com> MIME-Version: 1.0 Subject: [libcamera-devel] [PATCH 1/2] libcamera: pipeline: uvcvideo: Store time metadata 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: , X-Patchwork-Original-From: Gabby George via libcamera-devel From: Gabrielle George Reply-To: Gabby George Errors-To: libcamera-devel-bounces@lists.libcamera.org Sender: "libcamera-devel" Parse the raw timing data out of the metadata packets and store each sample in a circular buffer of size 32. To convert the latest frame's metadata into a system clock timestamp, use the metadata time information from 32 frames back stored in the back of the circular buffer. For every metadata buffer that comes in, store the timing information in the circular queue. The information at the head of the queue may be used instantly in the case where a corresponding video buffer has come in, or it may be used when the video buffer eventually does arrive. In any case, the information at the head of the queue will be used when it comes time for the timestamp to be set in the request metadata for the video buffer. If there has only been one frame of metadata, use the default (video buffer-provided) timestamp. If the circular buffer has not yet been filled with 32 frames, use the oldest frame. When the circular buffer reaches 32 frames, remove the oldest item. This approach creates synchronization between the metadata buffer that will be used to update the video buffer's requests' timestamp and the timing data itself, without having to perform checks ensuring synchronization. It also creates easy and efficient access to the front and back of the circular buffer. Signed-off-by: Gabby George --- src/libcamera/pipeline/uvcvideo/uvcvideo.cpp | 76 +++++++++++++++++--- 1 file changed, 67 insertions(+), 9 deletions(-) diff --git a/src/libcamera/pipeline/uvcvideo/uvcvideo.cpp b/src/libcamera/pipeline/uvcvideo/uvcvideo.cpp index 75a949b8..43ce4f8a 100644 --- a/src/libcamera/pipeline/uvcvideo/uvcvideo.cpp +++ b/src/libcamera/pipeline/uvcvideo/uvcvideo.cpp @@ -6,6 +6,7 @@ */ #include +#include #include #include #include @@ -47,6 +48,17 @@ struct UVCTimingBuf { __u16 sofDevice; } __attribute__((packed)); +/* Raw timestamp input to the timestamp calculation function. */ +struct UVCTimestampData { + unsigned long long tsHost; /* System clock timestamp in nanoseconds*/ + unsigned short sofHost; /* The usb clock at the time tsHost was taken*/ + unsigned int stcDevice; /* The UVC device source timestamp */ + unsigned short sofDevice; /* the usb clock at the time the STC was taken*/ + + /* presentation time stamp to be converted into a system clock timestamp */ + unsigned int ptsDevice; +}; + class UVCCameraData : public Camera::Private { public: @@ -72,15 +84,18 @@ public: std::map> formats_; std::queue pendingVideoBuffers_; - std::queue> pendingMetadata_; + std::queue pendingMetadata_; private: int initMetadata(MediaDevice *media); void completeRequest(FrameBuffer *buffer, uint64_t timestamp); void endCorruptedStream(); + void addTimestampData(uvc_meta_buf &rawMetadata, UVCTimingBuf &packed); + std::deque timeSamples_; const unsigned int frameStart_ = 1; const unsigned int maxVidBuffersInQueue_ = 1; + const unsigned int bufferRingSize_ = 32; bool generateId(); @@ -899,6 +914,13 @@ void UVCCameraData::endCorruptedStream() << "UVC metadata stream corrupted. Reverting to driver timestamps."; } +unsigned long long calculateTimestamp([[maybe_unused]] const UVCTimestampData &p1, + const UVCTimestampData &p2, + [[maybe_unused]] const unsigned int PTS) +{ + return p2.tsHost; +} + /* * If there is a metadata buffer that hasn't been matched with a * video buffer, check to see if it matches this video buffer. @@ -929,9 +951,17 @@ void UVCCameraData::bufferReady(FrameBuffer *buffer) if (!pendingMetadata_.empty()) { /* A metadata buffer was ready first. */ - unsigned int mdSequence = std::get<0>(pendingMetadata_.front()) + frameStart_; + unsigned int mdSequence = pendingMetadata_.front() + frameStart_; if (mdSequence == buffer->metadata().sequence) { - completeRequest(buffer, std::get<1>(pendingMetadata_.front())); + unsigned long long timestamp; + if (timeSamples_.size() > 1) { + timestamp = calculateTimestamp(timeSamples_.front(), + timeSamples_.back(), + timeSamples_.back().ptsDevice); + } else { + timestamp = buffer->metadata().timestamp; + } + completeRequest(buffer, timestamp); pendingMetadata_.pop(); return; } else { @@ -951,6 +981,28 @@ void UVCCameraData::bufferReady(FrameBuffer *buffer) } } +void UVCCameraData::addTimestampData(uvc_meta_buf &rawMetadata, UVCTimingBuf &packed) +{ + /* + * Copy over the buffer packet from the raw Metadata + * into values we can use. Populate the storage struct + * with the data we need to calculate timestamps. + * Add to the circular queue. + */ + UVCTimestampData data; + data.ptsDevice = packed.pts; + data.sofDevice = packed.sofDevice; + data.stcDevice = packed.stc; + data.sofHost = rawMetadata.sof; + data.tsHost = rawMetadata.ns; + + if (timeSamples_.size() == bufferRingSize_) { + timeSamples_.pop_front(); + } + + timeSamples_.push_back(data); +} + void UVCCameraData::bufferReadyMetadata(FrameBuffer *buffer) { if (!metadata_ || @@ -971,8 +1023,8 @@ void UVCCameraData::bufferReadyMetadata(FrameBuffer *buffer) Span memMeta = mappedMetadataBuffers_.at(pos).planes()[0]; uvc_meta_buf *metaBuf = reinterpret_cast(memMeta.data()); - //Span memTime = mappedMetadataBuffers_.at(pos).planes()[0]; - //UVCTimingBuf * timeBuf = reinterpret_cast(&memTime.data()[sizeof(uvc_meta_buf)]); + Span memTime = mappedMetadataBuffers_.at(pos).planes()[0]; + UVCTimingBuf *timeBuf = reinterpret_cast(&memTime.data()[sizeof(uvc_meta_buf)]); size_t UVCPayloadHeaderSize = sizeof(metaBuf->length) + sizeof(metaBuf->flags) + sizeof(UVCTimingBuf); @@ -981,6 +1033,8 @@ void UVCCameraData::bufferReadyMetadata(FrameBuffer *buffer) return; } + addTimestampData(*metaBuf, *timeBuf); + /* * Match a pending video buffer with this buffer's sequence. If * there is none available, put this timestamp information on the @@ -992,15 +1046,19 @@ void UVCCameraData::bufferReadyMetadata(FrameBuffer *buffer) unsigned int vidSequence = vidBuffer->metadata().sequence; if (vidSequence == mdSequence) { - completeRequest(vidBuffer, static_cast(metaBuf->ns)); + unsigned long long timestamp; + if (timeSamples_.size() > 1) { + timestamp = calculateTimestamp(timeSamples_.front(), + timeSamples_.back(), + timeSamples_.back().ptsDevice); + } + completeRequest(vidBuffer, timestamp); pendingVideoBuffers_.pop(); } else { endCorruptedStream(); } } else { - pendingMetadata_.push( - std::make_pair(buffer->metadata().sequence, - static_cast(metaBuf->ns))); + pendingMetadata_.push(buffer->metadata().sequence); } metadata_->queueBuffer(buffer); } From patchwork Fri Aug 25 06:37:07 2023 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Gabrielle George X-Patchwork-Id: 18962 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 908AFBF415 for ; Fri, 25 Aug 2023 06:37:33 +0000 (UTC) Received: from lancelot.ideasonboard.com (localhost [IPv6:::1]) by lancelot.ideasonboard.com (Postfix) with ESMTP id F2A8061E01; Fri, 25 Aug 2023 08:37:32 +0200 (CEST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/simple; d=libcamera.org; s=mail; t=1692945453; bh=Gz/op4V/CEz9kxR/Pgp31gCIBHQ/CRS0cdt4iLZv7dk=; h=To:Date:In-Reply-To:References:Subject:List-Id:List-Unsubscribe: List-Archive:List-Post:List-Help:List-Subscribe:From:Reply-To: From; b=vX9LrNVxVHYmvmnu3rTatwoQgDLe2T1pVAaYBcInQNDv/fsoIb1My0ehn3uriciku LbJ3HfGxmFkNqTO1/CKibTDEoWA59l5PTW20kpU7t06Rh0MdRdLUEX/0C0Vbr3fkwg 4lvq8Ti6sEeEPiMzbSbJlaCGoESK2VcNtfzsxHqsD5H6kwgkfEmxOcKfWKV3h25oSZ AfM9nKV9g1hfVfMt2T3MkTetUYiG7aBvqu8aMyV/k3VWzKQqMzeEpxMDSyhPQ4p+Bn vbXljuZo8THgZOCOzjiNFC8QwTRcU32KLk7oDg2l6kUQz+NP2f4NVRqqHbYi+3yIQ2 H/lUsbeK7aAuQ== Received: from mail-oo1-xc35.google.com (mail-oo1-xc35.google.com [IPv6:2607:f8b0:4864:20::c35]) by lancelot.ideasonboard.com (Postfix) with ESMTPS id AB97F61E01 for ; Fri, 25 Aug 2023 08:37:29 +0200 (CEST) Authentication-Results: lancelot.ideasonboard.com; dkim=pass (2048-bit key; unprotected) header.d=gmail.com header.i=@gmail.com header.b="EBNtYgin"; dkim-atps=neutral Received: by mail-oo1-xc35.google.com with SMTP id 006d021491bc7-5733bcf6eb6so372849eaf.0 for ; Thu, 24 Aug 2023 23:37:29 -0700 (PDT) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=gmail.com; s=20221208; t=1692945448; x=1693550248; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:to:from:from:to:cc:subject:date:message-id :reply-to; bh=LjTzj1CqlDJ8g91Ky0tegDwA35QuJF4yES22o5hmF0U=; b=EBNtYginfgsVHYTc0moNDLh/J578tWaPPzVrNzjKjjRqM9KytCh3vsNC8KRNV9rNc5 BFc4v5g1qm3i0Agc6avpMQfJP/SdnCsoCD30j1vdyW3iax5PaeO3aLxDxTq/7GU5Ki6T PsrIC6J6y8SyFdNeJ0/MK6i7WVOvx6iqOQcHv9O6pa+TF78XD4r4yNezarq+qCgZjBAn hWKfrVWc4p+ptmi0g+j3jSks+kvCpYFjtz6kJDMmcNsl0YFYYhjlC7lLRF62oRyr9+GM CjHGYVKX3Gill2fhZR9HZfM7R8qT7tn2aIwojJVQ7a7efoorNMZz0rjOXOEf1oOFsORI 0nIQ== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20221208; t=1692945448; x=1693550248; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:to:from:x-gm-message-state:from:to:cc :subject:date:message-id:reply-to; bh=LjTzj1CqlDJ8g91Ky0tegDwA35QuJF4yES22o5hmF0U=; b=cbBgjuFPbM5qaEPBhTnTmBsjXqM61KOyKySMe3h792MjH8ARjSTaY0FYqfLi9snQFA CUJ45gP6AKyYeZiIAE9iPiQufV+TyFlNgd/rsuXU/RAnSNh12dPtnCo8XMwJi1I50BeA KdonB9fM4iaY1//UOybe2opBUC3MWK7k64nTiOdvuBiwuu9/P5uW1JJk/kTY/C5oCWlc WTuGW0+7bYRvrdKirFHMMp7HRuo9VLEHxpIFg2KM6Qfw8McjVgbe6qefIMbi7gailQey 1+VZith2sGN2S4fFh/40P1Fa31vRpATTZk/TtbdEdfqWw1C6uTP2A1a0iydJdeb92HDt 5psQ== X-Gm-Message-State: AOJu0YxyBHdn4qn0YNEe8PpJ4M6Vx8NI4fPmyhBiSNMnfKfmetlVu29Y NhmSaVN/9gL3jcCncgj6sbFORtBrO20= X-Google-Smtp-Source: AGHT+IHIWmxtcqozip1mPXd43mA+5VRuWOJ/HF12rLxuDthdUcsQW0EWHELh69NgYs6FleMVL0ZmeQ== X-Received: by 2002:a4a:92cf:0:b0:573:4e21:5d25 with SMTP id j15-20020a4a92cf000000b005734e215d25mr16637ooh.9.1692945448053; Thu, 24 Aug 2023 23:37:28 -0700 (PDT) Received: from localhost.localdomain (97-115-76-16.ptld.qwest.net. [97.115.76.16]) by smtp.gmail.com with ESMTPSA id u41-20020a4a8c2c000000b0056688eea98csm623239ooj.27.2023.08.24.23.37.27 (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Thu, 24 Aug 2023 23:37:27 -0700 (PDT) To: libcamera-devel@lists.libcamera.org, kieran.bingham@ideasonboard.com, vedantparanjape160201@gmail.com, gabbymg94@gmail.com Date: Thu, 24 Aug 2023 23:37:07 -0700 Message-Id: <20230825063707.8734-3-gabbymg94@gmail.com> X-Mailer: git-send-email 2.34.1 In-Reply-To: <20230825063707.8734-1-gabbymg94@gmail.com> References: <20230825063707.8734-1-gabbymg94@gmail.com> MIME-Version: 1.0 Subject: [libcamera-devel] [PATCH 2/2] libcamera: pipeline: uvcvideo: Calculate UVC timestamps 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: , X-Patchwork-Original-From: Gabby George via libcamera-devel From: Gabrielle George Reply-To: Gabby George Errors-To: libcamera-devel-bounces@lists.libcamera.org Sender: "libcamera-devel" Convert the device clock presentation time stamp (pts) to a host system clock timestamp using the timing data provided by metadata buffers. This is done in two steps. The first step converts the pts into a usb clock time by linearly interpolating between two points where both the sof and the device clock time are known. The second step converts this sof into the host's system clock timestamp by interpolating the sof between two points where both the host timestamp and the usb clock timestamp (also known as frame number according to the UVCH field for it). These calculations take into account rollover potential of the metadata times. Signed-off-by: Gabby George --- src/libcamera/pipeline/uvcvideo/uvcvideo.cpp | 141 ++++++++++++++++++- 1 file changed, 137 insertions(+), 4 deletions(-) diff --git a/src/libcamera/pipeline/uvcvideo/uvcvideo.cpp b/src/libcamera/pipeline/uvcvideo/uvcvideo.cpp index 43ce4f8a..8cc31055 100644 --- a/src/libcamera/pipeline/uvcvideo/uvcvideo.cpp +++ b/src/libcamera/pipeline/uvcvideo/uvcvideo.cpp @@ -914,11 +914,145 @@ void UVCCameraData::endCorruptedStream() << "UVC metadata stream corrupted. Reverting to driver timestamps."; } -unsigned long long calculateTimestamp([[maybe_unused]] const UVCTimestampData &p1, +/** + * \brief Calculate a more accurate host SOF + * \param[in] sofHost The usb clock time taken by the host + * \param[in] sofDevice The usb clock time taken by the device + * that has full 11 bit precision. + * + * The precision of the host sof may be lower than the expected 11 bits. + * Obtain a more precise host sof by adding back in the lower 8 bits + * of the difference between the host sof and the more precise device SOF. + * + * \return An updated usb clock time for the host with 11 bits of precision + */ +static unsigned short recoverHostSOF(unsigned short sofHost, unsigned short sofDevice) +{ + char sofDelta; + + sofDelta = (sofHost - sofDevice) & 255; + + return (sofDevice + sofDelta) & 2047; +} + +/** + * \brief Convert the presentation timestamp recorded by the UVC device to + * a host timestamp. + * \param[in] p1 Timestamps taken by the usb clock, the device, and the host + * at an early point in time and provided by the UVC driver as metadata + * \param[in] p2 Timestamps taken by the usb clock, the device, and the host + * taken shortly after the presentation timestamp and provided by the UVC driver + * as metadata + * \param[in] PTS The presentation time stamp in device time to convert + * to a host timestamp. + * + * The following device to system clock timestamp conversion algorithm is based + * on the Linux kernel's implementation of UVC timestamp calculation. + * + * To convert the presentation time stamp provided by the device to a system + * clock timestamp, first convert the pts to the usb frame number (sof), + * and then from the usb sof to the system timestamp. The relationship between + * the device clock and the usb clock is linear, as is the relationship between + * the usb clock and the system clock. To calculate the equations needed for the + * conversion, V4L2 provides a metadata packet with a source timestamp (stc) + * and a usb clock sof taken at the same point in time, as well as a system + * timestamp and a usb clock sof pairing. + * + * Two sets of the timestamp metadata are used to recover this linear relationship + * and convert the pts into system clock time. + * + * \return The PTS timestamp converted to system clock time. + */ +unsigned long long calculateTimestamp(const UVCTimestampData &p1, const UVCTimestampData &p2, - [[maybe_unused]] const unsigned int PTS) + const unsigned int PTS) { - return p2.tsHost; + unsigned short sof1Device; + unsigned short sof2Device; + unsigned int stc1; + unsigned int stc2; + unsigned short sof1Host; + unsigned short sof2Host; + unsigned int mean; + unsigned int pts; + unsigned long long hostTS1; + unsigned long long hostTS2; + + float sof; + pts = PTS; + + /* Add 2048 to both sof values to prevent underflow */ + sof1Device = p1.sofDevice + 2048; + stc1 = p1.stcDevice; + sof2Device = p2.sofDevice + 2048; + stc2 = p2.stcDevice; + + /* Subtract out the first point's host timestamp for simplicity */ + hostTS1 = 0; + hostTS2 = p2.tsHost - p1.tsHost; + + /* + * Step 1: Convert the pts into an sof usb clock time + * from p1 and p2's stc and sof time pairs. + * + * The equation is: + * sof = ((sof1 - sof2) *pts + sof1 * stc2 - sof2 * stc1) / ( stc2 - stc1) + */ + + /* If the sof field rolled over, add 2048 so we can extrapolate the line. */ + if (sof2Device < sof1Device) { + sof2Device += 2048; + } + + /* + * If the stc value rolled over, subtract half the 32 bit range from the + * stc and pts values so they fit in the rollover window. + */ + if (stc2 < stc1) { + stc1 -= (1 << 31); + stc2 -= (1 << 31); + pts -= (1 << 31); + } + + /* Cast the values or they may overflow at the multiplication step */ + sof = static_cast( + (static_cast((sof2Device - sof1Device)) * static_cast(pts) + + static_cast(sof1Device) * static_cast(stc2) + - static_cast(sof2Device) * static_cast(stc1))) / + static_cast((stc2 - stc1)); + + /* + * Step 2: Similar to Step1, convert the calculated sof + * to system timestamp from p1 and p2's host timestamp and usb sof pairs + * + * The equation is: + * timestamp = ((ts2 - ts1) * sof + ts1 * sof2 - ts2 * sof1) / (sof2 - sof1) + */ + sof1Host = recoverHostSOF(p1.sofHost, p1.sofDevice) + 2048; + sof2Host = recoverHostSOF(p2.sofHost, p2.sofDevice) + 2048; + + if (sof2Host < sof1Host) { + sof2Host += 2048; + } + + /* + * This accounts for the possibility that the calculated sof + * rolled over and the host sof data fields did not. + */ + mean = (sof1Host + sof2Host) / 2; + + if ((mean - 1024) > sof) { + sof += 2048; + } else if (sof > mean + 1024) { + sof -= 2048; + } + + /* note: hostTS 1 has been set to zero so the difference is just the value of hostTS2.*/ + unsigned long long result = static_cast( + (hostTS2 * sof + hostTS1 * sof2Host - hostTS2 * sof1Host) / (sof2Host - sof1Host)); + + /* Add the subtracted system timestamp from p1 back in */ + return result + p1.tsHost; } /* @@ -943,7 +1077,6 @@ unsigned long long calculateTimestamp([[maybe_unused]] const UVCTimestampData &p */ void UVCCameraData::bufferReady(FrameBuffer *buffer) { - /* \todo Use the UVC metadata to calculate a more precise timestamp */ if (!metadata_ || buffer->metadata().sequence < frameStart_) { completeRequest(buffer, buffer->metadata().timestamp); return;