Added an example program to Get frames from ToF camera using V4L2 APIs

apps/server/server.cpp

@@ -50,6 +50,7 @@
 
 using namespace google::protobuf::io;
 
+static const int FRAME_PREPADDING_BYTES = 8;
 static int interrupted = 0;
 
 /* Available sensors */
@@ -199,7 +200,8 @@ int Network::callback_function(struct lws *wsi,
             buff_frame_to_send[0] = '1';
 
             n = lws_write(wsi, buff_frame_to_send + LWS_SEND_BUFFER_PRE_PADDING,
-                          (buff_frame_length + 1), LWS_WRITE_TEXT);
+                          (buff_frame_length + 1 + FRAME_PREPADDING_BYTES),
+                          LWS_WRITE_TEXT);
             m_frame_ready = false;
             if (lws_partial_buffered(wsi)) {
                 latest_sent_msg_is_was_buffered = true;
@@ -502,12 +504,21 @@ void invoke_sdk_api(payload::ClientRequest buff_recv) {
             free(buff_frame_to_send);
             buff_frame_to_send = NULL;
         }
-        buff_frame_to_send = (uint8_t *)malloc(
-            (buff_frame_length + LWS_SEND_BUFFER_PRE_PADDING + 1) *
-            sizeof(uint8_t));
-
-        memcpy(buff_frame_to_send + (LWS_SEND_BUFFER_PRE_PADDING + 1), buffer,
-               buff_frame_length * sizeof(uint8_t));
+        buff_frame_to_send =
+            (uint8_t *)malloc((buff_frame_length + LWS_SEND_BUFFER_PRE_PADDING +
+                               1 + FRAME_PREPADDING_BYTES) *
+                              sizeof(uint8_t));
+        int64_t *pTimestampLocation = reinterpret_cast<int64_t *>(
+            &buff_frame_to_send[LWS_SEND_BUFFER_PRE_PADDING + 1]);
+        *pTimestampLocation =
+            std::chrono::time_point_cast<std::chrono::milliseconds>(
+                std::chrono::system_clock::now())
+                .time_since_epoch()
+                .count();
+
+        memcpy(buff_frame_to_send +
+                   (LWS_SEND_BUFFER_PRE_PADDING + 1 + FRAME_PREPADDING_BYTES),
+               buffer, buff_frame_length * sizeof(uint8_t));
 
         m_frame_ready = true;
 

apps/uvc-app/tof-sdk-interface.cpp

@@ -427,6 +427,7 @@ void handleClientRequest(const char *in_buf, const size_t in_len,
     }
     } // switch
 
+    response.add_int32_payload(10);
     response.SerializeToString(&serverResponseBlob);
 
     *out_buf = (char *)malloc(serverResponseBlob.size());

azure-pipelines.yml

@@ -115,8 +115,8 @@ jobs:
       versionSpec: '3.9'
   - task: Cache@2
     inputs:
-      key: deps_windowsZ | "$(imageName)" 
-      restoreKeys: deps_windowsZ | "$(imageName)"
+      key: deps_windows | "$(imageName)" 
+      restoreKeys: deps_windows | "$(imageName)"
       path: $(Agent.BuildDirectory)/s/deps_installed
       cacheHitVar: CACHE_RESTORED
   - task: PowerShell@2

ci/azure/windows-build/build_sdk.ps1

@@ -7,10 +7,10 @@ mkdir build_Debug
 mkdir ../libs
 
 cd build_Release
-cmake -DUSE_DEPTH_COMPUTE_STUBS=1 -DWITH_OPENCV=on -DWITH_PYTHON=on -DCMAKE_PREFIX_PATH="../deps_installed/Release/glog;../deps_installed/Release/protobuf;../deps_installed/Release/websockets;..\deps_installed\OpenSSL-Win64" -DOpenCV_DIR="C:/tools/opencv/build/x64/vc15/lib" -DOPENSSL_INCLUDE_DIRS="..\deps_installed\OpenSSL-Win64\include" ..
+cmake -DUSE_DEPTH_COMPUTE_STUBS=1 -DWITH_OPENCV=on -DWITH_PYTHON=on -DCMAKE_PREFIX_PATH="../deps_installed/Release/glog;../deps_installed/Release/protobuf;../deps_installed/Release/websockets" -DOpenCV_DIR="C:/tools/opencv/build/x64/vc15/lib" ..
 cmake --build . --target install --config Release -j 4
 
 cd ../build_Debug
