diff --git a/.github/workflows/test.workflow.yml b/.github/workflows/test.workflow.yml index 550123a232..1c7fba7a81 100644 --- a/.github/workflows/test.workflow.yml +++ b/.github/workflows/test.workflow.yml @@ -54,7 +54,7 @@ jobs: uses: ./.github/workflows/test_child.yml with: flavor: "vanilla" - luxonis_os_versions_to_test: "['1.14.1', '1.15.0', '1.18.3']" + luxonis_os_versions_to_test: "['1.14.1', '1.15.0', 'dai-fsync-testing_7+b13ea7cecf9a16d5641893cf9778187a96dea3af']" luxonis_os_versions_to_test_rgb: "['1.18.3']" secrets: CONTAINER_REGISTRY: ${{ secrets.CONTAINER_REGISTRY }} @@ -66,7 +66,7 @@ jobs: uses: ./.github/workflows/test_child.yml with: flavor: "tsan" - luxonis_os_versions_to_test: "['1.18.3']" + luxonis_os_versions_to_test: "['dai-fsync-testing_7+b13ea7cecf9a16d5641893cf9778187a96dea3af']" secrets: CONTAINER_REGISTRY: ${{ secrets.CONTAINER_REGISTRY }} HIL_PAT_TOKEN: ${{ secrets.HIL_PAT_TOKEN }} @@ -77,7 +77,7 @@ jobs: uses: ./.github/workflows/test_child.yml with: flavor: "asan-ubsan" - luxonis_os_versions_to_test: "['1.18.3']" + luxonis_os_versions_to_test: "['dai-fsync-testing_7+b13ea7cecf9a16d5641893cf9778187a96dea3af']" secrets: CONTAINER_REGISTRY: ${{ secrets.CONTAINER_REGISTRY }} HIL_PAT_TOKEN: ${{ secrets.HIL_PAT_TOKEN }} @@ -87,7 +87,7 @@ jobs: if: needs.precheck.outputs.should_run == 'true' uses: ./.github/workflows/test_child_windows.yml with: - luxonis_os_versions_to_test: "['1.18.3']" + luxonis_os_versions_to_test: "['dai-fsync-testing_7+b13ea7cecf9a16d5641893cf9778187a96dea3af]" secrets: CONTAINER_REGISTRY: ${{ secrets.CONTAINER_REGISTRY }} HIL_PAT_TOKEN: ${{ secrets.HIL_PAT_TOKEN }} diff --git a/bindings/python/src/DeviceBindings.cpp b/bindings/python/src/DeviceBindings.cpp index 1da14cf743..915c4be649 100644 --- a/bindings/python/src/DeviceBindings.cpp +++ b/bindings/python/src/DeviceBindings.cpp @@ -830,6 +830,23 @@ void DeviceBindings::bind(pybind11::module& m, void* pCallstack) { }, py::arg("enable"), DOC(dai, DeviceBase, setTimesync, 2)) + .def( + "setM8FsyncRole", + [](DeviceBase& d, M8FsyncRole role) { + py::gil_scoped_release release; + return d.setM8FsyncRole(role); + }, + py::arg("role"), + DOC(dai, DeviceBase, setM8FsyncRole) + ) + .def( + "getM8FsyncRole", + [](DeviceBase& d) { + py::gil_scoped_release release; + return d.getM8FsyncRole(); + }, + DOC(dai, DeviceBase, getM8FsyncRole) + ) .def( "getDeviceName", [](DeviceBase& d) { diff --git a/bindings/python/src/pipeline/CommonBindings.cpp b/bindings/python/src/pipeline/CommonBindings.cpp index f2352cb626..b6578e4c9f 100644 --- a/bindings/python/src/pipeline/CommonBindings.cpp +++ b/bindings/python/src/pipeline/CommonBindings.cpp @@ -5,6 +5,7 @@ // depthai/ #include "depthai/common/CameraBoardSocket.hpp" +#include "depthai/common/M8FsyncRoles.hpp" #include "depthai/common/CameraFeatures.hpp" #include "depthai/common/CameraImageOrientation.hpp" #include "depthai/common/CameraSensorType.hpp" @@ -50,6 +51,7 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { py::class_ quaterniond(m, "Quaterniond", DOC(dai, Quaterniond)); py::class_ size2f(m, "Size2f", DOC(dai, Size2f)); py::enum_ cameraBoardSocket(m, "CameraBoardSocket", DOC(dai, CameraBoardSocket)); + py::enum_ m8FsyncRole(m, "M8FsyncRole", DOC(dai, M8FsyncRole)); py::enum_ connectionInterface(m, "connectionInterface", DOC(dai, ConnectionInterface)); py::enum_ cameraSensorType(m, "CameraSensorType", DOC(dai, CameraSensorType)); py::enum_ cameraImageOrientation(m, "CameraImageOrientation", DOC(dai, CameraImageOrientation)); @@ -221,6 +223,11 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { }); HEDLEY_DIAGNOSTIC_POP + // M8FsyncRole enum bindings + m8FsyncRole.value("AUTO_DETECT", M8FsyncRole::AUTO_DETECT) + .value("MASTER", M8FsyncRole::MASTER) + .value("SLAVE", M8FsyncRole::SLAVE); + // CameraSensorType enum bindings cameraSensorType.value("COLOR", CameraSensorType::COLOR) .value("MONO", CameraSensorType::MONO) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 1e26e9d084..4404ec7097 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+cd0fc231d8c860acf11ab424109d7e42500626ae") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+9c855c6821899797d55e3c7f6efa30bdba0e704f") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index bd1ce8d09e..5048647014 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "0ada9b9800478cf2c7056cea5c2bab2c8f6a2cbd") +set(DEPTHAI_DEVICE_SIDE_COMMIT "46e8c42f3fa78baedd7595a290d197277b7ead5a") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/examples/cpp/Misc/CMakeLists.txt b/examples/cpp/Misc/CMakeLists.txt index 9e42fa359b..4f73dd916e 100644 --- a/examples/cpp/Misc/CMakeLists.txt +++ b/examples/cpp/Misc/CMakeLists.txt @@ -2,4 +2,5 @@ project(misc_examples) cmake_minimum_required(VERSION 3.10) add_subdirectory(AutoReconnect) -add_subdirectory(Projectors) \ No newline at end of file +add_subdirectory(Projectors) +add_subdirectory(MultiDevice) \ No newline at end of file diff --git a/examples/cpp/Misc/MultiDevice/CMakeLists.txt b/examples/cpp/Misc/MultiDevice/CMakeLists.txt new file mode 100644 index 0000000000..cb7996d275 --- /dev/null +++ b/examples/cpp/Misc/MultiDevice/CMakeLists.txt @@ -0,0 +1,7 @@ +project(projectors_examples) +cmake_minimum_required(VERSION 3.10) + +## function: dai_add_example(example_name example_src enable_test use_pcl) +## function: dai_set_example_test_labels(example_name ...) + +dai_add_example(multi_device_m8_fsync multi_device_m8_fsync.cpp OFF OFF) \ No newline at end of file diff --git a/examples/cpp/Misc/MultiDevice/multi_device_m8_fsync.cpp b/examples/cpp/Misc/MultiDevice/multi_device_m8_fsync.cpp new file mode 100644 index 0000000000..09dff72555 --- /dev/null +++ b/examples/cpp/Misc/MultiDevice/multi_device_m8_fsync.cpp @@ -0,0 +1,192 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "depthai/capabilities/ImgFrameCapability.hpp" +#include "depthai/common/CameraBoardSocket.hpp" +#include "depthai/common/CameraExposureOffset.hpp" +#include "depthai/common/M8FsyncRoles.hpp" +#include "depthai/depthai.hpp" +#include "depthai/pipeline/MessageQueue.hpp" +#include "depthai/pipeline/Node.hpp" +#include "depthai/pipeline/datatype/ImgFrame.hpp" +#include "depthai/pipeline/node/Camera.hpp" +#include "depthai/xlink/XLinkConnection.hpp" + +class FPSCounter { + public: + std::vector> frameTimes; + + void tick() { + auto now = std::chrono::steady_clock::now(); + frameTimes.push_back(now); + if (frameTimes.size() > 100) { + frameTimes.erase(frameTimes.begin(), frameTimes.begin() + (frameTimes.size() - 100)); + } + } + + float getFps() { + if (frameTimes.size() <= 1) { + return 0.0f; + } + // Calculate the FPS + return float(frameTimes.size() - 1) * 1e6 / std::chrono::duration_cast(frameTimes.back() - frameTimes.front()).count(); + } +}; + + +int main(int argc, char** argv) { + + if (argc < 4) { + std::cout << "Usage: " << argv[0] << " [device_ip_3] ..." << std::endl; + std::exit(0); + } + + float TARGET_FPS = std::stof(argv[1]); + + std::vector DEVICE_INFOS; + for (int i = 2; i < argc; i++) { + DEVICE_INFOS.emplace_back(std::string(argv[i])); + } + + std::vector> queues; + std::vector master_pipelines; + std::vector slave_pipelines; + std::vector device_ids; + + for (auto deviceInfo : DEVICE_INFOS) + { + auto pipeline = dai::Pipeline(std::make_shared(deviceInfo)); + auto device = pipeline.getDefaultDevice(); + + std::cout << "=== Connected to " << deviceInfo.getDeviceId() << std::endl; + std::cout << " Device ID: " << device->getDeviceId() << std::endl; + std::cout << " Num of cameras: " << device->getConnectedCameras().size() << std::endl; + + // auto socket = device->getConnectedCameras()[0]; + auto socket = device->getConnectedCameras()[1]; + + auto cam = pipeline.create()->build(socket, std::nullopt, TARGET_FPS); + auto out_q = cam->requestOutput(std::make_pair(640, 480), dai::ImgFrame::Type::NV12, dai::ImgResizeMode::STRETCH)->createOutputQueue(); + + auto role = device->getM8FsyncRole(); + + if (role == dai::M8FsyncRole::MASTER) { + master_pipelines.push_back(pipeline); + } else if (role == dai::M8FsyncRole::SLAVE) { + slave_pipelines.push_back(pipeline); + } else { + throw std::runtime_error("Don't know how to handle role " + dai::toString(role)); + } + + queues.push_back(out_q); + device_ids.push_back(deviceInfo.getXLinkDeviceDesc().name); + } + + if (master_pipelines.size() > 1) { + throw std::runtime_error("Multiple masters detected!"); + } + if (master_pipelines.size() == 0) { + throw std::runtime_error("No master detected!"); + } + if (slave_pipelines.size() < 1) { + throw std::runtime_error("No slaves detected!"); + } + for (auto p : master_pipelines) { + p.start(); + } + for (auto p : slave_pipelines) { + p.start(); + } + + std::map> latest_frames; + std::vector receivedFrames; + std::vector fpsCounters(queues.size()); + + for(auto q : queues) { + receivedFrames.push_back(false); + } + + while (true) { + for (int i = 0; i < queues.size(); i++) { + auto q = queues[i]; + while (q->has()) { + latest_frames.emplace(std::make_pair(i,std::dynamic_pointer_cast(q->get()))); + if (!receivedFrames[i]) { + std::cout << "=== Received frame from " << device_ids[i] << std::endl; + receivedFrames[i] = true; + } + fpsCounters[i].tick(); + } + } + + if (latest_frames.size() == queues.size()) { + std::vector> ts_values; + for (auto pair : latest_frames) { + auto f = pair.second; + ts_values.push_back(f->getTimestamp(dai::CameraExposureOffset::END)); + } + + cv::Mat imgs; + for (int i = 0; i < queues.size(); i++) { + auto msg = latest_frames[i]; + auto fps = fpsCounters[i].getFps(); + auto frame = msg->getCvFrame(); + + cv::putText(frame, + device_ids[i] + " | Timestamp: " + std::to_string(ts_values[i].time_since_epoch().count()) + " | FPS: " + std::to_string(fps).substr(0, 5), + {20, 40}, + cv::FONT_HERSHEY_SIMPLEX, + 0.6, + {255, 0, 50}, + 2, + cv::LINE_AA); + + if (i == 0) { + imgs = frame; + } else { + cv::hconcat(frame, imgs, imgs); + } + } + + std::string sync_status = "out of sync"; + if (abs(std::chrono::duration_cast( + *std::max_element(ts_values.begin(), ts_values.end()) - + *std::min_element(ts_values.begin(), ts_values.end()) + ).count()) < 1e3) { + sync_status = "in sync"; + } + auto delta = *std::max_element(ts_values.begin(), ts_values.end()) - *std::min_element(ts_values.begin(), ts_values.end()); + cv::Scalar color = (sync_status == "in sync") ? cv::Scalar(0, 255, 0) : cv::Scalar(0, 0, 255); + + cv::putText(imgs, + sync_status + " | delta = " + std::to_string(1e-3 * float(std::chrono::duration_cast(delta).count())).substr(0, 5) + " ms", + {20, 80}, + cv::FONT_HERSHEY_SIMPLEX, + 0.7, + color, + 2, + cv::LINE_AA); + + cv::imshow("synced_view", imgs); + + latest_frames.clear(); + } + + if (cv::waitKey(1) == 'q') { + break; + } + } + + return 0; +} \ No newline at end of file diff --git a/examples/python/Misc/MultiDevice/multi_device_m8_fsync.py b/examples/python/Misc/MultiDevice/multi_device_m8_fsync.py new file mode 100644 index 0000000000..f01d017812 --- /dev/null +++ b/examples/python/Misc/MultiDevice/multi_device_m8_fsync.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 + +""" +Minimal changes to original script: + * Adds simple timestamp-based synchronisation across multiple devices. + * Presents frames side‑by‑side when they are within 1 / FPS seconds. + * Keeps v3 API usage and overall code structure intact. +""" + +import contextlib +import datetime + +import cv2 +import depthai as dai +import time +import math + +import argparse +# --------------------------------------------------------------------------- +# Configuration +# --------------------------------------------------------------------------- +SET_MANUAL_EXPOSURE = False # Set to True to use manual exposure settings +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- +class FPSCounter: + def __init__(self): + self.frameTimes = [] + + def tick(self): + now = time.time() + self.frameTimes.append(now) + self.frameTimes = self.frameTimes[-100:] + + def getFps(self): + if len(self.frameTimes) <= 1: + return 0 + # Calculate the FPS + return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) + + +def format_time(td: datetime.timedelta) -> str: + hours, remainder_seconds = divmod(td.seconds, 3600) + minutes, seconds = divmod(remainder_seconds, 60) + milliseconds, microseconds_remainder = divmod(td.microseconds, 1000) + days_prefix = f"{td.days} day{'s' if td.days != 1 else ''}, " if td.days else "" + return ( + f"{days_prefix}{hours:02d}:{minutes:02d}:{seconds:02d}." + f"{milliseconds:03d}.{microseconds_remainder:03d}" + ) + + +# --------------------------------------------------------------------------- +# Pipeline creation (unchanged API – only uses TARGET_FPS constant) +# --------------------------------------------------------------------------- +def createPipeline(pipeline: dai.Pipeline, socket: dai.CameraBoardSocket = dai.CameraBoardSocket.CAM_A): + camRgb = ( + pipeline.create(dai.node.Camera) + .build(socket, sensorFps=TARGET_FPS) + ) + output = ( + camRgb.requestOutput( + (640, 480), dai.ImgFrame.Type.NV12, dai.ImgResizeMode.STRETCH + ).createOutputQueue(blocking=False) + ) + if SET_MANUAL_EXPOSURE: + camRgb.initialControl.setManualExposure(1000, 100) + return pipeline, output + + +parser = argparse.ArgumentParser(add_help=False) +parser.add_argument("-d", "--devices", default=[], nargs="+", help="Device IPs") +parser.add_argument("-f", "--fps", type=float, default=30.0, help="Target FPS") +args = parser.parse_args() +DEVICE_INFOS = [dai.DeviceInfo(ip) for ip in args.devices] #The master camera needs to be first here +assert len(DEVICE_INFOS) > 1, "At least two devices are required for this example." + +TARGET_FPS = args.fps # Must match sensorFps in createPipeline() +SYNC_THRESHOLD_SEC = 1.0 / (2 * TARGET_FPS) # Max drift to accept as "in sync" +# --------------------------------------------------------------------------- +# Main +# --------------------------------------------------------------------------- +with contextlib.ExitStack() as stack: + + queues = [] + slave_pipelines = [] + master_pipelines = [] + device_ids = [] + + for idx, deviceInfo in enumerate(DEVICE_INFOS): + pipeline = stack.enter_context(dai.Pipeline(dai.Device(deviceInfo))) + device = pipeline.getDefaultDevice() + + print("=== Connected to", deviceInfo.getDeviceId()) + print(" Device ID:", device.getDeviceId()) + print(" Num of cameras:", len(device.getConnectedCameras())) + + # socket = device.getConnectedCameras()[0] + socket = device.getConnectedCameras()[1] + pipeline, out_q = createPipeline(pipeline, socket) + role = device.getM8FsyncRole() + if (role == dai.M8FsyncRole.MASTER): + master_pipelines.append(pipeline) + elif (role == dai.M8FsyncRole.SLAVE): + slave_pipelines.append(pipeline) + else: + raise RuntimeError(f"Don't know how to handle role {role}") + + queues.append(out_q) + device_ids.append(deviceInfo.getXLinkDeviceDesc().name) + + if (len(master_pipelines) > 1): + raise RuntimeError("Multiple masters detected!") + + if (len(master_pipelines) == 0): + raise RuntimeError("No master detected!") + + if (len(slave_pipelines) < 1): + raise RuntimeError("No slaves detected!") + + for p in master_pipelines: + p.start() + + for p in slave_pipelines: + p.start() + + + # Buffer for latest frames; key = queue index + latest_frames = {} + fpsCounters = [FPSCounter() for _ in queues] + receivedFrames = [False for _ in queues] + while True: + # ------------------------------------------------------------------- + # Collect the newest frame from each queue (non‑blocking) + # ------------------------------------------------------------------- + for idx, q in enumerate(queues): + while q.has(): + latest_frames[idx] = q.get() + if not receivedFrames[idx]: + print("=== Received frame from", device_ids[idx]) + receivedFrames[idx] = True + fpsCounters[idx].tick() + + # ------------------------------------------------------------------- + # Synchronise: we need at least one frame from every camera and their + # timestamps must align within SYNC_THRESHOLD_SEC. + # ------------------------------------------------------------------- + if len(latest_frames) == len(queues): + ts_values = [f.getTimestamp(dai.CameraExposureOffset.END).total_seconds() for f in latest_frames.values()] + # Build composite image side‑by‑side + imgs = [] + for i in range(len(queues)): + msg = latest_frames[i] + frame = msg.getCvFrame() + fps = fpsCounters[i].getFps() + cv2.putText( + frame, + f"{device_ids[i]} | Timestamp: {ts_values[i]} | FPS:{fps:.2f}", + (20, 40), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (255, 0, 50), + 2, + cv2.LINE_AA, + ) + imgs.append(frame) + + sync_status = "in sync" if abs(max(ts_values) - min(ts_values)) < 0.001 else "out of sync" + delta = max(ts_values) - min(ts_values) + color = (0, 255, 0) if sync_status == "in sync" else (0, 0, 255) + + cv2.putText( + imgs[0], + f"{sync_status} | delta = {delta*1e3:.3f} ms", + (20, 80), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + color, + 2, + cv2.LINE_AA, + ) + + cv2.imshow("synced_view", cv2.hconcat(imgs)) + latest_frames.clear() # Wait for next batch + + if cv2.waitKey(1) & 0xFF == ord("q"): + break + +cv2.destroyAllWindows() diff --git a/include/depthai/common/M8FsyncRoles.hpp b/include/depthai/common/M8FsyncRoles.hpp new file mode 100644 index 0000000000..e6ea8ee194 --- /dev/null +++ b/include/depthai/common/M8FsyncRoles.hpp @@ -0,0 +1,45 @@ +#pragma once +#include +#include +#include + +namespace dai { +/** + * Which Fsymc role the device will have. + * + * AUTO_DETECT denotes that the decision will be made by device. + * It will choose between MASTER and SLAVE. + */ +enum class M8FsyncRole : int32_t { + // This role is only used in setM8FsyncRole() function. + // It signals to the device to automatically detect M8 Fsync configuration. + AUTO_DETECT = -1, + + // Fsync master. + // STM generates Fsync signal and outputs it through M8 connector. + MASTER, + + // Fsync slave. + // Device must lock onto an external Fsync signal on the M8 connector. + SLAVE, +}; + +inline std::string toString(M8FsyncRole role) { + switch(role) { + case M8FsyncRole::AUTO_DETECT: + return "AUTO DETECT"; + case M8FsyncRole::MASTER: + return "MASTER"; + case M8FsyncRole::SLAVE: + return "SLAVE"; + default: + return "UNKNOWN"; + } +} + +} // namespace dai + +// Global namespace +inline std::ostream& operator<<(std::ostream& out, const dai::M8FsyncRole& socket) { + return out << dai::toString(socket); +} diff --git a/include/depthai/device/DeviceBase.hpp b/include/depthai/device/DeviceBase.hpp index 3a8afb371c..7916dbbcfc 100644 --- a/include/depthai/device/DeviceBase.hpp +++ b/include/depthai/device/DeviceBase.hpp @@ -17,6 +17,7 @@ // project #include "depthai/common/CameraBoardSocket.hpp" #include "depthai/common/CameraFeatures.hpp" +#include "depthai/common/M8FsyncRoles.hpp" #include "depthai/common/UsbSpeed.hpp" #include "depthai/device/CalibrationHandler.hpp" #include "depthai/device/DeviceGate.hpp" @@ -790,6 +791,19 @@ class DeviceBase { */ void setMaxReconnectionAttempts(int maxAttempts, std::function callBack = nullptr); + /** + * Sets M8 Fsync role for the device + * @param role M8 Fsync role to be set, AUTO_DETECT by default + * @returns Tuple of bool and string. Bool specifies if role was set without failures. String is the error message describing the failure reason. + */ + std::tuple setM8FsyncRole(M8FsyncRole role = M8FsyncRole::AUTO_DETECT); + + /** + * Gets M8 Fsync role for the device + * @returns Gets M8 Fsync role + */ + M8FsyncRole getM8FsyncRole(); + protected: std::shared_ptr connection; diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp index 480eb8ae05..e7d3a8a929 100644 --- a/src/device/DeviceBase.cpp +++ b/src/device/DeviceBase.cpp @@ -42,6 +42,7 @@ #include "XLink/XLinkTime.h" #include "nanorpc/core/client.h" #include "nanorpc/packer/nlohmann_msgpack.h" +#include "nanorpc/core/hash.h" #include "spdlog/details/os.h" #include "spdlog/fmt/bin_to_hex.h" #include "spdlog/fmt/chrono.h" @@ -60,6 +61,9 @@ constexpr int DEVICE_SEARCH_FIRST_TIMEOUT_MS = 30; const unsigned int DEFAULT_CRASHDUMP_TIMEOUT_MS = 9000; const unsigned int RPC_READ_TIMEOUT = 10000; +const unsigned int WAIT_FOR_DEVICE_READY_RPC_READ_TIMEOUT = 60000; + +std::string const WAIT_FOR_DEVICE_READY_RPC_NAME = "waitForDeviceReady"; // local static function static void getFlashingPermissions(bool& factoryPermissions, bool& protectedPermissions) { @@ -762,10 +766,18 @@ void DeviceBase::init2(Config cfg, const std::filesystem::path& pathToMvcmd, boo try { // Send request to device rpcStream->write(std::move(request)); + + auto id = nlohmann::json::from_msgpack(request).at(2).get(); + + unsigned int timeout = RPC_READ_TIMEOUT; + + if (id == nanorpc::core::hash_id(WAIT_FOR_DEVICE_READY_RPC_NAME)) { + timeout = WAIT_FOR_DEVICE_READY_RPC_READ_TIMEOUT; + } // Receive response back // Send to nanorpc to parse - return rpcStream->read(std::chrono::milliseconds(RPC_READ_TIMEOUT)); + return rpcStream->read(std::chrono::milliseconds(timeout)); } catch(const std::exception& e) { // If any exception is thrown, log it and rethrow pimpl->logger.debug("RPC error: {}", e.what()); @@ -1178,6 +1190,14 @@ void DeviceBase::crashDevice() { } } +std::tuple DeviceBase::setM8FsyncRole(M8FsyncRole role) { + return pimpl->rpcClient->call("setM8FsyncRole", role); +} + +M8FsyncRole DeviceBase::getM8FsyncRole() { + return pimpl->rpcClient->call("getM8FsyncRole"); +} + dai::Version DeviceBase::getIMUFirmwareVersion() { isClosed(); std::string versionStr = pimpl->rpcClient->call("getIMUFirmwareVersion").as(); @@ -1619,14 +1639,23 @@ bool DeviceBase::startPipelineImpl(const Pipeline& pipeline) { logCollection::logPipeline(schema, deviceInfo); this->pipelineSchema = schema; // Save the schema so it can be saved alongside the crashdump - // Build and start the pipeline bool success = false; std::string errorMsg; + + // Initialize the device (M8 Fsync slaves need to lock onto the signal first) + std::tie(success, errorMsg) = pimpl->rpcClient->call(WAIT_FOR_DEVICE_READY_RPC_NAME).as>(); + + if (!success) { + throw std::runtime_error("Device " + getDeviceId() + " not ready: " + errorMsg); + } + + // Build and start the pipeline std::tie(success, errorMsg) = pimpl->rpcClient->call("buildPipeline").as>(); + if(success) { pimpl->rpcClient->call("startPipeline"); } else { - throw std::runtime_error(errorMsg); + throw std::runtime_error("Device " + getDeviceId() + " error: " + errorMsg); return false; }