From fc1e57716989e03b5ec99d69dff760907baaa1b0 Mon Sep 17 00:00:00 2001 From: Eran Date: Tue, 3 Jan 2023 09:05:13 +0200 Subject: [PATCH 1/4] remove tm2 - 1 --- CMake/android_config.cmake | 1 - CMake/global_config.cmake | 9 - CMake/lrs_options.cmake | 1 - CMake/unix_config.cmake | 1 - CMakeLists.txt | 10 - common/fw/CMakeLists.txt | 10 +- common/model-views.cpp | 87 +- common/rendering.h | 18 +- common/viewer.cpp | 2 +- include/librealsense2/hpp/rs_device.hpp | 134 -- src/CMakeLists.txt | 4 - src/backend.h | 18 - src/context.cpp | 17 - src/context.h | 4 - src/core/motion.h | 23 - src/linux/backend-v4l2.cpp | 7 - src/mf/mf-backend.cpp | 8 - src/rs.cpp | 98 +- src/rsusb-backend/rsusb-backend.cpp | 8 - src/tm2/CMakeLists.txt | 21 - src/tm2/message-print.h | 117 -- src/tm2/t265-messages.h | 1532 ---------------- src/tm2/tm-boot.h | 54 - src/tm2/tm-device.cpp | 2170 ----------------------- src/tm2/tm-device.h | 219 --- src/tm2/tm-info.cpp | 61 - src/tm2/tm-info.h | 27 - src/win/win-helpers.cpp | 2 - wrappers/python/pyrs_device.cpp | 17 - 29 files changed, 32 insertions(+), 4648 deletions(-) delete mode 100644 src/tm2/CMakeLists.txt delete mode 100644 src/tm2/message-print.h delete mode 100644 src/tm2/t265-messages.h delete mode 100644 src/tm2/tm-boot.h delete mode 100644 src/tm2/tm-device.cpp delete mode 100644 src/tm2/tm-device.h delete mode 100644 src/tm2/tm-info.cpp delete mode 100644 src/tm2/tm-info.h diff --git a/CMake/android_config.cmake b/CMake/android_config.cmake index 9f734f9572..3cc1d1f88e 100644 --- a/CMake/android_config.cmake +++ b/CMake/android_config.cmake @@ -5,7 +5,6 @@ macro(os_set_flags) unset(WIN32) unset(UNIX) unset(APPLE) - set(BUILD_WITH_TM2 OFF) set(BUILD_UNIT_TESTS OFF) set(BUILD_LEGACY_LIVE_TEST OFF) set(BUILD_EXAMPLES OFF) diff --git a/CMake/global_config.cmake b/CMake/global_config.cmake index dab5c707a4..4e13dc2bbd 100644 --- a/CMake/global_config.cmake +++ b/CMake/global_config.cmake @@ -116,12 +116,3 @@ macro(global_target_config) endmacro() -macro(add_tm2) - message(STATUS "Building with TM2") - include(libusb_config) - target_link_libraries(${LRS_TARGET} PRIVATE usb) - if(USE_EXTERNAL_USB) - add_dependencies(${LRS_TARGET} libusb) - endif() - target_compile_definitions(${LRS_TARGET} PRIVATE WITH_TRACKING=1) -endmacro() diff --git a/CMake/lrs_options.cmake b/CMake/lrs_options.cmake index 0c088e9cfe..36d108ee8a 100644 --- a/CMake/lrs_options.cmake +++ b/CMake/lrs_options.cmake @@ -2,7 +2,6 @@ option(ENABLE_CCACHE "Build with ccache." ON) option(BUILD_WITH_CUDA "Enable CUDA" OFF) option(BUILD_GLSL_EXTENSIONS "Build GLSL extensions API" ON) option(BUILD_WITH_OPENMP "Use OpenMP" OFF) -option(BUILD_WITH_TM2 "Build with support for Intel TM2 tracking device" ON) option(BUILD_EASYLOGGINGPP "Build EasyLogging++ as a part of the build" ON) option(BUILD_WITH_STATIC_CRT "Build with static link CRT" ON) option(HWM_OVER_XU "Send HWM commands over UVC XU control" ON) diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index 13ba651157..cb52e7f493 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -42,7 +42,6 @@ macro(os_set_flags) if(APPLE) set(FORCE_RSUSB_BACKEND ON) - set(BUILD_WITH_TM2 ON) endif() if(FORCE_RSUSB_BACKEND) diff --git a/CMakeLists.txt b/CMakeLists.txt index 32d911f24d..3465754a0d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,12 +14,6 @@ if(${FORCE_LIBUVC} OR ${FORCE_WINUSB_UVC} OR ${ANDROID_USB_HOST_UVC}) set(FORCE_RSUSB_BACKEND ON) endif() -# Checking Internet connection, as TM2 needs to download the FW from amazon cloud -if(BUILD_WITH_TM2 AND NOT INTERNET_CONNECTION) - message(WARNING "No internet connection, disabling BUILD_WITH_TM2") - set(BUILD_WITH_TM2 OFF) -endif() - # Checking Internet connection, as DEPTH CAM needs to download the FW from amazon cloud if(IMPORT_DEPTH_CAM_FW AND NOT INTERNET_CONNECTION) message(WARNING "No internet connection, disabling IMPORT_DEPTH_CAM_FW") @@ -104,10 +98,6 @@ if(BUILD_NETWORK_DEVICE) add_subdirectory(src/compression) endif() -if(BUILD_WITH_TM2) - add_tm2() -endif() - if(IMPORT_DEPTH_CAM_FW) add_subdirectory(common/fw) endif() diff --git a/common/fw/CMakeLists.txt b/common/fw/CMakeLists.txt index 9696eb980d..a1482ee7e6 100644 --- a/common/fw/CMakeLists.txt +++ b/common/fw/CMakeLists.txt @@ -17,19 +17,12 @@ message(STATUS "D4XX_FW_VERSION: ${D4XX_FW_VERSION}") set(D4XX_FW_SHA1 8dc3b8a0b9fdf03410c15b30e04f57701985064a) set(D4XX_FW_URL "${REALSENSE_FIRMWARE_URL}/Releases/RS4xx/FW") - string(REGEX MATCH "SR3XX_RECOMMENDED_FIRMWARE_VERSION \"([0-9]+.[0-9]+.[0-9]+.[0-9]+)\"" _ ${ver}) set(SR3XX_FW_VERSION ${CMAKE_MATCH_1}) message(STATUS "SR3XX_FW_VERSION: ${SR3XX_FW_VERSION}") set(SR3XX_FW_SHA1 55237dba5d7db20e7c218975375d05b4210e9460) set(SR3XX_FW_URL "${REALSENSE_FIRMWARE_URL}/Releases/SR300/FW") -string(REGEX MATCH "T26X_FIRMWARE_VERSION \"([0-9]+.[0-9]+.[0-9]+.[0-9]+)\"" _ ${ver}) -set(T26X_FW_VERSION ${CMAKE_MATCH_1}) -message(STATUS "T26X_FW_VERSION: ${T26X_FW_VERSION}") -set(T26X_FW_SHA1 c3940ccbb0e3045603e4aceaa2d73427f96e24bc) -set(T26X_FW_URL "${REALSENSE_FIRMWARE_URL}/Releases/TM2/FW/target/${T26X_FW_VERSION}") - string(REGEX MATCH "L51X_RECOMMENDED_FIRMWARE_VERSION \"([0-9]+\.[0-9]+\.[0-9]+\.[0-9]+)\"" _ ${ver}) set(L51X_FW_VERSION ${CMAKE_MATCH_1}) message(STATUS "L51X_FW_VERSION: ${L51X_FW_VERSION}") @@ -86,12 +79,11 @@ endfunction() target_binary( "${D4XX_FW_URL}" "${D4XX_FW_VERSION}" "${D4XX_FW_SHA1}" D4XX_FW_Image .bin) target_binary( "${SR3XX_FW_URL}" "${SR3XX_FW_VERSION}" "${SR3XX_FW_SHA1}" SR3XX_FW_Image .bin) -target_binary( "${T26X_FW_URL}" "${T26X_FW_VERSION}" "${T26X_FW_SHA1}" target .mvcmd) target_binary( "${L51X_FW_URL}" "${L51X_FW_VERSION}" "${L51X_FW_SHA1}" L51X_FW_Image .bin) - install(TARGETS ${PROJECT_NAME} EXPORT realsense2Targets RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) + diff --git a/common/model-views.cpp b/common/model-views.cpp index cccc50b857..9971cf3512 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -4112,7 +4112,7 @@ namespace rs2 fps = s.second.profile.fps(); } auto curr_frame = p.get_position(); - uint64_t step = fps ? uint64_t(1000.0 / (float)fps * 1e6) : 1e6; + uint64_t step = fps ? uint64_t(1000.0 / (float)fps * 1e6) : 1000000ULL; if (curr_frame >= step) { p.seek(std::chrono::nanoseconds(curr_frame - step)); @@ -4215,7 +4215,7 @@ namespace rs2 fps = s.second.profile.fps(); } auto curr_frame = p.get_position(); - uint64_t step = fps ? uint64_t(1000.0 / (float)fps * 1e6) : 1e6; + uint64_t step = fps ? uint64_t(1000.0 / (float)fps * 1e6) : 1000000ULL; p.seek(std::chrono::nanoseconds(curr_frame + step)); } if (ImGui::IsItemHovered()) @@ -5039,87 +5039,6 @@ namespace rs2 bool something_to_show = false; ImGui::PushStyleColor(ImGuiCol_Text, dark_grey); - if (auto tm2_extensions = dev.as()) - { - something_to_show = true; - try - { - if (!tm2_extensions.is_loopback_enabled() && ImGui::Selectable("Enable loopback...", false, is_streaming ? ImGuiSelectableFlags_Disabled : 0)) - { - if (const char* ret = file_dialog_open(file_dialog_mode::open_file, "ROS-bag\0*.bag\0", NULL, NULL)) - { - tm2_extensions.enable_loopback(ret); - } - } - if (tm2_extensions.is_loopback_enabled() && ImGui::Selectable("Disable loopback...", false, is_streaming ? ImGuiSelectableFlags_Disabled : 0)) - { - tm2_extensions.disable_loopback(); - } - if (ImGui::IsItemHovered()) - { - if (is_streaming) - ImGui::SetTooltip("Stop streaming to use loopback functionality"); - else - ImGui::SetTooltip("Enter the device to loopback mode (inject frames from file to FW)"); - } - - if (auto tm_sensor = dev.first()) - { - if (ImGui::Selectable("Export Localization map")) - { - if (auto target_path = file_dialog_open(save_file, "Tracking device Localization map (RAW)\0*.map\0", NULL, NULL)) - { - error_message = safe_call([&]() - { - std::stringstream ss; - ss << "Exporting localization map to " << target_path << " ... "; - viewer.not_model->add_log(ss.str()); - bin_file_from_bytes(target_path, tm_sensor.export_localization_map()); - ss.clear(); - ss << "completed"; - viewer.not_model->add_log(ss.str()); - }); - } - } - - if (ImGui::IsItemHovered()) - { - ImGui::SetTooltip("Retrieve the localization map from device"); - } - - if (ImGui::Selectable("Import Localization map", false, is_streaming ? ImGuiSelectableFlags_Disabled : 0)) - { - if (auto source_path = file_dialog_open(open_file, "Tracking device Localization map (RAW)\0*.map\0", NULL, NULL)) - { - error_message = safe_call([&]() - { - std::stringstream ss; - ss << "Importing localization map from " << source_path << " ... "; - tm_sensor.import_localization_map(bytes_from_bin_file(source_path)); - ss << "completed"; - viewer.not_model->add_log(ss.str()); - }); - } - } - - if (ImGui::IsItemHovered()) - { - if (is_streaming) - ImGui::SetTooltip("Stop streaming to Import localization map"); - else - ImGui::SetTooltip("Load localization map from host to device"); - } - } - } - catch (const rs2::error& e) - { - error_message = error_to_string(e); - } - catch (const std::exception& e) - { - error_message = e.what(); - } - } if (_allow_remove) { @@ -7173,7 +7092,7 @@ namespace rs2 reset_trajectory(); rs2_pose pose_data = const_cast(pose).get_pose_data(); - auto t = tm2_pose_to_world_transformation(pose_data); + auto t = pose_to_world_transformation(pose_data); float model[4][4]; t.to_column_major((float*)model); diff --git a/common/rendering.h b/common/rendering.h index 37d1e0704f..35ac614eb4 100644 --- a/common/rendering.h +++ b/common/rendering.h @@ -197,31 +197,31 @@ namespace rs2 return { a * b.x, a * b.y }; } - inline matrix4 tm2_pose_to_world_transformation(const rs2_pose& pose) + inline matrix4 pose_to_world_transformation(const rs2_pose& pose) { matrix4 rotation(pose.rotation); matrix4 translation(pose.translation); - matrix4 G_tm2_body_to_tm2_world = translation * rotation; + matrix4 G_body_to_world = translation * rotation; float rotate_180_y[4][4] = { { -1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0,-1, 0 }, { 0, 0, 0, 1 } }; - matrix4 G_vr_body_to_tm2_body(rotate_180_y); - matrix4 G_vr_body_to_tm2_world = G_tm2_body_to_tm2_world * G_vr_body_to_tm2_body; + matrix4 G_vr_body_to_body(rotate_180_y); + matrix4 G_vr_body_to_world = G_body_to_world * G_vr_body_to_body; float rotate_90_x[4][4] = { { 1, 0, 0, 0 }, { 0, 0,-1, 0 }, { 0, 1, 0, 0 }, { 0, 0, 0, 1 } }; - matrix4 G_tm2_world_to_vr_world(rotate_90_x); - matrix4 G_vr_body_to_vr_world = G_tm2_world_to_vr_world * G_vr_body_to_tm2_world; + matrix4 G_world_to_vr_world(rotate_90_x); + matrix4 G_vr_body_to_vr_world = G_world_to_vr_world * G_vr_body_to_world; return G_vr_body_to_vr_world; } - inline rs2_pose correct_tm2_pose(const rs2_pose& pose) + inline rs2_pose correct_pose(const rs2_pose& pose) { - matrix4 G_vr_body_to_vr_world = tm2_pose_to_world_transformation(pose); + matrix4 G_vr_body_to_vr_world = pose_to_world_transformation(pose); rs2_pose res = pose; res.translation.x = G_vr_body_to_vr_world.mat[0][3]; res.translation.y = G_vr_body_to_vr_world.mat[1][3]; @@ -949,7 +949,7 @@ namespace rs2 draw_axes(0.3f, 2.f); // Drawing pose: - matrix4 pose_trans = tm2_pose_to_world_transformation(pose); + matrix4 pose_trans = pose_to_world_transformation(pose); float model[16]; pose_trans.to_column_major(model); diff --git a/common/viewer.cpp b/common/viewer.cpp index 5babb19dd9..7b6f02b2f3 100644 --- a/common/viewer.cpp +++ b/common/viewer.cpp @@ -2087,7 +2087,7 @@ namespace rs2 pose = f; rs2_pose pose_data = pose.get_pose_data(); - auto t = tm2_pose_to_world_transformation(pose_data); + auto t = pose_to_world_transformation(pose_data); float model[4][4]; t.to_column_major((float*)model); auto m = model; diff --git a/include/librealsense2/hpp/rs_device.hpp b/include/librealsense2/hpp/rs_device.hpp index de9616a88e..a0c8d60c8d 100644 --- a/include/librealsense2/hpp/rs_device.hpp +++ b/include/librealsense2/hpp/rs_device.hpp @@ -1058,139 +1058,5 @@ namespace rs2 private: std::shared_ptr _list; }; - - /** - * The tm2 class is an interface for T2XX devices, such as T265. - * - * For T265, it provides RS2_STREAM_FISHEYE (2), RS2_STREAM_GYRO, RS2_STREAM_ACCEL, and RS2_STREAM_POSE streams, - * and contains the following sensors: - * - * - pose_sensor: map and relocalization functions. - * - wheel_odometer: input for odometry data. - */ - class tm2 : public calibrated_device // TODO: add to wrappers [Python done] - { - public: - tm2(device d) - : calibrated_device(d) - { - rs2_error* e = nullptr; - if (rs2_is_device_extendable_to(_dev.get(), RS2_EXTENSION_TM2, &e) == 0 && !e) - { - _dev.reset(); - } - error::handle(e); - } - - /** - * Enter the given device into loopback operation mode that uses the given file as input for raw data - * \param[in] from_file Path to bag file with raw data for loopback - */ - void enable_loopback(const std::string& from_file) - { - rs2_error* e = nullptr; - rs2_loopback_enable(_dev.get(), from_file.c_str(), &e); - error::handle(e); - } - - /** - * Restores the given device into normal operation mode - */ - void disable_loopback() - { - rs2_error* e = nullptr; - rs2_loopback_disable(_dev.get(), &e); - error::handle(e); - } - - /** - * Checks if the device is in loopback mode or not - * \return true if the device is in loopback operation mode - */ - bool is_loopback_enabled() const - { - rs2_error* e = nullptr; - int is_enabled = rs2_loopback_is_enabled(_dev.get(), &e); - error::handle(e); - return is_enabled != 0; - } - - /** - * Connects to a given tm2 controller - * \param[in] mac_addr The MAC address of the desired controller - */ - void connect_controller(const std::array& mac_addr) - { - rs2_error* e = nullptr; - rs2_connect_tm2_controller(_dev.get(), mac_addr.data(), &e); - error::handle(e); - } - - /** - * Disconnects a given tm2 controller - * \param[in] id The ID of the desired controller - */ - void disconnect_controller(int id) - { - rs2_error* e = nullptr; - rs2_disconnect_tm2_controller(_dev.get(), id, &e); - error::handle(e); - } - - /** - * Set tm2 camera intrinsics - * \param[in] fisheye_senor_id The ID of the fisheye sensor - * \param[in] intrinsics value to be written to the device - */ - void set_intrinsics(int fisheye_sensor_id, const rs2_intrinsics& intrinsics) - { - rs2_error* e = nullptr; - auto fisheye_sensor = get_sensor_profile(RS2_STREAM_FISHEYE, fisheye_sensor_id); - rs2_set_intrinsics(fisheye_sensor.first.get().get(), fisheye_sensor.second.get(), &intrinsics, &e); - error::handle(e); - } - - /** - * Set tm2 camera extrinsics - * \param[in] from_stream only support RS2_STREAM_FISHEYE - * \param[in] from_id only support left fisheye = 1 - * \param[in] to_stream only support RS2_STREAM_FISHEYE - * \param[in] to_id only support right fisheye = 2 - * \param[in] extrinsics extrinsics value to be written to the device - */ - void set_extrinsics(rs2_stream from_stream, int from_id, rs2_stream to_stream, int to_id, rs2_extrinsics& extrinsics) - { - rs2_error* e = nullptr; - auto from_sensor = get_sensor_profile(from_stream, from_id); - auto to_sensor = get_sensor_profile(to_stream, to_id); - rs2_set_extrinsics(from_sensor.first.get().get(), from_sensor.second.get(), to_sensor.first.get().get(), to_sensor.second.get(), &extrinsics, &e); - error::handle(e); - } - - /** - * Set tm2 motion device intrinsics - * \param[in] stream_type stream type of the motion device - * \param[in] motion_intriniscs intrinsics value to be written to the device - */ - void set_motion_device_intrinsics(rs2_stream stream_type, const rs2_motion_device_intrinsic& motion_intriniscs) - { - rs2_error* e = nullptr; - auto motion_sensor = get_sensor_profile(stream_type, 0); - rs2_set_motion_device_intrinsics(motion_sensor.first.get().get(), motion_sensor.second.get(), &motion_intriniscs, &e); - error::handle(e); - } - - private: - - std::pair get_sensor_profile(rs2_stream stream_type, int stream_index) { - for (auto s : query_sensors()) { - for (auto p : s.get_stream_profiles()) { - if (p.stream_type() == stream_type && p.stream_index() == stream_index) - return std::pair(s, p); - } - } - return std::pair(); - } - }; } #endif // LIBREALSENSE_RS2_DEVICE_HPP diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 5dd00551ee..91775ffbf9 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -53,10 +53,6 @@ if(${BACKEND} STREQUAL RS2_USE_LIBUVC_BACKEND) include(${_rel_path}/libuvc/CMakeLists.txt) endif() -if (BUILD_WITH_TM2) - include(${_rel_path}/tm2/CMakeLists.txt) -endif() - if(BUILD_WITH_CUDA) include(${_rel_path}/cuda/CMakeLists.txt) endif() diff --git a/src/backend.h b/src/backend.h index f722f7a719..ae57021b6b 100644 --- a/src/backend.h +++ b/src/backend.h @@ -261,24 +261,6 @@ namespace librealsense return (a.file_path == b.file_path); } - struct tm2_device_info - { - void* device_ptr; - - operator std::string() const - { - std::ostringstream oss; - oss << device_ptr; - return oss.str(); - } - }; - - inline bool operator==(const tm2_device_info& a, - const tm2_device_info& b) - { - return (a.device_ptr == b.device_ptr); - } - struct hid_sensor { std::string name; diff --git a/src/context.cpp b/src/context.cpp index e66d042c02..63af0a92ea 100644 --- a/src/context.cpp +++ b/src/context.cpp @@ -21,10 +21,6 @@ #include "context.h" #include "fw-update/fw-update-factory.h" -#ifdef WITH_TRACKING -#include "tm2/tm-info.h" -#endif - template struct seq{}; template struct gen_seq : gen_seq{}; @@ -353,13 +349,6 @@ namespace librealsense std::copy(begin(sr300_devices), end(sr300_devices), std::back_inserter(list)); } -#ifdef WITH_TRACKING - if (mask & RS2_PRODUCT_LINE_T200) - { - auto tm2_devices = tm2_info::pick_tm2_devices(ctx, devices.usb_devices); - std::copy(begin(tm2_devices), end(tm2_devices), std::back_inserter(list)); - } -#endif // Supported recovery devices if (mask & RS2_PRODUCT_LINE_D400 || mask & RS2_PRODUCT_LINE_SR300 || mask & RS2_PRODUCT_LINE_L500) { @@ -595,12 +584,6 @@ namespace librealsense on_device_changed({},{}, prev_playback_devices, _playback_devices); } -#if WITH_TRACKING - void context::unload_tracking_module() - { - } -#endif - std::vector> group_devices_by_unique_id(const std::vector& devices) { std::map> map; diff --git a/src/context.h b/src/context.h index 1945a2dc5f..43ed06d8ed 100644 --- a/src/context.h +++ b/src/context.h @@ -129,10 +129,6 @@ namespace librealsense void add_software_device(std::shared_ptr software_device); -#if WITH_TRACKING - void unload_tracking_module(); -#endif - private: void on_device_changed(platform::backend_device_group old, platform::backend_device_group curr, diff --git a/src/core/motion.h b/src/core/motion.h index d19e529b4c..92348a760d 100644 --- a/src/core/motion.h +++ b/src/core/motion.h @@ -43,27 +43,4 @@ namespace librealsense virtual ~wheel_odometry_interface() = default; }; MAP_EXTENSION(RS2_EXTENSION_WHEEL_ODOMETER, librealsense::wheel_odometry_interface); - - class tm2_extensions - { - public: - virtual void enable_loopback(const std::string& input) = 0; - virtual void disable_loopback() = 0; - virtual bool is_enabled() const = 0; - virtual void connect_controller(const std::array& mac_address) = 0; - virtual void disconnect_controller(int id) = 0; - virtual ~tm2_extensions() = default; - }; - MAP_EXTENSION(RS2_EXTENSION_TM2, librealsense::tm2_extensions); - - class tm2_sensor_interface - { - public: - virtual void set_intrinsics(const stream_profile_interface& stream_profile, const rs2_intrinsics& intr) = 0; - virtual void set_extrinsics(const stream_profile_interface& from_profile, const stream_profile_interface& to_profile, const rs2_extrinsics& extr) = 0; - virtual void set_motion_device_intrinsics(const stream_profile_interface& stream_profile, const rs2_motion_device_intrinsic& intr) = 0; - virtual void reset_to_factory_calibration() = 0; - virtual void write_calibration() = 0; - }; - MAP_EXTENSION(RS2_EXTENSION_TM2_SENSOR, librealsense::tm2_sensor_interface); } diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index e8ee90a9ab..6376e66693 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -60,7 +60,6 @@ const size_t MAX_DEV_PARENT_DIR = 10; const double DEFAULT_KPI_FRAME_DROPS_PERCENTAGE = 0.05; -#include "../tm2/tm-boot.h" //D457 Dev. TODO -shall be refactored into the kernel headers. constexpr uint32_t RS_STREAM_CONFIG_0 = 0x4000; constexpr uint32_t RS_CAMERA_CID_BASE = (V4L2_CTRL_CLASS_CAMERA | RS_STREAM_CONFIG_0); @@ -2513,12 +2512,6 @@ namespace librealsense std::vector v4l_backend::query_usb_devices() const { auto device_infos = usb_enumerator::query_devices_info(); - // Give the device a chance to restart, if we don't catch - // it, the watcher will find it later. - if(tm_boot(device_infos)) { - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - device_infos = usb_enumerator::query_devices_info(); - } return device_infos; } diff --git a/src/mf/mf-backend.cpp b/src/mf/mf-backend.cpp index 5bac7554f2..6befc3015f 100644 --- a/src/mf/mf-backend.cpp +++ b/src/mf/mf-backend.cpp @@ -16,8 +16,6 @@ #include #include // std::tolower -#include "../tm2/tm-boot.h" - namespace { @@ -131,12 +129,6 @@ namespace librealsense std::vector wmf_backend::query_usb_devices() const { auto device_infos = usb_enumerator::query_devices_info(); - // Give the device a chance to restart, if we don't catch - // it, the watcher will find it later. - if(tm_boot(device_infos)) { - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - device_infos = usb_enumerator::query_devices_info(); - } return device_infos; } diff --git a/src/rs.cpp b/src/rs.cpp index 5304b32e0d..d3e78d4c99 100644 --- a/src/rs.cpp +++ b/src/rs.cpp @@ -1441,7 +1441,7 @@ int rs2_is_sensor_extendable_to(const rs2_sensor* sensor, rs2_extension extensio case RS2_EXTENSION_SOFTWARE_SENSOR : return VALIDATE_INTERFACE_NO_THROW(sensor->sensor, librealsense::software_sensor) != nullptr; case RS2_EXTENSION_POSE_SENSOR : return VALIDATE_INTERFACE_NO_THROW(sensor->sensor, librealsense::pose_sensor_interface) != nullptr; case RS2_EXTENSION_WHEEL_ODOMETER : return VALIDATE_INTERFACE_NO_THROW(sensor->sensor, librealsense::wheel_odometry_interface)!= nullptr; - case RS2_EXTENSION_TM2_SENSOR : return VALIDATE_INTERFACE_NO_THROW(sensor->sensor, librealsense::tm2_sensor_interface) != nullptr; + case RS2_EXTENSION_TM2_SENSOR : return false; case RS2_EXTENSION_COLOR_SENSOR : return VALIDATE_INTERFACE_NO_THROW(sensor->sensor, librealsense::color_sensor) != nullptr; case RS2_EXTENSION_MOTION_SENSOR : return VALIDATE_INTERFACE_NO_THROW(sensor->sensor, librealsense::motion_sensor) != nullptr; case RS2_EXTENSION_FISHEYE_SENSOR : return VALIDATE_INTERFACE_NO_THROW(sensor->sensor, librealsense::fisheye_sensor) != nullptr; @@ -1475,7 +1475,7 @@ int rs2_is_device_extendable_to(const rs2_device* dev, rs2_extension extension, case RS2_EXTENSION_ADVANCED_MODE : return VALIDATE_INTERFACE_NO_THROW(dev->device, librealsense::ds_advanced_mode_interface) != nullptr; case RS2_EXTENSION_RECORD : return VALIDATE_INTERFACE_NO_THROW(dev->device, librealsense::record_device) != nullptr; case RS2_EXTENSION_PLAYBACK : return VALIDATE_INTERFACE_NO_THROW(dev->device, librealsense::playback_device) != nullptr; - case RS2_EXTENSION_TM2 : return VALIDATE_INTERFACE_NO_THROW(dev->device, librealsense::tm2_extensions) != nullptr; + case RS2_EXTENSION_TM2 : return false; case RS2_EXTENSION_UPDATABLE : return VALIDATE_INTERFACE_NO_THROW(dev->device, librealsense::updatable) != nullptr; case RS2_EXTENSION_UPDATE_DEVICE : return VALIDATE_INTERFACE_NO_THROW(dev->device, librealsense::update_device_interface) != nullptr; case RS2_EXTENSION_GLOBAL_TIMER : return VALIDATE_INTERFACE_NO_THROW(dev->device, librealsense::global_time_interface) != nullptr; @@ -1580,10 +1580,6 @@ HANDLE_EXCEPTIONS_AND_RETURN(, ctx, file) void rs2_context_unload_tracking_module(rs2_context* ctx, rs2_error** error) BEGIN_API_CALL { -#if WITH_TRACKING - VALIDATE_NOT_NULL(ctx); - ctx->ctx->unload_tracking_module(); -#endif } HANDLE_EXCEPTIONS_AND_RETURN(, ctx) @@ -2727,60 +2723,37 @@ HANDLE_EXCEPTIONS_AND_RETURN(, severity, message) void rs2_loopback_enable(const rs2_device* device, const char* from_file, rs2_error** error) BEGIN_API_CALL { - VALIDATE_NOT_NULL(device); - VALIDATE_NOT_NULL(from_file); - - auto loopback = VALIDATE_INTERFACE(device->device, librealsense::tm2_extensions); - loopback->enable_loopback(from_file); - + throw not_implemented_exception( "deprecated" ); } HANDLE_EXCEPTIONS_AND_RETURN(, device, from_file) void rs2_loopback_disable(const rs2_device* device, rs2_error** error) BEGIN_API_CALL { - VALIDATE_NOT_NULL(device); - - auto loopback = VALIDATE_INTERFACE(device->device, librealsense::tm2_extensions); - loopback->disable_loopback(); + throw not_implemented_exception( "deprecated" ); } HANDLE_EXCEPTIONS_AND_RETURN(, device) int rs2_loopback_is_enabled(const rs2_device* device, rs2_error** error) BEGIN_API_CALL { - VALIDATE_NOT_NULL(device); - - auto loopback = VALIDATE_INTERFACE(device->device, librealsense::tm2_extensions); - return loopback->is_enabled() ? 1 : 0; + throw not_implemented_exception( "deprecated" ); } HANDLE_EXCEPTIONS_AND_RETURN(0, device) void rs2_connect_tm2_controller(const rs2_device* device, const unsigned char* mac, rs2_error** error) BEGIN_API_CALL { - VALIDATE_NOT_NULL(device); - VALIDATE_NOT_NULL(mac); - - auto tm2 = VALIDATE_INTERFACE(device->device, librealsense::tm2_extensions); - tm2->connect_controller({ mac[0], mac[1], mac[2], mac[3], mac[4], mac[5] }); + throw not_implemented_exception( "deprecated" ); } HANDLE_EXCEPTIONS_AND_RETURN(, device) void rs2_disconnect_tm2_controller(const rs2_device* device, int id, rs2_error** error) BEGIN_API_CALL { - VALIDATE_NOT_NULL(device); - - auto tm2 = VALIDATE_INTERFACE(device->device, librealsense::tm2_extensions); - tm2->disconnect_controller(id); + throw not_implemented_exception( "deprecated" ); } HANDLE_EXCEPTIONS_AND_RETURN(, device) void rs2_set_intrinsics(const rs2_sensor* sensor, const rs2_stream_profile* profile, const rs2_intrinsics* intrinsics, rs2_error** error) BEGIN_API_CALL { - VALIDATE_NOT_NULL(sensor); - VALIDATE_NOT_NULL(profile); - VALIDATE_NOT_NULL(intrinsics); - - auto tm2 = VALIDATE_INTERFACE(sensor->sensor, librealsense::tm2_sensor_interface); - tm2->set_intrinsics(*profile->profile, *intrinsics); + throw not_implemented_exception( "deprecated" ); } HANDLE_EXCEPTIONS_AND_RETURN(, sensor, profile, intrinsics) @@ -2796,68 +2769,33 @@ HANDLE_EXCEPTIONS_AND_RETURN( , sensor, intrinsics ) void rs2_set_extrinsics(const rs2_sensor* from_sensor, const rs2_stream_profile* from_profile, rs2_sensor* to_sensor, const rs2_stream_profile* to_profile, const rs2_extrinsics* extrinsics, rs2_error** error) BEGIN_API_CALL { - VALIDATE_NOT_NULL(from_sensor); - VALIDATE_NOT_NULL(from_profile); - VALIDATE_NOT_NULL(to_sensor); - VALIDATE_NOT_NULL(to_profile); - VALIDATE_NOT_NULL(extrinsics); - - auto from_dev = from_sensor->parent.device; - if (!from_dev) from_dev = from_sensor->sensor->get_device().shared_from_this(); - auto to_dev = to_sensor->parent.device; - if (!to_dev) to_dev = to_sensor->sensor->get_device().shared_from_this(); - - if (from_dev != to_dev) - { - LOG_ERROR("Cannot set extrinsics of two different devices \n"); - return; - } - - auto tm2 = VALIDATE_INTERFACE(from_sensor->sensor, librealsense::tm2_sensor_interface); - tm2->set_extrinsics(*from_profile->profile, *to_profile->profile, *extrinsics); + throw not_implemented_exception( "deprecated" ); } HANDLE_EXCEPTIONS_AND_RETURN(, from_sensor, from_profile, to_sensor, to_profile, extrinsics) void rs2_set_motion_device_intrinsics(const rs2_sensor* sensor, const rs2_stream_profile* profile, const rs2_motion_device_intrinsic* intrinsics, rs2_error** error) BEGIN_API_CALL { - VALIDATE_NOT_NULL(sensor); - VALIDATE_NOT_NULL(profile); - VALIDATE_NOT_NULL(intrinsics); - - auto tm2 = VALIDATE_INTERFACE(sensor->sensor, librealsense::tm2_sensor_interface); - tm2->set_motion_device_intrinsics(*profile->profile, *intrinsics); + throw not_implemented_exception( "deprecated" ); } HANDLE_EXCEPTIONS_AND_RETURN(, sensor, profile, intrinsics) void rs2_reset_to_factory_calibration(const rs2_device* device, rs2_error** error) BEGIN_API_CALL { VALIDATE_NOT_NULL(device); - auto tm2 = dynamic_cast(&device->device->get_sensor(0)); - if (tm2) - tm2->reset_to_factory_calibration(); - else - { - auto auto_calib = std::dynamic_pointer_cast(device->device); - if (!auto_calib) - throw std::runtime_error("this device does not supports reset to factory calibration"); - auto_calib->reset_to_factory_calibration(); - } + auto auto_calib = std::dynamic_pointer_cast(device->device); + if (!auto_calib) + throw std::runtime_error("this device does not support reset to factory calibration"); + auto_calib->reset_to_factory_calibration(); } HANDLE_EXCEPTIONS_AND_RETURN(, device) void rs2_write_calibration(const rs2_device* device, rs2_error** error) BEGIN_API_CALL { VALIDATE_NOT_NULL(device); - auto tm2 = dynamic_cast(&device->device->get_sensor(0)); - if (tm2) - tm2->write_calibration(); - else - { - auto auto_calib = std::dynamic_pointer_cast(device->device); - if (!auto_calib) - throw std::runtime_error("this device does not supports auto calibration"); - auto_calib->write_calibration(); - } + auto auto_calib = std::dynamic_pointer_cast(device->device); + if (!auto_calib) + throw std::runtime_error("this device does not support auto calibration"); + auto_calib->write_calibration(); } HANDLE_EXCEPTIONS_AND_RETURN(, device) diff --git a/src/rsusb-backend/rsusb-backend.cpp b/src/rsusb-backend/rsusb-backend.cpp index 7fcb9e513b..f32cbe68d9 100644 --- a/src/rsusb-backend/rsusb-backend.cpp +++ b/src/rsusb-backend/rsusb-backend.cpp @@ -9,8 +9,6 @@ #include "../hid/hid-device.h" #include "../usb/usb-enumerator.h" -#include "../tm2/tm-boot.h" - #include #include // std::tolower @@ -53,12 +51,6 @@ namespace librealsense std::vector rs_backend::query_usb_devices() const { auto device_infos = usb_enumerator::query_devices_info(); - // Give the device a chance to restart, if we don't catch - // it, the watcher will find it later. - if(tm_boot(device_infos)) { - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - device_infos = usb_enumerator::query_devices_info(); - } return device_infos; } diff --git a/src/tm2/CMakeLists.txt b/src/tm2/CMakeLists.txt deleted file mode 100644 index 8868e1ccb0..0000000000 --- a/src/tm2/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2019 Intel Corporation. All Rights Reserved. -target_sources(${LRS_TARGET} - PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/tm-boot.h" - "${CMAKE_CURRENT_LIST_DIR}/tm-device.h" - "${CMAKE_CURRENT_LIST_DIR}/tm-info.h" - "${CMAKE_CURRENT_LIST_DIR}/tm-device.cpp" - "${CMAKE_CURRENT_LIST_DIR}/tm-info.cpp" - "${CMAKE_CURRENT_LIST_DIR}/t265-messages.h" -) - -# TM2 is FW-less device, i.e. it receives its FW each time the device’s power is turned on. -# Therefore it is dependent on FW library that is created only when IMPORT_DEPTH_CAM_FW is on -if(NOT IMPORT_DEPTH_CAM_FW) - message(SEND_ERROR "BUILD_WITH_TM2 requires IMPORT_DEPTH_CAM_FW") -endif() -target_link_libraries(${LRS_TARGET} PRIVATE fw) # for T265 FW - -target_include_directories(${LRS_TARGET} PUBLIC $) -set_target_properties (${LRS_TARGET} PROPERTIES FOLDER Resources) diff --git a/src/tm2/message-print.h b/src/tm2/message-print.h deleted file mode 100644 index 88897ae5a7..0000000000 --- a/src/tm2/message-print.h +++ /dev/null @@ -1,117 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2017 Intel Corporation. All Rights Reserved. - -#pragma once - -#include - -#include "t265-messages.h" -using namespace t265; - -#define map_str(X) {X, #X} -std::map bulk_message_names = { - map_str(DEV_GET_DEVICE_INFO), - map_str(DEV_GET_TIME), - map_str(DEV_GET_AND_CLEAR_EVENT_LOG), - map_str(DEV_GET_SUPPORTED_RAW_STREAMS), - map_str(DEV_RAW_STREAMS_CONTROL), - map_str(DEV_GET_CAMERA_INTRINSICS), - map_str(DEV_GET_MOTION_INTRINSICS), - map_str(DEV_GET_EXTRINSICS), - map_str(DEV_SET_CAMERA_INTRINSICS), - map_str(DEV_SET_MOTION_INTRINSICS), - map_str(DEV_SET_EXTRINSICS), - map_str(DEV_LOG_CONTROL), - map_str(DEV_STREAM_CONFIG), - map_str(DEV_RAW_STREAMS_PLAYBACK_CONTROL), - map_str(DEV_READ_EEPROM), - map_str(DEV_WRITE_EEPROM), - map_str(DEV_SAMPLE), - map_str(DEV_START), - map_str(DEV_STOP), - map_str(DEV_STATUS), - map_str(DEV_GET_POSE), - map_str(DEV_EXPOSURE_MODE_CONTROL), - map_str(DEV_SET_EXPOSURE), - map_str(DEV_GET_TEMPERATURE), - map_str(DEV_SET_TEMPERATURE_THRESHOLD), - map_str(DEV_FIRMWARE_UPDATE), - map_str(DEV_GPIO_CONTROL), - map_str(DEV_TIMEOUT_CONFIGURATION), - map_str(DEV_SNAPSHOT), - map_str(DEV_READ_CONFIGURATION), - map_str(DEV_WRITE_CONFIGURATION), - map_str(DEV_RESET_CONFIGURATION), - map_str(DEV_LOCK_CONFIGURATION), - map_str(DEV_LOCK_EEPROM), - map_str(DEV_SET_LOW_POWER_MODE), - map_str(SLAM_STATUS), - map_str(SLAM_GET_OCCUPANCY_MAP_TILES), - map_str(SLAM_GET_LOCALIZATION_DATA), - map_str(SLAM_SET_LOCALIZATION_DATA_STREAM), - map_str(SLAM_SET_6DOF_INTERRUPT_RATE), - map_str(SLAM_6DOF_CONTROL), - map_str(SLAM_OCCUPANCY_MAP_CONTROL), - map_str(SLAM_GET_LOCALIZATION_DATA_STREAM), - map_str(SLAM_SET_STATIC_NODE), - map_str(SLAM_GET_STATIC_NODE), - map_str(SLAM_APPEND_CALIBRATION), - map_str(SLAM_CALIBRATION), - map_str(SLAM_RELOCALIZATION_EVENT), - map_str(DEV_ERROR), - map_str(SLAM_ERROR), - map_str(MAX_MESSAGE_ID), -}; - -std::map control_message_names = { - map_str(CONTROL_USB_RESET), - map_str(CONTROL_GET_INTERFACE_VERSION), - map_str(MAX_CONTROL_ID), -}; - - -template -static std::string message_name(const T & header) { - auto id = BULK_MESSAGE_ID(header.wMessageID); - if(bulk_message_names.count(id) != 0) { - return bulk_message_names.at(id); - } - std::stringstream s; - s << std::hex << "UNKNOWN ID 0x" << id; - return s.str(); -} - -std::map message_status_names = { - map_str(SUCCESS), - map_str(UNKNOWN_MESSAGE_ID), - map_str(INVALID_REQUEST_LEN), - map_str(INVALID_PARAMETER), - map_str(INTERNAL_ERROR), - map_str(UNSUPPORTED), - map_str(LIST_TOO_BIG), - map_str(MORE_DATA_AVAILABLE), - map_str(DEVICE_BUSY), - map_str(TIMEOUT), - map_str(TABLE_NOT_EXIST), - map_str(TABLE_LOCKED), - map_str(DEVICE_STOPPED), - map_str(TEMPERATURE_WARNING), - map_str(TEMPERATURE_STOP), - map_str(CRC_ERROR), - map_str(INCOMPATIBLE), - map_str(AUTH_ERROR), - map_str(DEVICE_RESET), - map_str(SLAM_NO_DICTIONARY), -}; - -template -static std::string status_name(const T & header) { - auto id = MESSAGE_STATUS(header.wStatus); - if(message_status_names.count(id) != 0) { - return message_status_names.at(id); - } - std::stringstream s; - s << std::hex << "UNKNOWN STATUS at 0x" << id; - return s.str(); -} - diff --git a/src/tm2/t265-messages.h b/src/tm2/t265-messages.h deleted file mode 100644 index 59bac2951a..0000000000 --- a/src/tm2/t265-messages.h +++ /dev/null @@ -1,1532 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2017 Intel Corporation. All Rights Reserved. - -/** -* @brief This file contains protocols definitions for TM2 USB vendor specific host/device interface -* -* Bulk In/Out endpoints - for general communication, the protocol on the bulk endpoints is in a request/response convention -* Additional Bulk In endpoint - for video stream (Stream endpoint) -* Interrupt endpoint - for periodic low latency data (namely 6DoF output) and IMU outputs -* All structures below should be treated as little-endian -*/ - -#pragma once - -#include -#include - -#pragma pack(push, 1) - -#ifdef _WIN32 -#pragma warning (push) -#pragma warning (disable : 4200) -#else -#pragma GCC diagnostic ignored "-Wpedantic" -#endif - -#define MAX_VIDEO_STREAMS 8 -#define MAX_FW_LOG_BUFFER_ENTRIES (512) -#define MAX_LOG_PAYLOAD_SIZE (44) -#define MAX_MESSAGE_LEN 1024 -#define MAX_EEPROM_BUFFER_SIZE (MAX_MESSAGE_LEN - sizeof(bulk_message_request_header) - 4) -#define MAX_TABLE_SIZE (MAX_MESSAGE_LEN - sizeof(bulk_message_response_header)) -#define MAX_EEPROM_CONFIGURATION_SIZE 1200 -#define MAX_GUID_LENGTH 128 -#define MAX_FW_UPDATE_FILE_COUNT 6 -#define MAX_SLAM_CALIBRATION_SIZE 10000 - -// Added from TrackingCommon.h -namespace t265 -{ - /** - * @brief Defines all sensors types (bSensorID/bCameraID/bMotionID) - */ - enum SensorType - { - Color = 0, - Depth = 1, - IR = 2, - Fisheye = 3, - Gyro = 4, - Accelerometer = 5, - Controller = 6, - Rssi = 7, - Velocimeter = 8, - Stereo = 9, - Pose = 10, - ControllerProperty = 11, - Mask = 12, - Max - }; - - enum PixelFormat - { - ANY = 0, /**< Any pixel format */ - Z16 = 1, /**< 16-bit per pixel - linear depth values. The depth is meters is equal to depth scale * pixel value */ - DISPARITY16 = 2, /**< 16-bit per pixel - linear disparity values. The depth in meters is equal to depth scale / pixel value */ - XYZ32F = 3, /**< 96-bit per pixel - 32 bit floating point 3D coordinates. */ - YUYV = 4, /**< 16-bit per pixel - Standard YUV pixel format as described in https://en.wikipedia.org/wiki/YUV */ - RGB8 = 5, /**< 24-bit per pixel - 8-bit Red, Green and Blue channels */ - BGR8 = 6, /**< 24-bit per pixel - 8-bit Blue, Green and Red channels, suitable for OpenCV */ - RGBA8 = 7, /**< 32-bit per pixel - 8-bit Red, Green, Blue channels + constant alpha channel equal to FF */ - BGRA8 = 8, /**< 32-bit per pixel - 8-bit Blue, Green, Red channels + constant alpha channel equal to FF */ - Y8 = 9, /**< 8-bit per pixel - grayscale image */ - Y16 = 10, /**< 16-bit per-pixel - grayscale image */ - RAW8 = 11, /**< 8-bit per pixel - raw image */ - RAW10 = 12, /**< 10-bit per pixel - Four 10-bit luminance values encoded into a 5-byte macropixel */ - RAW16 = 13 /**< 16-bit per pixel - raw image */ - }; - - /** - * @brief Defines all 6dof modes - */ - typedef enum { - SIXDOF_MODE_NORMAL = 0X0000, - SIXDOF_MODE_FAST_PLAYBACK = 0x0001, - SIXDOF_MODE_ENABLE_MAPPING = 0x0002, - SIXDOF_MODE_ENABLE_RELOCALIZATION = 0x0004, - SIXDOF_MODE_DISABLE_JUMPING = 0x0008, - SIXDOF_MODE_DISABLE_DYNAMIC_CALIBRATION = 0x0010, - SIXDOF_MODE_ENABLE_MAP_PRESERVATION = 0x0020, - SIXDOF_MODE_MAX = ((SIXDOF_MODE_FAST_PLAYBACK | SIXDOF_MODE_ENABLE_MAPPING | SIXDOF_MODE_ENABLE_RELOCALIZATION | SIXDOF_MODE_DISABLE_JUMPING | SIXDOF_MODE_DISABLE_DYNAMIC_CALIBRATION | SIXDOF_MODE_ENABLE_MAP_PRESERVATION) + 1) - } SIXDOF_MODE; - - inline SIXDOF_MODE &operator|=(SIXDOF_MODE &x, SIXDOF_MODE y) { - return x = static_cast(static_cast::type>(x) | - static_cast::type>(y)); - } - inline SIXDOF_MODE &operator&=(SIXDOF_MODE &x, SIXDOF_MODE y) { - return x = static_cast(static_cast::type>(x) & - static_cast::type>(y)); - } - inline SIXDOF_MODE operator~(SIXDOF_MODE x) { - return static_cast(~static_cast::type>(x)); - } - inline SIXDOF_MODE operator|(SIXDOF_MODE x, SIXDOF_MODE y) { return x |= y; } - inline SIXDOF_MODE operator&(SIXDOF_MODE x, SIXDOF_MODE y) { return x &= y; } - - /** - * @brief Defines all bulk messages ids - */ - typedef enum { - - /* Core device messages */ - DEV_GET_DEVICE_INFO = 0x0001, - DEV_GET_TIME = 0x0002, - DEV_GET_AND_CLEAR_EVENT_LOG = 0x0003, - DEV_GET_SUPPORTED_RAW_STREAMS = 0x0004, - DEV_RAW_STREAMS_CONTROL = 0x0005, - DEV_GET_CAMERA_INTRINSICS = 0x0006, - DEV_GET_MOTION_INTRINSICS = 0x0007, - DEV_GET_EXTRINSICS = 0x0008, - DEV_SET_CAMERA_INTRINSICS = 0x0009, - DEV_SET_MOTION_INTRINSICS = 0x000A, - DEV_SET_EXTRINSICS = 0x000B, - DEV_LOG_CONTROL = 0x000C, - DEV_STREAM_CONFIG = 0x000D, - DEV_RAW_STREAMS_PLAYBACK_CONTROL = 0x000E, - DEV_READ_EEPROM = 0x000F, - DEV_WRITE_EEPROM = 0x0010, - DEV_SAMPLE = 0x0011, - DEV_START = 0x0012, - DEV_STOP = 0x0013, - DEV_STATUS = 0x0014, - DEV_GET_POSE = 0x0015, - DEV_EXPOSURE_MODE_CONTROL = 0x0016, - DEV_SET_EXPOSURE = 0x0017, - DEV_GET_TEMPERATURE = 0x0018, - DEV_SET_TEMPERATURE_THRESHOLD = 0x0019, - DEV_FIRMWARE_UPDATE = 0x001C, - DEV_GPIO_CONTROL = 0x001D, - DEV_TIMEOUT_CONFIGURATION = 0x001E, - DEV_SNAPSHOT = 0x001F, - DEV_READ_CONFIGURATION = 0x0020, - DEV_WRITE_CONFIGURATION = 0x0021, - DEV_RESET_CONFIGURATION = 0x0022, - DEV_LOCK_CONFIGURATION = 0x0023, - DEV_LOCK_EEPROM = 0x0024, - DEV_SET_LOW_POWER_MODE = 0x0025, - - /* SLAM messages */ - SLAM_STATUS = 0x1001, - SLAM_GET_OCCUPANCY_MAP_TILES = 0x1002, - SLAM_GET_LOCALIZATION_DATA = 0x1003, - SLAM_SET_LOCALIZATION_DATA_STREAM = 0x1004, - SLAM_SET_6DOF_INTERRUPT_RATE = 0x1005, - SLAM_6DOF_CONTROL = 0x1006, - SLAM_OCCUPANCY_MAP_CONTROL = 0x1007, - SLAM_GET_LOCALIZATION_DATA_STREAM = 0x1009, - SLAM_SET_STATIC_NODE = 0x100A, - SLAM_GET_STATIC_NODE = 0x100B, - SLAM_APPEND_CALIBRATION = 0x100C, - SLAM_CALIBRATION = 0x100D, - SLAM_RELOCALIZATION_EVENT = 0x100E, - SLAM_REMOVE_STATIC_NODE = 0x100F, - - /* Error messages */ - DEV_ERROR = 0x8000, - SLAM_ERROR = 0x9000, - - /* Message IDs are 16-bits */ - MAX_MESSAGE_ID = 0xFFFF, - } BULK_MESSAGE_ID; - - - /** - * @brief Defines all bulk message return statuses - */ - typedef enum { - SUCCESS = 0X0000, - UNKNOWN_MESSAGE_ID = 0x0001, - INVALID_REQUEST_LEN = 0x0002, - INVALID_PARAMETER = 0x0003, - INTERNAL_ERROR = 0x0004, - UNSUPPORTED = 0x0005, - LIST_TOO_BIG = 0x0006, - MORE_DATA_AVAILABLE = 0x0007, - DEVICE_BUSY = 0x0008, /* Indicates that this command is not supported in the current device state, e.g.trying to change configuration after START */ - TIMEOUT = 0x0009, - TABLE_NOT_EXIST = 0x000A, /* The requested configuration table does not exists on the EEPROM */ - TABLE_LOCKED = 0x000B, /* The configuration table is locked for writing or permanently locked from unlocking */ - DEVICE_STOPPED = 0x000C, /* Used with DEV_STATUS messages to mark the last message over an endpoint after a DEV_STOP command */ - TEMPERATURE_WARNING = 0x0010, /* The device temperature reached 10 % from its threshold */ - TEMPERATURE_STOP = 0x0011, /* The device temperature reached its threshold, and the device stopped tracking */ - CRC_ERROR = 0x0012, /* CRC Error in firmware update */ - INCOMPATIBLE = 0x0013, /* Controller version is incompatible with TM2 version */ - AUTH_ERROR = 0x0014, /* Authentication error in firmware update */ - DEVICE_RESET = 0x0015, /* A device reset has occurred. The user may read the FW log for additional detail */ - SLAM_NO_DICTIONARY = 0x9001, /* No relocalization dictionary was loaded */ - } MESSAGE_STATUS; - - /** - * @brief Defines EEPROM lock states - */ - typedef enum { - EEPROM_LOCK_STATE_WRITEABLE = 0x0000, - EEPROM_LOCK_STATE_LOCKED = 0x0001, - EEPROM_LOCK_STATE_PERMANENT_LOCKED = 0x0003, - EEPROM_LOCK_STATE_MAX = 0xFFFF, - } EEPROM_LOCK_STATE; - - /** - * @brief Defines SKU info types - */ - typedef enum { - SKU_INFO_TYPE_WITHOUT_BLUETOOTH = 0x0000, - SKU_INFO_TYPE_WITH_BLUETOOTH = 0x0001, - SKU_INFO_TYPE_MAX = 0xFFFF, - } SKU_INFO_TYPE; - - /** - * @brief Defines all control messages ids - */ - typedef enum { - CONTROL_USB_RESET = 0x0010, - CONTROL_GET_INTERFACE_VERSION = 0x0011, - - /* Message IDs are 16-bits */ - MAX_CONTROL_ID = 0xFFFF, - } CONTROL_MESSAGE_ID; - - /** - * @brief Defines all FW status codes - */ - typedef enum { - FW_STATUS_CODE_OK = 0, - FW_STATUS_CODE_FAIL = 1, - FW_STATUS_CODE_NO_CALIBRATION_DATA = 1000 - } FW_STATUS_CODE; - - /** - * @brief Defines all SLAM status codes - */ - typedef enum { - SLAM_STATUS_CODE_SUCCESS = 0, /**< No error has occurred. */ - SLAM_STATUS_CODE_LOCALIZATION_DATA_SET_SUCCESS = 1, /**< Reading all localization data chunks completed successfully */ - } SLAM_STATUS_CODE; - - /** - * @brief Defines all SLAM error codes - */ - typedef enum { - SLAM_ERROR_CODE_NONE = 0, /**< No error has occurred. */ - SLAM_ERROR_CODE_VISION = 1, /**< No visual features were detected in the most recent image. This is normal in some circumstances, such as quick motion or if the device temporarily looks at a blank wall. */ - SLAM_ERROR_CODE_SPEED = 2, /**< The device moved more rapidly than expected for typical handheld motion. This may indicate that rc_Tracker has failed and is providing invalid data. */ - SLAM_ERROR_CODE_OTHER = 3, /**< A fatal internal error has occurred. */ - } SLAM_ERROR_CODE; - - /** - * @brief Bulk message request header struct - * - * Start of all USB message requests. - */ - typedef struct { - uint32_t dwLength; /**< Message length in bytes */ - uint16_t wMessageID; /**< ID of message */ - } bulk_message_request_header; - - - /** - * @brief Bulk message response header struct - * - * Start of all USB message responses. - */ - typedef struct { - uint32_t dwLength; /**< Message length in bytes */ - uint16_t wMessageID; /**< ID of message */ - uint16_t wStatus; /**< Status of request (MESSAGE_STATUS) */ - } bulk_message_response_header; - - - /** - * @brief Device Info libtm Message - * - * Retrieve information on the TM2 device. - */ - typedef struct { - uint8_t bDeviceType; /**< Device identifier: 0x1 = T250 */ - uint8_t bHwVersion; /**< ASIC Board version: 0x00 = ES0, 0x01 = ES1, 0x02 = ES2, 0x03 = ES3, 0xFF = Unknown */ - uint8_t bStatus; /**< Bits 0-3: device status: 0x0 = device functional, 0x1 = error, Bits 4-7: Reserved */ - uint8_t bEepromDataMajor; /**< Major part of the EEPROM data version */ - uint8_t bEepromDataMinor; /**< Minor part of the EEPROM data version */ - uint8_t bFWVersionMajor; /**< Major part of the Myriad firmware version */ - uint8_t bFWVersionMinor; /**< Minor part of the Myriad firmware version */ - uint8_t bFWVersionPatch; /**< Patch part of the Myriad firmware version */ - uint32_t dwFWVersionBuild; /**< Build part of the Myriad firmware version */ - uint32_t dwRomVersion; /**< Myriad ROM version */ - uint32_t dwStatusCode; /**< Status Code: S_OK = 0, E_FAIL = 1, E_NO_CALIBRATION_DATA = 1000 */ - uint32_t dwExtendedStatus; /**< Extended status information (details TBD) */ - uint64_t llSerialNumber; /**< Device serial number */ - uint8_t bCentralProtocolVersion; /**< Central BLE Protocol Version */ - uint8_t bCentralAppVersionMajor; /**< Major part of the Central firmware version */ - uint8_t bCentralAppVersionMinor; /**< Minor part of the Minor Central firmware version */ - uint8_t bCentralAppVersionPatch; /**< Patch part of the Patch Central firmware version */ - uint8_t bCentralSoftdeviceVersion; /**< Central BLE Softdevice Version */ - uint8_t bCentralBootloaderVersionMajor; /**< Major part of the Central firmware version */ - uint8_t bCentralBootloaderVersionMinor; /**< Minor part of the Minor Central firmware version */ - uint8_t bCentralBootloaderVersionPatch; /**< Patch part of the Patch Central firmware version */ - uint32_t dwCentralAppVersionBuild; /**< Build part of the Build Central firmware version */ - uint8_t bEepromLocked; /**< 0x0 - The EEPROM is fully writeable */ - /**< 0x1 - The upper quarter of the EEPROM memory is write-protected */ - /**< 0x3 - The upper quarter of the EEPROM memory is permanently write-protected */ - uint8_t bSKUInfo; /**< 1 for T260 - with ble, 0 for T265 - w/o ble */ - } device_info_libtm_message; - - - /** - * @brief Bulk Get Device Info Message - * - * Retrieve TM2 device information. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 6 bytes, wMessageID = DEV_GET_DEVICE_INFO */ - } bulk_message_request_get_device_info; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 49 or 8 bytes, wMessageID = DEV_GET_DEVICE_INFO */ - device_info_libtm_message message; /**< Device info */ - } bulk_message_response_get_device_info; - - - /** - * @brief Bulk Device stream config Message - * - * Sets the maximum message size the host can receive - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 10 bytes, wMessageID = DEV_STREAM_CONFIG */ - uint16_t wReserved; /**< Reserved = 0 */ - uint32_t dwMaxSize; /**< Max stream endpoint message buffer size (Android 16K, Windows/Linux 678,400) */ - } bulk_message_request_stream_config; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = DEV_STREAM_CONFIG */ - } bulk_message_response_stream_config; - - /** - * @brief Bulk Get Time Message - * - * Retrieve the TM2 time, representred in nanoseconds since device initialization. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 6 bytes, wMessageID = DEV_GET_TIME */ - } bulk_message_request_get_time; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 16 or 8 bytes, wMessageID = DEV_GET_TIME */ - uint64_t llNanoseconds; /**< Number of nanoseconds since device initialization */ - } bulk_message_response_get_time; - - - /** - * @brief Bulk Get and Clear Event Log Message - * - * Retrieves the device event log and clears it, used for debugging purposes. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 6 bytes, wMessageID = DEV_GET_AND_CLEAR_EVENT_LOG */ - } bulk_message_request_get_and_clear_event_log; - - typedef struct { - uint8_t source; /**< Log level and source - Bits 0-3: Verbosity Level, Bits 4-6: Log index, Bit 7 - log source. */ - uint8_t timestamp[7]; /**< Timestamp - Lowest 56bits of MV2 timestamp in nanoseconds. */ - uint16_t eventID; /**< Event ID - Bits 0-14: Event ID from an event enum which the host software shall be able to map to a descriptive message. */ - /**< - Bit 15: 0 - the payload type (if exists) is according to bits 7-8 from the "Payload size" field. */ - /**< - : 1 - the payload is a null-terminated C-string (bits 7-8 can be ignored). */ - uint8_t payloadSize; /**< Actual payload size - Bits 0-6: Actual payload size in bytes, 0 - 44 bytes. */ - /**< size = 0-12, the entry is a basic entry (32 bytes long). */ - /**< size = 13-44, the entry is an extended entry(64 bytes long). */ - /**< Bits 7-8: Payload data type: 0x0 - 32 bits integers array */ - /**< 0x1 - 32 bits floats array */ - /**< 0x2 - 8 bits byte array(binary data) */ - /**< 0x3 - Event specific defined structure */ - uint8_t threadID; /**< RTEMS thread ID. */ - uint16_t moduleID; /**< Module ID. */ - uint16_t lineNumber; /**< Line number in source code. */ - uint32_t functionSymbol; /**< Address of the current function. */ - uint8_t payload[MAX_LOG_PAYLOAD_SIZE]; /**< Event specific payload - This field is 12 bytes long for basic entries, and 44 bytes long for extended entries. */ - } device_event_log; - - typedef struct { - bulk_message_response_header header; /**< Message response header: wMessageID = DEV_GET_AND_CLEAR_EVENT_LOG */ - device_event_log bEventLog[MAX_FW_LOG_BUFFER_ENTRIES]; /**< Event log buffers */ - } bulk_message_response_get_and_clear_event_log; - - - /** - * @brief Supported Raw Stream libtm Message - * - * Single supported raw stream that can be streamed out of the device. - */ - typedef struct { - uint8_t bSensorID; /**< Bits 0-4: Type of sensor, supported values are: Color = 0, Depth = 1, IR = 2, Fisheye = 3, Gyro = 4, Accelerometer = 5 */ - /**< Bits 5-7: Sensor index - Zero based index of sensor with the same type within device. For example if the device supports two fisheye cameras, */ - /**< The first will use index 0 (bSensorID = 0x03) and the second will use index 1 (bSensorID = 0x23) */ - uint8_t bReserved; /**< Reserved = 0 */ - uint16_t wWidth; /**< Supported width (in pixels) of first stream, 0 for non-camera streams */ - uint16_t wHeight; /**< Supported height (in pixels) or first stream, 0 for non-camera streams */ - uint8_t bPixelFormat; /**< Pixel format of the stream, according to enum PixelFormat */ - union { /**< */ - uint8_t bOutputMode; /**< 0x0 - Send sensor outputs to the internal middlewares only, 0x1 - Send this sensor outputs also to the host over the USB interface. */ - uint8_t bReserved2; /**< Reserved and always 0. Sent from device to host. */ - }; /**< */ - uint16_t wStride; /**< Length in bytes of each line in the image (including padding). 0 for non-camera streams. */ - uint16_t wFramesPerSecond; /**< Supported FPS for first stream to be enabled */ - } supported_raw_stream_libtm_message; - - /** - * @brief Bulk Get Supported Raw Streams Message - * - * Retrieves a list of supported raw streams that can be streamed out of the device. - * Note that multiple entries indicated multiple stream configurations may be supported on the same camera. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 6 bytes, wMessageID = DEV_GET_SUPPORTED_RAW_STREAMS */ - } bulk_message_request_get_supported_raw_streams; - - typedef struct { - bulk_message_response_header header; /**< Message response header: wMessageID = DEV_GET_SUPPORTED_RAW_STREAMS */ - uint16_t wNumSupportedStreams; /**< Number of supported streams in list below */ - uint16_t wReserved; /**< Reserved = 0 */ - supported_raw_stream_libtm_message stream[]; /**< Supported stream info variable sized array */ - } bulk_message_response_get_supported_raw_streams; - - - /** - * @brief Bulk Raw Streams Control Message - * - * Enables streams to be directly streamed to the host. - * Following a successful invocation of this command, and a call to the start command, the enabled streams shall be sent at the expected frame rate to the algorithms, and every streams whose bOutputMode bit was set to 1 shall be sent over the stream endpoint. - * Other streams will be disabled, including those that were enabled by previous invocations. - * To disable all image streaming, this command should be called with wNumEnabledStreams = 0, in this case, all fields after wNumEnabledStreams are dropped. - * If there is a mismatch between wNumEnabledStreams and the size of the message, the firmware will return an INVALID_REQUEST_LEN error message. - * If the profile is not supported, the firmware shall return an INVALID_PARAMETER error message (e.g. trying to open an unsupported configuration by a sensor, not valid FPS or requesting two profiles of the same sensor). - * If the device is already streaming live from its sensors sending a DEV_ RAW_STREAMS_PLAYBACK_CONTROL shall fail with a DEVICE_BUSY error message. - * If no DEV_RAW_STREAMS_CONTROL command was called before the host calls to DEV_START, the first stream configuration for each sensor as reported by DEV_GET_SUPPORTED_RAW_STREAMS shall be used as default. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: wMessageID = DEV_RAW_STREAMS_CONTROL */ - uint16_t wNumEnabledStreams; /**< Number of enabled streams in list below */ - supported_raw_stream_libtm_message stream[]; /**< Supported stream info variable sized array */ - } bulk_message_request_raw_streams_control; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = DEV_RAW_STREAMS_CONTROL */ - } bulk_message_response_raw_streams_control; - - - /** - * @brief Bulk Raw Streams Playback Control Message - * - * Enables receiving streams from the host to be streamed to the MWs instead from the real sensors. - * Following a successful invocation of this command, and a call to the start command, the enabled streams shall be sent at the expected frame rate to the algorithms, and every streams whose bOutputMode bit was set to 1 shall be sent over the stream endpoint (even in the playback case, effectively loopback-ing the samples back to the host). - * Other streams will be disabled, including those that were enabled by previous invocations. - * To disable all image streaming, this command should be called with wNumEnabledStreams = 0, in this case, all fields after wNumEnabledStreams are dropped. - * If there is a mismatch between wNumEnabledStreams and the size of the message, the firmware will return an INVALID_REQUEST_LEN error message. - * If the profile is not supported, the firmware shall return an INVALID_PARAMETER error message (e.g. trying to open an unsupported configuration by a sensor, not valid FPS or requesting two profiles of the same sensor). - * If the device is already streaming live from its sensors sending a DEV_ RAW_STREAMS_PLAYBACK_CONTROL shall fail with a DEVICE_BUSY error message. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: wMessageID = DEV_RAW_STREAMS_PLAYBACK_CONTROL */ - uint16_t wNumEnabledStreams; /**< Number of enabled streams in list below */ - supported_raw_stream_libtm_message stream[]; /**< Supported stream info variable sized array */ - } bulk_message_request_raw_streams_playback_control; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = DEV_RAW_STREAMS_PLAYBACK_CONTROL */ - } bulk_message_response_raw_streams_playback_control; - - - /** - * @brief Bulk Get Camera Intrinsics Message - * - * Retrieves the intrinsic parameters of an individual camera in the device. - */ - typedef struct { - uint32_t dwWidth; /**< Width of the image in pixels */ - uint32_t dwHeight; /**< Height of the image in pixels */ - float_t flPpx; /**< Horizontal coordinate of the principal point of the image, as a pixel offset from the left edge */ - float_t flPpy; /**< Vertical coordinate of the principal point of the image, as a pixel offset from the top edge */ - float_t flFx; /**< Focal length of the image plane, as a multiple of pixel width */ - float_t flFy; /**< Focal length of the image plane, as a multiple of pixel Height */ - uint32_t dwDistortionModel; /**< Distortion model of the image: F-THETA = 1, NONE (UNDISTORTED) = 3, KANNALA_BRANDT4 = 4 */ - float_t flCoeffs[5]; /**< Distortion coefficients */ - } camera_intrinsics; - - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 7 bytes, wMessageID = DEV_GET_CAMERA_INTRINSICS */ - uint8_t bCameraID; /**< Bits 0-4: Type of requested camera intrinsics, supported values are: Color = 0, Depth = 1, IR = 2, Fisheye = 3 */ - /**< Bits 5-7: Camera index - Zero based index of camera with the same type within device. For example if the device supports two fisheye cameras, */ - /**< The first will use index 0 (bCameraID = 0x03) and the second will use index 1 (bCameraID = 0x23) */ - } bulk_message_request_get_camera_intrinsics; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 56 or 8 bytes, wMessageID = DEV_GET_CAMERA_INTRINSICS */ - camera_intrinsics intrinsics; /**< Intrinsics parameters of an individual camera in the device */ - } bulk_message_response_get_camera_intrinsics; - - - /** - * @brief Bulk Get Motion Module Intrinsics Message - * - * Retrieves the intrinsic parameters of an individual motion module in the device. - */ - typedef struct { - float_t flData[3][4]; /**< Scale matrix */ - float_t flNoiseVariances[3]; /**< Noise variances */ - float_t flBiasVariances[3]; /**< Bias variances */ - } motion_intrinsics; - - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 7 bytes, wMessageID = DEV_GET_MOTION_INTRINSICS */ - uint8_t bMotionID; /**< Bits 0-4: Type of requested motion module, supported values are: Gyro = 4, Accelerometer = 5 */ - /**< Bits 5-7: Motion module index - Zero based index of module with the same type within device. For example if the device supports two gyroscopes, */ - /**< The first gyroscope will use index 0 (bMotionID = 0x04) and the second gyroscope will use index 1 (bMotionID = 0x24) */ - } bulk_message_request_get_motion_intrinsics; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 80 or 8 bytes, wMessageID = DEV_GET_MOTION_INTRINSICS */ - motion_intrinsics intrinsics; /**< Intrinsics parameters of an individual motion module in the device */ - } bulk_message_response_get_motion_intrinsics; - - - /** - * @brief Bulk Get Extrinsics Message - * - * Retrieves the extrinsic pose of on individual sensor in the device relative to another one. - */ - typedef struct { - float_t flRotation[9]; /**< Column-major 3x3 rotation matrix */ - float_t flTranslation[3]; /**< 3 element translation vector, in meters */ - uint8_t bReferenceSensorID; /**< Reference sensor uses for extrinsics calculation */ - } sensor_extrinsics; - - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 8 bytes, wMessageID = DEV_GET_EXTRINSICS */ - uint8_t bSensorID; /**< Bits 0-4: Type of requested sensor extrinsics, supported values are: Color = 0, Depth = 1, IR = 2, Fisheye = 3, Gyro = 4, Accelerometer = 5, */ - /**< Bits 5-7: Camera index - Zero based index of sensor with the same type within device. For example if the device supports two fisheye cameras, */ - /**< The first will use index 0 (bSensorID = 0x03) and the second will use index 1 (bSensorID = 0x23) */ - } bulk_message_request_get_extrinsics; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 56 or 8 bytes, wMessageID = DEV_GET_EXTRINSICS */ - sensor_extrinsics extrinsics; /**< Extrinsics pose of an individual sensor in the device relative to another one */ - } bulk_message_response_get_extrinsics; - - typedef struct { - bulk_message_request_header header; - uint8_t bSensorID; - sensor_extrinsics extrinsics; - } bulk_message_request_set_extrinsics; - - typedef struct { - bulk_message_response_header header; - } bulk_message_response_set_extrinsics; - - /** - * @brief Bulk Set Camera Intrinsics Message - * - * Sets the intrinsic parameters of an individual camera in the device. - * These parameters shall not be written to the EEPROM. They shall be used in the next streaming session only and shall be discarded once a "stop" command is called. - * This command is available only when the middlewares (6DoF / Occupancy map) are disabled. - * If any middleware is enabled, this command will return an UNSUPPORTED error. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 56 bytes, wMessageID = DEV_SET_CAMERA_INTRINSICS */ - uint8_t bCameraID; /**< Bits 0-4: Type of requested camera intrinsics, supported values are: Color = 0, Depth = 1, IR = 2, Fisheye = 3 */ - /**< Bits 5-7: Camera index - Zero based index of camera with the same type within device. For example if the device supports two fisheye cameras, */ - /**< The first will use index 0 (bCameraID = 0x03) and the second will use index 1 (bCameraID = 0x23) */ - uint8_t bReserved; /**< Reserved = 0 */ - camera_intrinsics intrinsics; /**< Intrinsics parameters of an individual camera in the device */ - } bulk_message_request_set_camera_intrinsics; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = DEV_SET_CAMERA_INTRINSICS */ - } bulk_message_response_set_camera_intrinsics; - - - /** - * @brief Bulk Set Motion Module Intrinsics Message - * - * Sets the intrinsic parameters of an individual motion module in the device. - * These parameters shall not be written to the EEPROM. They shall be used in the next streaming session only and shall be discarded once a "stop" command is called. - * This command is available only when the middlewares (6DoF / Occupancy map) are disabled. - * If any middleware is enabled, this command will return an UNSUPPORTED error. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 80 bytes, wMessageID = DEV_SET_MOTION_INTRINSICS */ - uint8_t bMotionID; /**< Bits 0-4: Type of requested motion module, supported values are: Gyro = 4, Accelerometer = 5 */ - /**< Bits 5-7: Motion module index - Zero based index of module with the same type within device. For example if the device supports two gyroscopes, */ - /**< The first gyroscope will use index 0 (bMotionID = 0x04) and the second gyroscope will use index 1 (bMotionID = 0x24) */ - uint8_t bReserved; /**< Reserved = 0 */ - motion_intrinsics intrinsics; /**< Intrinsics parameters of an individual motion module in the device */ - } bulk_message_request_set_motion_intrinsics; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = DEV_SET_MOTION_INTRINSICS */ - } bulk_message_response_set_motion_intrinsics; - - - /** - * @brief Bulk Log Control Message - * - * Controls the logging parameters, such as verbosity and log mode. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 8 bytes, wMessageID = DEV_LOG_CONTROL */ - uint8_t bVerbosity; /**< Verbosity level */ - uint8_t bLogMode; /**< 0x00 - No rollover, logging will be paused after log is filled. Until cleared, first events will be stored */ - /**< 0x01 - Rollover mode */ - } bulk_message_request_log_control; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = DEV_LOG_CONTROL */ - } bulk_message_response_log_control; - - - /** - * @brief Bulk Reset Message - * - * Resets the device. The command supports a firmware reset with and without reload of the firmware. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 8 bytes, wMessageID = DEV_RESET */ - uint8_t bVerbosity; /**< Reset action: 0 - reset the VPU device and boot from the firmware already loaded in memory */ - /**< 1 - reset the VPU device to a "pre-load" state, i.e. firmware is loaded from ROM and awaits new firmware to be pushed from host via USB */ - uint8_t bReserved; /**< Reserved = 0 */ - } bulk_message_request_reset; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = DEV_RESET */ - } bulk_message_response_reset; - - - /** - * @brief Bulk Read EEPROM Message - * - * Read data from the EEPROM memory. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 10 bytes, wMessageID = DEV_READ_EEPROM */ - uint16_t wOffset; /**< EEPROM offset address to read from */ - uint16_t wSize; /**< The requested size in bytes of the data to read */ - } bulk_message_request_read_eeprom; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 10+wSize bytes, wMessageID = DEV_READ_EEPROM */ - uint16_t wSize; /**< Size in bytes of the read data */ - uint8_t bData[MAX_EEPROM_BUFFER_SIZE]; /**< The requested EEPROM data array */ - } bulk_message_response_read_eeprom; - - - /** - * @brief Bulk Write EEPROM data Message - * - * Write data to the EEPROM memory. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 10+wSize bytes, wMessageID = DEV_WRITE_EEPROM */ - uint16_t wOffset; /**< EEPROM offset address to write to */ - uint16_t wSize; /**< The size in bytes of the data to write to */ - uint8_t bData[MAX_EEPROM_BUFFER_SIZE]; /**< The requested EEPROM data array */ - } bulk_message_request_write_eeprom; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 10+wSize bytes, wMessageID = DEV_WRITE_EEPROM */ - uint16_t wSize; /**< The size of bytes written */ - } bulk_message_response_write_eeprom; - - - /** - * @brief Bulk Start Message - * - * Starts all the enabled streams and middlewares, using either default configuration or the parameters from calls to any of the configuration commands. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 6 bytes, wMessageID = DEV_START */ - } bulk_message_request_start; - - typedef struct { - bulk_message_response_header header; /**< Message request header: dwLength = 8 bytes, wMessageID = DEV_START */ - } bulk_message_response_start; - - - /** - * @brief Bulk Stop Message - * - * Stops all streams and middlewares running on the device. - * After a "stop" command the device shall suspend to a low-power state. It shall also reset all saved configuration parameters, - * Such as the next call to start shall return to use all the default parameters (unless new calls to any of the configuration commands are made). - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 6 bytes, wMessageID = DEV_STOP */ - } bulk_message_request_stop; - - typedef struct { - bulk_message_response_header header; /**< Message request header: dwLength = 8 bytes, wMessageID = DEV_STOP */ - } bulk_message_response_stop; - - - /** - * @brief Bulk Get Pose Message - * - * Returns the latest pose of the camera relative to its initial position, - * If relocalization data was set, this pose is relative to the relocalization database. - */ - typedef struct { - float_t flX; /**< X value of translation, in meters (relative to initial position) */ - float_t flY; /**< Y value of translation, in meters (relative to initial position) */ - float_t flZ; /**< Z value of translation, in meters (relative to initial position) */ - float_t flQi; /**< Qi component of rotation as represented in quaternion rotation (relative to initial position) */ - float_t flQj; /**< Qj component of rotation as represented in quaternion rotation (relative to initial position) */ - float_t flQk; /**< Qk component of rotation as represented in quaternion rotation (relative to initial position) */ - float_t flQr; /**< Qr component of rotation as represented in quaternion rotation (relative to initial position) */ - float_t flVx; /**< X value of velocity, in meter/sec */ - float_t flVy; /**< Y value of velocity, in meter/sec */ - float_t flVz; /**< Z value of velocity, in meter/sec */ - float_t flVAX; /**< X value of angular velocity, in RAD/sec */ - float_t flVAY; /**< Y value of angular velocity, in RAD/sec */ - float_t flVAZ; /**< Z value of angular velocity, in RAD/sec */ - float_t flAx; /**< X value of acceleration, in meter/sec^2 */ - float_t flAy; /**< Y value of acceleration, in meter/sec^2 */ - float_t flAz; /**< Z value of acceleration, in meter/sec^2 */ - float_t flAAX; /**< X value of angular acceleration, in RAD/sec^2 */ - float_t flAAY; /**< Y value of angular acceleration, in RAD/sec^2 */ - float_t flAAZ; /**< Z value of angular acceleration, in RAD/sec^2 */ - uint64_t llNanoseconds; /**< Timestamp of pose, measured in nanoseconds since device system initialization */ - uint32_t dwTrackerConfidence; /**< pose data confidence 0x0 - Failed, 0x1 - Low, 0x2 - Medium, 0x3 - High */ - uint32_t dwMapperConfidence; /**< Bits 0-1: 0x0 - Failed, 0x1 - Low, 0x2 - Medium, 0x3 - High, Bits 2-31: Reserved */ - uint32_t dwTrackerState; /**< tracker state 0x0 - Inactive, 0x3 Active 3DOF, 0x4 Active 6DOF, 0x7 Inertial only 3DOF */ - } pose_data; - - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 7 bytes, wMessageID = DEV_GET_POSE */ - uint8_t bIndex; /**< Index of HMD - 0x0 = HMD */ - } bulk_message_request_get_pose; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 100 or 8 bytes, wMessageID = DEV_GET_POSE */ - pose_data pose; /**< Short low-latency data (namely 6DoF pose data) */ - } bulk_message_response_get_pose; - - - /** - * @brief Bulk Set Exposure Control Message - * Enable/disable the auto-exposure management of the different video streams, and configure the powerline frequency rate. - * Calling this message is only supported before the streams are started. - * The default values for video streams 0 and 1 shall be auto-exposure enable with 50Hz flicker rate. - * The firmware only supports the following: - * 1. Disabling auto-exposure for all streams (bVideoStreamsMask==0) - * 2. Enabling auto-exposure for both video stream 0 and 1 together (bVideoStreamsMask ==0x3). - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 8 bytes, wMessageID = DEV_EXPOSURE_MODE_CONTROL */ - uint8_t bVideoStreamsMask; /**< Bitmask of the streams to apply the configuration to. bit X -> stream X: 0 - disable, 1 - enable */ - uint8_t bAntiFlickerMode; /**< Anti Flicker Mode: 0 - disable (e.g. for outside use), 1 - 50Hz, 2 - 60Hz, 3 - Auto (currently not supported) */ - } bulk_message_request_set_exposure_mode_control; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = DEV_EXPOSURE_MODE_CONTROL */ - } bulk_message_response_set_exposure_mode_control; - - - /** - * @brief Bulk Set Exposure Message - * - * Sets manual values for the video streams integration time and gain. - * The command supports setting these values to all required streams with a single call. - * The command shall return UNSUPPORTED if the stream is configure to use auto-exposure, and INVALID_PARAMETER if any of the values are out of range. - */ - typedef struct { - uint8_t bCameraID; /**< Bits 0-4: Type of requested camera, supported values are: Color = 0, Depth = 1, IR = 2, Fisheye = 3 */ - /**< Bits 5-7: Camera index - Zero based index of camera with the same type within device */ - uint8_t bReserved[3]; /**< Reserved = 0 */ - uint32_t dwIntegrationTime; /**< Integration time in micro-seconds */ - float_t fGain; /**< Digital gain */ - } stream_exposure; - - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = (8 + bNumberOfStreams*8 bytes), wMessageID = DEV_SET_EXPOSURE */ - uint8_t bNumOfVideoStreams; /**< Number of video streams to configure: Bits 0-2: stream index, Bits 3-7: reserved */ - uint8_t bReserved; /**< Reserved = 0 */ - stream_exposure stream[MAX_VIDEO_STREAMS]; /**< Stream exposure data variable sized array, according to wNumberOfStreams */ - } bulk_message_request_set_exposure; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = DEV_SET_EXPOSURE */ - } bulk_message_response_set_exposure; - - - /** - * @brief Bulk Get Temperature Message - * - * Returns temperature and temperature threshold from all temperature sensors (VPU, IMU, BLE) - */ - typedef struct { - uint32_t dwIndex; /**< Temperature sensor index: 0x0 - VPU, 0x1 - IMU, 0x2 - BLE */ - float_t fTemperature; /**< Sensor's temperature in Celius */ - float_t fThreshold; /**< The sensor's threshold temperature */ - } sensor_temperature; - - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 6 bytes, wMessageID = DEV_GET_TEMPERATURE */ - } bulk_message_request_get_temperature; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLendth = (12 + 12 * dwCount) bytes, wMessageID = DEV_GET_TEMPERATURE */ - uint32_t dwCount; /**< Number or temperature sensors */ - sensor_temperature temperature[]; /**< temperature variable sized array */ - } bulk_message_response_get_temperature; - - - /** - * @brief Bulk Set Temperature Threshold Message - * - * Set temperature threshold to requested sensors (VPU, IMU, BLE) - * The firmware shall actively monitor the temperature of the underlying sensors. - * When a component temperature is 10% close to its defined threshold, the firmware shall send a DEV_ERROR message with a TEMPERATURE_WARNING status to the host, - * When the temperature reach the threshold, the firmware shall stop all running algorithms and sensors (as if DEV_STOP was called), and send a TEMPERATURE_SHUTDOWN status to the user. - */ - typedef struct { - uint32_t dwIndex; /**< Temperature sensor index: 0x0 - VPU, 0x1 - IMU, 0x2 - BLE */ - float_t fThreshold; /**< The new sensor's threshold temperature */ - } sensor_set_temperature; - - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = (12 + 8 * dwCount) bytes, wMessageID = DEV_SET_TEMPERATURE_THRESHOLD */ - uint16_t wForceToken; /**< Token to force higher temperature threshold (80-100) */ - uint32_t dwCount; /**< Number or temperature sensors */ - sensor_set_temperature temperature[]; /**< temperature variable sized array */ - } bulk_message_request_set_temperature_threshold; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLendth = 8, wMessageID = DEV_SET_TEMPERATURE_THRESHOLD */ - } bulk_message_response_set_temperature_threshold; - - /** - * @brief Bulk GPIO control Message - * - * Enable manufacturing tools to directly control the following GPIOs - 74, 75, 76 & 77. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 7 bytes, wMessageID = DEV_GPIO_CONTROL */ - uint8_t bGpioControl; /**< Reserved = 0 */ - } bulk_message_request_gpio_control; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLendth = 8, wMessageID = DEV_GPIO_CONTROL */ - } bulk_message_response_gpio_control; - - - /** - * @brief Bulk configuration Lock Message - * - * Write-protect the manufactoring configuration tables from future changes. - * The locking is done in software by the firmware managing a lock bits in each configuration table metadata. - * The lock can be applied permanently, meaning it cannot be latter un-locked. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 12 bytes, wMessageID = DEV_LOCK_CONFIGURATION */ - uint16_t wTableType; /**< Table ID to lock */ - uint32_t dwLock; /**< 0x0 - Unlock, 0x1 - Lock */ - /**< 0xDEAD10CC - the configuration data shall be permanently locked. *** WARNING *** this might be an irreversible action. */ - /**< After calling this the write protection the settings cannot be modified and therefore the memory write protection is frozen. */ - } bulk_message_request_lock_configuration; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLendth = 8, wMessageID = DEV_LOCK_CONFIGURATION */ - } bulk_message_response_lock_configuration; - - - /** - * @brief Bulk eeprom Lock Message - * - * Write-protect the manufactoring configuration tables from future changes. - * Tables that will be locked: 0x8 - ODM, 0x6 - OEM, 0x10 - module info, 0x11 - internal data, 0x12 - HVS - * The locking is done in hardware by locking the upper quarter of the EEPROM memory - * The lock can be applied permanently, meaning it cannot be latter un-locked. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 12 bytes, wMessageID = DEV_LOCK_CONFIGURATION or DEV_LOCK_EEPROM */ - uint16_t wReserved; /**< Reserved = 0 */ - uint32_t dwLock; /**< 0x0 - Unlock, 0x1 - Lock */ - /**< 0xDEAD10CC - the configuration data shall be permanently locked. *** WARNING *** this might be an irreversible action. */ - /**< After calling this the write protection the settings cannot be modified and therefore the memory write protection is frozen. */ - } bulk_message_request_lock_eeprom; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLendth = 8, wMessageID = DEV_LOCK_CONFIGURATION */ - } bulk_message_response_lock_eeprom; - - - /** - * @brief Bulk read configuration Message - * - * Reads a configuration table from the device memory. - * The device shall return UNSUPPORTED if it does not recognize the requested TableType. - * The device shall return TABLE_NOT_EXIST if it recognize the table type but no such table exists yet in the EEPROM. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 8, wMessageID = DEV_READ_CONFIGURATION */ - uint16_t wTableId; /**< The ID of the requested configuration table */ - } bulk_message_request_read_configuration; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLendth = 8 bytes + size of table, wMessageID = DEV_READ_CONFIGURATION */ - uint8_t bTable[MAX_TABLE_SIZE]; /**< The requested configuration table, starting with the "standard" table header. */ - } bulk_message_response_read_configuration; - - - /** - * @brief Bulk write configuration Message - * - * Writes a configuration table to the device's EEPROM memory. - * This command shall only be supported while the device is stopped, otherwise it shall return DEVICE_BUSY. - * The device shall return UNSUPPORTED if it does not recognize the requested TableType. - * The device shall return TABLE_LOCKED if the configuration table is write protected and cannot be overridden. - * The new data shall be available immediately after completion without requiring a device reset, both to any firmware code or to an external client through a "read configuration" command. - * All internal data object in the firmware memory that were already initialized from some EEPROM data or previous "write configuration" command shall be invalidated and refreshed to the new written data. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 8 + size of table bytes, wMessageID = DEV_WRITE_CONFIGURATION */ - uint16_t wTableId; /**< The ID of the requested configuration table */ - uint8_t bTable[MAX_TABLE_SIZE]; /**< The requested configuration table, starting with the "standard" table header. */ - } bulk_message_request_write_configuration; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLendth = 8, wMessageID = DEV_WRITE_CONFIGURATION */ - } bulk_message_response_write_configuration; - - - /** - * @brief Bulk reset configuration Message - * - * Delete a configuration table from the internal EEPROM storage. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 8, wMessageID = DEV_RESET_CONFIGURATION */ - uint16_t wTableId; /**< The ID of the requested configuration table */ - } bulk_message_request_reset_configuration; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLendth = 8, wMessageID = DEV_RESET_CONFIGURATION */ - } bulk_message_response_reset_configuration; - - - /** - * @brief Bulk timeout configuration Message - * - * Host timeout - When the firmware is actively streaming or tracking, it shall monitor the host status to identify the cases where the host app crashed or stopped responding. - * The firmware shall use the host USB reads as the host's "heartbeat", so if outgoing packets were continuously dropped for over 1 second the firmware shall stop sending packets and stop all operations, - * as if a DEV_STOP command was sent, but without sending the DEV_STOPPED status event packet. - * The timeout shall be configurable or disabled through this DEV_TIMEOUT_CONFIGURATION command. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 8, wMessageID = DEV_TIMEOUT_CONFIGURATION */ - uint16_t wTimeoutInMillis; /**< The commands TX timeout in milliseconds to use. Zero to disable timeout. */ - } bulk_message_request_timeout_configuration; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLendth = 8, wMessageID = DEV_TIMEOUT_CONFIGURATION */ - } bulk_message_response_timeout_configuration; - - - /** - * @brief Bulk enable low power Message - * - * enable or disable low power mode of TM2 device - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 8, wMessageID = DEV_SET_LOW_POWER_MODE */ - uint8_t bEnabled; /**< 1 to enable low power mode, 0 to disable */ - uint8_t bReserved; /**< Reserved = 0 */ - } bulk_message_request_set_low_power_mode; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLendth = 8, wMessageID = DEV_SET_LOW_POWER_MODE */ - } bulk_message_response_set_low_power_mode; - - /** - * @brief Bulk Get Localization Data Message - * - * Trigger the FW to send interrupt_message_get_localization_data_stream with the localization data as created during a 6DoF session - * The response to this message is generated and streamed by the underlying firmware algorithm in run-time, so the total size of the data cannot be known in advance. - * The entire data will be streams in "chunks" sent using SLAM_GET_LOCALIZATION_DATA_STREAM messages over the stream endpoint. - * The firmware shall use a MORE_DATA_AVAILABLE status to indicate there are more data to send. The last chunk (possibly even a zero-size chunk) shall be marked with a SUCCESS status code. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 7 bytes, wMessageID = SLAM_GET_LOCALIZATION_DATA */ - uint8_t bReserved; /**< Reserved = 0 */ - } bulk_message_request_get_localization_data; - - typedef struct { - bulk_message_response_header header; /**< Message response header: wMessageID = SLAM_GET_LOCALIZATION_DATA */ - } bulk_message_response_get_localization_data; - - /** - * @brief Bulk Set Localization Data Stream Message - * - * Sets the localization data to be used to localize the 6DoF reports. - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 10 bytes + bPayload size, wMessageID = according to large message type */ - uint16_t wStatus; /**< SUCCESS: indicate last chunk, MORE_DATA_AVAILABLE: any other chunk */ - uint16_t wIndex; /**< A running counter starting at 0 identifying the chunk index in a single data transaction */ - uint8_t bPayload[]; /**< payload data variable sized array. Data format is algorithm specific and opaque to the USB and host stack */ - } bulk_message_large_stream; - - - /** - * @brief Bulk Set Static Node Message - * - * Set relative position of a static node - */ - typedef struct { - float_t flX; /**< X value of translation, in meters (in the coordinate system of the tracker relative to the current position) */ - float_t flY; /**< Y value of translation, in meters (in the coordinate system of the tracker relative to the current position) */ - float_t flZ; /**< Z value of translation, in meters (in the coordinate system of the tracker relative to the current position) */ - float_t flQi; /**< Qi component of rotation as represented in quaternion rotation (in the coordinate system of the tracker relative to the current position) */ - float_t flQj; /**< Qj component of rotation as represented in quaternion rotation (in the coordinate system of the tracker relative to the current position) */ - float_t flQk; /**< Qk component of rotation as represented in quaternion rotation (in the coordinate system of the tracker relative to the current position) */ - float_t flQr; /**< Qr component of rotation as represented in quaternion rotation (in the coordinate system of the tracker relative to the current position) */ - } static_node_data; - - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 164 bytes, wMessageID = SLAM_SET_STATIC_NODE */ - uint16_t wReserved; /**< Reserved = 0 */ - uint8_t bGuid[MAX_GUID_LENGTH]; /**< Null-terminated C-string, with max length 127 bytes plus one byte for the terminating null character */ - static_node_data data; /**< Static node data */ - } bulk_message_request_set_static_node; - - typedef struct { - bulk_message_response_header header; /**< Message response header: wMessageID = SLAM_SET_STATIC_NODE */ - } bulk_message_response_set_static_node; - - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 164 bytes, wMessageID = SLAM_REMOVE_STATIC_NODE */ - uint16_t wReserved; /**< Reserved = 0 */ - uint8_t bGuid[MAX_GUID_LENGTH]; /**< Null-terminated C-string, with max length 127 bytes plus one byte for the terminating null character */ - } bulk_message_request_remove_static_node; - - typedef struct { - bulk_message_response_header header; /**< Message response header: wMessageID = SLAM_REMOVE_STATIC_NODE */ - } bulk_message_response_remove_static_node; - - /** - * @brief Bulk Get Static Node Message - * - * Get relative position of a static node - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 136 bytes, wMessageID = SLAM_GET_STATIC_NODE */ - uint16_t wReserved; /**< Reserved = 0 */ - uint8_t bGuid[MAX_GUID_LENGTH]; /**< Null-terminated C-string, with max length 127 bytes plus one byte for the terminating null character */ - } bulk_message_request_get_static_node; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 36 byte, wMessageID = SLAM_GET_STATIC_NODE */ - static_node_data data; /**< Static node data */ - } bulk_message_response_get_static_node; - - /** - * @brief Bulk SLAM override calibration Message - * - * Append current SLAM calibration - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 10006 bytes, wMessageID = SLAM_APPEND_CALIBRATION */ - uint8_t calibration_append_string[MAX_SLAM_CALIBRATION_SIZE]; /**< Calibration string */ - } bulk_message_request_slam_append_calibration; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 8 byte, wMessageID = SLAM_APPEND_CALIBRATION */ - } bulk_message_response_slam_append_calibration; - - - /** - * @brief Bulk SLAM calibration Message - * - * Override current SLAM calibration - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 10006 bytes, wMessageID = SLAM_CALIBRATION */ - uint8_t calibration_string[MAX_SLAM_CALIBRATION_SIZE]; /**< Calibration string */ - } bulk_message_request_slam_calibration; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 8 byte, wMessageID = SLAM_CALIBRATION */ - } bulk_message_response_slam_calibration; - - /** - * @brief Bulk Set 6DoF Interrupt Rate Message - * - * Sets the rate of 6DoF interrupts. This is typically related to the host rendering rate. - */ - typedef struct { - uint8_t bInterruptRate; /**< Rate of 6DoF interrupts. The following values are supported: */ - /**< 0x0 - no interrupts */ - /**< 0x1 - interrupts on every fisheye camera update (30 interrupts per second for TM2) */ - /**< 0x2 - interrupts on every IMU update (400-500 interrupts per second for TM2) - default value */ - } sixdof_interrupt_rate_libtm_message; - - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 7 bytes, wMessageID = SLAM_SET_6DOF_INTERRUPT_RATE */ - sixdof_interrupt_rate_libtm_message message; /**< Rate of 6DoF interrupts */ - } bulk_message_request_set_6dof_interrupt_rate; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = SLAM_SET_6DOF_INTERRUPT_RATE */ - } bulk_message_response_set_6dof_interrupt_rate; - - - /** - * @brief Bulk 6DoF Control Message - * - * Enables / disables 6DoF calculation. - * If no SLAM_6DOF_CONTROL command was called before the host calls to DEV_START, the default value used shall be "Disable 6DoF". - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 9 bytes, wMessageID = SLAM_6DOF_CONTROL */ - uint8_t bEnable; /**< 0x00 - Disable 6DoF, 0x01 - Enable 6DoF */ - uint8_t bMode; /**< 0x00 - Normal Mode, 0x01 - Fast Playback, 0x02 - Mapping Enabled , 0x04 - Relocalization Enabled */ - } bulk_message_request_6dof_control; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = SLAM_6DOF_CONTROL */ - } bulk_message_response_6dof_control; - - - /** - * @brief Bulk Occupancy Map Control Message - * - * Enables/disables occupancy map calculation. Occupancy map calculation is based on 6DoF calculation, - * So it cannot be enabled when 6DoF is disabled, and an UNSUPPORTED error will be returned. - * If no SLAM_6DOF_CONTROL command was called before the host calls to DEV_START, the default value used shall be "Disable occupancy map". - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 7 bytes, wMessageID = SLAM_OCCUPANCY_MAP_CONTROL */ - uint8_t bEnable; /**< 0x00 - Disable occupancy map, 0x01 - Enable occupancy map */ - } bulk_message_request_occupancy_map_control; - - typedef struct { - bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = SLAM_OCCUPANCY_MAP_CONTROL */ - } bulk_message_response_occupancy_map_control; - - - /** - * @brief Stream Endpoint Protocol - * - * The stream endpoint is a bulk IN / OUT endpoint, used to stream out image streams as raw streams, as enabled in the "Enable Raw Streams" command, - * Or stream in image and IMU streams, as enabled in the "Enable Playback Streams" command. (IMU outputs are sent over the interrupt endpoint). - * Streaming streams both out and in at the same time shall be supported (effectively loopback - ing the input streams). - * The host reads from this endpoint to get new streaming data, and write new samples to playback them. - * Every sample is made from the common fields from below, with a variable specific metadata and data part (starting at byte 24 below). - */ - - - /** - * @brief Bulk raw stream header - */ - typedef struct { - bulk_message_request_header header; /**< Message request header: dwLength = 28 + dwMetadataLength + dwFrameLength bytes, wMessageID = DEV_SAMPLE */ - uint8_t bSensorID; /**< Bits 0-4: Type of sensor, supported values are: Color = 0, Depth = 1, IR = 2, Fisheye = 3, Gyro = 4, Accelerometer = 5 */ - /**< Bits 5-7: Sensor index - Zero based index of sensor with the same type within device. For example if the device supports two fisheye cameras, */ - /**< The first will use index 0 (bSensorID = 0x03) and the second will use index 1 (bSensorID = 0x23) */ - uint8_t bReserved; /**< Reserved = 0 */ - uint64_t llNanoseconds; /**< Frame integration timestamp, as measured in nanoseconds since device initialization */ - uint64_t llArrivalNanoseconds; /**< Frame arrival timestamp, as measured in nanoseconds since device initialization */ - uint32_t dwFrameId; /**< A running index of frames from every unique sensor. Starting from 0. */ - } bulk_message_raw_stream_header; - - - /** - * @brief Bulk raw video stream metadata - */ - typedef struct - { - uint32_t dwMetadataLength; /**< Metadata length in bytes (8 bytes) */ - uint32_t dwExposuretime; /**< Exposure time of this frame in microseconds */ - float_t fGain; /**< Gain multiplier of this frame */ - uint32_t dwFrameLength; /**< Length of frame below, in bytes, shall be equal to Stride X Height X BPP */ - uint8_t bFrameData[]; /**< Frame data variable sized array */ - } bulk_message_video_stream_metadata; - - - /** - * @brief Bulk raw video stream message - * - * Specific frame metadata and data for sensor IDs, color, depth, IR, Fisheye - */ - typedef struct - { - bulk_message_raw_stream_header rawStreamHeader; - bulk_message_video_stream_metadata metadata; - } bulk_message_video_stream; - - - /** - * @brief Bulk raw accelerometer stream metadata - */ - typedef struct - { - uint32_t dwMetadataLength; /**< Metadata length in bytes (4 bytes) */ - float_t flTemperature; /**< Accelerometer temperature */ - uint32_t dwFrameLength; /**< Length of frame below (12 bytes) */ - float_t flAx; /**< X value of acceleration, in meter/sec^2 */ - float_t flAy; /**< Y value of acceleration, in meter/sec^2 */ - float_t flAz; /**< Z value of acceleration, in meter/sec^2 */ - } bulk_message_accelerometer_stream_metadata; - - /** - * @brief Bulk raw accelerometer stream message - * - * Specific frame metadata and data for sensor IDs - accelerometer - */ - typedef struct - { - bulk_message_raw_stream_header rawStreamHeader; - bulk_message_accelerometer_stream_metadata metadata; - } bulk_message_accelerometer_stream; - - - /** - * @brief Bulk raw gyro stream metadata - */ - typedef struct - { - uint32_t dwMetadataLength; /**< Metadata length in bytes (4 bytes) */ - float_t flTemperature; /**< Gyro temperature */ - uint32_t dwFrameLength; /**< Length of frame below (12 bytes) */ - float_t flGx; /**< X value of gyro, in radians/sec */ - float_t flGy; /**< Y value of gyro, in radians/sec */ - float_t flGz; /**< Z value of gyro, in radians/sec */ - } bulk_message_gyro_stream_metadata; - - /** - * @brief Bulk raw gyro stream message - * - * Specific frame metadata and data for sensor IDs - gyro - */ - typedef struct - { - bulk_message_raw_stream_header rawStreamHeader; - bulk_message_gyro_stream_metadata metadata; - } bulk_message_gyro_stream; - - /** - * @brief Bulk raw velocimeter stream metadata - */ - typedef struct - { - uint32_t dwMetadataLength; /**< Metadata length in bytes (4 bytes) */ - float_t flTemperature; /**< velocimeter temperature */ - uint32_t dwFrameLength; /**< Length of frame below (12 bytes) */ - float_t flVx; /**< X value of velocimeter, in meters/sec */ - float_t flVy; /**< Y value of velocimeter, in meters/sec */ - float_t flVz; /**< Z value of velocimeter, in meters/sec */ - } bulk_message_velocimeter_stream_metadata; - - /** - * @brief Bulk raw velocimeter stream message - * - * Specific frame metadata and data for sensor IDs - velocimeter - */ - typedef struct - { - bulk_message_raw_stream_header rawStreamHeader; - bulk_message_velocimeter_stream_metadata metadata; - } bulk_message_velocimeter_stream; - - /** - * @brief Interrupt Endpoint Protocol - * - * The interrupt endpoint is used for short low-latency data (namely 6DoF pose data). - * The rate of the interrupts is determined by using the "Set 6DoF Interrupt Data" command. - */ - - /** - * @brief Interrupt message header struct - * - * Start of all Interrupt messages. - */ - typedef struct { - uint32_t dwLength; /**< Message length in bytes */ - uint16_t wMessageID; /**< ID of message */ - } interrupt_message_header; - - - /** - * @brief Interrupt error message - * - * A general error message - */ - typedef struct { - interrupt_message_header header; /**< Interrupt message header: dwLength = 8 bytes, wMessageID = DEV_ERROR */ - uint16_t wStatus; /**< Error code */ - } interrupt_message_general_error; - - - /** - * @brief Interrupt status message - * - * A status message - */ - typedef struct { - interrupt_message_header header; /**< Interrupt message header: dwLength = 8 bytes, wMessageID = DEV_STATUS */ - uint16_t wStatus; /**< Status of request (MESSAGE_STATUS) */ - } interrupt_message_status; - - /** - * @brief Interrupt get pose message - * - * returns the latest pose of the camera relative to its initial position - * If relocalization data was set, this pose is relative to the relocalization database - * The rate of the interrupts is determined by using the SLAM_SET_6DOF_INTERRUPT_RATE command. - */ - typedef struct { - interrupt_message_header header; /**< Interrupt message header: dwLength = 92 bytes, wMessageID = DEV_GET_POSE */ - uint8_t bIndex; /**< Index of HMD - 0x0 = HMD */ - uint8_t wReserved; /**< Reserved = 0 */ - pose_data pose; /**< Short low-latency data (namely 6DoF pose data) */ - } interrupt_message_get_pose; - - - /** - * @brief Interrupt raw stream header - */ - typedef struct { - interrupt_message_header header; /**< Message request header: dwLength = 28 + dwMetadataLength + dwFrameLength bytes, wMessageID = DEV_SAMPLE */ - uint8_t bSensorID; /**< Bits 0-4: Type of sensor, supported values are: Color = 0, Depth = 1, IR = 2, Fisheye = 3, Gyro = 4, Accelerometer = 5 */ - /**< Bits 5-7: Sensor index - Zero based index of sensor with the same type within device. For example if the device supports two fisheye cameras, */ - /**< The first will use index 0 (bSensorID = 0x03) and the second will use index 1 (bSensorID = 0x23) */ - uint8_t bReserved; /**< Reserved = 0 */ - uint64_t llNanoseconds; /**< Frame integration timestamp, as measured in nanoseconds since device initialization */ - uint64_t llArrivalNanoseconds; /**< Frame arrival timestamp, as measured in nanoseconds since device initialization */ - uint32_t dwFrameId; /**< A running index of frames from every unique sensor. Starting from 0. */ - } interrupt_message_raw_stream_header; - - - /** - * @brief Interrupt raw accelerometer stream metadata - */ - typedef struct - { - uint32_t dwMetadataLength; /**< Metadata length in bytes (4 bytes) */ - float_t flTemperature; /**< Accelerometer temperature */ - uint32_t dwFrameLength; /**< Length of frame below (12 bytes) */ - float_t flAx; /**< X value of acceleration, in meter/sec^2 */ - float_t flAy; /**< Y value of acceleration, in meter/sec^2 */ - float_t flAz; /**< Z value of acceleration, in meter/sec^2 */ - } interrupt_message_accelerometer_stream_metadata; - - - /** - * @brief Interrupt raw accelerometer stream message - * - * Specific frame metadata and data for sensor IDs - accelerometer - */ - typedef struct - { - interrupt_message_raw_stream_header rawStreamHeader; - interrupt_message_accelerometer_stream_metadata metadata; - } interrupt_message_accelerometer_stream; - - - /** - * @brief Interrupt raw gyro stream metadata - */ - typedef struct - { - uint32_t dwMetadataLength; /**< Metadata length in bytes (4 bytes) */ - float_t flTemperature; /**< Gyro temperature */ - uint32_t dwFrameLength; /**< Length of frame below (12 bytes) */ - float_t flGx; /**< X value of gyro, in radians/sec */ - float_t flGy; /**< Y value of gyro, in radians/sec */ - float_t flGz; /**< Z value of gyro, in radians/sec */ - } interrupt_message_gyro_stream_metadata; - - - /** - * @brief Interrupt raw gyro stream message - * - * Specific frame metadata and data for sensor IDs - gyro - */ - typedef struct - { - interrupt_message_raw_stream_header rawStreamHeader; - interrupt_message_gyro_stream_metadata metadata; - } interrupt_message_gyro_stream; - - - /** - * @brief Interrupt raw velocimeter stream metadata - */ - typedef struct - { - uint32_t dwMetadataLength; /**< Metadata length in bytes (4 bytes) */ - float_t flTemperature; /**< velocimeter temperature */ - uint32_t dwFrameLength; /**< Length of frame below (12 bytes) */ - float_t flVx; /**< X value of velocimeter, in radians/sec */ - float_t flVy; /**< Y value of velocimeter, in radians/sec */ - float_t flVz; /**< Z value of velocimeter, in radians/sec */ - } interrupt_message_velocimeter_stream_metadata; - - - /** - * @brief Interrupt raw velocimeter stream message - * - * Specific frame metadata and data for sensor IDs - velocimeter - */ - typedef struct - { - interrupt_message_raw_stream_header rawStreamHeader; - interrupt_message_velocimeter_stream_metadata metadata; - } interrupt_message_velocimeter_stream; - - /** - * @brief Interrupt SLAM error message - * - * SLAM error code message - */ - typedef struct { - interrupt_message_header header; /**< Interrupt message header: dwLength = 8 bytes, wMessageID = SLAM_ERROR */ - uint16_t wStatus; /**< Status: SLAM_ERROR_CODE_NONE = 0 - No error has occurred. */ - /**< SLAM_ERROR_CODE_VISION = 1 - No visual features were detected in the most recent image. */ - /**< This is normal in some circumstances, such as quick motion or if the device temporarily looks at a blank wall. */ - /**< SLAM_ERROR_CODE_SPEED = 2 - The device moved more rapidly than expected for typical handheld motion. */ - /**< This may indicate that rc_Tracker has failed and is providing invalid data. */ - /**< SLAM_ERROR_CODE_OTHER = 3 - A fatal internal error has occurred. */ - } interrupt_message_slam_error; - - /** - * @brief Interrupt Get Localization Data Stream message - * - * Triggers by bulk_message_request_get_localization_data. - * Returns the localization data as created during a 6DoF session. - * This message is generated and streamed by the underlying firmware algorithm in run-time, so the total size of the data cannot be known in advance. - * The entire data will be streams in "chunks" and the firmware will use the MORE_DATA_AVAILABLE to indicate there are more data to send. - * The last chunk (possibly even a zero-size chunk) will be marked with a SUCCESS status code. - */ - typedef struct { - interrupt_message_header header; /**< Interrupt message header: dwLength = 8 bytes + bLocalizationData size, wMessageID = SLAM_GET_LOCALIZATION_DATA_STREAM */ - uint16_t wStatus; /**< SUCCESS: indicate last chunk, MORE_DATA_AVAILABLE: any other chunk */ - uint16_t wIndex; /**< A running counter starting at 0 identifying the chunk index in a single data transaction */ - uint8_t bLocalizationData[]; /**< Localization data variable sized array. Data format is algorithm specific and opaque to the USB and host stack */ - } interrupt_message_get_localization_data_stream; - - - /** - * @brief Interrupt Set Localization Data Stream message - * - * Response after setting localization data using bulk_message_set_localization_data_stream - */ - typedef struct { - interrupt_message_header header; /**< Interrupt message header: dwLength = 8 bytes, wMessageID = SLAM_SET_LOCALIZATION_DATA_STREAM */ - uint16_t wStatus; /**< Status */ - uint8_t bReserved; /**< Reserved = 0 */ - } interrupt_message_set_localization_data_stream; - - - /** - * @brief Interrupt SLAM Relocalization Event message - * - * Event information when SLAM relocalizes. - */ - typedef struct { - interrupt_message_header header; /**< Interrupt message header: wMessageID = SLAM_RELOCALIZATION_EVENT */ - uint64_t llNanoseconds; /**< Timestamp of relocalization event, measured in nanoseconds since device system initialization */ - uint16_t wSessionId; /**< Session id of the relocalized map. Current session if 0, previous session otherwise */ - } interrupt_message_slam_relocalization_event; - - /** - * @brief Control message request header struct - * - * Start of all USB control message requests. - */ - typedef struct { - uint8_t bmRequestType; /**< Bit 7: Request direction (0 = Host to device - Out, 1 = Device to host - In) */ - /**< Bits 5 - 6 : Request type (0 = standard, 1 = class, 2 = vendor, 3 = reserved) */ - /**< Bits 0 - 4 : Recipient (0 = device, 1 = interface, 2 = endpoint, 3 = other) */ - uint8_t bRequest; /**< Actual request ID */ - uint8_t wValueL; /**< Unused */ - uint8_t wValueH; /**< Unused */ - uint8_t wIndexL; /**< Unused */ - uint8_t wIndexH; /**< Unused */ - uint8_t wLengthL; /**< Unused */ - uint8_t wLengthH; /**< Unused */ - } control_message_request_header; - - /** - * @brief Control reset Message - * - * Reset the device - */ - typedef struct { - control_message_request_header header; /**< Message request header: bmRequestType = 0x40 (Direction - host to device, type - vendor, recipient - device), wMessageID = CONTROL_USB_RESET */ - } control_message_request_reset; - - /** - * @brief Control get interface version Message - * - * Get FW interface version - */ - typedef struct { - uint32_t dwMajor; /**< Major part of the device supported interface API version, updated upon an incompatible API change */ - uint32_t dwMinor; /**< Minor part of the device supported interface API version, updated upon a backwards-compatible change */ - } interface_version_libtm_message; - - typedef struct { - control_message_request_header header; /**< Message request header: bmRequestType = 0xC0 (Direction - device to host, type - vendor, recipient - device), wMessageID = CONTROL_GET_INTERFACE_VERSION */ - } control_message_request_get_interface_version; - - typedef struct { - interface_version_libtm_message message; /**< Interface version */ - } control_message_response_get_interface_version; - - -#pragma pack(pop) - -} -#ifdef _WIN32 -#pragma warning (pop) -#endif diff --git a/src/tm2/tm-boot.h b/src/tm2/tm-boot.h deleted file mode 100644 index 7884d46586..0000000000 --- a/src/tm2/tm-boot.h +++ /dev/null @@ -1,54 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2017 Intel Corporation. All Rights Reserved. - -#pragma once - -#include "../usb/usb-device.h" -#include "../usb/usb-enumerator.h" -#include "../types.h" - -#ifdef WITH_TRACKING -#include "common/fw/target.h" -#endif - -namespace librealsense { - namespace platform { -#ifdef WITH_TRACKING - bool tm_boot(const std::vector & devices) - { - bool found = false; - for(const auto & device_info : devices) { - if(device_info.vid == 0x03E7 && device_info.pid == 0x2150) { - LOG_INFO("Found a T265 to boot"); - found = true; - auto dev = usb_enumerator::create_usb_device(device_info); - if (const auto& m = dev->open(0)) - { - // transfer the firmware data - int size{}; - auto target_hex = fw_get_target(size); - - if(!target_hex) - LOG_ERROR("librealsense failed to get T265 FW resource"); - - auto iface = dev->get_interface(0); - auto endpoint = iface->first_endpoint(RS2_USB_ENDPOINT_DIRECTION_WRITE); - uint32_t transfered = 0; - auto status = m->bulk_transfer(endpoint, const_cast(target_hex), static_cast(size), transfered, 1000); - if(status != RS2_USB_STATUS_SUCCESS) - LOG_ERROR("Error booting T265"); - } - else - LOG_ERROR("Failed to open T265 zero interface"); - } - } - return found; - } -#else - bool tm_boot(const std::vector & devices) - { - return false; - } -#endif - } -} diff --git a/src/tm2/tm-device.cpp b/src/tm2/tm-device.cpp deleted file mode 100644 index 4e9d80cc5a..0000000000 --- a/src/tm2/tm-device.cpp +++ /dev/null @@ -1,2170 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2017 Intel Corporation. All Rights Reserved. - -#ifndef NOMINMAX - #define NOMINMAX -#endif - -#include -#include -#include // PRIu64 -#include "tm-device.h" -#include "stream.h" -#include "media/playback/playback_device.h" -#include "media/ros/ros_reader.h" -#include "usb/usb-enumerator.h" - -#include - - -// uncomment to get debug messages at info severity -//#undef LOG_DEBUG -//#define LOG_DEBUG LOG_INFO - -using namespace std::chrono; - -#ifdef __APPLE__ - -#include -#define htobe64(x) OSSwapHostToBigInt64(x) - -#endif - -static uint64_t bytesSwap(uint64_t val) -{ -#if defined(__linux__) || defined(__APPLE__) - return htobe64(val); -#else - return _byteswap_uint64(val); -#endif -} - -#include "t265-messages.h" -using namespace t265; - -#include "message-print.h" - -#define SENSOR_TYPE_POS (0) -#define SENSOR_TYPE_MASK (0x001F << SENSOR_TYPE_POS) /**< Bits 0-4 */ -#define SENSOR_INDEX_POS (5) -#define SENSOR_INDEX_MASK (0x0007 << SENSOR_INDEX_POS) /**< Bits 5-7 */ -#define SET_SENSOR_ID(_type, _index) ((((_type) << SENSOR_TYPE_POS) & SENSOR_TYPE_MASK) | (((_index) << SENSOR_INDEX_POS) & SENSOR_INDEX_MASK)) -#define GET_SENSOR_INDEX(_sensorID) ((_sensorID & SENSOR_INDEX_MASK) >> SENSOR_INDEX_POS) -#define GET_SENSOR_TYPE(_sensorID) ((_sensorID & SENSOR_TYPE_MASK) >> SENSOR_TYPE_POS) - - -// on the MCCI (Myriad) device, endpoint numbering starts at 0 -#define ENDPOINT_DEV_IN 0 -#define ENDPOINT_DEV_OUT 0 -#define ENDPOINT_DEV_MSGS_IN 1 -#define ENDPOINT_DEV_MSGS_OUT 1 -#define ENDPOINT_DEV_INT_IN 2 -#define ENDPOINT_DEV_INT_OUT 2 - - -// on host, endpoint numbering starts at 1 and has a 0x80 when the -// direction is into the host -#define ENDPOINT_HOST_IN (ENDPOINT_DEV_OUT + 1 + 0x80) -#define ENDPOINT_HOST_OUT (ENDPOINT_DEV_IN + 1) -#define ENDPOINT_HOST_MSGS_IN (ENDPOINT_DEV_MSGS_OUT + 1 + 0x80) -#define ENDPOINT_HOST_MSGS_OUT (ENDPOINT_DEV_MSGS_IN + 1) -#define ENDPOINT_HOST_INT_IN (ENDPOINT_DEV_INT_OUT + 1 + 0x80) -#define ENDPOINT_HOST_INT_OUT (ENDPOINT_DEV_INT_IN + 1) - -enum log_level { - NO_LOG = 0x0000, - LOG_ERR = 0x0001, - INFO = 0x0002, - WARNING = 0x0003, - DEBUG = 0x0004, - VERBOSE = 0x0005, - TRACE = 0x0006, - MAX_LOG_LEVEL -}; - -#define FREE_CONT 0x8001 - -static const int MAX_TRANSFER_SIZE = 848 * 800 + sizeof(bulk_message_video_stream); // max size on ENDPOINT_HOST_IN -static const int USB_TIMEOUT = 10000/*ms*/; -static const int BUFFER_SIZE = 1024; // Max size for control transfers - -const std::map tm2_formats_map = -{ - { PixelFormat::ANY, RS2_FORMAT_ANY }, - { PixelFormat::Z16, RS2_FORMAT_Z16 }, - { PixelFormat::DISPARITY16, RS2_FORMAT_DISPARITY16 }, - { PixelFormat::XYZ32F, RS2_FORMAT_XYZ32F }, - { PixelFormat::YUYV, RS2_FORMAT_YUYV }, - { PixelFormat::RGB8, RS2_FORMAT_RGB8 }, - { PixelFormat::BGR8, RS2_FORMAT_BGR8 }, - { PixelFormat::RGBA8, RS2_FORMAT_RGBA8 }, - { PixelFormat::BGRA8, RS2_FORMAT_BGRA8 }, - { PixelFormat::Y8, RS2_FORMAT_Y8 }, - { PixelFormat::Y16, RS2_FORMAT_Y16 }, - { PixelFormat::RAW8, RS2_FORMAT_RAW8 }, - { PixelFormat::RAW10, RS2_FORMAT_RAW10 }, - { PixelFormat::RAW16, RS2_FORMAT_RAW16 } -}; - -inline rs2_format rs2_format_from_tm2(const uint8_t format) -{ - return tm2_formats_map.at(static_cast(format)); -} - -inline int tm2_sensor_type(rs2_stream rtype) -{ - if(rtype == RS2_STREAM_FISHEYE) - return SensorType::Fisheye; - else if(rtype == RS2_STREAM_ACCEL) - return SensorType::Accelerometer; - else if(rtype == RS2_STREAM_GYRO) - return SensorType::Gyro; - else if(rtype == RS2_STREAM_POSE) - return SensorType::Pose; - else - throw librealsense::invalid_value_exception("Invalid stream type"); -} - -inline int tm2_sensor_id(rs2_stream type, int stream_index) -{ - int stream_type = tm2_sensor_type(type); - if (stream_type == SensorType::Fisheye) - stream_index--; - - return stream_index; -} - -namespace librealsense -{ - struct video_frame_metadata { - int64_t arrival_ts; - uint32_t exposure_time; - }; - - struct motion_frame_metadata { - int64_t arrival_ts; - float temperature; - }; - - struct pose_frame_metadata { - int64_t arrival_ts; - }; - - enum temperature_type { TEMPERATURE_TYPE_ASIC, TEMPERATURE_TYPE_MOTION}; - - class temperature_option : public readonly_option - { - public: - float query() const override { return _ep.get_temperature(_type).fTemperature; } - - option_range get_range() const override { return _range; } - - bool is_enabled() const override { return true; } - - explicit temperature_option(tm2_sensor& ep, temperature_type type) : _ep(ep), _type(type), - _range(option_range{ 0, _ep.get_temperature(_type).fThreshold, 0, 0 }) { } - - private: - tm2_sensor& _ep; - temperature_type _type; - option_range _range; - }; - - class exposure_option : public option_base - { - public: - float query() const override { return _ep.get_exposure(); } - - void set(float value) override { return _ep.set_exposure(value); } - - const char* get_description() const override { return "Exposure"; } - - bool is_enabled() const override { return true; } - - explicit exposure_option(tm2_sensor& ep) : _ep(ep), - option_base(option_range{ 200, 16000, 0, 200 }) { } - - private: - tm2_sensor& _ep; - }; - - class exposure_mode_option : public option_base - { - public: - float query() const override { return !_ep.is_manual_exposure(); } - - void set(float value) override { return _ep.set_manual_exposure(1.f - value); } - - const char* get_description() const override { return "Auto-Exposure"; } - - bool is_enabled() const override { return true; } - - explicit exposure_mode_option(tm2_sensor& ep) : _ep(ep), - option_base(option_range{ 0, 1, 1, 1 }) { } - - private: - tm2_sensor& _ep; - }; - - class gain_option : public option_base - { - public: - float query() const override - { - return _ep.get_gain(); - } - - void set(float value) override - { - return _ep.set_gain(value); - } - - const char* get_description() const override { return "Gain"; } - - bool is_enabled() const override { return true; } - - explicit gain_option(tm2_sensor& ep) : _ep(ep), - option_base(option_range{ 1, 10, 0, 1 }) { } - - private: - tm2_sensor& _ep; - }; - - template - class tracking_mode_option : public option_base - { - public: - float query() const override { return !!(s._tm_mode & flag) ^ invert ? 1 : 0; } - - void set(float value) override { - if (s._is_streaming) - throw io_exception("Option is read-only while streaming"); - s._tm_mode = (!!value ^ invert) ? (s._tm_mode | flag) : (s._tm_mode & ~flag); - } - - const char* get_description() const override { return description; } - - bool is_enabled() const override { return !depends_on || (s._tm_mode & depends_on) ? true : false; } - - bool is_read_only() const override { return s._is_streaming; } - - explicit tracking_mode_option(tm2_sensor& sensor, const char *description_) : - s(sensor), description(description_), option_base(option_range{ 0, 1, 1, !!(sensor._tm_mode & flag) ^ invert ? 1.f : 0.f }) { } - - private: - tm2_sensor &s; - const char *description; - }; - - class asic_temperature_option : public temperature_option - { - public: - const char* get_description() const override - { - return "Current T265 Asic Temperature (degree celsius)"; - } - explicit asic_temperature_option(tm2_sensor& ep) :temperature_option(ep, temperature_type::TEMPERATURE_TYPE_ASIC) { } - }; - - class motion_temperature_option : public temperature_option - { - public: - - const char* get_description() const override - { - return "Current T265 IMU Temperature (degree celsius)"; - } - explicit motion_temperature_option(tm2_sensor& ep) :temperature_option(ep, temperature_type::TEMPERATURE_TYPE_MOTION) { } - }; - - class md_tm2_parser : public md_attribute_parser_base - { - public: - md_tm2_parser(rs2_frame_metadata_value type) : _type(type) {} - rs2_metadata_type get(const frame& frm) const override - { - if(_type == RS2_FRAME_METADATA_ACTUAL_EXPOSURE) - { - if (auto* vf = dynamic_cast(&frm)) - { - const video_frame_metadata* md = reinterpret_cast(frm.additional_data.metadata_blob.data()); - return (rs2_metadata_type)(md->exposure_time); - } - } - if(_type == RS2_FRAME_METADATA_TIME_OF_ARRIVAL) - { - // Note: additional_data.system_time is the arrival time - // (backend_time is what we have traditionally called - // system_time) - if (auto* vf = dynamic_cast(&frm)) - { - return (rs2_metadata_type)(vf->additional_data.system_time); - } - if (auto* mf = dynamic_cast(&frm)) - { - return (rs2_metadata_type)(mf->additional_data.system_time); - } - if (auto* pf = dynamic_cast(&frm)) - { - return (rs2_metadata_type)(pf->additional_data.system_time); - } - } - if(_type == RS2_FRAME_METADATA_FRAME_TIMESTAMP) - { - if (auto* vf = dynamic_cast(&frm)) - { - return (rs2_metadata_type)(vf->additional_data.timestamp*1e+3); - } - if (auto* mf = dynamic_cast(&frm)) - { - return (rs2_metadata_type)(mf->additional_data.timestamp*1e+3); - } - if (auto* pf = dynamic_cast(&frm)) - { - return (rs2_metadata_type)(pf->additional_data.timestamp*1e+3); - } - } - if (_type == RS2_FRAME_METADATA_TEMPERATURE) - { - if (auto* mf = dynamic_cast(&frm)) - { - const motion_frame_metadata* md = reinterpret_cast(frm.additional_data.metadata_blob.data()); - return (rs2_metadata_type)(md->temperature); - } - } - return 0; - } - - bool supports(const frame& frm) const override - { - if (_type == RS2_FRAME_METADATA_ACTUAL_EXPOSURE) - { - return dynamic_cast(&frm) != nullptr; - } - if (_type == RS2_FRAME_METADATA_TEMPERATURE) - { - return dynamic_cast(&frm) != nullptr; - } - if (_type == RS2_FRAME_METADATA_TIME_OF_ARRIVAL) - { - return dynamic_cast(&frm) != nullptr || dynamic_cast(&frm) != nullptr; - } - if (_type == RS2_FRAME_METADATA_FRAME_TIMESTAMP) - { - return (dynamic_cast(&frm) != nullptr) || (dynamic_cast(&frm) != nullptr) || (dynamic_cast(&frm) != nullptr); - } - return false; - } - private: - rs2_frame_metadata_value _type; - }; - - tm2_sensor::tm2_sensor(tm2_device* owner) - : sensor_base("Tracking Module", owner, this), _device(owner) - { - LOG_DEBUG("Making a sensor " << this); - _source.set_max_publish_list_size(256); //increase frame source queue size for TM2 - _data_dispatcher = std::make_shared(256); // make a queue of the same size to dispatch data messages - _data_dispatcher->start(); - register_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE, std::make_shared(RS2_FRAME_METADATA_ACTUAL_EXPOSURE)); - register_metadata(RS2_FRAME_METADATA_TEMPERATURE , std::make_shared(RS2_FRAME_METADATA_TEMPERATURE)); - //Replacing md parser for TIME_OF_ARRIVAL and FRAME_TIMESTAMP - _metadata_parsers->erase(RS2_FRAME_METADATA_TIME_OF_ARRIVAL); - _metadata_parsers->erase(RS2_FRAME_METADATA_FRAME_TIMESTAMP); - register_metadata(RS2_FRAME_METADATA_TIME_OF_ARRIVAL,std::make_shared(RS2_FRAME_METADATA_TIME_OF_ARRIVAL)); - register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, std::make_shared(RS2_FRAME_METADATA_FRAME_TIMESTAMP)); - - // Set log level - bulk_message_request_log_control log_request = {{ sizeof(log_request), DEV_LOG_CONTROL }}; - log_request.bVerbosity = log_level::LOG_ERR; - log_request.bLogMode = 0x1; // rollover mode - bulk_message_response_log_control log_response = {}; - _device->bulk_request_response(log_request, log_response); - - // start log thread - _log_poll_thread_stop = false; - _log_poll_thread = std::thread(&tm2_sensor::log_poll, this); - - // start time sync thread - last_ts = { std::chrono::duration(0) }; - device_to_host_ns = 0; - _time_sync_thread_stop = false; - _time_sync_thread = std::thread(&tm2_sensor::time_sync, this); - } - - tm2_sensor::~tm2_sensor() - { - // We have to do everything in dispose() because the tm2_device gets destroyed - // before this destructor gets called (since it is a member - // variable) - } - - void tm2_sensor::dispose() - { - // It is important not to throw here because this gets called - // from ~tm2_device - try { - _data_dispatcher->stop(); - // Use this as a proxy to know if we are still able to communicate with the device - bool device_valid = _stream_request && _interrupt_request; - - if (device_valid && _is_streaming) - stop(); - - if (device_valid && _is_opened) - close(); - - _time_sync_thread_stop = true; - _time_sync_thread.join(); - - if (device_valid) { - stop_stream(); - stop_interrupt(); - } - - _log_poll_thread_stop = true; - _log_poll_thread.join(); - } - catch (...) { - LOG_ERROR("An error has occurred while disposing the sensor!"); - } - - } - - //sensor - //////// - stream_profiles tm2_sensor::init_stream_profiles() - { - stream_profiles results; - std::map > profile_map; - - bulk_message_request_get_supported_raw_streams request = {{ sizeof(request), DEV_GET_SUPPORTED_RAW_STREAMS }}; - char buffer[BUFFER_SIZE]; - auto response = (bulk_message_response_get_supported_raw_streams *)buffer; - _device->bulk_request_response(request, *response, BUFFER_SIZE); - - // Note the pose stream is special. We need to create one for - // librealsense below, but it is not a raw_stream as defined - // by our current protocol. - - _supported_raw_streams.clear(); - for(int i = 0; i < response->wNumSupportedStreams; i++) { - auto tm_stream = response->stream[i]; - - auto sensor_type = GET_SENSOR_TYPE(tm_stream.bSensorID); - auto sensor_id = GET_SENSOR_INDEX(tm_stream.bSensorID); - if(sensor_type == SensorType::Fisheye) { - if (sensor_id > 1) { - LOG_ERROR("Fisheye with sensor id > 1: " << sensor_id); - continue; - } - - rs2_stream stream = RS2_STREAM_FISHEYE; - rs2_format format = rs2_format_from_tm2(tm_stream.bPixelFormat); - platform::stream_profile p = { tm_stream.wWidth, tm_stream.wHeight, tm_stream.wFramesPerSecond, uint32_t(format) }; - auto profile = std::make_shared(p); - profile->set_dims(p.width, p.height); - profile->set_stream_type(stream); - profile->set_stream_index(sensor_id + 1); // for nice presentation by the viewer - add 1 to stream index - profile->set_format(format); - profile->set_framerate(p.fps); - profile->set_unique_id(environment::get_instance().generate_stream_id()); - stream_profile sp = { profile->get_format(), stream, profile->get_stream_index(), p.width, p.height, p.fps }; - auto intrinsics = get_intrinsics(sp); - - profile->set_intrinsics([intrinsics]() { return intrinsics; }); - profile->tag_profile(profile_tag::PROFILE_TAG_DEFAULT | profile_tag::PROFILE_TAG_SUPERSET); - results.push_back(profile); - profile_map[tm_stream.bSensorID] = profile; - - LOG_INFO("Added a fisheye sensor id: " << sensor_id); - } - else if(sensor_type == SensorType::Gyro || sensor_type == SensorType::Accelerometer) { - std::string sensor_str = sensor_type == SensorType::Gyro ? "gyro" : "accel"; - if(sensor_id != 0) { - LOG_ERROR(sensor_str << " with sensor id != 0: " << sensor_id); - continue; - } - - rs2_format format = RS2_FORMAT_MOTION_XYZ32F; - rs2_stream stream = (sensor_type == SensorType::Gyro) ? RS2_STREAM_GYRO : RS2_STREAM_ACCEL; - if(stream == RS2_STREAM_GYRO && tm_stream.wFramesPerSecond != 200) { - LOG_DEBUG("Skipping gyro FPS " << tm_stream.wFramesPerSecond); - continue; - } - if(stream == RS2_STREAM_ACCEL && tm_stream.wFramesPerSecond != 62) { - LOG_DEBUG("Skipping accel FPS " << tm_stream.wFramesPerSecond); - continue; - } - auto profile = std::make_shared(platform::stream_profile{ uint32_t(format), 0, 0, tm_stream.wFramesPerSecond }); - profile->set_stream_type(stream); - profile->set_stream_index(sensor_id); // for nice presentation by the viewer - add 1 to stream index - profile->set_format(format); - profile->set_framerate(tm_stream.wFramesPerSecond); - profile->set_unique_id(environment::get_instance().generate_stream_id()); - auto intrinsics = get_motion_intrinsics(*profile); - profile->set_intrinsics([intrinsics]() { return intrinsics; }); - - profile->tag_profile(profile_tag::PROFILE_TAG_DEFAULT | profile_tag::PROFILE_TAG_SUPERSET); - results.push_back(profile); - profile_map[tm_stream.bSensorID] = profile; - - LOG_INFO("Added a " << sensor_str << " sensor id: " << sensor_id << " at " << tm_stream.wFramesPerSecond << "Hz"); - } - else if(sensor_type == SensorType::Velocimeter) { - LOG_INFO("Skipped a velocimeter stream"); - // Nothing to do for velocimeter streams yet - continue; - } - // This one should never show up, because the device doesn't - // actually stream poses over the bulk endpoint - else if(sensor_type == SensorType::Pose) { - LOG_ERROR("Found a pose stream but should not have one here"); - continue; - } - else if(sensor_type == SensorType::Mask) { - //TODO: implement Mask - continue; - } - else { - LOG_ERROR("Invalid stream type " << sensor_type << " with sensor id: " << sensor_id); - throw io_exception("Invalid stream type"); - } - - _supported_raw_streams.push_back(tm_stream); - } - LOG_DEBUG("Total streams " << _supported_raw_streams.size()); - - // Add a "special" pose stream profile, because we handle this - // one differently and it is implicitly always available - rs2_format format = RS2_FORMAT_6DOF; - const uint32_t pose_fps = 200; - auto profile = std::make_shared(platform::stream_profile{ uint32_t(format), 0, 0, pose_fps }); - profile->set_stream_type(RS2_STREAM_POSE); - profile->set_stream_index(0); - profile->set_format(format); - profile->set_framerate(pose_fps); - profile->set_unique_id(environment::get_instance().generate_stream_id()); - - profile->tag_profile(profile_tag::PROFILE_TAG_DEFAULT | profile_tag::PROFILE_TAG_SUPERSET); - // The RS2_STREAM_POSE profile is the reference for extrinsics - auto reference_profile = profile; - results.push_back(profile); - profile_map[SET_SENSOR_ID(SensorType::Pose, 0)] = profile; - - //add extrinsic parameters - for (auto profile : results) - { - auto current_extrinsics = get_extrinsics(*profile, 0); // TODO remove 0 - environment::get_instance().get_extrinsics_graph().register_extrinsics(*profile, *reference_profile, current_extrinsics); - } - - auto accel_it = std::find_if(results.begin(), results.end(), - [](std::shared_ptr spi) { return RS2_STREAM_ACCEL == spi->get_stream_type(); }); - auto gyro_it = std::find_if(results.begin(), results.end(), - [](std::shared_ptr spi) { return RS2_STREAM_GYRO == spi->get_stream_type(); }); - if ((accel_it != results.end()) && (gyro_it != results.end())) - environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*(accel_it->get()), *(gyro_it->get())); - return results; - } - - void tm2_sensor::open(const stream_profiles& requests) - { - LOG_DEBUG("T265 open"); - std::lock_guard lock(_tm_op_lock); - if (_is_streaming) - throw wrong_api_call_sequence_exception("open(...) failed. T265 device is streaming!"); - else if (_is_opened) - throw wrong_api_call_sequence_exception("open(...) failed. T265 device is already opened!"); - - _source.init(_metadata_parsers); - _source.set_sensor(this->shared_from_this()); - - //TODO - TM2_API currently supports a single profile is supported per stream. - // this function uses stream index to determine stream profile, and just validates the requested profile. - // need to fix this to search among supported profiles per stream. ( use sensor_base::resolve_requests?) - if (_loopback) - { - auto& loopback_sensor = _loopback->get_sensor(0); - //TODO: Future work: filter out raw streams according to requested output. - loopback_sensor.open(loopback_sensor.get_stream_profiles()); - } - - _active_raw_streams.clear(); - for(auto p : _supported_raw_streams) { - p.bOutputMode = 0; // disable output - _active_raw_streams.push_back(p); - } - - for (auto&& r : requests) { - auto sp = to_profile(r.get()); - int stream_index = sp.index; - rs2_stream stream_type = r->get_stream_type(); - int tm_sensor_type = tm2_sensor_type(stream_type); - int tm_sensor_id = tm2_sensor_id(stream_type, stream_index); - LOG_INFO("Request for stream type " << r->get_stream_type() << " with stream index " << stream_index); - - if(stream_type == RS2_STREAM_POSE) { - if(stream_index != 0) - throw invalid_value_exception("Invalid profile configuration - pose stream only supports index 0"); - LOG_DEBUG("Pose output enabled"); - _pose_output_enabled = true; - continue; - } - - bool found = false; - for(auto & tm_profile : _active_raw_streams) { - if(GET_SENSOR_TYPE(tm_profile.bSensorID) == tm_sensor_type && - GET_SENSOR_INDEX(tm_profile.bSensorID) == tm_sensor_id && - tm_profile.wFramesPerSecond == sp.fps) { - - if(tm_sensor_type != SensorType::Fisheye || - (tm_profile.wWidth == sp.width && tm_profile.wHeight == sp.height && - rs2_format_from_tm2(tm_profile.bPixelFormat) == sp.format)) { - if(_device->usb_info.conn_spec < platform::usb3_type) - LOG_ERROR("Streaming T265 video over USB " << platform::usb_spec_names.at(_device->usb_info.conn_spec) << " is unreliable, please use USB 3 or only stream poses"); - - found = true; - tm_profile.bOutputMode = 1; - break; - } - } - } - if(!found) - throw invalid_value_exception("Invalid profile configuration - no matching stream"); - } - - int fisheye_streams = 0; - for(auto tm_profile : _active_raw_streams) - if(GET_SENSOR_TYPE(tm_profile.bSensorID) == SensorType::Fisheye && tm_profile.bOutputMode) - fisheye_streams++; - - if(fisheye_streams == 1) - throw invalid_value_exception("Invalid profile configuration - setting a single FE stream is not supported"); - - int num_active = 0; - for(auto p : _active_raw_streams) - if(p.bOutputMode) - num_active++; - if(_pose_output_enabled) num_active++; - LOG_INFO("Activated " << num_active << "/" << _active_raw_streams.size() + 1 << " streams"); - - // Construct message to the FW to activate the streams we want - char buffer[BUFFER_SIZE]; - auto request = (bulk_message_request_raw_streams_control *)buffer; - request->header.wMessageID = _loopback ? DEV_RAW_STREAMS_PLAYBACK_CONTROL : DEV_RAW_STREAMS_CONTROL; - request->wNumEnabledStreams = uint32_t(_active_raw_streams.size()); - memcpy(request->stream, _active_raw_streams.data(), request->wNumEnabledStreams*sizeof(supported_raw_stream_libtm_message)); - request->header.dwLength = request->wNumEnabledStreams * sizeof(supported_raw_stream_libtm_message) + sizeof(request->header) + sizeof(request->wNumEnabledStreams); - bulk_message_response_raw_streams_control response; - for(int i = 0; i < 5; i++) { - _device->bulk_request_response(*request, response, sizeof(response), false); - if(response.header.wStatus == DEVICE_BUSY) { - LOG_WARNING("Device is busy, trying again"); - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - else if(response.header.wStatus == INVALID_REQUEST_LEN) - throw io_exception("open(...) failed. Invalid stream request packet"); - else if(response.header.wStatus == INVALID_PARAMETER) - throw io_exception("open(...) failed. Invalid stream specification"); - else if(response.header.wStatus != SUCCESS) - throw io_exception(rsutils::string::from() << "open(...) unknown error " << status_name(response.header)); - else - break; - } - if(response.header.wStatus == DEVICE_BUSY) - throw wrong_api_call_sequence_exception("open(...) failed to configure streams. T265 is running!"); - - bulk_message_request_6dof_control control_request = {{ sizeof(control_request), SLAM_6DOF_CONTROL }}; - control_request.bEnable = _pose_output_enabled; - control_request.bMode = _tm_mode; - bulk_message_response_6dof_control control_response = {}; - _device->bulk_request_response(control_request, control_response, sizeof(control_response), false); - if(control_response.header.wStatus == DEVICE_BUSY) - throw wrong_api_call_sequence_exception("open(...) failed. T265 is running!"); - else if(control_response.header.wStatus != SUCCESS) - throw io_exception(rsutils::string::from() << "open(...) unknown error opening " << status_name(response.header)); - - _is_opened = true; - set_active_streams(requests); - } - - void tm2_sensor::close() - { - std::lock_guard lock(_tm_op_lock); - LOG_DEBUG("T265 close"); - if (_is_streaming) - throw wrong_api_call_sequence_exception("close() failed. T265 device is streaming!"); - else if (!_is_opened) - throw wrong_api_call_sequence_exception("close() failed. T265 device was not opened!"); - - if (_loopback) { - auto& loopback_sensor = _loopback->get_sensor(0); - loopback_sensor.close(); - } - - //reset active profiles - _active_raw_streams.clear(); - _pose_output_enabled = false; - - _is_opened = false; - set_active_streams({}); - } - - void tm2_sensor::pass_frames_to_fw(frame_holder fref) - { - //TODO - /* - auto to_nanos = [](rs2_time_t t) { return std::chrono::duration_cast(std::chrono::duration(t)).count(); }; - auto frame_ptr = fref.frame; - auto get_md_or_default = [frame_ptr](rs2_frame_metadata_value md) { - rs2_metadata_type v = 0; - if (frame_ptr->supports_frame_metadata(md)) - v = frame_ptr->get_frame_metadata(md); - return v; - }; - - - int stream_index = fref.frame->get_stream()->get_stream_index() - 1; - auto lrs_stream = fref.frame->get_stream()->get_stream_type(); - SensorType tm_stream; - if (!try_convert(lrs_stream, tm_stream)) - { - LOG_ERROR("Failed to convert lrs stream " << lrs_stream << " to tm stream"); - return; - } - - //Pass frames to firmware - if (auto vframe = As(fref.frame)) - { - TrackingData::VideoFrame f = {}; - f.sensorIndex = stream_index; - //TODO: Librealsense frame numbers are 64 bit. This is a potentional interface gap - f.frameId = vframe->additional_data.frame_number; - f.profile.set(vframe->get_width(), vframe->get_height(), vframe->get_stride(), convertToTm2PixelFormat(vframe->get_stream()->get_format())); - f.exposuretime = get_md_or_default(RS2_FRAME_METADATA_ACTUAL_EXPOSURE); - f.frameLength = vframe->get_height()*vframe->get_stride()* (vframe->get_bpp() / 8); - f.data = vframe->data.data(); - f.timestamp = to_nanos(vframe->additional_data.timestamp); - f.systemTimestamp = to_nanos(vframe->additional_data.backend_timestamp); - f.arrivalTimeStamp = to_nanos(vframe->additional_data.system_time); - auto sts = _tm_dev->SendFrame(f); - if (sts != Status::SUCCESS) - { - LOG_ERROR("Failed to send video frame to TM"); - } - } - else if (auto mframe = As(fref.frame)) - { - auto st = mframe->get_stream()->get_stream_type(); - if (st == RS2_STREAM_ACCEL) - { - TrackingData::AccelerometerFrame f{}; - auto mdata = reinterpret_cast(mframe->data.data()); - f.acceleration.set(mdata[0], mdata[1], mdata[2]); - f.frameId = mframe->additional_data.frame_number; - f.sensorIndex = stream_index; - f.temperature = get_md_or_default(RS2_FRAME_METADATA_TEMPERATURE); - f.timestamp = to_nanos(mframe->additional_data.timestamp); - f.systemTimestamp = to_nanos(mframe->additional_data.backend_timestamp); - f.arrivalTimeStamp = to_nanos(mframe->additional_data.system_time); - auto sts = _tm_dev->SendFrame(f); - if (sts != Status::SUCCESS) - { - LOG_ERROR("Failed to send video frame to TM"); - } - } - else if(st == RS2_STREAM_GYRO) - { - TrackingData::GyroFrame f{}; - auto mdata = reinterpret_cast(mframe->data.data()); - f.angularVelocity.set(mdata[0], mdata[1], mdata[2]); - f.frameId = mframe->additional_data.frame_number; - f.sensorIndex = stream_index; - f.temperature = get_md_or_default(RS2_FRAME_METADATA_TEMPERATURE); - f.timestamp = to_nanos(mframe->additional_data.timestamp); - f.systemTimestamp = to_nanos(mframe->additional_data.backend_timestamp); - f.arrivalTimeStamp = to_nanos(mframe->additional_data.system_time); - auto sts = _tm_dev->SendFrame(f); - if (sts != Status::SUCCESS) - { - LOG_ERROR("Failed to send video frame to TM"); - } - } - else if (auto mframe = As(fref.frame)) - { - //Ignore - } - else - { - LOG_ERROR("Unhandled motion frame type"); - } - } - else - { - LOG_ERROR("Unhandled frame type"); - } - */ - } - - void tm2_sensor::start(frame_callback_ptr callback) - { - std::lock_guard lock(_tm_op_lock); - LOG_DEBUG("Starting T265"); - if (_is_streaming) - throw wrong_api_call_sequence_exception("start_streaming(...) failed. T265 device is already streaming!"); - else if (!_is_opened) - throw wrong_api_call_sequence_exception("start_streaming(...) failed. T265 device was not opened!"); - - start_interrupt(); - start_stream(); - - _source.set_callback(callback); - raise_on_before_streaming_changes(true); - - bulk_message_request_start request = {{ sizeof(request), DEV_START }}; - bulk_message_response_start response = {}; - _device->bulk_request_response(request, response, sizeof(response), false); - if(response.header.wStatus == DEVICE_BUSY) - throw wrong_api_call_sequence_exception("open(...) failed. T265 is already started!"); - else if(response.header.wStatus != SUCCESS) - throw io_exception(rsutils::string::from() << "open(...) unknown error starting " << status_name(response.header)); - - LOG_DEBUG("T265 started"); - - if (_loopback) { - auto& loopback_sensor = _loopback->get_sensor(0); - auto handle_file_frames = [&](frame_holder fref) { - pass_frames_to_fw(std::move(fref)); - }; - - frame_callback_ptr file_frames_callback = { new internal_frame_callback(handle_file_frames), - [](rs2_frame_callback* p) { p->release(); } }; - loopback_sensor.start(file_frames_callback); - } - - _is_streaming = true; - } - - void tm2_sensor::stop() - { - std::lock_guard lock(_tm_op_lock); - LOG_DEBUG("Stopping T265"); - if (!_is_streaming) - throw wrong_api_call_sequence_exception("stop_streaming() failed. T265 device is not streaming!"); - - if (_loopback) { - auto& loopback_sensor = _loopback->get_sensor(0); - loopback_sensor.stop(); - } - - // Send the stop message - bulk_message_request_stop request = {{ sizeof(request), DEV_STOP }}; - bulk_message_response_stop response = {}; - _device->bulk_request_response(request, response, sizeof(response), false); - if(response.header.wStatus == TIMEOUT) - LOG_WARNING("Got a timeout while trying to stop"); - else if(response.header.wStatus != SUCCESS) - throw io_exception(rsutils::string::from() << "Unknown error stopping " << status_name(response.header)); - - LOG_DEBUG("T265 stopped"); - - stop_stream(); - stop_interrupt(); - - raise_on_before_streaming_changes(false); - _is_streaming = false; - } - - rs2_intrinsics tm2_sensor::get_intrinsics(const stream_profile& profile) const - { - rs2_stream rtype = profile.stream; - int rs2_index = profile.index; - int sensor_type = tm2_sensor_type(rtype); - int sensor_id = tm2_sensor_id(rtype, rs2_index); - - bulk_message_request_get_camera_intrinsics request = {{ sizeof(request), DEV_GET_CAMERA_INTRINSICS }}; - request.bCameraID = SET_SENSOR_ID(sensor_type, sensor_id); - bulk_message_response_get_camera_intrinsics response = {}; - _device->bulk_request_response(request, response); - - rs2_intrinsics result; - result.width = response.intrinsics.dwWidth; - result.height = response.intrinsics.dwHeight; - result.ppx = response.intrinsics.flPpx; - result.ppy = response.intrinsics.flPpy; - result.fx = response.intrinsics.flFx; - result.fy = response.intrinsics.flFy; - if(response.intrinsics.dwDistortionModel == 1) result.model = RS2_DISTORTION_FTHETA; - else if(response.intrinsics.dwDistortionModel == 3) result.model = RS2_DISTORTION_NONE; - else if(response.intrinsics.dwDistortionModel == 4) result.model = RS2_DISTORTION_KANNALA_BRANDT4; - else - throw invalid_value_exception("Invalid distortion model"); - librealsense::copy_array(result.coeffs, response.intrinsics.flCoeffs); - - return result; - } - - rs2_motion_device_intrinsic tm2_sensor::get_motion_intrinsics(const motion_stream_profile_interface& profile) const - { - rs2_stream rtype = profile.get_stream_type(); - int rs2_index = profile.get_stream_index(); - int sensor_type = tm2_sensor_type(rtype); - int sensor_id = tm2_sensor_id(rtype, rs2_index); - - bulk_message_request_get_motion_intrinsics request = {{ sizeof(request), DEV_GET_MOTION_INTRINSICS }}; - request.bMotionID = SET_SENSOR_ID(sensor_type, sensor_id); - bulk_message_response_get_motion_intrinsics response = {}; - _device->bulk_request_response(request, response); - - rs2_motion_device_intrinsic result{}; - librealsense::copy_2darray(result.data, response.intrinsics.flData); - librealsense::copy_array(result.noise_variances, response.intrinsics.flNoiseVariances); - librealsense::copy_array(result.bias_variances, response.intrinsics.flBiasVariances); - return result; - } - - rs2_extrinsics tm2_sensor::get_extrinsics(const stream_profile_interface & profile, int tmsensor_id) const - { - rs2_stream rtype = profile.get_stream_type(); - int rs2_index = profile.get_stream_index(); - int sensor_type = tm2_sensor_type(rtype); - int sensor_id = tm2_sensor_id(rtype, rs2_index); - - bulk_message_request_get_extrinsics request = {{ sizeof(request), DEV_GET_EXTRINSICS }}; - request.bSensorID = SET_SENSOR_ID(sensor_type, sensor_id); - bulk_message_response_get_extrinsics response = {}; - _device->bulk_request_response(request, response); - - if(response.extrinsics.bReferenceSensorID != SET_SENSOR_ID(SensorType::Pose, 0)) { - LOG_ERROR("Unexpected reference sensor id " << response.extrinsics.bReferenceSensorID); - } - - rs2_extrinsics result{}; - librealsense::copy_array(result.rotation, response.extrinsics.flRotation); - librealsense::copy_array(result.translation, response.extrinsics.flTranslation); - - return result; - } - - void tm2_sensor::set_intrinsics(const stream_profile_interface& stream_profile, const rs2_intrinsics& intr) - { - bulk_message_request_set_camera_intrinsics request = {{ sizeof(request), DEV_SET_CAMERA_INTRINSICS }}; - - int stream_index = stream_profile.get_stream_index() - 1; - if(stream_index != 0 && stream_index != 1) - throw invalid_value_exception("Invalid fisheye stream"); - - request.bCameraID = SET_SENSOR_ID(SensorType::Fisheye, stream_index); - request.intrinsics.dwWidth = intr.width; - request.intrinsics.dwHeight = intr.height; - request.intrinsics.flFx = intr.fx; - request.intrinsics.flFy = intr.fy; - request.intrinsics.flPpx = intr.ppx; - request.intrinsics.flPpy = intr.ppy; - if(intr.model == RS2_DISTORTION_FTHETA) request.intrinsics.dwDistortionModel = 1; - else if(intr.model == RS2_DISTORTION_NONE) request.intrinsics.dwDistortionModel = 3; - else if(intr.model == RS2_DISTORTION_KANNALA_BRANDT4) request.intrinsics.dwDistortionModel = 4; - else - throw invalid_value_exception("Invalid distortion model"); - librealsense::copy_array(request.intrinsics.flCoeffs, intr.coeffs); - - bulk_message_response_set_camera_intrinsics response = {}; - _device->bulk_request_response(request, response); - } - - void tm2_sensor::set_extrinsics(const stream_profile_interface& from_profile, const stream_profile_interface& to_profile, const rs2_extrinsics& extr) - { - switch (to_profile.get_stream_type()) { - case RS2_STREAM_POSE: - set_extrinsics_to_ref(from_profile.get_stream_type(), from_profile.get_stream_index(), extr); - break; - case RS2_STREAM_FISHEYE: - { - auto inv = [](const rs2_extrinsics& src) { - rs2_extrinsics dst; - auto dst_rotation = dst.rotation; - for (auto i : { 0,3,6,1,4,7,2,5,8 }) { *dst_rotation++ = src.rotation[i]; } - dst.translation[0] = - src.rotation[0] * src.translation[0] - src.rotation[3] * src.translation[1] - src.rotation[6] * src.translation[2]; - dst.translation[1] = - src.rotation[1] * src.translation[0] - src.rotation[4] * src.translation[1] - src.rotation[7] * src.translation[2]; - dst.translation[2] = - src.rotation[2] * src.translation[0] - src.rotation[5] * src.translation[1] - src.rotation[8] * src.translation[2]; - return dst; - }; - - auto mult = [](const rs2_extrinsics& a, const rs2_extrinsics& b) { - rs2_extrinsics tf; - tf.rotation[0] = a.rotation[0] * b.rotation[0] + a.rotation[1] * b.rotation[3] + a.rotation[2] * b.rotation[6]; - tf.rotation[1] = a.rotation[0] * b.rotation[1] + a.rotation[1] * b.rotation[4] + a.rotation[2] * b.rotation[7]; - tf.rotation[2] = a.rotation[0] * b.rotation[2] + a.rotation[1] * b.rotation[5] + a.rotation[2] * b.rotation[8]; - tf.rotation[3] = a.rotation[3] * b.rotation[0] + a.rotation[4] * b.rotation[3] + a.rotation[5] * b.rotation[6]; - tf.rotation[4] = a.rotation[3] * b.rotation[1] + a.rotation[4] * b.rotation[4] + a.rotation[5] * b.rotation[7]; - tf.rotation[5] = a.rotation[3] * b.rotation[2] + a.rotation[4] * b.rotation[5] + a.rotation[5] * b.rotation[8]; - tf.rotation[6] = a.rotation[6] * b.rotation[0] + a.rotation[7] * b.rotation[3] + a.rotation[8] * b.rotation[6]; - tf.rotation[7] = a.rotation[6] * b.rotation[1] + a.rotation[7] * b.rotation[4] + a.rotation[8] * b.rotation[7]; - tf.rotation[8] = a.rotation[6] * b.rotation[2] + a.rotation[7] * b.rotation[5] + a.rotation[8] * b.rotation[8]; - tf.translation[0] = a.rotation[0] * b.translation[0] + a.rotation[1] * b.translation[1] + a.rotation[2] * b.translation[2] + a.translation[0]; - tf.translation[1] = a.rotation[3] * b.translation[0] + a.rotation[4] * b.translation[1] + a.rotation[5] * b.translation[2] + a.translation[1]; - tf.translation[2] = a.rotation[6] * b.translation[0] + a.rotation[7] * b.translation[1] + a.rotation[8] * b.translation[2] + a.translation[2]; - return tf; - }; - - auto& H_fe1_fe2 = extr; - auto H_fe2_fe1 = inv(H_fe1_fe2); - auto H_fe1_pose = get_extrinsics(from_profile, 0); - auto H_fe2_pose = mult(H_fe2_fe1, H_fe1_pose); //H_fe2_pose = H_fe2_fe1 * H_fe1_pose - set_extrinsics_to_ref(RS2_STREAM_FISHEYE, 2, H_fe2_pose); - break; - } - default: - throw invalid_value_exception("Invalid stream type: set_extrinsics only support fisheye stream"); - } - } - - void tm2_sensor::set_extrinsics_to_ref(rs2_stream stream_type, int stream_index, const rs2_extrinsics& extr) - { - int sensor_type = tm2_sensor_type(stream_type); - int sensor_id = tm2_sensor_id(stream_type, stream_index); - - bulk_message_request_set_extrinsics request = {{ sizeof(request), DEV_SET_EXTRINSICS }}; - request.bSensorID = SET_SENSOR_ID(sensor_type, sensor_id); - librealsense::copy_array(request.extrinsics.flRotation, extr.rotation); - librealsense::copy_array(request.extrinsics.flTranslation, extr.translation); - bulk_message_response_set_extrinsics response = {}; - - _device->bulk_request_response(request, response); - } - - void tm2_sensor::set_motion_device_intrinsics(const stream_profile_interface& stream_profile, const rs2_motion_device_intrinsic& intr) - { - int sensor_type = tm2_sensor_type(stream_profile.get_stream_type()); - int sensor_id = tm2_sensor_id(stream_profile.get_stream_type(), stream_profile.get_stream_index()); - - if (sensor_id != 0 || (sensor_type != RS2_STREAM_GYRO && sensor_type != RS2_STREAM_ACCEL)) - throw invalid_value_exception("Invalid stream index"); - - if(sensor_type != SensorType::Gyro || sensor_type != SensorType::Accelerometer) - throw invalid_value_exception("Invalid stream type"); - - bulk_message_request_set_motion_intrinsics request = {{ sizeof(request), DEV_SET_MOTION_INTRINSICS }}; - request.bMotionID = SET_SENSOR_ID(sensor_type, sensor_id); - librealsense::copy_2darray(request.intrinsics.flData, intr.data); - librealsense::copy_array(request.intrinsics.flNoiseVariances, intr.noise_variances); - librealsense::copy_array(request.intrinsics.flBiasVariances, intr.bias_variances); - - bulk_message_response_set_motion_intrinsics response = {}; - _device->bulk_request_response(request, response); - } - - void tm2_sensor::write_calibration() - { - bulk_message_request_write_configuration request = {{ sizeof(request), DEV_WRITE_CONFIGURATION }}; - request.wTableId = ID_OEM_CAL; - // We send an implicitly 0 length bTable and the right table - // id to tell us to write the calibration - request.header.dwLength = offsetof(bulk_message_request_write_configuration, bTable); - bulk_message_response_write_configuration response = {}; - _device->bulk_request_response(request, response); - } - - void tm2_sensor::reset_to_factory_calibration() - { - bulk_message_request_reset_configuration request = {{ sizeof(request), DEV_RESET_CONFIGURATION }}; - request.wTableId = ID_OEM_CAL; - bulk_message_response_reset_configuration response = {}; - _device->bulk_request_response(request, response); - } - - void tm2_sensor::dispatch_threaded(frame_holder frame) - { - // TODO: Replace with C++14 move capture - auto frame_holder_ptr = std::make_shared(); - *frame_holder_ptr = std::move(frame); - _data_dispatcher->invoke([this, frame_holder_ptr](dispatcher::cancellable_timer t) { - _source.invoke_callback(std::move(*frame_holder_ptr)); - }); - } - - void tm2_sensor::receive_pose_message(const interrupt_message_get_pose & pose_message) - { - const pose_data & pose = pose_message.pose; - // pose stream doesn't have a frame number, so we have to keep - // track ourselves - static unsigned long long frame_num = 0; - - auto ts = get_coordinated_timestamp(pose.llNanoseconds); - pose_frame_metadata frame_md = { 0 }; - frame_md.arrival_ts = duration_cast(ts.arrival_ts).count(); - - frame_additional_data additional_data(ts.device_ts.count(), frame_num++, ts.arrival_ts.count(), sizeof(frame_md), (uint8_t*)&frame_md, ts.global_ts.count(), 0, 0, false, 0.0); - - // Find the frame stream profile - std::shared_ptr profile = nullptr; - auto profiles = get_stream_profiles(); - for (auto&& p : profiles) - { - if (p->get_stream_type() == RS2_STREAM_POSE && - p->get_stream_index() == 0) // TODO: no reason to retrieve this every time - { - profile = p; - break; - } - } - if (profile == nullptr) - { - LOG_WARNING("Dropped frame. No valid profile"); - return; - } - - frame_holder frame = _source.alloc_frame(RS2_EXTENSION_POSE_FRAME, sizeof(librealsense::pose_frame::pose_info), additional_data, true); - if (frame.frame) - { - auto pose_frame = static_cast(frame.frame); - frame->set_timestamp(ts.global_ts.count()); - frame->set_timestamp_domain(RS2_TIMESTAMP_DOMAIN_GLOBAL_TIME); - frame->set_stream(profile); - - auto info = reinterpret_cast(pose_frame->data.data()); - info->translation = float3{pose.flX, pose.flY, pose.flZ}; - info->velocity = float3{pose.flVx, pose.flVy, pose.flVz}; - info->acceleration = float3{pose.flAx, pose.flAy, pose.flAz}; - info->rotation = float4{pose.flQi, pose.flQj, pose.flQk, pose.flQr}; - info->angular_velocity = float3{pose.flVAX, pose.flVAY, pose.flVAZ}; - info->angular_acceleration = float3{pose.flAAX, pose.flAAY, pose.flAAZ}; - info->tracker_confidence = pose.dwTrackerConfidence; - info->mapper_confidence = pose.dwMapperConfidence; - } - else - { - LOG_INFO("Dropped frame. alloc_frame(...) returned nullptr"); - return; - } - dispatch_threaded(std::move(frame)); - } - - void tm2_sensor::receive_accel_message(const interrupt_message_accelerometer_stream & message) - { - if (!_is_streaming) - { - LOG_WARNING("Frame received with streaming inactive"); - return; - } - - float3 data = { message.metadata.flAx, message.metadata.flAy, message.metadata.flAz }; - auto sensor_id = GET_SENSOR_INDEX(message.rawStreamHeader.bSensorID); - handle_imu_frame(message.rawStreamHeader.llNanoseconds, message.rawStreamHeader.dwFrameId, RS2_STREAM_ACCEL, sensor_id, data, message.metadata.flTemperature); - } - - void tm2_sensor::receive_gyro_message(const interrupt_message_gyro_stream & message) - { - if (!_is_streaming) - { - LOG_WARNING("Frame received with streaming inactive"); - return; - } - - float3 data = { message.metadata.flGx, message.metadata.flGy, message.metadata.flGz }; - auto sensor_id = GET_SENSOR_INDEX(message.rawStreamHeader.bSensorID); - handle_imu_frame(message.rawStreamHeader.llNanoseconds, message.rawStreamHeader.dwFrameId, RS2_STREAM_GYRO, sensor_id, data, message.metadata.flTemperature); - } - - void tm2_sensor::receive_video_message(const bulk_message_video_stream * message) - { - if (!_is_streaming) - { - LOG_WARNING("Frame received with streaming inactive"); - return; - } - if (message->metadata.dwFrameLength == 0) - { - LOG_WARNING("Dropped frame. Frame length is 0"); - return; - } - - auto bpp = get_image_bpp(RS2_FORMAT_Y8); - - auto ts = get_coordinated_timestamp(message->rawStreamHeader.llNanoseconds); - - video_frame_metadata video_md{}; - video_md.arrival_ts = duration_cast(ts.arrival_ts).count(); - video_md.exposure_time = message->metadata.dwExposuretime; - frame_additional_data additional_data(ts.device_ts.count(), message->rawStreamHeader.dwFrameId, ts.arrival_ts.count(), sizeof(video_md), (uint8_t*)&video_md, ts.global_ts.count(), 0, 0, false, 0.0); - - last_exposure = message->metadata.dwExposuretime; - last_gain = message->metadata.fGain; - - // TODO: llArrivalNanoseconds? - // Find the frame stream profile - int width, height, stride; - std::shared_ptr profile = nullptr; - auto profiles = get_stream_profiles(); - for (auto&& p : profiles) - { - if (p->get_stream_type() == RS2_STREAM_FISHEYE && - p->get_stream_index() == GET_SENSOR_INDEX(message->rawStreamHeader.bSensorID) + 1) - { - auto video = dynamic_cast(p.get()); //this must succeed for fisheye stream - profile = p; - width = stride = video->get_width(); - height = video->get_height(); - break; - } - } - if (profile == nullptr) - { - LOG_WARNING("Dropped frame. No valid profile"); - return; - } - - //Global base time sync may happen between two frames - //Make sure 2nd frame global timestamp is not impacted. - auto delta_dev_ts = ts.device_ts - last_ts.device_ts; - if (delta_dev_ts < delta_dev_ts.zero()) - delta_dev_ts = -delta_dev_ts; - - if (delta_dev_ts < std::chrono::microseconds(1000)) - ts.global_ts = last_ts.global_ts + delta_dev_ts; // keep stereo pairs times in sync - - last_ts = ts; - - //TODO - extension_type param assumes not depth - frame_holder frame = _source.alloc_frame(RS2_EXTENSION_VIDEO_FRAME, height * stride, additional_data, true); - if (frame.frame) - { - auto video = (video_frame*)(frame.frame); - video->assign(width, height, stride, bpp); - frame->set_timestamp(ts.global_ts.count()); - frame->set_timestamp_domain(RS2_TIMESTAMP_DOMAIN_GLOBAL_TIME); - frame->set_stream(profile); - frame->set_sensor(this->shared_from_this()); //TODO? uvc doesn't set it? - video->data.assign(message->metadata.bFrameData, message->metadata.bFrameData + (height * stride)); - } - else - { - LOG_INFO("Dropped frame. alloc_frame(...) returned nullptr"); - return; - } - dispatch_threaded(std::move(frame)); - } - - tm2_sensor::coordinated_ts tm2_sensor::get_coordinated_timestamp(uint64_t device_ns) - { - tm2_sensor::coordinated_ts result; - auto ts_nanos = duration(device_ns); - result.device_ts = duration(ts_nanos); - result.global_ts = duration(ts_nanos + duration(device_to_host_ns)); - result.arrival_ts = duration(environment::get_instance().get_time_service()->get_time()); - return result; - } - - bool tm2_sensor::start_interrupt() - { - std::vector buffer(BUFFER_SIZE); - - if (_interrupt_request) return false; - - _interrupt_callback = std::make_shared([&](platform::rs_usb_request request) { - uint32_t transferred = request->get_actual_length(); - if(transferred == 0) { // something went wrong, exit - LOG_ERROR("Interrupt transfer failed, exiting"); - _interrupt_request.reset(); - return; - } - - interrupt_message_header* header = (interrupt_message_header*)request->get_buffer().data(); - if(header->wMessageID == DEV_GET_POSE) - receive_pose_message(*((interrupt_message_get_pose*)header)); - else if(header->wMessageID == DEV_SAMPLE) { - if(_is_streaming) { - int sensor_type = GET_SENSOR_TYPE(((interrupt_message_raw_stream_header*)header)->bSensorID); - if(sensor_type == SensorType::Accelerometer) - receive_accel_message(*((interrupt_message_accelerometer_stream*)header)); - else if(sensor_type == SensorType::Gyro) - receive_gyro_message(*((interrupt_message_gyro_stream*)header)); - else if(sensor_type == SensorType::Velocimeter) - LOG_ERROR("Did not expect to receive a velocimeter message"); - else - LOG_ERROR("Received DEV_SAMPLE with unknown type " << sensor_type); - } - } - else if(header->wMessageID == SLAM_ERROR) { - //TODO: current librs ignores these - auto message = (interrupt_message_slam_error *)header; - if (message->wStatus == SLAM_ERROR_CODE_NONE) LOG_INFO("SLAM_ERROR none"); - else if(message->wStatus == SLAM_ERROR_CODE_VISION) LOG_WARNING("SLAM_ERROR Vision"); - else if(message->wStatus == SLAM_ERROR_CODE_SPEED) LOG_WARNING("SLAM_ERROR Speed"); - else if(message->wStatus == SLAM_ERROR_CODE_OTHER) LOG_WARNING("SLAM_ERROR Other"); - else LOG_WARNING("SLAM_ERROR Unknown"); - } - else if(header->wMessageID == DEV_ERROR) { - LOG_ERROR("DEV_ERROR " << status_name(*((bulk_message_response_header*)header))); - } - else if(header->wMessageID == DEV_STATUS) { - auto response = (bulk_message_response_header*)header; - if(response->wStatus == DEVICE_STOPPED) - LOG_DEBUG("Got device stopped message"); - else if(response->wStatus == TEMPERATURE_WARNING) - LOG_WARNING("T265 temperature warning"); - else - LOG_WARNING("Unhandled DEV_STATUS " << status_name(*response)); - } - else if(header->wMessageID == SLAM_SET_LOCALIZATION_DATA_STREAM) { - receive_set_localization_data_complete(*((interrupt_message_set_localization_data_stream *)header)); - } - else if(header->wMessageID == SLAM_RELOCALIZATION_EVENT) { - auto event = (const interrupt_message_slam_relocalization_event *)header; - auto ts = get_coordinated_timestamp(event->llNanoseconds); - std::string msg = rsutils::string::from() << "T2xx: Relocalization occurred. id: " << event->wSessionId << ", timestamp: " << ts.global_ts.count() << " ms"; - LOG_INFO(msg); - raise_relocalization_event(msg, ts.global_ts.count()); - } - else - LOG_ERROR("Unknown interrupt message " << message_name(*((bulk_message_response_header*)header)) << " with status " << status_name(*((bulk_message_response_header*)header))); - - _device->submit_request(request); - }); - - _interrupt_request = _device->interrupt_read_request(buffer, _interrupt_callback); - _device->submit_request(_interrupt_request); - return true; - } - - void tm2_sensor::stop_interrupt() - { - if (_interrupt_request) { - _interrupt_callback->cancel(); - if (_device->cancel_request(_interrupt_request)) { - _interrupt_request.reset(); - } - } - } - - bool tm2_sensor::start_stream() - { - std::vector buffer(MAX_TRANSFER_SIZE); - - if (_stream_request) return false; - - _stream_callback = std::make_shared([&](platform::rs_usb_request request) { - uint32_t transferred = request->get_actual_length(); - if(!transferred) { - LOG_ERROR("Stream transfer failed, exiting"); - _stream_request.reset(); - return; - } - - auto header = (bulk_message_raw_stream_header *)request->get_buffer().data(); - if (transferred != header->header.dwLength) { - LOG_ERROR("Bulk received " << transferred << " but header was " << header->header.dwLength << " bytes (max_response_size was " << MAX_TRANSFER_SIZE << ")"); - } - LOG_DEBUG("Got " << transferred << " on bulk stream endpoint"); - - if(header->header.wMessageID == DEV_STATUS) { - auto res = (bulk_message_response_header*)header; - if(res->wStatus == DEVICE_STOPPED) - LOG_DEBUG("Got device stopped message"); - else - LOG_WARNING("Unhandled DEV_STATUS " << status_name(*res)); - } - else if(header->header.wMessageID == SLAM_GET_LOCALIZATION_DATA_STREAM) { - LOG_DEBUG("GET_LOCALIZATION_DATA_STREAM status " << status_name(*((bulk_message_response_header*)header))); - receive_localization_data_chunk((interrupt_message_get_localization_data_stream *)header); - } - else if(header->header.wMessageID == DEV_SAMPLE) { - if(GET_SENSOR_TYPE(header->bSensorID) == SensorType::Fisheye) { - if(_is_streaming) - receive_video_message((bulk_message_video_stream *)header); - } - else - LOG_ERROR("Unexpected DEV_SAMPLE with " << GET_SENSOR_TYPE(header->bSensorID) << " " << GET_SENSOR_INDEX(header->bSensorID)); - } - else { - LOG_ERROR("Unexpected message on raw endpoint " << header->header.wMessageID); - } - - _device->submit_request(request); - }); - - _stream_request = _device->stream_read_request(buffer, _stream_callback); - _device->submit_request(_stream_request); - return true; - } - - void tm2_sensor::stop_stream() - { - if (_stream_request) { - _stream_callback->cancel(); - if (_device->cancel_request(_stream_request)) { - _stream_request.reset(); - } - } - } - - void tm2_sensor::print_logs(const std::unique_ptr & log) - { - int log_size = log->header.dwLength - sizeof(bulk_message_response_header); - int n_entries = log_size / sizeof(device_event_log); - device_event_log * entries = (device_event_log *)log->bEventLog; - - bool start_of_entry = true; - for (size_t i = 0; i < n_entries; i++) { - const auto & e = entries[i]; - uint64_t timestamp; - memcpy(×tamp, e.timestamp, sizeof(e.timestamp)); - LOG_INFO("T265 FW message: " << timestamp << ": [0x" << e.threadID << "/" << e.moduleID << ":" << e.lineNumber << "] " << e.payload); - start_of_entry = e.eventID == FREE_CONT ? false : true; - } - } - - bool tm2_sensor::log_poll_once(std::unique_ptr & log_buffer) - { - bulk_message_request_get_and_clear_event_log log_request = {{ sizeof(log_request), DEV_GET_AND_CLEAR_EVENT_LOG }}; - bulk_message_response_get_and_clear_event_log *log_response = log_buffer.get(); - platform::usb_status usb_response = _device->bulk_request_response(log_request, *log_response, sizeof(bulk_message_response_get_and_clear_event_log), false); - if(usb_response != platform::RS2_USB_STATUS_SUCCESS) return false; - - if(log_response->header.wStatus == INVALID_REQUEST_LEN || log_response->header.wStatus == INTERNAL_ERROR) - LOG_ERROR("T265 log size mismatch " << status_name(log_response->header)); - else if(log_response->header.wStatus != SUCCESS) - LOG_ERROR("Unexpected message on log endpoint " << message_name(log_response->header) << " with status " << status_name(log_response->header)); - - return true; - } - - void tm2_sensor::log_poll() - { - auto log_buffer = std::unique_ptr(new bulk_message_response_get_and_clear_event_log); - while(!_log_poll_thread_stop) { - if(log_poll_once(log_buffer)) { - print_logs(log_buffer); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - else { - LOG_INFO("Got bad response, stopping log_poll"); - break; - } - } - } - - void tm2_sensor::time_sync() - { - int tried_count = 0; - while(!_time_sync_thread_stop) { - bulk_message_request_get_time request = {{ sizeof(request), DEV_GET_TIME }}; - bulk_message_response_get_time response = {}; - - auto start = duration(environment::get_instance().get_time_service()->get_time()); - platform::usb_status usb_response = _device->bulk_request_response(request, response); - if(usb_response != platform::RS2_USB_STATUS_SUCCESS) { - LOG_INFO("Got bad response, stopping time sync"); - break; - } - auto finish = duration(environment::get_instance().get_time_service()->get_time()); - auto usb_delay = (finish - start) / 2; - - //If usb response takes too long, skip update. 0.25ms is 5% of 200Hz - if (!device_to_host_ns && usb_delay >= duration(0.25)) - { - //In case of slower USB, not to skip after a few tries. - if (tried_count++ < 3) continue; - } - - if (usb_delay < duration(0.25) || !device_to_host_ns) - { - double device_ms = (double)response.llNanoseconds*1e-6; - auto device = duration(device_ms); - auto diff = duration(start + usb_delay - device); - device_to_host_ns = diff.count(); - } - LOG_DEBUG("T265 time synced, host_ns: " << device_to_host_ns); - - // Only trigger this approximately every 500ms, but don't - // wait 500ms to stop if we are requested to stop - for(int i = 0; i < 10; i++) - if(!_time_sync_thread_stop) - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - } - } - - void tm2_sensor::enable_loopback(std::shared_ptr input) - { - std::lock_guard lock(_tm_op_lock); - if (_is_streaming || _is_opened) - throw wrong_api_call_sequence_exception("T2xx: Cannot enter loopback mode while device is open or streaming"); - _loopback = input; - } - - void tm2_sensor::disable_loopback() - { - std::lock_guard lock(_tm_op_lock); - _loopback.reset(); - } - - bool tm2_sensor::is_loopback_enabled() const - { - return _loopback != nullptr; - } - - void tm2_sensor::handle_imu_frame(unsigned long long tm_frame_ts, unsigned long long frame_number, rs2_stream stream_type, int index, float3 imu_data, float temperature) - { - auto ts = get_coordinated_timestamp(tm_frame_ts); - - motion_frame_metadata motion_md{}; - motion_md.arrival_ts = duration_cast(ts.arrival_ts).count(); - motion_md.temperature = temperature; - frame_additional_data additional_data(ts.device_ts.count(), frame_number, ts.arrival_ts.count(), sizeof(motion_md), (uint8_t*)&motion_md, ts.global_ts.count(), 0, 0, false, 0.0); - - // Find the frame stream profile - std::shared_ptr profile = nullptr; - auto profiles = get_stream_profiles(); - for (auto&& p : profiles) - { - if (p->get_stream_type() == stream_type && - p->get_stream_index() == index) - { - profile = p; - break; - } - } - if (profile == nullptr) - { - LOG_WARNING("Dropped frame. No valid profile"); - return; - } - - frame_holder frame = _source.alloc_frame(RS2_EXTENSION_MOTION_FRAME, 3 * sizeof(float), additional_data, true); - if (frame.frame) - { - auto motion_frame = static_cast(frame.frame); - frame->set_timestamp(ts.global_ts.count()); - frame->set_timestamp_domain(RS2_TIMESTAMP_DOMAIN_GLOBAL_TIME); - frame->set_stream(profile); - auto data = reinterpret_cast(motion_frame->data.data()); - data[0] = imu_data[0]; - data[1] = imu_data[1]; - data[2] = imu_data[2]; - } - else - { - LOG_INFO("Dropped frame. alloc_frame(...) returned nullptr"); - return; - } - dispatch_threaded(std::move(frame)); - } - - void tm2_sensor::raise_relocalization_event(const std::string& msg, double timestamp_ms) - { - notification event{ RS2_NOTIFICATION_CATEGORY_POSE_RELOCALIZATION, 0, RS2_LOG_SEVERITY_INFO, msg }; - event.timestamp = timestamp_ms; - get_notifications_processor()->raise_notification(event); - } - - void tm2_sensor::raise_error_notification(const std::string& msg) - { - notification error{ RS2_NOTIFICATION_CATEGORY_HARDWARE_ERROR, 0, RS2_LOG_SEVERITY_ERROR, msg }; - error.timestamp = duration(environment::get_instance().get_time_service()->get_time()).count(); - get_notifications_processor()->raise_notification(error); - } - - void tm2_sensor::receive_set_localization_data_complete(const interrupt_message_set_localization_data_stream & message) - { - if(_is_streaming) - LOG_ERROR("Received SET_LOCALIZATION_DATA_COMPLETE while streaming"); - - if (_async_op_status != _async_progress) - LOG_ERROR("Received SET_LOCALIZATION_DATA_COMPLETE without a transfer in progress"); - - if (message.wStatus == MESSAGE_STATUS::SUCCESS) { - _async_op_status = _async_success; - _async_op.notify_one(); - } - else { - LOG_INFO("SET_LOCALIZATION_DATA_COMPLETE error status " << status_name(*((bulk_message_response_header *)&message))); - _async_op_status = _async_fail; // do not notify here because we may get multiple of these messages - } - } - - void tm2_sensor::receive_localization_data_chunk(const interrupt_message_get_localization_data_stream * chunk) - { - size_t bytes = chunk->header.dwLength - offsetof(interrupt_message_get_localization_data_stream, bLocalizationData); - LOG_DEBUG("Received chunk " << chunk->wIndex << " with status " << chunk->wStatus << " length " << bytes); - - _async_op_res_buffer.reserve(_async_op_res_buffer.size() + bytes); - auto start = (const char*)chunk->bLocalizationData; - _async_op_res_buffer.insert(_async_op_res_buffer.end(), start, start + bytes); - if (chunk->wStatus == SUCCESS) { - _async_op_status = _async_success; - _async_op.notify_one(); - } - else if (chunk->wStatus != MORE_DATA_AVAILABLE) { - _async_op_status = _async_fail; - _async_op.notify_one(); - } - } - - std::string async_op_to_string(tm2_sensor::async_op_state val) - { - switch (val) - { - case tm2_sensor::_async_init: return "Init"; - case tm2_sensor::_async_progress: return "In Progress"; - case tm2_sensor::_async_success: return "Success"; - case tm2_sensor::_async_fail: return "Fail"; - default: return (rsutils::string::from() << " Unsupported type: " << val); - } - } - - tm2_sensor::async_op_state tm2_sensor::perform_async_transfer(std::function transfer_activator, - std::function on_success, const std::string& op_description) const - { - std::mutex _async_op_lock; - std::unique_lock lock(_async_op_lock); - - _async_op_status = _async_progress; - LOG_INFO(op_description << " in progress"); - - bool start_success = transfer_activator(); - if(!start_success) - return async_op_state::_async_fail; - - if (!_async_op.wait_for(lock, std::chrono::seconds(2), [this] { return _async_op_status != _async_progress;})) - LOG_WARNING(op_description << " aborted on timeout"); - else if (_async_op_status == _async_success) - on_success(); - else - LOG_ERROR(op_description << " ended with status " << async_op_to_string(_async_op_status)); - - auto res = _async_op_status; - _async_op_status = _async_init; - LOG_DEBUG(op_description << " completed with " << async_op_to_string(res)); - - return res; - } - - bool tm2_sensor::export_relocalization_map(std::vector& lmap_buf) const - { - std::lock_guard lock(_tm_op_lock); - auto sensor = _device->get_tm2_sensor(); - bool interrupt_started = sensor->start_interrupt(); - std::shared_ptr stop_interrupt(nullptr, [&](...) { - if (interrupt_started) - sensor->stop_interrupt(); - }); - bool stream_started = sensor->start_stream(); - std::shared_ptr stop_stream(nullptr, [&](...) { - if (stream_started) - sensor->stop_stream(); - }); - - // Export first sends SLAM_GET_LOCALIZATION_DATA on bulk - // endpoint and gets an acknowledgement there. That triggers - // the firmware to send map chunks via - // SLAM_GET_LOCALIZATION_DATA_STREAM messages on the stream - // endpoint, eventually ending in SUCCESS - auto res = perform_async_transfer( - [&]() { - _async_op_res_buffer.clear(); - bulk_message_request_get_localization_data request = {{ sizeof(request), SLAM_GET_LOCALIZATION_DATA }}; - bulk_message_response_get_localization_data response = {}; - int error = _device->bulk_request_response(request, response); - return !error; - }, - [&](){ - lmap_buf = this->_async_op_res_buffer; - }, - "Export localization map"); - - if (res != async_op_state::_async_success) - LOG_ERROR("Export localization map failed"); - - return res == async_op_state::_async_success; - } - - bool tm2_sensor::import_relocalization_map(const std::vector& lmap_buf) const - { - if(_is_streaming) - throw wrong_api_call_sequence_exception("Unable to import relocalization map while streaming"); - - std::lock_guard lock(_tm_op_lock); - - auto sensor = _device->get_tm2_sensor(); - bool interrupt_started = sensor->start_interrupt(); - std::shared_ptr stop_interrupt(nullptr, [&](...) { - if (interrupt_started) - sensor->stop_interrupt(); - }); - bool stream_started = sensor->start_stream(); - std::shared_ptr stop_stream(nullptr, [&](...) { - if (stream_started) - sensor->stop_stream(); - }); - - // Import the map by sending chunks of with id SLAM_SET_LOCALIZATION_DATA_STREAM - auto res = perform_async_transfer( - [this, &lmap_buf]() { - const int MAX_BIG_DATA_MESSAGE_LENGTH = 10250; //(10240 (10k) + large message header size) - size_t chunk_length = MAX_BIG_DATA_MESSAGE_LENGTH - offsetof(bulk_message_large_stream, bPayload); - size_t map_size = lmap_buf.size(); - std::unique_ptr buf(new uint8_t[MAX_BIG_DATA_MESSAGE_LENGTH]); - auto message = (bulk_message_large_stream *)buf.get(); - - if (map_size == 0) return false; - size_t left_length = map_size; - int chunk_number = 0; - while (left_length > 0) { - message->header.wMessageID = SLAM_SET_LOCALIZATION_DATA_STREAM; - size_t chunk_size = chunk_length; - message->wStatus = MESSAGE_STATUS::MORE_DATA_AVAILABLE; - if (left_length <= chunk_length) { - chunk_size = left_length; - message->wStatus = MESSAGE_STATUS::SUCCESS; - } - message->header.dwLength = uint32_t(chunk_size + offsetof(bulk_message_large_stream, bPayload)); - message->wIndex = chunk_number; - - memcpy(message->bPayload, lmap_buf.data() + (map_size - left_length), chunk_size); - - chunk_number++; - left_length -= chunk_size; - - LOG_DEBUG("Sending chunk length " << chunk_size << " of map size " << map_size); - _device->stream_write(&message->header); - } - return true; - }, - [&]() {}, - "Import localization map"); - - if (res != async_op_state::_async_success) - LOG_ERROR("Import localization map failed"); - - return res == async_op_state::_async_success; - } - - bool tm2_sensor::set_static_node(const std::string& guid, const float3& pos, const float4& orient_quat) const - { - bulk_message_request_set_static_node request = {{ sizeof(request), SLAM_SET_STATIC_NODE }}; - strncpy((char *)&request.bGuid[0], guid.c_str(), MAX_GUID_LENGTH-1); - request.data.flX = pos.x; - request.data.flY = pos.y; - request.data.flZ = pos.z; - request.data.flQi = orient_quat.x; - request.data.flQj = orient_quat.y; - request.data.flQk = orient_quat.z; - request.data.flQr = orient_quat.w; - bulk_message_response_set_static_node response = {}; - _device->bulk_request_response(request, response, sizeof(response), false); - if(response.header.wStatus == INTERNAL_ERROR) - return false; // Failed to set static node - else if(response.header.wStatus != SUCCESS) { - LOG_ERROR("Error: " << status_name(response.header) << " setting static node"); - return false; - } - - return true; - } - - bool tm2_sensor::get_static_node(const std::string& guid, float3& pos, float4& orient_quat) const - { - bulk_message_request_get_static_node request = {{ sizeof(request), SLAM_GET_STATIC_NODE }}; - strncpy((char *)&request.bGuid[0], guid.c_str(), MAX_GUID_LENGTH-1); - bulk_message_response_get_static_node response = {}; - - _device->bulk_request_response(request, response, sizeof(response), false); - if(response.header.wStatus == INTERNAL_ERROR) - return false; // Failed to get static node - else if(response.header.wStatus != SUCCESS) { - LOG_ERROR("Error: " << status_name(response.header) << " getting static node"); - return false; - } - - pos = float3{response.data.flX, response.data.flY, response.data.flZ}; - orient_quat = float4{response.data.flQi, response.data.flQj, response.data.flQk, response.data.flQr}; - - return true; - } - - bool tm2_sensor::remove_static_node(const std::string& guid) const - { - bulk_message_request_remove_static_node request = {{ sizeof(request), SLAM_REMOVE_STATIC_NODE }}; - strncpy((char *)&request.bGuid[0], guid.c_str(), MAX_GUID_LENGTH-1); - bulk_message_response_remove_static_node response = {}; - - _device->bulk_request_response(request, response, sizeof(response), false); - if(response.header.wStatus == INTERNAL_ERROR) - return false; // Failed to get static node - else if(response.header.wStatus != SUCCESS) { - LOG_ERROR("Error: " << status_name(response.header) << " deleting static node"); - return false; - } - return true; - } - - bool tm2_sensor::load_wheel_odometery_config(const std::vector& odometry_config_buf) const - { - std::vector buf; - buf.resize(odometry_config_buf.size() + sizeof(bulk_message_request_header)); - - LOG_INFO("Sending wheel odometry with " << buf.size()); - - bulk_message_request_slam_append_calibration request = {{ sizeof(request), SLAM_APPEND_CALIBRATION }}; - size_t bytes = std::min(odometry_config_buf.size(), size_t(MAX_SLAM_CALIBRATION_SIZE-1)); - strncpy((char *)request.calibration_append_string, (char *)odometry_config_buf.data(), bytes); - request.header.dwLength = uint32_t(offsetof(bulk_message_request_slam_append_calibration, calibration_append_string) + bytes); - - //TODO: There is no way on the firmware to know if this succeeds. - - _device->stream_write(&request.header); - - return true; - } - - bool tm2_sensor::send_wheel_odometry(uint8_t wo_sensor_id, uint32_t frame_num, const float3& translational_velocity) const - { - bulk_message_velocimeter_stream request = {{ sizeof(request), DEV_SAMPLE }}; - request.rawStreamHeader.bSensorID = SET_SENSOR_ID(SensorType::Velocimeter, wo_sensor_id); - // These two timestamps get set by the firmware when it is received - request.rawStreamHeader.llNanoseconds = 0; - request.rawStreamHeader.llArrivalNanoseconds = 0; - request.rawStreamHeader.dwFrameId = frame_num; - - request.metadata.dwMetadataLength = 4; - request.metadata.flTemperature = 0; - request.metadata.dwFrameLength = 12; - request.metadata.flVx = translational_velocity.x; - request.metadata.flVy = translational_velocity.y; - request.metadata.flVz = translational_velocity.z; - - _device->stream_write(&request.rawStreamHeader.header); - - return true; - } - - sensor_temperature tm2_sensor::get_temperature(int sensor_id) - { - uint8_t buffer[BUFFER_SIZE]; - bulk_message_request_get_temperature request = {{ sizeof(request), DEV_GET_TEMPERATURE }}; - bulk_message_response_get_temperature* response = (bulk_message_response_get_temperature*)buffer; - _device->bulk_request_response(request, *response, BUFFER_SIZE); - - if(uint32_t(sensor_id) > response->dwCount) - throw wrong_api_call_sequence_exception("Requested temperature for an unknown sensor id"); - - return response->temperature[sensor_id]; - } - - void tm2_sensor::set_exposure_and_gain(float exposure_ms, float gain) - { - bulk_message_request_set_exposure request = {{ sizeof(request), DEV_SET_EXPOSURE }}; - request.header.dwLength = offsetof(bulk_message_request_set_exposure, stream) + 2 * sizeof(stream_exposure); - request.bNumOfVideoStreams = 2; - for(int i = 0; i < 2; i++) { - request.stream[i].bCameraID = SET_SENSOR_ID(SensorType::Fisheye, i); - request.stream[i].dwIntegrationTime = exposure_ms; // TODO: you cannot set fractional ms, is this actually microseconds? - request.stream[i].fGain = gain; - } - bulk_message_response_set_exposure response = {}; - - _device->bulk_request_response(request, response); - } - - void tm2_sensor::set_exposure(float value) - { - if (!manual_exposure) - throw std::runtime_error("To control exposure you must set sensor to manual exposure mode prior to streaming"); - - set_exposure_and_gain(value, last_gain); - last_exposure = value; - } - - float tm2_sensor::get_exposure() const - { - return last_exposure; - } - - void tm2_sensor::set_gain(float value) - { - if (!manual_exposure) - throw std::runtime_error("To control gain you must set sensor to manual exposure mode prior to streaming"); - - set_exposure_and_gain(last_exposure, value); - last_gain = value; - } - - float tm2_sensor::get_gain() const - { - return last_gain; - } - - void tm2_sensor::set_manual_exposure(bool manual) - { - if(_is_streaming) - throw wrong_api_call_sequence_exception("Exposure mode cannot be controlled while streaming!"); - - bulk_message_request_set_exposure_mode_control request = {{ sizeof(request), DEV_EXPOSURE_MODE_CONTROL }}; - request.bAntiFlickerMode = 0; - request.bVideoStreamsMask = 0; - if(!manual) { - request.bVideoStreamsMask = 0x3; // bitstream 0011 to enable both fisheye autoexposure - } - - bulk_message_response_set_exposure_mode_control response = {}; - _device->bulk_request_response(request, response); - - manual_exposure = manual; - } - - /////////////// - // Device - - tm2_device::tm2_device( std::shared_ptr ctx, const platform::backend_device_group& group, bool register_device_notifications) : - device(ctx, group, register_device_notifications) - { - if(group.usb_devices.size() != 1 || group.uvc_devices.size() != 0 || group.hid_devices.size() !=0) - throw io_exception("Tried to create a T265 with a bad backend_device_group"); - - LOG_DEBUG("Creating a T265 device"); - - usb_device = platform::usb_enumerator::create_usb_device(group.usb_devices[0]); - if(!usb_device) - throw io_exception("Unable to create USB device"); - - usb_info = usb_device->get_info(); - const std::vector interfaces = usb_device->get_interfaces(); - for(auto & iface : interfaces) { - auto endpoints = iface->get_endpoints(); - for(const auto & endpoint : endpoints) { - int addr = endpoint->get_address(); - - if (addr == ENDPOINT_HOST_IN) endpoint_bulk_in = endpoint; - else if (addr == ENDPOINT_HOST_OUT) endpoint_bulk_out = endpoint; - else if (addr == ENDPOINT_HOST_MSGS_IN) endpoint_msg_in = endpoint; - else if (addr == ENDPOINT_HOST_MSGS_OUT) endpoint_msg_out = endpoint; - else if (addr == ENDPOINT_HOST_INT_IN) endpoint_int_in = endpoint; - else if (addr == ENDPOINT_HOST_INT_OUT) endpoint_int_out = endpoint; - else LOG_ERROR("Unknown endpoint address " << addr); - } - } - if(!endpoint_bulk_in || !endpoint_bulk_out || !endpoint_msg_in || !endpoint_msg_out || !endpoint_int_in || !endpoint_int_out) - throw io_exception("Missing a T265 usb endpoint"); - - - usb_messenger = usb_device->open(0); // T265 only has one interface, 0 - if(!usb_messenger) - throw io_exception("Unable to open device interface"); - - LOG_DEBUG("Successfully opened and claimed interface 0"); - - bulk_message_request_set_low_power_mode power_request = {{ sizeof(power_request), DEV_SET_LOW_POWER_MODE }}; - bulk_message_response_set_low_power_mode power_response = {}; - power_request.bEnabled = 0; // disable low power mode - auto response = bulk_request_response(power_request, power_response); - if(response != platform::RS2_USB_STATUS_SUCCESS) - throw io_exception("Unable to communicate with device"); - - bulk_message_request_get_device_info info_request = {{ sizeof(info_request), DEV_GET_DEVICE_INFO }}; - bulk_message_response_get_device_info info_response = {}; - bulk_request_response(info_request, info_response); - - if(info_response.message.bStatus == 0x1 || info_response.message.dwStatusCode == FW_STATUS_CODE_NO_CALIBRATION_DATA) - throw io_exception("T265 device is uncalibrated"); - - std::string serial = rsutils::string::from() << std::uppercase << std::hex << (bytesSwap(info_response.message.llSerialNumber) >> 16); - LOG_INFO("Serial: " << serial); - - LOG_INFO("Connection type: " << platform::usb_spec_names.at(usb_info.conn_spec)); - - register_info(RS2_CAMERA_INFO_NAME, tm2_device_name()); - register_info(RS2_CAMERA_INFO_SERIAL_NUMBER, serial); - std::string firmware = rsutils::string::from() << std::to_string(info_response.message.bFWVersionMajor) << "." << std::to_string(info_response.message.bFWVersionMinor) << "." << std::to_string(info_response.message.bFWVersionPatch) << "." << std::to_string(info_response.message.dwFWVersionBuild); - register_info(RS2_CAMERA_INFO_FIRMWARE_VERSION, firmware); - LOG_INFO("Firmware version: " << firmware); - register_info(RS2_CAMERA_INFO_PRODUCT_ID, hexify(usb_info.pid)); - register_info(RS2_CAMERA_INFO_PRODUCT_LINE, "T200"); - - register_info(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR, platform::usb_spec_names.at(usb_info.conn_spec)); - register_info(RS2_CAMERA_INFO_PHYSICAL_PORT, usb_info.id); // TODO uvc has device_path, usb_info has id and unique_id - - _sensor = std::make_shared(this); - add_sensor(_sensor); - - _sensor->register_option(rs2_option::RS2_OPTION_ASIC_TEMPERATURE, std::make_shared(*_sensor)); - _sensor->register_option(rs2_option::RS2_OPTION_MOTION_MODULE_TEMPERATURE, std::make_shared(*_sensor)); - - _sensor->register_option(rs2_option::RS2_OPTION_EXPOSURE, std::make_shared(*_sensor)); - _sensor->register_option(rs2_option::RS2_OPTION_GAIN, std::make_shared(*_sensor)); - _sensor->register_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, std::make_shared(*_sensor)); - - _sensor->register_option(rs2_option::RS2_OPTION_ENABLE_MAPPING, std::make_shared>(*_sensor, "Use an on device map (recommended)")); - _sensor->register_option(rs2_option::RS2_OPTION_ENABLE_RELOCALIZATION, std::make_shared>(*_sensor, "Use appearance based relocalization (depends on mapping)")); - _sensor->register_option(rs2_option::RS2_OPTION_ENABLE_POSE_JUMPING, std::make_shared>(*_sensor, "Allow pose jumping (depends on mapping)")); - _sensor->register_option(rs2_option::RS2_OPTION_ENABLE_DYNAMIC_CALIBRATION, std::make_shared>(*_sensor, "Enable dynamic calibration (recommended)")); - _sensor->register_option(rs2_option::RS2_OPTION_ENABLE_MAP_PRESERVATION, std::make_shared>(*_sensor, "Preserve the map from the previous run as if it was loaded")); - - // Adding the extrinsic nodes to the default group - auto tm2_profiles = _sensor->get_stream_profiles(); - for (auto && pf : tm2_profiles) - register_stream_to_extrinsic_group(*pf, 0); - - //For manual testing: enable_loopback("C:\\dev\\recording\\tm2.bag"); - } - - tm2_device::~tm2_device() - { - // Since the device claims the interface and opens it, it - // should also dispose of it - LOG_DEBUG("Stopping sensor"); - // Note this is the last chance the tm2_sensor gets to use USB - _sensor->dispose(); - LOG_DEBUG("Destroying T265 device"); - } - - void tm2_device::hardware_reset() - { - LOG_INFO("Sending hardware reset"); - uint32_t transferred; - usb_messenger->control_transfer(0x40, 0x10, 0, 0, nullptr, 0, transferred, USB_TIMEOUT); - } - - template - platform::usb_status tm2_device::bulk_request_response(const Request &request, Response &response, size_t max_response_size, bool assert_success) - { - std::lock_guard lock(bulk_mutex); - - // request - uint32_t length = request.header.dwLength; - uint16_t message_id = request.header.wMessageID; - LOG_DEBUG("Sending message " << message_name(request.header) << " length " << length); - uint32_t transferred = 0; - platform::usb_status e; - e = usb_messenger->bulk_transfer(endpoint_msg_out, (uint8_t*)&request, length, transferred, USB_TIMEOUT); - if (e != platform::RS2_USB_STATUS_SUCCESS) { - LOG_ERROR("Bulk request error " << platform::usb_status_to_string.at(e)); - return e; - } - if (transferred != length) { - LOG_ERROR("error: sent " << transferred << " not " << length); - return platform::RS2_USB_STATUS_OTHER; - } - - // response - if(max_response_size == 0) - max_response_size = sizeof(response); - LOG_DEBUG("Receiving message with max_response_size " << max_response_size); - - transferred = 0; - e = usb_messenger->bulk_transfer(endpoint_msg_in, (uint8_t*)&response, int(max_response_size), transferred, USB_TIMEOUT); - if (e != platform::RS2_USB_STATUS_SUCCESS) { - LOG_ERROR("Bulk response error " << platform::usb_status_to_string.at(e)); - return e; - } - if (transferred != response.header.dwLength) { - LOG_ERROR("Received " << transferred << " but header was " << response.header.dwLength << " bytes (max_response_size was " << max_response_size << ")"); - return platform::RS2_USB_STATUS_OTHER; - } - if (assert_success && MESSAGE_STATUS(response.header.wStatus) != MESSAGE_STATUS::SUCCESS) { - LOG_ERROR("Received " << message_name(response.header) << " with length " << response.header.dwLength << " but got non-zero status of " << status_name(response.header)); - } - LOG_DEBUG("Received " << message_name(response.header) << " with length " << response.header.dwLength); - return e; - } - - // all messages must have dwLength and wMessageID as first two members - platform::usb_status tm2_device::stream_write(const t265::bulk_message_request_header * request) - { - std::lock_guard lock(stream_mutex); - - uint32_t length = request->dwLength; - uint16_t message_id = request->wMessageID; - LOG_DEBUG("Sending stream message " << message_name(*request) << " length " << length); - uint32_t transferred = 0; - platform::usb_status e; - e = usb_messenger->bulk_transfer(endpoint_bulk_out, (uint8_t *)request, length, transferred, USB_TIMEOUT); - if (e != platform::RS2_USB_STATUS_SUCCESS) { - LOG_ERROR("Stream write error " << platform::usb_status_to_string.at(e)); - return e; - } - if (transferred != length) { - LOG_ERROR("error: sent " << transferred << " not " << length); - return platform::RS2_USB_STATUS_OTHER; - } - return e; - } - - platform::rs_usb_request tm2_device::stream_read_request(std::vector & buffer, std::shared_ptr callback) - { - auto request = usb_messenger->create_request(endpoint_bulk_in); - request->set_buffer(buffer); - request->set_callback(callback); - return request; - } - - void tm2_device::submit_request(platform::rs_usb_request request) - { - auto e = usb_messenger->submit_request(request); - if (e != platform::RS2_USB_STATUS_SUCCESS) - throw std::runtime_error("failed to submit request, error: " + platform::usb_status_to_string.at(e)); - } - - bool tm2_device::cancel_request(platform::rs_usb_request request) - { - auto e = usb_messenger->cancel_request(request); - if (e != platform::RS2_USB_STATUS_SUCCESS) - return false; - return true; - } - - platform::rs_usb_request tm2_device::interrupt_read_request(std::vector & buffer, std::shared_ptr callback) - { - auto request = usb_messenger->create_request(endpoint_int_in); - request->set_buffer(buffer); - request->set_callback(callback); - return request; - } - - /** - * Enable loopback will replace the input and ouput of the tm2 sensor - */ - void tm2_device::enable_loopback(const std::string& source_file) - { - auto ctx = get_context(); - std::shared_ptr raw_streams; - try - { - raw_streams = std::make_shared(ctx, std::make_shared(source_file, ctx)); - } - catch (const std::exception& e) - { - LOG_ERROR("Failed to create playback device from given file. File = \"" << source_file << "\". Exception: " << e.what()); - throw librealsense::invalid_value_exception("Failed to enable loopback"); - } - _sensor->enable_loopback(raw_streams); - update_info(RS2_CAMERA_INFO_NAME, rsutils::string::from() << tm2_device_name() << " (Loopback - " << source_file << ")"); - } - - void tm2_device::disable_loopback() - { - _sensor->disable_loopback(); - update_info(RS2_CAMERA_INFO_NAME, tm2_device_name()); - } - - bool tm2_device::is_enabled() const - { - return _sensor->is_loopback_enabled(); - } - - void tm2_device::register_stream_to_extrinsic_group(const stream_interface& stream, uint32_t group_index) - { - //T265 uses POSE sensor as reference sensor for extrinsics - auto tm2_profiles = _sensor->get_stream_profiles(); - int ref_index = 0; - for (int i = 0; i < tm2_profiles.size(); i++) - if (tm2_profiles[i]->get_stream_type() == RS2_STREAM_POSE) - { - ref_index = i; - break; - } - - _extrinsics[stream.get_unique_id()] = std::make_pair(group_index, tm2_profiles[ref_index]); - } - -} diff --git a/src/tm2/tm-device.h b/src/tm2/tm-device.h deleted file mode 100644 index 2c15b9b2b6..0000000000 --- a/src/tm2/tm-device.h +++ /dev/null @@ -1,219 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2017 Intel Corporation. All Rights Reserved. -#pragma once - -#include -#include - -#include "../device.h" -#include "../core/video.h" -#include "../core/motion.h" -#include "../media/playback/playback_device.h" - -#include "../usb/usb-device.h" -#include "../usb/usb-messenger.h" - -#include "t265-messages.h" - -namespace librealsense -{ - class tm2_sensor; - - class tm2_device : public virtual device, public tm2_extensions - { - public: - tm2_device(std::shared_ptr ctx, - const platform::backend_device_group& group, - bool register_device_notifications); - - virtual ~tm2_device(); - - void hardware_reset() override; - - void enable_loopback(const std::string& source_file) override; - void disable_loopback() override; - bool is_enabled() const override; - void connect_controller(const std::array& mac_address) override {}; - void disconnect_controller(int id) override {}; - - std::vector get_profiles_tags() const override - { - return std::vector(); - }; - bool compress_while_record() const override { return false; } - - std::shared_ptr get_tm2_sensor() { return _sensor; } - - protected: - void register_stream_to_extrinsic_group(const stream_interface& stream, uint32_t group_index); - - private: - static const char* tm2_device_name() - { - return "Intel RealSense T265"; - } - std::shared_ptr _sensor; - - platform::usb_device_info usb_info; - platform::rs_usb_device usb_device; - platform::rs_usb_messenger usb_messenger; - - platform::rs_usb_endpoint endpoint_msg_out, endpoint_msg_in; - platform::rs_usb_endpoint endpoint_bulk_out, endpoint_bulk_in; - platform::rs_usb_endpoint endpoint_int_out, endpoint_int_in; - - std::mutex bulk_mutex; - template platform::usb_status bulk_request_response(const Request &request, Response &response, size_t max_response_size = 0, bool assert_success = true); - - platform::rs_usb_request interrupt_read_request(std::vector & buffer, std::shared_ptr callback); - - std::mutex stream_mutex; - platform::usb_status stream_write(const t265::bulk_message_request_header * request); - - platform::rs_usb_request stream_read_request(std::vector & buffer, std::shared_ptr callback); - - void submit_request(platform::rs_usb_request request); - bool cancel_request(platform::rs_usb_request request); - - friend class tm2_sensor; - }; - - class tm2_sensor : public sensor_base, public video_sensor_interface, public wheel_odometry_interface, - public pose_sensor_interface, public tm2_sensor_interface - { - public: - tm2_sensor(tm2_device* owner); - virtual ~tm2_sensor(); - - // sensor interface - //////////////////// - stream_profiles init_stream_profiles() override; - void open(const stream_profiles& requests) override; - void close() override; - void start(frame_callback_ptr callback) override; - void stop() override; - rs2_intrinsics get_intrinsics(const stream_profile& profile) const override; - rs2_motion_device_intrinsic get_motion_intrinsics(const motion_stream_profile_interface& profile) const; - rs2_extrinsics get_extrinsics(const stream_profile_interface & profile, int sensor_id) const; - - void enable_loopback(std::shared_ptr input); - void disable_loopback(); - bool is_loopback_enabled() const; - void dispose(); - t265::sensor_temperature get_temperature(int sensor_id); - void set_exposure_and_gain(float exposure_ms, float gain); - void set_exposure(float value); - float get_exposure() const; - void set_gain(float value); - float get_gain() const; - bool is_manual_exposure() const { return manual_exposure; } - void set_manual_exposure(bool manual); - - // Pose interfaces - bool export_relocalization_map(std::vector& lmap_buf) const override; - bool import_relocalization_map(const std::vector& lmap_buf) const override; - bool set_static_node(const std::string& guid, const float3& pos, const float4& orient_quat) const override; - bool get_static_node(const std::string& guid, float3& pos, float4& orient_quat) const override; - bool remove_static_node(const std::string& guid) const override; - - // Wheel odometer - bool load_wheel_odometery_config(const std::vector& odometry_config_buf) const override; - bool send_wheel_odometry(uint8_t wo_sensor_id, uint32_t frame_num, const float3& translational_velocity) const override; - - enum async_op_state { - _async_init = 1 << 0, - _async_progress = 1 << 1, - _async_success = 1 << 2, - _async_fail = 1 << 3, - _async_max = 1 << 4 - }; - - // Async operations handler - async_op_state perform_async_transfer(std::function transfer_activator, - std::function on_success, const std::string& op_description) const; - // Recording interfaces - virtual void create_snapshot(std::shared_ptr& snapshot) const override {} - virtual void enable_recording(std::function record_action) override {} - virtual void create_snapshot(std::shared_ptr& snapshot) const override {} - virtual void enable_recording(std::function record_action) override {} - - //calibration write interface - static const uint16_t ID_OEM_CAL = 6; - void set_intrinsics(const stream_profile_interface& stream_profile, const rs2_intrinsics& intr) override; - void set_extrinsics(const stream_profile_interface& from_profile, const stream_profile_interface& to_profile, const rs2_extrinsics& extr) override; - void set_motion_device_intrinsics(const stream_profile_interface& stream_profile, const rs2_motion_device_intrinsic& intr) override; - void reset_to_factory_calibration() override; - void write_calibration() override; - void set_extrinsics_to_ref(rs2_stream stream_type, int stream_index, const rs2_extrinsics& extr); - - t265::SIXDOF_MODE _tm_mode = t265::SIXDOF_MODE_ENABLE_MAPPING | t265::SIXDOF_MODE_ENABLE_RELOCALIZATION; - - private: - void handle_imu_frame(unsigned long long tm_frame_ts, unsigned long long frame_number, rs2_stream stream_type, int index, float3 imu_data, float temperature); - void pass_frames_to_fw(frame_holder fref); - void raise_relocalization_event(const std::string& msg, double timestamp); - void raise_error_notification(const std::string& msg); - - mutable std::mutex _tm_op_lock; - std::shared_ptr_loopback; - mutable std::condition_variable _async_op; - mutable async_op_state _async_op_status; - mutable std::vector _async_op_res_buffer; - - std::vector _supported_raw_streams; - std::vector _active_raw_streams; - bool _pose_output_enabled{false}; - tm2_device * _device; - - void print_logs(const std::unique_ptr & log); - - bool start_stream(); - bool start_interrupt(); - void stop_stream(); - void stop_interrupt(); - void time_sync(); - void log_poll(); - bool log_poll_once(std::unique_ptr & log_buffer); - std::thread _time_sync_thread; - std::thread _log_poll_thread; - std::atomic _time_sync_thread_stop; - std::atomic _log_poll_thread_stop; - - platform::rs_usb_request _interrupt_request; - platform::rs_usb_request_callback _interrupt_callback; - - platform::rs_usb_request _stream_request; - platform::rs_usb_request_callback _stream_callback; - - float last_exposure = 200.f; - float last_gain = 1.f; - bool manual_exposure = false; - - std::atomic device_to_host_ns; - class coordinated_ts { - public: - std::chrono::duration device_ts; - std::chrono::duration global_ts; - std::chrono::duration arrival_ts; - }; - coordinated_ts last_ts; - - coordinated_ts get_coordinated_timestamp(uint64_t device_nanoseconds); - - template friend class tracking_mode_option; - - // threaded dispatch - std::shared_ptr _data_dispatcher; - void dispatch_threaded(frame_holder frame); - - // interrupt endpoint receive - void receive_pose_message(const t265::interrupt_message_get_pose & message); - void receive_accel_message(const t265::interrupt_message_accelerometer_stream & message); - void receive_gyro_message(const t265::interrupt_message_gyro_stream & message); - void receive_set_localization_data_complete(const t265::interrupt_message_set_localization_data_stream & message); - - // stream endpoint receive - void receive_video_message(const t265::bulk_message_video_stream * message); - void receive_localization_data_chunk(const t265::interrupt_message_get_localization_data_stream * chunk); - }; -} diff --git a/src/tm2/tm-info.cpp b/src/tm2/tm-info.cpp deleted file mode 100644 index a995540f35..0000000000 --- a/src/tm2/tm-info.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2017 Intel Corporation. All Rights Reserved. - -#include -#include -#include -#include -#include -#include - -#include "tm-info.h" -#include "tm-device.h" -#include "common/fw/target.h" - -namespace librealsense -{ - tm2_info::tm2_info(std::shared_ptr ctx, platform::usb_device_info hwm) - : device_info(ctx), _hwm(std::move(hwm)) - { - LOG_DEBUG("tm2_info created for " << this); - } - - tm2_info::~tm2_info() - { - LOG_DEBUG("tm2_info destroyed for " << this); - } - - std::shared_ptr tm2_info::create(std::shared_ptr ctx, - bool register_device_notifications) const - { - LOG_DEBUG("tm2_info::create " << this); - return std::make_shared(ctx, get_device_data(), register_device_notifications); - } - - platform::backend_device_group tm2_info::get_device_data() const - { - LOG_DEBUG("tm2_info::get_device_data " << this); - auto bdg = platform::backend_device_group({}, { _hwm }); - return bdg; - } - - std::vector> tm2_info::pick_tm2_devices( - std::shared_ptr ctx, - std::vector& usb) - { - std::vector> results; - // We shouldn't talk to the device here, it might not - // even be around anymore (this gets called with an out of - // date list on disconnect). - auto correct_pid = filter_by_product(usb, { 0x0B37 }); - if (correct_pid.size()) - { - LOG_INFO("Picked " << correct_pid.size() << "/" << usb.size() << " devices"); - - for(auto & dev : correct_pid) - results.push_back(std::make_shared(ctx, dev)); - } - - return results; - } -} diff --git a/src/tm2/tm-info.h b/src/tm2/tm-info.h deleted file mode 100644 index bff0d88f32..0000000000 --- a/src/tm2/tm-info.h +++ /dev/null @@ -1,27 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2017 Intel Corporation. All Rights Reserved. -#pragma once - -#include -#include - -#include "../context.h" -#include "tm-device.h" - -namespace librealsense -{ - class tm2_info : public device_info - { - public: - tm2_info(std::shared_ptr ctx, platform::usb_device_info hwm); - ~tm2_info(); - std::shared_ptr create(std::shared_ptr ctx, bool register_device_notifications) const override; - platform::backend_device_group get_device_data() const override; - - static std::vector> pick_tm2_devices( - std::shared_ptr ctx, - std::vector& usb); - private: - platform::usb_device_info _hwm; - }; -} diff --git a/src/win/win-helpers.cpp b/src/win/win-helpers.cpp index 396c328467..260ab0251f 100644 --- a/src/win/win-helpers.cpp +++ b/src/win/win-helpers.cpp @@ -25,9 +25,7 @@ #include // DEVPKEY_... //https://docs.microsoft.com/en-us/windows-hardware/drivers/usbcon/supported-usb-classes#microsoft-provided-usb-device-class-drivers -#ifndef WITH_TRACKING DEFINE_GUID(GUID_DEVINTERFACE_USB_DEVICE, 0xA5DCBF10L, 0x6530, 0x11D2, 0x90, 0x1F, 0x00, 0xC0, 0x4F, 0xB9, 0x51, 0xED); -#endif DEFINE_GUID(GUID_DEVINTERFACE_IMAGE_WIN10, 0x6bdd1fc6L, 0x810f, 0x11d0, 0xbe, 0xc7, 0x08, 0x00, 0x2b, 0xe2, 0x09, 0x2f); DEFINE_GUID(GUID_DEVINTERFACE_CAMERA_WIN10, 0xca3e7ab9, 0xb4c3, 0x4ae6, 0x82, 0x51, 0x57, 0x9e, 0xf9, 0x33, 0x89, 0x0f); diff --git a/wrappers/python/pyrs_device.cpp b/wrappers/python/pyrs_device.cpp index 1a1085333b..25bc53b408 100644 --- a/wrappers/python/pyrs_device.cpp +++ b/wrappers/python/pyrs_device.cpp @@ -30,7 +30,6 @@ void init_device(py::module &m) { .def(BIND_DOWNCAST(device, debug_protocol)) .def(BIND_DOWNCAST(device, playback)) .def(BIND_DOWNCAST(device, recorder)) - .def(BIND_DOWNCAST(device, tm2)) .def(BIND_DOWNCAST(device, updatable)) .def(BIND_DOWNCAST(device, update_device)) .def(BIND_DOWNCAST(device, auto_calibrated_device)) @@ -275,21 +274,5 @@ void init_device(py::module &m) { .def("front", &rs2::device_list::front) // No docstring in C++ .def("back", &rs2::device_list::back); // No docstring in C++ - py::class_ tm2(m, "tm2", "The tm2 class is an interface for T2XX devices, such as T265.\n" - "For T265, it provides RS2_STREAM_FISHEYE(2), RS2_STREAM_GYRO, " - "RS2_STREAM_ACCEL, and RS2_STREAM_POSE streams, and contains the following sensors:\n" - "-pose_sensor: map and relocalization functions.\n" - "-wheel_odometer: input for odometry data."); - tm2.def(py::init(), "device"_a) - .def("enable_loopback", &rs2::tm2::enable_loopback, "Enter the given device into " - "loopback operation mode that uses the given file as input for raw data", "filename"_a) - .def("disable_loopback", &rs2::tm2::disable_loopback, "Restores the given device into normal operation mode") - .def("is_loopback_enabled", &rs2::tm2::is_loopback_enabled, "Checks if the device is in loopback mode or not") - .def("set_intrinsics", &rs2::tm2::set_intrinsics, "Set camera intrinsics", "sensor_id"_a, "intrinsics"_a) - .def("set_extrinsics", &rs2::tm2::set_extrinsics, "Set camera extrinsics", "from_stream"_a, "from_id"_a, "to_stream"_a, "to_id"_a, "extrinsics"_a) - .def("set_motion_device_intrinsics", &rs2::tm2::set_motion_device_intrinsics, "Set motion device intrinsics", "stream_type"_a, "motion_intrinsics"_a) - .def("reset_to_factory_calibration", &rs2::tm2::reset_to_factory_calibration, "Reset to factory calibration") - .def("write_calibration", &rs2::tm2::write_calibration, "Write calibration to device's EEPROM"); - /** end rs_device.hpp **/ } From 7b93daa05c47f72a3a67257fd2adffc41515fd33 Mon Sep 17 00:00:00 2001 From: Eran Date: Tue, 3 Jan 2023 10:23:12 +0200 Subject: [PATCH 2/4] fix fw.rc; remove from GHA --- .github/workflows/buildsCI.yaml | 12 ++++++------ common/fw/CMakeLists.txt | 12 ++++++------ common/fw/fw.rc | 2 +- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/.github/workflows/buildsCI.yaml b/.github/workflows/buildsCI.yaml index c106a46eb1..6f8ebd97bd 100644 --- a/.github/workflows/buildsCI.yaml +++ b/.github/workflows/buildsCI.yaml @@ -53,7 +53,7 @@ jobs: cd ${{env.WIN_BUILD_DIR}} pwd ls - cmake ${LRS_SRC_DIR} -G "Visual Studio 16 2019" -DBUILD_SHARED_LIBS=true -DBUILD_EXAMPLES=true -DBUILD_TOOLS=true -DBUILD_WITH_TM2=false -DCHECK_FOR_UPDATES=true + cmake ${LRS_SRC_DIR} -G "Visual Studio 16 2019" -DBUILD_SHARED_LIBS=true -DBUILD_EXAMPLES=true -DBUILD_TOOLS=true -DCHECK_FOR_UPDATES=true - name: Build # Build your program with the given configuration @@ -97,7 +97,7 @@ jobs: run: | LRS_SRC_DIR=$(pwd) cd ${{env.WIN_BUILD_DIR}} - cmake ${LRS_SRC_DIR} -G "Visual Studio 16 2019" -DBUILD_SHARED_LIBS=false -DBUILD_EXAMPLES=false -DBUILD_TOOLS=true -DBUILD_WITH_TM2=false -DCHECK_FOR_UPDATES=false -DBUILD_WITH_DDS=true -DPYTHON_EXECUTABLE=${{env.PYTHON_PATH}} -DBUILD_PYTHON_BINDINGS=true + cmake ${LRS_SRC_DIR} -G "Visual Studio 16 2019" -DBUILD_SHARED_LIBS=false -DBUILD_EXAMPLES=false -DBUILD_TOOLS=true -DCHECK_FOR_UPDATES=false -DBUILD_WITH_DDS=true -DPYTHON_EXECUTABLE=${{env.PYTHON_PATH}} -DBUILD_PYTHON_BINDINGS=true - name: Build # Build your program with the given configuration @@ -142,7 +142,7 @@ jobs: run: | LRS_SRC_DIR=$(pwd) cd ${{env.WIN_BUILD_DIR}} - cmake ${LRS_SRC_DIR} -G "Visual Studio 16 2019" -DBUILD_SHARED_LIBS=true -DBUILD_EXAMPLES=false -DBUILD_TOOLS=false -DBUILD_WITH_TM2=false -DCHECK_FOR_UPDATES=false -DPYTHON_EXECUTABLE=${{env.PYTHON_PATH}} -DBUILD_PYTHON_BINDINGS=true -DFORCE_RSUSB_BACKEND=true -DBUILD_CSHARP_BINDINGS=true -DDOTNET_VERSION_LIBRARY="4.5" -DDOTNET_VERSION_EXAMPLES="4.5" + cmake ${LRS_SRC_DIR} -G "Visual Studio 16 2019" -DBUILD_SHARED_LIBS=true -DBUILD_EXAMPLES=false -DBUILD_TOOLS=false -DCHECK_FOR_UPDATES=false -DPYTHON_EXECUTABLE=${{env.PYTHON_PATH}} -DBUILD_PYTHON_BINDINGS=true -DFORCE_RSUSB_BACKEND=true -DBUILD_CSHARP_BINDINGS=true -DDOTNET_VERSION_LIBRARY="4.5" -DDOTNET_VERSION_EXAMPLES="4.5" - name: Build # Build your program with the given configuration @@ -178,7 +178,7 @@ jobs: shell: bash run: | cd build - cmake .. -DBUILD_SHARED_LIBS=false -DBUILD_EXAMPLES=true -DBUILD_TOOLS=true -DBUILD_WITH_TM2=false -DCHECK_FOR_UPDATES=true -DBUILD_LEGACY_LIVE_TEST=true -DBUILD_PYTHON_BINDINGS=true -DPYTHON_EXECUTABLE=$(which python3) + cmake .. -DBUILD_SHARED_LIBS=false -DBUILD_EXAMPLES=true -DBUILD_TOOLS=true -DCHECK_FOR_UPDATES=true -DBUILD_LEGACY_LIVE_TEST=true -DBUILD_PYTHON_BINDINGS=true -DPYTHON_EXECUTABLE=$(which python3) cmake --build . --config ${{env.LRS_RUN_CONFIG}} -- -j4 - name: Test @@ -241,7 +241,7 @@ jobs: shell: bash run: | cd build - cmake .. -DBUILD_SHARED_LIBS=true -DBUILD_EXAMPLES=false -DBUILD_TOOLS=true -DBUILD_WITH_TM2=false -DCHECK_FOR_UPDATES=false -DBUILD_WITH_DDS=true -DBUILD_PYTHON_BINDINGS=true -DPYTHON_EXECUTABLE=$(which python3) + cmake .. -DBUILD_SHARED_LIBS=true -DBUILD_EXAMPLES=false -DBUILD_TOOLS=true -DCHECK_FOR_UPDATES=false -DBUILD_WITH_DDS=true -DBUILD_PYTHON_BINDINGS=true -DPYTHON_EXECUTABLE=$(which python3) cmake --build . --config ${{env.LRS_RUN_CONFIG}} -- -j4 @@ -280,7 +280,7 @@ jobs: shell: bash run: | cd build - cmake .. -DBUILD_SHARED_LIBS=true -DBUILD_EXAMPLES=false -DBUILD_TOOLS=false -DBUILD_WITH_TM2=false -DCHECK_FOR_UPDATES=false -DFORCE_RSUSB_BACKEND=true -DBUILD_LEGACY_LIVE_TEST=true + cmake .. -DBUILD_SHARED_LIBS=true -DBUILD_EXAMPLES=false -DBUILD_TOOLS=false -DCHECK_FOR_UPDATES=false -DFORCE_RSUSB_BACKEND=true -DBUILD_LEGACY_LIVE_TEST=true cmake --build . --config $LRS_BUILD_CONFIG -- -j4 - name: Test diff --git a/common/fw/CMakeLists.txt b/common/fw/CMakeLists.txt index a1482ee7e6..db4dbf37d0 100644 --- a/common/fw/CMakeLists.txt +++ b/common/fw/CMakeLists.txt @@ -13,19 +13,19 @@ set(REALSENSE_FIRMWARE_URL "https://librealsense.intel.com" CACHE STRING string(REGEX MATCH "D4XX_RECOMMENDED_FIRMWARE_VERSION \"([0-9]+.[0-9]+.[0-9]+.[0-9]+)\"" _ ${ver}) set(D4XX_FW_VERSION ${CMAKE_MATCH_1}) -message(STATUS "D4XX_FW_VERSION: ${D4XX_FW_VERSION}") +#message(STATUS "D4XX_FW_VERSION: ${D4XX_FW_VERSION}") set(D4XX_FW_SHA1 8dc3b8a0b9fdf03410c15b30e04f57701985064a) set(D4XX_FW_URL "${REALSENSE_FIRMWARE_URL}/Releases/RS4xx/FW") string(REGEX MATCH "SR3XX_RECOMMENDED_FIRMWARE_VERSION \"([0-9]+.[0-9]+.[0-9]+.[0-9]+)\"" _ ${ver}) set(SR3XX_FW_VERSION ${CMAKE_MATCH_1}) -message(STATUS "SR3XX_FW_VERSION: ${SR3XX_FW_VERSION}") +#message(STATUS "SR3XX_FW_VERSION: ${SR3XX_FW_VERSION}") set(SR3XX_FW_SHA1 55237dba5d7db20e7c218975375d05b4210e9460) set(SR3XX_FW_URL "${REALSENSE_FIRMWARE_URL}/Releases/SR300/FW") string(REGEX MATCH "L51X_RECOMMENDED_FIRMWARE_VERSION \"([0-9]+\.[0-9]+\.[0-9]+\.[0-9]+)\"" _ ${ver}) set(L51X_FW_VERSION ${CMAKE_MATCH_1}) -message(STATUS "L51X_FW_VERSION: ${L51X_FW_VERSION}") +#message(STATUS "L51X_FW_VERSION: ${L51X_FW_VERSION}") set(L51X_FW_SHA1 ab73e5bfc520c0aa0340cada4b3e317b8fd31a4d) set(L51X_FW_URL "${REALSENSE_FIRMWARE_URL}/Releases/L5xx/FW") @@ -51,15 +51,15 @@ set_target_properties (${PROJECT_NAME} PROPERTIES FOLDER Resources) function(target_binary url version sha1 symbol ext) set(binary "${CMAKE_CURRENT_BINARY_DIR}/${symbol}-${version}${ext}") - message(STATUS "${url}/${symbol}-${version}${ext}") + message(STATUS "... ${url}/${symbol}-${version}${ext}") file(DOWNLOAD "${url}/${symbol}-${version}${ext}" "${binary}" EXPECTED_HASH SHA1=${sha1} STATUS status) list(GET status 0 error_code) if (NOT ${error_code} EQUAL 0) - message(FATAL_ERROR "Download firmwnare (${status}) - ${url}") + message(FATAL_ERROR "FAILED with status ${status}") else() - message(STATUS "Download firmware ${status} for ${symbol}-${version}${ext}") + #message(STATUS "Download firmware ${status} for ${symbol}-${version}${ext}") endif() string(TOUPPER ${symbol} SYMBOL) string(REPLACE "." "," version_commas ${version}) diff --git a/common/fw/fw.rc b/common/fw/fw.rc index 5b955cc3d0..6d5f15fa65 100644 --- a/common/fw/fw.rc +++ b/common/fw/fw.rc @@ -1,4 +1,4 @@ #include "D4XX_FW_Image.rc" #include "SR3XX_FW_Image.rc" #include "L51X_FW_Image.rc" -#include "target.rc" + From c514cc733c9eab47dc20308a85b54e62bb9140f1 Mon Sep 17 00:00:00 2001 From: Eran Date: Tue, 3 Jan 2023 10:25:53 +0200 Subject: [PATCH 3/4] fix warning in rsutils::version --- third-party/rsutils/include/rsutils/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/third-party/rsutils/include/rsutils/version.h b/third-party/rsutils/include/rsutils/version.h index d337db76e8..bf8330cef2 100644 --- a/third-party/rsutils/include/rsutils/version.h +++ b/third-party/rsutils/include/rsutils/version.h @@ -42,7 +42,7 @@ struct version { } - bool is_valid() const { return number; } + bool is_valid() const { return( number != 0 ); } operator bool() const { return is_valid(); } void clear() { number = 0; } From f958cca8f48e26f6b45d89158a0d1dc298ed2825 Mon Sep 17 00:00:00 2001 From: Eran Date: Tue, 3 Jan 2023 11:08:29 +0200 Subject: [PATCH 4/4] try to fix legacy live-tests --- unit-tests/unit-tests-live.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/unit-tests/unit-tests-live.cpp b/unit-tests/unit-tests-live.cpp index 6a389a3d09..87e4043d67 100644 --- a/unit-tests/unit-tests-live.cpp +++ b/unit-tests/unit-tests-live.cpp @@ -3714,7 +3714,7 @@ TEST_CASE("Per-frame metadata sanity check", "[live][!mayfail]") { std::vector frames_additional_data; auto frames = 0; - double start; + double start = 0; std::condition_variable cv; std::mutex m; auto first = true; @@ -5602,7 +5602,6 @@ TEST_CASE("Positional_Sensors_API", "[live]") else { CAPTURE(dev); - REQUIRE(dev.is()); REQUIRE_NOTHROW(dev.first()); auto pose_snr = dev.first(); CAPTURE(pose_snr); @@ -5736,7 +5735,6 @@ TEST_CASE("Wheel_Odometry_API", "[live]") else { CAPTURE(dev); - REQUIRE(dev.is()); auto wheel_odom_snr = dev.first(); CAPTURE(wheel_odom_snr); REQUIRE(wheel_odom_snr);