-cmake -DUSE_DEPTH_COMPUTE_STUBS=1 -DWITH_OPENCV=on -DWITH_PYTHON=on -DCMAKE_PREFIX_PATH="../deps_installed/Debug/glog;../deps_installed/Debug/protobuf;../deps_installed/Debug/websockets;..\deps_installed\OpenSSL-Win64" -DOpenCV_DIR="C:/tools/opencv/build/x64/vc15/lib" -DOPENSSL_INCLUDE_DIRS="..\deps_installed\OpenSSL-Win64\include" ..
+cmake -DUSE_DEPTH_COMPUTE_STUBS=1 -DWITH_OPENCV=on -DWITH_PYTHON=on -DCMAKE_PREFIX_PATH="../deps_installed/Debug/glog;../deps_installed/Debug/protobuf;../deps_installed/Debug/websockets" -DOpenCV_DIR="C:/tools/opencv/build/x64/vc15/lib" ..
 cmake --build . --target install --config Debug -j 4
 

ci/azure/windows-build/install_deps.ps1

@@ -1,8 +1,6 @@
 $ARCH=$Env:ARCH
 $GENERATOR=$Env:COMPILER
 
-choco install openssl
-
 $local_path=$pwd
 mkdir deps_installed
 
@@ -15,8 +13,6 @@ mkdir -p deps_installed/Debug/protobuf
 mkdir -p deps_installed/Release/websockets
 mkdir -p deps_installed/Debug/websockets
 
-cp -r C:/'Program Files'/OpenSSL-Win64 deps_installed
-
 #Install glog
 git clone --branch v0.6.0 --depth 1 https://github.com/google/glog
 cd glog
@@ -39,11 +35,11 @@ mkdir build_3_2_3_Release
 mkdir build_3_2_3_Debug
 
 cd build_3_2_3_Release
-cmake -DOPENSSL_ROOT_DIR="$local_path/deps_installed/OpenSSL-Win64" -DCMAKE_INSTALL_PREFIX="$local_path/deps_installed/Release/websockets" -G $GENERATOR ..
+cmake -DLWS_WITH_SSL=OFF -DCMAKE_INSTALL_PREFIX="$local_path/deps_installed/Release/websockets" -G $GENERATOR ..
 cmake --build . --target install --config Release -j 4 
 
 cd ../build_3_2_3_Debug
-cmake -DOPENSSL_ROOT_DIR="$local_path/deps_installed/OpenSSL-Win64" -DCMAKE_INSTALL_PREFIX="$local_path/deps_installed/Debug/websockets" -G $GENERATOR ..
+cmake -DLWS_WITH_SSL=OFF -DCMAKE_INSTALL_PREFIX="$local_path/deps_installed/Debug/websockets" -G $GENERATOR ..
 cmake --build . --target install --config Debug -j 4
 
 cd $local_path

examples/first-frame/main.cpp

@@ -40,9 +40,11 @@
 #else
 #include <aditof/log.h>
 #endif
+#include <chrono>
 #include <ios>
 #include <iostream>
 #include <map>
+#include <thread>
 
 using namespace aditof;
 
@@ -98,6 +100,12 @@ Status save_frame(aditof::Frame &frame, std::string frameType) {
 }
 
 int main(int argc, char *argv[]) {
+
+    // Start time
+    auto start_time = std::chrono::steady_clock::now();
+    // Duration of 30 minutes
+    auto duration = std::chrono::minutes(30);
+
     std::map<std::string, struct Argument> command_map = {
         {"-h", {"--help", false, "", ""}},
         {"-ip", {"--ip", false, "", ""}},
@@ -241,18 +249,52 @@ int main(int argc, char *argv[]) {
         LOG(ERROR) << "Could not start the camera!";
         return 0;
     }
-    aditof::Frame frame;
 
-    status = camera->requestFrame(&frame);
-    if (status != Status::OK) {
-        LOG(ERROR) << "Could not request frame!";
-        return 0;
-    } else {
-        LOG(INFO) << "succesfully requested frame!";
-    }
+    std::ofstream file("timestampDifference_and_requestFrameExecutionTime_values.txt", std::ios_base::trunc);
+    int64_t *timestamp;
+
+    while(true) {        
+        aditof::Frame frame;
+
+        // Check if 30 minutes have elapsed
+        if (std::chrono::steady_clock::now() - start_time >= duration) {
+            break;  // Exit the loop if 30 minutes have elapsed
+        }
 
-    save_frame(frame, "ir");
-    save_frame(frame, "depth");
+        auto start = std::chrono::high_resolution_clock::now();
+        status = camera->requestFrame(&frame);
+        auto end = std::chrono::high_resolution_clock::now();
+
+        // Calculate duration in milliseconds
+        std::chrono::duration<double, std::milli> requestFrameDuration = end - start;
+        // Print duration
+        LOG(INFO) << "requestFrame Execution time: " << requestFrameDuration.count() << " ms" << std::endl;
+
+        if (status != Status::OK) {
+            LOG(ERROR) << "Could not request frame!";
+            file << "Unable to get the frame" << "\n";
+        } else {
+            LOG(INFO) << "succesfully requested frame!";
+
+            frame.getData("timestamp", reinterpret_cast<uint16_t **>(&timestamp));
+            if (timestamp) {
+                LOG(INFO) << "frame timestamp is: " << *timestamp;
+
+                // Display amount of time between the moment the frame was captured and this moment
+                auto frameCapturedTimepointMs =
+                    std::chrono::time_point_cast<std::chrono::milliseconds>(
+                        std::chrono::system_clock::time_point(
+                            std::chrono::milliseconds(*timestamp)));
+                const auto nowMs =
+                    std::chrono::time_point_cast<std::chrono::milliseconds>(
+                        std::chrono::system_clock::now());
+                LOG(INFO) << "time passed since the frame was captured: "
+                    << (nowMs - frameCapturedTimepointMs).count() << "ms";
+                file << "requestFrameDuration: " << requestFrameDuration.count() << ", timestamp_diff: " << (nowMs - frameCapturedTimepointMs).count() << "\n";
+            }
+        }
+
+    }
 
     // Example of reading temperature from hardware
     uint16_t sensorTmp = 0;
@@ -261,20 +303,20 @@ int main(int argc, char *argv[]) {
     if (status != Status::OK) {
         LOG(ERROR) << "Could not read sensor temperature!";
     }
-
     status = camera->adsd3500GetLaserTemperature(laserTmp);
     if (status != Status::OK) {
         LOG(ERROR) << "Could not read laser temperature!";
     }
+    LOG(INFO) << "Sensor temperature: " << sensorTmp;
+    LOG(INFO) << "Laser temperature: " << laserTmp;
 
     status = camera->stop();
     if (status != Status::OK) {
         LOG(ERROR) << "Could not stop the camera!";
         return 0;
     }
 
-    LOG(INFO) << "Sensor temperature: " << sensorTmp;
-    LOG(INFO) << "Laser temperature: " << laserTmp;
+    file.close();
 
     return 0;
 }

sdcard-images-utils/nxp/patches/ubuntu_overlay/step1/usr/share/systemd/usb-gadget.sh

@@ -176,8 +176,8 @@ create_uvc() {
 	#echo 2048 > functions/$FUNCTION/streaming_maxpacket
 	#echo 1024 > functions/$FUNCTION/streaming_maxpacket
 
-	echo 15 > functions/$FUNCTION/streaming_maxburst
-	echo 3 > functions/$FUNCTION/streaming_interval
+	echo 1 > functions/$FUNCTION/streaming_maxburst
+	echo 1 > functions/$FUNCTION/streaming_interval
 
 	ln -s functions/$FUNCTION $CONFIG
 }

sdk/src/cameras/itof-camera/camera_itof.cpp

@@ -664,6 +664,14 @@ aditof::Status CameraItof::setFrameType(const std::string &frameType) {
         m_details.frameType.totalCaptures = 1;
     }
     m_details.frameType.dataDetails.clear();
+    FrameDataDetails timestampDetails;
+    timestampDetails.type = "timestamp";
+    timestampDetails.width = sizeof(int64_t);
+    timestampDetails.height = 1;
+    timestampDetails.subelementSize = 1;
+    timestampDetails.subelementsPerElement = 1;
+    m_details.frameType.dataDetails.emplace_back(timestampDetails);
+
     for (const auto &item : (*frameTypeIt).content) {
         if (item.type == "xyz" && !m_xyzEnabled) {
             continue;
@@ -808,18 +816,20 @@ aditof::Status CameraItof::requestFrame(aditof::Frame *frame,
     }
 
     uint16_t *frameDataLocation = nullptr;
+    std::string dataType;
     if (m_targetFramesAreComputed) {
-        frame->getData("frameData", &frameDataLocation);
+        dataType = "frameData";
     } else {
         if ((m_details.frameType.type == "pcm-native")) {
-            frame->getData("ir", &frameDataLocation);
+            dataType = "ir";
         } else if (m_details.frameType.type == "") {
             LOG(ERROR) << "Frame type not found!";
             return Status::INVALID_ARGUMENT;
         } else {
-            frame->getData("raw", &frameDataLocation);
+            dataType = "raw";
         }
     }
+    frame->getData("frameData", &frameDataLocation);
     if (!frameDataLocation) {
         LOG(WARNING) << "getframe failed to allocated valid frame";
         return status;
@@ -830,6 +840,14 @@ aditof::Status CameraItof::requestFrame(aditof::Frame *frame,
         LOG(WARNING) << "Failed to get frame from device";
         return status;
     }
+    int64_t *timestamp;
+    frame->getData("timestamp", reinterpret_cast<uint16_t **>(&timestamp));
+
+    frame->getData(dataType, &frameDataLocation);
+    if (!frameDataLocation) {
+        LOG(WARNING) << "getframe failed to allocated valid frame";
+        return status;
+    }
 
     if (!m_adsd3500Enabled && !m_isOffline &&
         (m_details.frameType.type != "pcm-native")) {

sdk/src/cameras/itof-frame/frame_impl.cpp

@@ -196,6 +196,9 @@ void FrameImpl::allocFrameData(const aditof::FrameDetails &details) {
                             total_captures](FrameDataDetails frameDetail) {
         if (frameDetail.type == "header") {
             return (unsigned long int)(embed_hdr_length / 2) * total_captures;
+        } else if (frameDetail.type == "timestamp") {
+            return (unsigned long int)frameDetail.height * frameDetail.width /
+                   2;
         } else if (frameDetail.type == "xyz") {
             return (unsigned long int)(frameDetail.height * frameDetail.width *
                                        sizeof(Point3I));

sdk/src/connections/network/network.cpp

@@ -242,6 +242,7 @@ int Network::SendCommand() {
     while (numRetry++ < MAX_RETRY_CNT &&
            Server_Connected[m_connectionId] != false) {
 
+        lws_callback_on_writable(web_socket.at(m_connectionId));
         lws_callback_on_writable(web_socket.at(m_connectionId));
         /*Acquire the lock*/
         std::unique_lock<std::recursive_mutex> mlock(m_mutex[m_connectionId]);

sdk/src/connections/target/adsd3500_sensor.cpp

@@ -23,6 +23,7 @@
 #include <cstring>
 #include <unistd.h>
 #endif
+#include <chrono>
 #include <linux/videodev2.h>
 #include <signal.h>
 #include <sstream>
@@ -682,13 +683,17 @@ aditof::Status Adsd3500Sensor::program(const uint8_t *firmware, size_t size) {
 aditof::Status Adsd3500Sensor::getFrame(uint16_t *buffer) {
 
     using namespace aditof;
+    using namespace std::chrono;
+
     struct v4l2_buffer buf[MAX_SUBFRAMES_COUNT];
     struct VideoDev *dev;
     Status status;
     unsigned int buf_data_len;
     uint8_t *pdata;
     dev = &m_implData->videoDevs[0];
     m_capturesPerFrame = 1;
+    int64_t *pTimestampLocation = reinterpret_cast<int64_t *>(buffer);
+
     for (int idx = 0; idx < m_capturesPerFrame; idx++) {
         status = waitForBufferPrivate(dev);
         if (status != Status::OK) {
@@ -700,12 +705,22 @@ aditof::Status Adsd3500Sensor::getFrame(uint16_t *buffer) {
             return status;
         }
 
+        // Get current time as soon as we get the frame from kernel space and
+        // write the timestamp right before the frame.
+        *pTimestampLocation = time_point_cast<milliseconds>(system_clock::now())
+                                  .time_since_epoch()
+                                  .count();
+
         status = getInternalBufferPrivate(&pdata, buf_data_len, buf[idx], dev);
         if (status != Status::OK) {
             return status;
         }
 
-        memcpy(buffer, pdata, buf_data_len);
+        // The timestamp occupies 8 bytes. FramePos will be 4 (aka the 4th pair
+        // of 2 bytes, because buffer points at 2 bytes at a time)
+        size_t framePos = sizeof(*pTimestampLocation) / sizeof(*buffer);
+
+        memcpy(buffer + framePos, pdata, buf_data_len);
 
         status = enqueueInternalBufferPrivate(buf[idx], dev);
         if (status != Status::OK) {