diff --git a/CMakeLists.txt b/CMakeLists.txt index 2b85e6968d..5784602b3e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,7 +121,10 @@ elseif(UNIX) endif() -option(BUILD_WITH_TM2 "Build with support for Intel TM2 tracking device" OFF) +option(BUILD_WITH_TM2 "Build with support for Intel TM2 tracking device" ON) +if(APPLE OR ANDROID_NDK_TOOLCHAIN_INCLUDED) + set(BUILD_WITH_TM2 OFF) +endif() option(BUILD_EASYLOGGINGPP "Build EasyLogging++ as a part of the build" ON) @@ -785,34 +788,38 @@ if(HWM_OVER_XU) add_definitions(-DHWM_OVER_XU) endif() -if (NOT USE_SYSTEM_LIBUSB) - if(NOT WIN32 OR FORCE_LIBUVC) - include(ExternalProject) - - ExternalProject_Add( - libusb - - GIT_REPOSITORY "https://github.com/libusb/libusb.git" - GIT_TAG "master" - - UPDATE_COMMAND ${CMAKE_COMMAND} -E copy - ${CMAKE_CURRENT_SOURCE_DIR}/third-party/libusb/CMakeLists.txt - ${CMAKE_CURRENT_BINARY_DIR}/third-party/libusb/CMakeLists.txt - PATCH_COMMAND "" - - SOURCE_DIR "third-party/libusb/" - CMAKE_ARGS -DCMAKE_CXX_STANDARD_LIBRARIES=${CMAKE_CXX_STANDARD_LIBRARIES} -DCMAKE_TOOLCHAIN_FILE=${CMAKE_TOOLCHAIN_FILE} -DCMAKE_INSTALL_PREFIX=${CMAKE_CURRENT_BINARY_DIR}/libusb_install - - TEST_COMMAND "" - ) - - set(LIBUSB_LOCAL_INCLUDE_PATH ${CMAKE_CURRENT_BINARY_DIR}/third-party/libusb) - - set(LIBUSB1_LIBRARY_DIRS ${CMAKE_CURRENT_BINARY_DIR}/libusb_install/lib) - link_directories(${LIBUSB1_LIBRARY_DIRS}) - - set(LIBUSB1_LIBRARIES usb log) - endif() +if ((NOT USE_SYSTEM_LIBUSB AND (NOT WIN32 OR FORCE_LIBUVC)) OR (BUILD_WITH_TM2 AND WIN32)) + include(ExternalProject) + + ExternalProject_Add( + libusb + + GIT_REPOSITORY "https://github.com/libusb/libusb.git" + GIT_TAG "master" + + UPDATE_COMMAND ${CMAKE_COMMAND} -E copy + ${CMAKE_CURRENT_SOURCE_DIR}/third-party/libusb/CMakeLists.txt + ${CMAKE_CURRENT_BINARY_DIR}/third-party/libusb/CMakeLists.txt + PATCH_COMMAND "" + + SOURCE_DIR "third-party/libusb/" + CMAKE_ARGS -DCMAKE_CXX_STANDARD_LIBRARIES=${CMAKE_CXX_STANDARD_LIBRARIES} + -DCMAKE_TOOLCHAIN_FILE=${CMAKE_TOOLCHAIN_FILE} + -DCMAKE_INSTALL_PREFIX=${CMAKE_CURRENT_BINARY_DIR}/libusb_install + TEST_COMMAND "" + ) + + set(LIBUSB_LOCAL_INCLUDE_PATH ${CMAKE_CURRENT_BINARY_DIR}/third-party/libusb) + + set(LIBUSB1_LIBRARY_DIRS ${CMAKE_CURRENT_BINARY_DIR}/libusb_install/lib) + link_directories(${LIBUSB1_LIBRARY_DIRS}) + + set(LIBUSB1_LIBRARIES usb) + + if(NOT WIN32) + set(LIBUSB1_LIBRARIES ${LIBUSB1_LIBRARIES} log) + endif() + endif() add_subdirectory(third-party/realsense-file) @@ -820,7 +827,7 @@ add_subdirectory(third-party/realsense-file) if (BUILD_WITH_TM2) message(STATUS "Building with TM2") - + add_subdirectory(third-party/libtm) if(WIN32) source_group("Source Files\\Devices\\Tracking" FILES src/tm2/tm-context.cpp @@ -835,22 +842,10 @@ if (BUILD_WITH_TM2) src/tm2/tm-conversions.h src/tm2/controller_event_serializer.h ) + add_dependencies(tm libusb) endif() list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/CMake) - find_package(libtm REQUIRED) - - if(BUILD_WITH_STATIC_CRT) - foreach(flag_var - CMAKE_CXX_FLAGS CMAKE_CXX_FLAGS_DEBUG CMAKE_CXX_FLAGS_RELEASE - CMAKE_CXX_FLAGS_MINSIZEREL CMAKE_CXX_FLAGS_RELWITHDEBINFO - CMAKE_C_FLAGS CMAKE_C_FLAGS_DEBUG CMAKE_C_FLAGS_RELEASE - CMAKE_C_FLAGS_MINSIZEREL CMAKE_C_FLAGS_RELWITHDEBINFO) - if(${flag_var} MATCHES "/MD") - string(REGEX REPLACE "/MD" "/MT" ${flag_var} "${${flag_var}}") - endif(${flag_var} MATCHES "/MD") - endforeach(flag_var) - endif() list(APPEND REALSENSE_HPP src/tm2/tm-context.h @@ -865,16 +860,6 @@ if (BUILD_WITH_TM2) src/tm2/tm-device.cpp src/tm2/tm-info.cpp ) - - include_directories(${LIBTM_INCLUDE_DIR}) - set(TRACKING_DEVICE_LIBS libtm) - - install(TARGETS libtm - EXPORT realsense2Targets - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - ) endif() if(LRS_TRY_USE_AVX) @@ -902,6 +887,8 @@ endif() if(BUILD_WITH_TM2) target_compile_definitions(realsense2 PRIVATE WITH_TRACKING=1 BUILD_STATIC=1) + target_link_libraries(realsense2 PRIVATE tm ${CMAKE_THREAD_LIBS_INIT} ${TRACKING_DEVICE_LIBS}) + target_include_directories(realsense2 PRIVATE third-party/libtm/libtm/include) endif() set_target_properties(realsense2 PROPERTIES VERSION ${REALSENSE_VERSION_STRING} SOVERSION ${REALSENSE_VERSION_MAJOR}) diff --git a/appveyor.yml b/appveyor.yml index 226f1177de..4835377a13 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -91,7 +91,7 @@ test_script: if ($LASTEXITCODE -ne 0) { - $test_failed += $element + $test_failed += "software-device" } foreach ($element in $records) diff --git a/common/model-views.cpp b/common/model-views.cpp index 143cd39b76..a77ca10787 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -1397,7 +1397,7 @@ namespace rs2 _pause = false; } - void subdevice_model::play(const std::vector& profiles, viewer_model& viewer) + void subdevice_model::play(const std::vector& profiles, viewer_model& viewer, std::shared_ptr syncer) { std::stringstream ss; ss << "Starting streaming of "; @@ -1412,11 +1412,11 @@ namespace rs2 s->open(profiles); try { - s->start([&](frame f) + s->start([&, syncer](frame f) { if (viewer.synchronization_enable && (!viewer.is_3d_view || viewer.is_3d_depth_source(f) || viewer.is_3d_texture_source(f))) { - viewer.s.invoke(f); + syncer->invoke(f); } else { @@ -1498,7 +1498,7 @@ namespace rs2 for (auto i = 0; i < RS2_OPTION_COUNT; i++) { auto opt = static_cast(i); - if (opt == RS2_OPTION_FRAMES_QUEUE_SIZE) continue; + if (skip_option(opt)) continue; if (std::find(drawing_order.begin(), drawing_order.end(), opt) == drawing_order.end()) { draw_option(opt, update_read_only_options, error_message, notifications); @@ -1511,7 +1511,7 @@ namespace rs2 return (uint64_t)std::count_if( std::begin(options_metadata), std::end(options_metadata), - [](const std::pair& p) {return p.second.supported && p.second.opt != RS2_OPTION_FRAMES_QUEUE_SIZE; }); + [](const std::pair& p) {return p.second.supported && !skip_option(p.second.opt); }); } bool option_model::draw_option(bool update_read_only_options, @@ -2100,7 +2100,7 @@ namespace rs2 if (render_pose) { total_top_bar_height += top_bar_height; // add additional bar height for pose - const int num_of_pose_buttons = 3; // trajectory, draw camera, boundary selection + const int num_of_pose_buttons = 2; // trajectory, draw camera ImGui::SetNextWindowPos({ stream_rect.x, stream_rect.y + buttons_heights }); ImGui::SetNextWindowSize({ stream_rect.w, buttons_heights }); @@ -2137,25 +2137,6 @@ namespace rs2 if (ImGui::IsItemHovered()) ImGui::SetTooltip("%s", tm2.trajectory_button.get_tooltip().c_str()); - // Draw boundary selection button - ImGui::SameLine(); - color_icon = tm2.boundary_button.is_pressed(); //draw boundary is on - color the icon - if (color_icon) - { - ImGui::PushStyleColor(ImGuiCol_Text, light_blue); - ImGui::PushStyleColor(ImGuiCol_TextSelectedBg, light_blue); - } - if (ImGui::Button(tm2.boundary_button.get_icon().c_str(), { 24, buttons_heights })) - { - tm2.boundary_button.toggle_button(); - } - if (color_icon) - { - ImGui::PopStyleColor(2); - } - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("%s", tm2.boundary_button.get_tooltip().c_str()); - ImGui::End(); } @@ -2799,8 +2780,10 @@ namespace rs2 void device_model::reset() { + syncer->remove_syncer(dev_syncer); subdevices.resize(0); _recorder.reset(); + } std::pair get_device_name(const device& dev) @@ -2825,7 +2808,8 @@ namespace rs2 } device_model::device_model(device& dev, std::string& error_message, viewer_model& viewer) - : dev(dev) + : dev(dev), + syncer(viewer.syncer) { for (auto&& sub : dev.query_sensors()) { @@ -2871,6 +2855,8 @@ namespace rs2 } void device_model::play_defaults(viewer_model& viewer) { + if(!dev_syncer) + dev_syncer = viewer.syncer->create_syncer(); for (auto&& sub : subdevices) { if (!sub->streaming) @@ -2888,7 +2874,7 @@ namespace rs2 if (profiles.empty()) continue; - sub->play(profiles, viewer); + sub->play(profiles, viewer, dev_syncer); for (auto&& profile : profiles) { @@ -2981,28 +2967,90 @@ namespace rs2 { ImGui::OpenPopup(name.c_str()); } + + if (ImGui::BeginPopupModal(name.c_str(), nullptr, ImGuiWindowFlags_AlwaysAutoResize)) + { + ImGui::Text("RealSense error calling:"); + ImGui::PushStyleColor(ImGuiCol_TextSelectedBg, regular_blue); + ImGui::InputTextMultiline("error", const_cast(error_message.c_str()), + error_message.size() + 1, { 500,100 }, ImGuiInputTextFlags_AutoSelectAll | ImGuiInputTextFlags_ReadOnly); + ImGui::PopStyleColor(); + + if (ImGui::Button("OK", ImVec2(120, 0))) + { + if (dont_show_this_error) + { + errors_not_to_show.insert(simplify_error_message(error_message)); + } + error_message = ""; + ImGui::CloseCurrentPopup(); + dont_show_this_error = false; + } + + ImGui::SameLine(); + ImGui::Checkbox("Don't show this error again", &dont_show_this_error); + + ImGui::EndPopup(); + } } + + ImGui::PopStyleColor(3); + ImGui::PopStyleVar(2); + } + + void rs2::viewer_model::popup_if_ui_not_aligned(ImFont* font_14) + { + constexpr const char* graphics_updated_driver = "https://downloadcenter.intel.com/download/27266/Graphics-Intel-Graphics-Driver-for-Windows-15-60-?product=80939"; + + if (continue_with_ui_not_aligned) + return; + + std::string error_message = to_string() << + "The application has detected possible UI alignment issue, \n" << + "sometimes caused by outdated graphics card drivers.\n" << + "For Intel Integrated Graphics driver \n"; + + auto flags = ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | + ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_NoTitleBar | + ImGuiWindowFlags_NoSavedSettings | ImGuiWindowFlags_AlwaysVerticalScrollbar; + + ImGui_ScopePushFont(font_14); + ImGui::PushStyleColor(ImGuiCol_PopupBg, sensor_bg); + ImGui::PushStyleColor(ImGuiCol_TextSelectedBg, white); + ImGui::PushStyleColor(ImGuiCol_Text, light_grey); + ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(10, 10)); + ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 1); + + std::string name = to_string() << " " << textual_icons::exclamation_triangle << " UI Offset Detected"; + + + ImGui::OpenPopup(name.c_str()); + if (ImGui::BeginPopupModal(name.c_str(), nullptr, ImGuiWindowFlags_AlwaysAutoResize)) { - ImGui::Text("RealSense error calling:"); ImGui::PushStyleColor(ImGuiCol_TextSelectedBg, regular_blue); - ImGui::InputTextMultiline("error", const_cast(error_message.c_str()), - error_message.size() + 1, { 500,100 }, ImGuiInputTextFlags_AutoSelectAll | ImGuiInputTextFlags_ReadOnly); + ImGui::Text(const_cast(error_message.c_str())); ImGui::PopStyleColor(); - if (ImGui::Button("OK", ImVec2(120, 0))) + ImGui::PushStyleColor(ImGuiCol_Button, transparent); + ImGui::PushStyleColor(ImGuiCol_ButtonHovered, transparent); + ImGui::PushStyleColor(ImGuiCol_ButtonActive, transparent); + ImGui::PushStyleColor(ImGuiCol_Text, light_blue); + ImGui::PushStyleColor(ImGuiCol_TextSelectedBg, white); + + ImGui::SetCursorPos({ 190, 42 }); + if (ImGui::Button("click here", { 150, 60 })) { - if (dont_show_this_error) - { - errors_not_to_show.insert(simplify_error_message(error_message)); - } - error_message = ""; - ImGui::CloseCurrentPopup(); - dont_show_this_error = false; + open_url(graphics_updated_driver); } - ImGui::SameLine(); - ImGui::Checkbox("Don't show this error again", &dont_show_this_error); + ImGui::PopStyleColor(5); + + if (ImGui::Button(" Ignore & Continue ", ImVec2(150, 0))) + { + continue_with_ui_not_aligned = true; + ImGui::CloseCurrentPopup(); + } ImGui::EndPopup(); } @@ -3169,21 +3217,36 @@ namespace rs2 source.frame_ready(std::move(res)); } + void post_processing_filters::start() + { + if (render_thread_active.exchange(true) == false) + { + viewer.syncer->start(); + render_thread = std::thread([&](){post_processing_filters::render_loop();}); + } + } + void post_processing_filters::stop() + { + if (render_thread_active.exchange(false) == true) + { + viewer.syncer->stop(); + render_thread.join(); + } + } void post_processing_filters::render_loop() { while (render_thread_active) { try { - frame frm; if(viewer.synchronization_enable) { - auto index = 0; - while (syncer_queue.try_wait_for_frame(&frm, 30) && ++index <= syncer_queue.capacity()) - { - processing_block.invoke(frm); - } + auto frames = viewer.syncer->try_wait_for_frames(); + for(auto f:frames) + { + processing_block.invoke(f); + } } else { @@ -3794,6 +3857,7 @@ namespace rs2 glTranslatef(0, 0, -1); + // Render "floor" grid for (int i = 0; i <= 6; i++) { glVertex3i(i - 3, 1, 0); @@ -3803,7 +3867,7 @@ namespace rs2 } glEnd(); - texture_buffer::draw_axis(0.1f, 1); + texture_buffer::draw_axes(0.4f, 1); if (draw_plane) { @@ -3864,7 +3928,6 @@ namespace rs2 rs2_vector translation{ pose_trans.mat[0][3], pose_trans.mat[1][3], pose_trans.mat[2][3] }; tracked_point p{ translation , pose_data.tracker_confidence }; //TODO: Osnat - use tracker_confidence or mapper_confidence ? tm2.draw_trajectory(p); - tm2.draw_boundary(p); } } @@ -4113,9 +4176,13 @@ namespace rs2 bool viewer_model::is_3d_texture_source(frame f) { - auto index = f.get_profile().unique_id(); + auto profile = f.get_profile().as(); + auto index = profile.unique_id(); auto mapped_index = streams_origin[index]; + if (!is_rasterizeable(profile.format())) + return false; + if (index == selected_tex_source_uid || mapped_index == selected_tex_source_uid || selected_tex_source_uid == -1) return true; return false; @@ -4123,6 +4190,7 @@ namespace rs2 bool viewer_model::is_3d_depth_source(frame f) { + auto index = f.get_profile().unique_id(); auto mapped_index = streams_origin[index]; @@ -5754,7 +5822,15 @@ namespace rs2 auto profiles = sub->get_selected_profiles(); try { - sub->play(profiles, viewer); + if(!dev_syncer) + dev_syncer = viewer.syncer->create_syncer(); + + std::string friendly_name = sub->s->get_info(RS2_CAMERA_INFO_NAME); + if (friendly_name.find("Tracking") != std::string::npos) + { + viewer.synchronization_enable = false; +} + sub->play(profiles, viewer, dev_syncer); } catch (const error& e) { @@ -5881,7 +5957,7 @@ namespace rs2 for (auto i = 0; i < RS2_OPTION_COUNT; i++) { auto opt = static_cast(i); - if (opt == RS2_OPTION_FRAMES_QUEUE_SIZE) continue; + if (skip_option(opt)) continue; if (std::find(drawing_order.begin(), drawing_order.end(), opt) == drawing_order.end()) { if (dev.is() && opt == RS2_OPTION_VISUAL_PRESET) @@ -5917,7 +5993,7 @@ namespace rs2 for (auto i = 0; i < RS2_OPTION_COUNT; i++) { auto opt = static_cast(i); - if (opt == RS2_OPTION_FRAMES_QUEUE_SIZE) continue; + if (skip_option(opt)) continue; pb->get_option(opt).draw_option( dev.is() || update_read_only_options, false, error_message, viewer.not_model); @@ -6086,7 +6162,7 @@ namespace rs2 for (auto i = 0; i < RS2_OPTION_COUNT; i++) { auto opt = static_cast(i); - if (opt == RS2_OPTION_FRAMES_QUEUE_SIZE) continue; + if (skip_option(opt)) continue; pb->get_option(opt).draw_option( dev.is() || update_read_only_options, false, error_message, viewer.not_model); @@ -6592,7 +6668,7 @@ namespace rs2 } else //draw axis { - texture_buffer::draw_axis(0.1f, 1.f); + texture_buffer::draw_axes(0.1f, 1.f); } } @@ -6664,76 +6740,6 @@ namespace rs2 } } - void tm2_model::draw_boundary(tracked_point& p) - { - if (!boundary_button.is_pressed()) - { - //TODO - separate button - if (boundary.size() > 0) - { - //cleanup last boundary - boundary.clear(); - } - return; - } - - // if new boundary - grab from trajectory - if (boundary.size() == 0) - { - std::vector trajectory_projection; - //create the boundary from the trajectory - for (auto&& v : trajectory) - { - // project the trajectory on XZ plane - ignore y coordinate of the point - float2 p{ v.first.x, v.first.z }; - trajectory_projection.push_back(p); - } - boundary = simplify_line(trajectory_projection); - } - // check if there is any boundary to render - if (boundary.size() == 0) - { - return; - } - - // check if the current position is inside or outside the boundary, to color it accordingly - float2 point{ p.first.x, p.first.z }; - bool inside = point_in_polygon_2D(boundary, point); - color c; - if (inside) - { - c = { 0.0f, 1.0f, 0.0f }; - } - else - { - c = { 1.0f, 0.0f, 0.0f }; - } - // draw the boundary lines parallel to XZ plane - glLineWidth(1.0f); - for (float height = -1.0f; height < 1.0f; height += 0.2f) - { - glBegin(GL_LINE_STRIP); - glColor3f(c[0], c[1], c[2]); - for (auto&& v : boundary) - { - glVertex3f(v.x, height, v.y); - } - glVertex3f(boundary[0].x, height, boundary[0].y); - glEnd(); - } - - // draw vertical lines along the boundary - glLineWidth(1.0f); - glBegin(GL_LINES); - glColor3f(c[0], c[1], c[2]); - for (auto&& v : boundary) - { - glVertex3f(v.x, -1.0f, v.y); - glVertex3f(v.x, 1.0f, v.y); - } - glEnd(); - } - std::string get_timestamped_file_name() { std::time_t now = std::time(NULL); diff --git a/common/model-views.h b/common/model-views.h index 52f0b9e372..311ea273c2 100644 --- a/common/model-views.h +++ b/common/model-views.h @@ -150,6 +150,17 @@ namespace rs2 // that most of the application is presented on int pick_scale_factor(GLFWwindow* window); + // avoid display the following options + bool static skip_option(rs2_option opt) + { + if (opt == RS2_OPTION_STREAM_FILTER || + opt == RS2_OPTION_STREAM_FORMAT_FILTER || + opt == RS2_OPTION_STREAM_INDEX_FILTER || + opt == RS2_OPTION_FRAMES_QUEUE_SIZE) + return true; + return false; + } + template void sort_together(std::vector& vec, std::vector& names) { @@ -274,6 +285,72 @@ namespace rs2 std::function _invoker; }; + class syncer_model + { + public: + syncer_model(): + _active(true){} + + std::shared_ptr create_syncer() + { + stop(); + std::lock_guard lock(_mutex); + auto shared_syncer = std::make_shared(); + rs2::frame_queue q; + + _syncers.push_back({shared_syncer,q}); + shared_syncer->start(q); + start(); + return shared_syncer; + } + + void remove_syncer(std::shared_ptr s) + { + stop(); + std::lock_guard lock(_mutex); + _syncers.erase(std::remove_if(_syncers.begin(), _syncers.end(), + [s](std::pair, rs2::frame_queue> pair) + { + return pair.first.get() == s.get(); + }), _syncers.end()) ; + start(); + } + + std::vector try_wait_for_frames() + { + std::lock_guard lock(_mutex); + + std::vector result; + for(auto&& s = _syncers.begin(); s!=_syncers.end() && _active; s++) + { + rs2::frameset f; + if(s->second.try_wait_for_frame(&f,1)) + { + result.push_back(f); + } + } + + return result; + } + + void stop() + { + _active.exchange(false); + } + + void start() + { + _active.exchange(true); + } + + private: + std::vector, rs2::frame_queue>> _syncers; + std::mutex _mutex; + std::atomic _active; + + }; + + class subdevice_model { public: @@ -290,7 +367,7 @@ namespace rs2 bool is_selected_combination_supported(); std::vector get_selected_profiles(); void stop(viewer_model& viewer); - void play(const std::vector& profiles, viewer_model& viewer); + void play(const std::vector& profiles, viewer_model& viewer, std::shared_ptr); void update(std::string& error_message, notifications_model& model); void draw_options(const std::vector& drawing_order, bool update_read_only_options, std::string& error_message, @@ -481,6 +558,8 @@ namespace rs2 void handle_hardware_events(const std::string& serialized_data); std::vector> subdevices; + std::shared_ptr syncer; + std::shared_ptr dev_syncer; bool is_streaming() const; bool metadata_supported = false; bool get_curr_advanced_controls = true; @@ -492,7 +571,6 @@ namespace rs2 bool _playback_repeat = true; bool _should_replay = false; bool show_device_info = false; - bool allow_remove = true; bool show_depth_only = false; bool show_stream_selection = true; @@ -642,22 +720,10 @@ namespace rs2 void update_texture(frame f) { pc->map_to(f); } /* Start the rendering thread in case its disabled */ - void start() - { - if (render_thread_active.exchange(true) == false) - { - render_thread = std::thread(&post_processing_filters::render_loop, this); - } - } + void start(); /* Stop the rendering thread in case its enabled */ - void stop() - { - if (render_thread_active.exchange(false) == true) - { - render_thread.join(); - } - } + void stop(); bool is_rendering() const { @@ -686,7 +752,6 @@ namespace rs2 const size_t resulting_queue_max_size; std::map frames_queue; - rs2::frame_queue syncer_queue; rs2::frame_queue resulting_queue; std::shared_ptr get_pc() const { return pc; } @@ -754,11 +819,9 @@ namespace rs2 void draw_controller_pose_object(); void draw_pose_object(); void draw_trajectory(tracked_point& p); - void draw_boundary(tracked_point& p); press_button_model trajectory_button{ u8"\uf1b0", u8"\uf1b0","Draw trajectory", "Stop drawing trajectory" }; press_button_model camera_object_button{ u8"\uf047", u8"\uf083", "Draw pose axis", "Draw camera pose" }; - press_button_model boundary_button{ u8"\uf278", u8"\uf278", "Set trajectory as boundary", "Discard boundary" }; private: void add_to_trajectory(tracked_point& p); @@ -825,7 +888,7 @@ namespace rs2 : ppf(*this), synchronization_enable(true) { - s.start(ppf.syncer_queue); + syncer = std::make_shared(); reset_camera(); rs2_error* e = nullptr; not_model.add_log(to_string() << "librealsense version: " << api_version_to_string(rs2_get_api_version(&e)) << "\n"); @@ -855,6 +918,8 @@ namespace rs2 void popup_if_error(ImFont* font, std::string& error_message); + void popup_if_ui_not_aligned(ImFont* font); + void show_event_log(ImFont* font_14, float x, float y, float w, float h); void show_3dviewer_header(ImFont* font, rs2::rect stream_rect, bool& paused, std::string& error_message); @@ -876,7 +941,7 @@ namespace rs2 std::map streams_origin; bool fullscreen = false; stream_model* selected_stream = nullptr; - + std::shared_ptr syncer; post_processing_filters ppf; tm2_model tm2; @@ -903,7 +968,8 @@ namespace rs2 float dim_level = 1.f; - rs2::asynchronous_syncer s; + + bool continue_with_ui_not_aligned = false; private: struct rgb { uint32_t r, g, b; diff --git a/common/rendering.h b/common/rendering.h index 1e1875dfe3..f0a45a207e 100644 --- a/common/rendering.h +++ b/common/rendering.h @@ -1162,29 +1162,29 @@ namespace rs2 last_queue[1].enqueue(rendered_frame); } - static void draw_axis(float axis_size = 1.f, float axisWidth = 4.f) + static void draw_axes(float axis_size = 1.f, float axisWidth = 4.f) { // Triangles For X axis glBegin(GL_TRIANGLES); glColor3f(1.0f, 0.0f, 0.0f); - glVertex3f(axis_size * 1.1f, 0.0f, 0.0f); - glVertex3f(axis_size, axis_size * 0.05f, 0.0f); - glVertex3f(axis_size, -0.05f * axis_size, 0.0f); + glVertex3f(axis_size * 1.1f, 0.f, 0.f); + glVertex3f(axis_size, -axis_size * 0.05f, 0.f); + glVertex3f(axis_size, axis_size * 0.05f, 0.f); + glVertex3f(axis_size * 1.1f, 0.f, 0.f); + glVertex3f(axis_size, 0.f, -axis_size * 0.05f); + glVertex3f(axis_size, 0.f, axis_size * 0.05f); glEnd(); // Triangles For Y axis glBegin(GL_TRIANGLES); - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(0.0f, -1.1f * axis_size, 0.0f); - glVertex3f(0.0f, -1.0f * axis_size, 0.05f * axis_size); - glVertex3f(0.0f, -1.0f * axis_size, -0.05f * axis_size); - glEnd(); - glBegin(GL_TRIANGLES); - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(0.0f, -1.1f * axis_size, 0.0f); - glVertex3f(0.05f * axis_size, -1.0f * axis_size, 0.0f); - glVertex3f(-0.05f * axis_size, -1.0f * axis_size, 0.0f); + glColor3f(0.f, 1.f, 0.f); + glVertex3f(0.f, axis_size * 1.1f, 0.0f); + glVertex3f(0.f, axis_size, 0.05f * axis_size); + glVertex3f(0.f, axis_size, -0.05f * axis_size); + glVertex3f(0.f, axis_size * 1.1f, 0.0f); + glVertex3f( 0.05f * axis_size, axis_size, 0.f); + glVertex3f(-0.05f * axis_size, axis_size, 0.f); glEnd(); // Triangles For Z axis @@ -1193,6 +1193,9 @@ namespace rs2 glVertex3f(0.0f, 0.0f, 1.1f * axis_size); glVertex3f(0.0f, 0.05f * axis_size, 1.0f * axis_size); glVertex3f(0.0f, -0.05f * axis_size, 1.0f * axis_size); + glVertex3f(0.0f, 0.0f, 1.1f * axis_size); + glVertex3f(0.05f * axis_size, 0.f, 1.0f * axis_size); + glVertex3f(-0.05f * axis_size, 0.f, 1.0f * axis_size); glEnd(); glLineWidth(axisWidth); @@ -1207,7 +1210,7 @@ namespace rs2 // Y axis - Green glColor3f(0.0f, 1.0f, 0.0f); glVertex3f(0.0f, 0.0f, 0.0f); - glVertex3f(0.0f, -axis_size, 0.0f); + glVertex3f(0.0f, axis_size, 0.0f); // Z axis - Blue glColor3f(0.0f, 0.0f, 1.0f); @@ -1282,7 +1285,7 @@ namespace rs2 glMatrixMode(GL_MODELVIEW); glPushMatrix(); - glViewport(0, 0, 1024, 1024); + glViewport(0, 0, 768, 768); glClearColor(0, 0, 0, 1); glClear(GL_COLOR_BUFFER_BIT); @@ -1291,15 +1294,16 @@ namespace rs2 glOrtho(-2.8, 2.8, -2.4, 2.4, -7, 7); - glRotatef(-25, 1.0f, 0.0f, 0.0f); + glRotatef(25, 1.0f, 0.0f, 0.0f); - glTranslatef(0, 0.33f, -1.f); + glTranslatef(0, -0.33f, -1.f); float norm = std::sqrt(x*x + y*y + z*z); - glRotatef(-45, 0.0f, 1.0f, 0.0f); + glRotatef(-135, 0.0f, 1.0f, 0.0f); + + draw_axes(); - draw_axis(); draw_circle(1, 0, 0, 0, 1, 0); draw_circle(0, 1, 0, 0, 0, 1); draw_circle(1, 0, 0, 0, 0, 1); @@ -1331,7 +1335,7 @@ namespace rs2 glBegin(GL_LINES); glColor3f(1.0f, 1.0f, 1.0f); glVertex3f(0.0f, 0.0f, 0.0f); - glVertex3f(-x / norm, -y / norm, -z / norm); + glVertex3f(x / norm, y / norm, z / norm); glEnd(); // Save model and projection matrix for later @@ -1347,14 +1351,14 @@ namespace rs2 const auto precision = 3; s1 << "(" << std::fixed << std::setprecision(precision) << x << "," << std::fixed << std::setprecision(precision) << y << "," << std::fixed << std::setprecision(precision) << z << ")"; - print_text_in_3d(-x, -y, -z, s1.str().c_str(), false, model, proj, 1 / norm); + print_text_in_3d(x, y, z, s1.str().c_str(), false, model, proj, 1 / norm); std::ostringstream s2; s2 << std::setprecision(precision) << norm; - print_text_in_3d(-x / 2, -y / 2, -z / 2, s2.str().c_str(), true, model, proj, 1 / norm); + print_text_in_3d(x / 2, y / 2, z / 2, s2.str().c_str(), true, model, proj, 1 / norm); } - glCopyTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, 0, 0, 1024, 1024, 0); + glCopyTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 0, 0, 768, 768, 0); glMatrixMode(GL_MODELVIEW); glPopMatrix(); @@ -1399,7 +1403,7 @@ namespace rs2 glLoadIdentity(); draw_grid(); - draw_axis(0.3f, 2.f); + draw_axes(0.3f, 2.f); // Drawing pose: matrix4 pose_trans = tm2_pose_to_world_transformation(pose); @@ -1411,7 +1415,7 @@ namespace rs2 glPushMatrix(); glLoadMatrixf(model); - draw_axis(0.3f, 2.f); + draw_axes(0.3f, 2.f); // remove model matrix from the rest of the render glPopMatrix(); @@ -1610,6 +1614,23 @@ namespace rs2 return false; } + inline bool is_rasterizeable(rs2_format format) + { + // Check whether the produced + switch (format) + { + case RS2_FORMAT_ANY: + case RS2_FORMAT_XYZ32F: + case RS2_FORMAT_MOTION_RAW: + case RS2_FORMAT_MOTION_XYZ32F: + case RS2_FORMAT_GPIO_RAW: + case RS2_FORMAT_6DOF: + return false; + default: + return true; + } + } + // RS4xx with RealTec RGB sensor may additionally require sensor orientation control to make runtime adjustments inline void rotate_rgb_image(device& dev,uint32_t res_width) { diff --git a/common/ux-alignment.cpp b/common/ux-alignment.cpp new file mode 100644 index 0000000000..ce57dcca31 --- /dev/null +++ b/common/ux-alignment.cpp @@ -0,0 +1,114 @@ +#include "ux-alignment.h" +#include +#include +#include +#include + +#ifdef _WIN32 +#undef APIENTRY +#define GLFW_EXPOSE_NATIVE_WIN32 +#define GLFW_EXPOSE_NATIVE_WGL +#include + +#include +#endif + +#define MAGIC 250 + +bool is_gui_aligned(GLFWwindow *win) +{ +#ifdef _WIN32 + try + { + auto hwn = glfwGetWin32Window(win); + if (hwn == nullptr) + return true; + + auto flags = ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | + ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_NoTitleBar | + ImGuiWindowFlags_NoSavedSettings | ImGuiWindowFlags_NoBringToFrontOnFocus; + + ImGui::SetNextWindowPos({ 0, 0 }); + + ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(0, 0)); + ImGui::Begin("is_gui_aligned", nullptr, flags); + + auto DrawList = ImGui::GetWindowDrawList(); + if (DrawList == nullptr) + return true; + + DrawList->AddRectFilled({ 0,0 }, { 1,1 }, ImColor(MAGIC / 255.f, 0.f, 0.f, 1.f)); + + ImGui::End(); + ImGui::PopStyleVar(); + + ImGui::Render(); + + glfwSwapBuffers(win); + + ImGui_ImplGlfw_NewFrame(1.f); + + SetFocus(hwn); + + int width = 1; + int height = 1; + + HDC hdc = GetDC(hwn); + if (hdc == nullptr) + return true; + + std::shared_ptr shared_hdc(&hdc, [&](HDC* hdc) {ReleaseDC(hwn, *hdc); DeleteDC(*hdc);}); + + HDC hCaptureDC = CreateCompatibleDC(hdc); + if (hCaptureDC == nullptr) + return true; + + std::shared_ptr shared_capture_hdc(&hCaptureDC, [&](HDC* hdc) {ReleaseDC(hwn, *hdc); DeleteDC(*hdc);}); + + HBITMAP hCaptureBitmap = CreateCompatibleBitmap(hdc, width, height); + if (hCaptureBitmap == nullptr) + return true; + + std::shared_ptr shared_bmp(&hCaptureBitmap, [&](HBITMAP* bmp) {DeleteObject(bmp);}); + + auto original = SelectObject(hCaptureDC, hCaptureBitmap); + if (original == nullptr) + return true; + + if (!BitBlt(hCaptureDC, 0, 0, width, height, hdc, 0, 0, SRCCOPY | CAPTUREBLT)) + return true; + + BITMAPINFO bmi = { 0 }; + bmi.bmiHeader.biSize = sizeof(bmi.bmiHeader); + bmi.bmiHeader.biWidth = width; + bmi.bmiHeader.biHeight = height; + bmi.bmiHeader.biPlanes = 1; + bmi.bmiHeader.biBitCount = 32; + bmi.bmiHeader.biCompression = BI_RGB; + + std::vector pPixels(width * height); + + auto res = GetDIBits( + hCaptureDC, + hCaptureBitmap, + 0, + height, + pPixels.data(), + &bmi, + DIB_RGB_COLORS + ); + + if (res <= 0 || res == ERROR_INVALID_PARAMETER) + return true; + + auto ret = pPixels[0].rgbRed == MAGIC; + return ret; + } + catch (...) + { + return true; + } +#else + return true; +#endif +} \ No newline at end of file diff --git a/common/ux-alignment.h b/common/ux-alignment.h new file mode 100644 index 0000000000..d05f72dc29 --- /dev/null +++ b/common/ux-alignment.h @@ -0,0 +1,16 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once + +#define GLFW_INCLUDE_GLU +#include + +//This function checks whether there is UI misalignment, +//sometimes caused by outdated graphics card drivers, +//it puts a red pixel (1x1 rect) on the top left corner of the window, +//take a snapshot and check the value of the top left corner pixel +//if its red - there is no misalignment +//otherwise - there is misalignment + +bool is_gui_aligned(GLFWwindow *win); diff --git a/common/ux-window.cpp b/common/ux-window.cpp index 99a70d3194..d2fff2ccfa 100644 --- a/common/ux-window.cpp +++ b/common/ux-window.cpp @@ -8,6 +8,8 @@ // int-rs-splash.hpp contains the PNG image from res/int-rs-splash.png #include "res/int-rs-splash.hpp" +#include "ux-alignment.h" + namespace rs2 { void ux_window::open_window() @@ -144,7 +146,7 @@ namespace rs2 } } }); - _first_frame = false; + } // If we are just getting started, render the Splash Screen instead of normal UI @@ -155,6 +157,11 @@ namespace rs2 begin_frame(); + if (_first_frame) + { + _is_ui_aligned = is_gui_aligned(_win); + _first_frame = false; + } glPushMatrix(); glViewport(0, 0, _fb_width, _fb_height); glClearColor(0.036f, 0.044f, 0.051f, 1.f); diff --git a/common/ux-window.h b/common/ux-window.h index 5aa9adab78..70ff1e45b6 100644 --- a/common/ux-window.h +++ b/common/ux-window.h @@ -62,6 +62,8 @@ namespace rs2 void add_on_load_message(const std::string& msg); + bool is_ui_aligned() { return _is_ui_aligned; } + private: void open_window(); @@ -92,5 +94,7 @@ namespace rs2 bool _fullscreen_pressed = false; bool _fullscreen = false; std::string _title; + + bool _is_ui_aligned = false; }; } diff --git a/config/99-realsense-libusb.rules b/config/99-realsense-libusb.rules index ac9c30dc66..f767ca528f 100644 --- a/config/99-realsense-libusb.rules +++ b/config/99-realsense-libusb.rules @@ -12,6 +12,7 @@ SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad3", MODE:="066 SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad4", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad5", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad6", MODE:="0666", GROUP:="plugdev" +SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0af2", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0af6", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0afe", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0aff", MODE:="0666", GROUP:="plugdev" @@ -21,10 +22,13 @@ SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b03", MODE:="066 SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b07", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b0c", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b0d", MODE:="0666", GROUP:="plugdev" +SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ENV{DEVTYPE}=="usb_device", ATTRS{idVendor}=="8087", ATTRS{idProduct}=="0af3", MODE="0666", GROUP="plugdev" SUBSYSTEMS=="usb", ENV{DEVTYPE}=="usb_device", ATTRS{idVendor}=="03e7", ATTRS{idProduct}=="2150", MODE="0666", GROUP="plugdev" KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad5", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'" DRIVER=="hid_sensor_custom", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad5", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'" +KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0af2", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'" +DRIVER=="hid_sensor_custom", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0af2", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'" KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0afe", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'" DRIVER=="hid_sensor_custom", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0afe", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'" KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0aff", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'" @@ -33,3 +37,5 @@ KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b00", MODE:="0777", DRIVER=="hid_sensor_custom", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b00", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'" KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b01", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'" DRIVER=="hid_sensor_custom", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b01", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'" +KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'" +DRIVER=="hid_sensor_custom", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'" diff --git a/doc/android/AndroidJavaApp.md b/doc/android/AndroidJavaApp.md index ea75c9bdcb..689cba619b 100644 --- a/doc/android/AndroidJavaApp.md +++ b/doc/android/AndroidJavaApp.md @@ -38,13 +38,26 @@ Click on `configure` and choose `SDK Manager`. 12. Replace the content of `MainActivity`, `native-lib.cpp`, `activity_main.xml` and `CMakeLists.txt` with [MainActivity](MainActivity.java_), [native-lib.cpp](./native-lib.cpp_), [activity_main.xml](./activity_main.xml_) and [CMakeLists.txt](./CMakeLists.txt_) respectively.

-13. Connect the Android device to the host machine. +13. In the `Gradle Scripts` section, go to the module-level `build.gradle` file. In the `defaultConfig` block, add an `ndk` block and configure the `abiFilters` option with `armeabi-v7a`: +``` +android { + ... + defaultConfig { + ... + ndk { + abiFilters 'armeabi-v7a' + } + } +} +``` + +14. Connect the Android device to the host machine. Click on `Run` and choose `Run 'app'`. Choose your Android device and click on the `OK` button. At the end of this process a new application supposed to appear at the Android device.

-14. Install [Terminal Emulator](https://en.wikipedia.org/wiki/Terminal_emulator) on your Android device from Google Play Store. -15. Use the USB OTG cable to connect the RealSense camera to your Android device. -16. Open the Terminal Emulator application and type below lines in order to move to Super User mode and change the USB permissions. +15. Install [Terminal Emulator](https://en.wikipedia.org/wiki/Terminal_emulator) on your Android device from Google Play Store. +16. Use the USB OTG cable to connect the RealSense camera to your Android device. +17. Open the Terminal Emulator application and type below lines in order to move to Super User mode and change the USB permissions. ```shell su setenforce 0 @@ -53,7 +66,7 @@ chmod 0777 /dev/bus/usb// ```

-17. Open the `realsense_app` application. +18. Open the `realsense_app` application. ## Expected Output * Streaming Depth data using rooted Samsung Galaxy S4 device. diff --git a/include/librealsense2/hpp/rs_sensor.hpp b/include/librealsense2/hpp/rs_sensor.hpp index b3995dda9d..d56bd7389d 100644 --- a/include/librealsense2/hpp/rs_sensor.hpp +++ b/include/librealsense2/hpp/rs_sensor.hpp @@ -352,7 +352,7 @@ namespace rs2 */ std::vector get_stream_profiles() const { - std::vector results; + std::vector results{}; rs2_error* e = nullptr; std::shared_ptr list( diff --git a/include/librealsense2/rs.h b/include/librealsense2/rs.h index 4db97907ee..fb9b052c3e 100644 --- a/include/librealsense2/rs.h +++ b/include/librealsense2/rs.h @@ -24,7 +24,7 @@ extern "C" { #define RS2_API_MAJOR_VERSION 2 #define RS2_API_MINOR_VERSION 16 -#define RS2_API_PATCH_VERSION 1 +#define RS2_API_PATCH_VERSION 2 #define RS2_API_BUILD_VERSION 0 #define STRINGIFY(arg) #arg diff --git a/package.xml b/package.xml index e86bf0fe23..72fd1fd393 100644 --- a/package.xml +++ b/package.xml @@ -7,7 +7,7 @@ librealsense2 - 2.16.1 + 2.16.2 Library for capturing data from the Intel(R) RealSense(TM) SR300 and D400 cameras. This effort was initiated to better support researchers, creative coders, and app developers in domains such as robotics, virtual reality, and the internet of things. Several often-requested features of RealSense(TM); devices are implemented in this project, including multi-camera capture. diff --git a/scripts/patch-opensuse-leap15.sh b/scripts/patch-opensuse-leap15.sh new file mode 100755 index 0000000000..13ba5e9bdc --- /dev/null +++ b/scripts/patch-opensuse-leap15.sh @@ -0,0 +1,120 @@ +#!/bin/bash -e + +SRC_VERSION_NAME=linux + +## from +## http://stackoverflow.com/questions/9293887/in-bash-how-do-i-convert-a-space-delimited-string-into-an-array + +FULL_NAME=linux-$( uname -r | sed -e 's/-default//') +read -a VERSION <<< $FULL_NAME + +SRC_VERSION_ID=${VERSION[0]} ## e.g. : 4.5.6 +SRC_VERSION_REL=${VERSION[1]} ## e.g. : 1 +LINUX_TYPE=${VERSION[2]} ## e.g. : ARCH +#remove trailing.0 +SRC_VERSION_ID=$(echo $SRC_VERSION_ID | sed -e 's/\.0$//' ) + +LINUX_BRANCH=opensuse-$SRC_VERSION_ID +KERNEL_NAME=linux-$SRC_VERSION_ID +PATCH_NAME=patch-$SRC_VERSION_ID + +# ARCH Linux -- KERNEL_NAME=linux-$SRC_VERSION_ID-$SRC_VERSION_REL-$ARCH.pkg.tar.xz + + +mkdir kernel +cd kernel + +sudo zypper in kernel-source kernel-default-devel + +mkdir $FULL_NAME +cp -r /usr/src/$FULL_NAME-obj/x86_64/default/* ./$FULL_NAME/ +cp -r /usr/src/$FULL_NAME/* ./$FULL_NAME/ + + +cd $FULL_NAME + + +## Get the patch + +# echo "Patching the kernel..." +### patch not working ? +# xz -dc ../$PATCH_NAME.xz | patch -p1 + + +echo "RealSense patch..." + +# Apply our RealSense specific patch +patch -p1 < ../../realsense-camera-formats-opensuse.patch + +# Prepare to compile modules + +## Get the config +# zcat /proc/config.gz > .config ## Not the good one ? + +cp /lib/modules/`uname -r`/build/.config . +cp /lib/modules/`uname -r`/build/Module.symvers . + +echo "Prepare the build" + +#cat Makefile | sed -e 's/\.\.\/\.\.\/\.\.\/linux-4.12.14-lp150.12.16/\.\.\/linux-4.12.14-lp150.12.16/' > Makefile.temp; mv Makefile.temp Makefile; +make -C . scripts oldconfig modules_prepare + +# Compile UVC modules +echo "Beginning compilation of uvc..." +#make modules +KBASE=`pwd` +cd drivers/media/usb/uvc +cp $KBASE/Module.symvers . +make -C $KBASE M=$KBASE/drivers/media/usb/uvc/ modules + +# Copy to sane location +#sudo cp $KBASE/drivers/media/usb/uvc/uvcvideo.ko ~/$LINUX_BRANCH-uvcvideo.ko +cd ../../../../../ + +cp $KBASE/drivers/media/usb/uvc/uvcvideo.ko ../uvcvideo.ko + +# Unload existing module if installed +echo "Unloading existing uvcvideo driver..." +sudo modprobe -r uvcvideo + +cd .. + +## Not sure yet about deleting and copying... + +# save the existing module + +MODULE_NAME=/lib/modules/`uname -r`/kernel/drivers/media/usb/uvc/uvcvideo.ko + +if [ -e $MODULE_NAME ]; then + sudo cp $MODULE_NAME $MODULE_NAME.backup + sudo rm $MODULE_NAME + + sudo cp uvcvideo.ko $MODULE_NAME +fi + +if [ -e $MODULE_NAME.xz ]; then + sudo cp $MODULE_NAME.xz $MODULE_NAME.xz.backup + sudo rm $MODULE_NAME.xz + + # compress + xz uvcvideo.ko + sudo cp uvcvideo.ko.xz $MODULE_NAME +fi + +if [ -e $MODULE_NAME.gz ]; then + sudo cp $MODULE_NAME.gz $MODULE_NAME.gz.backup + sudo rm $MODULE_NAME.gz + + # compress + gzip uvcvideo.ko + sudo cp uvcvideo.ko.gz $MODULE_NAME.gz +fi + +# Copy out to module directory + + +sudo modprobe uvcvideo + +rm -rf kernel + +echo "Script has completed. Please consult the installation guide for further instruction." diff --git a/scripts/patch-opensusetumbleweed.sh b/scripts/patch-opensusetumbleweed.sh new file mode 100755 index 0000000000..994dec8f62 --- /dev/null +++ b/scripts/patch-opensusetumbleweed.sh @@ -0,0 +1,118 @@ +#!/bin/bash -e + +SRC_VERSION_NAME=linux + +## from +## http://stackoverflow.com/questions/9293887/in-bash-how-do-i-convert-a-space-delimited-string-into-an-array + +FULL_NAME=$( uname -r | tr "-" "\n") +read -a VERSION <<< $FULL_NAME + +SRC_VERSION_ID=${VERSION[0]} ## e.g. : 4.5.6 +SRC_VERSION_REL=${VERSION[1]} ## e.g. : 1 +LINUX_TYPE=${VERSION[2]} ## e.g. : ARCH +#remove trailing.0 +SRC_VERSION_ID=$(echo $SRC_VERSION_ID | sed -e 's/\.0$//' ) + +LINUX_BRANCH=opensuse-$SRC_VERSION_ID +KERNEL_NAME=linux-$SRC_VERSION_ID +PATCH_NAME=patch-$SRC_VERSION_ID + +# ARCH Linux -- KERNEL_NAME=linux-$SRC_VERSION_ID-$SRC_VERSION_REL-$ARCH.pkg.tar.xz + +mkdir kernel +cd kernel + +## Get the kernel +wget https://www.kernel.org/pub/linux/kernel/v4.x/$KERNEL_NAME.tar.xz +wget https://www.kernel.org/pub/linux/kernel/v4.x/$PATCH_NAME.xz +#wget https://www.kernel.org/pub/linux/kernel/v4.x/$PATCH_NAME.sign + +echo "Extract the kernel" +tar xf $KERNEL_NAME.tar.xz + +cd $KERNEL_NAME + +## Get the patch + +# echo "Patching the kernel..." +### patch not working ? +# xz -dc ../$PATCH_NAME.xz | patch -p1 + + +echo "RealSense patch..." + +# Apply our RealSense specific patch +patch -p1 < ../../realsense-camera-formats-opensuse.patch + +# Prepare to compile modules + +## Get the config +# zcat /proc/config.gz > .config ## Not the good one ? + +cp /lib/modules/`uname -r`/build/.config . +cp /lib/modules/`uname -r`/build/Module.symvers . + +echo "Prepare the build" + +make scripts oldconfig modules_prepare + +# Compile UVC modules +echo "Beginning compilation of uvc..." +#make modules +KBASE=`pwd` +cd drivers/media/usb/uvc +cp $KBASE/Module.symvers . +make -C $KBASE M=$KBASE/drivers/media/usb/uvc/ modules + +# Copy to sane location +#sudo cp $KBASE/drivers/media/usb/uvc/uvcvideo.ko ~/$LINUX_BRANCH-uvcvideo.ko +cd ../../../../../ + +cp $KBASE/drivers/media/usb/uvc/uvcvideo.ko ../uvcvideo.ko + +# Unload existing module if installed +echo "Unloading existing uvcvideo driver..." +sudo modprobe -r uvcvideo + +cd .. + +## Not sure yet about deleting and copying... + +# save the existing module + +MODULE_NAME=/lib/modules/`uname -r`/kernel/drivers/media/usb/uvc/uvcvideo.ko + +if [ -e $MODULE_NAME ]; then + sudo cp $MODULE_NAME $MODULE_NAME.backup + sudo rm $MODULE_NAME + + sudo cp uvcvideo.ko $MODULE_NAME +fi + +if [ -e $MODULE_NAME.xz ]; then + sudo cp $MODULE_NAME.xz $MODULE_NAME.xz.backup + sudo rm $MODULE_NAME.xz + + # compress + xz uvcvideo.ko + sudo cp uvcvideo.ko.xz $MODULE_NAME +fi + +if [ -e $MODULE_NAME.gz ]; then + sudo cp $MODULE_NAME.gz $MODULE_NAME.gz.backup + sudo rm $MODULE_NAME.gz + + # compress + gzip uvcvideo.ko + sudo cp uvcvideo.ko.gz $MODULE_NAME.gz +fi + +# Copy out to module directory + + +sudo modprobe uvcvideo + +rm -rf kernel + +echo "Script has completed. Please consult the installation guide for further instruction." diff --git a/scripts/patch-realsense-ubuntu-odroid-xu4-4.14.sh b/scripts/patch-realsense-ubuntu-odroid-xu4-4.14.sh new file mode 100755 index 0000000000..2d004bdcc9 --- /dev/null +++ b/scripts/patch-realsense-ubuntu-odroid-xu4-4.14.sh @@ -0,0 +1,192 @@ +#!/bin/bash + +#This script is almost an exact copy of the patch-realsense-ubuntu-lts.sh script, tested on 4.14.47-132. +#There only two small additions / checks. Actually, this script also works for normal ubuntu-lts + +#Break execution on any error received +set -e + +#Locally suppress stderr to avoid raising not relevant messages +exec 3>&2 +exec 2> /dev/null +con_dev=$(ls /dev/video* | wc -l) +exec 2>&3 + +if [ $con_dev -ne 0 ]; +then + echo -e "\e[32m" + read -p "Remove all RealSense cameras attached. Hit any key when ready" + echo -e "\e[0m" +fi + +#Include usability functions +source ./scripts/patch-utils.sh + +# Get the required tools and headers to build the kernel +#sudo apt-get install linux-headers-generic build-essential git +sudo apt-get install build-essential git + +#Packages to build the patched modules +require_package libusb-1.0-0-dev +require_package libssl-dev + +retpoline_retrofit=0 + +LINUX_BRANCH=$(uname -r) +PLATFORM=$(uname -n) +#if [ "$PLATFORM" = "odroid" ]; then +# #kernel_branch="hwe" +# #kernel_branch=$LINUX_BRANCH +# kernel_branch="master" +#else +# kernel_branch=$(choose_kernel_branch $LINUX_BRANCH) +#fi +kernel_branch="master" +echo "Kernel branch: " $kernel_branch +# Construct branch name from distribution codename {xenial,bionic,..} and kernel version +#ubuntu_codename=`. /etc/os-release; echo ${UBUNTU_CODENAME/*, /}` +#if [ -z "$UBUNTU_CODENAME" ]; +#then + # Trusty Tahr shall use xenial code base +# ubuntu_codename="xenial" +# retpoline_retrofit=1 +#fi +ubuntu_codename="bionic" + +#kernel_name="ubuntu-${ubuntu_codename}-$kernel_branch" +kernel_name="odroid_bionic" + +#Distribution-specific packages +if [ ${ubuntu_codename} == "bionic" ]; +then + require_package libelf-dev + require_package elfutils +fi + + +# Get the linux kernel and change into source tree +#[ ! -d ${kernel_name} ] && git clone -b $kernel_branch git://#kernel.ubuntu.com/ubuntu/ubuntu-${ubuntu_codename}.git --depth #1 ./${kernel_name} +[ ! -d ${kernel_name} ] && git clone --depth 1 https://github.com/hardkernel/linux -b odroidxu4-4.14.y ./${kernel_name} +cd ${kernel_name} + +# Verify that there are no trailing changes., warn the user to make corrective action if needed +if [ $(git status | grep 'modified:' | wc -l) -ne 0 ]; +then + echo -e "\e[36mThe kernel has modified files:\e[0m" + git status | grep 'modified:' + echo -e "\e[36mProceeding will reset all local kernel changes. Press 'n' within 10 seconds to abort the operation" + set +e + read -n 1 -t 10 -r -p "Do you want to proceed? [Y/n]" response + set -e + response=${response,,} # tolower + if [[ $response =~ ^(n|N)$ ]]; + then + echo -e "\e[41mScript has been aborted on user requiest. Please resolve the modified files are rerun\e[0m" + exit 1 + else + echo -e "\e[0m" + echo -e "\e[32mUpdate the folder content with the latest from mainline branch\e[0m" + #git fetch origin $kernel_branch --depth 1 + git fetch origin odroidxu4-4.14.y --depth 1 + printf "Resetting local changes in %s folder\n " ${kernel_name} + #git reset --hard $kernel_branch + git reset --hard odroidxu4-4.14.y + fi +fi + +#Check if we need to apply patches or get reload stock drivers (Developers' option) +[ "$#" -ne 0 -a "$1" == "reset" ] && reset_driver=1 || reset_driver=0 + +if [ $reset_driver -eq 1 ]; +then + echo -e "\e[43mUser requested to rebuild and reinstall ubuntu-${ubuntu_codename} stock drivers\e[0m" +else + # Patching kernel for RealSense devices + echo -e "\e[32mApplying realsense-uvc patch\e[0m" + patch -p1 < ../scripts/realsense-camera-formats_ubuntu-${ubuntu_codename}-${kernel_branch}.patch + echo -e "\e[32mApplying realsense-metadata patch\e[0m" + patch -p1 < ../scripts/realsense-metadata-ubuntu-${ubuntu_codename}-${kernel_branch}.patch + echo -e "\e[32mApplying realsense-hid patch\e[0m" + patch -p1 < ../scripts/realsense-hid-ubuntu-${ubuntu_codename}-${kernel_branch}.patch + echo -e "\e[32mApplying realsense-powerlinefrequency-fix patch\e[0m" + patch -p1 < ../scripts/realsense-powerlinefrequency-control-fix.patch +fi + +# Copy configuration +sudo cp /lib/modules/$(uname -r)/build/.config . +sudo cp /lib/modules/$(uname -r)/build/Module.symvers . + +# Basic build for kernel modules +echo -e "\e[32mPrepare kernel modules configuration\e[0m" +#Retpoline script manual retrieval. based on https://github.com/IntelRealSense/librealsense/issues/1493 +#Required since the retpoline patches were introduced into Ubuntu kernels +if [ ! -f scripts/ubuntu-retpoline-extract-one ]; then + pwd + for f in $(find . -name 'retpoline-extract-one'); do cp ${f} scripts/ubuntu-retpoline-extract-one; done; + echo $$$ +fi +#sudo make silentoldconfig modules_prepare +sudo make odroidxu4_defconfig modules_prepare + +#Vermagic identity is required +IFS='.' read -a kernel_version <<< "$LINUX_BRANCH" +sudo sed -i "s/\".*\"/\"$LINUX_BRANCH\"/g" ./include/generated/utsrelease.h +sudo sed -i "s/.*/$LINUX_BRANCH/g" ./include/config/kernel.release +#Patch for Trusty Tahr (Ubuntu 14.05) with GCC not retrofitted with the retpoline patch. +[ $retpoline_retrofit -eq 1 ] && sudo sed -i "s/#ifdef RETPOLINE/#if (1)/g" ./include/linux/vermagic.h + +# Build the uvc, accel and gyro modules +KBASE=`pwd` +cd drivers/media/usb/uvc +sudo cp $KBASE/Module.symvers . + +echo -e "\e[32mCompiling uvc module\e[0m" +sudo make -j -C $KBASE M=$KBASE/drivers/media/usb/uvc/ modules +echo -e "\e[32mCompiling accelerometer and gyro modules\e[0m" +sudo make -j -C $KBASE M=$KBASE/drivers/iio/accel modules +sudo make -j -C $KBASE M=$KBASE/drivers/iio/gyro modules +echo -e "\e[32mCompiling v4l2-core modules\e[0m" +sudo make -j -C $KBASE M=$KBASE/drivers/media/v4l2-core modules + +# Copy and load the patched modules to a sane location +if [ -f $KBASE/drivers/media/usb/uvc/uvcvideo.ko ]; then + echo "Copying uvcvideo.ko" + sudo cp $KBASE/drivers/media/usb/uvc/uvcvideo.ko ~/$LINUX_BRANCH-uvcvideo.ko + try_module_insert uvcvideo ~/$LINUX_BRANCH-uvcvideo.ko /lib/modules/`uname -r`/kernel/drivers/media/usb/uvc/uvcvideo.ko +fi + +#if [ -f $KBASE/drivers/iio/accel/hid-sensor-accel-3d.ko ]; then +# echo "Copying hid-sensor-accel-3d.ko" +# sudo cp $KBASE/drivers/iio/accel/hid-sensor-accel-3d.ko ~/$LINUX_BRANCH-hid-sensor-accel-3d.ko +# try_module_insert hid_sensor_accel_3d ~/$LINUX_BRANCH-hid-sensor-accel-3d.ko /lib/modules/#`uname -r`/kernel/drivers/iio/accel/hid-sensor-accel-3d.ko +#fi +#if [ -f $KBASE/drivers/iio/gyro/hid-sensor-gyro-3d.ko ]; then +# echo "Copying hid-sensor-gyro-3d.ko" +# sudo cp $KBASE/drivers/iio/gyro/hid-sensor-gyro-3d.ko ~/$LINUX_BRANCH-hid-sensor-gyro-3d.ko +# try_module_insert hid_sensor_gyro_3d ~/$LINUX_BRANCH-hid-sensor-gyro-3d.ko /lib/modules/`uname -#r`/kernel/drivers/iio/gyro/hid-sensor-gyro-3d.ko +#fi +#if [ -f $KBASE/drivers/media/v4l2-core/videodev.ko ]; then +if [ -f $KBASE/drivers/media/v4l2-core/videobuf-core.ko ]; then + echo "Copying video drivers" + sudo cp $KBASE/drivers/media/v4l2-core/videobuf-core.ko ~/$LINUX_BRANCH-videobuf-core.ko + try_module_insert videobuf-core ~/$LINUX_BRANCH-videobuf-core.ko /lib/modules/`uname -r`/kernel/drivers/media/v4l2-core/videobuf-core.ko + + sudo cp $KBASE/drivers/media/v4l2-core/videobuf-vmalloc.ko ~/$LINUX_BRANCH-videobuf-vmalloc.ko + try_module_insert videobuf-vmalloc ~/$LINUX_BRANCH-videobuf-vmalloc.ko /lib/modules/`uname -r`/kernel/drivers/media/v4l2-core/videobuf-vmalloc.ko + + sudo cp $KBASE/drivers/media/v4l2-core/videobuf-dvb.ko ~/$LINUX_BRANCH-videobuf-dvb.ko + try_module_insert videobuf-dvb ~/$LINUX_BRANCH-videobuf-dvb.ko /lib/modules/`uname -r`/kernel/drivers/media/v4l2-core/videobuf-dvb.ko + + sudo cp $KBASE/drivers/media/v4l2-core/videobuf2-vmalloc.ko ~/$LINUX_BRANCH-videobuf2-vmalloc.ko + try_module_insert videobuf2-vmalloc ~/$LINUX_BRANCH-videobuf2-vmalloc.ko /lib/modules/`uname -r`/kernel/drivers/media/v4l2-core/videobuf2-vmalloc.ko + + sudo cp $KBASE/drivers/media/v4l2-core/v4l2-fwnode.ko ~/$LINUX_BRANCH-v4l2-fwnode.ko + try_module_insert v4l2-fwnode ~/$LINUX_BRANCH-v4l2-fwnode.ko /lib/modules/`uname -r`/kernel/drivers/media/v4l2-core/v4l2-fwnode.ko + + sudo cp $KBASE/drivers/media/v4l2-core/tuner.ko ~/$LINUX_BRANCH-tuner.ko + try_module_insert tuner ~/$LINUX_BRANCH-tuner.ko /lib/modules/`uname -r`/kernel/drivers/media/v4l2-core/tuner.ko +fi +echo -e "\e[32mPatched kernels modules were created successfully\n\e[0m" + + +echo -e "\e[92m\n\e[1mScript has completed. Please consult the installation guide for further instruction.\n\e[0m" diff --git a/scripts/realsense-camera-formats-opensuse.patch b/scripts/realsense-camera-formats-opensuse.patch new file mode 100644 index 0000000000..a6c685c25c --- /dev/null +++ b/scripts/realsense-camera-formats-opensuse.patch @@ -0,0 +1,173 @@ +diff --git a/drivers/media/usb/uvc/Makefile b/drivers/media/usb/uvc/Makefile +index c26d12f..d86cf22 100644 +--- a/drivers/media/usb/uvc/Makefile ++++ b/drivers/media/usb/uvc/Makefile +@@ -2,3 +2,4 @@ ++CONFIG_MODULE_SIG=n + uvcvideo-objs := uvc_driver.o uvc_queue.o uvc_v4l2.o uvc_video.o uvc_ctrl.o \ + uvc_status.o uvc_isight.o uvc_debugfs.o + ifeq ($(CONFIG_MEDIA_CONTROLLER),y) +diff --git a/drivers/media/usb/uvc/uvc_driver.c b/drivers/media/usb/uvc/uvc_driver.c +index d11fd6a..c819a39 100644 +--- a/drivers/media/usb/uvc/uvc_driver.c ++++ b/drivers/media/usb/uvc/uvc_driver.c +@@ -148,6 +148,81 @@ static struct uvc_format_desc uvc_fmts[] = { + .guid = UVC_GUID_FORMAT_H264, + .fcc = V4L2_PIX_FMT_H264, + }, ++ { ++ .name = "Greyscale 8 L/R (Y8I)", ++ .guid = UVC_GUID_FORMAT_Y8I, ++ .fcc = V4L2_PIX_FMT_Y8I, ++ }, ++ { ++ .name = "Greyscale 12 L/R (Y12I)", ++ .guid = UVC_GUID_FORMAT_Y12I, ++ .fcc = V4L2_PIX_FMT_Y12I, ++ }, ++ { ++ .name = "Depth data 16-bit (Z16)", ++ .guid = UVC_GUID_FORMAT_Z16, ++ .fcc = V4L2_PIX_FMT_Z16, ++ }, ++ { ++ .name = "Bayer 10-bit (SRGGB10P)", ++ .guid = UVC_GUID_FORMAT_RW10, ++ .fcc = V4L2_PIX_FMT_SRGGB10P, ++ }, ++ { ++ .name = "Raw data 8-bit (RAW8)", ++ .guid = UVC_GUID_FORMAT_RAW8, ++ .fcc = V4L2_PIX_FMT_RAW8, ++ }, ++ { ++ .name = "Raw data 16-bit (RW16)", ++ .guid = UVC_GUID_FORMAT_RW16, ++ .fcc = V4L2_PIX_FMT_RW16, ++ }, ++ { ++ .name = "Depth 16-bit (INVZ)", ++ .guid = UVC_GUID_FORMAT_INVZ, ++ .fcc = V4L2_PIX_FMT_INVZ, ++ }, ++ { ++ .name = "Depth:IR 16:8 24-bit (INZI)", ++ .guid = UVC_GUID_FORMAT_INZI, ++ .fcc = V4L2_PIX_FMT_INZI, ++ }, ++ { ++ .name = "Depth 16-bit (INVR)", ++ .guid = UVC_GUID_FORMAT_INVR, ++ .fcc = V4L2_PIX_FMT_INVR, ++ }, ++ { ++ .name = "Depth:IR 16:8 24-bit (INRI)", ++ .guid = UVC_GUID_FORMAT_INRI, ++ .fcc = V4L2_PIX_FMT_INRI, ++ }, ++ { ++ .name = "Infrared 8-bit (INVI)", ++ .guid = UVC_GUID_FORMAT_INVI, ++ .fcc = V4L2_PIX_FMT_INVI, ++ }, ++ { ++ .name = "FlickerIR 8-bit (RELI)", ++ .guid = UVC_GUID_FORMAT_RELI, ++ .fcc = V4L2_PIX_FMT_RELI, ++ }, ++ { ++ .name = "Luminosity data 8-bit (L8)", ++ .guid = UVC_GUID_FORMAT_L8, ++ .fcc = V4L2_PIX_FMT_Y8, ++ }, ++ { ++ .name = "Luminosity data 16-bit (L16)", ++ .guid = UVC_GUID_FORMAT_L16, ++ .fcc = V4L2_PIX_FMT_Y16, ++ }, ++ { ++ .name = "Depth data 16-bit (D16)", ++ .guid = UVC_GUID_FORMAT_D16, ++ .fcc = V4L2_PIX_FMT_Z16, ++ }, + }; + + /* ------------------------------------------------------------------------ +diff --git a/drivers/media/usb/uvc/uvcvideo.h b/drivers/media/usb/uvc/uvcvideo.h +index f0f2391..6704db7 100644 +--- a/drivers/media/usb/uvc/uvcvideo.h ++++ b/drivers/media/usb/uvc/uvcvideo.h +@@ -119,6 +119,51 @@ + #define UVC_GUID_FORMAT_H264 \ + { 'H', '2', '6', '4', 0x00, 0x00, 0x10, 0x00, \ + 0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71} ++#define UVC_GUID_FORMAT_Y8I \ ++ { 'Y', '8', 'I', ' ', 0x00, 0x00, 0x10, 0x00, \ ++ 0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71} ++#define UVC_GUID_FORMAT_Y12I \ ++ { 'Y', '1', '2', 'I', 0x00, 0x00, 0x10, 0x00, \ ++ 0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71} ++#define UVC_GUID_FORMAT_Z16 \ ++ { 'Z', '1', '6', ' ', 0x00, 0x00, 0x10, 0x00, \ ++ 0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71} ++#define UVC_GUID_FORMAT_RW10 \ ++ { 'R', 'W', '1', '0', 0x00, 0x00, 0x10, 0x00, \ ++ 0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71} ++#define UVC_GUID_FORMAT_RAW8 \ ++ { 'R', 'A', 'W', '8', 0x66, 0x1a, 0x42, 0xa2, \ ++ 0x90, 0x65, 0xd0, 0x18, 0x14, 0xa8, 0xef, 0x8a} ++#define UVC_GUID_FORMAT_RW16 \ ++ { 'R', 'W', '1', '6', 0x00, 0x00, 0x10, 0x00, \ ++ 0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71} ++#define UVC_GUID_FORMAT_INVZ \ ++ { 'I', 'N', 'V', 'Z', 0x90, 0x2d, 0x58, 0x4a, \ ++ 0x92, 0x0b, 0x77, 0x3f, 0x1f, 0x2c, 0x55, 0x6b} ++#define UVC_GUID_FORMAT_INZI \ ++ { 'I', 'N', 'Z', 'I', 0x66, 0x1a, 0x42, 0xa2, \ ++ 0x90, 0x65, 0xd0, 0x18, 0x14, 0xa8, 0xef, 0x8a} ++#define UVC_GUID_FORMAT_INVR \ ++ { 'I', 'N', 'V', 'R', 0x90, 0x2d, 0x58, 0x4a, \ ++ 0x92, 0x0b, 0x77, 0x3f, 0x1f, 0x2c, 0x55, 0x6b} ++#define UVC_GUID_FORMAT_INRI \ ++ { 'I', 'N', 'R', 'I', 0x90, 0x2d, 0x58, 0x4a, \ ++ 0x92, 0x0b, 0x77, 0x3f, 0x1f, 0x2c, 0x55, 0x6b} ++#define UVC_GUID_FORMAT_INVI \ ++ { 'I', 'N', 'V', 'I', 0xdb, 0x57, 0x49, 0x5e, \ ++ 0x8e, 0x3f, 0xf4, 0x79, 0x53, 0x2b, 0x94, 0x6f} ++#define UVC_GUID_FORMAT_RELI \ ++ { 'R', 'E', 'L', 'I', 0x14, 0x13, 0x43, 0xf9, \ ++ 0xa7, 0x5a, 0xee, 0x6b, 0xbf, 0x01, 0x2e, 0x23} ++#define UVC_GUID_FORMAT_L8 \ ++ { '2', 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, \ ++ 0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71} ++#define UVC_GUID_FORMAT_L16 \ ++ { 'Q', 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, \ ++ 0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71} ++#define UVC_GUID_FORMAT_D16 \ ++ { 'P', 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, \ ++ 0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71} + + /* ------------------------------------------------------------------------ + * Driver specific constants. +diff --git a/include/uapi/linux/videodev2.h b/include/uapi/linux/videodev2.h +index a0e87d1..7e00be4 100644 +--- a/include/uapi/linux/videodev2.h ++++ b/include/uapi/linux/videodev2.h +@@ -669,6 +669,17 @@ + #define V4L2_PIX_FMT_Z16 v4l2_fourcc('Z', '1', '6', ' ') /* Depth data 16-bit */ + #define V4L2_PIX_FMT_MT21C v4l2_fourcc('M', 'T', '2', '1') /* Mediatek compressed block mode */ + #define V4L2_PIX_FMT_INZI v4l2_fourcc('I', 'N', 'Z', 'I') /* Intel Planar Greyscale 10-bit and Depth 16-bit */ ++#define V4L2_PIX_FMT_Y8 v4l2_fourcc('Y', '8', ' ', ' ') /* Greyscale 8-bit */ ++#define V4L2_PIX_FMT_Y10 v4l2_fourcc('Y', '1', '0', ' ') /* Greyscale 10-bit */ ++#define V4L2_PIX_FMT_Y12 v4l2_fourcc('Y', '1', '2', ' ') /* Greyscale 12-bit */ ++#define V4L2_PIX_FMT_Y16 v4l2_fourcc('Y', '1', '6', ' ') /* Greyscale 16-bit */ ++#define V4L2_PIX_FMT_RAW8 v4l2_fourcc('R', 'A', 'W', '8') /* Raw data 8-bit */ ++#define V4L2_PIX_FMT_RW16 v4l2_fourcc('R', 'W', '1', '6') /* Raw data 16-bit */ ++#define V4L2_PIX_FMT_INVZ v4l2_fourcc('I', 'N', 'V', 'Z') /* 16 Depth */ ++#define V4L2_PIX_FMT_INVR v4l2_fourcc('I', 'N', 'V', 'R') /* 16 Depth */ ++#define V4L2_PIX_FMT_INRI v4l2_fourcc('I', 'N', 'R', 'I') /* 24 Depth/IR 16:8 */ ++#define V4L2_PIX_FMT_INVI v4l2_fourcc('I', 'N', 'V', 'I') /* 8 IR */ ++#define V4L2_PIX_FMT_RELI v4l2_fourcc('R', 'E', 'L', 'I') /* 8 IR alternating on off illumination */ + + /* 10bit raw bayer packed, 32 bytes for every 25 pixels, last LSB 6 bits unused */ + #define V4L2_PIX_FMT_IPU3_SBGGR10 v4l2_fourcc('i', 'p', '3', 'b') /* IPU3 packed 10-bit BGGR bayer */ diff --git a/scripts/realsense-metadata-ubuntu-bionic-master.patch b/scripts/realsense-metadata-ubuntu-bionic-master.patch index 50aaa5a952..44a09beac3 100644 --- a/scripts/realsense-metadata-ubuntu-bionic-master.patch +++ b/scripts/realsense-metadata-ubuntu-bionic-master.patch @@ -18,7 +18,7 @@ diff --git a/drivers/media/usb/uvc/uvc_driver.c b/drivers/media/usb/uvc/uvc_driv index f50a2b148..534f49135 100644 --- a/drivers/media/usb/uvc/uvc_driver.c +++ b/drivers/media/usb/uvc/uvc_driver.c -@@ -2771,6 +2771,141 @@ static const struct usb_device_id uvc_ids[] = { +@@ -2771,6 +2771,159 @@ static const struct usb_device_id uvc_ids[] = { .bInterfaceSubClass = 1, .bInterfaceProtocol = 0, .driver_info = UVC_QUIRK_FORCE_Y8 }, @@ -67,6 +67,15 @@ index f50a2b148..534f49135 100644 + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, ++ /* Intel D400 IMU Module */ ++ { .match_flags = USB_DEVICE_ID_MATCH_DEVICE ++ | USB_DEVICE_ID_MATCH_INT_INFO, ++ .idVendor = 0x8086, ++ .idProduct = 0x0af2, ++ .bInterfaceClass = USB_CLASS_VIDEO, ++ .bInterfaceSubClass = 1, ++ .bInterfaceProtocol = 0, ++ .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, + /* Intel D420/PWG depth camera */ + { .match_flags = USB_DEVICE_ID_MATCH_DEVICE + | USB_DEVICE_ID_MATCH_INT_INFO, @@ -156,6 +165,15 @@ index f50a2b148..534f49135 100644 + .bInterfaceClass = USB_CLASS_VIDEO, + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, ++ .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, ++ /* Intel D435i depth camera */ ++ { .match_flags = USB_DEVICE_ID_MATCH_DEVICE ++ | USB_DEVICE_ID_MATCH_INT_INFO, ++ .idVendor = 0x8086, ++ .idProduct = 0x0b3a, ++ .bInterfaceClass = USB_CLASS_VIDEO, ++ .bInterfaceSubClass = 1, ++ .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, /* Generic USB Video Class */ { USB_INTERFACE_INFO(USB_CLASS_VIDEO, 1, UVC_PC_PROTOCOL_UNDEFINED) }, diff --git a/scripts/realsense-metadata-ubuntu-xenial-Ubuntu-hwe-4.13.0-45.50_16.04.1.patch b/scripts/realsense-metadata-ubuntu-xenial-Ubuntu-hwe-4.13.0-45.50_16.04.1.patch index b0b22ad05e..3ecaa06c8c 100644 --- a/scripts/realsense-metadata-ubuntu-xenial-Ubuntu-hwe-4.13.0-45.50_16.04.1.patch +++ b/scripts/realsense-metadata-ubuntu-xenial-Ubuntu-hwe-4.13.0-45.50_16.04.1.patch @@ -15,7 +15,7 @@ diff --git a/drivers/media/usb/uvc/uvc_driver.c b/drivers/media/usb/uvc/uvc_driv index 70842c5..9cdc50a 100644 --- a/drivers/media/usb/uvc/uvc_driver.c +++ b/drivers/media/usb/uvc/uvc_driver.c -@@ -2742,6 +2742,141 @@ static struct usb_device_id uvc_ids[] = { +@@ -2742,6 +2742,159 @@ static struct usb_device_id uvc_ids[] = { .bInterfaceSubClass = 1, .bInterfaceProtocol = 0, .driver_info = UVC_QUIRK_FORCE_Y8 }, @@ -64,6 +64,15 @@ index 70842c5..9cdc50a 100644 + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, ++ /* Intel D400 IMU Module */ ++ { .match_flags = USB_DEVICE_ID_MATCH_DEVICE ++ | USB_DEVICE_ID_MATCH_INT_INFO, ++ .idVendor = 0x8086, ++ .idProduct = 0x0af2, ++ .bInterfaceClass = USB_CLASS_VIDEO, ++ .bInterfaceSubClass = 1, ++ .bInterfaceProtocol = 0, ++ .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, + /* Intel D420/PWG depth camera */ + { .match_flags = USB_DEVICE_ID_MATCH_DEVICE + | USB_DEVICE_ID_MATCH_INT_INFO, @@ -153,6 +162,15 @@ index 70842c5..9cdc50a 100644 + .bInterfaceClass = USB_CLASS_VIDEO, + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, ++ .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, ++ /* Intel D435i depth camera */ ++ { .match_flags = USB_DEVICE_ID_MATCH_DEVICE ++ | USB_DEVICE_ID_MATCH_INT_INFO, ++ .idVendor = 0x8086, ++ .idProduct = 0x0B3A, ++ .bInterfaceClass = USB_CLASS_VIDEO, ++ .bInterfaceSubClass = 1, ++ .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, /* Generic USB Video Class */ { USB_INTERFACE_INFO(USB_CLASS_VIDEO, 1, UVC_PC_PROTOCOL_UNDEFINED) }, diff --git a/scripts/realsense-metadata-ubuntu-xenial-Ubuntu-hwe-4.8.0-58.63_16.04.1.patch b/scripts/realsense-metadata-ubuntu-xenial-Ubuntu-hwe-4.8.0-58.63_16.04.1.patch index aa7c040647..d7924f64f6 100644 --- a/scripts/realsense-metadata-ubuntu-xenial-Ubuntu-hwe-4.8.0-58.63_16.04.1.patch +++ b/scripts/realsense-metadata-ubuntu-xenial-Ubuntu-hwe-4.8.0-58.63_16.04.1.patch @@ -15,7 +15,7 @@ diff --git a/drivers/media/usb/uvc/uvc_driver.c b/drivers/media/usb/uvc/uvc_driv index 04bf350..5fd10f0 100644 --- a/drivers/media/usb/uvc/uvc_driver.c +++ b/drivers/media/usb/uvc/uvc_driver.c -@@ -2699,6 +2699,141 @@ static struct usb_device_id uvc_ids[] = { +@@ -2699,6 +2699,159 @@ static struct usb_device_id uvc_ids[] = { .bInterfaceSubClass = 1, .bInterfaceProtocol = 0, .driver_info = UVC_QUIRK_FORCE_Y8 }, @@ -64,6 +64,15 @@ index 04bf350..5fd10f0 100644 + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, ++ /* Intel D400 IMU Module */ ++ { .match_flags = USB_DEVICE_ID_MATCH_DEVICE ++ | USB_DEVICE_ID_MATCH_INT_INFO, ++ .idVendor = 0x8086, ++ .idProduct = 0x0af2, ++ .bInterfaceClass = USB_CLASS_VIDEO, ++ .bInterfaceSubClass = 1, ++ .bInterfaceProtocol = 0, ++ .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, + /* Intel D420/PWG depth camera */ + { .match_flags = USB_DEVICE_ID_MATCH_DEVICE + | USB_DEVICE_ID_MATCH_INT_INFO, @@ -153,6 +162,15 @@ index 04bf350..5fd10f0 100644 + .bInterfaceClass = USB_CLASS_VIDEO, + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, ++ .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, ++ /* Intel D435i depth camera */ ++ { .match_flags = USB_DEVICE_ID_MATCH_DEVICE ++ | USB_DEVICE_ID_MATCH_INT_INFO, ++ .idVendor = 0x8086, ++ .idProduct = 0x0b3a, ++ .bInterfaceClass = USB_CLASS_VIDEO, ++ .bInterfaceSubClass = 1, ++ .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, /* Generic USB Video Class */ { USB_INTERFACE_INFO(USB_CLASS_VIDEO, 1, UVC_PC_PROTOCOL_UNDEFINED) }, diff --git a/scripts/realsense-metadata-ubuntu-xenial-hwe-zesty.patch b/scripts/realsense-metadata-ubuntu-xenial-hwe-zesty.patch index aa7c040647..d7924f64f6 100644 --- a/scripts/realsense-metadata-ubuntu-xenial-hwe-zesty.patch +++ b/scripts/realsense-metadata-ubuntu-xenial-hwe-zesty.patch @@ -15,7 +15,7 @@ diff --git a/drivers/media/usb/uvc/uvc_driver.c b/drivers/media/usb/uvc/uvc_driv index 04bf350..5fd10f0 100644 --- a/drivers/media/usb/uvc/uvc_driver.c +++ b/drivers/media/usb/uvc/uvc_driver.c -@@ -2699,6 +2699,141 @@ static struct usb_device_id uvc_ids[] = { +@@ -2699,6 +2699,159 @@ static struct usb_device_id uvc_ids[] = { .bInterfaceSubClass = 1, .bInterfaceProtocol = 0, .driver_info = UVC_QUIRK_FORCE_Y8 }, @@ -64,6 +64,15 @@ index 04bf350..5fd10f0 100644 + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, ++ /* Intel D400 IMU Module */ ++ { .match_flags = USB_DEVICE_ID_MATCH_DEVICE ++ | USB_DEVICE_ID_MATCH_INT_INFO, ++ .idVendor = 0x8086, ++ .idProduct = 0x0af2, ++ .bInterfaceClass = USB_CLASS_VIDEO, ++ .bInterfaceSubClass = 1, ++ .bInterfaceProtocol = 0, ++ .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, + /* Intel D420/PWG depth camera */ + { .match_flags = USB_DEVICE_ID_MATCH_DEVICE + | USB_DEVICE_ID_MATCH_INT_INFO, @@ -153,6 +162,15 @@ index 04bf350..5fd10f0 100644 + .bInterfaceClass = USB_CLASS_VIDEO, + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, ++ .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, ++ /* Intel D435i depth camera */ ++ { .match_flags = USB_DEVICE_ID_MATCH_DEVICE ++ | USB_DEVICE_ID_MATCH_INT_INFO, ++ .idVendor = 0x8086, ++ .idProduct = 0x0b3a, ++ .bInterfaceClass = USB_CLASS_VIDEO, ++ .bInterfaceSubClass = 1, ++ .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, /* Generic USB Video Class */ { USB_INTERFACE_INFO(USB_CLASS_VIDEO, 1, UVC_PC_PROTOCOL_UNDEFINED) }, diff --git a/scripts/realsense-metadata-ubuntu-xenial-hwe.patch b/scripts/realsense-metadata-ubuntu-xenial-hwe.patch index 50aaa5a952..44a09beac3 100644 --- a/scripts/realsense-metadata-ubuntu-xenial-hwe.patch +++ b/scripts/realsense-metadata-ubuntu-xenial-hwe.patch @@ -18,7 +18,7 @@ diff --git a/drivers/media/usb/uvc/uvc_driver.c b/drivers/media/usb/uvc/uvc_driv index f50a2b148..534f49135 100644 --- a/drivers/media/usb/uvc/uvc_driver.c +++ b/drivers/media/usb/uvc/uvc_driver.c -@@ -2771,6 +2771,141 @@ static const struct usb_device_id uvc_ids[] = { +@@ -2771,6 +2771,159 @@ static const struct usb_device_id uvc_ids[] = { .bInterfaceSubClass = 1, .bInterfaceProtocol = 0, .driver_info = UVC_QUIRK_FORCE_Y8 }, @@ -67,6 +67,15 @@ index f50a2b148..534f49135 100644 + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, ++ /* Intel D400 IMU Module */ ++ { .match_flags = USB_DEVICE_ID_MATCH_DEVICE ++ | USB_DEVICE_ID_MATCH_INT_INFO, ++ .idVendor = 0x8086, ++ .idProduct = 0x0af2, ++ .bInterfaceClass = USB_CLASS_VIDEO, ++ .bInterfaceSubClass = 1, ++ .bInterfaceProtocol = 0, ++ .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, + /* Intel D420/PWG depth camera */ + { .match_flags = USB_DEVICE_ID_MATCH_DEVICE + | USB_DEVICE_ID_MATCH_INT_INFO, @@ -156,6 +165,15 @@ index f50a2b148..534f49135 100644 + .bInterfaceClass = USB_CLASS_VIDEO, + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, ++ .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, ++ /* Intel D435i depth camera */ ++ { .match_flags = USB_DEVICE_ID_MATCH_DEVICE ++ | USB_DEVICE_ID_MATCH_INT_INFO, ++ .idVendor = 0x8086, ++ .idProduct = 0x0b3a, ++ .bInterfaceClass = USB_CLASS_VIDEO, ++ .bInterfaceSubClass = 1, ++ .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, /* Generic USB Video Class */ { USB_INTERFACE_INFO(USB_CLASS_VIDEO, 1, UVC_PC_PROTOCOL_UNDEFINED) }, diff --git a/scripts/realsense-metadata-ubuntu-xenial-master.patch b/scripts/realsense-metadata-ubuntu-xenial-master.patch index 69d55466e5..d7924f64f6 100644 --- a/scripts/realsense-metadata-ubuntu-xenial-master.patch +++ b/scripts/realsense-metadata-ubuntu-xenial-master.patch @@ -15,7 +15,7 @@ diff --git a/drivers/media/usb/uvc/uvc_driver.c b/drivers/media/usb/uvc/uvc_driv index 04bf350..5fd10f0 100644 --- a/drivers/media/usb/uvc/uvc_driver.c +++ b/drivers/media/usb/uvc/uvc_driver.c -@@ -2699,6 +2699,141 @@ static struct usb_device_id uvc_ids[] = { +@@ -2699,6 +2699,159 @@ static struct usb_device_id uvc_ids[] = { .bInterfaceSubClass = 1, .bInterfaceProtocol = 0, .driver_info = UVC_QUIRK_FORCE_Y8 }, @@ -64,6 +64,15 @@ index 04bf350..5fd10f0 100644 + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, ++ /* Intel D400 IMU Module */ ++ { .match_flags = USB_DEVICE_ID_MATCH_DEVICE ++ | USB_DEVICE_ID_MATCH_INT_INFO, ++ .idVendor = 0x8086, ++ .idProduct = 0x0af2, ++ .bInterfaceClass = USB_CLASS_VIDEO, ++ .bInterfaceSubClass = 1, ++ .bInterfaceProtocol = 0, ++ .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, + /* Intel D420/PWG depth camera */ + { .match_flags = USB_DEVICE_ID_MATCH_DEVICE + | USB_DEVICE_ID_MATCH_INT_INFO, @@ -136,6 +145,15 @@ index 04bf350..5fd10f0 100644 + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, ++ /* Intel D405 S depth camera */ ++ { .match_flags = USB_DEVICE_ID_MATCH_DEVICE ++ | USB_DEVICE_ID_MATCH_INT_INFO, ++ .idVendor = 0x8086, ++ .idProduct = 0x0b0c, ++ .bInterfaceClass = USB_CLASS_VIDEO, ++ .bInterfaceSubClass = 1, ++ .bInterfaceProtocol = 0, ++ .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, + /* Intel L500 depth camera */ + { .match_flags = USB_DEVICE_ID_MATCH_DEVICE + | USB_DEVICE_ID_MATCH_INT_INFO, @@ -145,11 +163,11 @@ index 04bf350..5fd10f0 100644 + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, -+ /* Intel D405 S depth camera */ ++ /* Intel D435i depth camera */ + { .match_flags = USB_DEVICE_ID_MATCH_DEVICE + | USB_DEVICE_ID_MATCH_INT_INFO, + .idVendor = 0x8086, -+ .idProduct = 0x0b0c, ++ .idProduct = 0x0b3a, + .bInterfaceClass = USB_CLASS_VIDEO, + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, diff --git a/scripts/realsense-metadata-ubuntu-xenial-v4.16.patch b/scripts/realsense-metadata-ubuntu-xenial-v4.16.patch index bfac4f96ca..54e144c1e2 100644 --- a/scripts/realsense-metadata-ubuntu-xenial-v4.16.patch +++ b/scripts/realsense-metadata-ubuntu-xenial-v4.16.patch @@ -29,7 +29,7 @@ index fd387bf..3219519 100644 /* * The Logitech cameras listed below have their interface class set to -@@ -2819,6 +2821,141 @@ static const struct usb_device_id uvc_ids[] = { +@@ -2819,6 +2821,159 @@ static const struct usb_device_id uvc_ids[] = { .bInterfaceSubClass = 1, .bInterfaceProtocol = 0, .driver_info = (kernel_ulong_t)&uvc_quirk_force_y8 }, @@ -78,6 +78,15 @@ index fd387bf..3219519 100644 + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_META(V4L2_META_FMT_D4XX) }, ++ /* Intel D400 IMU Module */ ++ { .match_flags = USB_DEVICE_ID_MATCH_DEVICE ++ | USB_DEVICE_ID_MATCH_INT_INFO, ++ .idVendor = 0x8086, ++ .idProduct = 0x0af2, ++ .bInterfaceClass = USB_CLASS_VIDEO, ++ .bInterfaceSubClass = 1, ++ .bInterfaceProtocol = 0, ++ .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, + /* Intel D420/PWG depth camera */ + { .match_flags = USB_DEVICE_ID_MATCH_DEVICE + | USB_DEVICE_ID_MATCH_INT_INFO, @@ -168,6 +177,15 @@ index fd387bf..3219519 100644 + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 0, + .driver_info = UVC_QUIRK_META(V4L2_META_FMT_D4XX) }, ++ /* Intel D435i depth camera */ ++ { .match_flags = USB_DEVICE_ID_MATCH_DEVICE ++ | USB_DEVICE_ID_MATCH_INT_INFO, ++ .idVendor = 0x8086, ++ .idProduct = 0x0b3a, ++ .bInterfaceClass = USB_CLASS_VIDEO, ++ .bInterfaceSubClass = 1, ++ .bInterfaceProtocol = 0, ++ .driver_info = UVC_QUIRK_APPEND_UVC_HEADER }, /* Generic USB Video Class */ { USB_INTERFACE_INFO(USB_CLASS_VIDEO, 1, UVC_PC_PROTOCOL_UNDEFINED) }, { USB_INTERFACE_INFO(USB_CLASS_VIDEO, 1, UVC_PC_PROTOCOL_15) }, diff --git a/scripts/realsense_metadata_win10.ps1 b/scripts/realsense_metadata_win10.ps1 index 606e1ec552..98ace1645f 100644 --- a/scripts/realsense_metadata_win10.ps1 +++ b/scripts/realsense_metadata_win10.ps1 @@ -67,7 +67,8 @@ $MultiPinDevices = "USB\VID_8086&PID_0B03&MI_00",# D460(DS5U) "USB\VID_8086&PID_0B07&MI_00",# D430+RGB(AWGC) "USB\VID_8086&PID_0B0C&MI_00",# D405 - "USB\VID_8086&PID_0B0D&MI_00" # L500 + "USB\VID_8086&PID_0B0D&MI_00",# L500 + "USB\VID_8086&PID_0B18&MI_00" # D435i #Inhibit system warnings and erros, such as permissions or missing values $ErrorActionPreference = "silentlycontinue" diff --git a/src/archive.cpp b/src/archive.cpp index 7e23aa34f2..57ee5799d1 100644 --- a/src/archive.cpp +++ b/src/archive.cpp @@ -2,6 +2,7 @@ #include "archive.h" #include #include "core/processing.h" +#include "core/video.h" #define MIN_DISTANCE 1e-6 @@ -39,12 +40,19 @@ namespace librealsense return std::make_tuple(texture_data[idx], texture_data[idx + 1], texture_data[idx + 2]); } + void points::export_to_ply(const std::string& fname, const frame_holder& texture) { + auto stream_profile = get_stream().get(); + auto video_stream_profile = dynamic_cast(stream_profile); + if (!video_stream_profile) + throw librealsense::invalid_value_exception("stream must be video stream"); const auto vertices = get_vertices(); const auto texcoords = get_texture_coordinates(); std::vector new_vertices; std::vector> new_tex; + std::map index2reducedIndex; + new_vertices.reserve(get_vertex_count()); new_tex.reserve(get_vertex_count()); assert(get_vertex_count()); @@ -52,6 +60,7 @@ namespace librealsense if (fabs(vertices[i].x) >= MIN_DISTANCE || fabs(vertices[i].y) >= MIN_DISTANCE || fabs(vertices[i].z) >= MIN_DISTANCE) { + index2reducedIndex[i] = new_vertices.size(); new_vertices.push_back(vertices[i]); if (texture) { @@ -60,6 +69,25 @@ namespace librealsense } } + const auto threshold = 0.05f; + auto width = video_stream_profile->get_width(); + std::vector> faces; + for (int x = 0; x < width - 1; ++x) { + for (int y = 0; y < video_stream_profile->get_height() - 1; ++y) { + auto a = y * width + x, b = y * width + x + 1, c = (y + 1)*width + x, d = (y + 1)*width + x + 1; + if (vertices[a].z && vertices[b].z && vertices[c].z && vertices[d].z + && abs(vertices[a].z - vertices[b].z) < threshold && abs(vertices[a].z - vertices[c].z) < threshold + && abs(vertices[b].z - vertices[d].z) < threshold && abs(vertices[c].z - vertices[d].z) < threshold) + { + if (index2reducedIndex.count(a) == 0 || index2reducedIndex.count(b) == 0 || index2reducedIndex.count(c) == 0 || + index2reducedIndex.count(d) == 0) + continue; + faces.emplace_back(index2reducedIndex[a], index2reducedIndex[b], index2reducedIndex[d]); + faces.emplace_back(index2reducedIndex[d], index2reducedIndex[c], index2reducedIndex[a]); + } + } + } + std::ofstream out(fname); out << "ply\n"; out << "format binary_little_endian 1.0\n" /*"format ascii 1.0\n"*/; @@ -74,6 +102,8 @@ namespace librealsense out << "property uchar green\n"; out << "property uchar blue\n"; } + out << "element face " << faces.size() << "\n"; + out << "property list uchar int vertex_indices\n"; out << "end_header\n"; out.close(); @@ -94,6 +124,14 @@ namespace librealsense out.write(reinterpret_cast(&z), sizeof(uint8_t)); } } + auto size = faces.size(); + for (int i = 0; i < size; ++i) { + int three = 3; + out.write(reinterpret_cast(&three), sizeof(uint8_t)); + out.write(reinterpret_cast(&(std::get<0>(faces[i]))), sizeof(int)); + out.write(reinterpret_cast(&(std::get<1>(faces[i]))), sizeof(int)); + out.write(reinterpret_cast(&(std::get<2>(faces[i]))), sizeof(int)); + } } size_t points::get_vertex_count() const @@ -332,7 +370,7 @@ namespace librealsense std::shared_ptr make_archive(rs2_extension type, std::atomic* in_max_frame_queue_size, std::shared_ptr ts, - std::shared_ptr parsers) + std::shared_ptr parsers) { switch (type) { diff --git a/src/context.cpp b/src/context.cpp index 8bb3443a4e..b739f36394 100644 --- a/src/context.cpp +++ b/src/context.cpp @@ -103,6 +103,19 @@ namespace librealsense { case backend_type::standard: _backend = platform::create_backend(); +#if WITH_TRACKING + _tm2_context = std::make_shared(this); + _tm2_context->on_device_changed += [this](std::shared_ptr removed, std::shared_ptr added)-> void + { + std::vector rs2_devices_info_added; + std::vector rs2_devices_info_removed; + if (removed) + rs2_devices_info_removed.push_back({ shared_from_this(), removed }); + if (added) + rs2_devices_info_added.push_back({ shared_from_this(), added }); + raise_devices_changed(rs2_devices_info_removed, rs2_devices_info_added); + }; +#endif break; case backend_type::record: _backend = std::make_shared(platform::create_backend(), filename, section, mode); @@ -117,19 +130,6 @@ namespace librealsense environment::get_instance().set_time_service(_backend->create_time_service()); _device_watcher = _backend->create_device_watcher(); -#if WITH_TRACKING - _tm2_context = std::make_shared(this); - _tm2_context->on_device_changed += [this](std::shared_ptr removed, std::shared_ptr added)-> void - { - std::vector rs2_devices_info_added; - std::vector rs2_devices_info_removed; - if(removed) - rs2_devices_info_removed.push_back({ shared_from_this(), removed }); - if (added) - rs2_devices_info_added.push_back({ shared_from_this(), added }); - raise_devices_changed(rs2_devices_info_removed, rs2_devices_info_added); - }; -#endif } @@ -311,7 +311,9 @@ namespace librealsense { platform::backend_device_group devices(_backend->query_uvc_devices(), _backend->query_usb_devices(), _backend->query_hid_devices()); - +#ifdef WITH_TRACKING + if (_tm2_context) _tm2_context->create_manager(); +#endif return create_devices(devices, _playback_devices, mask); } @@ -326,8 +328,11 @@ namespace librealsense auto ctx = t->shared_from_this(); #ifdef WITH_TRACKING - auto tm2_devices = tm2_info::pick_tm2_devices(ctx, _tm2_context->get_manager(), _tm2_context->query_devices()); - std::copy(begin(tm2_devices), end(tm2_devices), std::back_inserter(list)); + if (_tm2_context) + { + auto tm2_devices = tm2_info::pick_tm2_devices(ctx, _tm2_context->get_manager(), _tm2_context->query_devices()); + std::copy(begin(tm2_devices), end(tm2_devices), std::back_inserter(list)); + } #endif auto l500_devices = l500_info::pick_l500_devices(ctx, devices.uvc_devices, devices.usb_devices); diff --git a/src/core/roi.h b/src/core/roi.h index 932b3c5cd4..94afa32347 100644 --- a/src/core/roi.h +++ b/src/core/roi.h @@ -2,10 +2,11 @@ // Copyright(c) 2015 Intel Corporation. All Rights Reserved. #pragma once -#include "video.h" - #include +#include "core/extension.h" +#include "types.h" // exception + namespace librealsense { struct region_of_interest diff --git a/src/device.cpp b/src/device.cpp index 77c95af7b3..fd3dca400f 100644 --- a/src/device.cpp +++ b/src/device.cpp @@ -2,6 +2,8 @@ // Copyright(c) 2015 Intel Corporation. All Rights Reserved. #include "environment.h" +#include "core/video.h" +#include "core/motion.h" #include "device.h" using namespace librealsense; @@ -269,8 +271,7 @@ void librealsense::device::tag_profiles(stream_profiles profiles) const { for (auto tag : *_profiles_tags) { - auto vp = dynamic_cast(profile.get()); - if (vp) + if (auto vp = dynamic_cast(profile.get())) { if ((tag.stream == RS2_STREAM_ANY || vp->get_stream_type() == tag.stream) && (tag.format == RS2_FORMAT_ANY || vp->get_format() == tag.format) && @@ -280,6 +281,15 @@ void librealsense::device::tag_profiles(stream_profiles profiles) const (tag.stream_index == -1 || vp->get_stream_index() == tag.stream_index)) profile->tag_profile(tag.tag); } + else + if (auto mp = dynamic_cast(profile.get())) + { + if ((tag.stream == RS2_STREAM_ANY || mp->get_stream_type() == tag.stream) && + (tag.format == RS2_FORMAT_ANY || mp->get_format() == tag.format) && + (tag.fps == -1 || mp->get_framerate() == tag.fps) && + (tag.stream_index == -1 || mp->get_stream_index() == tag.stream_index)) + profile->tag_profile(tag.tag); + } } } -} \ No newline at end of file +} diff --git a/src/ds5/advanced_mode/advanced_mode.cpp b/src/ds5/advanced_mode/advanced_mode.cpp index 12550e39ff..62b90b7287 100644 --- a/src/ds5/advanced_mode/advanced_mode.cpp +++ b/src/ds5/advanced_mode/advanced_mode.cpp @@ -69,6 +69,7 @@ namespace librealsense break; case ds::RS430_PID: case ds::RS435_RGB_PID: + case ds::RS435I_PID: default_430(p); break; case ds::RS405_PID: diff --git a/src/ds5/ds5-color.cpp b/src/ds5/ds5-color.cpp index 70a90e9ad9..aa3a1ee16c 100644 --- a/src/ds5/ds5-color.cpp +++ b/src/ds5/ds5-color.cpp @@ -133,6 +133,14 @@ namespace librealsense color_ep->register_metadata(RS2_FRAME_METADATA_POWER_LINE_FREQUENCY, make_attribute_parser(&md_rgb_control::power_line_frequency, md_rgb_control_attributes::power_line_frequency_attribute, md_prop_offset)); color_ep->register_metadata(RS2_FRAME_METADATA_LOW_LIGHT_COMPENSATION, make_attribute_parser(&md_rgb_control::low_light_comp, md_rgb_control_attributes::low_light_comp_attribute, md_prop_offset)); + // Starting with firmware 5.10.9, auto-exposure ROI is available for color sensor + if (_fw_version >= firmware_version("5.10.9.0")) + { + roi_sensor_interface* roi_sensor; + if (roi_sensor = dynamic_cast(color_ep.get())) + roi_sensor->set_roi_method(std::make_shared(*_hw_monitor, ds::fw_cmd::SETRGBAEROI)); + } + return color_ep; } diff --git a/src/ds5/ds5-color.h b/src/ds5/ds5-color.h index b5568ae8d2..c867c57df5 100644 --- a/src/ds5/ds5-color.h +++ b/src/ds5/ds5-color.h @@ -30,7 +30,9 @@ namespace librealsense std::shared_ptr> _color_extrinsic; }; - class ds5_color_sensor : public uvc_sensor, public video_sensor_interface + class ds5_color_sensor : public uvc_sensor, + public video_sensor_interface, + public roi_sensor_base { public: explicit ds5_color_sensor(ds5_color* owner, diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index 6f53e0b7c5..81114acddc 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -24,45 +24,41 @@ namespace librealsense { - class ds5_auto_exposure_roi_method : public region_of_interest_method + ds5_auto_exposure_roi_method::ds5_auto_exposure_roi_method( + const hw_monitor& hwm, + ds::fw_cmd cmd) + : _hw_monitor(hwm), _cmd(cmd) {} + + void ds5_auto_exposure_roi_method::set(const region_of_interest& roi) { - public: - explicit ds5_auto_exposure_roi_method(const hw_monitor& hwm) : _hw_monitor(hwm) {} + command cmd(_cmd); + cmd.param1 = roi.min_y; + cmd.param2 = roi.max_y; + cmd.param3 = roi.min_x; + cmd.param4 = roi.max_x; + _hw_monitor.send(cmd); + } - void set(const region_of_interest& roi) override - { - command cmd(ds::SETAEROI); - cmd.param1 = roi.min_y; - cmd.param2 = roi.max_y; - cmd.param3 = roi.min_x; - cmd.param4 = roi.max_x; - _hw_monitor.send(cmd); - } + region_of_interest ds5_auto_exposure_roi_method::get() const + { + region_of_interest roi; + command cmd(_cmd + 1); + auto res = _hw_monitor.send(cmd); - region_of_interest get() const override + if (res.size() < 4 * sizeof(uint16_t)) { - region_of_interest roi; - command cmd(ds::GETAEROI); - auto res = _hw_monitor.send(cmd); - - if (res.size() < 4 * sizeof(uint16_t)) - { - throw std::runtime_error("Invalid result size!"); - } - - auto words = reinterpret_cast(res.data()); + throw std::runtime_error("Invalid result size!"); + } - roi.min_y = words[0]; - roi.max_y = words[1]; - roi.min_x = words[2]; - roi.max_x = words[3]; + auto words = reinterpret_cast(res.data()); - return roi; - } + roi.min_y = words[0]; + roi.max_y = words[1]; + roi.min_x = words[2]; + roi.max_x = words[3]; - private: - const hw_monitor& _hw_monitor; - }; + return roi; + } std::vector ds5_device::send_receive_raw_data(const std::vector& input) { @@ -278,6 +274,26 @@ namespace librealsense return _hw_monitor->send(cmd); } + ds::d400_caps ds5_device::parse_device_capabilities() const + { + using namespace ds; + std::array gvd_buf; + _hw_monitor->get_gvd(gvd_buf.size(), gvd_buf.data(), GVD); + + // Opaque retrieval + d400_caps val{d400_caps::CAP_UNDEFINED}; + if (gvd_buf[active_projector]) // DepthActiveMode + val |= d400_caps::CAP_ACTIVE_PROJECTOR; + if (gvd_buf[rgb_sensor]) // WithRGB + val |= d400_caps::CAP_RGB_SENSOR; + if (gvd_buf[imu_sensor]) + val |= d400_caps::CAP_IMU_SENSOR; + if (0xFF != (gvd_buf[fisheye_sensor_lb] & gvd_buf[fisheye_sensor_hb])) + val |= d400_caps::CAP_FISHEYE_SENSOR; + + return val; + } + std::shared_ptr ds5_device::create_depth_device(std::shared_ptr ctx, const std::vector& all_device_infos) { @@ -307,6 +323,7 @@ namespace librealsense _depth_stream(new stream(RS2_STREAM_DEPTH)), _left_ir_stream(new stream(RS2_STREAM_INFRARED, 1)), _right_ir_stream(new stream(RS2_STREAM_INFRARED, 2)), + _device_capabilities(ds::d400_caps::CAP_UNDEFINED), _depth_device_idx(add_sensor(create_depth_device(ctx, group.uvc_devices))) { init(ctx, group); @@ -353,9 +370,12 @@ namespace librealsense _coefficients_table_raw = [this]() { return get_raw_calibration_table(coefficients_table_id); }; - std::string device_name = (rs400_sku_names.end() != rs400_sku_names.find(group.uvc_devices.front().pid)) ? rs400_sku_names.at(group.uvc_devices.front().pid) : "RS4xx"; + auto pid = group.uvc_devices.front().pid; + std::string device_name = (rs400_sku_names.end() != rs400_sku_names.find(pid)) ? rs400_sku_names.at(pid) : "RS4xx"; _fw_version = firmware_version(_hw_monitor->get_firmware_version_string(GVD, camera_fw_version_offset)); - recommended_fw_version = firmware_version("5.10.3.0"); + _recommended_fw_version = firmware_version("5.10.3.0"); + if (_fw_version >= firmware_version("5.10.4.0")) + _device_capabilities = parse_device_capabilities(); auto serial = _hw_monitor->get_module_serial_string(GVD, module_serial_offset); auto& depth_ep = get_depth_sensor(); @@ -380,7 +400,6 @@ namespace librealsense depth_ep.register_pixel_format(pf_y12i); // L+R - Calibration not rectified } - auto pid = group.uvc_devices.front().pid; auto pid_hex_str = hexify(pid >> 8) + hexify(static_cast(pid)); std::string is_camera_locked{ "" }; @@ -390,8 +409,9 @@ namespace librealsense is_camera_locked = (is_locked) ? "YES" : "NO"; #ifdef HWM_OVER_XU - //if hw_monitor was created by usb replace it xu - if (group.usb_devices.size() > 0) + //if hw_monitor was created by usb replace it with xu + // D400_IMU will remain using USB interface due to HW limitations + if ((group.usb_devices.size() > 0) && (RS400_IMU_PID != pid)) { _hw_monitor = std::make_shared( std::make_shared( @@ -400,6 +420,7 @@ namespace librealsense get_depth_sensor())); } #endif + depth_ep.register_pu(RS2_OPTION_GAIN); auto exposure_option = std::make_shared>(depth_ep, depth_xu, @@ -432,7 +453,6 @@ namespace librealsense new polling_error_handler(1000, std::move(error_control), depth_ep.get_notifications_processor(), - std::unique_ptr(new ds5_notification_decoder()))); depth_ep.register_option(RS2_OPTION_ERROR_POLLING_ENABLED, std::make_shared(_polling_error_handler.get())); @@ -513,15 +533,15 @@ namespace librealsense register_info(RS2_CAMERA_INFO_DEBUG_OP_CODE, std::to_string(static_cast(fw_cmd::GLD))); register_info(RS2_CAMERA_INFO_ADVANCED_MODE, ((advanced_mode) ? "YES" : "NO")); register_info(RS2_CAMERA_INFO_PRODUCT_ID, pid_hex_str); - register_info(RS2_CAMERA_INFO_RECOMMENDED_FIRMWARE_VERSION, recommended_fw_version); + register_info(RS2_CAMERA_INFO_RECOMMENDED_FIRMWARE_VERSION, _recommended_fw_version); if (usb_modality) register_info(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR, usb_type_str); std::string curr_version= _fw_version; - std::string minimal_version = recommended_fw_version; + std::string minimal_version = _recommended_fw_version; - if (_fw_version < recommended_fw_version) + if (_fw_version < _recommended_fw_version) { std::weak_ptr weak = depth_ep.get_notifications_processor(); std::thread notification_thread = std::thread([weak, curr_version, minimal_version]() diff --git a/src/ds5/ds5-device.h b/src/ds5/ds5-device.h index 9c35aa6fc2..ac296b66f7 100644 --- a/src/ds5/ds5-device.h +++ b/src/ds5/ds5-device.h @@ -13,6 +13,19 @@ namespace librealsense { + class ds5_auto_exposure_roi_method : public region_of_interest_method + { + public: + explicit ds5_auto_exposure_roi_method(const hw_monitor& hwm, + ds::fw_cmd cmd = ds::fw_cmd::SETAEROI); + + void set(const region_of_interest& roi) override; + region_of_interest get() const override; + private: + const ds::fw_cmd _cmd; + const hw_monitor& _hw_monitor; + }; + class ds5_device : public virtual device, public debug_interface { public: @@ -42,14 +55,17 @@ namespace librealsense float get_stereo_baseline_mm() const; + ds::d400_caps parse_device_capabilities() const; + void init(std::shared_ptr ctx, const platform::backend_device_group& group); friend class ds5_depth_sensor; std::shared_ptr _hw_monitor; - firmware_version _fw_version; - firmware_version recommended_fw_version; + firmware_version _fw_version; + firmware_version _recommended_fw_version; + ds::d400_caps _device_capabilities; std::shared_ptr _depth_stream; std::shared_ptr _left_ir_stream; diff --git a/src/ds5/ds5-factory.cpp b/src/ds5/ds5-factory.cpp index 7f6c8b0c84..37dad9b88a 100644 --- a/src/ds5/ds5-factory.cpp +++ b/src/ds5/ds5-factory.cpp @@ -197,6 +197,14 @@ namespace librealsense tags.push_back({ RS2_STREAM_INFRARED, 1, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_INFRARED, 2, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET }); } + + if (_fw_version >= firmware_version("5.10.4.0")) + { + tags.push_back({ RS2_STREAM_FISHEYE, -1, 640, 480, RS2_FORMAT_RAW8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT}); + tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 125, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + } + return tags; }; }; @@ -294,15 +302,22 @@ namespace librealsense tags.push_back({ RS2_STREAM_DEPTH, -1, 1280, 720, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_INFRARED, 1, 1280, 720, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_INFRARED, 2, 1280, 720, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET }); - tags.push_back({ RS2_STREAM_FISHEYE, -1, 640, 480, RS2_FORMAT_RAW8, 30, profile_tag::PROFILE_TAG_SUPERSET }); + } else { tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_INFRARED, 1, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_INFRARED, 2, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET }); - tags.push_back({ RS2_STREAM_FISHEYE, -1, 640, 480, RS2_FORMAT_RAW8, 30, profile_tag::PROFILE_TAG_SUPERSET }); } + + tags.push_back({ RS2_STREAM_FISHEYE, -1, 640, 480, RS2_FORMAT_RAW8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT}); + if (_fw_version >= firmware_version("5.10.4.0")) // Back-compat with records is required + { + tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 125, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + } + return tags; }; }; @@ -383,6 +398,72 @@ namespace librealsense }; }; + + class rs435i_device : public ds5_active, + public ds5_color, + public ds5_motion, + public ds5_advanced_mode_base + { + public: + rs435i_device(std::shared_ptr ctx, + const platform::backend_device_group group, + bool register_device_notifications) + : device(ctx, group, register_device_notifications), + ds5_device(ctx, group), + ds5_active(ctx, group), + ds5_color(ctx, group), + ds5_motion(ctx, group), + ds5_advanced_mode_base(ds5_device::_hw_monitor, get_depth_sensor()) {} + + std::shared_ptr create_matcher(const frame_holder& frame) const; + + std::vector get_profiles_tags() const override + { + std::vector tags; + auto usb_spec = get_usb_spec(); + bool usb3mode = (usb_spec >= platform::usb3_type || usb_spec == platform::usb_undefined); + + uint32_t width = usb3mode ? 1280 : 640; + uint32_t height = usb3mode ? 720 : 480; + uint32_t fps = usb3mode ? 30 : 15; + + tags.push_back({ RS2_STREAM_COLOR, -1, width, height, RS2_FORMAT_RGB8, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_DEPTH, -1, width, height, RS2_FORMAT_Z16, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_INFRARED, -1, width, height, RS2_FORMAT_Y8, fps, profile_tag::PROFILE_TAG_SUPERSET }); + + // TODO - Check which IMU shall be activated by default + //tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + //tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 125, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + + return tags; + }; + }; + + + class rs400_imu_device : public ds5_motion, + public ds5_advanced_mode_base + { + public: + rs400_imu_device(std::shared_ptr ctx, + const platform::backend_device_group group, + bool register_device_notifications) + : device(ctx, group, register_device_notifications), + ds5_device(ctx, group), + ds5_motion(ctx, group), + ds5_advanced_mode_base(ds5_device::_hw_monitor, get_depth_sensor()) {} + + std::shared_ptr create_matcher(const frame_holder& frame) const; + + std::vector get_profiles_tags() const override + { + std::vector tags; + tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 125, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + + return tags; + }; + }; + std::shared_ptr ds5_info::create(std::shared_ptr ctx, bool register_device_notifications) const { @@ -415,8 +496,12 @@ namespace librealsense return std::make_shared(ctx, group, register_device_notifications); case RS435_RGB_PID: return std::make_shared(ctx, group, register_device_notifications); + case RS435I_PID: + return std::make_shared(ctx, group, register_device_notifications); case RS_USB2_PID: return std::make_shared(ctx, group, register_device_notifications); + case RS400_IMU_PID: + return std::make_shared(ctx, group, register_device_notifications); default: throw std::runtime_error(to_string() << "Unsupported RS400 model! 0x" << std::hex << std::setw(4) << std::setfill('0') <<(int)pid); @@ -536,7 +621,11 @@ namespace librealsense std::vector streams = { _depth_stream.get() , _left_ir_stream.get() , _right_ir_stream.get()}; std::vector mm_streams = { _fisheye_stream.get(), _accel_stream.get(), - _gyro_stream.get()}; + _gyro_stream.get(), + _gpio_streams[0].get(), + _gpio_streams[1].get(), + _gpio_streams[2].get(), + _gpio_streams[3].get()}; if (frame.frame->supports_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER)) { @@ -573,7 +662,11 @@ namespace librealsense std::vector streams = { _depth_stream.get() , _left_ir_stream.get() , _right_ir_stream.get() }; std::vector mm_streams = { _fisheye_stream.get(), _accel_stream.get(), - _gyro_stream.get() }; + _gyro_stream.get(), + _gpio_streams[0].get(), + _gpio_streams[1].get(), + _gpio_streams[2].get(), + _gpio_streams[3].get()}; if (!frame.frame->supports_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER)) { @@ -600,7 +693,11 @@ namespace librealsense std::vector streams = { _depth_stream.get() , _left_ir_stream.get() , _right_ir_stream.get(), _color_stream.get() }; std::vector mm_streams = { _fisheye_stream.get(), _accel_stream.get(), - _gyro_stream.get()}; + _gyro_stream.get(), + _gpio_streams[0].get(), + _gpio_streams[1].get(), + _gpio_streams[2].get(), + _gpio_streams[3].get()}; if (frame.frame->supports_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER)) { @@ -610,4 +707,27 @@ namespace librealsense streams.insert(streams.end(), mm_streams.begin(), mm_streams.end()); return matcher_factory::create(RS2_MATCHER_DEFAULT, streams); } + + std::shared_ptr rs435i_device::create_matcher(const frame_holder& frame) const + { + std::vector streams = { _depth_stream.get() , _left_ir_stream.get() , _right_ir_stream.get(), _color_stream.get() }; + // TODO - A proper matcher for High-FPS sensor is required + std::vector mm_streams = { _accel_stream.get(), _gyro_stream.get()}; + + if (frame.frame->supports_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER)) + { + return create_composite_matcher({ matcher_factory::create(RS2_MATCHER_DLR_C, streams), + matcher_factory::create(RS2_MATCHER_DEFAULT, mm_streams) }); + } + streams.insert(streams.end(), mm_streams.begin(), mm_streams.end()); + return matcher_factory::create(RS2_MATCHER_DEFAULT, streams); + } + + std::shared_ptr rs400_imu_device::create_matcher(const frame_holder& frame) const + { + // TODO - A proper matcher for High-FPS sensor is required + std::vector mm_streams = { _accel_stream.get(), _gyro_stream.get()}; + return matcher_factory::create(RS2_MATCHER_DEFAULT, mm_streams); + } + } diff --git a/src/ds5/ds5-motion.cpp b/src/ds5/ds5-motion.cpp index 1f036077a1..fcc44fe801 100644 --- a/src/ds5/ds5-motion.cpp +++ b/src/ds5/ds5-motion.cpp @@ -269,16 +269,9 @@ namespace librealsense for (auto i = 0; i < 4; i++) _gpio_streams[i] = std::make_shared(RS2_STREAM_GPIO, i+1); - auto&& backend = ctx->get_backend(); - _tm1_eeprom_raw = [this]() { return get_tm1_eeprom_raw(); }; _tm1_eeprom = [this]() { return get_tm1_eeprom(); }; - _fisheye_calibration_table_raw = [this]() - { - uint8_t* fe_calib_ptr = reinterpret_cast(&(*_tm1_eeprom).calibration_table.calib_model.fe_calibration); - return std::vector(fe_calib_ptr, fe_calib_ptr+ fisheye_calibration_table_size); - }; _accel_intrinsics = [this]() { return (*_tm1_eeprom).calibration_table.imu_calib_table.accel_intrinsics; }; _gyro_intrinsics = [this](){ return (*_tm1_eeprom).calibration_table.imu_calib_table.gyro_intrinsics; }; @@ -288,12 +281,72 @@ namespace librealsense motion_module_fw_version = _hw_monitor->get_firmware_version_string(GVD, motion_module_fw_version_offset); } + initialize_fisheye_sensor(ctx,group); + + // Make sure all MM streams are positioned with the same extrinsics + environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*_accel_stream, *_gyro_stream); + register_stream_to_extrinsic_group(*_gyro_stream, 0); + register_stream_to_extrinsic_group(*_accel_stream, 0); + + for (auto i = 0; i < 4; i++) + { + environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*_accel_stream, *_gpio_streams[i]); + register_stream_to_extrinsic_group(*_gpio_streams[i], 0); + } + + // Try to add hid endpoint + auto hid_ep = create_hid_device(ctx, group.hid_devices, _fw_version); + if (hid_ep) + { + _motion_module_device_idx = add_sensor(hid_ep); + + try + { + hid_ep->register_option(RS2_OPTION_ENABLE_MOTION_CORRECTION, + std::make_shared(hid_ep.get(), + *_accel_intrinsics, + *_gyro_intrinsics, + option_range{ 0, 1, 1, 1 })); + } + catch (const std::exception& ex) + { + LOG_ERROR("Motion Device is not calibrated! Motion Data Correction will not be available! Error: " << ex.what()); + } + + if (!motion_module_fw_version.empty()) + register_info(RS2_CAMERA_INFO_FIRMWARE_VERSION, motion_module_fw_version); + } + } + + void ds5_motion::initialize_fisheye_sensor(std::shared_ptr ctx, const platform::backend_device_group& group) + { + using namespace ds; + auto fisheye_infos = filter_by_mi(group.uvc_devices, 3); - if (fisheye_infos.size() != 1) - throw invalid_value_exception("RS450 model is expected to include a single fish-eye device!"); + fisheye_infos = filter_device_by_capability(fisheye_infos,d400_caps::CAP_FISHEYE_SENSOR); - std::unique_ptr ds5_timestamp_reader_backup(new ds5_timestamp_reader(environment::get_instance().get_time_service())); + bool fe_dev_present = (fisheye_infos.size() == 1); + bool fe_capability = (d400_caps::CAP_UNDEFINED == _device_capabilities) ? + true : !!(static_cast(_device_capabilities&d400_caps::CAP_FISHEYE_SENSOR)); + + // Motion module w/o FishEye sensor + if (!(fe_dev_present | fe_capability)) return; + + // Inconsistent FW + if (fe_dev_present ^ fe_capability) + throw invalid_value_exception(to_string() + << "Inconsistent HW/FW setup, FW FishEye capability = " << fe_capability + << ", FishEye devices " << std::dec << fisheye_infos.size() + << " while expecting " << fe_capability); + + _fisheye_calibration_table_raw = [this]() + { + uint8_t* fe_calib_ptr = reinterpret_cast(&(*_tm1_eeprom).calibration_table.calib_model.fe_calibration); + return std::vector(fe_calib_ptr, fe_calib_ptr+ fisheye_calibration_table_size); + }; + std::unique_ptr ds5_timestamp_reader_backup(new ds5_timestamp_reader(environment::get_instance().get_time_service())); + auto&& backend = ctx->get_backend(); auto fisheye_ep = std::make_shared(this, backend.create_uvc_device(fisheye_infos.front()), std::unique_ptr(new ds5_timestamp_reader_from_metadata(std::move(ds5_timestamp_reader_backup)))); @@ -377,44 +430,5 @@ namespace librealsense return from_pose(ex); }); - - // Depth->Fisheye is not available - //environment::get_instance().get_extrinsics_graph().register_extrinsics(*_depth_stream, *_fisheye_stream, _depth_to_fisheye); - environment::get_instance().get_extrinsics_graph().register_extrinsics(*_fisheye_stream, *_accel_stream, _fisheye_to_imu); - - register_stream_to_extrinsic_group(*_fisheye_stream, 0); - register_stream_to_extrinsic_group(*_accel_stream, 0); - - // Make sure all MM streams are positioned with the same extrinsics - environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*_accel_stream, *_gyro_stream); - register_stream_to_extrinsic_group(*_gyro_stream, 0); - for (auto i = 0; i < 4; i++) - { - environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*_accel_stream, *_gpio_streams[i]); - register_stream_to_extrinsic_group(*_gpio_streams[i], 0); - } - - // Try to add hid endpoint - auto hid_ep = create_hid_device(ctx, group.hid_devices, _fw_version); - if (hid_ep) - { - _motion_module_device_idx = add_sensor(hid_ep); - - try - { - hid_ep->register_option(RS2_OPTION_ENABLE_MOTION_CORRECTION, - std::make_shared(hid_ep.get(), - *_accel_intrinsics, - *_gyro_intrinsics, - option_range{ 0, 1, 1, 1 })); - } - catch (const std::exception& ex) - { - LOG_ERROR("Motion Device is not calibrated! Motion Data Correction will not be available! Error: " << ex.what()); - } - - if (!motion_module_fw_version.empty()) - register_info(RS2_CAMERA_INFO_FIRMWARE_VERSION, motion_module_fw_version); - } } } diff --git a/src/ds5/ds5-motion.h b/src/ds5/ds5-motion.h index 3dfa67a7c6..89d8247ada 100644 --- a/src/ds5/ds5-motion.h +++ b/src/ds5/ds5-motion.h @@ -23,11 +23,14 @@ namespace librealsense const platform::extension_unit* fisheye_xu); private: + friend class ds5_fisheye_sensor; friend class ds5_hid_sensor; - uint8_t _fisheye_device_idx = -1; - uint8_t _motion_module_device_idx = -1; + void initialize_fisheye_sensor(std::shared_ptr ctx, const platform::backend_device_group& group); + + optional_value _fisheye_device_idx; + optional_value _motion_module_device_idx; lazy> _tm1_eeprom_raw; lazy _tm1_eeprom; diff --git a/src/ds5/ds5-private.cpp b/src/ds5/ds5-private.cpp index e880c56fbe..3fbc7b20bc 100644 --- a/src/ds5/ds5-private.cpp +++ b/src/ds5/ds5-private.cpp @@ -179,10 +179,12 @@ namespace librealsense case RS460_PID: case RS430_PID: case RS420_PID: + case RS400_IMU_PID: found = (result.mi == 3); break; case RS430_MM_PID: case RS420_MM_PID: + case RS435I_PID: found = (result.mi == 6); break; case RS415_PID: @@ -204,5 +206,27 @@ namespace librealsense } return false; } + + + std::vector filter_device_by_capability(const std::vector& devices, + d400_caps caps) + { + std::vector results; + + switch (caps) + { + case d400_caps::CAP_FISHEYE_SENSOR: + std::copy_if(devices.begin(),devices.end(),std::back_inserter(results), + [](const platform::uvc_device_info& info) + { return fisheye_pid.find(info.pid) != fisheye_pid.end();}); + break; + default: + throw invalid_value_exception(to_string() + << "Capability filters are not implemented for val " + << std::hex << caps << std::dec); + } + return results; + } + } // librealsense::ds } // namespace librealsense diff --git a/src/ds5/ds5-private.h b/src/ds5/ds5-private.h index 714eaa7df0..366f1e8f6a 100644 --- a/src/ds5/ds5-private.h +++ b/src/ds5/ds5-private.h @@ -8,6 +8,7 @@ #include #include +#include namespace librealsense @@ -20,14 +21,16 @@ namespace librealsense const uint16_t RS430_PID = 0x0ad4; // AWG const uint16_t RS430_MM_PID = 0x0ad5; // AWGT const uint16_t RS_USB2_PID = 0x0ad6; // USB2 + const uint16_t RS400_IMU_PID = 0x0af2; // IMU const uint16_t RS420_PID = 0x0af6; // PWG const uint16_t RS420_MM_PID = 0x0afe; // PWGT - const uint16_t RS410_MM_PID = 0x0aff; // ASR + const uint16_t RS410_MM_PID = 0x0aff; // ASRT const uint16_t RS400_MM_PID = 0x0b00; // PSR const uint16_t RS430_MM_RGB_PID = 0x0b01; // AWGCT const uint16_t RS460_PID = 0x0b03; // DS5U const uint16_t RS435_RGB_PID = 0x0b07; // AWGC const uint16_t RS405_PID = 0x0b0c; // DS5U + const uint16_t RS435I_PID = 0x0b3a; // D435i // DS5 depth XU identifiers const uint8_t DS5_HWMONITOR = 1; @@ -40,21 +43,24 @@ namespace librealsense const uint8_t DS5_ENABLE_AUTO_WHITE_BALANCE = 0xA; const uint8_t DS5_ENABLE_AUTO_EXPOSURE = 0xB; + // Devices supported by the current version static const std::set rs400_sku_pid = { ds::RS400_PID, - ds::RS400_MM_PID, ds::RS410_PID, - ds::RS410_MM_PID, ds::RS415_PID, - ds::RS420_PID, - ds::RS420_MM_PID, ds::RS430_PID, ds::RS430_MM_PID, + ds::RS_USB2_PID, + ds::RS400_IMU_PID, + ds::RS420_PID, + ds::RS420_MM_PID, + ds::RS410_MM_PID, + ds::RS400_MM_PID, ds::RS430_MM_RGB_PID, - ds::RS435_RGB_PID, ds::RS460_PID, + ds::RS435_RGB_PID, ds::RS405_PID, - ds::RS_USB2_PID + ds::RS435I_PID, }; static const std::set multi_sensors_pid = { @@ -67,6 +73,14 @@ namespace librealsense ds::RS435_RGB_PID }; + static const std::set fisheye_pid = { + ds::RS400_MM_PID, + ds::RS410_MM_PID, + ds::RS420_MM_PID, + ds::RS430_MM_PID, + ds::RS430_MM_RGB_PID, + }; + static const std::map rs400_sku_names = { { RS400_PID, "Intel RealSense D400"}, { RS400_MM_PID, "Intel RealSense D400 with Tracking Module"}, @@ -77,11 +91,14 @@ namespace librealsense { RS420_MM_PID, "Intel RealSense D420 with Tracking Module"}, { RS430_PID, "Intel RealSense D430"}, { RS430_MM_PID, "Intel RealSense D430 with Tracking Module"}, - { RS430_MM_PID, "Intel RealSense D430 with Tracking Module and RGB Module"}, + { RS430_MM_RGB_PID, "Intel RealSense D430 with Tracking and RGB Modules"}, { RS435_RGB_PID, "Intel RealSense D435"}, { RS460_PID, "Intel RealSense D460" }, { RS405_PID, "Intel RealSense D405" }, - { RS_USB2_PID, "Intel RealSense USB2" } + { RS435I_PID, "Intel RealSense D435I" }, + { RS_USB2_PID, "Intel RealSense USB2" }, + { RS400_IMU_PID, "Intel RealSense IMU" } + }; // DS5 fisheye XU identifiers @@ -111,6 +128,8 @@ namespace librealsense GET_EXTRINSICS = 0x53, // get extrinsics SET_CAM_SYNC = 0x69, // set Inter-cam HW sync mode [0-default, 1-master, 2-slave] GET_CAM_SYNC = 0x6A, // fet Inter-cam HW sync mode + SETRGBAEROI = 0x75, // set RGB auto-exposure region of interest + GETRGBAEROI = 0x76, // get RGB auto-exposure region of interest }; const int etDepthTableControl = 9; // Identifier of the depth table control @@ -130,6 +149,50 @@ namespace librealsense INTERCAM_SYNC_MAX }; + enum class d400_caps : uint16_t + { + CAP_UNDEFINED = 0, + CAP_ACTIVE_PROJECTOR = (1u << 0), // + CAP_RGB_SENSOR = (1u << 1), // Dedicated RGB sensor + CAP_FISHEYE_SENSOR = (1u << 2), // TM1 + CAP_IMU_SENSOR = (1u << 3), + CAP_MAX + }; + + static const std::map d400_capabilities_names = { + { d400_caps::CAP_UNDEFINED, "Undefined" }, + { d400_caps::CAP_ACTIVE_PROJECTOR, "Active Projector" }, + { d400_caps::CAP_RGB_SENSOR, "RGB Sensor" }, + { d400_caps::CAP_FISHEYE_SENSOR, "Fisheye Sensor" }, + { d400_caps::CAP_IMU_SENSOR, "IMU Sensor" } + }; + + inline d400_caps operator &(const d400_caps lhs, const d400_caps rhs) + { + return static_cast(static_cast(lhs) & static_cast(rhs)); + } + + inline d400_caps operator |(const d400_caps lhs, const d400_caps rhs) + { + return static_cast(static_cast(lhs) | static_cast(rhs)); + } + + inline d400_caps& operator |=(d400_caps& lhs, d400_caps rhs) + { + return lhs = lhs | rhs; + } + + inline std::ostream& operator <<(std::ostream& stream, const d400_caps& cap) + { + for (auto i : { d400_caps::CAP_ACTIVE_PROJECTOR,d400_caps::CAP_RGB_SENSOR, + d400_caps::CAP_FISHEYE_SENSOR, d400_caps::CAP_IMU_SENSOR}) + { + if (i==(i&cap)) + stream << d400_capabilities_names.at(i) << " "; + } + return stream; + } + const std::string DEPTH_STEREO = "Stereo Module"; struct table_header @@ -348,22 +411,28 @@ namespace librealsense enum gvd_fields { + // Keep sorted camera_fw_version_offset = 12, + is_camera_locked_offset = 25, module_serial_offset = 48, - motion_module_fw_version_offset = 212, - is_camera_locked_offset = 25 + fisheye_sensor_lb = 112, + fisheye_sensor_hb = 113, + active_projector = 170, + rgb_sensor = 174, + imu_sensor = 178, + motion_module_fw_version_offset = 212 }; enum calibration_table_id { - coefficients_table_id = 25, - depth_calibration_id = 31, - rgb_calibration_id = 32, - fisheye_calibration_id = 33, - imu_calibration_id = 34, - lens_shading_id = 35, - projector_id = 36, - max_id = -1 + coefficients_table_id = 25, + depth_calibration_id = 31, + rgb_calibration_id = 32, + fisheye_calibration_id = 33, + imu_calibration_id = 34, + lens_shading_id = 35, + projector_id = 36, + max_id = -1 }; struct ds5_calibration @@ -483,5 +552,7 @@ namespace librealsense { rec_error, "REC error" }, }; + std::vector filter_device_by_capability(const std::vector& devices, d400_caps caps); + } // librealsense::ds } // namespace librealsense diff --git a/src/image.cpp b/src/image.cpp index 772f767f5a..740dfba26f 100644 --- a/src/image.cpp +++ b/src/image.cpp @@ -98,29 +98,37 @@ namespace librealsense #pragma pack(pop) - inline void copy_hid_axes(byte * const dest[], const byte * source, double factor, int count) + inline void copy_hid_axes(byte * const dest[], const byte * source, double factor) { auto hid = (hid_data*)(source); - float axes[3] = { static_cast((hid->x) * factor), + // The IMU sensor orientation shall be aligned with depth sensor's coordinate system + // Note that the implementation follows D435i installation pose and will require refactoring for other designs + // Reference spec: Bosch BMI055 + float axes[3] = { static_cast((hid->x) * -factor), static_cast((hid->y) * factor), - static_cast((hid->z) * factor) }; + static_cast((hid->z) * -factor) }; librealsense::copy(dest[0], axes, sizeof(axes)); } + // The Accelerometer input format: signed int 16bit. data units 1LSB=0.001g; + // Librealsense output format: floating point 32bit. units m/s^2, template void unpack_accel_axes(byte * const dest[], const byte * source, int width, int height) { - auto count = width * height; - static const float gravity = 9.80665f; // Standard Gravitation Acceleration - static const double accelerator_transform_factor = 0.001f*gravity; - copy_hid_axes(dest, source, accelerator_transform_factor, count); + static constexpr float gravity = 9.80665f; // Standard Gravitation Acceleration + static constexpr double accelerator_transform_factor = 0.001*gravity; + + copy_hid_axes(dest, source, accelerator_transform_factor); } + // The Gyro input format: signed int 16bit. data units 1LSB=0.1deg/sec; + // Librealsense output format: floating point 32bit. units rad/sec, template void unpack_gyro_axes(byte * const dest[], const byte * source, int width, int height) { - auto count = width * height; - static const double gyro_transform_factor = 0.1* M_PI / 180.f; - copy_hid_axes(dest, source, gyro_transform_factor, count); + static constexpr double deg2rad = M_PI / 180.; + static const double gyro_transform_factor = 0.1 * deg2rad; + + copy_hid_axes(dest, source, gyro_transform_factor); } void unpack_hid_raw_data(byte * const dest[], const byte * source, int width, int height) diff --git a/src/libuvc/libuvc.cpp b/src/libuvc/libuvc.cpp index 2310d6d65e..d8791d1e82 100644 --- a/src/libuvc/libuvc.cpp +++ b/src/libuvc/libuvc.cpp @@ -591,12 +591,92 @@ namespace librealsense throw std::runtime_error("insufficient data writen to USB"); } + // Translate between UVC Spec and RS + int32_t rs2_value_translate(uvc_req_code action, rs2_option option, int32_t value) const + { + // Value may be translated according to action/option value + int32_t translated_value = value; + + switch (action) + { + case UVC_GET_CUR: // Translating from UVC 1.5 Spec up to RS + if (option == RS2_OPTION_ENABLE_AUTO_EXPOSURE) + { + switch (value) + { + case UVC_AE_MODE_D3_AP: + translated_value = 1; + break; + case UVC_AE_MODE_D0_MANUAL: + translated_value = 0; + break; + default: + throw std::runtime_error("Unsupported GET value for RS2_OPTION_ENABLE_AUTO_EXPOSURE"); + } + } + break; + + case UVC_SET_CUR: // Translating from RS down to UVC 1.5 Spec + if (option == RS2_OPTION_ENABLE_AUTO_EXPOSURE) + { + switch (value) + { + case 1: + // Enabling auto exposure + translated_value = UVC_AE_MODE_D3_AP; + break; + case 0: + // Disabling auto exposure + translated_value = UVC_AE_MODE_D0_MANUAL; + break; + default: + throw std::runtime_error("Unsupported SET value for RS2_OPTION_ENABLE_AUTO_EXPOSURE"); + } + } + break; + + case UVC_GET_MIN: + if (option == RS2_OPTION_ENABLE_AUTO_EXPOSURE) + { + translated_value = 0; // Hardcoded MIN value + } + break; + + case UVC_GET_MAX: + if (option == RS2_OPTION_ENABLE_AUTO_EXPOSURE) + { + translated_value = 1; // Hardcoded MAX value + } + break; + + case UVC_GET_RES: + if (option == RS2_OPTION_ENABLE_AUTO_EXPOSURE) + { + translated_value = 1; // Hardcoded RES (step) value + } + break; + + case UVC_GET_DEF: + if (option == RS2_OPTION_ENABLE_AUTO_EXPOSURE) + { + translated_value = 1; // Hardcoded DEF value + } + break; + + default: + throw std::runtime_error("Unsupported action translation"); + } + return translated_value; + } + + bool get_pu(rs2_option opt, int32_t& value) const override { int unit; int control = rs2_option_to_ctrl_selector(opt, unit); value = get_data_usb( UVC_GET_CUR, control, unit); + value = rs2_value_translate(UVC_GET_CUR, opt, value); return true; } @@ -605,6 +685,7 @@ namespace librealsense int unit; int control = rs2_option_to_ctrl_selector(opt, unit); + value = rs2_value_translate(UVC_SET_CUR, opt, value); set_data_usb( UVC_SET_CUR, control, unit, value); return true; } @@ -613,10 +694,18 @@ namespace librealsense { int unit; int control = rs2_option_to_ctrl_selector(option, unit); + int min = get_data_usb( UVC_GET_MIN, control, unit); + min = rs2_value_translate(UVC_GET_MIN, option, value); + int max = get_data_usb( UVC_GET_MAX, control, unit); + max = rs2_value_translate(UVC_GET_MAX, option, value); + int step = get_data_usb( UVC_GET_RES, control, unit); + step = rs2_value_translate(UVC_GET_RES, option, value); + int def = get_data_usb( UVC_GET_DEF, control, unit); + def = rs2_value_translate(UVC_GET_DEF, option, value); control_range result(min, max, step, def); diff --git a/src/libuvc/libuvc.h b/src/libuvc/libuvc.h index 30e66d6bce..29c24c5984 100644 --- a/src/libuvc/libuvc.h +++ b/src/libuvc/libuvc.h @@ -20,6 +20,11 @@ extern "C" { struct libusb_context; struct libusb_device_handle; + +#define UVC_AE_MODE_D0_MANUAL ( 1 << 0 ) +#define UVC_AE_MODE_D1_AUTO ( 1 << 1 ) +#define UVC_AE_MODE_D2_SP ( 1 << 2 ) +#define UVC_AE_MODE_D3_AP ( 1 << 3 ) /** UVC error types, based on libusb errors * @ingroup diag diff --git a/src/linux/backend-hid.cpp b/src/linux/backend-hid.cpp index bcdd26c0f7..d6085a680e 100644 --- a/src/linux/backend-hid.cpp +++ b/src/linux/backend-hid.cpp @@ -187,7 +187,15 @@ namespace librealsense hid_custom_sensor::~hid_custom_sensor() { - stop_capture(); + try + { + if (_is_capturing) + stop_capture(); + } + catch(...) + { + LOG_ERROR("An error has occurred while hid_custom_sensor dtor()!"); + } } std::vector hid_custom_sensor::get_report_data(const std::string& report_name, custom_sensor_report_field report_field) @@ -453,10 +461,14 @@ namespace librealsense iio_hid_sensor::~iio_hid_sensor() { - write_integer_to_param("buffer/enable", 0); - stop_capture(); + try + { + write_integer_to_param("buffer/enable", 0); + stop_capture(); - clear_buffer(); + clear_buffer(); + } + catch(...){} // clear inputs. _inputs.clear(); diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 036bb237bc..7d990141fc 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -999,7 +999,7 @@ namespace librealsense } else { - LOG_ERROR("Video frame arrived in idle mode."); // TODO - verification + LOG_INFO("Video frame arrived in idle mode."); // TODO - verification } } else diff --git a/src/media/playback/playback_sensor.cpp b/src/media/playback/playback_sensor.cpp index 5be3cd0260..22eea48592 100644 --- a/src/media/playback/playback_sensor.cpp +++ b/src/media/playback/playback_sensor.cpp @@ -128,26 +128,29 @@ notifications_callback_ptr playback_sensor::get_notifications_callback() const void playback_sensor::start(frame_callback_ptr callback) { LOG_DEBUG("Start sensor " << m_sensor_id); + std::lock_guard l(m_mutex); if (m_is_started == false) { started(m_sensor_id, callback); + m_user_callback = callback ; m_is_started = true; - m_user_callback = callback; } } void playback_sensor::stop(bool invoke_required) { LOG_DEBUG("Stop sensor " << m_sensor_id); + if (m_is_started == true) { - stopped(m_sensor_id, invoke_required); m_is_started = false; for (auto dispatcher : m_dispatchers) { dispatcher.second->stop(); } + std::lock_guard l(m_mutex); m_user_callback.reset(); + stopped(m_sensor_id, invoke_required); } } void playback_sensor::stop() diff --git a/src/media/playback/playback_sensor.h b/src/media/playback/playback_sensor.h index bd1275a279..ba8678ed89 100644 --- a/src/media/playback/playback_sensor.h +++ b/src/media/playback/playback_sensor.h @@ -104,6 +104,8 @@ namespace librealsense frame_interface* pframe = nullptr; std::swap((*pf).frame, pframe); + std::lock_guard l(m_mutex); + m_user_callback->on_frame((rs2_frame*)pframe); update_last_pushed_frame(); }; diff --git a/src/proc/align.cpp b/src/proc/align.cpp index b1b9a17b30..a6eb51e68a 100644 --- a/src/proc/align.cpp +++ b/src/proc/align.cpp @@ -359,6 +359,7 @@ namespace librealsense case 3: move_other_to_depth(z_pixels, reinterpret_cast*>(source), reinterpret_cast*>(dest), to, _pixel_top_left_int, bottom_right); + break; case 4: move_other_to_depth(z_pixels, reinterpret_cast*>(source), reinterpret_cast*>(dest), to, _pixel_top_left_int, bottom_right); @@ -536,7 +537,6 @@ namespace librealsense auto aligned_profile = std::make_shared(original_profile.clone(original_profile.stream_type(), original_profile.stream_index(), original_profile.format())); int aligned_unique_id = get_unique_id(original_profile, to_profile, *aligned_profile); aligned_profile->get()->profile->set_unique_id(aligned_unique_id); - environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*aligned_profile.get()->get()->profile, *original_profile.get()->profile); aligned_profile->get()->profile->set_framerate(original_profile.fps()); if (auto original_video_profile = As(original_profile.get()->profile)) { diff --git a/src/proc/colorizer.cpp b/src/proc/colorizer.cpp index 874a1a2620..4ae6250a49 100644 --- a/src/proc/colorizer.cpp +++ b/src/proc/colorizer.cpp @@ -124,7 +124,7 @@ namespace librealsense } }; colorizer::colorizer() - : _min(0.f), _max(6.f), _equalize(true), _stream() + : _min(0.f), _max(6.f), _equalize(true), _target_stream_profile() { _stream_filter.stream = RS2_STREAM_DEPTH; _stream_filter.format = RS2_FORMAT_Z16; @@ -196,10 +196,10 @@ namespace librealsense rs2::frame colorizer::process_frame(const rs2::frame_source& source, const rs2::frame& f) { - if (!_stream || (f.get_profile().get() != _stream->get())) + if (!_source_stream_profile || f.get_profile().unique_id() != _source_stream_profile.unique_id()) { - _stream = std::make_shared(f.get_profile().clone(RS2_STREAM_DEPTH, 0, RS2_FORMAT_RGB8)); - environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*_stream->get()->profile, *f.get_profile().get()->profile); + _source_stream_profile = f.get_profile(); + _target_stream_profile = f.get_profile().clone(RS2_STREAM_DEPTH, 0, RS2_FORMAT_RGB8); } auto make_equalized_histogram = [this](const rs2::video_frame& depth, rs2::video_frame rgb) { @@ -272,7 +272,7 @@ namespace librealsense auto vf = f.as(); //rs2_extension ext = f.is() ? RS2_EXTENSION_DISPARITY_FRAME : RS2_EXTENSION_DEPTH_FRAME; - ret = source.allocate_video_frame(*_stream, f, 3, vf.get_width(), vf.get_height(), vf.get_width() * 3, RS2_EXTENSION_VIDEO_FRAME); + ret = source.allocate_video_frame(_target_stream_profile, f, 3, vf.get_width(), vf.get_height(), vf.get_width() * 3, RS2_EXTENSION_VIDEO_FRAME); if (_equalize) make_equalized_histogram(f, ret); diff --git a/src/proc/colorizer.h b/src/proc/colorizer.h index 7f75f77f1b..4b143e83e4 100644 --- a/src/proc/colorizer.h +++ b/src/proc/colorizer.h @@ -107,6 +107,7 @@ namespace librealsense { std::vector _maps; int _map_index = 0; int _preset = 0; - std::shared_ptr _stream; + rs2::stream_profile _target_stream_profile; + rs2::stream_profile _source_stream_profile; }; } diff --git a/src/proc/decimation-filter.cpp b/src/proc/decimation-filter.cpp index 2d4ebb2586..6ef7c4bef5 100644 --- a/src/proc/decimation-filter.cpp +++ b/src/proc/decimation-filter.cpp @@ -6,11 +6,13 @@ #include #include +#include "environment.h" #include "option.h" #include "context.h" +#include "core/video.h" #include "proc/synthetic-stream.h" #include "proc/decimation-filter.h" -#include "environment.h" + #define PIX_SORT(a,b) { if ((a)>(b)) PIX_SWAP((a),(b)); } #define PIX_SWAP(a,b) { pixelvalue temp=(a);(a)=(b);(b)=temp; } @@ -273,7 +275,7 @@ namespace librealsense void decimation_filter::update_output_profile(const rs2::frame& f) { - if (_options_changed || (f.get_profile().get() != _source_stream_profile.get())) + if (_options_changed || !_source_stream_profile || f.get_profile().unique_id() != _source_stream_profile.unique_id()) { _options_changed = false; _source_stream_profile = f.get_profile(); @@ -303,7 +305,6 @@ namespace librealsense auto vp = _source_stream_profile.as(); auto tmp_profile = _source_stream_profile.clone(_source_stream_profile.stream_type(), _source_stream_profile.stream_index(), _source_stream_profile.format()); - environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*(stream_interface*)(_source_stream_profile.get()->profile), *(stream_interface*)(tmp_profile.get()->profile)); auto src_vspi = dynamic_cast(_source_stream_profile.get()->profile); auto tgt_vspi = dynamic_cast(tmp_profile.get()->profile); rs2_intrinsics src_intrin = src_vspi->get_intrinsics(); diff --git a/src/proc/disparity-transform.cpp b/src/proc/disparity-transform.cpp index b0afbea102..85e3c04f93 100644 --- a/src/proc/disparity-transform.cpp +++ b/src/proc/disparity-transform.cpp @@ -7,6 +7,7 @@ #include "option.h" #include "context.h" #include "ds5/ds5-private.h" +#include "core/video.h" #include "proc/synthetic-stream.h" #include "proc/disparity-transform.h" #include "software-device.h" @@ -93,7 +94,7 @@ namespace librealsense void disparity_transform::update_transformation_profile(const rs2::frame& f) { - if (f.get_profile().get() != _source_stream_profile.get()) + if(!_source_stream_profile || f.get_profile().unique_id() != _source_stream_profile.unique_id()) { _source_stream_profile = f.get_profile(); @@ -151,7 +152,6 @@ namespace librealsense { auto tgt_format = _transform_to_disparity ? RS2_FORMAT_DISPARITY32 : RS2_FORMAT_Z16; _target_stream_profile = _source_stream_profile.clone(RS2_STREAM_DEPTH, 0, tgt_format); - environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*(stream_interface*)(_source_stream_profile.get()->profile), *(stream_interface*)(_target_stream_profile.get()->profile)); auto src_vspi = dynamic_cast(_source_stream_profile.get()->profile); auto tgt_vspi = dynamic_cast(_target_stream_profile.get()->profile); rs2_intrinsics src_intrin = src_vspi->get_intrinsics(); diff --git a/src/proc/hole-filling-filter.cpp b/src/proc/hole-filling-filter.cpp index 8a2872bf38..84adaa26dd 100644 --- a/src/proc/hole-filling-filter.cpp +++ b/src/proc/hole-filling-filter.cpp @@ -70,15 +70,11 @@ namespace librealsense void hole_filling_filter::update_configuration(const rs2::frame& f) { - if (f.get_profile().get() != _source_stream_profile.get()) + if (!_source_stream_profile || f.get_profile().unique_id() != _source_stream_profile.unique_id()) { _source_stream_profile = f.get_profile(); _target_stream_profile = _source_stream_profile.clone(RS2_STREAM_DEPTH, 0, _source_stream_profile.format()); - environment::get_instance().get_extrinsics_graph().register_same_extrinsics( - *(stream_interface*)(f.get_profile().get()->profile), - *(stream_interface*)(_target_stream_profile.get()->profile)); - _extension_type = f.is() ? RS2_EXTENSION_DISPARITY_FRAME : RS2_EXTENSION_DEPTH_FRAME; _bpp = (_extension_type == RS2_EXTENSION_DISPARITY_FRAME) ? sizeof(float) : sizeof(uint16_t); auto vp = _target_stream_profile.as(); diff --git a/src/proc/pointcloud.cpp b/src/proc/pointcloud.cpp index 8e54fc9520..9ed0a0966c 100644 --- a/src/proc/pointcloud.cpp +++ b/src/proc/pointcloud.cpp @@ -52,23 +52,28 @@ namespace librealsense float2 pixel_to_texcoord(const rs2_intrinsics *intrin, const float2 & pixel) { return{ pixel.x / (intrin->width), pixel.y / (intrin->height) }; } float2 project_to_texcoord(const rs2_intrinsics *intrin, const float3 & point) { return pixel_to_texcoord(intrin, project(intrin, point)); } - bool pointcloud::stream_changed(const rs2::stream_profile& old, const rs2::stream_profile& curr) + void pointcloud::set_extrinsics() { - auto v_old = old.as(); - auto v_curr = curr.as(); - - return v_old.height() != v_curr.height(); + if (_output_stream && _other_stream && !_extrinsics) + { + rs2_extrinsics ex; + const rs2_stream_profile* ds = _output_stream; + const rs2_stream_profile* os = _other_stream.get_profile(); + if (environment::get_instance().get_extrinsics_graph().try_fetch_extrinsics( + *ds->profile, *os->profile, &ex)) + { + _extrinsics = ex; + } + } } void pointcloud::inspect_depth_frame(const rs2::frame& depth) { - if (!_output_stream.get() || _depth_stream->unique_id() != depth.get_profile().unique_id() || - stream_changed(*_depth_stream, depth.get_profile())) + if (!_output_stream || _depth_stream.get_profile().unique_id() != depth.get_profile().unique_id()) { - _output_stream = std::make_shared(depth.get_profile().as().clone( - RS2_STREAM_DEPTH, depth.get_profile().stream_index(), RS2_FORMAT_XYZ32F)); - _depth_stream = std::make_shared(depth.get_profile().as()); - environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*_output_stream->get()->profile, *depth.get_profile().get()->profile); + _output_stream = depth.get_profile().as().clone( + RS2_STREAM_DEPTH, depth.get_profile().stream_index(), RS2_FORMAT_XYZ32F); + _depth_stream = depth; _depth_intrinsics = optional_value(); _depth_units = optional_value(); _extrinsics = optional_value(); @@ -99,56 +104,34 @@ namespace librealsense found_depth_units = true; } - if (_other_stream && !_extrinsics) - { - rs2_extrinsics ex; - const rs2_stream_profile* d = _depth_stream->get(); - const rs2_stream_profile* o = _other_stream->get(); - if (environment::get_instance().get_extrinsics_graph().try_fetch_extrinsics( - *d->profile, *o->profile, &ex)) - { - _extrinsics = ex; - } - } + set_extrinsics(); } void pointcloud::inspect_other_frame(const rs2::frame& other) { if (_stream_filter != _prev_stream_filter) { - _other_stream = nullptr; _prev_stream_filter = _stream_filter; } - if (_extrinsics.has_value() && (_other_stream && other.get_profile().as() == *_other_stream.get())) + if (_extrinsics.has_value() && other.get_profile().unique_id() == _other_stream.get_profile().unique_id()) return; - auto osp = other.get_profile().get(); - _other_stream = std::make_shared(rs2::stream_profile(osp).as()); + _other_stream = other; _other_intrinsics = optional_value(); _extrinsics = optional_value(); if (!_other_intrinsics) { - if (auto video = _other_stream->as()) + auto stream_profile = _other_stream.get_profile(); + if (auto video = stream_profile.as()) { _other_intrinsics = video.get_intrinsics(); _occlusion_filter->set_texel_intrinsics(_other_intrinsics.value()); } } - if (_output_stream && !_extrinsics) - { - rs2_extrinsics ex; - const rs2_stream_profile* os = _output_stream->get(); - const rs2_stream_profile* of = other.get_profile().get(); - if (environment::get_instance().get_extrinsics_graph().try_fetch_extrinsics( - *os->profile, *of->profile, &ex - )) - { - _extrinsics = ex; - } - } + set_extrinsics(); } void pointcloud::pre_compute_x_y_map() @@ -416,7 +399,7 @@ namespace librealsense rs2::frame pointcloud::process_depth_frame(const rs2::frame_source& source, const rs2::depth_frame& depth) { - auto res = source.allocate_points(*_output_stream, depth); + auto res = source.allocate_points(_output_stream, depth); auto pframe = (librealsense::points*)(res.get()); auto depth_data = (const uint16_t*)depth.get_data(); @@ -463,8 +446,7 @@ namespace librealsense return res; } - pointcloud::pointcloud() : - _other_stream(nullptr) + pointcloud::pointcloud() { _occlusion_filter = std::make_shared(); @@ -536,16 +518,15 @@ namespace librealsense rs2::frame rv; if (auto composite = f.as()) { + auto texture = composite.first(_stream_filter.stream); + inspect_other_frame(texture); + auto depth = composite.first(RS2_STREAM_DEPTH, RS2_FORMAT_Z16); inspect_depth_frame(depth); rv = process_depth_frame(source, depth); - - auto texture = composite.first(_stream_filter.stream); - inspect_other_frame(texture); } else { - if (f.is()) { inspect_depth_frame(f); diff --git a/src/proc/pointcloud.h b/src/proc/pointcloud.h index 3837d45d09..a18d604c19 100644 --- a/src/proc/pointcloud.h +++ b/src/proc/pointcloud.h @@ -24,15 +24,14 @@ namespace librealsense // Intermediate translation table of (depth_x*depth_y) with actual texel coordinates per depth pixel std::vector _pixels_map; - std::shared_ptr _output_stream; - std::shared_ptr _other_stream; - std::shared_ptr _depth_stream; + rs2::stream_profile _output_stream; + rs2::frame _other_stream; + rs2::frame _depth_stream; void inspect_depth_frame(const rs2::frame& depth); void inspect_other_frame(const rs2::frame& other); rs2::frame process_depth_frame(const rs2::frame_source& source, const rs2::depth_frame& depth); - - bool stream_changed(const rs2::stream_profile& old, const rs2::stream_profile& curr); + void set_extrinsics(); std::vector _pre_compute_map_x; std::vector _pre_compute_map_y; diff --git a/src/proc/spatial-filter.cpp b/src/proc/spatial-filter.cpp index a1199f8a7d..d5f5716ee5 100644 --- a/src/proc/spatial-filter.cpp +++ b/src/proc/spatial-filter.cpp @@ -164,15 +164,11 @@ namespace librealsense void spatial_filter::update_configuration(const rs2::frame& f) { - if (f.get_profile().get() != _source_stream_profile.get()) + if (!_source_stream_profile || f.get_profile().unique_id() != _source_stream_profile.unique_id()) { _source_stream_profile = f.get_profile(); _target_stream_profile = _source_stream_profile.clone(RS2_STREAM_DEPTH, 0, _source_stream_profile.format()); - environment::get_instance().get_extrinsics_graph().register_same_extrinsics( - *(stream_interface*)(f.get_profile().get()->profile), - *(stream_interface*)(_target_stream_profile.get()->profile)); - _extension_type = f.is() ? RS2_EXTENSION_DISPARITY_FRAME : RS2_EXTENSION_DEPTH_FRAME; _bpp = (_extension_type == RS2_EXTENSION_DISPARITY_FRAME) ? sizeof(float) : sizeof(uint16_t); auto vp = _target_stream_profile.as(); diff --git a/src/proc/synthetic-stream.cpp b/src/proc/synthetic-stream.cpp index f323133587..f875c87cf8 100644 --- a/src/proc/synthetic-stream.cpp +++ b/src/proc/synthetic-stream.cpp @@ -74,6 +74,9 @@ namespace librealsense { results.push_back(res); } + //if frame was processed as frameset, don't process single frames + if (f.is()) + break; } } @@ -88,18 +91,46 @@ namespace librealsense rs2::frame generic_processing_block::prepare_output(const rs2::frame_source& source, rs2::frame input, std::vector results) { + // this function prepares the processing block output frame(s) by the following heuristic: + // in case the input is a single frame, return the processed frame. + // in case the input frame is a frameset, create an output frameset from the input frameset and the processed frame by the following heuristic: + // if one of the input frames has the same stream type and format as the processed frame, + // remove the input frame from the output frameset (i.e. temporal filter), otherwise kepp the input frame (i.e. colorizer). + // the only exception is in case one of the input frames is z16 or disparity and the result frame is disparity or z16 respectively, + // in this case the the input frmae will be removed. + if (results.empty()) { return input; } + + bool disparity_result_frame = false; + bool depth_result_frame = false; + + for (auto f : results) + { + auto format = f.get_profile().format(); + if (format == RS2_FORMAT_DISPARITY32 || format == RS2_FORMAT_DISPARITY16) + disparity_result_frame = true; + if (format == RS2_FORMAT_Z16) + depth_result_frame = true; + } + std::vector original_set; if (auto composite = input.as()) - composite.foreach([&original_set](const rs2::frame& frame) { original_set.push_back(frame); }); + composite.foreach([&](const rs2::frame& frame) + { + auto format = frame.get_profile().format(); + if (depth_result_frame && (format == RS2_FORMAT_DISPARITY32 || format == RS2_FORMAT_DISPARITY16)) + return; + if (disparity_result_frame && format == RS2_FORMAT_Z16) + return; + original_set.push_back(frame); + }); else { return results[0]; } - original_set.push_back(input); for (auto s : original_set) { diff --git a/src/proc/temporal-filter.cpp b/src/proc/temporal-filter.cpp index e603ebf0c7..31347dee8a 100644 --- a/src/proc/temporal-filter.cpp +++ b/src/proc/temporal-filter.cpp @@ -151,15 +151,11 @@ namespace librealsense void temporal_filter::update_configuration(const rs2::frame& f) { - if (f.get_profile().get() != _source_stream_profile.get()) + if (!_source_stream_profile || f.get_profile().unique_id() != _source_stream_profile.unique_id()) { _source_stream_profile = f.get_profile(); _target_stream_profile = _source_stream_profile.clone(RS2_STREAM_DEPTH, 0, _source_stream_profile.format()); - environment::get_instance().get_extrinsics_graph().register_same_extrinsics( - *(stream_interface*)(f.get_profile().get()->profile), - *(stream_interface*)(_target_stream_profile.get()->profile)); - //TODO - reject any frame other than depth/disparity _extension_type = f.is() ? RS2_EXTENSION_DISPARITY_FRAME : RS2_EXTENSION_DEPTH_FRAME; _bpp = (_extension_type == RS2_EXTENSION_DISPARITY_FRAME) ? sizeof(float) : sizeof(uint16_t); diff --git a/src/software-device.h b/src/software-device.h index 87d53b7c63..686a56ad2a 100644 --- a/src/software-device.h +++ b/src/software-device.h @@ -3,8 +3,6 @@ #pragma once #include "core/streaming.h" -#include "core/video.h" -#include "device.h" #include "device.h" #include "context.h" diff --git a/src/stream.h b/src/stream.h index 2d64397ffa..5d16cd641a 100644 --- a/src/stream.h +++ b/src/stream.h @@ -117,6 +117,7 @@ namespace librealsense std::function int_func = _calc_intrinsics; res->set_intrinsics([int_func]() { return int_func(); }); res->set_framerate(get_framerate()); + environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*res, *this); return res; } diff --git a/src/tm2/controller_event_serializer.h b/src/tm2/controller_event_serializer.h index c8ee0ae457..fe02e805bd 100644 --- a/src/tm2/controller_event_serializer.h +++ b/src/tm2/controller_event_serializer.h @@ -23,41 +23,6 @@ namespace librealsense return oss.str(); } - std::string get_string(perc::Status value) - { - -#define CASE_RETURN_STR(X) case perc::Status::##X: {\ - static std::string s##X##_str = make_less_screamy(#X);\ - return s##X##_str; } - - switch (value) - { - CASE_RETURN_STR(SUCCESS) - CASE_RETURN_STR(COMMON_ERROR) - CASE_RETURN_STR(FEATURE_UNSUPPORTED) - CASE_RETURN_STR(ERROR_PARAMETER_INVALID) - CASE_RETURN_STR(INIT_FAILED) - CASE_RETURN_STR(ALLOC_FAILED) - CASE_RETURN_STR(ERROR_USB_TRANSFER) - CASE_RETURN_STR(ERROR_EEPROM_VERIFY_FAIL) - CASE_RETURN_STR(ERROR_FW_INTERNAL) - CASE_RETURN_STR(BUFFER_TOO_SMALL) - CASE_RETURN_STR(NOT_SUPPORTED_BY_FW) - CASE_RETURN_STR(DEVICE_BUSY) - CASE_RETURN_STR(TIMEOUT) - CASE_RETURN_STR(TABLE_NOT_EXIST) - CASE_RETURN_STR(TABLE_LOCKED) - CASE_RETURN_STR(DEVICE_STOPPED) - CASE_RETURN_STR(TEMPERATURE_WARNING) - CASE_RETURN_STR(TEMPERATURE_STOP) - CASE_RETURN_STR(CRC_ERROR) - CASE_RETURN_STR(INCOMPATIBLE) - CASE_RETURN_STR(SLAM_NO_DICTIONARY) - default: return to_string() << "Unknown (" << (int)value << ")"; - } -#undef CASE_RETURN_STR - } - inline std::ostream& operator<<(std::ostream& os, const perc::TrackingData::Version& v) { return os << v.major << "." << v.minor << "." << v.patch << "." << v.build; @@ -95,7 +60,7 @@ namespace librealsense static std::string serialized_data(const perc::TrackingData::ControllerConnectedEventFrame& frame) { std::string serialized_data = to_string() << - "\"status\": \"" << get_string(frame.status) << "\"," + "\"status\": \"" << (int)frame.status << "\"," "\"controllerId\": " << (int)frame.controllerId << "," "\"manufacturerId\": " << (int)frame.manufacturerId << "," "\"protocol\": \"" << frame.protocol << "\"," diff --git a/src/tm2/tm-context.cpp b/src/tm2/tm-context.cpp index f1904ea238..1986a545a0 100644 --- a/src/tm2/tm-context.cpp +++ b/src/tm2/tm-context.cpp @@ -17,14 +17,25 @@ using namespace perc; namespace librealsense { tm2_context::tm2_context(context* ctx) - : _is_disposed(false), _t(&tm2_context::thread_proc, this), _ctx(ctx) + : _is_disposed(false), _ctx(ctx) + { + } + + void tm2_context::create_manager() { - _manager = std::shared_ptr(perc::TrackingManager::CreateInstance(this), - [](perc::TrackingManager* ptr) { perc::TrackingManager::ReleaseInstance(ptr); }); - if (_manager == nullptr) { - LOG_ERROR("Failed to create TrackingManager"); - throw std::runtime_error("Failed to create TrackingManager"); + std::lock_guard lock(_manager_mutex); + if (_manager == nullptr) + { + _t = std::thread(&tm2_context::thread_proc, this); + _manager = std::shared_ptr(perc::TrackingManager::CreateInstance(this), + [](perc::TrackingManager* ptr) { perc::TrackingManager::ReleaseInstance(ptr); }); + if (_manager == nullptr) + { + LOG_ERROR("Failed to create TrackingManager"); + throw std::runtime_error("Failed to create TrackingManager"); + } + } } auto version = _manager->version(); LOG_INFO("LibTm version 0x" << std::hex << version); @@ -48,7 +59,7 @@ namespace librealsense } - void tm2_context::onStateChanged(TrackingManager::EventType state, TrackingDevice* dev) + void tm2_context::onStateChanged(TrackingManager::EventType state, TrackingDevice* dev, TrackingData::DeviceInfo devInfo) { std::shared_ptr added; std::shared_ptr removed; @@ -73,9 +84,9 @@ namespace librealsense on_device_changed(removed, added); } - void tm2_context::onError(TrackingManager::Error error, TrackingDevice* dev) + void tm2_context::onError(Status error, TrackingDevice* dev) { - LOG_ERROR("Error occured while connecting device:" << dev << " Error: 0x" << std::hex << error); + LOG_ERROR("Error occured while connecting device:" << dev << " Error: 0x" << std::hex << static_cast(error)); } void tm2_context::thread_proc() @@ -84,7 +95,7 @@ namespace librealsense { if (!_manager) { - std::this_thread::yield(); + std::this_thread::sleep_for(std::chrono::microseconds(100)); continue; } _manager->handleEvents(); diff --git a/src/tm2/tm-context.h b/src/tm2/tm-context.h index 04b58e594e..b3bfda465c 100644 --- a/src/tm2/tm-context.h +++ b/src/tm2/tm-context.h @@ -17,17 +17,19 @@ namespace librealsense public: tm2_context(context* ctx); ~tm2_context(); + void create_manager(); std::shared_ptr get_manager() const; std::vector query_devices() const; signal, std::shared_ptr> on_device_changed; // TrackingManager::Listener - void onStateChanged(perc::TrackingManager::EventType state, perc::TrackingDevice*) override; - void onError(perc::TrackingManager::Error error, perc::TrackingDevice*) override; + void onStateChanged(perc::TrackingManager::EventType state, perc::TrackingDevice* device, perc::TrackingData::DeviceInfo deviceInfo) override; + void onError(perc::Status error, perc::TrackingDevice*) override; private: void thread_proc(); friend class connect_disconnect_listener; std::shared_ptr _listener; std::shared_ptr _manager; + std::mutex _manager_mutex; std::vector _devices; context* _ctx; diff --git a/src/tm2/tm-device.cpp b/src/tm2/tm-device.cpp index eddde9376d..33b786452b 100644 --- a/src/tm2/tm-device.cpp +++ b/src/tm2/tm-device.cpp @@ -33,6 +33,52 @@ namespace librealsense int64_t arrival_ts; }; + enum temperature_type { TEMPERATURE_TYPE_ASIC, TEMPERATURE_TYPE_MOTION}; + + class temperature_option : public readonly_option + { + public: + float query() const override + { + if (!is_enabled()) + throw wrong_api_call_sequence_exception("query option is allow only in streaming!"); + return _ep.get_temperature().sensor[_type].temperature; + } + + 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().sensor[type].threshold, 0, 0 }) { } + + private: + tm2_sensor & _ep; + option_range _range; + temperature_type _type; + }; + + class asic_temperature_option : public temperature_option + { + public: + const char* get_description() const override + { + return "Current TM2 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 TM2 Motion Module 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: @@ -162,14 +208,14 @@ namespace librealsense profile->set_format(convertTm2PixelFormat(tm_profile.profile.pixelFormat)); profile->set_framerate(p.fps); profile->set_unique_id(environment::get_instance().generate_stream_id()); - if (tm_profile.sensorIndex == 0) + if (tm_profile.sensorIndex == 0 || tm_profile.sensorIndex == 1) { - profile->make_default(); + profile->tag_profile(profile_tag::PROFILE_TAG_DEFAULT | profile_tag::PROFILE_TAG_SUPERSET); } stream_profile sp = { stream, profile->get_stream_index(), p.width, p.height, p.fps, profile->get_format() }; auto intrinsics = get_intrinsics(sp); profile->set_intrinsics([intrinsics]() { return intrinsics; }); - results.push_back(profile); + if (tm_profile.sensorIndex <= 1) results.push_back(profile); //TODO - need to register to have resolve_requests work //native_pixel_format pf; @@ -186,17 +232,17 @@ namespace librealsense rs2_format format = RS2_FORMAT_MOTION_XYZ32F; auto profile = std::make_shared(platform::stream_profile{ 0, 0, tm_profile.fps, static_cast(format) }); profile->set_stream_type(RS2_STREAM_GYRO); - profile->set_stream_index(tm_profile.sensorIndex + 1); // for nice presentation by the viewer - add 1 to stream index + profile->set_stream_index(tm_profile.sensorIndex); // for nice presentation by the viewer - add 1 to stream index profile->set_format(format); profile->set_framerate(tm_profile.fps); profile->set_unique_id(environment::get_instance().generate_stream_id()); + auto intrinsics = get_motion_intrinsics(*profile); + profile->set_intrinsics([intrinsics]() { return intrinsics; }); if (tm_profile.sensorIndex == 0) { - profile->make_default(); + profile->tag_profile(profile_tag::PROFILE_TAG_DEFAULT | profile_tag::PROFILE_TAG_SUPERSET); + results.push_back(profile); } - auto intrinsics = get_motion_intrinsics(*profile); - profile->set_intrinsics([intrinsics]() { return intrinsics; }); - results.push_back(profile); } //extract accelerometer profiles @@ -206,17 +252,17 @@ namespace librealsense rs2_format format = RS2_FORMAT_MOTION_XYZ32F; auto profile = std::make_shared(platform::stream_profile{ 0, 0, tm_profile.fps, static_cast(format) }); profile->set_stream_type(RS2_STREAM_ACCEL); - profile->set_stream_index(tm_profile.sensorIndex + 1); // for nice presentation by the viewer - add 1 to stream index + profile->set_stream_index(tm_profile.sensorIndex); // for nice presentation by the viewer - add 1 to stream index profile->set_format(format); profile->set_framerate(tm_profile.fps); profile->set_unique_id(environment::get_instance().generate_stream_id()); + auto intrinsics = get_motion_intrinsics(*profile); + profile->set_intrinsics([intrinsics]() { return intrinsics; }); if (tm_profile.sensorIndex == 0) { - profile->make_default(); + profile->tag_profile(profile_tag::PROFILE_TAG_DEFAULT | profile_tag::PROFILE_TAG_SUPERSET); + results.push_back(profile); } - auto intrinsics = get_motion_intrinsics(*profile); - profile->set_intrinsics([intrinsics]() { return intrinsics; }); - results.push_back(profile); } //extract 6Dof profiles - TODO @@ -233,17 +279,15 @@ namespace librealsense auto profile = std::make_shared(platform::stream_profile{ 0, 0, fps, static_cast(format) }); profile->set_stream_type(RS2_STREAM_POSE); // TM2_API - 6dof profile type enum behaves the same as stream index - profile->set_stream_index(static_cast(tm_profile.profileType) + 1); + profile->set_stream_index(static_cast(tm_profile.profileType)); profile->set_format(format); profile->set_framerate(fps); profile->set_unique_id(environment::get_instance().generate_stream_id()); if (tm_profile.profileType == SixDofProfile0) { - profile->make_default(); + profile->tag_profile(profile_tag::PROFILE_TAG_DEFAULT | profile_tag::PROFILE_TAG_SUPERSET); + results.push_back(profile); } - results.push_back(profile); - - //TODO - do I need to define native_pixel_format for RS2_STREAM_POSE? and how to draw it? } @@ -327,14 +371,14 @@ namespace librealsense for (auto&& r : requests) { auto sp = to_profile(r.get()); - int stream_index = sp.index - 1; + int stream_index = sp.index; switch (r->get_stream_type()) { case RS2_STREAM_FISHEYE: { + stream_index -= 1; // for multiple streams, the index starts from 1 //TODO: check bound for _tm_supported_profiles.___[] - auto tm_profile = _tm_supported_profiles.video[stream_index]; if (tm_profile.fps == sp.fps && tm_profile.profile.height == sp.height && @@ -382,6 +426,13 @@ namespace librealsense throw invalid_value_exception("Invalid stream type"); } } + + if (_tm_active_profiles.video[0].enabled == _tm_active_profiles.video[1].enabled && + _tm_active_profiles.video[0].outputEnabled != _tm_active_profiles.video[1].outputEnabled) + { + throw invalid_value_exception("Invalid profile configuration - setting a single FE stream is not supported"); + } + _is_opened = true; set_active_streams(requests); } @@ -630,7 +681,7 @@ namespace librealsense video_md.arrival_ts = tm_frame.arrivalTimeStamp; video_md.exposure_time = tm_frame.exposuretime; - frame_additional_data additional_data(ts_ms.count(), tm_frame.frameId, system_ts_ms.count(), sizeof(video_md), (uint8_t*)&video_md, tm_frame.arrivalTimeStamp); + frame_additional_data additional_data(ts_ms.count(), tm_frame.frameId, system_ts_ms.count(), sizeof(video_md), (uint8_t*)&video_md, tm_frame.arrivalTimeStamp, 0 ,0, false); // Find the frame stream profile std::shared_ptr profile = nullptr; @@ -684,7 +735,7 @@ namespace librealsense } float3 data = { tm_frame.acceleration.x, tm_frame.acceleration.y, tm_frame.acceleration.z }; - handle_imu_frame(tm_frame, tm_frame.frameId, RS2_STREAM_ACCEL, tm_frame.sensorIndex + 1, data, tm_frame.temperature); + handle_imu_frame(tm_frame, tm_frame.frameId, RS2_STREAM_ACCEL, tm_frame.sensorIndex, data, tm_frame.temperature); } void tm2_sensor::onGyroFrame(perc::TrackingData::GyroFrame& tm_frame) @@ -695,7 +746,7 @@ namespace librealsense return; } float3 data = { tm_frame.angularVelocity.x, tm_frame.angularVelocity.y, tm_frame.angularVelocity.z }; - handle_imu_frame(tm_frame, tm_frame.frameId, RS2_STREAM_GYRO, tm_frame.sensorIndex + 1, data, tm_frame.temperature); + handle_imu_frame(tm_frame, tm_frame.frameId, RS2_STREAM_GYRO, tm_frame.sensorIndex, data, tm_frame.temperature); } void tm2_sensor::onPoseFrame(perc::TrackingData::PoseFrame& tm_frame) @@ -714,7 +765,7 @@ namespace librealsense pose_frame_metadata frame_md = { 0 }; frame_md.arrival_ts = tm_frame.arrivalTimeStamp; - frame_additional_data additional_data(ts_ms.count(), frame_num++, system_ts_ms.count(), sizeof(frame_md), (uint8_t*)&frame_md, tm_frame.arrivalTimeStamp); + frame_additional_data additional_data(ts_ms.count(), frame_num++, system_ts_ms.count(), sizeof(frame_md), (uint8_t*)&frame_md, tm_frame.arrivalTimeStamp, 0, 0, false); // Find the frame stream profile std::shared_ptr profile = nullptr; @@ -723,7 +774,7 @@ namespace librealsense { //TODO - assuming single profile per motion stream if (p->get_stream_type() == RS2_STREAM_POSE && - p->get_stream_index() == tm_frame.sourceIndex + 1) + p->get_stream_index() == tm_frame.sourceIndex) { profile = p; break; @@ -789,7 +840,7 @@ namespace librealsense } else { - raise_error_notification(to_string() << "Connection to controller " << (int)frame.controllerId << " failed. (Statue: " << get_string(frame.status) << ")"); + raise_error_notification(to_string() << "Connection to controller " << (int)frame.controllerId << " failed."); } } @@ -822,7 +873,7 @@ namespace librealsense motion_md.arrival_ts = tm_frame_ts.arrivalTimeStamp; motion_md.temperature = temperature; - frame_additional_data additional_data(ts_ms.count(), frame_number, system_ts_ms.count(), sizeof(motion_md), (uint8_t*)&motion_md, tm_frame_ts.arrivalTimeStamp); + frame_additional_data additional_data(ts_ms.count(), frame_number, system_ts_ms.count(), sizeof(motion_md), (uint8_t*)&motion_md, tm_frame_ts.arrivalTimeStamp, 0, 0, false); // Find the frame stream profile std::shared_ptr profile = nullptr; @@ -886,7 +937,7 @@ namespace librealsense auto status = _tm_dev->ControllerConnect(c, controller_id); if (status != Status::SUCCESS) { - raise_error_notification(to_string() << "Failed to send connect to controller " << c.macAddress << "(Status: " << get_string(status) << ")"); + raise_error_notification(to_string() << "Failed to send connect to controller " << c.macAddress); } else { @@ -900,7 +951,7 @@ namespace librealsense perc::Status status = _tm_dev->ControllerDisconnect(id); if (status != Status::SUCCESS) { - raise_error_notification(to_string() << "Failed to disconnect to controller " << id << "(Status: " << get_string(status) << ")"); + raise_error_notification(to_string() << "Failed to disconnect to controller " << id); } else { @@ -911,6 +962,19 @@ namespace librealsense } } + TrackingData::Temperature tm2_sensor::get_temperature() + { + if (!_tm_dev) + throw wrong_api_call_sequence_exception("TM2 device is not available"); + TrackingData::Temperature temperature; + auto status = _tm_dev->GetTemperature(temperature); + if (status != Status::SUCCESS) + { + throw io_exception("Failed to query TM2 temperature option"); + } + return temperature; + } + /////////////// // Device @@ -931,7 +995,7 @@ namespace librealsense } register_info(RS2_CAMERA_INFO_NAME, tm2_device_name()); register_info(RS2_CAMERA_INFO_SERIAL_NUMBER, to_string() << info.serialNumber); - register_info(RS2_CAMERA_INFO_FIRMWARE_VERSION, to_string() << info.fw.major << "." << info.fw.minor << "." << info.fw.patch << "." << info.fw.build); + register_info(RS2_CAMERA_INFO_FIRMWARE_VERSION, to_string() << info.version.fw.major << "." << info.version.fw.minor << "." << info.version.fw.patch << "." << info.version.fw.build); register_info(RS2_CAMERA_INFO_PRODUCT_ID, to_string() << info.usbDescriptor.idProduct); std::string device_path = std::string("vid_") + std::to_string(info.usbDescriptor.idVendor) + @@ -942,12 +1006,16 @@ namespace librealsense _sensor = std::make_shared(this, dev); 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)); + //For manual testing: enable_loopback("C:\\dev\\recording\\tm2.bag"); } tm2_device::~tm2_device() { - for (auto&& d : get_context()->query_devices()) + for (auto&& d : get_context()->query_devices(RS2_PRODUCT_LINE_ANY_INTEL)) { for (auto&& tmd : d->get_device_data().tm2_devices) { diff --git a/src/tm2/tm-device.h b/src/tm2/tm-device.h index b5796f32ff..2afdcdbc81 100644 --- a/src/tm2/tm-device.h +++ b/src/tm2/tm-device.h @@ -6,6 +6,7 @@ #include #include "../device.h" +#include "../core/video.h" #include "../core/motion.h" #include "TrackingManager.h" #include "../media/playback/playback_device.h" @@ -28,6 +29,10 @@ namespace librealsense 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(); + }; private: static const char* tm2_device_name() { @@ -70,6 +75,8 @@ namespace librealsense void attach_controller(const std::array& mac_addr); void detach_controller(int id); void dispose(); + perc::TrackingData::Temperature get_temperature(); + private: void handle_imu_frame(perc::TrackingData::TimestampedData& 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); diff --git a/src/win/win-uvc.cpp b/src/win/win-uvc.cpp index b32e59d088..47d44429e0 100644 --- a/src/win/win-uvc.cpp +++ b/src/win/win-uvc.cpp @@ -491,7 +491,8 @@ namespace librealsense double d = val * 0.0001; double l = (d != 0) ? std::log2(d) : 1; long v = static_cast(std::roundl(l)); - assert(v <= 0 && v >= -8); + // Exposure values use logarithmic scale and can reach -13 with D400 + assert(v <= 0 && v >= -15); return v; } diff --git a/src/win7/drivers/IntelRealSense_D400_series_win7.inf b/src/win7/drivers/IntelRealSense_D400_series_win7.inf index c92c9528f7..0e78926e5f 100644 --- a/src/win7/drivers/IntelRealSense_D400_series_win7.inf +++ b/src/win7/drivers/IntelRealSense_D400_series_win7.inf @@ -45,7 +45,11 @@ DriverVer=12/29/2014,23.56.40.396 %DeviceNameHMD435% =USB_Install, USB\VID_8086&PID_0B07&MI_05 %DeviceNameDepthDS5U% =USB_Install, USB\VID_8086&PID_0B03&MI_00 %DeviceNameHMDS5U% =USB_Install, USB\VID_8086&PID_0B03&MI_03 +%DeviceNameDFU% =USB_Install, USB\VID_8086&PID_0ADB +%DeviceNameDepthSR300% =USB_Install, USB\VID_8086&PID_0AA5&MI_02 +%DeviceNameColorSR300% =USB_Install, USB\VID_8086&PID_0AA5&MI_00 +%DeviceNameHMSR300% =USB_Install, USB\VID_8086&PID_0AA5&MI_04 [Standard.NTamd64] %DeviceNameDepthD410% =USB_Install, USB\VID_8086&PID_0AD2&MI_00 @@ -60,6 +64,11 @@ DriverVer=12/29/2014,23.56.40.396 %DeviceNameHMD435% =USB_Install, USB\VID_8086&PID_0B07&MI_05 %DeviceNameDepthDS5U% =USB_Install, USB\VID_8086&PID_0B03&MI_00 %DeviceNameHMDS5U% =USB_Install, USB\VID_8086&PID_0B03&MI_03 +%DeviceNameDFU% =USB_Install, USB\VID_8086&PID_0ADB + +%DeviceNameDepthSR300% =USB_Install, USB\VID_8086&PID_0AA5&MI_02 +%DeviceNameColorSR300% =USB_Install, USB\VID_8086&PID_0AA5&MI_00 +%DeviceNameHMSR300% =USB_Install, USB\VID_8086&PID_0AA5&MI_04 ; ========== Class definition =========== @@ -145,6 +154,10 @@ DeviceNameDepthD435="Intel(R) RealSense(TM) Camera D435 Depth" DeviceNameHMD435="Intel(R) RealSense(TM) Camera D435 HW Monitor" DeviceNameDepthDS5U="Intel(R) RealSense(TM) Camera DS5U Depth" DeviceNameHMDS5U="Intel(R) RealSense(TM) Camera DS5U HW Monitor" +DeviceNameColorSR300="Intel(R) RealSense(TM) Camera SR300 Color" +DeviceNameDepthSR300="Intel(R) RealSense(TM) Camera SR300 Depth" +DeviceNameHMSR300="Intel(R) RealSense(TM) Camera SR300 HW Monitor" +DeviceNameDFU = "Intel(R) RealSense(TM) DFU" REG_MULTI_SZ = 0x00010000 diff --git a/third-party/libtm/.gitignore b/third-party/libtm/.gitignore new file mode 100644 index 0000000000..75b2b2760b --- /dev/null +++ b/third-party/libtm/.gitignore @@ -0,0 +1,26 @@ +output_build +*.user +.settings +*.jar +*.apk +*.iml +.gradle +/local.properties +/.idea/workspace.xml +/.idea/libraries +.DS_Store +/build +*-build/ +/captures +.externalNativeBuild +Version.h +/.vs +/versions.log +/resources/target.mvcmd +/resources/central_bl.bin +/resources/central_app.bin +/resources/remote_versions.log +/libtm/src/fw.h +/libtm/src/CentralBlFw.h +/libtm/src/CentralAppFw.h +/libtm/src/version.rc \ No newline at end of file diff --git a/third-party/libtm/CMakeLists.txt b/third-party/libtm/CMakeLists.txt new file mode 100644 index 0000000000..3200de513c --- /dev/null +++ b/third-party/libtm/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 2.8) +project(libtm) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + +set(LIBTM_ROOT "${CMAKE_CURRENT_SOURCE_DIR}") +set(LIBTM_INCLUDE_DIR "${LIBTM_ROOT}/libtm/include") +set(INFRA_INCLUDE_DIR "${LIBTM_ROOT}/libtm/src/infra") +set(LIBTM_SRC_DIR "${LIBTM_ROOT}/libtm/src") +set(LIBTM_RESOURCES_DIR "${LIBTM_ROOT}/resources") + +include(versions.cmake) + +add_definitions(-DBUILD_STATIC) + +# Build resources (FW, Central, Controller binaries) +add_subdirectory(resources) +add_subdirectory(libtm) +add_subdirectory(tools) + +message(STATUS "----------------------------------------------------------------------------") +message(STATUS "CMake Done") +message(STATUS "----------------------------------------------------------------------------") \ No newline at end of file diff --git a/third-party/libtm/libtm/CMakeLists.txt b/third-party/libtm/libtm/CMakeLists.txt new file mode 100644 index 0000000000..6da89fd37b --- /dev/null +++ b/third-party/libtm/libtm/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 2.8) +project(libtm) + +STRING(REGEX REPLACE "^([0-9]+)\\.[0-9]+\\.[0-9]+\\.[0-9]+" "\\1" LIBTM_VERSION_MAJOR "${HOST_VERSION}") +STRING(REGEX REPLACE "^[0-9]+\\.([0-9]+)\\.[0-9]+\\.[0-9]+" "\\1" LIBTM_VERSION_MINOR "${HOST_VERSION}") +STRING(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+)\\.[0-9]+" "\\1" LIBTM_VERSION_PATCH "${HOST_VERSION}") +STRING(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.[0-9]+\\.([0-9]+)" "\\1" LIBTM_VERSION_BUILD "${HOST_VERSION}") + +set (LIBTM_API_VERSION_MAJOR 10) # Major part of the device supported interface API version, updated upon an incompatible API change +set (LIBTM_API_VERSION_MINOR 0) # Minor part of the device supported interface API version, updated upon a backwards-compatible change + +set(LIBVERSION ${LIBTM_VERSION_MAJOR}.${LIBTM_VERSION_MINOR}.${LIBTM_VERSION_PATCH}) + +# Retrieve Git branch name +execute_process(COMMAND git rev-parse --abbrev-ref HEAD + WORKING_DIRECTORY "${LIBTM_ROOT}" + OUTPUT_VARIABLE GIT_BRANCH + OUTPUT_STRIP_TRAILING_WHITESPACE) + +message(STATUS "--------------------------------------------------------------------------------------------------------------------------------------------------------------") +message(STATUS "Building ${PROJECT_NAME} project on ${OS}, LIBTM version [${LIBTM_VERSION_MAJOR}.${LIBTM_VERSION_MINOR}.${LIBTM_VERSION_PATCH}.${LIBTM_VERSION_BUILD}], API version [${LIBTM_API_VERSION_MAJOR}.${LIBTM_API_VERSION_MINOR}], branch [${GIT_BRANCH}], FW [${FW_VERSION}], Central APP [${CENTRAL_APP_VERSION}], Central BL [${CENTRAL_BL_VERSION}]") + +# Configure version file according to libtm version definitions and branch name above +message(STATUS "Creating version file ${PROJECT_SOURCE_DIR}/src/Version.h") +configure_file("${PROJECT_SOURCE_DIR}/src/Version.h.in" "${PROJECT_SOURCE_DIR}/src/Version.h") +configure_file("${PROJECT_SOURCE_DIR}/src/version.rc.in" "${PROJECT_SOURCE_DIR}/src/version.rc") + +# Add subdirectory to the build +add_subdirectory(src) diff --git a/third-party/libtm/libtm/include/TrackingCommon.h b/third-party/libtm/libtm/include/TrackingCommon.h new file mode 100644 index 0000000000..504d52e17e --- /dev/null +++ b/third-party/libtm/libtm/include/TrackingCommon.h @@ -0,0 +1,142 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include + +#ifdef _WIN32 +#include +#ifdef tm_EXPORTS +#define DLL_EXPORT __declspec(dllexport) +#else +#ifdef BUILD_STATIC +#define DLL_EXPORT +#else +#define DLL_EXPORT __declspec(dllimport) +#endif +#endif /* tm_EXPORTS */ +#else /* defined (WIN32) */ +#define DLL_EXPORT +#endif + + +#define IN +#define OUT + +#ifdef __unix +#include +#include +#define fopen_s(pFile,filename,mode) ((*(pFile))=fopen((filename), (mode)))==NULL +#endif + +namespace perc { + +#ifdef _WIN32 + typedef HANDLE Handle; +#define ILLEGAL_HANDLE NULL +#else + typedef int Handle; +#define ILLEGAL_HANDLE (-1) + +#endif + + #define MAC_ADDRESS_SIZE 6 + #define CONTROLLER_SENSOR_DATA_SIZE 6 + + typedef uint8_t SensorId; + + /** + * @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, + Max + }; + + enum class Status + { + SUCCESS = 0, /**< No issues found */ + COMMON_ERROR = 1, /**< FW unknown message error */ + FEATURE_UNSUPPORTED = 2, /**< Feature is partly implemented and currently unsupported by host */ + ERROR_PARAMETER_INVALID = 3, /**< Invalid parameter */ + INIT_FAILED = 4, /**< Init flow failed (Loading FW, Identifying device) */ + ALLOC_FAILED = 5, /**< Allocation error */ + ERROR_USB_TRANSFER = 6, /**< USB Error while transmitting message to FW */ + ERROR_EEPROM_VERIFY_FAIL = 7, /**< EEPROM after write verification failed */ + ERROR_FW_INTERNAL = 8, /**< Internal FW error */ + BUFFER_TOO_SMALL = 9, /**< Buffer is too small for this operation */ + NOT_SUPPORTED_BY_FW = 10, /**< Feature is currently unsupported by FW */ + DEVICE_BUSY = 11, /**< Indicates that this command is not supported in the current device state, e.g.trying to change configuration after START */ + TIMEOUT = 12, /**< Controller connection failed due to timeout */ + TABLE_NOT_EXIST = 13, /**< The requested configuration table does not exists on the EEPROM */ + TABLE_LOCKED = 14, /**< The configuration table is locked for writing or permanently locked from unlocking */ + DEVICE_STOPPED = 15, /**< The last message over interrupt/stream endpoint after a DEV_STOP command */ + TEMPERATURE_WARNING = 16, /**< The device temperature reached 10 % from its threshold */ + TEMPERATURE_STOP = 17, /**< The device temperature reached its threshold, and the device stopped tracking */ + CRC_ERROR = 18, /**< CRC Error in firmware update */ + INCOMPATIBLE = 19, /**< Controller version is incompatible with TM2 version */ + SLAM_NO_DICTIONARY = 20, /**< No relocalization dictionary was loaded */ + NO_CALIBRATION_DATA = 21, /**< No calibration data is available */ + SLAM_LOCALIZATION_DATA_SET_SUCCESS = 22, /**< Reading all localization data chunks completed successfully */ + SLAM_ERROR_CODE_VISION = 23, /**< 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 = 24, /**< 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 = 25, /**< A fatal SLAM internal error has occurred */ + CONTROLLER_CALIBRATION_VALIDATION_FAILURE = 26, /**< Controller Calibration validation error */ + CONTROLLER_CALIBRATION_FLASH_ACCESS_FAILURE = 27, /**< Controller Flash access failure */ + CONTROLLER_CALIBRATION_IMU_FAILURE = 28, /**< Controller Calibration IMU failure */ + CONTROLLER_CALIBRATION_INTERNAL_FAILURE = 29, /**< Controller Calibration internal failure */ + AUTH_ERROR = 30, /**< Authentication error in firmware update or error in image signature */ + LIST_TOO_BIG = 31, /**< Image size is too big */ + DEVICE_RESET = 32, /**< A device reset has occurred. The user may read the FW log for additional details */ + NO_BLUETOOTH = 33, /**< The device doesn't have bluetooth, so the command failed */ + }; + + 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 interrupt rates + */ + typedef enum { + SIXDOF_INTERRUPT_RATE_NONE = 0X0000, /* No interrupts*/ + SIXDOF_INTERRUPT_RATE_FISHEYE = 0x0001, /* Interrupts on every fisheye camera update (30 interrupts per second for TM2) */ + SIXDOF_INTERRUPT_RATE_IMU = 0x0002, /* Interrupts on every IMU update (400-500 interrupts per second for TM2) - default value*/ + SIXDOF_INTERRUPT_RATE_MAX + } SIXDOF_INTERRUPT_RATE; + + /** + * @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_MAX + } SIXDOF_MODE; + +} diff --git a/third-party/libtm/libtm/include/TrackingData.h b/third-party/libtm/libtm/include/TrackingData.h new file mode 100644 index 0000000000..668064d90a --- /dev/null +++ b/third-party/libtm/libtm/include/TrackingData.h @@ -0,0 +1,873 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include "TrackingCommon.h" +#include +#include + +namespace perc +{ + #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) + #define MAX_VIDEO_STREAMS 8 + #define MAX_SUPPORTED_STREAMS 32 + #define MAX_USB_TREE_DEPTH 64 + #define MAX_LOCALIZATION_DATA_CHUNK_SIZE 1024 + #define MAX_CONFIGURATION_SIZE 1016 + #define MAX_VERSION 0xFFFF + #define MAX_LOG_BUFFER_ENTRIES 1024 + #define MAX_LOG_BUFFER_ENTRY_SIZE (256) + #define MAX_LOG_BUFFER_MODULE_SIZE (32) + + enum ProfileType + { + HMD = 0, + Controller1 = 1, + Controller2 = 2, + ProfileTypeMax = 3, + }; + + enum VideoProfileType + { + VideoProfile0 = 0, /* Sensor index 0 - HMD High exposure left camera */ + VideoProfile1 = 1, /* Sensor index 1 - HMD High exposure right camera */ + VideoProfile2 = 2, /* Sensor index 2 - HMD Low exposure left camera for controller 1 tracking */ + VideoProfile3 = 3, /* Sensor index 3 - HMD Low exposure right camera for controller 2 tracking */ + VideoProfileMax = 4, + }; + + enum AccelerometerProfileType + { + AccelerometerProfile0 = 0, /* Sensor index 0 - HMD Accelerometer */ + AccelerometerProfile1 = 1, /* Sensor index 1 - Controller 1 Accelerometer */ + AccelerometerProfile2 = 2, /* Sensor index 2 - Controller 2 Accelerometer */ + AccelerometerProfileMax = 3, + }; + + enum GyroProfileType + { + GyroProfile0 = 0, /* Sensor index 0 - HMD Gyro */ + GyroProfile1 = 1, /* Sensor index 1 - Controller 1 Gyro */ + GyroProfile2 = 2, /* Sensor index 2 - Controller 2 Gyro */ + GyroProfileMax = 3, + }; + + enum VelocimeterProfileType + { + VelocimeterProfile0 = 0, /* External Sensor index 0 - HMD Velocimeter */ + VelocimeterProfile1 = 1, /* External Sensor index 1 - HMD Velocimeter */ + VelocimeterProfileMax = 2, + }; + + enum SixDofProfileType + { + SixDofProfile0 = 0, /* Source index 0 - HMD 6dof */ + SixDofProfile1 = 1, /* Source index 1 - Controller 1 6dof */ + SixDofProfile2 = 2, /* Source index 2 - Controller 2 6dof */ + SixDofProfileMax = 3, + }; + + enum LogVerbosityLevel { + None = 0x0000, /**< Logging is disabled */ + Error = 0x0001, /**< Error conditions */ + Info = 0x0002, /**< High importance informational entries. this verbosity level exists to allow logging of general information on production images. */ + Warning = 0x0003, /**< Warning messages */ + Debug = 0x0004, /**< Debug informational entries */ + Verbose = 0x0005, /**< Even more informational entries */ + Trace = 0x0006 /**< On entry and exit from every function call. also shall be used for profiling */ + }; + + enum LogOutputMode { + LogOutputModeScreen = 0x0000, /**< All logs print to screen output */ + LogOutputModeBuffer = 0x0001, /**< All logs saved to buffer */ + LogOutputModeMax = 0x0002, + }; + + enum TemperatureSensorType { + TemperatureSensorVPU = 0x0000, /**< Temperature Sensor located in the Vision Processing Unit */ + TemperatureSensorIMU = 0x0001, /**< Temperature Sensor located in the Inertial Measurement Unit */ + TemperatureSensorBLE = 0x0002, /**< Temperature Sensor located in the Bluetooth Low Energy Unit */ + TemperatureSensorMax = 0x0003, + }; + + enum LockType { + HardwareLock = 0x0000, /**< The locking is done in hardware by locking the upper quarter of the EEPROM memory */ + SoftwareLock = 0x0001, /**< The locking is done in software by the firmware managing a lock bits in each configuration table metadata */ + MaxLockType = 0x0002, + }; + + enum EepromLockState { + LockStateWriteable = 0x0000, /**< The EEPROM is fully writeable */ + LockStateLocked = 0x0001, /**< The upper quarter of the EEPROM memory is write-protected */ + LockStatePermanentLocked = 0x0002, /**< The upper quarter of the EEPROM memory is permanently write-protected */ + LockStateMax = 0x0003, + }; + + enum AddressType { + AddressTypePublic = 0x0000, /**< Public Address - Static IEEE assigned Mac Address build from manufacturer assigned ID (24 bits) and unique device ID (24 bits) */ + AddressTypeRandom = 0x0001, /**< Random Address - Randomized temporary Address used for a certain amount of time (48 bits) */ + AddressTypeMax = 0x0002, /**< Unspecified Address */ + }; + + class TrackingData + { + public: + class Axis { + public: + Axis(float _x = 0.0, float _y = 0.0, float _z = 0.0) : x(_x), y(_y), z(_z) {} + void set(float _x, float _y, float _z) + { + x = _x; + y = _y; + z = _z; + } + float x; /**< X value, used for translation/acceleration/velocity */ + float y; /**< Y value, used for translation/acceleration/velocity */ + float z; /**< Z value, used for translation/acceleration/velocity */ + }; + + class EulerAngles { + public: + EulerAngles(float _x = 0.0, float _y = 0.0, float _z = 0.0) : x(_x), y(_y), z(_z) {} + void set(float _x, float _y, float _z) + { + x = _x; + y = _y; + z = _z; + } + float x; /**< X value, used for angular velocity/angular acceleration */ + float y; /**< Y value, used for angular velocity/angular acceleration */ + float z; /**< Z value, used for angular velocity/angular acceleration */ + }; + + class Quaternion { + public: + Quaternion(float _i = 0.0, float _j = 0.0, float _k = 0.0, float _r = 0.0) : i(_i), j(_j), k(_k), r(_r) {} + void set(float _i, float _j, float _k, float _r) + { + i = _i; + j = _j; + k = _k; + r = _r; + } + float i; /**< Qi components of rotation as represented in quaternion rotation */ + float j; /**< Qj components of rotation as represented in quaternion rotation */ + float k; /**< Qk components of rotation as represented in quaternion rotation */ + float r; /**< Qr components of rotation as represented in quaternion rotation */ + }; + + class Version { + public: + Version(uint32_t _major = MAX_VERSION, uint32_t _minor = MAX_VERSION, uint32_t _patch = MAX_VERSION, uint32_t _build = MAX_VERSION) { + major = _major; + minor = _minor; + patch = _patch; + build = _build; + } + + void set(uint32_t _major = MAX_VERSION, uint32_t _minor = MAX_VERSION, uint32_t _patch = MAX_VERSION, uint32_t _build = MAX_VERSION) { + major = _major; + minor = _minor; + patch = _patch; + build = _build; + } + + uint32_t major; /**< Major part of version */ + uint32_t minor; /**< Minor part of version */ + uint32_t patch; /**< Patch part of version */ + uint32_t build; /**< Build part of version */ + }; + + class TimestampedData + { + public: + TimestampedData() : timestamp(0), arrivalTimeStamp(0), systemTimestamp(0) {}; + int64_t timestamp; /**< Frame integration timestamp, as measured in nanoseconds since device initialization */ + int64_t arrivalTimeStamp; /**< Frame arrival timestamp, as measured in nanoseconds since device initialization */ + int64_t systemTimestamp; /**< Host correlated time stamp in nanoseconds */ + }; + + class PoseFrame : public TimestampedData { + public: + PoseFrame() : sourceIndex(0), trackerConfidence(0), mapperConfidence(0) {}; + uint8_t sourceIndex; /**< Index of HMD or controller - 0x0 = HMD, 0x1 - controller 1, 0x2 - controller 2 */ + Axis translation; /**< X, Y, Z values of translation, in meters (relative to initial position) */ + Axis velocity; /**< X, Y, Z values of velocity, in meter/sec */ + Axis acceleration; /**< X, Y, Z values of acceleration, in meter/sec^2 */ + Quaternion rotation; /**< Qi, Qj, Qk, Qr components of rotation as represented in quaternion rotation (relative to initial position) */ + EulerAngles angularVelocity; /**< X, Y, Z values of angular velocity, in radians/sec */ + EulerAngles angularAcceleration; /**< X, Y, Z values of angular acceleration, in radians/sec^2 */ + uint32_t trackerConfidence; /**< pose data confidence 0x0 - Failed, 0x1 - Low, 0x2 - Medium, 0x3 - High */ + uint32_t mapperConfidence; /**< pose data confidence 0x0 - Failed, 0x1 - Low, 0x2 - Medium, 0x3 - High */ + }; + + class GyroFrame : public TimestampedData { + public: + GyroFrame() : temperature(0), sensorIndex(0), frameId(0) {}; + uint8_t sensorIndex; /**< Zero based index of sensor with the same type within device */ + uint32_t frameId; /**< A running index of frames from every unique sensor, starting from 0 */ + Axis angularVelocity; /**< X, Y, Z values of gyro, in radians/sec */ + float temperature; /**< Gyro temperature */ + }; + + class VelocimeterFrame : public TimestampedData { + public: + VelocimeterFrame() : temperature(0), sensorIndex(0), frameId(0) {}; + uint8_t sensorIndex; /**< Zero based index of sensor with the same type within device */ + uint32_t frameId; /**< A running index of frames from every unique sensor, starting from 0 */ + Axis angularVelocity; /**< X, Y, Z values of velocimeter, in radians/sec */ + float temperature; /**< Velocimeter temperature */ + }; + + class AccelerometerFrame : public TimestampedData { + public: + AccelerometerFrame() : temperature(0), sensorIndex(0), frameId(0) {}; + uint8_t sensorIndex; /**< Zero based index of sensor with the same type within device */ + uint32_t frameId; /**< A running index of frames from every unique sensor, starting from 0 */ + Axis acceleration; /**< X, Y, Z values of acceleration, in meter/sec^2 */ + float temperature; /**< Accelerometer temperature */ + }; + + class ControllerFrame : public TimestampedData { + public: + ControllerFrame() : sensorIndex(0), frameId(0), eventId(0), instanceId(0), sensorData{0} {}; + uint8_t sensorIndex; /**< Zero based index of sensor with the same type within device */ + uint32_t frameId; /**< A running index of frames from every unique sensor, starting from 0 */ + uint8_t eventId; /**< Event ID - button, trackpad or battery (vendor specific), supported values 0-63 */ + uint8_t instanceId; /**< Instance of the sensor in case of multiple sensors */ + uint8_t sensorData[CONTROLLER_SENSOR_DATA_SIZE]; /**< Sensor data that is pass-through from the controller firmware */ + }; + + class RssiFrame : public TimestampedData { + public: + RssiFrame() : sensorIndex(0), frameId(0), signalStrength(0) {}; + uint8_t sensorIndex; /**< Zero based index of sensor with the same type within device */ + uint32_t frameId; /**< A running index of frames from every unique sensor, starting from 0 */ + float_t signalStrength; /**< Sampled signal strength (dB), a value closer to 0 indicates a stronger signal */ + }; + + class ControllerDiscoveryEventFrame : public TimestampedData { + public: + ControllerDiscoveryEventFrame() : macAddress{0}, addressType(AddressTypeRandom), manufacturerId(0), vendorData(0), protocol(), app(), softDevice(), bootLoader() {}; + uint8_t macAddress[MAC_ADDRESS_SIZE]; /**< Discovered device byte array of MAC address */ + AddressType addressType; /**< Discovered device address type: Public or Random */ + uint16_t manufacturerId; /**< Identifier of the controller manufacturer */ + uint16_t vendorData; /**< vendor specific data copied from the controller advertisement */ + Version protocol; /**< Controller BLE protocol version - only major part is active */ + Version app; /**< Controller App version - only major, minor, build parts are active */ + Version softDevice; /**< Controller Soft Device version - only major part is active */ + Version bootLoader; /**< Controller Boot Loader version - only major, minor, build parts are active */ + }; + + class ControllerConnectedEventFrame : public TimestampedData { + public: + ControllerConnectedEventFrame() : status(Status::SUCCESS), controllerId(0), manufacturerId(0), protocol(), app(), softDevice(), bootLoader() {}; + Status status; /**< Connection status: SUCCESS - connection succeeded */ + /**< TIMEOUT - connection timed out */ + /**< INCOMPATIBLE - connection succeeded but controller version is incompatible with TM2 version */ + uint8_t controllerId; /**< Disconnected controller identifier (1 or 2) */ + uint16_t manufacturerId; /**< Identifier of the controller manufacturer */ + Version protocol; /**< Controller BLE protocol version - only major part is active */ + Version app; /**< Controller App version - only major, minor, build parts are active */ + Version softDevice; /**< Controller Soft Device version - only major part is active */ + Version bootLoader; /**< Controller Boot Loader version - only major, minor, build parts are active */ + }; + + class ControllerDisconnectedEventFrame : public TimestampedData { + public: + ControllerDisconnectedEventFrame() : controllerId(0) {}; + uint8_t controllerId; /**< Disconnected controller identifier (1 or 2) */ + }; + + class ControllerCalibrationEventFrame : public TimestampedData { + public: + ControllerCalibrationEventFrame() : controllerId(0), status(Status::SUCCESS) {}; + uint8_t controllerId; /**< Calibrated controller identifier (1 or 2) */ + Status status; /**< Calibration status: SUCCESS - Controller was calibrated successfully */ + /**< CONTROLLER_CALIBRATION_VALIDATION_FAILURE - validation failure */ + /**< CONTROLLER_CALIBRATION_FLASH_ACCESS_FAILURE - flash access failure */ + /**< CONTROLLER_CALIBRATION_IMU_FAILURE - IMU failure */ + /**< CONTROLLER_CALIBRATION_INTERNAL_FAILURE - Internal failure */ + }; + + class ControllerFWEventFrame : public TimestampedData { + public: + ControllerFWEventFrame() : status(Status::SUCCESS), macAddress{0}, progress(0) {}; + Status status; /**< Progress status: SUCCESS - update progresses successfully */ + /**< CRC_ERROR - error in image integrity */ + /**< AUTH_ERROR - error in image signature */ + /**< LIST_TOO_BIG - image size too big */ + /**< TIMEOUT - operation did not complete due to device disconnect or timeout */ + uint8_t macAddress[MAC_ADDRESS_SIZE]; /**< MAC address of the updated device. All zeros for central FW update */ + uint8_t progress; /**< Progress counter (percentage of update complete) */ + }; + + class StatusEventFrame : public TimestampedData { + public: + StatusEventFrame() : status(Status::SUCCESS) {}; + Status status; /**< Status code */ + }; + + class ControllerLedEventFrame : public TimestampedData { + public: + uint8_t controllerId; /**< Controller identifier (1 or 2) */ + uint8_t ledId; /**< Led identifier (1 or 2) */ + uint32_t intensity; /**< Led intensity [0-100] */ + }; + + class RawProfile + { + public: + RawProfile() : width(0), height(0), stride(0), pixelFormat(PixelFormat::ANY) {} + void set(uint16_t _width, uint16_t _height, uint16_t _stride, PixelFormat _format) { + width = _width; + height = _height; + stride = _stride; + pixelFormat = _format; + } + uint16_t stride; /**< Length in bytes of each line in the image (including padding). 0 for non-camera streams. */ + uint16_t width; /**< Supported width (in pixels) of first stream, 0 for non-camera streams */ + uint16_t height; /**< Supported height (in pixels) or first stream, 0 for non-camera streams */ + PixelFormat pixelFormat; /**< Pixel format of the stream, according to enum PixelFormat */ + }; + + class VideoFrame : public TimestampedData { + public: + uint8_t sensorIndex; /**< Zero based index of sensor with the same type within device */ + uint32_t frameId; /**< Running index of frames from every unique sensor. Starting from 0. */ + RawProfile profile; /**< Frame format profile - includes width, height, stride, pixelFormat */ + uint32_t exposuretime; /**< Exposure time of this frame in microseconds */ + float_t gain; /**< Gain multiplier of this frame */ + uint32_t frameLength; /**< Length of frame below, in bytes, shall be equal to Stride X Height X BPP */ + const uint8_t* data; /**< Frame data pointer */ + }; + + class EnableConfig + { + public: + EnableConfig() : enabled(false), outputEnabled(false), fps(0), sensorIndex(0) {}; + void set(bool _enabled, bool _outputEnabled, uint16_t _fps, uint8_t _sensorIndex) { + enabled = _enabled; + outputEnabled = _outputEnabled; + fps = _fps; + sensorIndex = _sensorIndex; + } + bool enabled; /**< true if this profile is enabled */ + bool outputEnabled; /**< 0x0 - Send sensor outputs to the internal middlewares only, 0x1 - Send this sensor outputs also to the host over the USB interface. */ + uint16_t fps; /**< Supported frame per second for this profile */ + uint8_t sensorIndex; /**< Zero based index of sensor with the same type within device */ + }; + + class GyroProfile : public EnableConfig + { + + }; + + class VelocimeterProfile : public EnableConfig + { + + }; + + class AccelerometerProfile : public EnableConfig + { + + }; + + class SixDofProfile + { + public: + SixDofProfile() : enabled(false), mode(SIXDOF_MODE_ENABLE_MAPPING | SIXDOF_MODE_ENABLE_RELOCALIZATION), interruptRate(SIXDOF_INTERRUPT_RATE::SIXDOF_INTERRUPT_RATE_IMU), profileType(SixDofProfileMax) {}; + SixDofProfile(bool _enabled, uint8_t _mode, SIXDOF_INTERRUPT_RATE _interruptRate, SixDofProfileType _profileType) : + enabled(_enabled), mode(_mode), interruptRate(_interruptRate), profileType(_profileType) {}; + void set(bool _enabled, uint8_t _mode, SIXDOF_INTERRUPT_RATE _interruptRate, SixDofProfileType _profileType) + { + enabled = _enabled; + mode = _mode; + interruptRate = _interruptRate; + profileType = _profileType; + } + + bool enabled; /**< true if this profile is enabled */ + uint8_t mode; /**< 0x00 - Normal Mode, 0x01 - Fast Playback, 0x02 - Enable Mapping */ + SixDofProfileType profileType; /**< Type of this 6dof profile - HMD, Controller 1, Controller 2 */ + SIXDOF_INTERRUPT_RATE interruptRate; /**< 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 */ + }; + + class VideoProfile : public EnableConfig + { + public: + RawProfile profile; /**< Raw video profile */ + }; + + class Profile + { + public: + + Profile() : playbackEnabled(false) { + for (uint8_t i = 0; i < VideoProfileMax; i++) + { + video[i].outputEnabled = false; + video[i].enabled = false; + video[i].fps = 0; + video[i].sensorIndex = VideoProfileMax; + video[i].profile.set(0, 0, 0, ANY); + } + + for (uint8_t i = 0; i < GyroProfileMax; i++) + { + gyro[i].enabled = false; + gyro[i].outputEnabled = false; + gyro[i].fps = 0; + gyro[i].sensorIndex = GyroProfileMax; + } + + for (uint8_t i = 0; i < VelocimeterProfileMax; i++) + { + velocimeter[i].enabled = false; + velocimeter[i].outputEnabled = false; + velocimeter[i].fps = 0; + velocimeter[i].sensorIndex = VelocimeterProfileMax; + } + + for (uint8_t i = 0; i < AccelerometerProfileMax; i++) + { + accelerometer[i].enabled = false; + accelerometer[i].outputEnabled = false; + accelerometer[i].fps = 0; + accelerometer[i].sensorIndex = AccelerometerProfileMax; + } + + for (uint8_t i = 0; i < SixDofProfileMax; i++) + { + sixDof[i].enabled = false; + sixDof[i].mode = (SIXDOF_MODE_ENABLE_MAPPING | SIXDOF_MODE_ENABLE_RELOCALIZATION); + sixDof[i].interruptRate = SIXDOF_INTERRUPT_RATE_MAX; + sixDof[i].profileType = SixDofProfileMax; + } + }; + + void reset(void) { + + playbackEnabled = false; + + for (uint8_t i = 0; i < VideoProfileMax; i++) + { + video[i].outputEnabled = false; + video[i].enabled = false; + video[i].fps = 0; + video[i].sensorIndex = VideoProfileMax; + video[i].profile.set(0, 0, 0, ANY); + } + + for (uint8_t i = 0; i < GyroProfileMax; i++) + { + gyro[i].enabled = false; + gyro[i].outputEnabled = false; + gyro[i].fps = 0; + gyro[i].sensorIndex = GyroProfileMax; + } + + for (uint8_t i = 0; i < VelocimeterProfileMax; i++) + { + velocimeter[i].enabled = false; + velocimeter[i].outputEnabled = false; + velocimeter[i].fps = 0; + velocimeter[i].sensorIndex = VelocimeterProfileMax; + } + + for (uint8_t i = 0; i < AccelerometerProfileMax; i++) + { + accelerometer[i].enabled = false; + accelerometer[i].outputEnabled = false; + accelerometer[i].fps = 0; + accelerometer[i].sensorIndex = AccelerometerProfileMax; + } + + for (uint8_t i = 0; i < SixDofProfileMax; i++) + { + sixDof[i].enabled = false; + sixDof[i].mode = (SIXDOF_MODE_ENABLE_MAPPING | SIXDOF_MODE_ENABLE_RELOCALIZATION); + sixDof[i].interruptRate = SIXDOF_INTERRUPT_RATE_MAX; + sixDof[i].profileType = SixDofProfileMax; + } + }; + + void set(VideoProfile _video, bool _enabled, bool _outputEnabled) { + if (_video.sensorIndex < VideoProfileMax) + { + video[_video.sensorIndex] = _video; + video[_video.sensorIndex].enabled = _enabled; + video[_video.sensorIndex].outputEnabled = _outputEnabled; + } + } + + void set(GyroProfile _gyro, bool _enabled, bool _outputEnabled) { + if (_gyro.sensorIndex < GyroProfileMax) + { + gyro[_gyro.sensorIndex] = _gyro; + gyro[_gyro.sensorIndex].enabled = _enabled; + gyro[_gyro.sensorIndex].outputEnabled = _outputEnabled; + } + } + + void set(VelocimeterProfile _velocimeter, bool _enabled, bool _outputEnabled) { + if (_velocimeter.sensorIndex < VelocimeterProfileMax) + { + velocimeter[_velocimeter.sensorIndex] = _velocimeter; + velocimeter[_velocimeter.sensorIndex].enabled = _enabled; + velocimeter[_velocimeter.sensorIndex].outputEnabled = _outputEnabled; + } + } + + void set(AccelerometerProfile _accelerometer, bool _enabled, bool _outputEnabled) { + if (_accelerometer.sensorIndex < AccelerometerProfileMax) + { + accelerometer[_accelerometer.sensorIndex] = _accelerometer; + accelerometer[_accelerometer.sensorIndex].enabled = _enabled; + accelerometer[_accelerometer.sensorIndex].outputEnabled = _outputEnabled; + } + } + + void set(SixDofProfile _sixDof, bool _enabled) { + if (_sixDof.profileType < SixDofProfileMax) + { + sixDof[(uint8_t)_sixDof.profileType] = _sixDof; + sixDof[(uint8_t)_sixDof.profileType].enabled = _enabled; + } + } + + VideoProfile video[VideoProfileMax]; /**< All supported Video profiles according to sensor index */ + GyroProfile gyro[GyroProfileMax]; /**< All supported Gyro profiles according to sensor index */ + AccelerometerProfile accelerometer[AccelerometerProfileMax]; /**< All supported Accelerometer profiles according to sensor index */ + SixDofProfile sixDof[SixDofProfileMax]; /**< All supported SixDof profiles according to sensor profile (HMD, Controller1, Controlelr2) */ + VelocimeterProfile velocimeter[VelocimeterProfileMax]; /**< All supported Velocimeter profiles */ + bool playbackEnabled; /**< Enable video playback mode */ + }; + + class DeviceInfo + { + public: + class UsbConnectionDescriptor { + public: + UsbConnectionDescriptor() : idVendor(0), idProduct(0), bcdUSB(0), port(0), bus(0), portChainDepth(0), portChain{0} {} + uint16_t idVendor; /**< USB Vendor ID: DFU Device = 0x03E7, Device = 0x8087 */ + uint16_t idProduct; /**< USB Product ID: DFU Device = 0x2150, Device = 0x0AF3 */ + uint16_t bcdUSB; /**< USB specification release number: 0x100 = USB 1.0, 0x110 = USB 1.1, 0x200 = USB 2.0, 0x300 = USB 3.0 */ + uint8_t port; /**< Number of the port that the device is connected to */ + uint8_t bus; /**< Number of the bus that the device is connected to */ + uint8_t portChainDepth; /**< Number of ports in the port tree of this device */ + uint8_t portChain[MAX_USB_TREE_DEPTH]; /**< List of all port numbers from root for this device */ + }; + + class DeviceStatus { + public: + DeviceStatus() : hw(Status::SUCCESS), host(Status::SUCCESS) {} + Status host; /**< Host status */ + Status hw; /**< HW status */ + }; + + class DeviceVersion { + public: + DeviceVersion() {} + Version deviceInterface; /**< Device supported interface API version */ + Version host; /**< libtm Host version */ + Version fw; /**< Myriad firmware version */ + Version hw; /**< ASIC Board version: 0x00 = ES0, 0x01 = ES1, 0x02 = ES2, 0x03 = ES3, 0xFF = Unknown - only major part is active */ + Version centralApp; /**< Central firmware version */ + Version centralProtocol; /**< Central BLE protocol version - only major part is active */ + Version centralBootLoader; /**< Central BLE protocol version - only major, minor, patch parts are active */ + Version centralSoftDevice; /**< Central BLE protocol version - only major part is active */ + Version eeprom; /**< EEPROM data version - only major and minor parts are active */ + Version rom; /**< Myriad ROM version - only major part is active */ + }; + + DeviceInfo() : deviceType(0), serialNumber(0), numGyroProfile(0), numVelocimeterProfile(0), numAccelerometerProfiles(0), numVideoProfiles(0), eepromLockState(LockStateMax) {}; + UsbConnectionDescriptor usbDescriptor; /**< USB Connection Descriptor includes USB info and physical location */ + DeviceStatus status; /**< HW and Host status */ + DeviceVersion version; /**< HW, FW, Host, Central, EEPROM, ROM, interface versions */ + EepromLockState eepromLockState; /**< EEPROM Lock state */ + uint8_t deviceType; /**< Device identifier: 0x1 = T250 */ + uint64_t serialNumber; /**< Device serial number */ + uint8_t numGyroProfile; /**< Number of Gyro Supported Profiles returned by Supported RAW Streams */ + uint8_t numVelocimeterProfile; /**< Number of Velocimeter Supported Profiles returned by Supported RAW Streams */ + uint8_t numAccelerometerProfiles; /**< Number of Accelerometer Supported Profiles returned by Supported RAW Streams */ + uint8_t numVideoProfiles; /**< Number of Video Supported Profiles returned by Supported RAW Streams */ + }; + + class LogControl { + public: + LogControl() : verbosity(None), outputMode(LogOutputModeMax), rolloverMode(false) {}; + LogControl(LogVerbosityLevel _verbosity, LogOutputMode _OutputMode, bool _RolloverMode) { + verbosity = _verbosity; + outputMode = _OutputMode; + rolloverMode = _RolloverMode; + } + + LogVerbosityLevel verbosity; /**< Log Verbosity level */ + LogOutputMode outputMode; /**< Output mode - screen or buffer */ + bool rolloverMode; /**< Rollover mode - False: no rollover (logging will be paused after log is filled, Until cleared, only first logs will be stored), True: Last logs will be stored */ + }; + + class Log { + public: + class LocalTime { + public: + LocalTime() : year(0), month(0), dayOfWeek(0), day(0), hour(0), minute(0), second(0), milliseconds(0) {} + uint16_t year; /**< Year */ + uint16_t month; /**< Month */ + uint16_t dayOfWeek; /**< Day of week */ + uint16_t day; /**< Day */ + uint16_t hour; /**< Hour */ + uint16_t minute; /**< Minute */ + uint16_t second; /**< Second */ + uint16_t milliseconds; /**< Milliseconds */ + }; + + class LogEntry { + public: + + LogEntry() : timeStamp(0), localTimeStamp(), verbosity(None), deviceID(0), threadID(0), moduleID{0}, functionSymbol(0), lineNumber(0), payloadSize(0), payload{ 0 } {} + int64_t timeStamp; /**< Host log - Current timestamp relative to host initialization (nanosec) */ + /**< FW log - Current timestamp relative to device initialization (nanosec) */ + LocalTime localTimeStamp; /**< Host log - Current time of log */ + /**< FW log - Capture time of FW log */ + LogVerbosityLevel verbosity; /**< Verbosity level */ + uint64_t deviceID; /**< Device ID */ + uint32_t threadID; /**< Thread ID */ + char moduleID[MAX_LOG_BUFFER_MODULE_SIZE]; /**< Module ID */ + uint32_t functionSymbol; /**< Function symbol */ + uint32_t lineNumber; /**< Line number */ + uint32_t payloadSize; /**< payload size */ + char payload[MAX_LOG_BUFFER_ENTRY_SIZE]; /**< payload */ + }; + + Log() : entries(0), maxEntries(0), entry{} {} + LogEntry entry[MAX_LOG_BUFFER_ENTRIES]; /**< Log entry info and payload */ + uint32_t entries; /**< Active log entries in the buffer */ + uint32_t maxEntries; /**< Max entries (for GetFWLog call rate calculations) */ + }; + + class CameraIntrinsics { + public: + CameraIntrinsics() : width(0), height(0), ppx(0), ppy(0), fx(0), fy(0), distortionModel(0){ + memset(coeffs, 0, sizeof(coeffs)); + }; + + uint32_t width; /**< Width of the image in pixels */ + uint32_t height; /**< Height of the image in pixels */ + float_t ppx; /**< Horizontal coordinate of the principal point of the image, as a pixel offset from the left edge */ + float_t ppy; /**< Vertical coordinate of the principal point of the image, as a pixel offset from the top edge */ + float_t fx; /**< Focal length of the image plane, as a multiple of pixel width */ + float_t fy; /**< Focal length of the image plane, as a multiple of pixel Height */ + uint32_t distortionModel; /**< Distortion model of the image: NONE = 0, MODIFIED_BROWN_CONRADY = 1, INVERSE_BROWN_CONRADY = 2, FTHETA = 3, KANNALA_BRANDT4 = 4 */ + float_t coeffs[5]; /**< Distortion coefficients */ + }; + + class MotionIntrinsics { + public: + MotionIntrinsics() { + memset(data, 0, sizeof(data)); + memset(noiseVariances, 0, sizeof(noiseVariances)); + memset(biasVariances, 0, sizeof(biasVariances)); + }; + + float_t data[3][4]; /**< Scale matrix */ + float_t noiseVariances[3]; /**< Noise variances */ + float_t biasVariances[3]; /**< Bias variances */ + }; + + class SensorExtrinsics { + public: + SensorExtrinsics() : referenceSensorId(0) { + memset(rotation, 0, sizeof(rotation)); + memset(translation, 0, sizeof(translation)); + }; + + float_t rotation[9]; /**< Column-major 3x3 rotation matrix */ + float_t translation[3]; /**< 3 element translation vector, in meters */ + SensorId referenceSensorId; /**< Reference sensor uses for extrinsics calculation */ + }; + + class ExposureModeControl { + public: + ExposureModeControl() : antiFlickerMode(0) + { + memset(videoStreamAutoExposure, 0, sizeof(videoStreamAutoExposure)); + }; + + ExposureModeControl(uint8_t _bVideoStreamsAutoExposureMask, uint8_t _bAntiFlickerMode) + { + for (uint8_t i = 0; i < VideoProfileMax; i++) + { + videoStreamAutoExposure[i] = (bool)((_bVideoStreamsAutoExposureMask >> i) & 1); + } + + antiFlickerMode = _bAntiFlickerMode; + } + + bool videoStreamAutoExposure[VideoProfileMax]; /**< Video streams to apply the auto exposure configuration */ + uint8_t antiFlickerMode; /**< Anti Flicker Mode: 0 - disable (e.g. for outside use), 1 - 50Hz, 2 - 60Hz, 3 - Auto (currently not supported) */ + }; + + class Exposure { + public: + + class StreamExposure { + public: + StreamExposure() : cameraId(0), integrationTime(0), gain(0) {}; + StreamExposure(uint8_t _cameraId, uint32_t _integrationTime, float_t _gain) : cameraId(_cameraId), integrationTime(_integrationTime), gain(_gain) {}; + uint8_t cameraId; /**< 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 */ + uint32_t integrationTime; /**< Integration time in micro-seconds (Max 16000 uSec) */ + float_t gain; /**< Digital gain */ + }; + + Exposure() : numOfVideoStreams(0) { memset(stream, 0, sizeof(stream)); }; + uint8_t numOfVideoStreams; /**< Number of video streams to configure */ + StreamExposure stream[MAX_VIDEO_STREAMS]; /**< Stream exposure data variable sized array, according to wNumberOfStreams */ + }; + + class Temperature { + public: + class SensorTemperature { + public: + SensorTemperature() : index(TemperatureSensorMax), temperature(0.0), threshold(0.0) {}; + SensorTemperature(TemperatureSensorType _index, float_t _temperature, float_t _threshold) : index(_index), temperature(_temperature), threshold(_threshold) {}; + TemperatureSensorType index; /**< Temperature sensor index: 0x0 - VPU, 0x1 - IMU, 0x2 - BLE */ + float_t temperature; /**< Sensor temperature (Celsius) */ + float_t threshold; /**< Sensor temperature threshold (Celsius) */ + }; + + Temperature() : numOfSensors(0) { memset(sensor, 0, sizeof(sensor)); }; + uint32_t numOfSensors; /**< Number of temperature sensors */ + SensorTemperature sensor[TemperatureSensorMax]; /**< Stream exposure data variable sized array, according to wNumberOfStreams */ + }; + + class LocalizationDataFrame { + public: + LocalizationDataFrame() : moreData(false), chunkIndex(0), length(0), buffer(NULL) { }; + bool moreData; /**< moreData indicates there are more data to send. The last chunk (possibly even a zero-size chunk) shall be marked moreData = false */ + Status status; /**< Set localization status */ + uint32_t chunkIndex; /**< A running counter starting in 0 identifying the chunk in a single data stream */ + uint32_t length; /**< The length in bytes of the localization buffer */ + uint8_t* buffer; /**< Localization data buffer pointer of max size MAX_CONFIGURATION_SIZE bytes, Data format is algorithm specific and opaque to the USB and host stack */ + }; + + class RelativePose { + public: + RelativePose() {}; + Axis translation; /**< X, Y, Z values of translation, in meters (in the coordinate system of the tracker relative to the current position) */ + Quaternion rotation; /**< Qi, Qj, Qk, Qr components of rotation as represented in quaternion rotation (in the coordinate system of the tracker relative to the current position) */ + }; + + class GeoLocalization { + public: + GeoLocalization() : latitude(0), longitude(0), altitude(0) { }; + GeoLocalization(double_t _latitude, double_t _longitude, double_t _altitude) : latitude(_latitude), longitude(_longitude), altitude(_altitude) {}; + void set(double_t _latitude, double_t _longitude, double_t _altitude) + { + latitude = _latitude; + longitude = _longitude; + altitude = _altitude; + }; + double_t latitude; /**< Latitude in degrees */ + double_t longitude; /**< Longitude in degrees */ + double_t altitude; /**< Altitude in meters above the WGS 84 reference ellipsoid */ + }; + + class ControllerAssociatedDevices { + public: + ControllerAssociatedDevices() : macAddress1{0}, macAddress2{0}, addressType1(AddressTypeRandom), addressType2(AddressTypeRandom) {}; + ControllerAssociatedDevices(uint8_t* _macAddress1, uint8_t* _macAddress2 = nullptr, AddressType _addressType1 = AddressTypeRandom, AddressType _addressType2 = AddressTypeRandom) + { + set(_macAddress1, _macAddress2, _addressType1, _addressType2); + } + + void set(uint8_t* _macAddress1, uint8_t* _macAddress2 = nullptr, AddressType _addressType1 = AddressTypeRandom, AddressType _addressType2 = AddressTypeRandom) + { + copy(macAddress1, _macAddress1); + copy(macAddress2, _macAddress2); + addressType1 = _addressType1; + addressType2 = _addressType2; + } + + uint8_t macAddress1[MAC_ADDRESS_SIZE]; /**< MAC address of controller 1 */ + uint8_t macAddress2[MAC_ADDRESS_SIZE]; /**< MAC address of controller 2, set to all zeros if controller is not setup */ + AddressType addressType1; /**< Controller 1 Mac Address Type: Public or Random */ + AddressType addressType2; /**< Controller 2 Mac Address Type: Public or Random */ + + private: + /**< Copy source address to destination address if source is not NULL, else zero destination address */ + void copy(uint8_t* dstAddress, uint8_t* srcAddress) + { + for (uint8_t i = 0; i < MAC_ADDRESS_SIZE; i++) + { + dstAddress[i] = ((srcAddress == nullptr) ? 0 : srcAddress[i]); + } + } + }; + + class ControllerDeviceConnect { + public: + ControllerDeviceConnect() : macAddress{0}, timeout(0), addressType(AddressTypeRandom){}; + ControllerDeviceConnect(uint8_t* _macAddress, uint16_t _timeout, AddressType _addressType = AddressTypeRandom) : addressType(_addressType), timeout(_timeout) + { + for (uint8_t i = 0; i < MAC_ADDRESS_SIZE; i++) + { + macAddress[i] = _macAddress[i]; + } + } + + uint8_t macAddress[MAC_ADDRESS_SIZE]; /**< MAC address of controller */ + AddressType addressType; /**< Address Type: Public or Random */ + uint16_t timeout; /**< Controller connect timeout (msec) */ + }; + + class ControllerData { + public: + ControllerData() : controllerId (0), commandId(0), buffer(NULL), bufferSize(0){}; + ControllerData(uint8_t _controllerId, uint8_t _commandId, uint16_t _bufferSize, uint8_t* _buffer) : controllerId(_controllerId), commandId(_commandId), bufferSize(_bufferSize), buffer(_buffer) {}; + void set(uint8_t _controllerId, uint8_t _commandId, uint16_t _bufferSize, uint8_t* _buffer) + { + controllerId = _controllerId; + commandId = _commandId; + bufferSize = _bufferSize; + buffer = _buffer; + } + + uint8_t controllerId; /**< Controller identifier (1 or 2) */ + uint8_t commandId; /**< Command to be sent to the controller (vendor specific) - values 0-63 supported */ + uint16_t bufferSize; /**< Size of data buffer */ + uint8_t* buffer; /**< Controller data to be sent. Data format is vendor specific */ + }; + + class ControllerFW { + public: + ControllerFW() : macAddress{0}, image(NULL), imageSize(0), addressType(AddressTypeRandom) {}; + ControllerFW(uint8_t* _macAddress, uint16_t _imageSize, uint8_t* _image, AddressType _addressType = AddressTypeRandom) : addressType(_addressType), imageSize(_imageSize), image(_image) + { + if (_macAddress != NULL) + { + for (uint8_t i = 0; i < MAC_ADDRESS_SIZE; i++) + { + macAddress[i] = _macAddress[i]; + } + } + } + + uint8_t macAddress[MAC_ADDRESS_SIZE]; /**< MAC address of controller to be updated */ + AddressType addressType; /**< Discovered device address type: Public or Random */ + uint32_t imageSize; /**< Size of Controller FW image */ + uint8_t* image; /**< New controller FW image to be updated */ + }; + }; +} \ No newline at end of file diff --git a/third-party/libtm/libtm/include/TrackingDevice.h b/third-party/libtm/libtm/include/TrackingDevice.h new file mode 100644 index 0000000000..89d6afbd8c --- /dev/null +++ b/third-party/libtm/libtm/include/TrackingDevice.h @@ -0,0 +1,605 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include "TrackingCommon.h" +#include "TrackingData.h" + +#define IN +#define OUT + +namespace perc +{ + class DLL_EXPORT TrackingDevice + { + public: + class DLL_EXPORT Listener + { + public: + + /** + * @brief onPoseFrame + * The function will be called once TrackingDevice has a new pose data + * + * @param pose - Pose object containing the pose data + */ + virtual void onPoseFrame(OUT TrackingData::PoseFrame& pose) {} + + /** + * @brief onVideoFrame + * The function will be called once TrackingDevice has a new video (color / depth / fisheye) frame + * + * @param frame - VideoFrame object containing frame + */ + virtual void onVideoFrame(OUT TrackingData::VideoFrame& frame) {} + + /** + * @brief onAccelerometerFrame + * The function will be called once TrackingDevice has a new accelerometer data + * + * @param frame - Accelerometer object containing frame + */ + virtual void onAccelerometerFrame(OUT TrackingData::AccelerometerFrame& frame) {} + + /** + * @brief onGyroFrame + * The function will be called once TrackingDevice has a new gyro data + * + * @param frame - Gyro object containing frame + */ + virtual void onGyroFrame(OUT TrackingData::GyroFrame& frame) {} + + /** + * @brief onVelocimeterFrame + * The function will be called once TrackingDevice has a new velocimeter data + * + * @param frame - Velocimeter object containing frame + */ + virtual void onVelocimeterFrame(OUT TrackingData::VelocimeterFrame& frame) {} + + /** + * @brief onControllerDiscoveryEventFrame + * The function will be called once TrackingDevice has a new controller discovery event + * + * @param frame - Controller object containing frame + */ + virtual void onControllerDiscoveryEventFrame(OUT TrackingData::ControllerDiscoveryEventFrame& frame) {} + + /** + * @brief onControllerFrame + * The function will be called once TrackingDevice has a new controller frame + * + * @param frame - Controller object containing frame + */ + virtual void onControllerFrame(OUT TrackingData::ControllerFrame& frame) {} + + /** + * @brief onControllerConnectedEventFrame + * The function will be called once TrackingDevice has a new controller connected event + * + * @param frame - Controller object containing frame + */ + virtual void onControllerConnectedEventFrame(OUT TrackingData::ControllerConnectedEventFrame& frame) {} + + /** + * @brief onControllerDisconnectedEventFrame + * The function will be called once TrackingDevice has a new controller disconnected event + * + * @param frame - Controller object containing frame + */ + virtual void onControllerDisconnectedEventFrame(OUT TrackingData::ControllerDisconnectedEventFrame& frame) {} + + /** + * @brief onControllerCalibrationEventFrame + * The function will be called once the controller calibration process is finished + * + * @param frame - Controller object containing frame + */ + virtual void onControllerCalibrationEventFrame(OUT TrackingData::ControllerCalibrationEventFrame& frame) {} + + /** + * @brief onRssiFrame + * The function will be called once TrackingDevice has a new RSSI frame + * + * @param frame - Rssi object containing frame + */ + virtual void onRssiFrame(OUT TrackingData::RssiFrame& frame) {} + + /** + * @brief onLocalizationDataEventFrame + * The function will be called once TrackingDevice has a new localization data event + * + * @param frame - Localization Data object containing frame for Set or Get localization + * Get localization data - relevant fields: status, mapIndex, moreData, chunkIndex, length, buffer + * Set localization data - relevant fields: status, mapIndex + */ + virtual void onLocalizationDataEventFrame(OUT TrackingData::LocalizationDataFrame& frame) {} + + /** + * @brief onFWUpdateEvent + * The host interface shall support firmware update progress events + * + * @param frame - Firmware update event containing frame indicates the progress status + */ + virtual void onFWUpdateEvent(OUT TrackingData::ControllerFWEventFrame& frame) {} + + /** + * @brief onStatusEvent + * The host interface shall support general status event + * + * @param frame - Status event frame indicates the current status + */ + virtual void onStatusEvent(OUT TrackingData::StatusEventFrame& frame) {} + + /** + * @brief onControllerLedEvent + * + * @param frame - Controller Led event frame indicates the controller led intensity [0-100] + */ + virtual void onControllerLedEvent(OUT TrackingData::ControllerLedEventFrame& frame) {} + }; + + + public: + virtual ~TrackingDevice() {} + + /** + * @brief GetSupportedProfile + * Initialize basic profile according to all supported streams - adding only the first profile on every profile index + * Gyro: sensorIndex = all supported sensorIndex, fps = first supported gyro stream with this sensorIndex, enabled = false, outputEnabled = false + * Accelerometer: sensorIndex = all supported sensorIndex, fps = first supported accelerometer stream with this sensorIndex, enabled = false, outputEnabled = false + * Video: sensorIndex = all supported sensorIndex, fps/RawProfile = first supported video stream with this sensorIndex, enabled = false, outputEnabled = false + * SixDof: profileType = all profileTypes, rate = SIXDOF_INTERRUPT_RATE_IMU, enabled = false + * + * @return Status + */ + virtual Status GetSupportedProfile(OUT TrackingData::Profile& profile) = 0; + + /** + * @brief start + * Start streaming of all stream that were previously configured. + * @param Data listener + * @param includes all required configuration according to stream session. + * User shouldn't change running configuration during streaming + * TODO: review set/get functions + * @return Status + */ + virtual Status Start(IN Listener*, IN TrackingData::Profile* = NULL) = 0; + + /** + * @brief stop + * Stop streaming of all stream that were previously configured, all stream configuration parameters will be cleared. + * @return Status + */ + virtual Status Stop(void) = 0; + + /** + * @brief GetDeviceInfo + * Retrieve information on the TM2 device + * @param message - Device info message buffer + * @return Status + */ + virtual Status GetDeviceInfo(OUT TrackingData::DeviceInfo& info) = 0; + + /** + * @brief GetSupportedRawStreams + * Get all FW supported raw streams: Video, Gyro, Accelerometer. Velocimeter. + * @param videoProfiles - Video profile buffer containing all supported video streams (Buffer size should be according to DeviceInfo.numVideoProfiles) + * @param gyroProfiles - Gyro profile buffer containing all supported gyro streams (Buffer size should be according to DeviceInfo.numGyroProfiles) + * @param accelerometerProfiles - Accelerometer profile buffer containing all supported accelerometer streams (Buffer size should be according to DeviceInfo.numAccelerometerProfiles) + * @param velocimeterProfiles - Velocimeter profile buffer containing all supported velocimeter streams (Buffer size should be according to DeviceInfo.numVelocimeterProfile) + * @return Status + */ + virtual Status GetSupportedRawStreams(OUT TrackingData::VideoProfile* videoProfiles, OUT TrackingData::GyroProfile* gyroProfiles, OUT TrackingData::AccelerometerProfile* accelerometerProfiles, OUT TrackingData::VelocimeterProfile* velocimeterProfiles = nullptr) = 0; + + /** + * @brief SetFWLogControl + * Controls the FW logging parameters, such as verbosity and log mode. + * @param logControl - logControl object to fill with verbosity and log mode + * @return Status + */ + virtual Status SetFWLogControl(IN const TrackingData::LogControl& logControl) = 0; + + /** + * @brief GetFWLog + * Get FW log entries - every call to this function will get and clean all FW log entries + * Need to enable FW logs (SetFWLogControl) before calling GetFWLog + * Use entries and maxEntries fields to calculate the GetFWLog call rate to get max FW logs with min GetFWLog requests, + * For example: In case log entries is equal to log maxEntries, FW log buffer is completely filled so several FW logs are dumped, + * Increase the GetFWLog call rate to decrease the dumped logs amount. + * @param log - log object to fill with log entries + * @return Status + */ + virtual Status GetFWLog(OUT TrackingData::Log& log) = 0; + + /** + * @brief GetCameraIntrinsics + * Retrieves the intrinsic parameters of an individual camera in the device + * @param id - Sensor Id (Sensor type + sensor index) + * @param intrinsics - intrinsic object to fill + * @return Status + */ + virtual Status GetCameraIntrinsics(IN SensorId id, OUT TrackingData::CameraIntrinsics& intrinsics) = 0; + + /** + * @brief SetCameraIntrinsics + * Set the intrinsic parameters of an individual camera in the device + * @param id - Sensor Id (Sensor type + sensor index) + * @param message - intrinsic parameters to set + * @return Status + */ + virtual Status SetCameraIntrinsics(IN SensorId id, IN const TrackingData::CameraIntrinsics& intrinsics) = 0; + + /** + * @brief GetMotionModuleIntrinsics + * Retrieves the intrinsic parameters of an individual motion module in the device + * @param id - Sensor Id (Sensor type + sensor index) + * @param message - Buffer of intrinsic parameters + * @return Status + */ + virtual Status GetMotionModuleIntrinsics(IN SensorId id, OUT TrackingData::MotionIntrinsics& intrinsics) = 0; + + /** + * @brief SetMotionModuleIntrinsics + * Set the intrinsic parameters of an individual motion module in the device + * @param id - Sensor Id (Sensor type + sensor index) + * @param message - Buffer of intrinsic parameters + * @return Status + */ + virtual Status SetMotionModuleIntrinsics(IN SensorId id, IN const TrackingData::MotionIntrinsics& intrinsics) = 0; + + /** + * @brief GetExtrinsics + * Retrieves extrinsic pose of on individual sensor in the device relative to another one + * @param id - Sensor Id (Sensor type + sensor index) + * @param extrinsics - Output container for extrinsic parameters + * @return Status + */ + virtual Status GetExtrinsics(IN SensorId id, OUT TrackingData::SensorExtrinsics& extrinsics) = 0; + + /** + * @brief SetOccupancyMapControl + * 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 SetOccupancyMapControl command was called before the host calls to Start, the default value used shall be disabled. + * @param enable - Enable/Disable occupancy map calculation + * @return Status + */ + /* Currently not supported at FW side */ + //virtual Status SetOccupancyMapControl(uint8_t enable) = 0; + + /** + * @brief GetPose + * Retrieves the latest pose of the camera relative to its initial position + * @param pose - pose frame container + * @param sourceIndex - 0x0 = HMD, 0x1 - controller 1, 0x2 - controller 2 + * @return Status + */ + /* Currently not supported at FW side */ + //virtual Status GetPose(TrackingData::PoseFrame& pose, uint8_t sourceIndex) = 0; + + /** + * @brief SetExposureModeControl + * 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). + * @param mode - VideoStreamsMask and AntiFlickerMode + * @return Status + */ + virtual Status SetExposureModeControl(IN const TrackingData::ExposureModeControl& mode) = 0; + + /** + * @brief SetExposure + * Sets manual values for the video streams integration time and gain. + * @param exposure - Exposure data for all video streams + * @return Status + */ + virtual Status SetExposure(IN const TrackingData::Exposure& exposure) = 0; + + /** + * @brief GetTemperature + * Returns temperature and temperature threshold from all temperature sensors (VPU, IMU, BLE) + * @param temperature - Sensor temperature container + * @return Status + */ + virtual Status GetTemperature(OUT TrackingData::Temperature& temperature) = 0; + + /** + * @brief SetTemperatureThreshold + * 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. + * @param temperature - Temperature threshold to set per temperature sensors (relevant fields - index, threshold) + * @param token - Secured token to override max temperature threshold + * @return Status + */ + virtual Status SetTemperatureThreshold(IN const TrackingData::Temperature& temperature, IN uint32_t token) = 0; + + /** + * @brief LockConfiguration + * Write-protect the manufacturing configuration tables from future changes. + * This can be done in hardware by locking the upper quarter of the EEPROM memory, or by software by the firmware managing a lock bits in each configuration table metadata. + * @param type - Hardware or software. + * @param lock - False: unlock the configuration table. + * True: lock the configuration table. + * @return Status + */ + virtual Status LockConfiguration(IN LockType type, IN bool lock) = 0; + + /** + * @brief PermanentLockConfiguration + * Permanent Write-protect the manufacturing configuration tables from future changes. + * This can be done in hardware by locking the upper quarter of the EEPROM memory, or by software by the firmware managing a lock bits in each configuration table metadata. + * Warning - This is an irreversible action. + * @param type - Hardware or software. + * @param token - Secured token + * @return Status + */ + virtual Status PermanentLockConfiguration(IN LockType type, IN uint32_t token) = 0; + + /** + * @brief ReadConfiguration + * Reads a configuration table from the device memory. + * The host shall return UNSUPPORTED if it does not recognize the requested TableType. + * The host shall return TABLE_NOT_EXIST if it recognize the table type but no such table exists yet in the EEPROM. + * The host shall return BUFFER_TOO_SMALL if input buffer is too small for needed table type. + * @param configurationId - The ID of the requested configuration table. + * @param size - EEPROM read configuration buffer size (max value = MAX_CONFIGURATION_SIZE). + * @param buffer - EEPROM read configuration buffer pointer. + * @param actualSize - Actual EEPROM read configuration buffer size. + * @return Status + */ + virtual Status ReadConfiguration(IN uint16_t configurationId, IN uint16_t size, OUT uint8_t* buffer, OUT uint16_t* actualSize = nullptr) = 0; + + /** + * @brief WriteConfiguration + * 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 host shall return UNSUPPORTED if it does not recognize the requested TableType. + * The host 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. + * @param configurationId - The ID of the requested configuration table. + * @param size - EEPROM read configuration buffer size (max value = MAX_CONFIGURATION_SIZE). + * @param buffer - EEPROM read configuration buffer pointer. + * @return Status + */ + virtual Status WriteConfiguration(IN uint16_t configurationId, IN uint16_t size, IN uint8_t* buffer) = 0; + + /** + * @brief DeleteConfiguration + * Delete a configuration table from the internal EEPROM storage. + * @param configurationId - The ID of the requested configuration table. + * @return Status + */ + virtual Status DeleteConfiguration(IN uint16_t configurationId) = 0; + + /** + * @brief GetLocalizationData + * Returns 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" using callback onLocalizationDataEventFrame + * @param listener - Data listener + * @return Status + */ + virtual Status GetLocalizationData(IN Listener* listener) = 0; + + /** + * @brief SetLocalizationData + * Sets the localization data to be used to localize the 6DoF reports. + * Caution - this call may take several seconds. + * @param listener - Data listener + * @param length - The length in bytes of the localization buffer + * @param buffer - Localization data buffer (Max size MAX_CONFIGURATION_SIZE) + * @return Status + */ + virtual Status SetLocalizationData(IN Listener* listener, IN uint32_t length, IN const uint8_t* buffer) = 0; + + /** + * @brief ResetLocalizationData + * Resets the localization data + * @param flag - 0 - Reset all localization data tables, 1 - reset only the map by its mapIndex + * @return Status + */ + virtual Status ResetLocalizationData(IN uint8_t flag) = 0; + + /** + * @brief SetStaticNode + * Set a relative position of a static node + * @param guid - Unique name (Null-terminated C-string) for the static node, max length is 127 bytes plus one byte for the terminating null character + * Using guid that already exist in the map overrides the previous node + * @param relativePose - Static node relative pose data including translation and rotation + * @return Status + */ + virtual Status SetStaticNode(IN const char* guid, IN const TrackingData::RelativePose& relativePose) = 0; + + /** + * @brief GetStaticNode + * Get relative position of a static node + * @param guid - Unique name (Null-terminated C-string) for the static node, max length 127 bytes plus one byte for the terminating null character + * @param relativePose - Static node relative pose data including translation and rotation + * @return Status + */ + virtual Status GetStaticNode(IN const char* guid, OUT TrackingData::RelativePose& relativePose) = 0; + + /** + * @brief SetGeoLocation + * Sets the geographical location (e.g. GPS data). This data can be later used by the algorithms to correct IMU readings. + * @param geoLocation - latitude, longitude, altitude + * @return Status + */ + virtual Status SetGeoLocation(IN const TrackingData::GeoLocalization& geoLocation) = 0; + + /** + * @brief EepromRead + * Reads Eeprom from device + * @param offset - Offset from start of Eeprom + * @param size - Size of memory to read + * @param buffer - where to put the read data. Buffer should be at least of 'size' bytes + * @param actual - The actual number of bytes read from EEPROM. + * + * @return Status + */ + virtual Status EepromRead(IN uint16_t offset, IN uint16_t size, OUT uint8_t* buffer, OUT uint16_t& actual) = 0; + + /** + * @brief EepromWrite + * Writes Eeprom from device + * @param offset - Offset from start of Eeprom + * @param size - Size of memory to write + * @param buffer - A buffer to be written to the eeprom. + * @param actual - The actual number of bytes written to EEPROM. The fucntions returns this value to user. + * @param verify - Should the written data be verified afterwards using EepromRead calls (will take more time) + * + * @return Status + */ + virtual Status EepromWrite(IN uint16_t offset, IN uint16_t size, IN uint8_t* buffer, OUT uint16_t& actual, IN bool verify = false) = 0; + + /** + * @brief Reset + * Resets the device and loads FW + * Caution - this function is non blocking, need to sleep at least 2 seconds afterwards to let the FW load again + * + * @return Status + */ + virtual Status Reset(void) = 0; + + /** + * @brief SendFrame + * Sends video frame to the device + * @param frame - Video frame + * + * @return Status + */ + virtual Status SendFrame(IN const TrackingData::VideoFrame& frame) = 0; + + /** + * @brief SendFrame + * Sends Gyro frame to the device + * @param frame - Gyro frame + * + * @return Status + */ + virtual Status SendFrame(IN const TrackingData::GyroFrame& frame) = 0; + + /** + * @brief SendFrame + * Sends Velocimeter frame to the device + * @param frame - Velocimeter frame + * + * @return Status + */ + virtual Status SendFrame(IN const TrackingData::VelocimeterFrame& frame) = 0; + + /** + * @brief SendFrame + * Sends Accelerometer frame to the device + * @param frame - Accelerometer frame + * + * @return Status + */ + virtual Status SendFrame(IN const TrackingData::AccelerometerFrame& frame) = 0; + + /** + * @brief ControllerConnect + * Connect Controller to the device, + * If the device is already connected to the same MAC address, or if there are no controllers left to connect, libtm shall return a Status::DEVICE_BUSY + * Otherwise, libtm shall reply immediately, and the reply contains an assigned controller ID. + * This is not an indication that the connect succeeded, rather only that the connect started. + * libtm will later send a status of the connection on onControllerConnectedEventFrame. + * @param device - macAddress (Input) - MAC address of the controller to be connected + * timeout (Input) - Connect timeout (msec) + * @param controllerId - controllerId (Output) - controllerId (1 or 2) assigned by the device + * + * @return Status + */ + virtual Status ControllerConnect(IN const TrackingData::ControllerDeviceConnect& device, OUT uint8_t& controllerId) = 0; + + /** + * @brief ControllerDisconnect + * Disconnect Controller from the device + * Disconnection completion indication will be done upon onControllerDisconnectedEventFrame + * @param controllerId - Controller identifier (1 or 2) + * + * @return Status + */ + virtual Status ControllerDisconnect(IN uint8_t controllerId) = 0; + + /** + * @brief ControllerStartCalibration + * Start the controller calibration process, + * When the controller receives the indication to start calibration, it starts reading its gyroscope output for 30 seconds. + * The controller averages the readings over the first 25 seconds and uses this value as the gyro bias. + * The controller then averages the readings over the next 5 seconds and compares the value to the gyro bias from the first 25 seconds. + * If the values are the same (up to TBD noise value), then the gyro bias is stored to flash and the controller sends the onControllerCalibrationEventFrame with success. + * Otherwise, the controller reports a failure. + * @param controllerId - controllerId (Output) - controllerId (1 or 2) + * + * @return Status + */ + virtual Status ControllerStartCalibration(IN uint8_t controllerId) = 0; + + /** + * @brief GetAssociatedDevices + * Get Controller associated devices from the EEPROM + * @param devices - buffer to fill associated devices + * + * @return Status + */ + virtual Status GetAssociatedDevices(OUT TrackingData::ControllerAssociatedDevices& devices) = 0; + + /** + * @brief SetAssociatedDevices + * Set Controller associated devices to the EEPROM + * @param devices - buffers to take associated devices + * + * @return Status + */ + virtual Status SetAssociatedDevices(IN const TrackingData::ControllerAssociatedDevices& devices) = 0; + + /** + * @brief ControllerSendData + * Sends opaque data to the controller. The vendor's controller code is responsible for interpreting the data. + * Example data could be a command to set a LED on the controller or set a vibration pattern on a haptic device. + * Controller must be connected before sending data. + * @param controllerData - data to send to the controller + * + * @return Status + */ + virtual Status ControllerSendData(IN const TrackingData::ControllerData& controllerData) = 0; + + /** + * @brief ControllerRssiTestControl + * Start or Stop the RSSI test + * @param controllerId - Controller identifier (1 or 2) + * @param testControl - True = start the test, False = stop the test + * + * @return Status + */ + virtual Status ControllerRssiTestControl(IN uint8_t controllerId, IN bool testControl) = 0; + + /** + * @brief SetGpioControl + * Enable manufacturing tools to directly control the following GPIOs - 74, 75, 76 & 77. + * @param gpioControl - Setting each bit to 0 will toggle the corresponding GPIO low, Setting to 1 will toggle it to high. + * Bit 0: GPIO 74, Bit 1: GPIO 75, Bit 2: GPIO 76, Bit 3: GPIO 77, Bits 4-7: Reserved + * + * @return Status + */ + virtual Status SetGpioControl(IN uint8_t gpioControl) = 0; + + /** + * @brief ControllerFWUpdate + + * Updates the FW image on a connected controller device. + * @param ControllerFW - Controller FW image to update + * + * @return Status + */ + virtual Status ControllerFWUpdate(const TrackingData::ControllerFW& fw) = 0; + }; +} diff --git a/third-party/libtm/libtm/include/TrackingManager.h b/third-party/libtm/libtm/include/TrackingManager.h new file mode 100644 index 0000000000..a890104837 --- /dev/null +++ b/third-party/libtm/libtm/include/TrackingManager.h @@ -0,0 +1,50 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include "TrackingCommon.h" +#include "TrackingDevice.h" +#include "TrackingData.h" + +namespace perc +{ + class DLL_EXPORT TrackingManager + { + public: + enum EventType + { + ATTACH = 0, + DETACH + }; + + class Listener + { + public: + // state : enum ATTACH, DETACH.... + virtual void onStateChanged(EventType state, TrackingDevice* device, TrackingData::DeviceInfo deviceInfo) = 0; + virtual void onError(Status, TrackingDevice* device)= 0; + }; + + public: + // factory + static TrackingManager* CreateInstance(Listener*, void* param = 0); + // release existing manager and clean the pointer + static void ReleaseInstance(TrackingManager*& manager); + // interface + virtual Handle getHandle() = 0; + virtual Status handleEvents(bool blocking = true) = 0; + virtual size_t getDeviceList(TrackingDevice** list, unsigned int maxListSize) = 0; + virtual Status setHostLogControl(IN const TrackingData::LogControl& logControl) = 0; + virtual Status getHostLog(OUT TrackingData::Log* log) = 0; + + /** + * libtm version + * MAJOR = 0xFF00000000000000 + * MINOR = 0x00FF000000000000 + * PATCH = 0x0000FFFF00000000 + * Build = 0x00000000FFFFFFFF + */ + virtual uint64_t version() = 0; + virtual ~TrackingManager() {} + }; +} diff --git a/third-party/libtm/libtm/src/.bdsignore b/third-party/libtm/libtm/src/.bdsignore new file mode 100644 index 0000000000..d17d0eea09 --- /dev/null +++ b/third-party/libtm/libtm/src/.bdsignore @@ -0,0 +1 @@ +fw.h \ No newline at end of file diff --git a/third-party/libtm/libtm/src/CMakeLists.txt b/third-party/libtm/libtm/src/CMakeLists.txt new file mode 100644 index 0000000000..146a68549f --- /dev/null +++ b/third-party/libtm/libtm/src/CMakeLists.txt @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 2.8) +cmake_policy(SET CMP0015 NEW) + +project(tm) + +include_directories(${LIBUSB_LOCAL_INCLUDE_PATH} + ${LIBTM_INCLUDE_DIR} + ${INFRA_INCLUDE_DIR} +) + +IF(WIN32) + set(OS "win") + include_directories(${LIBUSB_LOCAL_INCLUDE_PATH}/libusb) +ELSE() + set(OS "lin") +ENDIF(WIN32) + +#Source Files +set(SOURCE_FILES + ${LIBTM_INCLUDE_DIR}/TrackingManager.h + ${LIBTM_INCLUDE_DIR}/TrackingDevice.h + ${LIBTM_INCLUDE_DIR}/TrackingCommon.h + ${LIBTM_INCLUDE_DIR}/TrackingData.h + + Manager.h + Manager.cpp + Device.h + Device.cpp + Message.h + UsbPlugListener.h + UsbPlugListener.cpp + CompleteTask.h + Common.h + Common.cpp + + infra/Log.h + infra/Log.cpp + infra/Event.h + infra/Fence.h + infra/EventHandler.h + infra/Poller.h + infra/Poller_${OS}.cpp + infra/Dispatcher.h + infra/Dispatcher.cpp + infra/Fsm.h + infra/Fsm.cpp + infra/Utils.h + infra/Utils.cpp + infra/Semaphore.h + infra/Semaphore_${OS}.cpp + infra/Event_${OS}.cpp +) + +#Add versioning to DLL +IF(WIN32) + set(SOURCE_FILES "${SOURCE_FILES};version.rc") +ENDIF(WIN32) + +#Building Library +set(SDK_LIB_TYPE "STATIC") +add_definitions(-DBUILD_STATIC) + +message(STATUS "Building project ${PROJECT_NAME} as ${SDK_LIB_TYPE} library ${CMAKE_INSTALL_LIBDIR}") +add_library(${PROJECT_NAME} ${SDK_LIB_TYPE} ${SOURCE_FILES}) + +#LINK_LIBRARIES +target_link_libraries(${PROJECT_NAME} + ${OS_SPECIFIC_LIBS} + ${LIBUSB1_LIBRARIES} +) + +set_target_properties(${PROJECT_NAME} PROPERTIES VERSION "${LIBVERSION}" SOVERSION "${LIBTM_VERSION_MAJOR}") +set_target_properties (${PROJECT_NAME} PROPERTIES FOLDER Library) + +set(CMAKECONFIG_INSTALL_DIR "${CMAKE_INSTALL_LIBDIR}/../cmake/realsense2") + +install(TARGETS ${PROJECT_NAME} + EXPORT realsense2Targets + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} +) + diff --git a/third-party/libtm/libtm/src/Common.cpp b/third-party/libtm/libtm/src/Common.cpp new file mode 100644 index 0000000000..579a276478 --- /dev/null +++ b/third-party/libtm/libtm/src/Common.cpp @@ -0,0 +1,377 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#include +#include +#include "Common.h" +#include "Message.h" +#include "TrackingData.h" +#include "Utils.h" + +using namespace perc; + +/* Description: This function prints data in hexadecimal format */ +/* Parameters: IN data - data to print */ +/* IN data_len - data length */ +/* Returns: Void */ +void print_data(IN unsigned char * data, IN unsigned int data_len) +{ + unsigned int i; + for (i = 0; i < data_len; i++) + { + printf("0x%X ", data[i]); + if (i % 16 == 15) + { + printf("\n"); + } + } + printf("\n\n"); +} + +perc::Status fwToHostStatus(perc::MESSAGE_STATUS status) +{ + switch (status) + { + case MESSAGE_STATUS::INTERNAL_ERROR: return perc::Status::ERROR_FW_INTERNAL; + case MESSAGE_STATUS::SUCCESS: return perc::Status::SUCCESS; + case MESSAGE_STATUS::MORE_DATA_AVAILABLE: return perc::Status::SUCCESS; + case MESSAGE_STATUS::UNKNOWN_MESSAGE_ID: return Status::NOT_SUPPORTED_BY_FW; + case MESSAGE_STATUS::INVALID_PARAMETER: return Status::ERROR_PARAMETER_INVALID; + case MESSAGE_STATUS::DEVICE_BUSY: return Status::DEVICE_BUSY; + case MESSAGE_STATUS::TIMEOUT: return Status::TIMEOUT; + case MESSAGE_STATUS::TABLE_NOT_EXIST: return Status::TABLE_NOT_EXIST; + case MESSAGE_STATUS::TABLE_LOCKED: return Status::TABLE_LOCKED; + case MESSAGE_STATUS::DEVICE_STOPPED: return Status::DEVICE_STOPPED; + case MESSAGE_STATUS::TEMPERATURE_WARNING: return Status::TEMPERATURE_WARNING; + case MESSAGE_STATUS::TEMPERATURE_STOP: return Status::TEMPERATURE_STOP; + case MESSAGE_STATUS::CRC_ERROR: return Status::CRC_ERROR; + case MESSAGE_STATUS::INCOMPATIBLE: return Status::INCOMPATIBLE; + case MESSAGE_STATUS::SLAM_NO_DICTIONARY: return Status::SLAM_NO_DICTIONARY; + case MESSAGE_STATUS::AUTH_ERROR: return Status::AUTH_ERROR; + case MESSAGE_STATUS::DEVICE_RESET: return Status::DEVICE_RESET; + case MESSAGE_STATUS::LIST_TOO_BIG: return Status::LIST_TOO_BIG; + case MESSAGE_STATUS::NO_BLUETOOTH: return Status::NO_BLUETOOTH; + default: + return perc::Status::COMMON_ERROR; + } +} + + +perc::Status slamToHostStatus(perc::SLAM_STATUS_CODE status) +{ + switch (status) + { + case SLAM_STATUS_CODE::SLAM_STATUS_CODE_SUCCESS: return perc::Status::SUCCESS; + case SLAM_STATUS_CODE::SLAM_STATUS_CODE_LOCALIZATION_DATA_SET_SUCCESS: return perc::Status::SLAM_LOCALIZATION_DATA_SET_SUCCESS; + default: + return perc::Status::COMMON_ERROR; + } +} + +perc::Status slamErrorToHostStatus(perc::SLAM_ERROR_CODE error) +{ + switch (error) + { + case SLAM_ERROR_CODE::SLAM_ERROR_CODE_NONE: return perc::Status::SUCCESS; + case SLAM_ERROR_CODE::SLAM_ERROR_CODE_VISION: return perc::Status::SLAM_ERROR_CODE_VISION; + case SLAM_ERROR_CODE::SLAM_ERROR_CODE_SPEED: return perc::Status::SLAM_ERROR_CODE_SPEED; + case SLAM_ERROR_CODE::SLAM_ERROR_CODE_OTHER: return perc::Status::SLAM_ERROR_CODE_OTHER; + default: + return perc::Status::COMMON_ERROR; + } +} + +perc::Status controllerCalibrationToHostStatus(perc::CONTROLLER_CALIBRATION_STATUS_CODE status) +{ + switch (status) + { + case CONTROLLER_CALIBRATION_STATUS_CODE::CONTROLLER_CALIBRATION_STATUS_SUCCEEDED: return perc::Status::SUCCESS; + case CONTROLLER_CALIBRATION_STATUS_CODE::CONTROLLER_CALIBRATION_STATUS_VALIDATION_FAILURE: return perc::Status::CONTROLLER_CALIBRATION_VALIDATION_FAILURE; + case CONTROLLER_CALIBRATION_STATUS_CODE::CONTROLLER_CALIBRATION_STATUS_FLASH_ACCESS_FAILURE: return perc::Status::CONTROLLER_CALIBRATION_FLASH_ACCESS_FAILURE; + case CONTROLLER_CALIBRATION_STATUS_CODE::CONTROLLER_CALIBRATION_STATUS_IMU_FAILURE: return perc::Status::CONTROLLER_CALIBRATION_IMU_FAILURE; + case CONTROLLER_CALIBRATION_STATUS_CODE::CONTROLLER_CALIBRATION_STATUS_INTERNAL_FAILURE: return perc::Status::CONTROLLER_CALIBRATION_INTERNAL_FAILURE; + default: return perc::Status::CONTROLLER_CALIBRATION_INTERNAL_FAILURE; + } +} + +TrackingData::PoseFrame poseMessageToClass(perc::pose_data msg, uint8_t sourceIndex, nsecs_t systemTime) +{ + TrackingData::PoseFrame pose; + + /* Make sure systemTime doesn't go backward */ + static nsecs_t prevSystemTime = 0; + if (systemTime < prevSystemTime) + { + systemTime = prevSystemTime; + } + else + { + prevSystemTime = systemTime; + } + + pose.sourceIndex = sourceIndex; + pose.acceleration.set(msg.flAx, msg.flAy, msg.flAz); + pose.angularAcceleration.set(msg.flAAX, msg.flAAY, msg.flAAZ); + pose.angularVelocity.set(msg.flVAX, msg.flVAY, msg.flVAZ); + pose.rotation.set(msg.flQi, msg.flQj, msg.flQk, msg.flQr); + pose.timestamp = msg.llNanoseconds; + pose.systemTimestamp = systemTime; + pose.translation.set(msg.flX, msg.flY, msg.flZ); + pose.velocity.set(msg.flVx, msg.flVy, msg.flVz); + pose.trackerConfidence = msg.dwTrackerConfidence; + pose.mapperConfidence = msg.dwMapperConfidence; + + return pose; +} + +TrackingData::CameraIntrinsics cameraIntrinsicsMessageToClass(perc::camera_intrinsics msg) +{ + TrackingData::CameraIntrinsics intrinsics; + + intrinsics.width = msg.dwWidth; + intrinsics.height = msg.dwHeight; + intrinsics.ppx = msg.flPpx; + intrinsics.ppy = msg.flPpy; + intrinsics.fx = msg.flFx; + intrinsics.fy = msg.flFy; + intrinsics.distortionModel = msg.dwDistortionModel; + for (uint8_t i = 0; i < 5; i++) + { + intrinsics.coeffs[i] = msg.flCoeffs[i]; + } + + return intrinsics; +} + +TrackingData::MotionIntrinsics motionIntrinsicsMessageToClass(perc::motion_intrinsics msg) +{ + TrackingData::MotionIntrinsics intrinsics; + + for (uint8_t i = 0; i < 3; i++) + { + for (uint8_t j = 0; j < 4; j++) + { + intrinsics.data[i][j] = msg.flData[i][j]; + } + } + + for (uint8_t i = 0; i < 3; i++) + { + intrinsics.noiseVariances[i] = msg.flNoiseVariances[i]; + } + + for (uint8_t i = 0; i < 3; i++) + { + intrinsics.biasVariances[i] = msg.flBiasVariances[i]; + } + + return intrinsics; +} + +motion_intrinsics motionIntrinsicsClassToMessage(perc::TrackingData::MotionIntrinsics intrinsics) +{ + motion_intrinsics msg; + + for (uint8_t i = 0; i < 3; i++) + { + for (uint8_t j = 0; j < 4; j++) + { + msg.flData[i][j] = intrinsics.data[i][j]; + } + } + + for (uint8_t i = 0; i < 3; i++) + { + msg.flNoiseVariances[i] = intrinsics.noiseVariances[i]; + } + + for (uint8_t i = 0; i < 3; i++) + { + msg.flBiasVariances[i] = intrinsics.biasVariances[i]; + } + + return msg; +} + +TrackingData::SensorExtrinsics sensorExtrinsicsMessageToClass(perc::sensor_extrinsics msg) +{ + TrackingData::SensorExtrinsics extrinsics; + + for (uint8_t i = 0; i < 9; i++) + { + extrinsics.rotation[i] = msg.flRotation[i]; + } + + for (uint8_t i = 0; i < 3; i++) + { + extrinsics.translation[i] = msg.flTranslation[i]; + } + + extrinsics.referenceSensorId = msg.bReferenceSensorID; + + return extrinsics; +} + + +std::string messageCodeToString(libusb_transfer_type type, uint16_t code) +{ + switch (type) + { + case LIBUSB_TRANSFER_TYPE_BULK: + switch (code) + { + case DEV_GET_DEVICE_INFO: return "DEV_GET_DEVICE_INFO"; + case DEV_GET_TIME: return "DEV_GET_TIME"; + case DEV_GET_AND_CLEAR_EVENT_LOG: return "DEV_GET_AND_CLEAR_EVENT_LOG"; + case DEV_GET_SUPPORTED_RAW_STREAMS: return "DEV_GET_SUPPORTED_RAW_STREAMS"; + case DEV_RAW_STREAMS_CONTROL: return "DEV_RAW_STREAMS_CONTROL"; + case DEV_GET_CAMERA_INTRINSICS: return "DEV_GET_CAMERA_INTRINSICS"; + case DEV_GET_MOTION_INTRINSICS: return "DEV_GET_MOTION_INTRINSICS"; + case DEV_GET_EXTRINSICS: return "DEV_GET_EXTRINSICS"; + case DEV_SET_CAMERA_INTRINSICS: return "DEV_SET_CAMERA_INTRINSICS"; + case DEV_SET_MOTION_INTRINSICS: return "DEV_SET_MOTION_INTRINSICS"; + case DEV_SET_EXTRINSICS: return "DEV_SET_EXTRINSICS"; + case DEV_LOG_CONTROL: return "DEV_LOG_CONTROL"; + case DEV_STREAM_CONFIG: return "DEV_STREAM_CONFIG"; + case DEV_RAW_STREAMS_PLAYBACK_CONTROL: return "DEV_RAW_STREAMS_PLAYBACK_CONTROL"; + case DEV_READ_EEPROM: return "DEV_READ_EEPROM"; + case DEV_WRITE_EEPROM: return "DEV_WRITE_EEPROM"; + case DEV_SAMPLE: return "DEV_SAMPLE"; + case DEV_START: return "DEV_START"; + case DEV_STOP: return "DEV_STOP"; + case DEV_STATUS: return "DEV_STATUS"; + case DEV_GET_POSE: return "DEV_GET_POSE"; + case DEV_EXPOSURE_MODE_CONTROL: return "DEV_EXPOSURE_MODE_CONTROL"; + case DEV_SET_EXPOSURE: return "DEV_SET_EXPOSURE"; + case DEV_GET_TEMPERATURE: return "DEV_GET_TEMPERATURE"; + case DEV_SET_TEMPERATURE_THRESHOLD: return "DEV_SET_TEMPERATURE_THRESHOLD"; + case DEV_SET_GEO_LOCATION: return "DEV_SET_GEO_LOCATION"; + case DEV_FLUSH: return "DEV_FLUSH"; + case DEV_FIRMWARE_UPDATE: return "DEV_FIRMWARE_UPDATE"; + case DEV_GPIO_CONTROL: return "DEV_GPIO_CONTROL"; + case DEV_TIMEOUT_CONFIGURATION: return "DEV_TIMEOUT_CONFIGURATION"; + case DEV_SNAPSHOT: return "DEV_SNAPSHOT"; + case DEV_READ_CONFIGURATION: return "DEV_READ_CONFIGURATION"; + case DEV_WRITE_CONFIGURATION: return "DEV_WRITE_CONFIGURATION"; + case DEV_RESET_CONFIGURATION: return "DEV_RESET_CONFIGURATION"; + case DEV_LOCK_CONFIGURATION: return "DEV_LOCK_CONFIGURATION"; + case DEV_LOCK_EEPROM: return "DEV_LOCK_EEPROM"; + case SLAM_STATUS: return "SLAM_STATUS"; + case SLAM_GET_OCCUPANCY_MAP_TILES: return "SLAM_GET_OCCUPANCY_MAP_TILES"; + case SLAM_GET_LOCALIZATION_DATA: return "SLAM_GET_LOCALIZATION_DATA"; + case SLAM_SET_LOCALIZATION_DATA_STREAM: return "SLAM_SET_LOCALIZATION_DATA_STREAM"; + case SLAM_SET_6DOF_INTERRUPT_RATE: return "SLAM_SET_6DOF_INTERRUPT_RATE"; + case SLAM_6DOF_CONTROL: return "SLAM_6DOF_CONTROL"; + case SLAM_OCCUPANCY_MAP_CONTROL: return "SLAM_OCCUPANCY_MAP_CONTROL"; + case SLAM_RESET_LOCALIZATION_DATA: return "SLAM_RESET_LOCALIZATION_DATA"; + case SLAM_GET_LOCALIZATION_DATA_STREAM: return "SLAM_GET_LOCALIZATION_DATA_STREAM"; + case SLAM_SET_STATIC_NODE: return "SLAM_SET_STATIC_NODE"; + case SLAM_GET_STATIC_NODE: return "SLAM_GET_STATIC_NODE"; + case CONTROLLER_POSE_CONTROL: return "CONTROLLER_POSE_CONTROL"; + case CONTROLLER_STATUS_CHANGE_EVENT: return "CONTROLLER_STATUS_CHANGE_EVENT"; + case CONTROLLER_DEVICE_CONNECT: return "CONTROLLER_DEVICE_CONNECT"; + case CONTROLLER_DEVICE_DISCOVERY_EVENT: return "CONTROLLER_DEVICE_DISCOVERY_EVENT"; + case CONTROLLER_DEVICE_DISCONNECT: return "CONTROLLER_DEVICE_DISCONNECT"; + case CONTROLLER_READ_ASSOCIATED_DEVICES: return "CONTROLLER_READ_ASSOCIATED_DEVICES"; + case CONTROLLER_WRITE_ASSOCIATED_DEVICES: return "CONTROLLER_WRITE_ASSOCIATED_DEVICES"; + case CONTROLLER_DEVICE_DISCONNECTED_EVENT: return "CONTROLLER_DEVICE_DISCONNECTED_EVENT"; + case CONTROLLER_DEVICE_CONNECTED_EVENT: return "CONTROLLER_DEVICE_CONNECTED_EVENT"; + case CONTROLLER_RSSI_TEST_CONTROL: return "CONTROLLER_RSSI_TEST_CONTROL"; + case CONTROLLER_SEND_DATA: return "CONTROLLER_SEND_DATA"; + case CONTROLLER_START_CALIBRATION: return "CONTROLLER_START_CALIBRATION"; + case CONTROLLER_CALIBRATION_STATUS_EVENT: return "CONTROLLER_CALIBRATION_STATUS_EVENT"; + case CONTROLLER_DEVICE_LED_INTENSITY_EVENT: return "CONTROLLER_DEVICE_LED_INTENSITY_EVENT"; + case DEV_ERROR: return "DEV_ERROR"; + case SLAM_ERROR: return "SLAM_ERROR"; + case CONTROLLER_ERROR: return "CONTROLLER_ERROR"; + default: return "UNKNOWN MESSAGE CODE"; + } + + case LIBUSB_TRANSFER_TYPE_CONTROL: + switch (code) + { + case CONTROL_USB_RESET: return "CONTROL_USB_RESET"; + case CONTROL_GET_INTERFACE_VERSION: return "CONTROL_GET_INTERFACE_VERSION"; + default: return "UNKNOWN MESSAGE CODE"; + } + default: return "UNKNOWN TRANSFER TYPE"; + } +} + + +std::string statusCodeToString(perc::MESSAGE_STATUS status) +{ + switch (status) + { + case MESSAGE_STATUS::SUCCESS: return "SUCCESS"; + case MESSAGE_STATUS::UNKNOWN_MESSAGE_ID: return "UNKNOWN_MESSAGE_ID"; + case MESSAGE_STATUS::INVALID_REQUEST_LEN: return "INVALID_REQUEST_LEN"; + case MESSAGE_STATUS::INVALID_PARAMETER: return "INVALID_PARAMETER"; + case MESSAGE_STATUS::INTERNAL_ERROR: return "INTERNAL_ERROR"; + case MESSAGE_STATUS::UNSUPPORTED: return "UNSUPPORTED"; + case MESSAGE_STATUS::LIST_TOO_BIG: return "LIST_TOO_BIG"; + case MESSAGE_STATUS::MORE_DATA_AVAILABLE: return "MORE_DATA_AVAILABLE"; + case MESSAGE_STATUS::DEVICE_BUSY: return "DEVICE_BUSY"; + case MESSAGE_STATUS::TIMEOUT: return "TIMEOUT"; + case MESSAGE_STATUS::TABLE_NOT_EXIST: return "TABLE_NOT_EXIST"; + case MESSAGE_STATUS::TABLE_LOCKED: return "TABLE_LOCKED"; + case MESSAGE_STATUS::DEVICE_STOPPED: return "DEVICE_STOPPED"; + case MESSAGE_STATUS::TEMPERATURE_WARNING: return "TEMPERATURE_WARNING"; + case MESSAGE_STATUS::TEMPERATURE_STOP: return "TEMPERATURE_STOP"; + case MESSAGE_STATUS::CRC_ERROR: return "CRC_ERROR"; + case MESSAGE_STATUS::INCOMPATIBLE: return "INCOMPATIBLE"; + case MESSAGE_STATUS::SLAM_NO_DICTIONARY: return "SLAM_NO_DICTIONARY"; + case MESSAGE_STATUS::NO_BLUETOOTH: return "NO_BLUETOOTH"; + default: return "UNKNOWN STATUS"; + } +} + +std::string slamStatusCodeToString(SLAM_STATUS_CODE status) +{ + switch (status) + { + case SLAM_STATUS_CODE_SUCCESS: return "SLAM_STATUS_CODE_SUCCESS"; + case SLAM_STATUS_CODE_LOCALIZATION_DATA_SET_SUCCESS: return "SLAM_STATUS_CODE_LOCALIZATION_DATA_SET_SUCCESS"; + default: return "UNKNOWN STATUS"; + } +} + +std::string slamErrorCodeToString(SLAM_ERROR_CODE error) +{ + switch (error) + { + case SLAM_ERROR_CODE_NONE: return "SLAM_ERROR_CODE_NONE"; + case SLAM_ERROR_CODE_VISION: return "SLAM_ERROR_CODE_VISION"; + case SLAM_ERROR_CODE_SPEED: return "SLAM_ERROR_CODE_SPEED"; + case SLAM_ERROR_CODE_OTHER: return "SLAM_ERROR_CODE_OTHER"; + default: return "UNKNOWN ERROR"; + } +} + +std::string controllerCalibrationStatusCodeToString(CONTROLLER_CALIBRATION_STATUS_CODE status) +{ + switch (status) + { + case CONTROLLER_CALIBRATION_STATUS_SUCCEEDED: return "CONTROLLER_CALIBRATION_STATUS_SUCCEEDED"; + case CONTROLLER_CALIBRATION_STATUS_VALIDATION_FAILURE: return "CONTROLLER_CALIBRATION_STATUS_VALIDATION_FAILURE"; + case CONTROLLER_CALIBRATION_STATUS_FLASH_ACCESS_FAILURE: return "CONTROLLER_CALIBRATION_STATUS_FLASH_ACCESS_FAILURE"; + case CONTROLLER_CALIBRATION_STATUS_IMU_FAILURE: return "CONTROLLER_CALIBRATION_STATUS_IMU_FAILURE"; + case CONTROLLER_CALIBRATION_STATUS_INTERNAL_FAILURE: return "CONTROLLER_CALIBRATION_STATUS_INTERNAL_FAILURE"; + default: return "UNKNOWN ERROR"; + } +} + +std::string sensorToString(perc::SensorType sensorType) +{ + switch (sensorType) + { + case Fisheye: return "Fisheye"; + case Gyro: return "Gyro"; + case Accelerometer: return "Accelerometer"; + case Controller: return "Controller"; + case Rssi: return "Rssi"; + case Velocimeter: return "Velocimeter"; + default: return "Unknown"; + } +} + diff --git a/third-party/libtm/libtm/src/Common.h b/third-party/libtm/libtm/src/Common.h new file mode 100644 index 0000000000..e8cb6d4223 --- /dev/null +++ b/third-party/libtm/libtm/src/Common.h @@ -0,0 +1,45 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "TrackingCommon.h" +#include "TrackingData.h" +#include "Message.h" +#include "Device.h" +#include "Utils.h" +#include + +void print_data(IN unsigned char * data, IN unsigned int data_len); + +extern perc::Status fwToHostStatus(perc::MESSAGE_STATUS status); +perc::Status slamToHostStatus(perc::SLAM_STATUS_CODE status); +perc::Status slamErrorToHostStatus(perc::SLAM_ERROR_CODE error); +perc::Status controllerCalibrationToHostStatus(perc::CONTROLLER_CALIBRATION_STATUS_CODE status); +std::string messageCodeToString(libusb_transfer_type type, uint16_t code); +std::string statusCodeToString(perc::MESSAGE_STATUS status); +std::string slamStatusCodeToString(perc::SLAM_STATUS_CODE status); +std::string slamErrorCodeToString(perc::SLAM_ERROR_CODE error); +std::string controllerCalibrationStatusCodeToString(perc::CONTROLLER_CALIBRATION_STATUS_CODE status); +perc::TrackingData::PoseFrame poseMessageToClass(perc::pose_data msg, uint8_t sensorIndex, nsecs_t systemTime); +perc::TrackingData::CameraIntrinsics cameraIntrinsicsMessageToClass(perc::camera_intrinsics msg); +perc::TrackingData::MotionIntrinsics motionIntrinsicsMessageToClass(perc::motion_intrinsics msg); +perc::motion_intrinsics motionIntrinsicsClassToMessage(perc::TrackingData::MotionIntrinsics intrinsics); +perc::TrackingData::SensorExtrinsics sensorExtrinsicsMessageToClass(perc::sensor_extrinsics msg); +std::string sensorToString(perc::SensorType sensorType); + +template +constexpr auto toUnderlying(E e) -> typename std::underlying_type::type +{ + return static_cast::type>(e); +} + +template< typename T > +struct arrayDeleter +{ + void operator ()(T const * p) + { + delete[] p; + } +}; + diff --git a/third-party/libtm/libtm/src/CompleteTask.h b/third-party/libtm/libtm/src/CompleteTask.h new file mode 100644 index 0000000000..4ad8d2a0f5 --- /dev/null +++ b/third-party/libtm/libtm/src/CompleteTask.h @@ -0,0 +1,728 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include "Utils.h" +#include "TrackingCommon.h" +#include "TrackingDevice.h" +#include "TrackingManager.h" +#include +#include +#include + +#define LATENCY_MAX_TIMEDIFF_MSEC 1000 /* Latency warning print is aggregated per this diff */ +#define LATENCY_MAX_EVENTS_PER_SEC 10 /* Latency warning print is printed if exceeding this */ + +perc::Status fwToHostStatus(perc::MESSAGE_STATUS status); + +namespace perc { + enum + { + POSE_TASK = 1, + FRAME_TASK = 2, + USB_DEVICE_TASK = 3, + VIDEO_FRAME_COMPLETE_TASK = 4, + GYRO_FRAME_COMPLETE_TASK = 5, + ACCEL_FRAME_COMPLETE_TASK = 6, + VELOCIMETER_FRAME_COMPLETE_TASK = 7, + RSSI_COMPLETE_TASK = 8, + CONTROLLER_DISCOVERY_EVENT_FRAME_COMPLETE_TASK = 9, + CONTROLLER_EVENT_FRAME_COMPLETE_TASK = 10, + CONTROLLER_CONNECT_EVENT_FRAME_COMPLETE_TASK = 11, + CONTROLLER_DISCONNECT_EVENT_FRAME_COMPLETE_TASK = 12, + CONTROLLER_CALIBRATION_EVENT_FRAME_COMPLETE_TASK = 13, + LOCALIZATION_DATA_STREAM_EVENT_FRAME_COMPLETE_TASK = 14, + ERROR_COMPLETE_TASK = 15, + CONTROLLER_LED_COMPLETE_TASK = 16, + }; + + class CompleteTask + { + public: + CompleteTask(int type, void* owner, bool _mustComplete = false) : Type(type), mOwner(owner), mMustComplete(_mustComplete) {} + virtual ~CompleteTask() {} + virtual void complete() = 0; + virtual bool mustComplete() { return mMustComplete; } + virtual void* getOwner() { return mOwner; } + private: + int Type; + void* mOwner; + bool mMustComplete; /* Tasks that must be completed upon STOP */ + }; + + class PoseCompleteTask : public CompleteTask + { + public: + PoseCompleteTask(TrackingDevice::Listener* l, TrackingData::PoseFrame& message, TrackingDevice* owner) : CompleteTask(POSE_TASK, owner), mListener(l), mOwner(owner), mMessage(message){} + virtual ~PoseCompleteTask() {} + virtual void complete() override + { + if (mListener) + { + static uint32_t latencyEventPerSec = 0; + auto start = systemTime(); + static nsecs_t prevStart = start; + mListener->onPoseFrame(mMessage); + auto finish = systemTime(); + + auto diff = ns2ms(finish - start); + if (diff > 0) + { + /* Latency event occur */ + latencyEventPerSec++; + } + + if (ns2ms(start - prevStart) > LATENCY_MAX_TIMEDIFF_MSEC) + { + /* 1 sec passed between previous latency check */ + if (latencyEventPerSec > LATENCY_MAX_EVENTS_PER_SEC) + { + LOG(mOwner, LOG_WARN, LOG_TAG, __LINE__, "High latency warning (%d msec): %d Pose latency events occurred in the last second, consider onPoseFrame callback optimization to avoid frame drops", diff, latencyEventPerSec); + } + latencyEventPerSec = 0; + prevStart = start; + } + } + } + private: + TrackingDevice::Listener* mListener; + TrackingData::PoseFrame mMessage; + TrackingDevice* mOwner; + }; + + class UsbCompleteTask : public CompleteTask + { + public: + UsbCompleteTask(TrackingManager::Listener* l, TrackingDevice* dev, TrackingData::DeviceInfo* deviceInfo, TrackingManager::EventType e, TrackingManager* owner) : + CompleteTask(USB_DEVICE_TASK, owner, true), mDevice(dev), mDeviceInfo(*deviceInfo), mEventType(e), mListener(l) {} + virtual ~UsbCompleteTask() {} + virtual void complete() override + { + if (mListener) + { + mListener->onStateChanged(mEventType, mDevice, mDeviceInfo); + } + } + private: + TrackingManager::Listener* mListener; + TrackingDevice* mDevice; + TrackingManager::EventType mEventType; + TrackingData::DeviceInfo mDeviceInfo; + }; + + class ErrorTask : public CompleteTask + { + public: + ErrorTask(TrackingManager::Listener* l, TrackingDevice* dev, Status e, TrackingManager* owner) : + CompleteTask(ERROR_COMPLETE_TASK, owner, true), mDevice(dev), mErrorStatus(e), mListener(l) {} + virtual ~ErrorTask() {} + virtual void complete() override + { + if (mListener) + { + mListener->onError(mErrorStatus, mDevice); + } + } + private: + TrackingManager::Listener* mListener; + TrackingDevice* mDevice; + Status mErrorStatus; + + }; + + class FrameBuffersOwner + { + public: + virtual void putBufferBack(SensorId id, std::shared_ptr& frame) = 0; + }; + + class VideoFrameCompleteTask : public CompleteTask + { + public: + VideoFrameCompleteTask(TrackingDevice::Listener* l, std::shared_ptr& frame, FrameBuffersOwner* frameBufferOwner, TrackingDevice* owner, nsecs_t systemTimeStamp, uint16_t width, uint16_t height) : + mFrame(frame), mFrameBufferOwner(frameBufferOwner), mListener(l), mOwner(owner), CompleteTask(VIDEO_FRAME_COMPLETE_TASK, owner) + { + bulk_message_video_stream* frameBuffer = (bulk_message_video_stream*)mFrame.get(); + mVideoFrame.data = (uint8_t*)mFrame.get() + sizeof(bulk_message_video_stream); + mVideoFrame.profile.set(width, height, width, PixelFormat::Y8); + mVideoFrame.systemTimestamp = systemTimeStamp; + mVideoFrame.timestamp = frameBuffer->rawStreamHeader.llNanoseconds; + mVideoFrame.arrivalTimeStamp = frameBuffer->rawStreamHeader.llArrivalNanoseconds; + mVideoFrame.sensorIndex = GET_SENSOR_INDEX(frameBuffer->rawStreamHeader.bSensorID); + mVideoFrame.frameId = frameBuffer->rawStreamHeader.dwFrameId; + mVideoFrame.exposuretime = frameBuffer->metadata.dwExposuretime; + mVideoFrame.gain = frameBuffer->metadata.fGain; + mVideoFrame.frameLength = frameBuffer->metadata.dwFrameLength; + + mId = frameBuffer->rawStreamHeader.bSensorID; + } + virtual ~VideoFrameCompleteTask() + { + mFrameBufferOwner->putBufferBack(mId, mFrame); + } + virtual void complete() override + { + if (mListener) + { + static uint32_t latencyEventPerSec = 0; + auto start = systemTime(); + static nsecs_t prevStart = start; + mListener->onVideoFrame(mVideoFrame); + auto finish = systemTime(); + + auto diff = ns2ms(finish - start); + if (diff > 0) + { + /* Latency event occur */ + latencyEventPerSec++; + } + + if (ns2ms(start - prevStart) > LATENCY_MAX_TIMEDIFF_MSEC) + { + /* 1 sec passed between previous latency check */ + if (latencyEventPerSec > LATENCY_MAX_EVENTS_PER_SEC) + { + LOG(mOwner, LOG_WARN, LOG_TAG, __LINE__, "High latency warning (%d msec): %d Video latency events occurred in the last second, consider onVideoFrame callback optimization to avoid frame drops", diff, latencyEventPerSec); + } + latencyEventPerSec = 0; + prevStart = start; + } + } + } + + public: + TrackingData::VideoFrame mVideoFrame; + + private: + TrackingDevice::Listener* mListener; + std::shared_ptr mFrame; + FrameBuffersOwner* mFrameBufferOwner; + SensorId mId; + TrackingDevice* mOwner; + }; + + class AccelerometerFrameCompleteTask : public CompleteTask + { + public: + AccelerometerFrameCompleteTask(TrackingDevice::Listener* l, interrupt_message_raw_stream_header* imuHeader, int64_t timeStampShift, TrackingDevice* owner) : + mId(imuHeader->bSensorID), mListener(l), mOwner(owner), CompleteTask(ACCEL_FRAME_COMPLETE_TASK, owner) + { + interrupt_message_accelerometer_stream_metadata metaData = ((interrupt_message_accelerometer_stream*)imuHeader)->metadata; + + mFrame.sensorIndex = GET_SENSOR_INDEX(imuHeader->bSensorID); + mFrame.frameId = imuHeader->dwFrameId; + mFrame.acceleration.set(metaData.flAx, metaData.flAy, metaData.flAz); + mFrame.temperature = metaData.flTemperature; + mFrame.timestamp = imuHeader->llNanoseconds; + mFrame.arrivalTimeStamp = imuHeader->llArrivalNanoseconds; + mFrame.systemTimestamp = imuHeader->llNanoseconds + timeStampShift; + } + + virtual ~AccelerometerFrameCompleteTask() {} + virtual void complete() override + { + if (mListener) + { + static uint32_t latencyEventPerSec = 0; + auto start = systemTime(); + static nsecs_t prevStart = start; + mListener->onAccelerometerFrame(mFrame); + auto finish = systemTime(); + + auto diff = ns2ms(finish - start); + if (diff > 0) + { + /* Latency event occur */ + latencyEventPerSec++; + } + + if (ns2ms(start - prevStart) > LATENCY_MAX_TIMEDIFF_MSEC) + { + /* 1 sec passed between previous latency check */ + if (latencyEventPerSec > LATENCY_MAX_EVENTS_PER_SEC) + { + LOG(mOwner, LOG_WARN, LOG_TAG, __LINE__, "High latency warning (%d msec): %d Accelerometer latency events occurred in the last second, consider onAccelerometerFrame callback optimization to avoid frame drops", diff, latencyEventPerSec); + } + latencyEventPerSec = 0; + prevStart = start; + } + } + } + + public: + TrackingData::AccelerometerFrame mFrame; + + private: + TrackingDevice::Listener* mListener; + SensorId mId; + TrackingDevice* mOwner; + }; + + class GyroFrameCompleteTask : public CompleteTask + { + public: + GyroFrameCompleteTask(TrackingDevice::Listener* l, interrupt_message_raw_stream_header* imuHeader, int64_t timeStampShift, TrackingDevice* owner) : mListener(l), mOwner(owner), CompleteTask(GYRO_FRAME_COMPLETE_TASK, owner) + { + interrupt_message_gyro_stream_metadata metaData = ((interrupt_message_gyro_stream*)imuHeader)->metadata; + mFrame.sensorIndex = GET_SENSOR_INDEX(imuHeader->bSensorID); + mFrame.frameId = imuHeader->dwFrameId; + mFrame.angularVelocity.set(metaData.flGx, metaData.flGy, metaData.flGz); + mFrame.temperature = metaData.flTemperature; + mFrame.timestamp = imuHeader->llNanoseconds; + mFrame.arrivalTimeStamp = imuHeader->llArrivalNanoseconds; + mFrame.systemTimestamp = imuHeader->llNanoseconds + timeStampShift; + } + virtual ~GyroFrameCompleteTask() {} + virtual void complete() override + { + if (mListener) + { + static uint32_t latencyEventPerSec = 0; + auto start = systemTime(); + static nsecs_t prevStart = start; + mListener->onGyroFrame(mFrame); + auto finish = systemTime(); + + auto diff = ns2ms(finish - start); + if (diff > 0) + { + /* Latency event occur */ + latencyEventPerSec++; + } + + if (ns2ms(start - prevStart) > LATENCY_MAX_TIMEDIFF_MSEC) + { + /* 1 sec passed between previous latency check */ + if (latencyEventPerSec > LATENCY_MAX_EVENTS_PER_SEC) + { + LOG(mOwner, LOG_WARN, LOG_TAG, __LINE__, "High latency warning (%d msec): %d Gyro latency events occurred in the last second, consider onGyroFrame callback optimization to avoid frame drops", diff, latencyEventPerSec); + } + latencyEventPerSec = 0; + prevStart = start; + } + } + } + + public: + TrackingData::GyroFrame mFrame; + + private: + TrackingDevice::Listener* mListener; + TrackingDevice* mOwner; + }; + + class VelocimeterFrameCompleteTask : public CompleteTask + { + public: + VelocimeterFrameCompleteTask(TrackingDevice::Listener* l, interrupt_message_raw_stream_header* imuHeader, int64_t timeStampShift, TrackingDevice* owner) : mListener(l), mOwner(owner), CompleteTask(VELOCIMETER_FRAME_COMPLETE_TASK, owner) + { + interrupt_message_velocimeter_stream_metadata metaData = ((interrupt_message_velocimeter_stream*)imuHeader)->metadata; + mFrame.sensorIndex = GET_SENSOR_INDEX(imuHeader->bSensorID); + mFrame.frameId = imuHeader->dwFrameId; + mFrame.angularVelocity.set(metaData.flVx, metaData.flVy, metaData.flVz); + mFrame.temperature = metaData.flTemperature; + mFrame.timestamp = imuHeader->llNanoseconds; + mFrame.arrivalTimeStamp = imuHeader->llArrivalNanoseconds; + mFrame.systemTimestamp = imuHeader->llNanoseconds + timeStampShift; + } + virtual ~VelocimeterFrameCompleteTask() {} + virtual void complete() override + { + if (mListener) + { + static uint32_t latencyEventPerSec = 0; + auto start = systemTime(); + static nsecs_t prevStart = start; + mListener->onVelocimeterFrame(mFrame); + auto finish = systemTime(); + + auto diff = ns2ms(finish - start); + if (diff > 0) + { + /* Latency event occur */ + latencyEventPerSec++; + } + + if (ns2ms(start - prevStart) > LATENCY_MAX_TIMEDIFF_MSEC) + { + /* 1 sec passed between previous latency check */ + if (latencyEventPerSec > LATENCY_MAX_EVENTS_PER_SEC) + { + LOG(mOwner, LOG_WARN, LOG_TAG, __LINE__, "High latency warning (%d msec): %d Velocimeter latency events occurred in the last second, consider onVelocimeterFrame callback optimization to avoid frame drops", diff, latencyEventPerSec); + } + latencyEventPerSec = 0; + prevStart = start; + } + } + } + + public: + TrackingData::VelocimeterFrame mFrame; + + private: + TrackingDevice::Listener* mListener; + TrackingDevice* mOwner; + }; + + class ControllerFrameCompleteTask : public CompleteTask + { + public: + ControllerFrameCompleteTask(TrackingDevice::Listener* l, bulk_message_raw_stream_header* header, int64_t timeStampShift, TrackingDevice* owner) : + mListener(l), mOwner(owner), CompleteTask(CONTROLLER_EVENT_FRAME_COMPLETE_TASK, owner) + { + bulk_message_controller_stream_metadata metaData = ((bulk_message_controller_stream*)header)->metadata; + mFrame.sensorIndex = GET_SENSOR_INDEX(header->bSensorID); + mFrame.frameId = header->dwFrameId; + mFrame.timestamp = header->llNanoseconds; + mFrame.arrivalTimeStamp = header->llArrivalNanoseconds; + mFrame.systemTimestamp = header->llNanoseconds + timeStampShift; + mFrame.eventId = metaData.bEventID; + mFrame.instanceId = metaData.bInstanceId; + perc::copy(mFrame.sensorData, metaData.bSensorData, sizeof(mFrame.sensorData)); + } + virtual ~ControllerFrameCompleteTask() {} + virtual void complete() override + { + if (mListener) + { + static uint32_t latencyEventPerSec = 0; + auto start = systemTime(); + static nsecs_t prevStart = start; + mListener->onControllerFrame(mFrame); + auto finish = systemTime(); + + auto diff = ns2ms(finish - start); + if (diff > 0) + { + /* Latency event occur */ + latencyEventPerSec++; + } + + if (ns2ms(start - prevStart) > LATENCY_MAX_TIMEDIFF_MSEC) + { + /* 1 sec passed between previous latency check */ + if (latencyEventPerSec > LATENCY_MAX_EVENTS_PER_SEC) + { + LOG(mOwner, LOG_WARN, LOG_TAG, __LINE__, "High latency warning (%d msec): %d Controller latency events occurred in the last second, consider onControllerFrame callback optimization to avoid frame drops", diff, latencyEventPerSec); + } + latencyEventPerSec = 0; + prevStart = start; + } + } + } + private: + TrackingDevice::Listener* mListener; + TrackingData::ControllerFrame mFrame; + TrackingDevice* mOwner; + }; + + + class RssiFrameCompleteTask : public CompleteTask + { + public: + RssiFrameCompleteTask(TrackingDevice::Listener* l, bulk_message_raw_stream_header* header, int64_t timeStampShift, TrackingDevice* owner) : mListener(l), mOwner(owner), CompleteTask(RSSI_COMPLETE_TASK, owner) + { + bulk_message_rssi_stream_metadata metaData = ((bulk_message_rssi_stream*)header)->metadata; + mFrame.sensorIndex = GET_SENSOR_INDEX(header->bSensorID); + mFrame.frameId = header->dwFrameId; + mFrame.timestamp = header->llNanoseconds; + mFrame.arrivalTimeStamp = header->llArrivalNanoseconds; + mFrame.systemTimestamp = header->llNanoseconds + timeStampShift; + mFrame.signalStrength = metaData.flSignalStrength; + } + virtual ~RssiFrameCompleteTask() {} + virtual void complete() override + { + if (mListener) + { + static uint32_t latencyEventPerSec = 0; + auto start = systemTime(); + static nsecs_t prevStart = start; + mListener->onRssiFrame(mFrame); + auto finish = systemTime(); + + auto diff = ns2ms(finish - start); + if (diff > 0) + { + /* Latency event occur */ + latencyEventPerSec++; + } + + if (ns2ms(start - prevStart) > LATENCY_MAX_TIMEDIFF_MSEC) + { + /* 1 sec passed between previous latency check */ + if (latencyEventPerSec > LATENCY_MAX_EVENTS_PER_SEC) + { + LOG(mOwner, LOG_WARN, LOG_TAG, __LINE__, "High latency warning (%d msec): %d Rssi latency events occurred in the last second, consider onRssiFrame callback optimization to avoid frame drops", diff, latencyEventPerSec); + } + latencyEventPerSec = 0; + prevStart = start; + } + } + } + private: + TrackingDevice::Listener* mListener; + TrackingData::RssiFrame mFrame; + TrackingDevice* mOwner; + }; + + class ControllerDiscoveryEventFrameCompleteTask : public CompleteTask + { + public: + ControllerDiscoveryEventFrameCompleteTask(TrackingDevice::Listener* l, interrupt_message_controller_device_discovery* controllerDeviceDiscovery, TrackingDevice* owner) : + mListener(l), CompleteTask(CONTROLLER_DISCOVERY_EVENT_FRAME_COMPLETE_TASK, owner) + { + perc::copy(mFrame.macAddress, controllerDeviceDiscovery->bMacAddress, sizeof(controllerDeviceDiscovery->bMacAddress)); + mFrame.addressType = (AddressType)controllerDeviceDiscovery->bAddressType; + mFrame.manufacturerId = controllerDeviceDiscovery->info.wManufacturerId; + mFrame.vendorData = controllerDeviceDiscovery->info.bVendorData; + mFrame.protocol.set(controllerDeviceDiscovery->info.bProtocolVersion); + mFrame.app.set(controllerDeviceDiscovery->info.bAppVersionMajor, controllerDeviceDiscovery->info.bAppVersionMinor, controllerDeviceDiscovery->info.bAppVersionPatch); + mFrame.bootLoader.set(controllerDeviceDiscovery->info.bBootloaderVersionMajor, controllerDeviceDiscovery->info.bBootloaderVersionMinor, controllerDeviceDiscovery->info.bBootloaderVersionPatch); + mFrame.softDevice.set(controllerDeviceDiscovery->info.bSoftdeviceVersion); + } + virtual ~ControllerDiscoveryEventFrameCompleteTask() {} + virtual void complete() override + { + if (mListener) + { + mListener->onControllerDiscoveryEventFrame(mFrame); + } + } + private: + TrackingDevice::Listener* mListener; + TrackingData::ControllerDiscoveryEventFrame mFrame; + }; + + class ControllerConnectEventFrameCompleteTask : public CompleteTask + { + public: + ControllerConnectEventFrameCompleteTask(TrackingDevice::Listener* l, interrupt_message_controller_connected* controllerConnected, TrackingDevice* owner) : + mListener(l), CompleteTask(CONTROLLER_CONNECT_EVENT_FRAME_COMPLETE_TASK, owner) + { + mFrame.controllerId = controllerConnected->bControllerID; + mFrame.manufacturerId = controllerConnected->info.wManufacturerId; + mFrame.protocol.set(controllerConnected->info.bProtocolVersion); + mFrame.app.set(controllerConnected->info.bAppVersionMajor, controllerConnected->info.bAppVersionMinor, controllerConnected->info.bAppVersionPatch); + mFrame.bootLoader.set(controllerConnected->info.bBootloaderVersionMajor, controllerConnected->info.bBootloaderVersionMinor, controllerConnected->info.bBootloaderVersionPatch); + mFrame.softDevice.set(controllerConnected->info.bSoftdeviceVersion); + mFrame.status = fwToHostStatus((MESSAGE_STATUS)controllerConnected->wStatus); + } + virtual ~ControllerConnectEventFrameCompleteTask() {} + virtual void complete() override + { + if (mListener) + { + mListener->onControllerConnectedEventFrame(mFrame); + } + } + private: + TrackingDevice::Listener* mListener; + TrackingData::ControllerConnectedEventFrame mFrame; + }; + + class ControllerDisconnectEventFrameCompleteTask : public CompleteTask + { + public: + ControllerDisconnectEventFrameCompleteTask(TrackingDevice::Listener* l, interrupt_message_controller_disconnected* controllerDisconnected, TrackingDevice* owner) : + mListener(l), CompleteTask(CONTROLLER_DISCONNECT_EVENT_FRAME_COMPLETE_TASK, owner) + { + mFrame.controllerId = controllerDisconnected->bControllerID; + } + virtual ~ControllerDisconnectEventFrameCompleteTask() {} + virtual void complete() override + { + if (mListener) + { + mListener->onControllerDisconnectedEventFrame(mFrame); + } + } + private: + TrackingDevice::Listener* mListener; + TrackingData::ControllerDisconnectedEventFrame mFrame; + }; + + class ControllerCalibrationEventFrameCompleteTask : public CompleteTask + { + public: + ControllerCalibrationEventFrameCompleteTask(TrackingDevice::Listener* l, uint8_t controllerID, Status status, TrackingDevice* owner) : + mListener(l), CompleteTask(CONTROLLER_CALIBRATION_EVENT_FRAME_COMPLETE_TASK, owner) + { + mFrame.controllerId = controllerID; + mFrame.status = status; + } + virtual ~ControllerCalibrationEventFrameCompleteTask() {} + virtual void complete() override + { + if (mListener) + { + mListener->onControllerCalibrationEventFrame(mFrame); + } + } + private: + TrackingDevice::Listener* mListener; + TrackingData::ControllerCalibrationEventFrame mFrame; + }; + + class LocalizationDataEventFrameCompleteTask : public CompleteTask + { + public: + /* Get localization data stream */ + LocalizationDataEventFrameCompleteTask(TrackingDevice::Listener* l, std::shared_ptr& frame, FrameBuffersOwner* frameBufferOwner, TrackingDevice* owner) : + mListener(l), mFrameBufferOwner(frameBufferOwner), mFrame(frame), CompleteTask(LOCALIZATION_DATA_STREAM_EVENT_FRAME_COMPLETE_TASK, owner, true) + { + + interrupt_message_get_localization_data_stream* localizationData = (interrupt_message_get_localization_data_stream*)mFrame.get(); + mLocalizationFrame.status = fwToHostStatus((MESSAGE_STATUS)localizationData->wStatus); + if ((MESSAGE_STATUS)localizationData->wStatus == MESSAGE_STATUS::MORE_DATA_AVAILABLE) + { + mLocalizationFrame.moreData = true; + } + + mLocalizationFrame.buffer = localizationData->bLocalizationData; + mLocalizationFrame.chunkIndex = localizationData->wIndex; + mLocalizationFrame.length = localizationData->header.dwLength - offsetof(interrupt_message_get_localization_data_stream, bLocalizationData); + } + + /* Set localization data stream */ + LocalizationDataEventFrameCompleteTask(TrackingDevice::Listener* l, interrupt_message_set_localization_data_stream* localizationData, TrackingDevice* owner) : + mListener(l), mFrame(NULL), mFrameBufferOwner(NULL), CompleteTask(LOCALIZATION_DATA_STREAM_EVENT_FRAME_COMPLETE_TASK, owner, true) + { + mLocalizationFrame.status = fwToHostStatus((MESSAGE_STATUS)localizationData->wStatus); + mLocalizationFrame.buffer = NULL; + mLocalizationFrame.length = 0; + mLocalizationFrame.chunkIndex = 0; + mLocalizationFrame.moreData = false; + } + + virtual ~LocalizationDataEventFrameCompleteTask() + { + if (mLocalizationFrame.buffer != NULL) + { + mFrameBufferOwner->putBufferBack(0, mFrame); + } + } + + virtual void complete() override + { + if (mListener) + { + mListener->onLocalizationDataEventFrame(mLocalizationFrame); + } + } + private: + TrackingDevice::Listener* mListener; + std::shared_ptr mFrame; + FrameBuffersOwner* mFrameBufferOwner; + TrackingData::LocalizationDataFrame mLocalizationFrame; + }; + + class FWUpdateCompleteTask : public CompleteTask + { + public: + + FWUpdateCompleteTask(TrackingDevice::Listener* l, interrupt_message_fw_update_stream* FWImage, TrackingDevice* owner) : + mListener(l), CompleteTask(DEV_FIRMWARE_UPDATE, owner, true) + { + mFrame.status = fwToHostStatus((MESSAGE_STATUS)FWImage->wStatus); + mFrame.progress = FWImage->bProgress; + perc::copy(mFrame.macAddress, FWImage->bMacAddress, sizeof(FWImage->bMacAddress)); + } + + virtual ~FWUpdateCompleteTask() = default; + + virtual void complete() override + { + if (mListener) + { + mListener->onFWUpdateEvent(mFrame); + } + } + private: + TrackingDevice::Listener* mListener; + TrackingData::ControllerFWEventFrame mFrame; + }; + + class StatusEventFrameCompleteTask : public CompleteTask + { + public: + + StatusEventFrameCompleteTask(TrackingDevice::Listener* l, perc::Status status, TrackingDevice* owner) : mListener(l), CompleteTask(DEV_STATUS, owner, true) + { + mFrame.status = status; + } + + virtual ~StatusEventFrameCompleteTask() = default; + + virtual void complete() override + { + if (mListener) + { + mListener->onStatusEvent(mFrame); + } + } + private: + TrackingDevice::Listener* mListener; + TrackingData::StatusEventFrame mFrame; + }; + + class CompleteQueueHandler + { + public: + virtual void addTask(std::shared_ptr&) = 0; + virtual void removeTasks(void* owner, bool completeTasks) = 0; + }; + + class ControllerLedFrameCompleteTask : public CompleteTask + { + public: + ControllerLedFrameCompleteTask(TrackingDevice::Listener* l, interrupt_message_controller_led_intensity* led, int64_t timeStampShift, TrackingDevice* owner) : mListener(l), mOwner(owner), CompleteTask(CONTROLLER_LED_COMPLETE_TASK, owner) + { + mFrame.controllerId = GET_SENSOR_INDEX(led->rawStreamHeader.bSensorID); + mFrame.timestamp = led->rawStreamHeader.llNanoseconds; + mFrame.arrivalTimeStamp = led->rawStreamHeader.llArrivalNanoseconds; + mFrame.systemTimestamp = led->rawStreamHeader.llNanoseconds + timeStampShift; + mFrame.intensity = led->intensity; + mFrame.ledId = led->ledId; + } + virtual ~ControllerLedFrameCompleteTask() {} + virtual void complete() override + { + if (mListener) + { + static uint32_t latencyEventPerSec = 0; + auto start = systemTime(); + static nsecs_t prevStart = start; + mListener->onControllerLedEvent(mFrame); + auto finish = systemTime(); + + auto diff = ns2ms(finish - start); + if (diff > 0) + { + /* Latency event occur */ + latencyEventPerSec++; + } + + if (ns2ms(start - prevStart) > LATENCY_MAX_TIMEDIFF_MSEC) + { + /* 1 sec passed between previous latency check */ + if (latencyEventPerSec > LATENCY_MAX_EVENTS_PER_SEC) + { + LOG(mOwner, LOG_WARN, LOG_TAG, __LINE__, "High latency warning (%d msec): %d led latency events occurred in the last second, consider onled callback optimization to avoid frame drops", diff, latencyEventPerSec); + } + latencyEventPerSec = 0; + prevStart = start; + } + } + } + + public: + TrackingData::ControllerLedEventFrame mFrame; + + private: + TrackingDevice::Listener* mListener; + TrackingDevice* mOwner; + }; + + +} diff --git a/third-party/libtm/libtm/src/Device.cpp b/third-party/libtm/libtm/src/Device.cpp new file mode 100644 index 0000000000..83e382e7d2 --- /dev/null +++ b/third-party/libtm/libtm/src/Device.cpp @@ -0,0 +1,4220 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#define LOG_TAG "Device" +#define LOG_NDEBUG 0 /* Enable LOGV */ + +#include +#include +#include +#include +#include +#include +#include +#include "Device.h" +#include "Common.h" +#include "CompleteTask.h" +#include "Version.h" +#include "Utils.h" +#include "UsbPlugListener.h" +#include "TrackingData.h" +#include "CentralAppFw.h" +#include "CentralBlFw.h" +//#include "types.h" + +#define CHUNK_SIZE 512 +#define BUFFER_SIZE 1024 +#define SYNC_TIME_FREQUENCY_NS 500000000 // in nano seconds, every 500 msec +#define SYNC_TIME_DIFF_MSEC 1 +#define MAX_6DOF_TIMEDIFF_MSEC 5 /* 6dof must arrive every 5 msec */ +#define WAIT_FOR_STOP_STATUS_MSEC 200 /* Time to wait before closing interrupt/event threads - give opportunity for the FW to send the DEVICE_STOPPED status */ +#define MAX_FRAME_HEIGHT 1080 +#define MAX_FRAME_WIDTH 1920 +#define MAX_BIG_DATA_MESSAGE_LENGTH 10250 //(10K + large message header size) +#define FRAMES_PER_STREAM 100 +#define PERMANENT_LOCK_CONFIGURATION_TOKEN 0xDEAD10CC +#define BULK_ENDPOINT_MESSAGES 2 /* Endpoint address for sending/receiving all bulk messages */ +#define BULK_ENDPOINT_FRAMES 1 /* Endpoint address for sending/receiving all frames (Fisheye, Gyro, Accelerometer) */ +#define TO_HOST LIBUSB_ENDPOINT_IN /* to host */ +#define TO_DEVICE LIBUSB_ENDPOINT_OUT /* to device */ + +namespace perc { + // ---------------------------------------------------------------------------- + Device::Device(libusb_device* dev, Dispatcher* dispatcher, EventHandler* owner, CompleteQueueHandler* taskHandler) : + mListener(nullptr), mLibusbDevice(dev), mDevice(nullptr), mDispatcher(dispatcher), + mStreamEndpointThreadStop(false), mInterruptEndpointThreadStop(false), mStreamEndpointThreadActive(false), mInterruptEndpointThreadActive(false), mOwner(owner), mTaskHandler(taskHandler), mCleared(false), + mEndpointBulkMessages(BULK_ENDPOINT_MESSAGES), mStreamEndpoint(BULK_ENDPOINT_FRAMES), mEepromChunkSize(CHUNK_SIZE), mSyncTimeout(SYNC_TIME_FREQUENCY_NS), + mPlaybackIsOn(false), mSyncTimeEnabled(false), mUsbState(DEVICE_USB_STATE_INIT), mDeviceStatus(Status::SUCCESS), mHasBluetooth(true), mFWInterfaceVersion{0} + { + memset(&mDeviceInfo, 0, sizeof(mDeviceInfo)); + supported_raw_stream_libtm_message streams[MAX_SUPPORTED_STREAMS]; + uint16_t actual = 0; + Status status = Status::SUCCESS; + + DEVICELOGV("Creating Device"); + + ASSERT(mDispatcher); + mFsm.init(FSM(main), this, mDispatcher, LOG_TAG); + + auto rc = libusb_open(mLibusbDevice, &mDevice); + if (rc != 0) + { + if (rc == LIBUSB_ERROR_NOT_SUPPORTED) + { + DEVICELOGE("Error while opening device (%s), Please install driver for Intel(R) RealSense(TM) T250 device", libusb_error_name(rc)); + } + else + { + DEVICELOGE("Error while opening device. LIBUSB_ERROR_CODE: %d (%s)", rc, libusb_error_name(rc)); + } + + return; + } + + /* Device USB marked as open - to close it on destructor */ + mUsbState = DEVICE_USB_STATE_OPENED; + + mEndpointInterrupt = FindInterruptEndpoint(); + if (mEndpointInterrupt == -1) + { + DEVICELOGE("Interrupt endpoint not found! "); + return; + } + + rc = libusb_claim_interface(mDevice, INTERFACE_INDEX); + if (rc != 0) + { + DEVICELOGE("Error: Failed to claim USB interface. LIBUSB_ERROR_CODE: 0x%X (%s)", rc, libusb_error_name(rc)); + return; + } + + /* Device USB marked as claimed - to release it on destructor */ + mUsbState = DEVICE_USB_STATE_CLAIMED; + + status = GetUsbConnectionDescriptor(); + if (status != Status::SUCCESS) + { + DEVICELOGE("Error: Failed to get USB connection descriptor (0x%X)", status); + return; + } + + // Pending for FW + //status = DeviceFlush(); + //if (status != Status::SUCCESS) + //{ + // DEVICELOGE("Error: Failed to flush device (0x%X)", status); + // return; + //} + + status = GetInterfaceVersionInternal(); + if (status != Status::SUCCESS) + { + DEVICELOGE("Error: Failed to get interface (0x%X)", status); + return; + } + + /* Device USB marked as ready - There are no fatal errors on device creation, from this point every error will be forwarded to the device state machine (to support user reset) */ + mUsbState = DEVICE_USB_STATE_READY; + + if ((mFWInterfaceVersion.dwMajor != LIBTM_API_VERSION_MAJOR) || (mFWInterfaceVersion.dwMinor < LIBTM_API_VERSION_MINOR)) + { + DEVICELOGE("Error: Interface version mismatch: Host %d.%d, FW %d.%d", LIBTM_API_VERSION_MAJOR, LIBTM_API_VERSION_MINOR, mFWInterfaceVersion.dwMajor, mFWInterfaceVersion.dwMinor); + mDispatcher->postMessage(&mFsm, Message(ON_ERROR)); + mDeviceStatus = Status::INIT_FAILED; + } + + status = GetDeviceInfoInternal(); + if (status != Status::SUCCESS) + { + DEVICELOGE("Error: Failed to get device info (0x%X)", status); + mDispatcher->postMessage(&mFsm, Message(ON_ERROR)); + mDeviceStatus = Status::INIT_FAILED; + return; + } + + if (mDeviceInfo.bCentralAppVersionMajor == 0 && + mDeviceInfo.bCentralAppVersionMinor == 0 && + mDeviceInfo.bCentralAppVersionPatch == 0 && + mDeviceInfo.dwCentralAppVersionBuild == 0) + { + mHasBluetooth = false; + } + else + { + mHasBluetooth = true; + } + + // Pending for FW + //status = SetDeviceStreamConfig(MAX_BIG_DATA_MESSAGE_LENGTH); + //if (status != Status::SUCCESS) + //{ + // DEVICELOGE("Error: Set device stream config (0x%X)", status); + // mDispatcher->postMessage(&mFsm, Message(ON_ERROR)); + // mDeviceStatus = Status::INIT_FAILED; + // return; + //} + + mFrameTempBufferSize = (MAX_FRAME_HEIGHT * MAX_FRAME_WIDTH) + sizeof(supported_raw_stream_libtm_message); + AllocateBuffers(); + + status = GetSupportedRawStreamsInternal(streams, MAX_SUPPORTED_STREAMS, actual); + if (status != Status::SUCCESS) + { + DEVICELOGE("Error: Failed to get supported raw stream (0x%X)", status); + mDispatcher->postMessage(&mFsm, Message(ON_ERROR)); + mDeviceStatus = Status::INIT_FAILED; + return; + } + + for (int i = 0; i < actual; i++) + { + switch (GET_SENSOR_TYPE(streams[i].bSensorID)) + { + case SensorType::Fisheye: + { + TrackingData::VideoProfile p; + p.set(true, false, streams[i].wFramesPerSecond, GET_SENSOR_INDEX(streams[i].bSensorID)); + p.profile.set(streams[i].wWidth, streams[i].wHeight, streams[i].wStride, static_cast(streams[i].bPixelFormat)); + mVideoProfiles.push_back(p); + break; + } + + case SensorType::Gyro: + { + TrackingData::GyroProfile p; + p.set(true, false, streams[i].wFramesPerSecond, GET_SENSOR_INDEX(streams[i].bSensorID)); + mGyroProfiles.push_back(p); + break; + } + + case SensorType::Accelerometer: + { + TrackingData::AccelerometerProfile p; + p.set(true, false, streams[i].wFramesPerSecond, GET_SENSOR_INDEX(streams[i].bSensorID)); + mAccelerometerProfiles.push_back(p); + break; + } + + case SensorType::Velocimeter: + { + TrackingData::VelocimeterProfile p; + p.set(true, false, streams[i].wFramesPerSecond, GET_SENSOR_INDEX(streams[i].bSensorID)); + mVelocimeterProfiles.push_back(p); + break; + } + + case SensorType::Rssi: + { + break; + } + + default: + DEVICELOGE("Unknown SensorType support (0x%X)", GET_SENSOR_TYPE(streams[i].bSensorID)); + break; + } // end of switch + } + + if (mHasBluetooth) + { + status = CentralFWUpdate(); + if (status != Status::SUCCESS) + { + DEVICELOGE("Central firmware update failed with error (0x%X)", status); + mDispatcher->postMessage(&mFsm, Message(ON_ERROR)); + mDeviceStatus = Status::INIT_FAILED; + return; + } + } + SyncTime(); + + // schedule the listener itself for every 500 msec + mDispatcher->scheduleTimer(this, mSyncTimeout, Message(0)); + + return; + } + + Status Device::SyncTime() + { + bulk_message_request_get_time req = {0}; + bulk_message_response_get_time res = {0}; + nsecs_t start; + nsecs_t finish = systemTime(); + uint32_t syncTry = 1; + + req.header.wMessageID = DEV_GET_TIME; + req.header.dwLength = sizeof(req); + res.header.wStatus = 0xFF; + + while (true) + { + start = systemTime(); + + Bulk_Message msg((uint8_t*)&req, sizeof(req), (uint8_t*)&res, sizeof(res), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mFsm.fireEvent(msg); + + if (msg.Result != 0) + break; + + finish = systemTime(); + + auto diff = ns2ms(finish - start); + if ((diff) <= SYNC_TIME_DIFF_MSEC) + { + mTM2CorrelatedTimeStampShift = start + ((finish-start)/2) - res.llNanoseconds; + break; + } + syncTry++; + } + + if (res.header.wStatus == 0) + { + DEVICELOGV("TM2 and Host clocks were synced in %d nanosec after %d tries", ns2ms(finish - start), syncTry); + } + else + { + DEVICELOGE("Error syncing TM2 and Host clocks"); + } + + return Status::SUCCESS; + } + + const char* temperatureSensor(uint8_t index) + { + switch (index) + { + case 0x00: return "VPU"; + case 0x01: return "IMU"; + case 0x02: return "BLE"; + } + return "Unknown"; + } + + const char* hwVersion(uint8_t hwVersion) + { + switch (hwVersion) + { + case 0x00: return "ES0"; + case 0x01: return "ES1"; + case 0x02: return "ES2"; + case 0x03: return "ES3"; + case 0x04: return "ES4"; + } + return "Unknown"; + } + + const char* fwLogVerbosityLevel(LogVerbosityLevel level) + { + switch (level) + { + case Error: return "[E]"; + case Info: return "[I]"; + case Warning: return "[W]"; + case Debug: return "[D]"; + case Verbose: return "[V]"; + case Trace: return "[T]"; + } + return "[Unknown]"; + } + + enum module_id { + MODULE_UNDEFINED = 0x0000, + MODULE_MAIN = 0x0001, + MODULE_USB = 0x0002, + MODULE_IMU_DRV = 0x0003, + MODULE_VIDEO = 0x0004, + MODULE_MEMPOOL = 0x0005, + MODULE_MESSAGE_IO = 0X0006, + MODULE_LOGGER = 0x0007, + MODULE_LOG_TESTS = 0x0008, + MODULE_CENTRAL_MGR = 0x000A, + MODULE_HMD_TRACKING = 0x000B, + MODULE_CONTROLLERS = 0x000C, + MODULE_PACKET_IO = 0x000D, + MODULE_DFU = 0x000E, + MODULE_CONFIG_TABLES = 0x000F, + MODULE_LAST_VALUE = 0xFFFF + }; + + const char* fwModuleID(module_id module) + { + switch (module) + { + case MODULE_UNDEFINED: return "UNDEFINED"; + case MODULE_MAIN: return "MAIN"; + case MODULE_USB: return "USB"; + case MODULE_IMU_DRV: return "IMU_DRV"; + case MODULE_VIDEO: return "VIDEO"; + case MODULE_MEMPOOL: return "MEMPOOL"; + case MODULE_MESSAGE_IO: return "MESSAGE_IO"; + case MODULE_LOGGER: return "LOGGER"; + case MODULE_LOG_TESTS: return "LOG_TESTS"; + case MODULE_CENTRAL_MGR: return "CENTRAL_MGR"; + case MODULE_HMD_TRACKING: return "HMD_TRACKING"; + case MODULE_CONTROLLERS: return "CONTROLLERS"; + case MODULE_PACKET_IO: return "PACKET_IO"; + case MODULE_DFU: return "DFU"; + case MODULE_CONFIG_TABLES: return "CONFIG_TABLE"; + } + return "UNKNOWN"; + } + + Status Device::GetFWLog(TrackingData::Log& log) + { + bulk_message_request_get_and_clear_event_log request = {0}; + bulk_message_response_get_and_clear_event_log response = {0}; + uint8_t outputMode; + uint8_t verbosity; + uint8_t rolloverMode; + + request.header.wMessageID = DEV_GET_AND_CLEAR_EVENT_LOG; + request.header.dwLength = sizeof(request); + + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + return Status::ERROR_USB_TRANSFER; + } + + if (response.header.dwLength > sizeof(bulk_message_response_get_and_clear_event_log)) + { + DEVICELOGE("Error: Log size (%u) is too big for bulk_message_response_get_and_clear_event_log (%u) - Host->FW API issue", response.header.dwLength, sizeof(bulk_message_response_get_and_clear_event_log)); + return Status::BUFFER_TOO_SMALL; + } + + auto logSize = (response.header.dwLength - sizeof(bulk_message_response_header)); + log.entries = (uint32_t)(logSize / sizeof(device_event_log)); + log.maxEntries = MAX_FW_LOG_BUFFER_ENTRIES; + DEVICELOGV("log size = %d, entries = %d, entry size = %d", logSize, log.entries, sizeof(device_event_log)); + + if (logSize > (MAX_LOG_BUFFER_ENTRIES * MAX_LOG_BUFFER_ENTRY_SIZE)) + { + DEVICELOGE("Error: Log size (%u) is too big for TrackingData::Log (%u)", logSize, (MAX_LOG_BUFFER_ENTRIES * MAX_LOG_BUFFER_ENTRY_SIZE)); + return Status::BUFFER_TOO_SMALL; + } + + __perc_Log_Get_Configuration(LogSourceFW, &outputMode, &verbosity, &rolloverMode); + + if (log.entries == log.maxEntries) + { + DEVICELOGW("Got full FW log buffer (%d entries) - increase GetFWLog call rate to retrieve more FW logs", log.entries); + } + + if (outputMode == LogOutputModeScreen) + { + if (log.entries > 0) + { + DEVICELOGD("Received %d FW log entries", log.entries); + DEVICELOGD("FW: [Verbosity] [Thread] [Function] [Module](Line): "); + } + } + + HostLocalTime localTime = getLocalTime(); + TrackingData::Log::LocalTime localTimeStamp; + + localTimeStamp.year = localTime.year; + localTimeStamp.month = localTime.month; + localTimeStamp.dayOfWeek = localTime.dayOfWeek; + localTimeStamp.day = localTime.day; + localTimeStamp.hour = localTime.hour; + localTimeStamp.minute = localTime.minute; + localTimeStamp.second = localTime.second; + localTimeStamp.milliseconds = localTime.milliseconds; + + for (uint32_t i = 0; i < log.entries; i++) + { + /* remove new line character */ + std::remove(response.bEventLog[i].payload, response.bEventLog[i].payload + 44, '\n'); + + /* Build 8 bytes timestamp from 7 bytes FW timestamp */ + perc::copy(&log.entry[i].timeStamp, response.bEventLog[i].timestamp, sizeof(response.bEventLog[i].timestamp)); + log.entry[i].localTimeStamp = localTimeStamp; + log.entry[i].functionSymbol = response.bEventLog[i].functionSymbol; + log.entry[i].lineNumber = response.bEventLog[i].lineNumber; + perc::copy(&log.entry[i].moduleID, fwModuleID((module_id)response.bEventLog[i].moduleID), MAX_LOG_BUFFER_MODULE_SIZE); + log.entry[i].threadID = response.bEventLog[i].threadID; + log.entry[i].verbosity = (LogVerbosityLevel)response.bEventLog[i].source; + log.entry[i].deviceID = (uint64_t)this; + log.entry[i].payloadSize = response.bEventLog[i].payloadSize; + perc::copy(&log.entry[i].payload, response.bEventLog[i].payload, response.bEventLog[i].payloadSize); + + if (outputMode == LogOutputModeScreen) + { + DEVICELOGD("FW: %012llu %s [%02d] [0x%X] [%s](%d): %s", + log.entry[i].timeStamp, + fwLogVerbosityLevel((LogVerbosityLevel)response.bEventLog[i].source), + response.bEventLog[i].threadID, + response.bEventLog[i].functionSymbol, + fwModuleID((module_id)response.bEventLog[i].moduleID), + response.bEventLog[i].lineNumber, + response.bEventLog[i].payload + ); + } + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + void Device::onTimeout(uintptr_t timerId, const Message & msg) + { + if (mSyncTimeEnabled) + { + SyncTime(); + } + + // Schedule the listener itself for every 500 msec + mDispatcher->scheduleTimer(this, mSyncTimeout, Message(0)); + } + + Status Device::Set6DoFControl(TrackingData::SixDofProfile& profile) + { + bulk_message_request_6dof_control req = {0}; + bulk_message_response_6dof_control res = {0}; + + req.header.wMessageID = SLAM_6DOF_CONTROL; + req.header.dwLength = sizeof(req); + req.bEnable = profile.enabled; + req.bMode = profile.mode; + + DEVICELOGD("Set 6Dof Control %s, Mode 0x%X", (req.bEnable) ? "Enabled" : "Disabled", req.bMode); + Bulk_Message msg((uint8_t*)&req, sizeof(req), (uint8_t*)&res, sizeof(res), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + mDispatcher->sendMessage(&mFsm, msg); + + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("Error Transferring SLAM_6DOF_CONTROL"); + return Status::ERROR_USB_TRANSFER; + } + + return Status::SUCCESS; + } + + Status Device::SetController6DoFControl(bool enabled, uint8_t numOfControllers) + { + if (!mHasBluetooth && enabled) + { + DEVICELOGE("cannot SetController6DoFControl with controllers enabled, there is no bluetooth in the device"); + return Status::NO_BLUETOOTH; + } + bulk_message_request_controller_pose_control req; + bulk_message_response_controller_pose_control res = {0}; + + req.header.wMessageID = CONTROLLER_POSE_CONTROL; + req.header.dwLength = sizeof(req); + req.bEnable = enabled; + req.bMode = 0; + req.bNumControllers = numOfControllers; + + DEVICELOGD("Set Controller 6Dof Control %s for %d controllers", (req.bEnable) ? "Enabled" : "Disabled", req.bNumControllers); + Bulk_Message msg((uint8_t*)&req, sizeof(req), (uint8_t*)&res, sizeof(res), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + mDispatcher->sendMessage(&mFsm, msg); + + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("Error Transferring CONTROLLER_POSE_CONTROL"); + return Status::ERROR_USB_TRANSFER; + } + return Status::SUCCESS; + } + + ProfileType Device::getProfileType(uint8_t sensorID) + { + ProfileType profileType = ProfileTypeMax; + + switch (GET_SENSOR_TYPE(sensorID)) + { + case SensorType::Fisheye: + { + profileType = ProfileType::HMD; + } + break; + + case SensorType::Gyro: + { + if (GET_SENSOR_INDEX(sensorID) == GyroProfile0) + { + profileType = ProfileType::HMD; + } + + if (GET_SENSOR_INDEX(sensorID) == GyroProfile1) + { + profileType = ProfileType::Controller1; + } + + if (GET_SENSOR_INDEX(sensorID) == GyroProfile2) + { + profileType = ProfileType::Controller2; + } + } + break; + + case SensorType::Accelerometer: + { + if (GET_SENSOR_INDEX(sensorID) == AccelerometerProfile0) + { + profileType = ProfileType::HMD; + } + + if (GET_SENSOR_INDEX(sensorID) == AccelerometerProfile1) + { + profileType = ProfileType::Controller1; + } + + if (GET_SENSOR_INDEX(sensorID) == AccelerometerProfile2) + { + profileType = ProfileType::Controller2; + } + } + break; + + default: + break; + } + + return profileType; + } + + Status Device::Reset(void) + { + control_message_request_reset request; + request.header.bmRequestType = (LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE); + request.header.bRequest = CONTROL_USB_RESET; + + Control_Message msg((uint8_t*)&request, sizeof(request)); + + DEVICELOGD("Reseting device"); + + /* Calling onControlMessage */ + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("Error Transferring CONTROL_USB_RESET"); + return Status::ERROR_USB_TRANSFER; + } + + return Status::SUCCESS; + } + + void Device::onExit() + { + std::lock_guard lock(mDeletionMutex); + if (mCleared) + return; + + mFsm.fireEvent(Message(ON_STOP)); + + if (mUsbState >= DEVICE_USB_STATE_CLAIMED) + { + libusb_release_interface(mDevice, INTERFACE_INDEX); + } + + if (mUsbState >= DEVICE_USB_STATE_OPENED) + { + libusb_close(mDevice); + } + + mDispatcher->removeHandler(&mFsm); + mFsm.done(); + mCleared = true; + } + + // ---------------------------------------------------------------------------- + Device::~Device() + { + onExit(); + } + + // ---------------------------------------------------------------------------- + Status Device::GetSupportedProfile(TrackingData::Profile& profile) + { + Status st = Status::SUCCESS; + TrackingData::DeviceInfo deviceInfo; + st = this->GetDeviceInfo(deviceInfo); + if (st != Status::SUCCESS) + { + DEVICELOGE("Error: Failed getting device info (0x%X)", st); + return st; + } + + std::vector videoProfiles; + std::vector gyroProfiles; + std::vector velocimeterProfiles; + std::vector accelerometerProfiles; + TrackingData::SixDofProfile sixDofProfile; + + videoProfiles.resize(deviceInfo.numVideoProfiles); + gyroProfiles.resize(deviceInfo.numGyroProfile); + velocimeterProfiles.resize(deviceInfo.numVelocimeterProfile); + accelerometerProfiles.resize(deviceInfo.numAccelerometerProfiles); + + st = this->GetSupportedRawStreams(videoProfiles.data(), gyroProfiles.data(), accelerometerProfiles.data(), velocimeterProfiles.data()); + if (st != Status::SUCCESS) + { + DEVICELOGE("Error: Failed getting supported stream (0x%X)", st); + return st; + } + + for (uint8_t i = 0; i < GyroProfileMax; i++) + { + for (auto it = gyroProfiles.begin(); it != gyroProfiles.end(); ++it) + { + if ((*it).sensorIndex == i) + { + /* Adding the first Gyro profile on every Gyro index or the requested gyro profile according to the FPS */ + if ((profile.gyro[i].fps == 0) || ((*it).fps == profile.gyro[i].fps)) + { + profile.set((*it), false, false); + gyroProfiles.erase(it); + break; + } + } + } + } + + for (uint8_t i = 0; i < AccelerometerProfileMax; i++) + { + for (auto it = accelerometerProfiles.begin(); it != accelerometerProfiles.end(); ++it) + { + if ((*it).sensorIndex == i) + { + /* Adding the first Accelerometer profile on every Accelerometer index or the requested Accelerometer profile according to the FPS */ + if ((profile.accelerometer[i].fps == 0) || ((*it).fps == profile.accelerometer[i].fps)) + { + profile.set((*it), false, false); + accelerometerProfiles.erase(it); + break; + } + } + } + } + + for (uint8_t i = 0; i < VelocimeterProfileMax; i++) + { + for (auto it = velocimeterProfiles.begin(); it != velocimeterProfiles.end(); ++it) + { + if ((*it).sensorIndex == i) + { + /* Adding the first Velocimeter profile on every Velocimeter index or the requested Velocimeter profile according to the FPS */ + profile.set((*it), false, false); + velocimeterProfiles.erase(it); + break; + } + } + } + + for (uint8_t i = 0; i < VideoProfileMax; i++) + { + for (auto it = videoProfiles.begin(); it != videoProfiles.end(); ++it) + { + /* Choose Video profile from supported list according to: Height+Width+FPS or default (first) profile */ + if ((*it).sensorIndex == i) + { + if ((profile.video[i].profile.height * profile.video[i].profile.width * profile.video[i].fps) != 0) + { + /* Adding the requested Video profile from this Video index according to the Height, Width, FPS */ + if ((profile.video[i].profile.height == (*it).profile.height) && (profile.video[i].profile.width == (*it).profile.width) && (profile.video[i].fps == (*it).fps)) + { + profile.set((*it), false, false); + videoProfiles.erase(it); + break; + } + } + else + { + /* Adding the default (first) Video profile from this Video index */ + profile.set((*it), false, false); + videoProfiles.erase(it); + break; + } + } + } + } + + for (uint8_t i = 0; i < SixDofProfileMax; i++) + { + sixDofProfile.set(false, (SIXDOF_MODE_ENABLE_MAPPING | SIXDOF_MODE_ENABLE_RELOCALIZATION), SIXDOF_INTERRUPT_RATE::SIXDOF_INTERRUPT_RATE_IMU, (SixDofProfileType)i); + profile.set(sixDofProfile, false); + } + + profile.playbackEnabled = false; + + return st; + } + + // ---------------------------------------------------------------------------- + Status Device::Start(Listener* listener, TrackingData::Profile* profile) + { + mSyncTimeEnabled = true; + + if (profile != NULL) + { + supported_raw_stream_libtm_message profiles[MAX_SUPPORTED_STREAMS] = { 0 }; + Status status = Status::SUCCESS; + uint8_t sixdofControllerNum = 0; + bool sixdofControllerEnabled = false; + int activeProfiles = 0; + + status = SetPlayback(profile->playbackEnabled); + if (status != Status::SUCCESS) + { + DEVICELOGE("Error: Failed setting playback (0x%X)", status); + return status; + } + + /* Video Profiles */ + for (uint8_t i = 0; i < VideoProfileMax; i++) + { + if (profile->video[i].enabled) + { + profiles[activeProfiles].wFramesPerSecond = profile->video[i].fps; + profiles[activeProfiles].bOutputMode = profile->video[i].outputEnabled; + profiles[activeProfiles].wWidth = profile->video[i].profile.width; + profiles[activeProfiles].wHeight = profile->video[i].profile.height; + profiles[activeProfiles].wStride = profile->video[i].profile.stride; + profiles[activeProfiles].bPixelFormat = profile->video[i].profile.pixelFormat; + profiles[activeProfiles].bSensorID = SET_SENSOR_ID(SensorType::Fisheye, i); + activeProfiles++; + } + } + + /* Gyro Profiles */ + for (uint8_t i = 0; i < GyroProfileMax; i++) + { + if (profile->gyro[i].enabled) + { + profiles[activeProfiles].bSensorID = SET_SENSOR_ID(SensorType::Gyro, i); + profiles[activeProfiles].bOutputMode = profile->gyro[i].outputEnabled; + profiles[activeProfiles].wFramesPerSecond = profile->gyro[i].fps; + activeProfiles++; + } + } + + /* Velocimeter Profiles */ + for (uint8_t i = 0; i < VelocimeterProfileMax; i++) + { + if (profile->velocimeter[i].enabled) + { + profiles[activeProfiles].bSensorID = SET_SENSOR_ID(SensorType::Velocimeter, i); + profiles[activeProfiles].bOutputMode = profile->velocimeter[i].outputEnabled; + profiles[activeProfiles].wFramesPerSecond = 0; + activeProfiles++; + } + } + + /* Accelerometer Profiles */ + for (uint8_t i = 0; i < AccelerometerProfileMax; i++) + { + if (profile->accelerometer[i].enabled) + { + profiles[activeProfiles].bSensorID = SET_SENSOR_ID(SensorType::Accelerometer, i); + profiles[activeProfiles].bOutputMode = profile->accelerometer[i].outputEnabled; + profiles[activeProfiles].wFramesPerSecond = profile->accelerometer[i].fps; + activeProfiles++; + } + } + + if (activeProfiles > 0) + { + status = SetEnabledRawStreams(profiles, activeProfiles); + if (status != Status::SUCCESS) + { + DEVICELOGE("Error: Failed setting Supported Raw Streams (0x%X)", status); + return status; + } + } + + /* 6dof HMD profile */ + status = Set6DoFControl(profile->sixDof[SixDofProfile0]); + if (status != Status::SUCCESS) + { + DEVICELOGE("Error: Failed setting 6dof Control (0x%X)", status); + return status; + } + + /* 6dof Controllers profile */ + for (uint8_t i = (SixDofProfile0+1); i < SixDofProfileMax; i++) + { + if (profile->sixDof[i].enabled == true) + { + sixdofControllerNum++; + sixdofControllerEnabled = true; + } + } + + status = SetController6DoFControl(sixdofControllerEnabled, sixdofControllerNum); + if (status != Status::SUCCESS) + { + DEVICELOGE("Error: Failed setting Controller 6dof Control (0x%X)", status); + return status; + } + + + /* Currently FW doesn't support setting interrupt rate */ + //sixdof_interrupt_rate_libtm_message msgIntRate; + //msgIntRate.bInterruptRate = toUnderlying(profile->sixDof[SixDofHMD].interruptRate); + //status = Set6DofInterruptRate(msgIntRate); + //if (status != Status::SUCCESS) + // return status; + + } + + MessageON_START msg(listener); + mDispatcher->sendMessage(&mFsm, msg); + + if (msg.Result != 0) + { + mSyncTimeEnabled = false; + } + + return msg.Result == 0 ? Status::SUCCESS : Status::COMMON_ERROR; + } + + // ---------------------------------------------------------------------------- + Status Device::Stop(void) + { + mSyncTimeEnabled = false; + + Message msg(ON_STOP); + mDispatcher->sendMessage(&mFsm, msg); + + return msg.Result == 0 ? Status::SUCCESS : Status::COMMON_ERROR; + } + + Status Device::GetSupportedRawStreams(TrackingData::VideoProfile* videoProfilesBuffer, TrackingData::GyroProfile* gyroProfilesBuffer, TrackingData::AccelerometerProfile* accelerometerProfilesBuffer, TrackingData::VelocimeterProfile* velocimeterProfilesBuffer) + { + unsigned int count = 0; + for (auto p : mVideoProfiles) + { + videoProfilesBuffer[count++] = p; + } + + count = 0; + for (auto p : mAccelerometerProfiles) + { + accelerometerProfilesBuffer[count++] = p; + } + + count = 0; + for (auto p : mGyroProfiles) + { + gyroProfilesBuffer[count++] = p; + } + + if (velocimeterProfilesBuffer != nullptr) + { + count = 0; + for (auto p : mVelocimeterProfiles) + { + velocimeterProfilesBuffer[count++] = p; + } + } + + return Status::SUCCESS; + } + + void Device::printSupportedRawStreams(supported_raw_stream_libtm_message* pRawStreams, uint32_t rawStreamsCount) + { + DEVICELOGD("---+----------------------------+--------+-------+--------+--------+--------+--------+------"); + DEVICELOGD(" # | Sensor | Frames | Width | Height | Pixel | Output | Stride | Rsvd "); + DEVICELOGD(" | Name | Type | Idx | PerSec | | | Format | Mode | | "); + DEVICELOGD("---+---------------+------+-----+--------+-------+--------+--------+--------+--------+------"); + + for (uint32_t i = 0; i < rawStreamsCount; i++) + { + DEVICELOGD("%02d | %-13s | 0x%02X | %01d | %-6d | %-5d | %-5d | %-3d | 0x%01X | %-3d | %-3d", i, sensorToString(SensorType(GET_SENSOR_TYPE(pRawStreams[i].bSensorID))).c_str(), GET_SENSOR_TYPE(pRawStreams[i].bSensorID), GET_SENSOR_INDEX(pRawStreams[i].bSensorID), + pRawStreams[i].wFramesPerSecond, pRawStreams[i].wWidth, pRawStreams[i].wHeight, pRawStreams[i].bPixelFormat, pRawStreams[i].bOutputMode, pRawStreams[i].wStride, pRawStreams[i].bReserved); + } + DEVICELOGD("---+---------------+------+-----+--------+-------+--------+--------+--------+--------+------"); + DEVICELOGD(""); + } + + Status Device::GetSupportedRawStreamsInternal(supported_raw_stream_libtm_message * message, uint16_t wBufferSize, uint16_t& wNumSupportedStreams) + { + wNumSupportedStreams = 0; + + if (!message) + { + return Status::ERROR_PARAMETER_INVALID; + } + + bulk_message_request_get_supported_raw_streams req; + + req.header.dwLength = sizeof(req); + req.header.wMessageID = DEV_GET_SUPPORTED_RAW_STREAMS; + + uint8_t responseBuffer[BUFFER_SIZE]; + + Bulk_Message msg((uint8_t*)&req, sizeof(req), responseBuffer, BUFFER_SIZE, mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mFsm.fireEvent(msg); + + bulk_message_response_get_supported_raw_streams* res = (bulk_message_response_get_supported_raw_streams*)responseBuffer; + + uint16_t streamsToCopy; + Status st = Status::SUCCESS; + + if (res->header.wStatus != 0) + return fwToHostStatus((MESSAGE_STATUS)res->header.wStatus); + + if (wBufferSize > res->wNumSupportedStreams) + { + streamsToCopy = res->wNumSupportedStreams; + } + else + { + streamsToCopy = wBufferSize; + st = Status::BUFFER_TOO_SMALL; + } + + wNumSupportedStreams = streamsToCopy; + + perc::copy(message, res->stream, streamsToCopy*sizeof(supported_raw_stream_libtm_message)); + + DEVICELOGD("Get Supported RAW Streams (%d)", streamsToCopy); + printSupportedRawStreams(res->stream, streamsToCopy); + + return st; + } + + void Device::AllocateBuffers() + { + while (mFramesBuffersLists.size() > 0) + { + mFramesBuffersLists.pop_front(); + } + + for (int j = 0; j < FRAMES_PER_STREAM; j++) + { + mFramesBuffersLists.push_back(std::shared_ptr(new uint8_t[mFrameTempBufferSize], arrayDeleter())); + DEVICELOGV("frame buffers pushed back - %p", mFramesBuffersLists.back().get()); + } + } + + Status Device::SetEnabledRawStreams(supported_raw_stream_libtm_message * message, uint16_t wNumEnabledStreams) + { + if (!message) + { + return Status::ERROR_PARAMETER_INVALID; + } + + MessageON_SET_ENABLED_STREAMS setMsg(message, wNumEnabledStreams); + + mDispatcher->sendMessage(&mFsm, setMsg); + + return setMsg.Result == 0 ? Status::SUCCESS : Status::COMMON_ERROR; + } + + Status Device::SetFWLogControl(const TrackingData::LogControl& logControl) + { + /* FW log control */ + bulk_message_request_log_control request = {0}; + bulk_message_response_log_control response = {0}; + + __perc_Log_Set_Configuration(LogSourceFW, logControl.outputMode, logControl.verbosity, logControl.rolloverMode); + + request.header.wMessageID = DEV_LOG_CONTROL; + request.header.dwLength = sizeof(request); + request.bVerbosity = logControl.verbosity; + request.bLogMode = logControl.rolloverMode; + + DEVICELOGD("Set FW Log Control, output to %s, verbosity = 0x%X, Rollover mode = 0x%X", (logControl.outputMode == LogOutputModeScreen) ? "Screen" : "Buffer", request.bVerbosity, request.bLogMode); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::GetCameraIntrinsics(SensorId id, TrackingData::CameraIntrinsics& intrinsics) + { + bulk_message_request_get_camera_intrinsics request = {0}; + bulk_message_response_get_camera_intrinsics response = {0}; + + if ((GET_SENSOR_TYPE(id) != SensorType::Color) && + (GET_SENSOR_TYPE(id) != SensorType::Depth) && + (GET_SENSOR_TYPE(id) != SensorType::IR) && + (GET_SENSOR_TYPE(id) != SensorType::Fisheye)) + { + DEVICELOGE("Unsupported SensorId (0x%X)", id); + return Status::ERROR_PARAMETER_INVALID; + } + + request.header.wMessageID = DEV_GET_CAMERA_INTRINSICS; + request.header.dwLength = sizeof(request); + request.bCameraID = id; + + DEVICELOGD("Get camera intrinsics for sensor [%d,%d]", GET_SENSOR_TYPE(id), GET_SENSOR_INDEX(id)); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + if ((MESSAGE_STATUS)response.header.wStatus == MESSAGE_STATUS::SUCCESS) + { + intrinsics = cameraIntrinsicsMessageToClass(response.intrinsics); + DEVICELOGD("Width = %d", intrinsics.width); + DEVICELOGD("Height = %d", intrinsics.height); + DEVICELOGD("Ppx = %f", intrinsics.ppx); + DEVICELOGD("Ppy = %f", intrinsics.ppy); + DEVICELOGD("Fx = %f", intrinsics.fx); + DEVICELOGD("Fy = %f", intrinsics.fy); + DEVICELOGD("DistortionModel = %d", intrinsics.distortionModel); + for (int i = 0; i < 5; i++) + { + DEVICELOGD("Coeffs[%d] = %f", i, intrinsics.coeffs[i]); + } + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::SetCameraIntrinsics(SensorId id, const TrackingData::CameraIntrinsics& intrinsics) + { + bulk_message_request_set_camera_intrinsics request = {0}; + bulk_message_response_set_camera_intrinsics response = {0}; + + if ((GET_SENSOR_TYPE(id) != SensorType::Color) && + (GET_SENSOR_TYPE(id) != SensorType::Depth) && + (GET_SENSOR_TYPE(id) != SensorType::IR) && + (GET_SENSOR_TYPE(id) != SensorType::Fisheye)) + { + DEVICELOGE("Unsupported SensorId (0x%X)", id); + return Status::ERROR_PARAMETER_INVALID; + } + + request.header.wMessageID = DEV_SET_CAMERA_INTRINSICS; + request.header.dwLength = sizeof(request); + request.bCameraID = id; + + request.intrinsics.dwWidth = intrinsics.width; + request.intrinsics.dwHeight = intrinsics.height; + request.intrinsics.flPpx = intrinsics.ppx; + request.intrinsics.flPpy = intrinsics.ppy; + request.intrinsics.flFx = intrinsics.fx; + request.intrinsics.flFy = intrinsics.fy; + request.intrinsics.dwDistortionModel = intrinsics.distortionModel; + for (uint8_t i = 0; i < 5; i++) + { + request.intrinsics.flCoeffs[i] = intrinsics.coeffs[i]; + } + + DEVICELOGD("Set camera intrinsics for sensor [%d,%d]", GET_SENSOR_TYPE(request.bCameraID), GET_SENSOR_INDEX(request.bCameraID)); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + if ((MESSAGE_STATUS)response.header.wStatus == MESSAGE_STATUS::SUCCESS) + { + DEVICELOGD("Width = %d", request.intrinsics.dwWidth); + DEVICELOGD("Height = %d", request.intrinsics.dwHeight); + DEVICELOGD("Ppx = %f", request.intrinsics.flPpx); + DEVICELOGD("Ppy = %f", request.intrinsics.flPpy); + DEVICELOGD("Fx = %f", request.intrinsics.flFx); + DEVICELOGD("Fy = %f", request.intrinsics.flFy); + DEVICELOGD("DistortionModel = %d", request.intrinsics.dwDistortionModel); + for (int i = 0; i < 5; i++) + { + DEVICELOGD("Coeffs[%d] = %f", i, request.intrinsics.flCoeffs[i]); + } + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::GetMotionModuleIntrinsics(SensorId id, TrackingData::MotionIntrinsics& intrinsics) + { + bulk_message_request_get_motion_intrinsics request = {0}; + bulk_message_response_get_motion_intrinsics response = {0}; + + if ((GET_SENSOR_TYPE(id) != SensorType::Gyro) && (GET_SENSOR_TYPE(id) != SensorType::Accelerometer)) + { + DEVICELOGE("Unsupported SensorId (0x%X)", id); + return Status::ERROR_PARAMETER_INVALID; + } + + request.header.wMessageID = DEV_GET_MOTION_INTRINSICS; + request.header.dwLength = sizeof(request); + request.bMotionID = id; + + DEVICELOGD("Get motion module intrinsics for sensor [%d,%d]", GET_SENSOR_TYPE(id), GET_SENSOR_INDEX(id)); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + if ((MESSAGE_STATUS)response.header.wStatus == MESSAGE_STATUS::SUCCESS) + { + intrinsics = motionIntrinsicsMessageToClass(response.intrinsics); + + for (uint8_t i = 0; i < 3; i++) + { + for (uint8_t j = 0; j < 4; j++) + { + DEVICELOGD("Data[%d][%d] = %f", i, j, intrinsics.data[i][j]); + } + } + + for (uint8_t i = 0; i < 3; i++) + { + DEVICELOGD("NoiseVariances[%d] = %f", i, intrinsics.noiseVariances[i]); + } + + for (uint8_t i = 0; i < 3; i++) + { + DEVICELOGD("BiasVariances[%d] = %f", i, intrinsics.biasVariances[i]); + } + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::SetMotionModuleIntrinsics(SensorId id, const TrackingData::MotionIntrinsics& intrinsics) + { + bulk_message_request_set_motion_intrinsics request = {0}; + bulk_message_response_set_motion_intrinsics response = {0}; + + if ((GET_SENSOR_TYPE(id) != SensorType::Gyro) && (GET_SENSOR_TYPE(id) != SensorType::Accelerometer)) + { + DEVICELOGE("Unsupported SensorId (0x%X)", id); + return Status::ERROR_PARAMETER_INVALID; + } + + request.header.wMessageID = DEV_SET_MOTION_INTRINSICS; + request.header.dwLength = sizeof(request); + request.bMotionID = id; + request.intrinsics = motionIntrinsicsClassToMessage(intrinsics); + + DEVICELOGD("Set motion module intrinsics for sensor [%d,%d]", GET_SENSOR_TYPE(id), GET_SENSOR_INDEX(id)); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + if ((MESSAGE_STATUS)response.header.wStatus == MESSAGE_STATUS::SUCCESS) + { + for (uint8_t i = 0; i < 3; i++) + { + for (uint8_t j = 0; j < 4; j++) + { + DEVICELOGD("Data[%d][%d] = %f", i, j, request.intrinsics.flData[i][j]); + } + } + + for (uint8_t i = 0; i < 3; i++) + { + DEVICELOGD("NoiseVariances[%d] = %f", i, request.intrinsics.flNoiseVariances[i]); + } + + for (uint8_t i = 0; i < 3; i++) + { + DEVICELOGD("BiasVariances[%d] = %f", i, request.intrinsics.flBiasVariances[i]); + } + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::GetExtrinsics(SensorId id, TrackingData::SensorExtrinsics& extrinsics) + { + bulk_message_request_get_extrinsics request; + bulk_message_response_get_extrinsics response = {0}; + + if (GET_SENSOR_TYPE(id) >= SensorType::Max) + { + DEVICELOGE("Unsupported SensorId (0x%X)", id); + return Status::ERROR_PARAMETER_INVALID; + } + + request.header.wMessageID = DEV_GET_EXTRINSICS; + request.header.dwLength = sizeof(request); + request.bSensorID = id; + + DEVICELOGD("Get Extrinsics pose for sensor [%d,%d]", GET_SENSOR_TYPE(id), GET_SENSOR_INDEX(id)); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + if ((MESSAGE_STATUS)response.header.wStatus == MESSAGE_STATUS::SUCCESS) + { + extrinsics = sensorExtrinsicsMessageToClass(response.extrinsics); + + DEVICELOGD("Reference sensor [%d,%d]", GET_SENSOR_TYPE(extrinsics.referenceSensorId), GET_SENSOR_INDEX(extrinsics.referenceSensorId)); + + for (uint8_t i = 0; i < 9; i++) + { + DEVICELOGD("Rotation[%d] = %f", i, extrinsics.rotation[i]); + } + + for (uint8_t i = 0; i < 3; i++) + { + DEVICELOGD("Translation[%d] = %f", i, extrinsics.translation[i]); + } + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::SetOccupancyMapControl(uint8_t enable) + { + bulk_message_request_occupancy_map_control request = {0}; + bulk_message_response_occupancy_map_control response = {0}; + + request.header.wMessageID = SLAM_OCCUPANCY_MAP_CONTROL; + request.header.dwLength = sizeof(request); + request.bEnable = enable; + + DEVICELOGD("Set Occupancy Map Control %s", (request.bEnable) ? "Enabled" : "Disabled"); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::GetPose(TrackingData::PoseFrame& pose, uint8_t sourceIndex) + { + bulk_message_request_get_pose request = {0}; + bulk_message_response_get_pose response = {0}; + + request.header.wMessageID = DEV_GET_POSE; + request.header.dwLength = sizeof(request); + request.bIndex = sourceIndex; + + DEVICELOGD("Get Pose"); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + if ((MESSAGE_STATUS)response.header.wStatus == MESSAGE_STATUS::SUCCESS) + { + pose = poseMessageToClass(response.pose, request.bIndex, response.pose.llNanoseconds + mTM2CorrelatedTimeStampShift); + DEVICELOGD("SourceIndex = 0x%X", pose.sourceIndex); + DEVICELOGD("Timestamp %" PRId64 "", pose.timestamp); + DEVICELOGD("SystemTimestamp %" PRId64 "", pose.systemTimestamp); + DEVICELOGD("ArrivalTimeStamp %" PRId64 "", pose.arrivalTimeStamp); + DEVICELOGD("translation [X,Y,Z] = [%f,%f,%f]", pose.translation.x, pose.translation.y, pose.translation.z); + DEVICELOGD("rotation [I,J,K,R] = [%f,%f,%f,%f]", pose.rotation.i, pose.rotation.j, pose.rotation.k, pose.rotation.r); + DEVICELOGD("velocity [X,Y,Z] = [%f,%f,%f]", pose.velocity.x, pose.velocity.y, pose.velocity.z); + DEVICELOGD("angularVelocity [X,Y,Z] = [%f,%f,%f]", pose.angularVelocity.x, pose.angularVelocity.y, pose.angularVelocity.z); + DEVICELOGD("acceleration [X,Y,Z] = [%f,%f,%f]", pose.acceleration.x, pose.acceleration.y, pose.acceleration.z); + DEVICELOGD("angularAcceleration [X,Y,Z] = [%f,%f,%f]", pose.angularAcceleration.x, pose.angularAcceleration.y, pose.angularAcceleration.z); + DEVICELOGD("trackerConfidence = 0x%X", pose.trackerConfidence); + DEVICELOGD("mapperConfidence = 0x%X", pose.mapperConfidence); + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::SetExposureModeControl(const TrackingData::ExposureModeControl& mode) + { + bulk_message_request_set_exposure_mode_control request = {0}; + bulk_message_response_set_exposure_mode_control response = {0}; + + request.header.wMessageID = DEV_EXPOSURE_MODE_CONTROL; + request.header.dwLength = sizeof(request); + request.bAntiFlickerMode = mode.antiFlickerMode; + + for (uint8_t i = 0; i < VideoProfileMax; i++) + { + if (mode.videoStreamAutoExposure[i] == true) + { + request.bVideoStreamsMask |= (1 << i); + } + } + + DEVICELOGD("Set Exposure Mode Control, bStreamsMask = 0x%X, AntiFlickerMode = 0x%X", request.bVideoStreamsMask, request.bAntiFlickerMode); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::SetExposure(const TrackingData::Exposure& exposure) + { + if (exposure.numOfVideoStreams > MAX_VIDEO_STREAMS) + { + DEVICELOGE("numOfVideoStreams (%d) too big, max supported %d streams", exposure.numOfVideoStreams, MAX_VIDEO_STREAMS); + return Status::ERROR_PARAMETER_INVALID; + } + + bulk_message_request_set_exposure request = {0}; + bulk_message_response_set_exposure response = {0}; + + uint32_t messageLength = offsetof(bulk_message_request_set_exposure, stream) + exposure.numOfVideoStreams * sizeof(stream_exposure); + + request.header.wMessageID = DEV_SET_EXPOSURE; + request.header.dwLength = messageLength; + request.bNumOfVideoStreams = exposure.numOfVideoStreams; + + for (uint8_t i = 0; i < exposure.numOfVideoStreams; i++) + { + if (GET_SENSOR_TYPE(exposure.stream[i].cameraId) > SensorType::Fisheye) + { + DEVICELOGE("stream[%d] = Unsupported cameraId (0x%X)", i, exposure.stream[i].cameraId); + return Status::ERROR_PARAMETER_INVALID; + } + + request.stream[i].bCameraID = exposure.stream[i].cameraId; + request.stream[i].dwIntegrationTime = exposure.stream[i].integrationTime; + request.stream[i].fGain = exposure.stream[i].gain; + } + + DEVICELOGD("Set Exposure for %d video streams", request.bNumOfVideoStreams); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + if ((MESSAGE_STATUS)response.header.wStatus == MESSAGE_STATUS::SUCCESS) + { + for (uint32_t i = 0; i < request.bNumOfVideoStreams; i++) + { + DEVICELOGD("VideoStream [%d]: cameraId 0x%X, dwIntegrationTime %d (usec), Gain %f", i, request.stream[i].bCameraID, request.stream[i].dwIntegrationTime, request.stream[i].fGain); + } + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::GetTemperature(TrackingData::Temperature& temperature) + { + uint8_t responseBuffer[BUFFER_SIZE]; + bulk_message_request_get_temperature request; + bulk_message_response_get_temperature* response = (bulk_message_response_get_temperature*)responseBuffer; + + request.header.wMessageID = DEV_GET_TEMPERATURE; + request.header.dwLength = sizeof(request); + + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)response, BUFFER_SIZE, mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST, USB_TRANSFER_MEDIUM_TIMEOUT_MS); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + if ((MESSAGE_STATUS)response->header.wStatus == MESSAGE_STATUS::SUCCESS) + { + if (response->dwCount > TemperatureSensorMax) + { + DEVICELOGE("Error: Got Temperature for %u sensors, exceeding max sensors (%d)", response->dwCount, TemperatureSensorMax); + return Status::ERROR_PARAMETER_INVALID; + } + + for (uint8_t i = 0; i < response->dwCount; i++) + { + if (response->temperature[i].dwIndex >= TemperatureSensorMax) + { + DEVICELOGE("Error: Temperature Sensor (%d) is unknown, max temperature sensor = 0x%X", temperature.sensor[i].index, TemperatureSensorMax - 1); + return Status::ERROR_PARAMETER_INVALID; + } + } + + temperature.numOfSensors = response->dwCount; + DEVICELOGD("Got Temperature for %u sensors, (Status 0x%X)", temperature.numOfSensors, response->header.wStatus); + if (temperature.numOfSensors > 0) + { + DEVICELOGD("---+-------------+-------------+-----------"); + DEVICELOGD(" # | Sensor | Temperature | Threshold "); + DEVICELOGD(" | Info | Idx | (Celsius) | (Celsius) "); + DEVICELOGD("---+------+------+-------------+-----------"); + + for (uint32_t i = 0; i < temperature.numOfSensors; i++) + { + temperature.sensor[i].index = (TemperatureSensorType)response->temperature[i].dwIndex; + temperature.sensor[i].temperature = response->temperature[i].fTemperature; + temperature.sensor[i].threshold = response->temperature[i].fThreshold; + + DEVICELOGD("%02d | %s | 0x%02X | %-11.2f | %.2f", i, temperatureSensor(temperature.sensor[i].index), temperature.sensor[i].index, temperature.sensor[i].temperature, temperature.sensor[i].threshold); + } + DEVICELOGD("---+------+------+-------------+-----------"); + } + } + + return fwToHostStatus((MESSAGE_STATUS)response->header.wStatus); + } + + Status Device::SetTemperatureThreshold(const TrackingData::Temperature& temperature, uint32_t token) + { + if (temperature.numOfSensors > TemperatureSensorMax) + { + DEVICELOGE("Error: numOfSensors (%u) is too big, max supported sensors = %d", temperature.numOfSensors, TemperatureSensorMax); + return Status::ERROR_PARAMETER_INVALID; + } + + for (uint8_t i = 0; i < temperature.numOfSensors; i++) + { + if (temperature.sensor[i].index >= TemperatureSensorMax) + { + DEVICELOGE("Error: Temperature Sensor (%d) is unknown, max temperature sensor = 0x%X", temperature.sensor[i].index, TemperatureSensorMax-1); + return Status::ERROR_PARAMETER_INVALID; + } + } + + uint8_t requestBuffer[BUFFER_SIZE]; + bulk_message_request_set_temperature_threshold* request = (bulk_message_request_set_temperature_threshold*)requestBuffer; + bulk_message_response_set_temperature_threshold response = {0}; + + request->header.wMessageID = DEV_SET_TEMPERATURE_THRESHOLD; + request->header.dwLength = offsetof(bulk_message_request_set_temperature_threshold, temperature) + temperature.numOfSensors * sizeof(sensor_set_temperature); + request->wForceToken = token; + request->dwCount = temperature.numOfSensors; + + for (uint8_t i = 0; i < temperature.numOfSensors; i++) + { + request->temperature[i].dwIndex = temperature.sensor[i].index; + request->temperature[i].fThreshold = temperature.sensor[i].threshold; + } + + DEVICELOGD("Set Temperature threshold for %d sensors, Token = 0x%X", temperature.numOfSensors, request->wForceToken); + Bulk_Message msg((uint8_t*)request, request->header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + if ((MESSAGE_STATUS)response.header.wStatus == MESSAGE_STATUS::SUCCESS) + { + DEVICELOGD("---+-------------+-----------"); + DEVICELOGD(" # | Sensor | Threshold "); + DEVICELOGD(" | Type | Idx | (Celsius) "); + DEVICELOGD("---+------+------+-----------"); + + for (uint32_t i = 0; i < request->dwCount; i++) + { + DEVICELOGD("%02d | %s | 0x%02X | %.2f", i, temperatureSensor(request->temperature[i].dwIndex), request->temperature[i].dwIndex, request->temperature[i].fThreshold); + } + + DEVICELOGD("---+------+------+-----------"); + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::LockConfiguration(LockType type, bool lock) + { + bulk_message_request_lock_configuration request = {0}; + bulk_message_response_lock_configuration response = {0}; + + switch (type) + { + case HardwareLock: + request.header.wMessageID = DEV_LOCK_EEPROM; + break; + case SoftwareLock: + request.header.wMessageID = DEV_LOCK_CONFIGURATION; + break; + default: + DEVICELOGE("Error: unknown lock type (0x%X)", type); + return Status::ERROR_PARAMETER_INVALID; + break; + } + + request.header.dwLength = sizeof(request); + request.dwLock = uint32_t(lock); + + DEVICELOGD("Set %s Configuration Lock: Lock = %s", (request.header.wMessageID == DEV_LOCK_EEPROM)?"Hardware":"Software", (lock == true) ? "True" : "False"); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::PermanentLockConfiguration(LockType type, uint32_t token) + { + if (token != PERMANENT_LOCK_CONFIGURATION_TOKEN) + { + DEVICELOGE("Error: Bad token (0x%X)", token); + return Status::ERROR_PARAMETER_INVALID; + } + + bulk_message_request_lock_configuration request = {0}; + bulk_message_response_lock_configuration response = {0}; + + switch (type) + { + case HardwareLock: + request.header.wMessageID = DEV_LOCK_EEPROM; + break; + case SoftwareLock: + request.header.wMessageID = DEV_LOCK_CONFIGURATION; + break; + default: + DEVICELOGE("Error: unknown lock type (0x%X)", type); + return Status::ERROR_PARAMETER_INVALID; + break; + } + + request.header.wMessageID = DEV_LOCK_CONFIGURATION; + request.header.dwLength = sizeof(request); + request.dwLock = token; + + DEVICELOGD("Set Permanent %s Configuration Lock", (request.header.wMessageID == DEV_LOCK_EEPROM) ? "Hardware" : "Software"); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::ReadConfiguration(uint16_t configurationId, uint16_t size, uint8_t* buffer, uint16_t* actualSize) + { + if ((buffer == NULL) || (size == 0) || (size > MAX_CONFIGURATION_SIZE)) + { + DEVICELOGE("Error: Invalid parameters: buffer = 0x%p, size = %d", buffer, size); + return Status::ERROR_PARAMETER_INVALID; + } + + uint8_t responseBuffer[BUFFER_SIZE] = {0}; + bulk_message_request_read_configuration request; + bulk_message_response_read_configuration* response = (bulk_message_response_read_configuration*)responseBuffer; + + request.header.wMessageID = DEV_READ_CONFIGURATION; + request.header.dwLength = sizeof(request); + request.wTableId = configurationId; + + DEVICELOGD("Set Read Configuration: configurationId = 0x%X, size = %d", configurationId, size); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)response, BUFFER_SIZE, mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST, USB_TRANSFER_MEDIUM_TIMEOUT_MS); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + if ((MESSAGE_STATUS)response->header.wStatus == MESSAGE_STATUS::SUCCESS) + { + uint32_t bufferSize = response->header.dwLength - offsetof(bulk_message_response_read_configuration, bTable); + if (size < bufferSize) + { + DEVICELOGE("Error: Buffer size %d is too small: EEPROM table %d needs at least %d bytes buffer", size, configurationId, bufferSize); + return Status::BUFFER_TOO_SMALL; + } + + perc::copy(buffer, response->bTable, bufferSize); + + if (actualSize != nullptr) + { + *actualSize = bufferSize; + } + } + + return fwToHostStatus((MESSAGE_STATUS)response->header.wStatus); + } + + Status Device::WriteConfiguration(uint16_t configurationId, uint16_t size, uint8_t* buffer) + { + if ((buffer == NULL) || (size == 0) || (size > MAX_CONFIGURATION_SIZE)) + { + DEVICELOGE("Error: Invalid parameters: buffer = 0x%p, size = %d", buffer, size); + return Status::ERROR_PARAMETER_INVALID; + } + + uint8_t requestBuffer[BUFFER_SIZE] = {0}; + bulk_message_request_write_configuration* request = (bulk_message_request_write_configuration*)requestBuffer; + bulk_message_response_write_configuration response = {0}; + + request->header.wMessageID = DEV_WRITE_CONFIGURATION; + request->header.dwLength = size + offsetof(bulk_message_request_write_configuration, bTable); + request->wTableId = configurationId; + perc::copy(request->bTable, buffer, size); + + DEVICELOGD("Set Write Configuration: configurationId = 0x%X, size = %d", configurationId, size); + Bulk_Message msg((uint8_t*)request, request->header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST, USB_TRANSFER_MEDIUM_TIMEOUT_MS); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::DeleteConfiguration(uint16_t configurationId) + { + bulk_message_request_reset_configuration request = {0}; + bulk_message_response_reset_configuration response = {0}; + + request.header.wMessageID = DEV_RESET_CONFIGURATION; + request.header.dwLength = sizeof(request); + request.wTableId = configurationId; + + DEVICELOGD("Set Delete Configuration: configurationId = 0x%X", configurationId); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::SetTimeoutConfiguration(uint16_t timeoutMsec) + { + bulk_message_request_timeout_configuration request = {0}; + bulk_message_response_timeout_configuration response = {0}; + + request.header.wMessageID = DEV_TIMEOUT_CONFIGURATION; + request.header.dwLength = sizeof(request); + request.wTimeoutInMillis = timeoutMsec; + + DEVICELOGD("Set Timeout Configuration: timeoutMsec = %d", timeoutMsec); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::GetLocalizationData(Listener* listener) + { + if (listener == NULL) + { + DEVICELOGE("Error: Invalid parameters: listener = %p, length = %d", listener); + return Status::ERROR_PARAMETER_INVALID; + } + + MessageON_ASYNC_START setMsg(listener, SLAM_GET_LOCALIZATION_DATA, 0, NULL); + mDispatcher->sendMessage(&mFsm, setMsg); + + return setMsg.Result == 0 ? Status::SUCCESS : Status::COMMON_ERROR; + } + + Status Device::SetLocalizationData(Listener* listener, uint32_t length, const uint8_t* buffer) + { + if ((listener == NULL ) || (length <= 0) || (buffer == NULL)) + { + DEVICELOGE("Error: Invalid parameters: listener = %p, buffer = %p, length = %d", listener, buffer, length); + return Status::ERROR_PARAMETER_INVALID; + } + + MessageON_ASYNC_START setMsg(listener, SLAM_SET_LOCALIZATION_DATA_STREAM, length, buffer); + mDispatcher->sendMessage(&mFsm, setMsg); + + return setMsg.Result == 0 ? Status::SUCCESS : Status::COMMON_ERROR; + } + + Status Device::ResetLocalizationData(uint8_t flag) + { + bulk_message_request_reset_localization_data request = {0}; + bulk_message_response_reset_localization_data response = {0}; + + if (flag > 1) + { + DEVICELOGE("Error: Flag (%d) is unknown", flag); + return Status::ERROR_PARAMETER_INVALID; + } + + request.header.wMessageID = SLAM_RESET_LOCALIZATION_DATA; + request.header.dwLength = sizeof(request); + request.bFlag = flag; + + DEVICELOGD("Set Reset Localization Data - Flag 0x%X", flag); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::SetStaticNode(const char* guid, const TrackingData::RelativePose& relativePose) + { + bulk_message_request_set_static_node request = { 0 }; + bulk_message_response_set_static_node response = { 0 }; + + auto length = perc::stringLength(guid, MAX_GUID_LENGTH); + if (length > (MAX_GUID_LENGTH-1)) + { + DEVICELOGE("Error: guid length is too big, max length = %d", (MAX_GUID_LENGTH - 1)); + return Status::ERROR_PARAMETER_INVALID; + } + + request.header.wMessageID = SLAM_SET_STATIC_NODE; + request.header.dwLength = sizeof(request); + request.data.flX = relativePose.translation.x; + request.data.flY = relativePose.translation.y; + request.data.flZ = relativePose.translation.z; + request.data.flQi = relativePose.rotation.i; + request.data.flQj = relativePose.rotation.j; + request.data.flQk = relativePose.rotation.k; + request.data.flQr = relativePose.rotation.r; + snprintf((char*)request.bGuid, length+1, "%s", guid); + + DEVICELOGD("Set Static Node: guid [%s], translation [%f,%f,%f], rotation [%f,%f,%f,%f]", guid, request.data.flX, request.data.flY, request.data.flZ, request.data.flQi, request.data.flQj, request.data.flQk, request.data.flQr); + + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::GetStaticNode(const char* guid, TrackingData::RelativePose& relativePose) + { + bulk_message_request_get_static_node request = { 0 }; + bulk_message_response_get_static_node response = { 0 }; + + auto length = perc::stringLength(guid, MAX_GUID_LENGTH); + if (length > (MAX_GUID_LENGTH - 1)) + { + DEVICELOGE("Error: guid length is too big, max length = %d", (MAX_GUID_LENGTH - 1)); + return Status::ERROR_PARAMETER_INVALID; + } + + request.header.wMessageID = SLAM_GET_STATIC_NODE; + request.header.dwLength = sizeof(request); + snprintf((char*)request.bGuid, length + 1, "%s", guid); + + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + DEVICELOGD("Get Static Node: guid [%s], translation [%f,%f,%f], rotation [%f,%f,%f,%f]", guid, response.data.flX, response.data.flY, response.data.flZ, response.data.flQi, response.data.flQj, response.data.flQk, response.data.flQr); + + relativePose.translation.x = response.data.flX; + relativePose.translation.y = response.data.flY; + relativePose.translation.z = response.data.flZ; + relativePose.rotation.i = response.data.flQi; + relativePose.rotation.j = response.data.flQj; + relativePose.rotation.k = response.data.flQk; + relativePose.rotation.r = response.data.flQr; + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + + Status Device::SetGeoLocation(const TrackingData::GeoLocalization& geoLocation) + { + bulk_message_request_set_geo_location request = {0}; + bulk_message_response_set_geo_location response = {0}; + + request.header.wMessageID = DEV_SET_GEO_LOCATION; + request.header.dwLength = sizeof(request); + request.dfLatitude = geoLocation.latitude; + request.dfLongitude = geoLocation.longitude; + request.dfAltitude = geoLocation.altitude; + + DEVICELOGD("Set Geo Localization - Latitude = %lf, Longitude = %lf, Altitude = %lf", geoLocation.latitude, geoLocation.longitude, geoLocation.altitude); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::Set6DofInterruptRate(sixdof_interrupt_rate_libtm_message message) + { + bulk_message_request_set_6dof_interrupt_rate request = {0}; + bulk_message_response_set_6dof_interrupt_rate response = {0}; + + if (message.bInterruptRate >= SIXDOF_INTERRUPT_RATE_MAX) + { + DEVICELOGE("Got an invalid 6dof interrupt rate (%d)", message.bInterruptRate); + return Status::ERROR_PARAMETER_INVALID; + } + + request.header.wMessageID = SLAM_SET_6DOF_INTERRUPT_RATE; + request.header.dwLength = sizeof(request); + request.message.bInterruptRate = message.bInterruptRate; + + DEVICELOGD("Set 6Dof Interrupt rate to %d", message.bInterruptRate); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + void Device::putBufferBack(SensorId id, std::shared_ptr& frame) + { + std::lock_guard lg(mFramesBuffersMutex); + mFramesBuffersLists.push_back(frame); + DEVICELOGV("frame buffers increased (%d) - %p", mFramesBuffersLists.size(), mFramesBuffersLists.back().get()); + + } + + int Device::FindInterruptEndpoint() + { + int result = -1; + + struct libusb_config_descriptor *config; + int rc = libusb_get_active_config_descriptor(mLibusbDevice, &config); + if (rc < 0) + { + DEVICELOGE("Error while getting active config descriptor. LIBUSB_ERROR_CODE: %d (%s)", rc, libusb_error_name(rc)); + return rc; + } + + /* Looping on all interfaces supported by this USB configuration descriptor */ + for (int iface_idx = 0; iface_idx < config->bNumInterfaces; iface_idx++) + { + const struct libusb_interface *iface = &config->interface[iface_idx]; + + /* Looping on all interface descriptors (alternate settings) for a particular USB interface */ + for (int altsetting_idx = 0; altsetting_idx < iface->num_altsetting; altsetting_idx++) + { + const struct libusb_interface_descriptor *altsetting = &iface->altsetting[altsetting_idx]; + + /* Looping on all endpoint descriptors for a particular USB interface descriptor */ + for (int ep_idx = 0; ep_idx < altsetting->bNumEndpoints; ep_idx++) + { + const struct libusb_endpoint_descriptor *ep = &altsetting->endpoint[ep_idx]; + + /* Searching for interrupt endpoint directed to host */ + if ((TO_HOST == (ep->bEndpointAddress & LIBUSB_ENDPOINT_DIR_MASK)) && (LIBUSB_TRANSFER_TYPE_INTERRUPT == (ep->bmAttributes & LIBUSB_TRANSFER_TYPE_MASK))) + { + result = ep->bEndpointAddress; + goto found; + } + } + } + + } + + found: + DEVICELOGV("Found interrupt endpoint address: %d", result); + libusb_free_config_descriptor(config); + return result; + } + + Status Device::EepromRead(uint16_t offset, uint16_t size, uint8_t * buffer, uint16_t& actual) + { + uint16_t transferred = 0; + Status st = Status::SUCCESS; + uint16_t actualChunk = 0; + actual = 0; + + DEVICELOGV("Reading EEPROM: offset 0x%X, size %d (bytes)", offset, size); + while (transferred < size) + { + unsigned int currentChunkSize; + currentChunkSize = ((size - transferred) > mEepromChunkSize) ? mEepromChunkSize : (size - transferred); + st = ReadEepromChunk(offset + transferred, currentChunkSize, buffer + transferred, actualChunk); + actual += actualChunk; + transferred += currentChunkSize; + if (st != Status::SUCCESS) + { + DEVICELOGE("Error Reading EEPROM chunk: offset 0x%X, size %d (bytes), status 0x%X", offset, size, st); + return st; + } + } + + return st; + } + + Status Device::ReadEepromChunk(uint16_t offset, uint16_t size, uint8_t * buffer, uint16_t& actual) + { + bulk_message_request_read_eeprom request = {0}; + bulk_message_response_read_eeprom response = {0}; + + if (size > MAX_EEPROM_BUFFER_SIZE || !buffer) + { + DEVICELOGE("Parameter error: size %d (bytes), buffer %p", size, buffer); + return Status::ERROR_PARAMETER_INVALID; + } + + request.header.dwLength = sizeof(bulk_message_request_read_eeprom); + request.header.wMessageID = DEV_READ_EEPROM; + request.wSize = size; + request.wOffset = offset; + + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + mDispatcher->sendMessage(&mFsm, msg); + + if (msg.Result == toUnderlying(Status::SUCCESS)) + { + perc::copy(buffer, &response.bData, size); + } + else + { + return Status::ERROR_USB_TRANSFER; + } + + DEVICELOGV("Reading EEPROM chunk: offset 0x%X, size %d (bytes), actual %d, status 0x%X", offset, size, response.wSize, response.header.wStatus); + if (response.header.wStatus == 0) + { + actual = response.wSize; + return Status::SUCCESS; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::EepromWrite(uint16_t offset, uint16_t size, uint8_t * buffer, uint16_t& actual, bool verify) + { + uint16_t transferred = 0; + Status st = Status::SUCCESS; + uint16_t actualChunk = 0; + actual = 0; + DEVICELOGD("Writing EEPROM: offset 0x%X, size %d (bytes)", offset, size); + while (transferred < size) + { + unsigned int currentChunkSize; + currentChunkSize = ((size - transferred) > mEepromChunkSize) ? mEepromChunkSize : (size - transferred); + st = WriteEepromChunk(offset + transferred, currentChunkSize, buffer + transferred, actualChunk, verify); + actual += actualChunk; + transferred += currentChunkSize; + if (st != Status::SUCCESS) + { + DEVICELOGE("Error Writing EEPROM chunk: offset 0x%X, size %d (bytes), status 0x%X", offset, size, st); + return st; + } + } + return st; + } + + Status Device::WriteEepromChunk(uint16_t offset, uint16_t size, uint8_t * buffer, uint16_t& actual, bool verify) + { + bulk_message_request_write_eeprom request = {0}; + bulk_message_response_write_eeprom response = {0}; + + if (size > mEepromChunkSize) + { + DEVICELOGE("Parameter error: size %d > maxChunkSize %d", size, mEepromChunkSize); + return Status::ERROR_PARAMETER_INVALID; + } + + request.header.dwLength = sizeof(bulk_message_request_write_eeprom); + request.header.wMessageID = DEV_WRITE_EEPROM; + request.wSize = size; + request.wOffset = offset; + perc::copy(&request.bData[0], buffer, size); + + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + mDispatcher->sendMessage(&mFsm, msg); + + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error Writing EEPROM chunk: offset 0x%X, size %d (bytes), result 0x%X", offset, size, msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + DEVICELOGV("Writing EEPROM chunk: offset 0x%X, size %d (bytes), actual %d, status 0x%X", offset, size, response.wSize, response.header.wStatus); + if (response.header.wStatus == 0) + { + actual = response.wSize; + + if (verify) + { + /* This EEPROM has a write time of 5 msec per PAGE, waiting for write to be completed before verifying */ + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + + uint16_t tmp; + std::vector tmpBuf; + tmpBuf.resize(size); + DEVICELOGV("Verifing EEPROM chunk: offset 0x%X, size %d (bytes)", offset, size); + EepromRead(offset, size, (uint8_t*)tmpBuf.data(), tmp); + int rc = memcmp((uint8_t*)tmpBuf.data(), buffer, size); + if (rc) + { + DEVICELOGE("Verifing EEPROM chunk failed: offset 0x%X, size %d (bytes)", offset, size); + return Status::ERROR_EEPROM_VERIFY_FAIL; + } + } + return Status::SUCCESS; + } + + return Status::ERROR_PARAMETER_INVALID; + } + + Status Device::SendFrame(const TrackingData::VelocimeterFrame& frame) + { + std::vector buf; + buf.resize(sizeof(bulk_message_velocimeter_stream)); + + bulk_message_velocimeter_stream* req = (bulk_message_velocimeter_stream*)buf.data(); + + req->rawStreamHeader.header.dwLength = sizeof(bulk_message_velocimeter_stream); + req->rawStreamHeader.header.wMessageID = DEV_SAMPLE; + req->rawStreamHeader.bSensorID = SET_SENSOR_ID(SensorType::Velocimeter, frame.sensorIndex); + req->rawStreamHeader.llNanoseconds = frame.timestamp; + req->rawStreamHeader.bReserved = 0; + req->rawStreamHeader.llArrivalNanoseconds = frame.arrivalTimeStamp; + req->rawStreamHeader.dwFrameId = frame.frameId; + + req->metadata.dwMetadataLength = offsetof(bulk_message_velocimeter_stream_metadata, dwFrameLength) - sizeof(req->metadata.dwMetadataLength); + req->metadata.flTemperature = frame.temperature; + req->metadata.dwFrameLength = sizeof(bulk_message_velocimeter_stream_metadata) - offsetof(bulk_message_velocimeter_stream_metadata, dwFrameLength) - sizeof(req->metadata.dwFrameLength); + req->metadata.flVx = frame.angularVelocity.x; + req->metadata.flVy = frame.angularVelocity.y; + req->metadata.flVz = frame.angularVelocity.z; + + int actual; + auto rc = libusb_bulk_transfer(mDevice, mStreamEndpoint | TO_DEVICE, (unsigned char*)req, (int)buf.size(), &actual, 100); + if (rc != 0 || actual == 0) + { + DEVICELOGE("Error while sending velocimeter frame to device: %d", rc); + return Status::ERROR_USB_TRANSFER; + } + + return Status::SUCCESS; + } + + Status Device::SendFrame(const TrackingData::GyroFrame& frame) + { + std::vector buf; + buf.resize(sizeof(bulk_message_gyro_stream)); + + bulk_message_gyro_stream* req = (bulk_message_gyro_stream*)buf.data(); + + req->rawStreamHeader.header.dwLength = sizeof(bulk_message_gyro_stream); + req->rawStreamHeader.header.wMessageID = DEV_SAMPLE; + req->rawStreamHeader.bSensorID = SET_SENSOR_ID(SensorType::Gyro, frame.sensorIndex); + req->rawStreamHeader.llNanoseconds = frame.timestamp; + req->rawStreamHeader.bReserved = 0; + req->rawStreamHeader.llArrivalNanoseconds = frame.arrivalTimeStamp; + req->rawStreamHeader.dwFrameId = frame.frameId; + + req->metadata.dwMetadataLength = offsetof(bulk_message_gyro_stream_metadata, dwFrameLength) - sizeof(req->metadata.dwMetadataLength); + req->metadata.flTemperature = frame.temperature; + req->metadata.dwFrameLength = sizeof(bulk_message_gyro_stream_metadata) - offsetof(bulk_message_gyro_stream_metadata, dwFrameLength) - sizeof(req->metadata.dwFrameLength); + req->metadata.flGx = frame.angularVelocity.x; + req->metadata.flGy = frame.angularVelocity.y; + req->metadata.flGz = frame.angularVelocity.z; + + int actual; + auto rc = libusb_bulk_transfer(mDevice, mStreamEndpoint | TO_DEVICE, (unsigned char*)req, (int)buf.size(), &actual, 100); + if (rc != 0 || actual == 0) + { + DEVICELOGE("Error while sending gyro frame to device: %d", rc); + return Status::ERROR_USB_TRANSFER; + } + + return Status::SUCCESS; + } + + Status Device::SendFrame(const TrackingData::AccelerometerFrame& frame) + { + std::vector buf; + buf.resize(sizeof(bulk_message_accelerometer_stream)); + + bulk_message_accelerometer_stream* req = (bulk_message_accelerometer_stream*)buf.data(); + req->rawStreamHeader.header.dwLength = sizeof(bulk_message_accelerometer_stream); + req->rawStreamHeader.header.wMessageID = DEV_SAMPLE; + req->rawStreamHeader.bSensorID = SET_SENSOR_ID(SensorType::Accelerometer, frame.sensorIndex); + req->rawStreamHeader.llNanoseconds = frame.timestamp; + req->rawStreamHeader.bReserved = 0; + req->rawStreamHeader.llArrivalNanoseconds = frame.arrivalTimeStamp; + req->rawStreamHeader.dwFrameId = frame.frameId; + + req->metadata.dwMetadataLength = offsetof(bulk_message_accelerometer_stream_metadata, dwFrameLength) - sizeof(req->metadata.dwMetadataLength); + req->metadata.flTemperature = frame.temperature; + req->metadata.dwFrameLength = sizeof(bulk_message_accelerometer_stream_metadata) - offsetof(bulk_message_accelerometer_stream_metadata, dwFrameLength) - sizeof(req->metadata.dwFrameLength); + req->metadata.flAx = frame.acceleration.x; + req->metadata.flAy = frame.acceleration.y; + req->metadata.flAz = frame.acceleration.z; + + int actual; + auto rc = libusb_bulk_transfer(mDevice, mStreamEndpoint | TO_DEVICE, (unsigned char*)req, (int)buf.size(), &actual, 100); + if (rc != 0 || actual == 0) + { + DEVICELOGE("Error while sending accelerometer frame to device: %d", rc); + return Status::ERROR_USB_TRANSFER; + } + + return Status::SUCCESS; + } + + Status Device::SendFrame(const TrackingData::VideoFrame& frame) + { + std::vector buf; + buf.resize(frame.profile.stride * frame.profile.height + sizeof(bulk_message_video_stream)); + + bulk_message_video_stream* msg = (bulk_message_video_stream*)buf.data(); + + msg->rawStreamHeader.header.dwLength = (uint32_t)buf.size(); + msg->rawStreamHeader.header.wMessageID = DEV_SAMPLE; + msg->rawStreamHeader.bSensorID = SET_SENSOR_ID(SensorType::Fisheye, frame.sensorIndex); + msg->rawStreamHeader.bReserved = 0; + msg->rawStreamHeader.llNanoseconds = frame.timestamp; + msg->rawStreamHeader.llArrivalNanoseconds = frame.arrivalTimeStamp; + msg->rawStreamHeader.dwFrameId = frame.frameId; + + msg->metadata.dwMetadataLength = frame.profile.stride*frame.profile.height + sizeof(bulk_message_video_stream_metadata); + msg->metadata.dwExposuretime = frame.exposuretime; + msg->metadata.fGain = frame.gain; + msg->metadata.dwFrameLength = frame.profile.stride*frame.profile.height; + + perc::copy(buf.data() + sizeof(bulk_message_video_stream), frame.data, frame.profile.stride*frame.profile.height); + + int actual; + auto rc = libusb_bulk_transfer(mDevice, mStreamEndpoint | TO_DEVICE, buf.data(), (int)buf.size(), &actual, 100); + if (rc != 0 || actual == 0) + { + DEVICELOGE("Error while sending frame to device: %d", rc); + return Status::ERROR_USB_TRANSFER; + } + + return Status::SUCCESS; + } + + Status Device::ControllerConnect(const TrackingData::ControllerDeviceConnect& device, uint8_t& controllerId) + { + bulk_message_request_controller_device_connect request = {0}; + bulk_message_response_controller_device_connect response = {0}; + + if (device.addressType >= AddressTypeMax) + { + DEVICELOGE("Error: Unsupported addressType (0x%X)", device.addressType); + return Status::ERROR_PARAMETER_INVALID; + } + + request.header.wMessageID = CONTROLLER_DEVICE_CONNECT; + request.header.dwLength = sizeof(request); + request.wTimeout = device.timeout; + request.bAddressType = uint8_t(device.addressType); + perc::copy(request.bMacAddress, device.macAddress, MAC_ADDRESS_SIZE); + + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST, USB_TRANSFER_SLOW_TIMEOUT_MS); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + if ((MESSAGE_STATUS)response.header.wStatus == MESSAGE_STATUS::SUCCESS) + { + DEVICELOGD("Sent Controller Device Connect request for Controller MacAddress = [%02X:%02X:%02X:%02X:%02X:%02X], [AddressType 0x%X], timeout %d (msec), Received ControllerId %d", + request.bMacAddress[0], request.bMacAddress[1], request.bMacAddress[2], request.bMacAddress[3], request.bMacAddress[4], request.bMacAddress[5], request.bAddressType, request.wTimeout, response.bControllerID); + } + else + { + DEVICELOGE("Error: Failed to send Controller Device Connect request for Controller MacAddress = [%02X:%02X:%02X:%02X:%02X:%02X], [AddressType 0x%X], timeout %d (msec)", + request.bMacAddress[0], request.bMacAddress[1], request.bMacAddress[2], request.bMacAddress[3], request.bMacAddress[4], request.bMacAddress[5], request.bAddressType, request.wTimeout); + } + + controllerId = response.bControllerID; + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::ControllerDisconnect(uint8_t controllerId) + { + bulk_message_request_controller_device_disconnect request = {0}; + bulk_message_response_controller_device_disconnect response = {0}; + + request.header.wMessageID = CONTROLLER_DEVICE_DISCONNECT; + request.header.dwLength = sizeof(request); + request.bControllerID = controllerId; + + DEVICELOGD("Set Controller Device Disconnect request on controllerId %d", controllerId); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST, USB_TRANSFER_SLOW_TIMEOUT_MS); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::ControllerStartCalibration(uint8_t controllerId) + { + bulk_message_request_controller_start_calibration request = { 0 }; + bulk_message_response_controller_start_calibration response = { 0 }; + + request.header.wMessageID = CONTROLLER_START_CALIBRATION; + request.header.dwLength = sizeof(request); + request.bControllerID = controllerId; + + DEVICELOGD("Set Controller calibration request on controllerId %d", controllerId); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST, USB_TRANSFER_SLOW_TIMEOUT_MS); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::GetAssociatedDevices(TrackingData::ControllerAssociatedDevices& devices) + { + bulk_message_request_controller_read_associated_devices request = {0}; + bulk_message_response_controller_read_associated_devices response = {0}; + + request.header.wMessageID = CONTROLLER_READ_ASSOCIATED_DEVICES; + request.header.dwLength = sizeof(request); + + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + devices.set(response.bMacAddress1, response.bMacAddress2, (AddressType)response.bAddressType1, (AddressType)response.bAddressType2); + + DEVICELOGD("Got Associated Devices from the EEPROM: Device1 %02X:%02X:%02X:%02X:%02X:%02X, Address type 0x%X, Device2 %02X:%02X:%02X:%02X:%02X:%02X, Address type 0x%X (Status 0x%X)", + devices.macAddress1[0], devices.macAddress1[1], devices.macAddress1[2], devices.macAddress1[3], devices.macAddress1[4], devices.macAddress1[5], devices.addressType1, + devices.macAddress2[0], devices.macAddress2[1], devices.macAddress2[2], devices.macAddress2[3], devices.macAddress2[4], devices.macAddress2[5], devices.addressType2, response.header.wStatus); + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::SetAssociatedDevices(const TrackingData::ControllerAssociatedDevices& devices) + { + bulk_message_request_controller_write_associated_devices request = {0}; + bulk_message_response_controller_write_associated_devices response = {0}; + + request.header.wMessageID = CONTROLLER_WRITE_ASSOCIATED_DEVICES; + request.header.dwLength = sizeof(request); + + if ((devices.macAddress1[0] == 0) && (devices.macAddress1[1] == 0) && (devices.macAddress1[2] == 0) && (devices.macAddress1[3] == 0) && (devices.macAddress1[4] == 0) && (devices.macAddress1[5] == 0)) + { + DEVICELOGE("Error: MacAddress1 can't be zero"); + return Status::ERROR_PARAMETER_INVALID; + } + + if (devices.addressType1 >= AddressTypeMax) + { + DEVICELOGE("Error: Unsupported addressType1 (0x%X)", devices.addressType1); + return Status::ERROR_PARAMETER_INVALID; + } + + if (devices.addressType2 >= AddressTypeMax) + { + DEVICELOGE("Error: Unsupported addressType2 (0x%X)", devices.addressType2); + return Status::ERROR_PARAMETER_INVALID; + } + + perc::copy(&request.bMacAddress1, devices.macAddress1, MAC_ADDRESS_SIZE); + perc::copy(&request.bMacAddress2, devices.macAddress2, MAC_ADDRESS_SIZE); + request.bAddressType1 = devices.addressType1; + request.bAddressType2 = devices.addressType2; + + DEVICELOGD("Set Associated Devices to the EEPROM: Device1 %02X:%02X:%02X:%02X:%02X:%02X, Address type 0x%X, Device2 %02X:%02X:%02X:%02X:%02X:%02X, Address type 0x%X", + request.bMacAddress1[0], request.bMacAddress1[1], request.bMacAddress1[2], request.bMacAddress1[3], request.bMacAddress1[4], request.bMacAddress1[5], request.bAddressType1, + request.bMacAddress2[0], request.bMacAddress2[1], request.bMacAddress2[2], request.bMacAddress2[3], request.bMacAddress2[4], request.bMacAddress2[5], request.bAddressType2); + + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::ControllerSendData(const TrackingData::ControllerData& controllerData) + { + if (controllerData.bufferSize + offsetof(bulk_message_request_controller_send_data, bControllerData) > BUFFER_SIZE) + { + DEVICELOGE("BufferSize (%d bytes) must not exceed %d bytes", controllerData.bufferSize, BUFFER_SIZE - offsetof(bulk_message_request_controller_send_data, bControllerData)); + return Status::COMMON_ERROR; + } + + if (controllerData.bufferSize == 0) + { + DEVICELOGE("BufferSize (%d bytes) too small", controllerData.bufferSize); + return Status::BUFFER_TOO_SMALL; + } + + uint8_t requestBuffer[BUFFER_SIZE]; + bulk_message_request_controller_send_data* request = (bulk_message_request_controller_send_data*)requestBuffer; + bulk_message_response_controller_send_data response = {0}; + + request->header.wMessageID = CONTROLLER_SEND_DATA; + request->header.dwLength = offsetof(bulk_message_request_controller_send_data, bControllerData) + controllerData.bufferSize; + request->bControllerID = controllerData.controllerId; + request->bCommandID = controllerData.commandId; + perc::copy(request->bControllerData, controllerData.buffer, controllerData.bufferSize); + + DEVICELOGD("Set Controller Send Data: controllerId = 0x%02X, commandId = 0x%02X, bufferSize = %02d", controllerData.controllerId, controllerData.commandId, controllerData.bufferSize); + Bulk_Message msg((uint8_t*)request, request->header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::ControllerRssiTestControl(uint8_t controllerId, bool testControl) + { + bulk_message_request_controller_rssi_test_control request = {0}; + bulk_message_response_controller_rssi_test_control response = {0}; + + request.header.wMessageID = CONTROLLER_RSSI_TEST_CONTROL; + request.header.dwLength = sizeof(request); + request.bControllerID = controllerId; + request.bTestControl = testControl; + + DEVICELOGD("Set Controller RSSI test Control: controllerId = 0x%X, testControl = %s", controllerId, (testControl == 0)?"False":"True"); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::SetGpioControl(uint8_t gpioControl) + { + bulk_message_request_gpio_control request = {0}; + bulk_message_response_gpio_control response = {0}; + + request.header.wMessageID = DEV_GPIO_CONTROL; + request.header.dwLength = sizeof(request); + request.bGpioControl = gpioControl; + + DEVICELOGD("Set GPIO Control = 0x%X", gpioControl); + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + + mDispatcher->sendMessage(&mFsm, msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::CentralLoadFW(uint8_t* buffer) + { + if (!mHasBluetooth) + { + DEVICELOGE("cannot CentralLoadFW, there is no bluetooth in the device"); + return Status::NO_BLUETOOTH; + } + uint32_t addressSize = offsetof(message_fw_update_request, bNumFiles); + std::vector msgArr(addressSize + CENTRAL_APP_SIZE, 0); + perc::copy(msgArr.data() + addressSize, buffer, CENTRAL_APP_SIZE); + message_fw_update_request* msg = (message_fw_update_request*)(msgArr.data()); + MessageON_ASYNC_START setMsg(&mCentralListener, DEV_FIRMWARE_UPDATE, (uint32_t)msgArr.size(), (uint8_t*)msg); + mFsm.fireEvent(setMsg); + if (setMsg.Result != 0) + { + DEVICELOGE("Failed to start ON_ASYNC state when updating the central fw update : %d", setMsg.Result); + return Status::COMMON_ERROR; + } + + std::mutex asyncMutex; + std::unique_lock lk(asyncMutex); + mAsyncCV.wait(lk); + + MessageON_ASYNC_STOP stopMsg; + mFsm.fireEvent(stopMsg); + if (stopMsg.Result != 0) + { + DEVICELOGE("Failed to stop ON_ASYNC state when updating the central fw update : %d", stopMsg.Result); + return Status::COMMON_ERROR; + } + + return Status::SUCCESS; + } + + Status Device::CentralFWUpdate() + { + if(!mHasBluetooth) + { + DEVICELOGE("cannot CentralFWUpdate, there is no bluetooth in the device"); + return Status::NO_BLUETOOTH; + } + + bool updateApp = false; + + if (CentralBlFw::Version[0] != mDeviceInfo.bCentralBootloaderVersionMajor || + CentralBlFw::Version[1] != mDeviceInfo.bCentralBootloaderVersionMinor || + CentralBlFw::Version[2] != mDeviceInfo.bCentralBootloaderVersionPatch) + { + DEVICELOGD("Updating Central Boot Loader FW [%u.%u.%u] --> [%u.%u.%u]", + mDeviceInfo.bCentralBootloaderVersionMajor, mDeviceInfo.bCentralBootloaderVersionMinor, mDeviceInfo.bCentralBootloaderVersionPatch, + CentralBlFw::Version[0], CentralBlFw::Version[1], CentralBlFw::Version[2]); + auto status = CentralLoadFW((uint8_t*)CentralBlFw::Buffer); + if (status != Status::SUCCESS) + { + return status; + } + updateApp = true; + } + + if (updateApp == true || + CentralAppFw::Version[0] != mDeviceInfo.bCentralAppVersionMajor || + CentralAppFw::Version[1] != mDeviceInfo.bCentralAppVersionMinor || + CentralAppFw::Version[2] != mDeviceInfo.bCentralAppVersionPatch || + CentralAppFw::Version[3] != mDeviceInfo.dwCentralAppVersionBuild) + { + DEVICELOGD("Updating Central Application FW [%u.%u.%u.%u] --> [%u.%u.%u.%u]", + mDeviceInfo.bCentralAppVersionMajor, mDeviceInfo.bCentralAppVersionMinor, mDeviceInfo.bCentralAppVersionPatch, mDeviceInfo.dwCentralAppVersionBuild, + CentralAppFw::Version[0], CentralAppFw::Version[1], CentralAppFw::Version[2], CentralAppFw::Version[3]); + auto status = CentralLoadFW((uint8_t*)CentralAppFw::Buffer); + if (status != Status::SUCCESS) + { + return status; + } + } + return Status::SUCCESS; + } + + Status Device::ControllerFWUpdate(const TrackingData::ControllerFW& fw) + { + if (!mHasBluetooth) + { + DEVICELOGE("cannot ControllerFWUpdate, there is no bluetooth in the device"); + return Status::NO_BLUETOOTH; + } + if (fw.imageSize == 0) + { + DEVICELOGE("FW image size (%d bytes) too small", fw.imageSize); + return Status::BUFFER_TOO_SMALL; + } + + uint32_t addressSize = offsetof(message_fw_update_request, bNumFiles); + std::vector msgArr(addressSize + fw.imageSize, 0); + message_fw_update_request* msg = (message_fw_update_request*)(msgArr.data()); + msg->bAddressType = (uint8_t)fw.addressType; + + if (msg->bAddressType >= AddressTypeMax) + { + DEVICELOGE("Error: Unsupported addressType (0x%X) in FW image", msg->bAddressType); + return Status::ERROR_PARAMETER_INVALID; + } + + perc::copy(msgArr.data(), fw.macAddress, MAC_ADDRESS_SIZE); + perc::copy(msgArr.data() + addressSize, fw.image, fw.imageSize); + + Large_Message setMsg(mListener, DEV_FIRMWARE_UPDATE, (uint32_t)msgArr.size(), (uint8_t*)msg); + mDispatcher->sendMessage(&mFsm, setMsg); + + if (setMsg.Result != 0) + { + DEVICELOGE("Received error when loading controller image : %d", setMsg.Result); + return Status::COMMON_ERROR; + } + return Status::SUCCESS; + } + + /* Stream/Frame Endpoint Thread */ + void Device::streamEndpointThread() + { + /* Making sure this thread gets high CPU priority */ + std::lock_guard lk(streamThreadMutex); + + DEVICELOGD("Thread Start - Stream thread (Video/Controllers/Rssi/Localization frames)"); + + mStreamEndpointThreadActive = true; + + nsecs_t timeOfFirstByte = 0; + uint64_t totalBytesReceived = 0; + + while (mStreamEndpointThreadStop == false) + { + int actual = 0; + auto rc = libusb_bulk_transfer(mDevice, mStreamEndpoint | TO_HOST, mFramesBuffersLists.front().get(), mFrameTempBufferSize, &actual, 100); + if (rc == LIBUSB_ERROR_TIMEOUT) + continue; + + if (rc != 0 || actual == 0) + { + DEVICELOGE("FW crashed - got error in stream endpoint thread function: status = %d (%s), actual = %d", rc, libusb_error_name(rc), actual); + mDispatcher->postMessage(&mFsm, Message(ON_ERROR)); + break; + } + + // Add some statistics calculations + totalBytesReceived += actual; + if (timeOfFirstByte == 0) + timeOfFirstByte = systemTime(); + else + { + auto currentTime = systemTime(); + double diff = (double)currentTime - timeOfFirstByte; + diff /= 1000000000; + if (diff != 0) + { + double speed = ((double)totalBytesReceived) / diff; + DEVICELOGV("Current transfer speed on Frame Endpoint is: %.2f bytes per second, Total bytes received %lld, time passed %.2f", speed, totalBytesReceived, diff); + } + } + + bulk_message_raw_stream_header* header = (bulk_message_raw_stream_header*)mFramesBuffersLists.front().get(); + switch (header->header.wMessageID) + { + case DEV_SAMPLE: + { + switch (GET_SENSOR_TYPE(header->bSensorID)) + { + case SensorType::Fisheye: + { + /* After calling addTask, VideoFrameCompleteTask ptr can be destructed from whoever come last: */ + /* 1. streamEndpointThread (This function) */ + /* 2. Manager::handleEvents */ + /* in case of 1 - The VideoFrameCompleteTask destructor is called inside the brackets (before releasing mFramesBuffersMutex) */ + /* To prevent deadlock we are using recursive_mutex which allows calling lock inside lock in the same thread */ + std::lock_guard lg(mFramesBuffersMutex); + if (mFramesBuffersLists.size() > 1) + { + std::shared_ptr videoFrameCompleteTask = std::make_shared(mListener, + mFramesBuffersLists.front(), + this, + this, + header->llNanoseconds + mTM2CorrelatedTimeStampShift, + mWidthsMap[header->bSensorID], + mHeightsMap[header->bSensorID]); + std::shared_ptr completeTask = videoFrameCompleteTask; + + TrackingData::VideoFrame* frame = &videoFrameCompleteTask->mVideoFrame; + int64_t offset = (int64_t)(frame->frameId) - (int64_t)videoPrevFrame[frame->sensorIndex].prevFrameId; + if (videoPrevFrame[frame->sensorIndex].prevFrameId != 0) + { + if (offset > 1) + { + DEVICELOGW("Video[%d] frame drops occurred - %lld missing frames [FrameId %d-%d], time diff = %d (msec)", + frame->sensorIndex, offset - 1, videoPrevFrame[frame->sensorIndex].prevFrameId + 1, frame->frameId - 1, ns2ms(frame->timestamp - videoPrevFrame[frame->sensorIndex].prevFrameTimeStamp)); + } + else if (offset < 1) + { + DEVICELOGW("Video[%d] frame reorder occurred - prev frameId = %d, new frameId = %d, time diff = %d (msec)", + frame->sensorIndex, videoPrevFrame[frame->sensorIndex].prevFrameId, frame->frameId, ns2ms(frame->timestamp - videoPrevFrame[frame->sensorIndex].prevFrameTimeStamp)); + } + } + + videoPrevFrame[frame->sensorIndex].prevFrameTimeStamp = frame->timestamp; + videoPrevFrame[frame->sensorIndex].prevFrameId = frame->frameId; + + DEVICELOGV("frame buffers decreased (%d) - %p", mFramesBuffersLists.size()-1, mFramesBuffersLists.front().get()); + + mFramesBuffersLists.pop_front(); + mTaskHandler->addTask(completeTask); + } + else + { + /* Todo: Consider allocating more buffers */ + DEVICELOGE("No more frame buffers (%d), dropping frame", mFramesBuffersLists.size()); + } + break; + } // end of switch + + case SensorType::Controller: + { + std::shared_ptr ptr = std::make_shared(mListener, header, mTM2CorrelatedTimeStampShift, this); + mTaskHandler->addTask(ptr); + break; + } + + case SensorType::Rssi: + { + std::shared_ptr ptr = std::make_shared(mListener, header, mTM2CorrelatedTimeStampShift, this); + mTaskHandler->addTask(ptr); + break; + } + + default: + DEVICELOGE("Unsupported sensor (0x%X) on stream endpoint thread function", header->bSensorID); + break; + } + break; + } + + case SLAM_GET_LOCALIZATION_DATA_STREAM: + { + std::lock_guard lg(mFramesBuffersMutex); + interrupt_message_get_localization_data_stream* getLocalizationDataStream = (interrupt_message_get_localization_data_stream*)header; + uint16_t status = getLocalizationDataStream->wStatus; + + DEVICELOGV("Got Localization Data frame: status = 0x%X, moredata = %s, chunkIndex = %d, length = %d", + status, + (((MESSAGE_STATUS)getLocalizationDataStream->wStatus == MESSAGE_STATUS::MORE_DATA_AVAILABLE)) ? "True" : "False", + getLocalizationDataStream->wIndex, + getLocalizationDataStream->header.dwLength - offsetof(interrupt_message_get_localization_data_stream, bLocalizationData)); + + if (mFramesBuffersLists.size() > 1) + { + std::shared_ptr ptr = std::make_shared(mListener, mFramesBuffersLists.front(), this, this); + mFramesBuffersLists.pop_front(); + mTaskHandler->addTask(ptr); + } + else + { + /* Todo: Consider allocating more buffers */ + DEVICELOGE("No more get localization buffers (%d), dropping get localization frame", mFramesBuffersLists.size()); + } + + /* Check if this is the last Get localization frame, and exit ASYNC state */ + if (status == (uint16_t)MESSAGE_STATUS::SUCCESS) + { + MessageON_ASYNC_STOP setMsg; + mDispatcher->postMessage(&mFsm, setMsg); + } + break; + } + + case DEV_STATUS: + { + interrupt_message_status* status = (interrupt_message_status*)header; + DEVICELOGD("Got DEV status %s (0x%X) on stream endpoint", statusCodeToString((MESSAGE_STATUS)status->wStatus).c_str(), status->wStatus); + + auto hostStatus = fwToHostStatus((MESSAGE_STATUS)status->wStatus); + + /* DEVICE_STOPPED is indicated once when device exists from ACTIVE_STATE, all other statuses will be indicated here */ + if (hostStatus != Status::DEVICE_STOPPED) + { + /* Creating onStatusEvent to indicate hostStatus */ + std::shared_ptr ptr = std::make_shared(mListener, hostStatus, this); + mTaskHandler->addTask(ptr); + } + break; + } + + default: + { + DEVICELOGE("Unsupported message (0x%X) on stream endpoint thread function", header->header.wMessageID); + break; + } + } + } // end of while + + mStreamEndpointThreadActive = false; + DEVICELOGD("Thread Stop - Stream thread (Video/Controllers/Rssi/Localization frames)"); + } + + /* Event Endpoint Thread */ + void Device::interruptEndpointThread() + { + /* Making sure this thread gets high CPU priority */ + std::lock_guard lk(eventThreadMutex); + + DEVICELOGD("Thread Start - Interrupt thread (Accelerometer/Velocimeter/Gyro/6DOF/Controller/Localization events)"); + mInterruptEndpointThreadActive = true; + + unsigned char msgBuffer[BUFFER_SIZE]; + interrupt_message_header* header = (interrupt_message_header*)msgBuffer; + + while (mInterruptEndpointThreadStop == false) + { + int actual = 0; + int result; + + result = libusb_interrupt_transfer(mDevice, mEndpointInterrupt, msgBuffer, BUFFER_SIZE, &actual, 100); + if (result == LIBUSB_ERROR_TIMEOUT) + { + continue; + } + + if (result != 0 || actual < sizeof(interrupt_message_header)) + { + DEVICELOGE("FW crashed - got error in interrupt endpoint thread function: status = %d (%s), actual = %d", result, libusb_error_name(result), actual); + mDispatcher->postMessage(&mFsm, Message(ON_ERROR)); + break; + } + + switch (header->wMessageID) + { + case DEV_GET_POSE: + { + interrupt_message_get_pose poseMsg = *((interrupt_message_get_pose*)header); + auto pose = poseMessageToClass(poseMsg.pose, poseMsg.bIndex, poseMsg.pose.llNanoseconds + mTM2CorrelatedTimeStampShift); + std::shared_ptr ptr = std::make_shared(mListener, pose, this); + + /* Pose must arrive every 5 msec */ + int64_t offsetMsec = (pose.timestamp - sixdofPrevFrame[pose.sourceIndex].prevFrameTimeStamp) / ms2ns(MAX_6DOF_TIMEDIFF_MSEC); + if (sixdofPrevFrame[pose.sourceIndex].prevFrameTimeStamp != 0) + { + if (offsetMsec > MAX_6DOF_TIMEDIFF_MSEC) + { + DEVICELOGW("Pose[%d] frame drops occurred - %lld missing frames, prev pose time = %lld, new pose time = %lld, time diff = %d (msec)", + pose.sourceIndex, offsetMsec - 1, sixdofPrevFrame[pose.sourceIndex].prevFrameTimeStamp, pose.timestamp, ns2ms(pose.timestamp - sixdofPrevFrame[pose.sourceIndex].prevFrameTimeStamp)); + } + else if (offsetMsec < 0) + { + DEVICELOGW("Pose[%d] frame reorder occurred - prev pose time = %lld, new pose time = %lld, time diff = %d (msec)", + pose.sourceIndex, sixdofPrevFrame[pose.sourceIndex].prevFrameTimeStamp, pose.timestamp, ns2ms(pose.timestamp - sixdofPrevFrame[pose.sourceIndex].prevFrameTimeStamp)); + } + } + + sixdofPrevFrame[pose.sourceIndex].prevFrameTimeStamp = pose.timestamp; + + mTaskHandler->addTask(ptr); + break; + } + + case DEV_SAMPLE: + { + interrupt_message_raw_stream_header* imuHeader = (interrupt_message_raw_stream_header*)header; + switch (GET_SENSOR_TYPE(imuHeader->bSensorID)) + { + case SensorType::Gyro: + { + std::shared_ptr gyroFrameCompleteTask = std::make_shared(mListener, imuHeader, mTM2CorrelatedTimeStampShift, this); + if (gyroFrameCompleteTask != NULL) + { + TrackingData::GyroFrame* frame = &gyroFrameCompleteTask->mFrame; + int64_t offset = (int64_t)(frame->frameId) - (int64_t)gyroPrevFrame[frame->sensorIndex].prevFrameId; + if (gyroPrevFrame[frame->sensorIndex].prevFrameId != 0) + { + if (offset > 1) + { + DEVICELOGW("Gyro[%d] frame drops occurred - %lld missing frames [FrameId %d-%d], time diff = %d (msec)", + frame->sensorIndex, offset - 1, gyroPrevFrame[frame->sensorIndex].prevFrameId + 1, frame->frameId - 1, ns2ms(frame->timestamp - gyroPrevFrame[frame->sensorIndex].prevFrameTimeStamp)); + } + else if (offset < 1) + { + DEVICELOGW("Gyro[%d] frame reorder occurred - prev frameId = %d, new frameId = %d, time diff = %d (msec)", + frame->sensorIndex, gyroPrevFrame[frame->sensorIndex].prevFrameId, frame->frameId, ns2ms(frame->timestamp - gyroPrevFrame[frame->sensorIndex].prevFrameTimeStamp)); + } + } + + gyroPrevFrame[frame->sensorIndex].prevFrameTimeStamp = frame->timestamp; + gyroPrevFrame[frame->sensorIndex].prevFrameId = frame->frameId; + + + std::shared_ptr completeTask = gyroFrameCompleteTask; + mTaskHandler->addTask(completeTask); + break; + } + } + + case SensorType::Velocimeter: + { + std::shared_ptr velocimeterFrameCompleteTask = std::make_shared(mListener, imuHeader, mTM2CorrelatedTimeStampShift, this); + if (velocimeterFrameCompleteTask != NULL) + { + TrackingData::VelocimeterFrame* frame = &velocimeterFrameCompleteTask->mFrame; + int64_t offset = (int64_t)(frame->frameId) - (int64_t)velocimeterPrevFrame[frame->sensorIndex].prevFrameId; + if (velocimeterPrevFrame[frame->sensorIndex].prevFrameId != 0) + { + if (offset > 1) + { + DEVICELOGW("Velocimeter[%d] frame drops occurred - %lld missing frames [FrameId %d-%d], time diff = %d (msec)", + frame->sensorIndex, offset - 1, velocimeterPrevFrame[frame->sensorIndex].prevFrameId + 1, frame->frameId - 1, ns2ms(frame->timestamp - velocimeterPrevFrame[frame->sensorIndex].prevFrameTimeStamp)); + } + else if (offset < 1) + { + DEVICELOGW("Velocimeter[%d] frame reorder occurred - prev frameId = %d, new frameId = %d, time diff = %d (msec)", + frame->sensorIndex, velocimeterPrevFrame[frame->sensorIndex].prevFrameId, frame->frameId, ns2ms(frame->timestamp - velocimeterPrevFrame[frame->sensorIndex].prevFrameTimeStamp)); + } + } + + velocimeterPrevFrame[frame->sensorIndex].prevFrameTimeStamp = frame->timestamp; + velocimeterPrevFrame[frame->sensorIndex].prevFrameId = frame->frameId; + + std::shared_ptr completeTask = velocimeterFrameCompleteTask; + mTaskHandler->addTask(completeTask); + break; + } + } + + case SensorType::Accelerometer: + { + std::shared_ptr accelerometerFrameCompleteTask = std::make_shared(mListener, imuHeader, mTM2CorrelatedTimeStampShift, this); + if (accelerometerFrameCompleteTask != NULL) + { + TrackingData::AccelerometerFrame* frame = &accelerometerFrameCompleteTask->mFrame; + int64_t offset = (int64_t)(frame->frameId) - (int64_t)accelerometerPrevFrame[frame->sensorIndex].prevFrameId; + if (accelerometerPrevFrame[frame->sensorIndex].prevFrameId != 0) + { + if (offset > 1) + { + DEVICELOGW("Accelerometer[%d] frame drops occurred - %lld missing frames [FrameId %d-%d], time diff = %d (msec)", + frame->sensorIndex, offset - 1, accelerometerPrevFrame[frame->sensorIndex].prevFrameId + 1, frame->frameId - 1, ns2ms(frame->timestamp - accelerometerPrevFrame[frame->sensorIndex].prevFrameTimeStamp)); + } + else if (offset < 1) + { + DEVICELOGW("Accelerometer[%d] frame reorder occurred - prev frameId = %d, new frameId = %d, time diff = %d (msec)", + frame->sensorIndex, accelerometerPrevFrame[frame->sensorIndex].prevFrameId, frame->frameId, ns2ms(frame->timestamp - accelerometerPrevFrame[frame->sensorIndex].prevFrameTimeStamp)); + } + } + + accelerometerPrevFrame[frame->sensorIndex].prevFrameTimeStamp = frame->timestamp; + accelerometerPrevFrame[frame->sensorIndex].prevFrameId = frame->frameId; + + std::shared_ptr completeTask = accelerometerFrameCompleteTask; + mTaskHandler->addTask(completeTask); + break; + } + } + + default: + DEVICELOGE("Unsupported sensor (0x%X) on interrupt endpoint thread function", imuHeader->bSensorID); + break; + } + break; + } + + case DEV_STATUS: + { + interrupt_message_status* status = (interrupt_message_status*)header; + DEVICELOGD("Got DEV status %s (0x%X) on interrupt endpoint", statusCodeToString((MESSAGE_STATUS)status->wStatus).c_str(), status->wStatus); + + auto hostStatus = fwToHostStatus((MESSAGE_STATUS)status->wStatus); + + /* DEVICE_STOPPED is indicated once when device exists from ACTIVE_STATE, all other statuses will be indicated here */ + if (hostStatus != Status::DEVICE_STOPPED) + { + /* Creating onStatusEvent to indicate hostStatus */ + std::shared_ptr ptr = std::make_shared(mListener, hostStatus, this); + mTaskHandler->addTask(ptr); + } + + break; + } + + case SLAM_STATUS: + { + interrupt_message_status* status = (interrupt_message_status*)header; + DEVICELOGD("Got SLAM status %s (0x%X) on interrupt endpoint", slamStatusCodeToString((SLAM_STATUS_CODE)status->wStatus).c_str(), status->wStatus); + + if ((SLAM_STATUS_CODE)status->wStatus != SLAM_STATUS_CODE_SUCCESS) + { + auto hostStatus = slamToHostStatus((SLAM_STATUS_CODE)status->wStatus); + std::shared_ptr ptr = std::make_shared(mListener, hostStatus, this); + mTaskHandler->addTask(ptr); + } + + break; + } + + case SLAM_ERROR: + { + interrupt_message_slam_error* error = (interrupt_message_slam_error*)header; + DEVICELOGD("Got SLAM error %s (0x%X) on interrupt endpoint", slamErrorCodeToString((SLAM_ERROR_CODE)error->wStatus).c_str(), error->wStatus); + + if ((SLAM_ERROR_CODE)error->wStatus != SLAM_ERROR_CODE_NONE) + { + auto hostStatus = slamErrorToHostStatus((SLAM_ERROR_CODE)error->wStatus); + std::shared_ptr ptr = std::make_shared(mListener, hostStatus, this); + mTaskHandler->addTask(ptr); + } + + break; + } + + case CONTROLLER_DEVICE_DISCOVERY_EVENT: + { + interrupt_message_controller_device_discovery* controllerDeviceDiscovery = (interrupt_message_controller_device_discovery*)header; + std::shared_ptr ptr = std::make_shared(mListener, controllerDeviceDiscovery, this); + + DEVICELOGD("Controller discovered: Mac Address [%02X:%02X:%02X:%02X:%02X:%02X], AddressType [0x%X], Manufacturer ID [0x%X], Vendor Data [0x%X], App [%u.%u.%u], Boot Loader [%u.%u.%u], Soft Device [%u], Protocol [%u]", + controllerDeviceDiscovery->bMacAddress[0], controllerDeviceDiscovery->bMacAddress[1], controllerDeviceDiscovery->bMacAddress[2], controllerDeviceDiscovery->bMacAddress[3], controllerDeviceDiscovery->bMacAddress[4], controllerDeviceDiscovery->bMacAddress[5], + controllerDeviceDiscovery->bAddressType, + controllerDeviceDiscovery->info.wManufacturerId, + controllerDeviceDiscovery->info.bVendorData, + controllerDeviceDiscovery->info.bAppVersionMajor, controllerDeviceDiscovery->info.bAppVersionMinor, controllerDeviceDiscovery->info.bAppVersionPatch, + controllerDeviceDiscovery->info.bBootloaderVersionMajor, controllerDeviceDiscovery->info.bBootloaderVersionMinor, controllerDeviceDiscovery->info.bBootloaderVersionPatch, + controllerDeviceDiscovery->info.bSoftdeviceVersion, + controllerDeviceDiscovery->info.bProtocolVersion); + + mTaskHandler->addTask(ptr); + break; + } + + case CONTROLLER_CALIBRATION_STATUS_EVENT: + { + interrupt_message_controller_calibration_status* calibrationStatus = (interrupt_message_controller_calibration_status*)header; + DEVICELOGD("Got Controller[%d] calibration status %s (0x%X) on interrupt endpoint", calibrationStatus->bControllerID, controllerCalibrationStatusCodeToString((CONTROLLER_CALIBRATION_STATUS_CODE)calibrationStatus->wStatus).c_str(), calibrationStatus->wStatus); + + auto hostStatus = controllerCalibrationToHostStatus((CONTROLLER_CALIBRATION_STATUS_CODE)calibrationStatus->wStatus); + std::shared_ptr ptr = std::make_shared(mListener, calibrationStatus->bControllerID, hostStatus, this); + mTaskHandler->addTask(ptr); + break; + } + + case CONTROLLER_DEVICE_CONNECTED_EVENT: + { + interrupt_message_controller_connected* controllerDeviceConnect = (interrupt_message_controller_connected*)header; + std::shared_ptr ptr = std::make_shared(mListener, controllerDeviceConnect, this); + + std::string status = "Error"; + switch ((MESSAGE_STATUS)controllerDeviceConnect->wStatus) + { + case MESSAGE_STATUS::SUCCESS: + status = "Success"; + break; + case MESSAGE_STATUS::TIMEOUT: + status = "Timeout"; + break; + case MESSAGE_STATUS::INCOMPATIBLE: + status = "Incompatible"; + break; + } + + DEVICELOGD("---------------------------------------"); + DEVICELOGD("Controller %d connection Info", controllerDeviceConnect->bControllerID); + DEVICELOGD("--------------------+------------------"); + DEVICELOGD("Status | %s (0x%X)", status.c_str(), controllerDeviceConnect->wStatus); + DEVICELOGD("Controller ID | %d", controllerDeviceConnect->bControllerID); + DEVICELOGD("Manufacturer ID | 0x%X", controllerDeviceConnect->info.wManufacturerId); + DEVICELOGD("App Version | %u.%u.%u", controllerDeviceConnect->info.bAppVersionMajor, controllerDeviceConnect->info.bAppVersionMinor, controllerDeviceConnect->info.bAppVersionPatch); + DEVICELOGD("Boot Loader Version | %u.%u.%u", controllerDeviceConnect->info.bBootloaderVersionMajor, controllerDeviceConnect->info.bBootloaderVersionMinor, controllerDeviceConnect->info.bBootloaderVersionPatch); + DEVICELOGD("Soft Device Version | %u", controllerDeviceConnect->info.bSoftdeviceVersion); + DEVICELOGD("Protocol Version | %u", controllerDeviceConnect->info.bProtocolVersion); + DEVICELOGD("--------------------+------------------"); + + mTaskHandler->addTask(ptr); + break; + } + + case CONTROLLER_DEVICE_DISCONNECTED_EVENT: + { + interrupt_message_controller_disconnected* controllerDeviceDisconnect = (interrupt_message_controller_disconnected*)header; + std::shared_ptr ptr = std::make_shared(mListener, controllerDeviceDisconnect, this); + mTaskHandler->addTask(ptr); + break; + } + + case CONTROLLER_DEVICE_LED_INTENSITY_EVENT: + { + interrupt_message_controller_led_intensity *ledMessage = (interrupt_message_controller_led_intensity*)msgBuffer; + std::shared_ptr ptr = std::make_shared(mListener, ledMessage, mTM2CorrelatedTimeStampShift, this); + mTaskHandler->addTask(ptr); + break; + } + + case SLAM_SET_LOCALIZATION_DATA_STREAM: + { + interrupt_message_set_localization_data_stream* setLocalizationDataStream = (interrupt_message_set_localization_data_stream*)header; + std::shared_ptr ptr = std::make_shared(mListener, setLocalizationDataStream, this); + + DEVICELOGV("Got Set Localization Data frame complete: status = 0x%X", setLocalizationDataStream->wStatus); + + MessageON_ASYNC_STOP setMsg; + mDispatcher->postMessage(&mFsm, setMsg); + + mTaskHandler->addTask(ptr); + break; + } + + case DEV_FIRMWARE_UPDATE: + { + interrupt_message_fw_update_stream* fwUpdateStream = (interrupt_message_fw_update_stream*)header; + std::shared_ptr ptr = std::make_shared(mListener, fwUpdateStream, this); + + uint32_t isCentralFWUpdate = 0; + for (int i = 0; i < MAC_ADDRESS_SIZE; i++) + { + isCentralFWUpdate += fwUpdateStream->bMacAddress[i]; + } + if (isCentralFWUpdate == 0) + { + DEVICELOGD("Got Central FW update frame: status = 0x%X, progress = %u%%", fwUpdateStream->wStatus, fwUpdateStream->bProgress); + if (fwUpdateStream->wStatus != (uint16_t)MESSAGE_STATUS::SUCCESS) + { + DEVICELOGE("Failed on BLE FW update with status = 0x%X", fwUpdateStream->wStatus); + mDispatcher->postMessage(&mFsm, Message(ON_ERROR)); + } + } + else + { + DEVICELOGD("Got Controller FW update frame: MAC address [%02X:%02X:%02X:%02X:%02X:%02X], status = 0x%X, progress = %u%%", + fwUpdateStream->bMacAddress[0], fwUpdateStream->bMacAddress[1], fwUpdateStream->bMacAddress[2], fwUpdateStream->bMacAddress[3], fwUpdateStream->bMacAddress[4], fwUpdateStream->bMacAddress[5], + fwUpdateStream->wStatus, fwUpdateStream->bProgress); + } + + if ((fwUpdateStream->wStatus != (uint16_t)MESSAGE_STATUS::SUCCESS || + fwUpdateStream->bProgress == 100) && mFsm.getCurrentState() == ASYNC_STATE) + { + mAsyncCV.notify_one(); + } + mTaskHandler->addTask(ptr); + break; + } + + default: + DEVICELOGE("Unsupported message (0x%X) on interrupt endpoint thread function", header->wMessageID); + break; + } + } + + mInterruptEndpointThreadActive = false; + DEVICELOGD("Thread Stop - Interrupt thread (Accelerometer/Velocimeter/Gyro/6DOF/Controller/Localization events)"); + return; + } + + Status Device::GetDeviceInfoInternal() + { + bulk_message_request_get_device_info request = {0}; + bulk_message_response_get_device_info response = {0}; + std::stringstream portChainString; + + request.header.wMessageID = DEV_GET_DEVICE_INFO; + request.header.dwLength = sizeof(request); + + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + mFsm.fireEvent(msg); + + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + if (memcmp(&mDeviceInfo, &response.message, sizeof(mDeviceInfo)) == 0) + { + /* Device info didn't change */ + return Status::SUCCESS; + } + + mDeviceInfo = response.message; + + if (mUsbDescriptor.portChainDepth > 1) + { + const char* seperator = ""; + portChainString << "(PC "; + + for (uint8_t i = 0; i < mUsbDescriptor.portChainDepth; i++) + { + portChainString << seperator << (unsigned int)mUsbDescriptor.portChain[i]; + seperator = "-->"; + } + portChainString << " Camera)"; + } + + auto libusbVersion = libusb_get_version(); + + DEVICELOGD("-----------------------------------------------------------------------------"); + DEVICELOGD("Device Info"); + DEVICELOGD("-----------------------------------------------------------------------------"); + DEVICELOGD("USB | Vendor ID | 0x%04X", mUsbDescriptor.idVendor); + DEVICELOGD(" | Product ID | 0x%04X", mUsbDescriptor.idProduct); + DEVICELOGD(" | Speed | %s (0x%04X)", UsbPlugListener::usbSpeed(mUsbDescriptor.bcdUSB), mUsbDescriptor.bcdUSB); + DEVICELOGD(" | Bus Number | %u", mUsbDescriptor.bus); + DEVICELOGD(" | Port Number | %u %s", mUsbDescriptor.port, portChainString.str().c_str()); + DEVICELOGD("HW | Type | 0x%X (%s)", mDeviceInfo.bDeviceType, (mDeviceInfo.bDeviceType == 0x1) ? "TM2" : "Error"); + DEVICELOGD(" | Status | 0x%X (%s)", (mDeviceInfo.bStatus & 0x7), ((mDeviceInfo.bStatus & 0x7) == 0) ? "Ok" : "Error"); + DEVICELOGD(" | Status Code | 0x%X", mDeviceInfo.dwStatusCode); + DEVICELOGD(" | Extended Status | 0x%X", mDeviceInfo.dwExtendedStatus); + DEVICELOGD(" | HW Version | 0x%X (%s)", mDeviceInfo.bHwVersion, hwVersion(mDeviceInfo.bHwVersion)); + DEVICELOGD(" | ROM Version | 0x%08X", mDeviceInfo.dwRomVersion); + DEVICELOGD(" | EEPROM Version | %u.%u", mDeviceInfo.bEepromDataMajor, mDeviceInfo.bEepromDataMinor); + DEVICELOGD(" | EEPROM Lock State | %s (0x%X)", (mDeviceInfo.bEepromLocked == EEPROM_LOCK_STATE_PERMANENT_LOCKED) ? "Permanent locked" : ((mDeviceInfo.bEepromLocked == EEPROM_LOCK_STATE_LOCKED) ? "Locked" : (mDeviceInfo.bEepromLocked == EEPROM_LOCK_STATE_WRITEABLE) ? "Writeable" : "Unknown"), mDeviceInfo.bEepromLocked); + DEVICELOGD(" | Serial Number | %" PRIx64, bytesSwap(mDeviceInfo.llSerialNumber) >> 16); + DEVICELOGD("FW | FW Version | %u.%u.%u.%u", mDeviceInfo.bFWVersionMajor, mDeviceInfo.bFWVersionMinor, mDeviceInfo.bFWVersionPatch, mDeviceInfo.dwFWVersionBuild); + DEVICELOGD(" | FW Interface Version | %u.%u", mFWInterfaceVersion.dwMajor, mFWInterfaceVersion.dwMinor); + DEVICELOGD(" | Central App Version | %u.%u.%u.%u", mDeviceInfo.bCentralAppVersionMajor, mDeviceInfo.bCentralAppVersionMinor, mDeviceInfo.bCentralAppVersionPatch, mDeviceInfo.dwCentralAppVersionBuild); + DEVICELOGD(" | Central Boot Loader Version | %u.%u.%u", mDeviceInfo.bCentralBootloaderVersionMajor, mDeviceInfo.bCentralBootloaderVersionMinor, mDeviceInfo.bCentralBootloaderVersionPatch); + DEVICELOGD(" | Central Soft Device Version | %u", mDeviceInfo.bCentralSoftdeviceVersion); + DEVICELOGD(" | Central Protocol Version | %u", mDeviceInfo.bCentralProtocolVersion); + DEVICELOGD("Host | Host Version | %u.%u.%u.%u", LIBTM_VERSION_MAJOR, LIBTM_VERSION_MINOR, LIBTM_VERSION_PATCH, LIBTM_VERSION_BUILD); + DEVICELOGD(" | Host Interface Version | %u.%u", LIBTM_API_VERSION_MAJOR, LIBTM_API_VERSION_MINOR); + DEVICELOGD(" | Status | 0x%X (%s)", mDeviceStatus, (mDeviceStatus == Status::SUCCESS) ? "Ok" : "Error"); + DEVICELOGD(" | Device ID | 0x%p", this); + DEVICELOGD("LibUsb | LibUsb Version | %u.%u.%u.%u %s", libusbVersion->major, libusbVersion->minor, libusbVersion->micro, libusbVersion->nano, libusbVersion->rc); + DEVICELOGD("-----------------------------------------------------------------------------"); + DEVICELOGD(" "); + + switch (mDeviceInfo.dwStatusCode) + { + case FW_STATUS_CODE_OK: + return (Status)msg.Result; + break; + case FW_STATUS_CODE_FAIL: + DEVICELOGE("FW Error: Device info returned init failed"); + return Status::ERROR_FW_INTERNAL; + break; + case FW_STATUS_CODE_NO_CALIBRATION_DATA: + DEVICELOGW("FW Error: ******** Device is not calibrated! ********"); + return (Status)msg.Result; + break; + default: + DEVICELOGE("FW Error: Device unknown error (0x%X)", mDeviceInfo.dwStatusCode); + return Status::ERROR_FW_INTERNAL; + break; + } + } + + Status Device::DeviceFlush() + { + bulk_message_request_flush request = {0}; + unsigned char msgBuffer[BUFFER_SIZE] = {0}; + bulk_message_response_flush* response = (bulk_message_response_flush*)&msgBuffer; + bool stopFlush = false; + int actual = 0; + + request.header.wMessageID = DEV_FLUSH; + request.header.dwLength = sizeof(request); + request.ddwToken = std::chrono::duration_cast(std::chrono::high_resolution_clock::now().time_since_epoch()).count(); + + DEVICELOGD("Flushing Command EndPoint - Start"); + + int rc = libusb_bulk_transfer(mDevice, mEndpointBulkMessages | TO_DEVICE, (uint8_t*)&request, BUFFER_SIZE, &actual, USB_TRANSFER_FAST_TIMEOUT_MS); + if (rc != 0 || actual != BUFFER_SIZE) // lets assume no message will be more than 2Gb size + { + DEVICELOGE("USB Error (0x%X)",rc); + return Status::ERROR_USB_TRANSFER; + } + + while (stopFlush == false) + { + DEVICELOGD("Flushing Command EndPoint..."); + rc = libusb_bulk_transfer(mDevice, mEndpointBulkMessages | TO_HOST, (unsigned char*)response, BUFFER_SIZE, &actual, USB_TRANSFER_FAST_TIMEOUT_MS); + + if (response->header.wStatus == toUnderlying(MESSAGE_STATUS::UNKNOWN_MESSAGE_ID)) + { + DEVICELOGE("Command %s is not supported by FW", messageCodeToString(LIBUSB_TRANSFER_TYPE_BULK, response->header.wMessageID).c_str()); + //return Status::NOT_SUPPORTED_BY_FW; + stopFlush = true; + } + + switch (response->header.wMessageID) + { + case DEV_FLUSH: + { + if (response->ddwToken == request.ddwToken) + { + stopFlush = true; + } + break; + } + } + } + + DEVICELOGD("Flushing Command EndPoint - Finish"); + + DEVICELOGD("Flushing Stream EndPoint - Start"); + stopFlush = false; + + while (stopFlush == false) + { + DEVICELOGD("Flushing Stream EndPoint..."); + auto rc = libusb_bulk_transfer(mDevice, mStreamEndpoint | TO_HOST, (unsigned char *)response, BUFFER_SIZE, &actual, USB_TRANSFER_FAST_TIMEOUT_MS); + if (rc == LIBUSB_ERROR_TIMEOUT) + continue; + + if (rc != 0 || actual == 0) + { + DEVICELOGE("Error while flushing stream endpoint (0x%X)", rc); + mDispatcher->postMessage(&mFsm, Message(ON_ERROR)); + break; + } + + if ((response->header.wMessageID == DEV_FLUSH) && (response->ddwToken == request.ddwToken)) + { + stopFlush = true; + } + } + + DEVICELOGD("Flushing Stream EndPoint - Finish"); + + DEVICELOGD("Flushing Event EndPoint - Start"); + stopFlush = false; + + while (stopFlush == false) + { + DEVICELOGD("Flushing Event EndPoint..."); + auto rc = libusb_interrupt_transfer(mDevice, mEndpointInterrupt, (unsigned char *)response, BUFFER_SIZE, &actual, USB_TRANSFER_FAST_TIMEOUT_MS); + if (rc == LIBUSB_ERROR_TIMEOUT) + { + continue; + } + + if (rc != 0 || actual == 0) + { + DEVICELOGE("Error while flushing event endpoint (0x%X)", rc); + mDispatcher->postMessage(&mFsm, Message(ON_ERROR)); + break; + } + + if ((response->header.wMessageID == DEV_FLUSH) && (response->ddwToken == request.ddwToken)) + { + stopFlush = true; + } + } + DEVICELOGD("Flushing Event EndPoint - Finish"); + + return Status::SUCCESS; + } + + Status Device::SetDeviceStreamConfig(uint32_t maxSize) + { + bulk_message_request_stream_config request = {0}; + bulk_message_response_stream_config response = {0}; + + request.header.wMessageID = DEV_STREAM_CONFIG; + request.header.dwLength = sizeof(request); + request.dwMaxSize = maxSize; + + DEVICELOGD("Set device stream config - MaxStreamEndpointSize = %d", request.dwMaxSize); + + Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST, USB_TRANSFER_FAST_TIMEOUT_MS); + mFsm.fireEvent(msg); + + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", msg.Result); + return Status::ERROR_USB_TRANSFER; + } + + return fwToHostStatus((MESSAGE_STATUS)response.header.wStatus); + } + + Status Device::GetUsbConnectionDescriptor() + { + libusb_device_descriptor desc = { 0 }; + auto rc = libusb_get_device_descriptor(mLibusbDevice, &desc); + if (rc != 0) + { + DEVICELOGE("Error: Failed to get device descriptor. LIBUSB_ERROR_CODE: 0x%X (%s)", rc, libusb_error_name(rc)); + return Status::COMMON_ERROR; + } + + mUsbDescriptor.idProduct = desc.idProduct; + mUsbDescriptor.idVendor = desc.idVendor; + mUsbDescriptor.bcdUSB = desc.bcdUSB; + mUsbDescriptor.bus = libusb_get_bus_number(mLibusbDevice); + mUsbDescriptor.port = libusb_get_port_number(mLibusbDevice); + mUsbDescriptor.portChainDepth = libusb_get_port_numbers(mLibusbDevice, &mUsbDescriptor.portChain[0], 64); + + return Status::SUCCESS; + } + + Status Device::GetInterfaceVersionInternal() + { + control_message_request_get_interface_version request = {0}; + control_message_response_get_interface_version response = {0}; + + request.header.bmRequestType = (LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE); + request.header.bRequest = CONTROL_GET_INTERFACE_VERSION; + + Control_Message msg((uint8_t*)&request, sizeof(request), (uint8_t*)&response, sizeof(response)); + + /* Calling onControlMessage */ + mFsm.fireEvent(msg); + if (msg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("Error Transferring CONTROL_GET_INTERFACE_VERSION"); + return Status::ERROR_USB_TRANSFER; + } + + mFWInterfaceVersion = response.message; + DEVICELOGV("Interface Version = %d.%d", mFWInterfaceVersion.dwMajor, mFWInterfaceVersion.dwMinor); + + return Status::SUCCESS; + } + + Status Device::GetDeviceInfo(TrackingData::DeviceInfo& info) + { + Status status = Status::SUCCESS; + status = GetDeviceInfoInternal(); + if (status != Status::SUCCESS) + { + DEVICELOGE("Error: Get Device Info error (0x%X)", status); + return status; + } + + if (mDeviceInfo.bDeviceType > 1) + { + DEVICELOGE("Error reading device info"); + return Status::COMMON_ERROR; + } + + info.usbDescriptor = mUsbDescriptor; + info.version.deviceInterface.set(mFWInterfaceVersion.dwMajor, mFWInterfaceVersion.dwMinor); + info.version.eeprom.set(mDeviceInfo.bEepromDataMajor, mDeviceInfo.bEepromDataMinor); + info.version.host.set(LIBTM_VERSION_MAJOR, LIBTM_VERSION_MINOR, LIBTM_VERSION_PATCH, LIBTM_VERSION_BUILD); + info.version.fw.set(mDeviceInfo.bFWVersionMajor, mDeviceInfo.bFWVersionMinor, mDeviceInfo.bFWVersionPatch, mDeviceInfo.dwFWVersionBuild); + info.version.rom.set(mDeviceInfo.dwRomVersion); + info.version.centralApp.set(mDeviceInfo.bCentralAppVersionMajor, mDeviceInfo.bCentralAppVersionMinor, mDeviceInfo.bCentralAppVersionPatch, mDeviceInfo.dwCentralAppVersionBuild); + info.version.centralBootLoader.set(mDeviceInfo.bCentralBootloaderVersionMajor, mDeviceInfo.bCentralBootloaderVersionMinor, mDeviceInfo.bCentralBootloaderVersionPatch); + info.version.centralSoftDevice.set(mDeviceInfo.bCentralSoftdeviceVersion); + info.version.centralProtocol.set(mDeviceInfo.bCentralProtocolVersion); + info.version.hw.set(mDeviceInfo.bHwVersion); + info.serialNumber = bytesSwap(mDeviceInfo.llSerialNumber); + info.deviceType = mDeviceInfo.bDeviceType; + info.status.host = mDeviceStatus; + + switch (mDeviceInfo.dwStatusCode) + { + case FW_STATUS_CODE_OK: + info.status.hw = Status::SUCCESS; + break; + case FW_STATUS_CODE_FAIL: + info.status.hw = Status::INIT_FAILED; + break; + case FW_STATUS_CODE_NO_CALIBRATION_DATA: + info.status.hw = Status::NO_CALIBRATION_DATA; + break; + default: + info.status.hw = Status::COMMON_ERROR; + break; + } + + switch (mDeviceInfo.bEepromLocked) + { + case EEPROM_LOCK_STATE_WRITEABLE: + info.eepromLockState = LockStateWriteable; + break; + case EEPROM_LOCK_STATE_LOCKED: + info.eepromLockState = LockStateLocked; + break; + case EEPROM_LOCK_STATE_PERMANENT_LOCKED: + info.eepromLockState = LockStatePermanentLocked; + break; + default: + info.eepromLockState = LockStateMax; + break; + } + + info.numAccelerometerProfiles = (uint8_t)mAccelerometerProfiles.size(); + info.numGyroProfile = (uint8_t)mGyroProfiles.size(); + info.numVelocimeterProfile = (uint8_t)mVelocimeterProfiles.size(); + info.numVideoProfiles = (uint8_t)mVideoProfiles.size(); + + return Status::SUCCESS; + } + + Status Device::SetPlayback(bool on) + { + MessageON_PLAYBACK msg(on); + + mDispatcher->sendMessage(&mFsm, msg); + return (Status)msg.Result; + } + + // -[FSM]---------------------------------------------------------------------- + // + // [FSM State::ANY] + + // [FSM State::IDLE] + DEFINE_FSM_STATE_ENTRY(Device, IDLE_STATE) + { + // DEVICELOGD("Entry to IDLE_STATE"); + } + + DEFINE_FSM_ACTION(Device, IDLE_STATE, ON_INIT, msg) + { + msg.Result = toUnderlying(Status::SUCCESS); + } + DEFINE_FSM_GUARD(Device, IDLE_STATE, ON_START, msg) + { + /* Reset 6dof/IMU/Video device statistics from previous run to eliminate wrong drop/reorder errors */ + ResetStatistics(); + + StartThreads(true, true); + + bulk_message_request_start req; + req.header.dwLength = sizeof(bulk_message_request_start); + req.header.wMessageID = DEV_START; + bulk_message_response_start res = {0}; + + DEVICELOGD("Set Start"); + Bulk_Message msg1((uint8_t*)&req, req.header.dwLength, (uint8_t*)&res, sizeof(res), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST, USB_TRANSFER_MEDIUM_TIMEOUT_MS); + onBulkMessage(msg1); + + msg.Result = msg1.Result; + + if (msg1.Result == 0) + { + if (res.header.wStatus == 0) + { + return true; + } + + msg.Result = toUnderlying(fwToHostStatus((MESSAGE_STATUS)res.header.wStatus)); + } + + DEVICELOGE("Error: Set Start Failed (0x%X)", msg.Result); + + StopThreads(true, true, true); + + return false; + } + + DEFINE_FSM_GUARD(Device, IDLE_STATE, ON_ASYNC_START, msg) + { + StartThreads(true, true); + return true; + } + + DEFINE_FSM_ACTION(Device, IDLE_STATE, ON_START, msg) + { + // update listener + MessageON_START m = dynamic_cast(msg); + mListener = m.listener; + } + + DEFINE_FSM_ACTION(Device, IDLE_STATE, ON_STOP, msg) + { + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_GUARD(Device, ACTIVE_STATE, ON_STOP, msg) + { + bulk_message_request_stop req; + req.header.dwLength = sizeof(bulk_message_request_stop); + req.header.wMessageID = DEV_STOP; + bulk_message_response_stop res = {0}; + + DEVICELOGD("Set Stop"); + Bulk_Message msg1((uint8_t*)&req, req.header.dwLength, (uint8_t*)&res, sizeof(res), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST, USB_TRANSFER_MEDIUM_TIMEOUT_MS); + onBulkMessage(msg1); + + msg.Result = msg1.Result; + if (msg1.Result == 0) + { + if (res.header.wStatus == 0) + { + msg.Result = 0; + return true; + } + + msg.Result = toUnderlying(fwToHostStatus((MESSAGE_STATUS)res.header.wStatus)); + return false; + } + return false; + + } + + DEFINE_FSM_ACTION(Device, IDLE_STATE, ON_DETACH, msg) + { + msg.Result = toUnderlying(Status::COMMON_ERROR); + } + + // [FSM State::ACTIVE] + DEFINE_FSM_STATE_ENTRY(Device, ACTIVE_STATE) + { + //DEVICELOGD("Entry to ACTIVE_STATE"); + } + + DEFINE_FSM_STATE_EXIT(Device, ACTIVE_STATE) + { + StopThreads(true, true, true); + + if (mTaskHandler) + { + /* Creating onStatusEvent to indicate DEVICE_STOPPED status */ + std::shared_ptr ptr = std::make_shared(mListener, Status::DEVICE_STOPPED, this); + mTaskHandler->addTask(ptr); + + /* Clean all events from our device completion queue */ + mTaskHandler->removeTasks(this, false); + } + + mListener = NULL; + } + + void Device::StartThreads(bool interruptThread, bool frameThread) + { + DEVICELOGV("Starting interruptThread = %s, frameThread = %s", (interruptThread ? "True" : "False"), (frameThread ? "True" : "False")); + + mStreamEndpointThreadStop = false; + mInterruptEndpointThreadStop = false; + + if (interruptThread == true) + { + /* Start Interrupt thread */ + mInterruptEPThread = std::thread(&Device::interruptEndpointThread, this); + + /* Wait for interrupt thread to start */ + while (mInterruptEndpointThreadActive == false); + } + + if (frameThread == true) + { + /* Start Bulk thread */ + mBulkEPThread = std::thread(&Device::streamEndpointThread, this); + + + /* Wait for Bulk thread to start */ + while (mStreamEndpointThreadActive == false); + } + + DEVICELOGV("All threads started"); + } + + void Device::StopThreads(bool force, bool interruptThread, bool frameThread) + { + DEVICELOGV("Stopping interruptThread = %s, frameThread = %s, force = %s", (interruptThread?"True":"False"), (frameThread ? "True" : "False"), (force ? "True" : "False")); + + if (force == true) + { + /* Time to wait before closing interrupt / event threads - give opportunity for the FW to send the DEVICE_STOPPED status to all endpoint threads */ + std::this_thread::sleep_for(std::chrono::milliseconds(WAIT_FOR_STOP_STATUS_MSEC)); + + mStreamEndpointThreadStop = true; + mInterruptEndpointThreadStop = true; + } + + if (interruptThread == true) + { + /* Stop Interrupt thread and wait for it to stop */ + if (mInterruptEPThread.joinable()) + { + mInterruptEPThread.join(); + } + } + + if (frameThread == true) + { + /* Stop Bulk thread and wait for it to stop */ + if (mBulkEPThread.joinable()) + { + mBulkEPThread.join(); + } + } + + DEVICELOGV("All threads stopped"); + } + + DEFINE_FSM_STATE_ENTRY(Device, ASYNC_STATE) + { + + } + + DEFINE_FSM_STATE_EXIT(Device, ASYNC_STATE) + { + StopThreads(true, true, true); + + // clean all events from our device completion queue + if (mTaskHandler) + { + mTaskHandler->removeTasks(this, true); + } + + mListener = NULL; + } + + DEFINE_FSM_ACTION(Device, ASYNC_STATE, ON_INIT, msg) + { + DEVICELOGE("State [ASYNC_STATE] got event [ON_INIT] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ASYNC_STATE, ON_DONE, msg) + { + DEVICELOGE("State [ASYNC_STATE] got event [ON_DONE] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ASYNC_STATE, ON_START, msg) + { + DEVICELOGE("State [ASYNC_STATE] got event [ON_START] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ASYNC_STATE, ON_STOP, msg) + { + DEVICELOGE("State [ASYNC_STATE] got event [ON_STOP] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ASYNC_STATE, ON_DETACH, msg) + { + DEVICELOGE("State [ASYNC_STATE] got event [ON_DETACH] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ASYNC_STATE, ON_ERROR, msg) + { + DEVICELOGE("State [ASYNC_STATE] got event [ON_ERROR] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ASYNC_STATE, ON_BULK_MESSAGE, msg) + { + onBulkMessage(msg); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ASYNC_STATE, ON_LARGE_MESSAGE, msg) + { + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ASYNC_STATE, ON_CONTROL_MESSAGE, msg) + { + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ASYNC_STATE, ON_PLAYBACK_SET, msg) + { + DEVICELOGE("State [ASYNC_STATE] got event [ON_PLAYBACK_SET] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ASYNC_STATE, ON_SET_ENABLED_STREAMS, msg) + { + DEVICELOGE("State [ASYNC_STATE] got event [ON_SET_ENABLED_STREAMS] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ASYNC_STATE, ON_ASYNC_START, msg) + { + DEVICELOGE("State [ASYNC_STATE] got event [ON_ASYNC_START] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ASYNC_STATE, ON_ASYNC_STOP, msg) + { + msg.Result = toUnderlying(Status::SUCCESS); + } + + + /** + * @brief onBulkMessage - Bulk Endpoint Protocol + * + * The bulk IN/OUT endpoints are used for general communication from the host. + * The protocol on the bulk endpoints is in a request/response convention, + * i.e. a bulk OUT transfer (request) is always followed by a bulk IN transfer (response). + */ + void Device::onBulkMessage(const Message& msg) + { + Bulk_Message usbMsg = dynamic_cast(msg); + int actual = 0; + unsigned char buffer[BUFFER_SIZE] = { 0 }; + perc::copy(buffer, usbMsg.mSrc, usbMsg.srcSize); + bulk_message_request_header* header = (bulk_message_request_header*)buffer; + + int rc = libusb_bulk_transfer(mDevice, usbMsg.mEndpointOut, buffer, BUFFER_SIZE, &actual, usbMsg.mTimeoutInMs); + + DEVICELOGV("Sent request - MessageID: 0x%X (%s), Len: %d, UsbLen: %d, Actual: %d, rc: %d (%s)", + header->wMessageID, messageCodeToString(LIBUSB_TRANSFER_TYPE_BULK, header->wMessageID).c_str(), header->dwLength, BUFFER_SIZE, actual, rc, libusb_error_name(rc)); + if (rc != 0 || actual != BUFFER_SIZE ) // lets assume no message will be more than 2Gb size + { + DEVICELOGE("ERROR: Bulk transfer message 0x%X (%s) request to device got %s. Bytes requested %d, bytes transferred %d", + header->wMessageID, messageCodeToString(LIBUSB_TRANSFER_TYPE_BULK, header->wMessageID).c_str(), libusb_error_name(rc), usbMsg.srcSize, actual); + msg.Result = toUnderlying(Status::ERROR_USB_TRANSFER); + return; + } + + rc = libusb_bulk_transfer(mDevice, usbMsg.mEndpointIn, usbMsg.mDst, usbMsg.dstSize, &actual, usbMsg.mTimeoutInMs); + + bulk_message_response_header* res = (bulk_message_response_header*)usbMsg.mDst; + + DEVICELOGV("Got response - MessageID: 0x%X (%s), Len: %d, Status: 0x%X, UsbLen: %d, Actual: %d, rc: %d (%s)", + res->wMessageID, messageCodeToString(LIBUSB_TRANSFER_TYPE_BULK, res->wMessageID).c_str(), res->dwLength, res->wStatus, usbMsg.dstSize, actual, rc, libusb_error_name(rc)); + + if ((rc == 0) && (header->wMessageID != res->wMessageID)) + { + DEVICELOGE("Command mismatch - Expected 0x%X (%s) length %d, Received 0x%X (%s) length %d", + header->wMessageID, messageCodeToString(LIBUSB_TRANSFER_TYPE_BULK, header->wMessageID).c_str(), usbMsg.dstSize, + res->wMessageID, messageCodeToString(LIBUSB_TRANSFER_TYPE_BULK, res->wMessageID).c_str(), res->dwLength); + } + + if (rc != 0 || actual != usbMsg.dstSize) // lets assume no message will be more than 2Gb size + { + msg.Result = toUnderlying(Status::ERROR_USB_TRANSFER); + if (actual == 0) + { + DEVICELOGE("ERROR: Bulk transfer message 0x%X (%s) response to host got %s. Host did not return answer", header->wMessageID, messageCodeToString(LIBUSB_TRANSFER_TYPE_BULK, header->wMessageID).c_str(),libusb_error_name(rc)); + msg.Result = toUnderlying(Status::ERROR_USB_TRANSFER); + } + else + { + if (res->wStatus == toUnderlying(MESSAGE_STATUS::UNKNOWN_MESSAGE_ID)) + { + DEVICELOGE("Command %s is not supported by FW", messageCodeToString(LIBUSB_TRANSFER_TYPE_BULK, res->wMessageID).c_str()); + msg.Result = toUnderlying(Status::NOT_SUPPORTED_BY_FW); + } + else if(actual > usbMsg.dstSize) + { + DEVICELOGD("WARNING: Bulk transfer message 0x%X (%s) response to host got %s. Bytes requested %d, bytes transferred %d", header->wMessageID, messageCodeToString(LIBUSB_TRANSFER_TYPE_BULK, header->wMessageID).c_str(), libusb_error_name(rc), usbMsg.dstSize, actual); + msg.Result = toUnderlying(Status::ERROR_USB_TRANSFER); + } + else /* actual < usbMsg.dstSize*/ + { + msg.Result = toUnderlying(Status::SUCCESS); + } + } + + return; + } + + if (res->wStatus != toUnderlying(MESSAGE_STATUS::SUCCESS)) + { + DEVICELOGE("MessageID 0x%X (%s) failed with status 0x%X", res->wMessageID, messageCodeToString(LIBUSB_TRANSFER_TYPE_BULK, header->wMessageID).c_str(), res->wStatus); + msg.Result = toUnderlying(Status::COMMON_ERROR); + } + else + { + msg.Result = toUnderlying(Status::SUCCESS); + } + } + + void Device::onControlMessage(const Message& msg) + { + Control_Message usbMsg = dynamic_cast(msg); + control_message_request_header* header = (control_message_request_header*)usbMsg.mSrc; + + DEVICELOGD("Sending Control request - MessageID: 0x%X (%s)", header->bRequest, messageCodeToString(LIBUSB_TRANSFER_TYPE_CONTROL, header->bRequest).c_str()); + int result = libusb_control_transfer(mDevice, header->bmRequestType, header->bRequest, usbMsg.mValue, usbMsg.mIndex, usbMsg.mDst, usbMsg.dstSize, usbMsg.mTimeoutInMs); + + /* Control request Successed if result == expected dstSize or got PIPE error on reset message */ + if ((result == usbMsg.dstSize) || ((header->bRequest == CONTROL_USB_RESET) && (result == LIBUSB_ERROR_PIPE))) + { + msg.Result = toUnderlying(Status::SUCCESS); + return; + } + + DEVICELOGE("ERROR %s while control transfer of messageID: 0x%X (%s)", libusb_error_name(result), header->bRequest, messageCodeToString(LIBUSB_TRANSFER_TYPE_CONTROL, header->bRequest).c_str()); + msg.Result = toUnderlying(Status::ERROR_USB_TRANSFER); + } + + void Device::SendLargeMessage(const Message& msg) + { + Large_Message* pMsg = (Large_Message*)&msg; + const uint32_t length = pMsg->mLength; + const uint8_t* buffer = pMsg->mBuffer; + mListener = pMsg->mListener; + + uint32_t maxChunkLength = MAX_BIG_DATA_MESSAGE_LENGTH - offsetof(bulk_message_large_stream, bPayload); + uint32_t leftLength = length; + uint32_t chunkLength = 0; + uint16_t index = 0; + int actual = 0; + + DEVICELOGD("Set large message send - Total length %d", length); + + auto start = systemTime(); + + bulk_message_large_stream* stream = (bulk_message_large_stream*)malloc(MAX_BIG_DATA_MESSAGE_LENGTH); + if (stream == NULL) + { + LOGE("Error allocating %d buffer", MAX_BIG_DATA_MESSAGE_LENGTH); + msg.Result = toUnderlying(Status::ALLOC_FAILED); + return; + } + + while (leftLength > 0) + { + if (leftLength > maxChunkLength) + { + chunkLength = maxChunkLength; + stream->wStatus = (uint16_t)MESSAGE_STATUS::MORE_DATA_AVAILABLE; + } + else + { + chunkLength = leftLength; + stream->wStatus = (uint16_t)MESSAGE_STATUS::SUCCESS; + } + + DEVICELOGD("Set large message - Chunk %03d: [%09d - %09d], Length %d, Left %d, Status 0x%X", index, (length - leftLength), (length - leftLength + chunkLength), chunkLength, leftLength, stream->wStatus); + + stream->header.wMessageID = pMsg->mMessageId; + stream->header.dwLength = chunkLength + offsetof(bulk_message_large_stream, bPayload); + stream->wIndex = index++; + perc::copy(&stream->bPayload, buffer + (length - leftLength), chunkLength); + + actual = 0; + auto rc = libusb_bulk_transfer(mDevice, mStreamEndpoint | TO_DEVICE, (unsigned char*)stream, stream->header.dwLength, &actual, 5000); + if (rc != 0 || actual == 0) + { + DEVICELOGE("Error while sending large message chunk %d (0x%X)", index, rc); + msg.Result = toUnderlying(Status::ERROR_USB_TRANSFER); + return; + } + leftLength -= chunkLength; + } + + auto finish = systemTime(); + DEVICELOGD("Finished setting large message data - Total length %d with %d chunks in %d (msec)", length, index, ns2ms(finish - start)); + + free(stream); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, IDLE_STATE, ON_ASYNC_START, msg) + { + MessageON_ASYNC_START pMsg = dynamic_cast(msg); + switch (pMsg.mMessageId) + { + case SLAM_GET_LOCALIZATION_DATA: + { + bulk_message_request_get_localization_data request = { 0 }; + bulk_message_response_get_localization_data response = { 0 }; + + mListener = pMsg.mListener; + request.header.wMessageID = pMsg.mMessageId; + request.header.dwLength = sizeof(request); + + DEVICELOGD("Get Localization Data"); + + Bulk_Message bulkMsg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + onBulkMessage(bulkMsg); + + if (bulkMsg.Result != toUnderlying(Status::SUCCESS)) + { + DEVICELOGE("USB Error (0x%X)", bulkMsg.Result); + msg.Result = toUnderlying(Status::ERROR_USB_TRANSFER); + return; + } + + msg.Result = response.header.wStatus; + break; + } + + case SLAM_SET_LOCALIZATION_DATA_STREAM: + { + return SendLargeMessage(msg); + } + + case DEV_FIRMWARE_UPDATE: + { + return SendLargeMessage(msg); + } + } + + return; + } + + DEFINE_FSM_ACTION(Device, IDLE_STATE, ON_ASYNC_STOP, msg) + { + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, IDLE_STATE, ON_SET_ENABLED_STREAMS, msg) + { + MessageON_SET_ENABLED_STREAMS pMsg = dynamic_cast(msg); + auto message = pMsg.mMessage; + auto wNumEnabledStreams = pMsg.mNumEnabledStreams; + + for (int i = 0; i < wNumEnabledStreams; i++) + { + if (message[i].bPixelFormat != PixelFormat::ANY) + { + mFrameTempBufferSize = message[i].wHeight*message[i].wStride + sizeof(bulk_message_video_stream); + + AllocateBuffers(); + break; + } + } + + for (int i = 0; i < wNumEnabledStreams; i++) + { + if (GET_SENSOR_TYPE(message[i].bSensorID) <= SensorType::Fisheye) + { + mWidthsMap[message[i].bSensorID] = message[i].wWidth; + mHeightsMap[message[i].bSensorID] = message[i].wHeight; + } + } + + uint8_t reqBuffer[BUFFER_SIZE]; + bulk_message_request_raw_streams_control* req = (bulk_message_request_raw_streams_control*)reqBuffer; + + req->header.wMessageID = mPlaybackIsOn ? DEV_RAW_STREAMS_PLAYBACK_CONTROL : DEV_RAW_STREAMS_CONTROL; + req->wNumEnabledStreams = wNumEnabledStreams; + req->header.dwLength = wNumEnabledStreams * sizeof(supported_raw_stream_libtm_message) + sizeof(req->header) + sizeof(req->wNumEnabledStreams); + + for (int i = 0; i < wNumEnabledStreams; i++) + { + req->stream[i] = message[i]; + } + + bulk_message_response_raw_streams_control res; + + DEVICELOGD("Set %d Supported RAW Streams %sControl", req->wNumEnabledStreams, (mPlaybackIsOn)?"Playback ":""); + printSupportedRawStreams(message, wNumEnabledStreams); + + Bulk_Message msgUsb((uint8_t*)req, req->header.dwLength, (uint8_t*)&res, sizeof(res), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST); + onBulkMessage(msgUsb); + + msg.Result = msgUsb.Result; + } + + DEFINE_FSM_ACTION(Device, IDLE_STATE, ON_BULK_MESSAGE, msg) + { + onBulkMessage(msg); + } + + DEFINE_FSM_ACTION(Device, IDLE_STATE, ON_LARGE_MESSAGE, msg) + { + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, IDLE_STATE, ON_PLAYBACK_SET, msg) + { + MessageON_PLAYBACK pMsg = dynamic_cast(msg); + this->mPlaybackIsOn = pMsg.set; + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, IDLE_STATE, ON_CONTROL_MESSAGE, msg) + { + onControlMessage(msg); + } + + DEFINE_FSM_ACTION(Device, IDLE_STATE, ON_ERROR, msg) + { + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ERROR_STATE, ON_STOP, msg) + { + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ERROR_STATE, ON_CONTROL_MESSAGE, msg) + { + onControlMessage(msg); + } + + DEFINE_FSM_ACTION(Device, ERROR_STATE, ON_BULK_MESSAGE, msg) + { + //DEVICELOGE("State [ERROR_STATE] got event [ON_BULK_MESSAGE] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ERROR_STATE, ON_LARGE_MESSAGE, msg) + { + //DEVICELOGE("State [ERROR_STATE] got event [ON_LARGE_MESSAGE] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_STATE_ENTRY(Device, ERROR_STATE) + { + DEVICELOGE("Entered state [ERROR_STATE]"); + mSyncTimeEnabled = false; + } + + DEFINE_FSM_ACTION(Device, ACTIVE_STATE, ON_BULK_MESSAGE, msg) + { + onBulkMessage(msg); + } + + DEFINE_FSM_ACTION(Device, ACTIVE_STATE, ON_LARGE_MESSAGE, msg) + { + SendLargeMessage(msg); + } + + DEFINE_FSM_ACTION(Device, ACTIVE_STATE, ON_CONTROL_MESSAGE, msg) + { + onControlMessage(msg); + } + + DEFINE_FSM_ACTION(Device, ACTIVE_STATE, ON_STOP, msg) + { + + } + + DEFINE_FSM_ACTION(Device, ACTIVE_STATE, ON_DETACH, msg) + { + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ACTIVE_STATE, ON_ERROR, msg) + { + DEVICELOGE("State [ACTIVE_STATE] got event [ON_ERROR] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ACTIVE_STATE, ON_INIT, msg) + { + DEVICELOGE("State [ACTIVE_STATE] got event [ON_INIT] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ACTIVE_STATE, ON_DONE, msg) + { + DEVICELOGE("State [ACTIVE_STATE] got event [ON_DONE] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ACTIVE_STATE, ON_START, msg) + { + DEVICELOGE("State [ACTIVE_STATE] got event [ON_START] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ACTIVE_STATE, ON_PLAYBACK_SET, msg) + { + DEVICELOGE("State [ACTIVE_STATE] got event [ON_PLAYBACK_SET] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ACTIVE_STATE, ON_ASYNC_START, msg) + { + DEVICELOGE("State [ACTIVE_STATE] got event [ON_ASYNC_START] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + DEFINE_FSM_ACTION(Device, ACTIVE_STATE, ON_ASYNC_STOP, msg) + { + DEVICELOGE("State [ACTIVE_STATE] got event [ON_ASYNC_STOP] ==> [ERROR_STATE]"); + msg.Result = toUnderlying(Status::SUCCESS); + } + + // [FSM State::ERROR] + /*DEFINE_FSM_ACTION(Device, ERROR_STATE, ON_DONE, msg) + { + }*/ + + // -[Definition]--------------------------------------------------------------- + #define FSM_ERROR_TIMEOUT 1000 + + // [FSM State::IDLE] + DEFINE_FSM_STATE_BEGIN(Device, IDLE_STATE) + TRANSITION_INTERNAL(ON_INIT, 0, ACTION(Device, IDLE_STATE, ON_INIT)) + TRANSITION(ON_DETACH, 0, ACTION(Device, IDLE_STATE, ON_DETACH), ERROR_STATE) + TRANSITION(ON_START, GUARD(Device, IDLE_STATE, ON_START), ACTION(Device, IDLE_STATE, ON_START), ACTIVE_STATE) + TRANSITION(ON_STOP, 0, ACTION(Device, IDLE_STATE, ON_STOP), IDLE_STATE) + TRANSITION(ON_ERROR, 0, ACTION(Device, IDLE_STATE, ON_ERROR), ERROR_STATE) + TRANSITION(ON_ASYNC_STOP, 0, ACTION(Device, IDLE_STATE, ON_ERROR), ERROR_STATE) + TRANSITION(ON_ASYNC_START, GUARD(Device, IDLE_STATE, ON_ASYNC_START), ACTION(Device, IDLE_STATE, ON_ASYNC_START), ASYNC_STATE) + TRANSITION_INTERNAL(ON_BULK_MESSAGE, 0, ACTION(Device, IDLE_STATE, ON_BULK_MESSAGE)) + TRANSITION_INTERNAL(ON_LARGE_MESSAGE, 0, ACTION(Device, IDLE_STATE, ON_LARGE_MESSAGE)) + TRANSITION_INTERNAL(ON_CONTROL_MESSAGE, 0, ACTION(Device, IDLE_STATE, ON_CONTROL_MESSAGE)) + TRANSITION_INTERNAL(ON_PLAYBACK_SET, 0, ACTION(Device, IDLE_STATE, ON_PLAYBACK_SET)) + TRANSITION_INTERNAL(ON_SET_ENABLED_STREAMS, 0, ACTION(Device, IDLE_STATE, ON_SET_ENABLED_STREAMS)) + DEFINE_FSM_STATE_END(Device, IDLE_STATE, ENTRY(Device, IDLE_STATE), 0) + + // [FSM State::ASYNC] + DEFINE_FSM_STATE_BEGIN(Device, ASYNC_STATE) + TRANSITION(ON_INIT, 0, ACTION(Device, ASYNC_STATE, ON_INIT), ERROR_STATE) + TRANSITION(ON_DONE, 0, ACTION(Device, ASYNC_STATE, ON_DONE), ERROR_STATE) + TRANSITION(ON_START, 0, ACTION(Device, ASYNC_STATE, ON_START), ERROR_STATE) + TRANSITION(ON_STOP, 0, ACTION(Device, ASYNC_STATE, ON_STOP), ERROR_STATE) + TRANSITION(ON_DETACH, 0, ACTION(Device, ASYNC_STATE, ON_DETACH), ERROR_STATE) + TRANSITION(ON_ERROR, 0, ACTION(Device, ASYNC_STATE, ON_ERROR), ERROR_STATE) + TRANSITION(ON_PLAYBACK_SET, 0, ACTION(Device, ASYNC_STATE, ON_PLAYBACK_SET), ERROR_STATE) + TRANSITION(ON_ASYNC_START, 0, ACTION(Device, ASYNC_STATE, ON_ASYNC_START), ERROR_STATE) + TRANSITION(ON_ASYNC_STOP, 0, ACTION(Device, ASYNC_STATE, ON_ASYNC_STOP), IDLE_STATE) + TRANSITION_INTERNAL(ON_BULK_MESSAGE, 0, ACTION(Device, ASYNC_STATE, ON_BULK_MESSAGE)) + TRANSITION_INTERNAL(ON_LARGE_MESSAGE, 0, ACTION(Device, ASYNC_STATE, ON_LARGE_MESSAGE)) + TRANSITION_INTERNAL(ON_CONTROL_MESSAGE, 0, ACTION(Device, ASYNC_STATE, ON_CONTROL_MESSAGE)) + TRANSITION_AFTER(0, ACTION(Device, ASYNC_STATE, ON_ERROR), ERROR_STATE, 60000) /* Protect ASYNC state - can't be in this state more than 60 sec, usually means the async operation never completed */ + DEFINE_FSM_STATE_END(Device, ASYNC_STATE, ENTRY(Device, ASYNC_STATE), EXIT(Device, ASYNC_STATE)) + + // [FSM State::ACTIVE] + DEFINE_FSM_STATE_BEGIN(Device, ACTIVE_STATE) + TRANSITION(ON_STOP, GUARD(Device, ACTIVE_STATE, ON_STOP), ACTION(Device, ACTIVE_STATE, ON_STOP), IDLE_STATE) + TRANSITION(ON_STOP, 0, ACTION(Device, ACTIVE_STATE, ON_STOP), ERROR_STATE) + TRANSITION(ON_DETACH, 0, ACTION(Device, ACTIVE_STATE, ON_DETACH), ERROR_STATE) + TRANSITION(ON_ERROR, 0, ACTION(Device, ACTIVE_STATE, ON_ERROR), ERROR_STATE) + TRANSITION(ON_INIT, 0, ACTION(Device, ACTIVE_STATE, ON_INIT), ERROR_STATE) + TRANSITION(ON_DONE, 0, ACTION(Device, ACTIVE_STATE, ON_DONE), ERROR_STATE) + TRANSITION(ON_START, 0, ACTION(Device, ACTIVE_STATE, ON_START), ERROR_STATE) + TRANSITION(ON_PLAYBACK_SET, 0, ACTION(Device, ACTIVE_STATE, ON_PLAYBACK_SET), ERROR_STATE) + TRANSITION(ON_ASYNC_START, 0, ACTION(Device, ACTIVE_STATE, ON_ASYNC_START), ERROR_STATE) + TRANSITION(ON_ASYNC_STOP, 0, ACTION(Device, ACTIVE_STATE, ON_ASYNC_STOP), IDLE_STATE) + TRANSITION_INTERNAL(ON_BULK_MESSAGE, 0, ACTION(Device, ACTIVE_STATE, ON_BULK_MESSAGE)) + TRANSITION_INTERNAL(ON_LARGE_MESSAGE, 0, ACTION(Device, ACTIVE_STATE, ON_LARGE_MESSAGE)) + TRANSITION_INTERNAL(ON_CONTROL_MESSAGE, 0, ACTION(Device, ACTIVE_STATE, ON_CONTROL_MESSAGE)) + DEFINE_FSM_STATE_END(Device, ACTIVE_STATE, ENTRY(Device, ACTIVE_STATE), EXIT(Device, ACTIVE_STATE)) + + // [FSM State::ERROR] + DEFINE_FSM_STATE_BEGIN(Device, ERROR_STATE) + //TRANSITION(ON_DONE, 0, ACTION(ERROR_STATE, ON_DONE), IDLE_STATE) + TRANSITION_INTERNAL(ON_STOP, 0, ACTION(Device, ERROR_STATE, ON_STOP)) + TRANSITION_INTERNAL(ON_BULK_MESSAGE, 0, ACTION(Device, ERROR_STATE, ON_BULK_MESSAGE)) + TRANSITION_INTERNAL(ON_LARGE_MESSAGE, 0, ACTION(Device, ERROR_STATE, ON_LARGE_MESSAGE)) + TRANSITION_INTERNAL(ON_CONTROL_MESSAGE, 0, ACTION(Device, ERROR_STATE, ON_CONTROL_MESSAGE)) + DEFINE_FSM_STATE_END(Device, ERROR_STATE, ENTRY(Device, ERROR_STATE), 0) + + // -[FSM: Definition]---------------------------------------------------------- + DEFINE_FSM_BEGIN(Device, main) + STATE(Device, IDLE_STATE) + STATE(Device, ASYNC_STATE) + STATE(Device, ACTIVE_STATE) + STATE(Device, ERROR_STATE) + DEFINE_FSM_END() + + // ---------------------------------------------------------------------------- +} // namespace diff --git a/third-party/libtm/libtm/src/Device.h b/third-party/libtm/libtm/src/Device.h new file mode 100644 index 0000000000..5b825f6a4b --- /dev/null +++ b/third-party/libtm/libtm/src/Device.h @@ -0,0 +1,414 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include "TrackingCommon.h" +#include "TrackingDevice.h" +#include "TrackingData.h" +#include "Fsm.h" +#include "Message.h" +#include +#include "CompleteTask.h" +#include +#include +#include +#include + +#define INTERFACE_INDEX 0 /* TM2 device interface descriptor index */ + +#ifdef _WIN32 +#pragma warning (push) +#pragma warning (disable : 4200) +#endif +#include "libusb.h" +#ifdef _WIN32 +#pragma warning (pop) +#endif + +namespace perc +{ + class Device : public TrackingDevice, + public EventHandler, + public FrameBuffersOwner + { + public: + Device(libusb_device*, Dispatcher* dispatcher, EventHandler* owner, CompleteQueueHandler* taskHandler); + virtual ~Device(); + + // [interface] - TrackingDevice + virtual Status GetSupportedProfile(TrackingData::Profile& profile); + virtual Status Start(Listener*, TrackingData::Profile* = NULL); + virtual Status Stop(); + virtual Status GetDeviceInfo(TrackingData::DeviceInfo& info); + virtual Status GetSupportedRawStreams(TrackingData::VideoProfile* videoProfiles, TrackingData::GyroProfile* gyroProfiles, TrackingData::AccelerometerProfile* accelerometerProfiles, TrackingData::VelocimeterProfile* velocimeterProfiles = nullptr); + virtual Status SetFWLogControl(const TrackingData::LogControl& logControl); + virtual Status GetFWLog(TrackingData::Log& log); + virtual Status GetCameraIntrinsics(SensorId id, TrackingData::CameraIntrinsics& intrinsics); + virtual Status SetCameraIntrinsics(SensorId id, const TrackingData::CameraIntrinsics& intrinsics); + virtual Status GetMotionModuleIntrinsics(SensorId id, TrackingData::MotionIntrinsics& intrinsics); + virtual Status SetMotionModuleIntrinsics(SensorId id, const TrackingData::MotionIntrinsics& intrinsics); + virtual Status GetExtrinsics(SensorId id, TrackingData::SensorExtrinsics& extrinsics); + virtual Status SetOccupancyMapControl(uint8_t enable); + virtual Status GetPose(TrackingData::PoseFrame& pose, uint8_t sourceIndex); + virtual Status SetExposureModeControl(const TrackingData::ExposureModeControl& mode); + virtual Status SetExposure(const TrackingData::Exposure& exposure); + virtual Status GetTemperature(TrackingData::Temperature& temperature); + virtual Status SetTemperatureThreshold(const TrackingData::Temperature& temperature, uint32_t token); + virtual Status LockConfiguration(LockType type, bool lock); + virtual Status PermanentLockConfiguration(LockType type, uint32_t token); + virtual Status ReadConfiguration(uint16_t tableType, uint16_t size, uint8_t* buffer, uint16_t* actualSize = nullptr); + virtual Status WriteConfiguration(uint16_t tableType, uint16_t size, uint8_t* buffer); + virtual Status DeleteConfiguration(uint16_t tableType); + virtual Status GetLocalizationData(Listener* listener); + virtual Status SetLocalizationData(Listener* listener, uint32_t length, const uint8_t* buffer); + virtual Status ResetLocalizationData(uint8_t flag); + virtual Status SetStaticNode(const char* guid, const TrackingData::RelativePose& relativePose); + virtual Status GetStaticNode(const char* guid, TrackingData::RelativePose& relativePose); + virtual Status SetGeoLocation(const TrackingData::GeoLocalization& geoLocation); + virtual Status EepromRead(uint16_t offset, uint16_t size, uint8_t* buffer, uint16_t& actual); + virtual Status EepromWrite(uint16_t offset, uint16_t size, uint8_t* buffer, uint16_t& actual, bool verify = false); + virtual Status Reset(void); + virtual Status SendFrame(const TrackingData::VelocimeterFrame& frame); + virtual Status SendFrame(const TrackingData::VideoFrame& frame); + virtual Status SendFrame(const TrackingData::GyroFrame& frame); + virtual Status SendFrame(const TrackingData::AccelerometerFrame& frame); + virtual Status ControllerConnect(const TrackingData::ControllerDeviceConnect& device, uint8_t& controllerId); + virtual Status ControllerDisconnect(uint8_t controllerId); + virtual Status ControllerStartCalibration(uint8_t controllerId); + virtual Status GetAssociatedDevices(TrackingData::ControllerAssociatedDevices& devices); + virtual Status SetAssociatedDevices(const TrackingData::ControllerAssociatedDevices& devices); + virtual Status ControllerSendData(const TrackingData::ControllerData& controllerData); + virtual Status ControllerRssiTestControl(uint8_t controllerId, bool testControl); + virtual Status SetGpioControl(uint8_t gpioControl); + virtual Status ControllerFWUpdate(const TrackingData::ControllerFW& FW) override; + // [interface] EventHandler + virtual void onTimeout(uintptr_t timerId, const Message &msg); + + bool IsDeviceReady() { return (mUsbState == DEVICE_USB_STATE_READY); } + + protected: + // [interface] - EventHandler + virtual void onMessage(const Message &) override {}; + virtual void onExit() override; + + virtual void putBufferBack(SensorId id, std::shared_ptr& frame); + + // [USB States] + typedef enum { + DEVICE_USB_STATE_INIT = 0, + DEVICE_USB_STATE_OPENED, + DEVICE_USB_STATE_CLAIMED, + DEVICE_USB_STATE_READY, + DEVICE_USB_STATE_MAX + } DEVICE_USB_STATE; + + // [FSM] declaration + Fsm mFsm; + DECLARE_FSM(main); + + // [States] + enum { + IDLE_STATE = FSM_STATE_USER_DEFINED, + ASYNC_STATE, + ACTIVE_STATE, + ERROR_STATE, + }; + + // [Messages] + enum { + ON_INIT = FSM_EVENT_USER_DEFINED, + ON_DONE, + ON_START, + ON_STOP, + ON_DETACH, + ON_ERROR, + ON_BULK_MESSAGE, + ON_CONTROL_MESSAGE, + ON_PLAYBACK_SET, + ON_SET_ENABLED_STREAMS, + ON_ASYNC_START, + ON_ASYNC_STOP, + ON_LARGE_MESSAGE + }; + + /* The timeout that USB transfer should wait before giving up due to no response being received */ + enum { + USB_TRANSFER_FAST_TIMEOUT_MS = 100, /* Most of the messages uses fast timeout */ + USB_TRANSFER_MEDIUM_TIMEOUT_MS = 5000, /* All Control messages + Start/Stop/GetTemperature may need medium timeout to complete */ + USB_TRANSFER_SLOW_TIMEOUT_MS = 16000, /* Controller connect/disconnect may need slow timeout to complete */ + }; + + // [State] IDLE + DECLARE_FSM_STATE(IDLE_STATE); + DECLARE_FSM_STATE_ENTRY(IDLE_STATE); + DECLARE_FSM_ACTION(IDLE_STATE, ON_INIT); + DECLARE_FSM_ACTION(IDLE_STATE, ON_START); + DECLARE_FSM_ACTION(IDLE_STATE, ON_DETACH); + DECLARE_FSM_ACTION(IDLE_STATE, ON_PLAYBACK_SET); + DECLARE_FSM_ACTION(IDLE_STATE, ON_STOP); + DECLARE_FSM_ACTION(IDLE_STATE, ON_ERROR); + DECLARE_FSM_ACTION(IDLE_STATE, ON_BULK_MESSAGE); + DECLARE_FSM_ACTION(IDLE_STATE, ON_LARGE_MESSAGE); + DECLARE_FSM_ACTION(IDLE_STATE, ON_CONTROL_MESSAGE); + DECLARE_FSM_ACTION(IDLE_STATE, ON_SET_ENABLED_STREAMS); + DECLARE_FSM_ACTION(IDLE_STATE, ON_ASYNC_START); + DECLARE_FSM_ACTION(IDLE_STATE, ON_ASYNC_STOP); + DECLARE_FSM_GUARD(IDLE_STATE, ON_START); + DECLARE_FSM_GUARD(IDLE_STATE, ON_ASYNC_START); + + + // [State] ASYNC + DECLARE_FSM_STATE(ASYNC_STATE); + DECLARE_FSM_STATE_ENTRY(ASYNC_STATE); + DECLARE_FSM_STATE_EXIT(ASYNC_STATE); + DECLARE_FSM_ACTION(ASYNC_STATE, ON_INIT); + DECLARE_FSM_ACTION(ASYNC_STATE, ON_DONE); + DECLARE_FSM_ACTION(ASYNC_STATE, ON_START); + DECLARE_FSM_ACTION(ASYNC_STATE, ON_STOP); + DECLARE_FSM_ACTION(ASYNC_STATE, ON_DETACH); + DECLARE_FSM_ACTION(ASYNC_STATE, ON_ERROR); + DECLARE_FSM_ACTION(ASYNC_STATE, ON_BULK_MESSAGE); + DECLARE_FSM_ACTION(ASYNC_STATE, ON_LARGE_MESSAGE); + DECLARE_FSM_ACTION(ASYNC_STATE, ON_CONTROL_MESSAGE); + DECLARE_FSM_ACTION(ASYNC_STATE, ON_PLAYBACK_SET); + DECLARE_FSM_ACTION(ASYNC_STATE, ON_SET_ENABLED_STREAMS); + DECLARE_FSM_ACTION(ASYNC_STATE, ON_ASYNC_START); + DECLARE_FSM_ACTION(ASYNC_STATE, ON_ASYNC_STOP); + + + // [State] ACTIVE + DECLARE_FSM_STATE(ACTIVE_STATE); + DECLARE_FSM_STATE_ENTRY(ACTIVE_STATE); + DECLARE_FSM_STATE_EXIT(ACTIVE_STATE); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_DETACH); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_INIT); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_DONE); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_START); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_PLAYBACK_SET); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_ASYNC_START); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_ASYNC_STOP); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_ERROR); + DECLARE_FSM_GUARD(ACTIVE_STATE, ON_STOP); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_STOP); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_BULK_MESSAGE); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_LARGE_MESSAGE); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_CONTROL_MESSAGE); + + + // [State] ERROR + DECLARE_FSM_STATE(ERROR_STATE); + DECLARE_FSM_STATE_ENTRY(ERROR_STATE); + DECLARE_FSM_ACTION(ERROR_STATE, ON_STOP); + DECLARE_FSM_ACTION(ERROR_STATE, ON_BULK_MESSAGE); + DECLARE_FSM_ACTION(ERROR_STATE, ON_LARGE_MESSAGE); + DECLARE_FSM_ACTION(ERROR_STATE, ON_CONTROL_MESSAGE); + //DECLARE_FSM_ACTION(ERROR_STATE, ON_DONE); + + class CentralListener : public TrackingDevice::Listener + { + virtual void onFWUpdateEvent(OUT TrackingData::ControllerFWEventFrame& frame) override {} + }; + + class Bulk_Message : public Message { + public: + Bulk_Message(unsigned char* request, unsigned int sizeOfRequest, unsigned char* response, unsigned int sizeOfResponse, int endpointOut, int endpointIn, int timeoutInMs = USB_TRANSFER_FAST_TIMEOUT_MS) : + Message(ON_BULK_MESSAGE), srcSize(sizeOfRequest), dstSize(sizeOfResponse), mSrc(request), mDst(response), mEndpointIn(endpointIn), mEndpointOut(endpointOut), mTimeoutInMs(timeoutInMs) + { } + unsigned char* mSrc; + unsigned char* mDst; + int srcSize; + int dstSize; + int mEndpointIn; + int mEndpointOut; + int mTimeoutInMs; + }; + + class Large_Message : public Message { + public: + Large_Message(TrackingDevice::Listener *listener, const uint16_t messageId, const uint32_t length, const uint8_t* buffer) : + Message(ON_LARGE_MESSAGE), mListener(listener), mMessageId(messageId), mLength(length), mBuffer(buffer) {} + TrackingDevice::Listener *mListener; + const uint16_t mMessageId; + const uint32_t mLength; + const uint8_t* mBuffer; + }; + + class Control_Message : public Message { + public: + Control_Message(unsigned char* request, unsigned int sizeOfRequest, unsigned char* response = NULL, unsigned int sizeOfResponse = 0, uint16_t value = 0, uint16_t index = 0, unsigned int timeoutInMs = USB_TRANSFER_MEDIUM_TIMEOUT_MS) : + Message(ON_CONTROL_MESSAGE), srcSize(sizeOfRequest), dstSize(sizeOfResponse), mSrc(request), mDst(response), mValue(value), mIndex(index), mTimeoutInMs(timeoutInMs) + { } + unsigned char* mSrc; + unsigned char* mDst; + int srcSize; + int dstSize; + int mTimeoutInMs; + uint16_t mValue; + uint16_t mIndex; + }; + + // private messages + class MessageON_START : public Message { + public: + MessageON_START(TrackingDevice::Listener *l) : Message(ON_START), + listener(l){} + TrackingDevice::Listener *listener; + }; + + class MessageON_SET_ENABLED_STREAMS : public Message { + public: + MessageON_SET_ENABLED_STREAMS(supported_raw_stream_libtm_message * message, uint16_t wNumEnabledStreams) : Message(ON_SET_ENABLED_STREAMS), + mMessage(message), mNumEnabledStreams(wNumEnabledStreams) {} + supported_raw_stream_libtm_message * mMessage; + uint16_t mNumEnabledStreams; + }; + + class MessageON_PLAYBACK : public Message { + public: + MessageON_PLAYBACK(bool on) : Message(ON_PLAYBACK_SET), + set(on) {} + bool set; + }; + + class MessageON_ASYNC_START : public Message { + public: + MessageON_ASYNC_START(TrackingDevice::Listener *listener, const uint16_t messageId, const uint32_t length, const uint8_t* buffer) : + Message(ON_ASYNC_START), mListener(listener), mMessageId(messageId), mLength(length), mBuffer(buffer) {} + TrackingDevice::Listener *mListener; + const uint16_t mMessageId; + const uint32_t mLength; + const uint8_t* mBuffer; + }; + + class MessageON_ASYNC_STOP : public Message { + public: + MessageON_ASYNC_STOP() : Message(ON_ASYNC_STOP) {} + }; + + private: + CentralListener mCentralListener; + TrackingDevice::Listener* mListener; + libusb_device* mLibusbDevice; + libusb_device_handle* mDevice; + + Dispatcher* mDispatcher; + EventHandler* mOwner; + CompleteQueueHandler* mTaskHandler; + std::condition_variable mAsyncCV; + void interruptEndpointThread(); + void streamEndpointThread(); + std::thread mInterruptEPThread; + std::thread mBulkEPThread; + std::atomic mStreamEndpointThreadStop; + std::atomic mInterruptEndpointThreadStop; + std::atomic mStreamEndpointThreadActive; + std::atomic mInterruptEndpointThreadActive; + + std::mutex streamThreadMutex; + std::mutex eventThreadMutex; + + class PrevFrameData { + public: + PrevFrameData() : prevFrameTimeStamp(0), prevFrameId(0) {}; + PrevFrameData(int64_t _prevFrameTimeStamp, uint32_t _prevFrameId) : prevFrameTimeStamp(_prevFrameTimeStamp), prevFrameId(_prevFrameId) {}; + void Reset() + { + prevFrameTimeStamp = 0; + prevFrameId = 0; + }; + + int64_t prevFrameTimeStamp; + uint32_t prevFrameId; + }; + + void ResetStatistics() + { + for (uint8_t i = 0; i < SixDofProfileMax; i++) + { + sixdofPrevFrame[i].Reset(); + } + + for (uint8_t i = 0; i < VideoProfileMax; i++) + { + videoPrevFrame[i].Reset(); + } + + for (uint8_t i = 0; i < GyroProfileMax; i++) + { + gyroPrevFrame[i].Reset(); + } + + for (uint8_t i = 0; i < AccelerometerProfileMax; i++) + { + accelerometerPrevFrame[i].Reset(); + } + } + + PrevFrameData sixdofPrevFrame[SixDofProfileMax]; + PrevFrameData videoPrevFrame[VideoProfileMax]; + PrevFrameData gyroPrevFrame[GyroProfileMax]; + PrevFrameData velocimeterPrevFrame[VelocimeterProfileMax]; + PrevFrameData accelerometerPrevFrame[AccelerometerProfileMax]; + + int mEndpointInterrupt; + int mEndpointBulkMessages; + int mStreamEndpoint; + int mEepromChunkSize; + + int FindInterruptEndpoint(); + Status SetPlayback(bool on); + Status Set6DofInterruptRate(sixdof_interrupt_rate_libtm_message message); + + void printSupportedRawStreams(supported_raw_stream_libtm_message* pRawStreams, uint32_t rawStreamsCount); + Status SetEnabledRawStreams(supported_raw_stream_libtm_message * message, uint16_t wNumEnabledStreams); + Status GetSupportedRawStreamsInternal(supported_raw_stream_libtm_message * message, uint16_t wBufferSize, uint16_t& wNumSupportedStreams); + Status SetTimeoutConfiguration(uint16_t timeoutMsec); + void onBulkMessage(const Message& msg); + void onControlMessage(const Message& msg); + void SendLargeMessage(const Message& msg); + Status CentralFWUpdate(); + Status CentralLoadFW(uint8_t* buffer); + std::mutex mDeletionMutex; + bool mCleared; + unsigned int mFrameTempBufferSize; + + device_info_libtm_message mDeviceInfo; + + Status WriteEepromChunk(uint16_t offset, uint16_t size, uint8_t* buffer, uint16_t& actual, bool verify = false); + Status ReadEepromChunk(uint16_t offset, uint16_t size, uint8_t * buffer, uint16_t& actual); + Status GetUsbConnectionDescriptor(); + Status GetDeviceInfoInternal(); + Status DeviceFlush(); + Status SetDeviceStreamConfig(uint32_t maxSize); + Status GetInterfaceVersionInternal(); + interface_version_libtm_message mFWInterfaceVersion; + TrackingData::DeviceInfo::UsbConnectionDescriptor mUsbDescriptor; + Status SyncTime(); + void AllocateBuffers(); + Status Set6DoFControl(TrackingData::SixDofProfile& profile); + Status SetController6DoFControl(bool enabled, uint8_t numOfControllers); + ProfileType getProfileType(uint8_t sensorID); + + long long mTM2CorrelatedTimeStampShift; + nsecs_t mSyncTimeout; + + void StartThreads(bool interruptThread, bool frameThread); + void StopThreads(bool force, bool interruptThread, bool frameThread); + + std::recursive_mutex mFramesBuffersMutex; /* This mutex can be acquired several times by the same thread */ + std::list> mFramesBuffersLists; + std::map mWidthsMap; + std::map mHeightsMap; + + bool mPlaybackIsOn; + bool mSyncTimeEnabled; + bool mHasBluetooth; + DEVICE_USB_STATE mUsbState; + Status mDeviceStatus; + + std::vector mGyroProfiles; + std::vector mVelocimeterProfiles; + std::vector mAccelerometerProfiles; + std::vector mVideoProfiles; + }; +} diff --git a/third-party/libtm/libtm/src/Loader.cpp b/third-party/libtm/libtm/src/Loader.cpp new file mode 100644 index 0000000000..5b0b9b480f --- /dev/null +++ b/third-party/libtm/libtm/src/Loader.cpp @@ -0,0 +1,134 @@ +/******************************************************************************* +INTEL CORPORATION PROPRIETARY INFORMATION +Copyright(c) 2017 Intel Corporation. All Rights Reserved. +*******************************************************************************/ + +#include +#include +#include "Loader.h" + +#include +#include +#include +#include + + +using namespace std; +using namespace tracking; + + + +static inline void highres_gettime(highres_time_t *ptr) { + clock_gettime(CLOCK_REALTIME, ptr); +} + +static inline double highres_elapsed_ms(highres_time_t *start, highres_time_t *end) { + struct timespec temp; + if((end->tv_nsec - start->tv_nsec) < 0) { + temp.tv_sec = end->tv_sec - start->tv_sec - 1; + temp.tv_nsec = 1000000000 + end->tv_nsec-start->tv_nsec; + } else { + temp.tv_sec = end->tv_sec - start->tv_sec; + temp.tv_nsec = end->tv_nsec - start->tv_nsec; + } + return (double)(temp.tv_sec * 1000) + (((double)temp.tv_nsec) * 0.000001); +} + + +Status tracking::loader::send_file(const char *fname) { + FILE *file; + uint8_t *tx_buf, *p; + int res; + int wb, twb, wbr; + unsigned long filesize; + double elapsedTime; + highres_time_t t1, t2; + + file = fopen(fname, "rb"); + if(file == NULL) { + perror(fname); + return FILE_IO_ERROR; + } + + fseek(file, 0, SEEK_END); + filesize = ftell(file); + rewind(file); + + if(!(tx_buf = (uint8_t*)malloc(filesize))) { + perror("buffer"); + fclose(file); + return INTERNAL_ERROR; + } + + if(fread(tx_buf, 1, filesize, file) != filesize) { + perror(fname); + fclose(file); + free(tx_buf); + return FILE_IO_ERROR; + } + fclose(file); + + elapsedTime = 0; + twb = 0; + p = tx_buf; + + printf("Performing bulk write of %lu byte%s from %s...\n", filesize, filesize == 1 ? "" : "s", fname); + + while(twb < filesize) { + highres_gettime(&t1); + wb = filesize - twb; + if(wb > DEFAULT_CHUNKSZ) + wb = DEFAULT_CHUNKSZ; + wbr = 0; + res = libusb_bulk_transfer(m_dev_handle, EP_OUT, p, wb, &wbr, TIMEOUT_MS); + if((res == USB_ERR_TIMEOUT) && (twb != 0)) + break; + highres_gettime(&t2); + if((res < 0) || (wb != wbr)) { + fprintf(stdout, "bulk write: %d\n", res); + free(tx_buf); + return USB_OPERATION_FAILED; + } + elapsedTime += highres_elapsed_ms(&t1, &t2); + twb += wbr; + p += wbr; + } + + double MBpS = + ((double)filesize / 1048576.) / + (elapsedTime * 0.001); + + printf("Successfully sent %ld bytes of data in %lf ms (%lf MB/s)\n", filesize, elapsedTime, MBpS); + + free(tx_buf); + + return SUCCESS; +} + +Status tracking::loader::load_image(char *filename) +{ + libusb_context *context = NULL; + + Status ret_val = SUCCESS; + + int st = libusb_init(&context); + + if (st!=0) + { + printf("Error while initializing libusb\n"); + return USB_INIT_ERROR; + } + + m_dev_handle = libusb_open_device_with_vid_pid(context, m_vid, m_pid); + + if (m_dev_handle == NULL) { + return INTERNAL_ERROR; + } + + ret_val = send_file(filename); + + libusb_close(m_dev_handle); + + return ret_val; +} + diff --git a/third-party/libtm/libtm/src/Loader.h b/third-party/libtm/libtm/src/Loader.h new file mode 100644 index 0000000000..1e04bdb468 --- /dev/null +++ b/third-party/libtm/libtm/src/Loader.h @@ -0,0 +1,52 @@ +/******************************************************************************* +INTEL CORPORATION PROPRIETARY INFORMATION +Copyright(c) 2017 Intel Corporation. All Rights Reserved. +*******************************************************************************/ + +#include +#include +#include +#include "Common.h" +#include "libtm_message.h" + + +#define DEV_PID 0x2150 +#define DEV_VID 0x03e7 + +#define EP_OUT 1 + +#define TIMEOUT_MS 60000 +#define DEFAULT_CHUNKSZ 1024 + +#define DEV_INTERFACE 0 + +#define USB_ERR_NONE 0 +#define USB_ERR_TIMEOUT -1 +#define USB_ERR_FAILED -2 +#define USB_ERR_INVALID -3 + +namespace tracking +{ + typedef struct timespec highres_time_t; + + class loader + { + public: + loader() : m_dev_handle(0), m_pid(DEV_PID), m_vid(DEV_VID) + { + } + + void set_pid(int pid) { m_pid = pid; } + void set_vid(int vid) { m_vid = vid; } + + Status load_image(char * filename); + + private: + + Status send_file(const char *fname); + + libusb_device_handle *m_dev_handle; + + int m_pid, m_vid; + }; +} diff --git a/third-party/libtm/libtm/src/Main.cpp b/third-party/libtm/libtm/src/Main.cpp new file mode 100644 index 0000000000..7eafaa794b --- /dev/null +++ b/third-party/libtm/libtm/src/Main.cpp @@ -0,0 +1,655 @@ +/******************************************************************************* +INTEL CORPORATION PROPRIETARY INFORMATION +Copyright(c) 2017 Intel Corporation. All Rights Reserved. +*******************************************************************************/ + +#include +#include +#include +#include "Protocol.h" +#include "Message.h" + +#define PRODUCT_ID 0xF63B +#define VENDOR_ID 0x040E +#define MAX_MESSAGE_SIZE 1024 + +void send_bulk_message_get_device_info(libusb_device_handle * device_handle) +{ + bulk_message_request_get_device_info request; + bulk_message_response_get_device_info response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 6, DEV_GET_DEVICE_INFO); + + /* Print message request */ + printf("DEV_GET_DEVICE_INFO request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("DEV_GET_DEVICE_INFO response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_get_time(libusb_device_handle * device_handle) +{ + bulk_message_request_get_time request; + bulk_message_response_get_time response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 6, DEV_GET_TIME); + + /* Print message request */ + printf("DEV_GET_TIME request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("DEV_GET_TIME response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_get_supported_raw_streams(libusb_device_handle * device_handle) +{ + bulk_message_request_get_supported_raw_streams request; + uint8_t response[MAX_MESSAGE_SIZE]; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 6, DEV_GET_SUPPORTED_RAW_STREAMS); + + /* Print message request */ + printf("DEV_GET_SUPPORTED_RAW_STREAMS request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("DEV_GET_SUPPORTED_RAW_STREAMS response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_raw_streams_control(libusb_device_handle * device_handle) +{ + bulk_message_request_raw_streams_control* request = NULL; + bulk_message_response_raw_streams_control response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + int request_length = sizeof(bulk_message_request_raw_streams_control) + sizeof(supported_raw_stream_libtm_message); + + /* request message will include one stream */ + request = (bulk_message_request_raw_streams_control*)malloc(request_length); + + /* Prepare get device info message */ + memset(request, 0, request_length); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)request, request_length, DEV_RAW_STREAMS_CONTROL); + request->wNumEnabledStreams = 1; + request->stream[0].bSensorID = SET_SENSOR_ID(SENSOR_TYPE_COLOR, 0); + request->stream[0].wFramesPerSecond = 30; + request->stream[0].wWidth = 1920; + request->stream[0].wHeight = 1080; + request->stream[0].bReserved = 0; + + /* Print message request */ + printf("DEV_RAW_STREAMS_CONTROL request\n"); + print_message((unsigned char *)request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)request, request_length, &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("DEV_RAW_STREAMS_CONTROL response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_get_camera_intrinsics(libusb_device_handle * device_handle) +{ + bulk_message_request_get_camera_intrinsics request; + bulk_message_response_get_camera_intrinsics response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 7, DEV_GET_CAMERA_INTRINSICS); + request.bCameraID = SET_SENSOR_ID(SENSOR_TYPE_COLOR, 0); + + /* Print message request */ + printf("DEV_GET_CAMERA_INTRINSICS request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("DEV_GET_CAMERA_INTRINSICS response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_get_motion_intrinsics(libusb_device_handle * device_handle) +{ + bulk_message_request_get_motion_intrinsics request; + bulk_message_response_get_motion_intrinsics response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 7, DEV_GET_MOTION_INTRINSICS); + request.bMotionID = SET_SENSOR_ID(SENSOR_TYPE_GYRO, 0); + + /* Print message request */ + printf("DEV_GET_MOTION_INTRINSICS request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("DEV_GET_MOTION_INTRINSICS response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_get_extrinsics(libusb_device_handle * device_handle) +{ + bulk_message_request_get_extrinsics request; + bulk_message_response_get_extrinsics response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 8, DEV_GET_EXTRINSICS); + request.bSensorID = SET_SENSOR_ID(SENSOR_TYPE_COLOR, 0); + request.bReferenceSensorID = SET_SENSOR_ID(SENSOR_TYPE_DEPTH, 1); + + /* Print message request */ + printf("DEV_GET_EXTRINSICS request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("DEV_GET_EXTRINSICS response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_set_camera_intrinsics(libusb_device_handle * device_handle) +{ + bulk_message_request_set_camera_intrinsics request; + bulk_message_response_set_camera_intrinsics response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + int i = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 56, DEV_SET_CAMERA_INTRINSICS); + request.bCameraID = SET_SENSOR_ID(SENSOR_TYPE_COLOR, 0); + request.intrinsics.dwWidth = 1; + request.intrinsics.dwHeight = 2; + request.intrinsics.flPpx = 3; + request.intrinsics.flPpy = 4; + request.intrinsics.flFx = 5; + request.intrinsics.flFy = 6; + request.intrinsics.dwDistortionModel = 7; + for (i = 0; i < 5; i++) + { + request.intrinsics.flCoeffs[i] = i+8; + } + + /* Print message request */ + printf("DEV_SET_CAMERA_INTRINSICS request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("DEV_SET_CAMERA_INTRINSICS response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_set_motion_intrinsics(libusb_device_handle * device_handle) +{ + bulk_message_request_set_motion_intrinsics request; + bulk_message_response_set_motion_intrinsics response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + int i = 0; + int j = 0; + int k = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 80, DEV_SET_MOTION_INTRINSICS); + request.bMotionID = SET_SENSOR_ID(SENSOR_TYPE_GYRO, 0); + + for (i = 0; i < 3; i++, k++) + { + for (j = 0; j < 4; j++, k++) + { + request.intrinsics.flData[i][j] = k; + } + } + + for (i = 0; i < 3; i++, k++) + { + request.intrinsics.flNoiseVariances[i] = k; + } + + for (i = 0; i < 3; i++, k++) + { + request.intrinsics.flBiasVariances[i] = k; + } + + /* Print message request */ + printf("DEV_SET_MOTION_INTRINSICS request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("DEV_SET_MOTION_INTRINSICS response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_set_extrinsics(libusb_device_handle * device_handle) +{ + bulk_message_request_set_extrinsics request; + bulk_message_response_set_extrinsics response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + int i = 0; + int j = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 56, DEV_SET_EXTRINSICS); + request.bSensorID = SET_SENSOR_ID(SENSOR_TYPE_COLOR, 0); + request.bReferenceSensorID = SET_SENSOR_ID(SENSOR_TYPE_DEPTH, 1); + + for (i = 0; i < 9; i++, j++) + { + request.extrinsics.flRotation[i] = j; + } + + for (i = 0; i < 3; i++, j++) + { + request.extrinsics.flTranslation[i] = j; + } + + /* Print message request */ + printf("DEV_SET_EXTRINSICS request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("DEV_SET_EXTRINSICS response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_log_control(libusb_device_handle * device_handle) +{ + bulk_message_request_log_control request; + bulk_message_response_log_control response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 56, DEV_LOG_CONTROL); + request.bVerbosity = 0; + request.bLogMode = 0; + + /* Print message request */ + printf("DEV_LOG_CONTROL request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("DEV_LOG_CONTROL response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_get_pose(libusb_device_handle * device_handle) +{ + bulk_message_request_get_pose request; + bulk_message_response_get_pose response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 6, DEV_GET_POSE); + + /* Print message request */ + printf("DEV_GET_POSE request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("DEV_GET_POSE response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_get_occupancy_map_tiles(libusb_device_handle * device_handle) +{ + bulk_message_request_get_occupancy_map_tiles request; + bulk_message_response_get_occupancy_map_tiles response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 6, SLAM_GET_OCCUPANCY_MAP_TILES); + + /* Print message request */ + printf("SLAM_GET_OCCUPANCY_MAP_TILES request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("SLAM_GET_OCCUPANCY_MAP_TILES response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_get_localization_data(libusb_device_handle * device_handle) +{ + bulk_message_request_get_localization_data request; + bulk_message_response_get_localization_data response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 6, SLAM_GET_LOCALIZATION_DATA); + + /* Print message request */ + printf("SLAM_GET_LOCALIZATION_DATA request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("SLAM_GET_LOCALIZATION_DATA response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_set_localization_data(libusb_device_handle * device_handle) +{ + bulk_message_request_set_localization_data request; + bulk_message_response_set_localization_data response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 7, SLAM_SET_LOCALIZATION_DATA); + + /* Print message request */ + printf("SLAM_SET_LOCALIZATION_DATA request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("SLAM_SET_LOCALIZATION_DATA response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_set_6dof_interrupt_rate(libusb_device_handle * device_handle) +{ + bulk_message_request_set_6dof_interrupt_rate request; + bulk_message_response_set_6dof_interrupt_rate response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 7, SLAM_SET_6DOF_INTERRUPT_RATE); + request.message.bInterruptRate = 1; + + /* Print message request */ + printf("SLAM_SET_6DOF_INTERRUPT_RATE request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("SLAM_SET_6DOF_INTERRUPT_RATE response\n"); + print_message((unsigned char *)&response); +} + +void send_bulk_message_6dof_control_enable(libusb_device_handle * device_handle) +{ + bulk_message_request_6dof_control request; + bulk_message_response_6dof_control response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 7, SLAM_6DOF_CONTROL); + request.bEnable = 1; + + /* Print message request */ + printf("SLAM_6DOF_CONTROL request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("SLAM_6DOF_CONTROL response\n"); + print_message((unsigned char *)&response); +} + +int receive_interrupt_message_6dof_pose_data(libusb_device_handle * device_handle) +{ + interrupt_message_get_pose response; + int host_data_in_bytes_received = 0; + int result = 0; + + /* Prepare get device info message */ + memset(&response, 0, sizeof(response)); + + /* receive interrupt 6dof pose data */ + result = interrupt_transfer_read_message(device_handle, (unsigned char *)&response, &host_data_in_bytes_received); + if (result == 0) + { + /* Print message response */ + printf("Interrupt 6dof pose data message\n"); + print_message((unsigned char *)&response); + } + + return result; +} + +void send_bulk_message_6dof_control_disable(libusb_device_handle * device_handle) +{ + bulk_message_request_6dof_control request; + bulk_message_response_6dof_control response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + int i = 0; + int result = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 7, SLAM_6DOF_CONTROL); + request.bEnable = 0; + + /* Print message request */ + printf("SLAM_6DOF_CONTROL request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + result = bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("SLAM_6DOF_CONTROL response\n"); + print_message((unsigned char *)&response); + + if (result == 0) + { + printf("Waiting for interrupt endpoint to be cleaned\n"); + while (result == 0) + { + result = receive_interrupt_message_6dof_pose_data(device_handle); + printf("Received interrupt 6dof pose data message %d\n",i); + i++; + } + printf("interrupt endpoint is now cleaned\n"); + + } + else + { + printf("Error disabling 6DOF\n"); + } +} + +void send_bulk_message_enable_occupancy_map(libusb_device_handle * device_handle) +{ + bulk_message_request_occupancy_map_control request; + bulk_message_response_occupancy_map_control response; + int host_data_out_bytes_sent = 0; + int host_data_in_bytes_received = 0; + + /* Prepare get device info message */ + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + init_message_request_header((unsigned char *)&request, 7, SLAM_OCCUPANCY_MAP_CONTROL); + request.message.bEnable = 1; + + /* Print message request */ + printf("SLAM_OCCUPANCY_MAP_CONTROL request\n"); + print_message((unsigned char *)&request); + + /* Send and receive device info */ + bulk_transfer_exchange_message(device_handle, (unsigned char *)&request, sizeof(request), &host_data_out_bytes_sent, (unsigned char *)&response, &host_data_in_bytes_received); + + /* Print message response */ + printf("SLAM_OCCUPANCY_MAP_CONTROL response\n"); + print_message((unsigned char *)&response); +} + + +int main(void) +{ + struct libusb_device_handle * device_handle = NULL; + int result = 0; + int i = 0; + int max_interrupt_message = 100; + + printf("TM2 Demo (version %d)\n", TM2_API_VERSION); + + /* Initialise libusb, must be called before other libusb routines are used */ + result = libusb_init(NULL); + if (result < 0) + { + fprintf(stderr, "Error initializing libusb: %s\n", libusb_error_name(result)); + exit(1); + } + + /* Finding a device with a particular idVendor/idProduct combination */ + device_handle = libusb_open_device_with_vid_pid(NULL, VENDOR_ID, PRODUCT_ID); + if (device_handle == NULL) + { + fprintf(stderr, "Error finding USB device\n"); + goto out; + } + + /* Claim an interface on a given device handle - must claim the interface before performing I/O on any of its endpoints */ + result = libusb_claim_interface(device_handle, INTERFACE_INDEX); + if (result < 0) + { + fprintf(stderr, "Error claiming interface: %s\n", libusb_error_name(result)); + goto out; + } + + /* Sending all supported messages */ + send_bulk_message_get_device_info(device_handle); + send_bulk_message_get_time(device_handle); + send_bulk_message_get_supported_raw_streams(device_handle); + send_bulk_message_raw_streams_control(device_handle); + send_bulk_message_set_camera_intrinsics(device_handle); + send_bulk_message_get_camera_intrinsics(device_handle); + send_bulk_message_set_motion_intrinsics(device_handle); + send_bulk_message_get_motion_intrinsics(device_handle); + send_bulk_message_set_extrinsics(device_handle); + send_bulk_message_get_extrinsics(device_handle); + send_bulk_message_log_control(device_handle); + send_bulk_message_get_pose(device_handle); + send_bulk_message_get_occupancy_map_tiles(device_handle); + send_bulk_message_get_localization_data(device_handle); + send_bulk_message_set_localization_data(device_handle); + send_bulk_message_set_6dof_interrupt_rate(device_handle); + send_bulk_message_6dof_control_enable(device_handle); + for (i = 0; i +#include +#include "Manager.h" +#include +#include "Version.h" +#include +#include "fw.h" +#include "Common.h" + +using namespace std::chrono; +using namespace std; + +#define DEFAULT_CHUNKSZ 1024*32 +#define EP_OUT 1 +#define TIMEOUT_MS 3000 + +namespace perc { +// -[factory]------------------------------------------------------------------ + +TrackingManager* TrackingManager::CreateInstance(Listener* lis, void* param) +{ + try + { + return new Manager(lis, param); + } + catch (std::exception& e) + { + LOGE(e.what()); + return nullptr; + } +} + +void TrackingManager::ReleaseInstance(TrackingManager*& manager) +{ + try + { + if (manager) + { + delete manager; + manager = nullptr; + } + } + catch (std::exception& e) + { + LOGE(e.what()); + } +} + +// -[interface]---------------------------------------------------------------- +Manager::Manager(Listener* lis, void* param) : mDispatcher(new Dispatcher()), mListener(nullptr), mContext(nullptr), mFwFileName(""), mLibUsbDeviceToTrackingDeviceMap(), mEvent(), mCompleteQMutex(), mCompleteQ(), mTrackingDeviceInfoMap() +{ + // start running context + mThread = std::thread([this] { + mFsm.init(FSM(main), this, mDispatcher.get(), LOG_TAG); + while (this->mDispatcher->handleEvents() >= 0); + mFsm.done(); + }); + if (init(lis, param) != Status::SUCCESS) + { + throw std::runtime_error("Failed to init manager"); + } + + + TrackingData::LogControl logControl(LogVerbosityLevel::None, + LogOutputMode::LogOutputModeBuffer, + true); + + setHostLogControl(logControl); +} + + +Manager::~Manager() +{ + done(); +} + +Status Manager::onAttach(libusb_device * device) +{ + MessageON_LIBUSB_EVENT msg(true, device); + mFsm.fireEvent(msg); + if (msg.Result == 0) + return Status::SUCCESS; + return Status::INIT_FAILED; +} + +void Manager::onDetach(libusb_device * device) +{ + mFsm.fireEvent(MessageON_LIBUSB_EVENT(false, device)); +} + +void Manager::addTask(std::shared_ptr& newTask) +{ + //mDispatcher->postMessage(&mFsm, MessageON_NEW_TASK(newTask)); + lock_guard lock(mCompleteQMutex); + mCompleteQ.push_back(newTask); + mEvent.signal(); +} + +Status Manager::init(Listener *listener, void *param) +{ + return processMessage(MessageON_INIT(listener, param)); +} + +void Manager::removeTasks(void * owner, bool completeTasks) +{ + mFsm.fireEvent(MessageON_REMOVE_TASKS(owner, completeTasks)); +} + +// ---------------------------------------------------------------------------- +Status Manager::handleEvents(bool blocking) +{ + shared_ptr task = nullptr; + size_t count; + + { + lock_guard lock(mCompleteQMutex); + count = mCompleteQ.size(); + mEvent.reset(); + } + + LOGV("Manager::handleEvents is about to handle %d tasks", count); + while (true) + { + task = nullptr; + { + lock_guard lock(mCompleteQMutex); + if ( count > 0 && mCompleteQ.size() > 0 ) + { + task = mCompleteQ.front(); + mCompleteQ.pop_front(); + count--; + } + } + if (task) + task->complete(); + else + break; + } + return Status::SUCCESS; +} + + +// ---------------------------------------------------------------------------- +void Manager::done() +{ + processMessage(Message(ON_DONE)); + + if (mThread.joinable()) { + try { + mDispatcher->endEventsLoop(); + mThread.join(); + } + catch (...) { + LOGE("mThread.join() exception"); + } + } + + for (auto item : mLibUsbDeviceToTrackingDeviceMap) + { + delete item.second; + libusb_unref_device(item.first); + } +} + +size_t Manager::getDeviceList(TrackingDevice ** list, unsigned int maxListSize) +{ + // TODO: move to sendmessage and dispatcher context + auto count = maxListSize > mLibUsbDeviceToTrackingDeviceMap.size() ? mLibUsbDeviceToTrackingDeviceMap.size() : maxListSize; + + size_t i = 0; + for (auto item : mLibUsbDeviceToTrackingDeviceMap) + { + *list++ = item.second; + i--; + if (i == count) + break; + } + return count; +} + +Status Manager::setHostLogControl(IN const TrackingData::LogControl& logControl) +{ + LOGF("Set Host log Control, output to %s, verbosity = 0x%X, Rollover mode = 0x%X", (logControl.outputMode == LogOutputModeScreen)?"Screen":"Buffer", logControl.verbosity, logControl.rolloverMode); + __perc_Log_Set_Configuration(LogSourceHost, logControl.outputMode, logControl.verbosity, logControl.rolloverMode); + return Status::SUCCESS; +} + +Status Manager::getHostLog(OUT TrackingData::Log* log) +{ + __perc_Log_Get_Log((void*)log); + return Status::SUCCESS; +} + +// -[impl]--------------------------------------------------------------------- +Status Manager::processMessage(const Message &msg, int prio) +{ + msg.Result = toUnderlying(Status::COMMON_ERROR); + + int ret = mDispatcher->sendMessage(&mFsm, msg, prio); + if (ret < 0) { + LOGE("mDispatcher->sendMessage ret %d", ret); + return Status::COMMON_ERROR; + } + return (Status)msg.Result; +} + + + +Status Manager::loadFileToDevice(libusb_device * device, const char * fileName) +{ + Status status = Status::SUCCESS; + + if (!fileName || !device) + { + LOGE("Error while loading file %s to device 0x%p - bad input", fileName, device); + return Status::INIT_FAILED; + } + + FILE* file; + auto res = fopen_s(&file, fileName, "rb"); + if (res != 0) + { + LOGE("Error while loading file %s to device 0x%p - can't open file", fileName, device); + return Status::INIT_FAILED; + } + + unsigned int size; + + fseek(file, 0, SEEK_END); + size = ftell(file); + rewind(file); + + unsigned char* txBuf; + + if (!(txBuf = (unsigned char*)malloc(size))) + { + LOGE("Error while loading file %s (size %d) to device 0x%p - memory allocation error", fileName, size, device); + status = Status::INIT_FAILED; + goto cleanup; + } + + if (fread(txBuf, 1, size, file) != size) + { + LOGE("Error while loading file %s (size %d) to device 0x%p - read file error", fileName, size, device); + status = Status::INIT_FAILED; + goto cleanup; + } + + status = loadBufferToDevice(device, txBuf, size); + +cleanup: + fclose(file); + if (txBuf) + { + free(txBuf); + } + return status; +} + + +Status Manager::loadBufferToDevice(libusb_device * device, unsigned char * buffer, size_t size) +{ + LOGV("loading image to device 0x%p", device); + + if (!device || !buffer || size == 0) + { + LOGE("Error while loading image - device 0x%p, buffer 0x%p, size %d", device, buffer, size); + return Status::INIT_FAILED; + } + + libusb_device_handle * devHandle; + auto start = high_resolution_clock::now(); + uint64_t duration = 0; + int bytesWritten = 0; + + auto rc = libusb_open(device, &devHandle); + if (rc != 0) + { + LOGE("Error while loading image - libusb_open failed (0x%X), will try again later", rc); + return Status::INIT_FAILED; + } + + rc = libusb_claim_interface(devHandle, INTERFACE_INDEX); + if (rc != 0) + { + LOGE("Error while loading image - libusb_claim_interface failed (0x%X)", rc); + libusb_close(devHandle); + return Status::INIT_FAILED; + } + + rc = libusb_bulk_transfer(devHandle, EP_OUT, buffer, (int)size, &bytesWritten, TIMEOUT_MS); + if (rc != 0 || size != bytesWritten) + { + LOGE("Error while loading image - libusb_bulk_transfer failed. LIBUSB_ERROR_CODE: %d (%s)", rc, libusb_error_name(rc)); + libusb_release_interface(devHandle, INTERFACE_INDEX); + libusb_close(devHandle); + return Status::INIT_FAILED; + } + + //W\A for pipe error bug + rc = libusb_bulk_transfer(devHandle, EP_OUT, buffer, 0, &bytesWritten, TIMEOUT_MS); + // status check ignored according to Movidius sample code + //if (rc != 0 || 0 != bytesWritten) + //{ + // LOGE("Error while loading image - libusb_bulk_transfer failed. LIBUSB_ERROR_CODE: %d (%s)", rc, libusb_error_name(rc)); + // libusb_release_interface(devHandle, INTERFACE_INDEX); + // libusb_close(devHandle); + // return Status::INIT_FAILED; + //} + + duration = duration_cast(high_resolution_clock::now() - start).count(); + LOGD("USB Device FW Load finish - FW image loaded in %d mSec", duration); + + libusb_release_interface(devHandle, INTERFACE_INDEX); + libusb_close(devHandle); + return Status::SUCCESS; +} + +uint64_t Manager::version() +{ + return LIBTM_VERSION; +} +// -[FSM]---------------------------------------------------------------------- +// +// [FSM State::ANY] + +// [FSM State::IDLE] +DEFINE_FSM_STATE_ENTRY(Manager, IDLE_STATE) +{ +} + +DEFINE_FSM_GUARD(Manager, IDLE_STATE, ON_INIT, msg) +{ + return 0 == libusb_init(&mContext); +} + +DEFINE_FSM_ACTION(Manager, IDLE_STATE, ON_INIT, msg) +{ + auto initMsg = dynamic_cast(msg); + mListener = initMsg.listener; + char* t = (char*)initMsg.context; + if (t) + { + string tmp(t); + mFwFileName = tmp; + } + msg.Result = toUnderlying(Status::SUCCESS); +} + +// [FSM State::ACTIVE] +DEFINE_FSM_STATE_ENTRY(Manager, ACTIVE_STATE) +{ + mUsbPlugListener = make_shared(*this); +} + +DEFINE_FSM_STATE_EXIT(Manager, ACTIVE_STATE) +{ +} + +DEFINE_FSM_GUARD(Manager, ACTIVE_STATE, ON_REMOVE_TASKS, msg) +{ + auto m = dynamic_cast(msg); + if (!m.mOwner) + return false; + return true; +} + +DEFINE_FSM_ACTION(Manager, ACTIVE_STATE, ON_REMOVE_TASKS, msg) +{ + lock_guard lock(mCompleteQMutex); + uint32_t nonCompleteTasks = 0; + + auto it = mCompleteQ.begin(); + auto m = dynamic_cast(msg); + + while (it != mCompleteQ.end()) + { + if ((*it)->getOwner() == m.mOwner) + { + auto itTmp = it; + it++; + + /* Check if asked to complete all pending tasks (upon exiting from ASYNC state) or this is a mustComplete tasks (status / error / localization / fw update tasks) */ + if ((m.mCompleteTasks == true) || (itTmp->get()->mustComplete() == true)) + { + itTmp->get()->complete(); + } + else + { + nonCompleteTasks++; + } + + mCompleteQ.erase(itTmp); + continue; + } + it++; + } + + if (nonCompleteTasks > 0) + { + /* If there are non completed tasks in this queue, this means that some of the callbacks (onVideo, onPose, onGyro, onAccelerometer) have a high latency */ + /* Which prevent the processor from processing all tasks in this queue while on ACTIVE state, which means loosing callbacks data */ + LOGE("Stopping Manager - Cleaned %d non complete callbacks (onVideoFrame, onPoseFrame, onGyroFrame, onAccelerometerFrame, onControllerframe) - latency is too high!", nonCompleteTasks); + } + +} + +DEFINE_FSM_GUARD(Manager, ACTIVE_STATE, ON_NEW_TASK, msg) +{ + auto m = dynamic_cast(msg); + if (!m.task) + return false; + return true; +} + +DEFINE_FSM_ACTION(Manager, ACTIVE_STATE, ON_NEW_TASK, msg) +{ +/* lock_guard lock(mCompleteQMutex); + auto m = dynamic_cast(msg); + mCompleteQ.push_back(m.task); + mEvent.signal();*/ +} + +DEFINE_FSM_ACTION(Manager, ACTIVE_STATE, ON_ATTACH, msg) +{ + Status status = Status::SUCCESS; + msg.Result = toUnderlying(Status::ERROR_USB_TRANSFER); + + auto m = dynamic_cast(msg); + libusb_device_descriptor desc = { 0 }; + + auto rc = libusb_get_device_descriptor(m.device, &desc); + if (rc) + { + LOGE("Error while getting device descriptor. LIBUSB_ERROR_CODE: %d (%s)", rc, libusb_error_name(rc)); + return; + } + + /* USB Device Firmware Upgrade (DFU) */ + if (mUsbPlugListener->identifyUDFDevice(&desc) == true) + { + libusb_ref_device(m.device); + + // load firmware here + if (mFwFileName != "") + { + LOGD("USB Device FW Load start - loading external FW image from %s", mFwFileName.c_str()); + status = loadFileToDevice(m.device, mFwFileName.c_str()); + } + else + { + LOGD("USB Device FW Load start - loading internal FW image \"%s\"", FW_VERSION); + auto size = sizeof(target_hex); + status = loadBufferToDevice(m.device, (unsigned char*)target_hex, size); + } + } + + if (status != Status::SUCCESS) + { + msg.Result = toUnderlying(Status::INIT_FAILED); + return; + } + + /* Real device arrived - do something */ + if (mUsbPlugListener->identifyDevice(&desc) == true) + { + /* To debug libusb issues, enable logs using libusb_set_debug(mContext, LIBUSB_LOG_LEVEL_INFO) */ + + auto device = new Device(m.device, mDispatcher.get(), this, this); + if (device->IsDeviceReady() == false) + { + delete device; + msg.Result = toUnderlying(Status::INIT_FAILED); + return; + } + + libusb_ref_device(m.device); + mLibUsbDeviceToTrackingDeviceMap[m.device] = device; + + shared_ptr ptr; + TrackingData::DeviceInfo deviceInfo; + device->GetDeviceInfo(deviceInfo); + mTrackingDeviceInfoMap[device] = deviceInfo; + + ptr = make_shared(mListener, device, &deviceInfo, ATTACH, this); + this->addTask(ptr); + + if (deviceInfo.status.hw != Status::SUCCESS) + { + ptr = make_shared(mListener, device, deviceInfo.status.hw, this); + this->addTask(ptr); + } + else if (deviceInfo.status.host != Status::SUCCESS) + { + ptr = make_shared(mListener, device, deviceInfo.status.host, this); + this->addTask(ptr); + } + + mDispatcher->registerHandler(device); + } + //libusb_ref_device(m.device); + msg.Result = toUnderlying(Status::SUCCESS); +} + +DEFINE_FSM_ACTION(Manager, ACTIVE_STATE, ON_DETACH, msg) +{ + msg.Result = toUnderlying(Status::ERROR_USB_TRANSFER); + libusb_device_descriptor desc = { 0 }; + auto m = dynamic_cast(msg); + auto rc = libusb_get_device_descriptor(m.device, &desc); + if (rc) + { + LOGE("Error while getting device descriptor. LIBUSB_ERROR_CODE: %d (%s)", rc, libusb_error_name(rc)); + return; + } + + if (mUsbPlugListener->identifyDevice(&desc) == true) + { + auto device = mLibUsbDeviceToTrackingDeviceMap[m.device]; + mLibUsbDeviceToTrackingDeviceMap.erase(m.device); + mDispatcher->removeHandler(device); + + auto deviceInfo = mTrackingDeviceInfoMap[device]; + mTrackingDeviceInfoMap.erase(device); + + // notify listener on device erased + shared_ptr ptr = make_shared(mListener, device, &deviceInfo, DETACH, this); + this->addTask(ptr); + delete device; + libusb_unref_device(m.device); + } + else if (mUsbPlugListener->identifyUDFDevice(&desc) == true) + { + libusb_unref_device(m.device); + } + + //libusb_unref_device(m.device); + msg.Result = toUnderlying(Status::SUCCESS); +} + +DEFINE_FSM_ACTION(Manager, ACTIVE_STATE, ON_DONE, msg) +{ + msg.Result = toUnderlying(Status::SUCCESS); +} + +DEFINE_FSM_ACTION(Manager, ACTIVE_STATE, ON_ERROR, msg) +{ + msg.Result = toUnderlying(Status::SUCCESS); +} + +// [FSM State::ERROR] +DEFINE_FSM_ACTION(Manager, ERROR_STATE, ON_DONE, msg) +{ +} + +// -[Definition]--------------------------------------------------------------- +#define FSM_ERROR_TIMEOUT 1000 + +// [FSM State::IDLE] +DEFINE_FSM_STATE_BEGIN(Manager, IDLE_STATE) + TRANSITION(ON_INIT, GUARD(Manager, IDLE_STATE, ON_INIT), ACTION(Manager, IDLE_STATE, ON_INIT), ACTIVE_STATE) +DEFINE_FSM_STATE_END(Manager, IDLE_STATE, ENTRY(Manager, IDLE_STATE), 0) + +// [FSM State::ACTIVE] +DEFINE_FSM_STATE_BEGIN(Manager, ACTIVE_STATE) + TRANSITION(ON_DONE, 0, ACTION(Manager, ACTIVE_STATE, ON_DONE), IDLE_STATE) + TRANSITION_INTERNAL(ON_ATTACH, 0, ACTION(Manager, ACTIVE_STATE, ON_ATTACH)) + TRANSITION_INTERNAL(ON_DETACH, 0, ACTION(Manager, ACTIVE_STATE, ON_DETACH)) + TRANSITION_INTERNAL(ON_NEW_TASK, GUARD(Manager, ACTIVE_STATE, ON_NEW_TASK), ACTION(Manager, ACTIVE_STATE, ON_NEW_TASK)) + TRANSITION_INTERNAL(ON_REMOVE_TASKS, GUARD(Manager, ACTIVE_STATE, ON_REMOVE_TASKS), ACTION(Manager, ACTIVE_STATE, ON_REMOVE_TASKS)) + TRANSITION(ON_ERROR, 0, ACTION(Manager, ACTIVE_STATE, ON_ERROR), ERROR_STATE) +DEFINE_FSM_STATE_END(Manager, ACTIVE_STATE, ENTRY(Manager, ACTIVE_STATE), EXIT(Manager, ACTIVE_STATE)) + +// [FSM State::ERROR] +DEFINE_FSM_STATE_BEGIN(Manager, ERROR_STATE) + TRANSITION(ON_DONE, 0, ACTION(Manager, ERROR_STATE, ON_DONE), IDLE_STATE) +DEFINE_FSM_STATE_END(Manager, ERROR_STATE, 0, 0) + +// -[FSM: Definition]---------------------------------------------------------- +DEFINE_FSM_BEGIN(Manager, main) + STATE(Manager, IDLE_STATE) + STATE(Manager, ACTIVE_STATE) + STATE(Manager, ERROR_STATE) +DEFINE_FSM_END() + +// ---------------------------------------------------------------------------- +} // namespace tracking diff --git a/third-party/libtm/libtm/src/Manager.h b/third-party/libtm/libtm/src/Manager.h new file mode 100644 index 0000000000..b1dad3b369 --- /dev/null +++ b/third-party/libtm/libtm/src/Manager.h @@ -0,0 +1,155 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include +#include +#include +#include +#include "Dispatcher.h" +#include "Fsm.h" +#include "TrackingManager.h" +#include "Device.h" +#include "TrackingCommon.h" +#include "UsbPlugListener.h" +#include "Event.h" +#include "CompleteTask.h" +#include + +namespace perc +{ + class Manager : public TrackingManager, + public EventHandler, + public CompleteQueueHandler, + public UsbPlugListener::Owner + { + public: + + Manager(Listener*, void* param = 0); + virtual ~Manager(); + // [interface] TrackingManager + virtual Handle getHandle() { return mEvent.handle(); }; + virtual Status handleEvents(bool blocking); + virtual size_t getDeviceList(TrackingDevice** list, unsigned int maxListSize); + virtual Status setHostLogControl(IN const TrackingData::LogControl& logControl); + virtual Status getHostLog(OUT TrackingData::Log* log); + virtual uint64_t version(); + + // [interface] CompleteQueueHandler + virtual void addTask(std::shared_ptr&) override; + virtual void removeTasks(void* owner, bool completeTasks) override; + + protected: + std::thread mThread; + std::shared_ptr mDispatcher; + + // [interface] EventHandler + virtual void onMessage(const Message &) override {} + virtual void onTimeout(uintptr_t timerId, const Message &msg) override {} + virtual void onRead(int fd, void *act) {} + virtual void onException(int fd, void *act) {} + + // [interface] UsbPlugListener::Owner + virtual Status onAttach(libusb_device*) override; + virtual void onDetach(libusb_device*) override; + virtual libusb_context* context() override { return mContext; } + virtual Dispatcher& dispatcher() override { return *mDispatcher.get(); } + + + // [FSM] declaration + Fsm mFsm; + DECLARE_FSM(main); + + // [States] + enum { + IDLE_STATE = FSM_STATE_USER_DEFINED, + ACTIVE_STATE, + ERROR_STATE, + }; + + // [Events] + enum { + ON_INIT = FSM_EVENT_USER_DEFINED, + ON_DONE, + ON_ATTACH, + ON_DETACH, + ON_ERROR, + ON_NEW_TASK, + ON_REMOVE_TASKS + }; + + // [State] IDLE + DECLARE_FSM_STATE(IDLE_STATE); + DECLARE_FSM_STATE_ENTRY(IDLE_STATE); + DECLARE_FSM_GUARD(IDLE_STATE, ON_INIT); + DECLARE_FSM_ACTION(IDLE_STATE, ON_INIT); + + // [State] ACTIVE + DECLARE_FSM_STATE(ACTIVE_STATE); + DECLARE_FSM_STATE_ENTRY(ACTIVE_STATE); + DECLARE_FSM_STATE_EXIT(ACTIVE_STATE); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_ATTACH); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_DETACH); + DECLARE_FSM_GUARD(ACTIVE_STATE, ON_NEW_TASK); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_NEW_TASK); + DECLARE_FSM_GUARD(ACTIVE_STATE, ON_REMOVE_TASKS); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_REMOVE_TASKS); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_DONE); + DECLARE_FSM_ACTION(ACTIVE_STATE, ON_ERROR); + + // [State] ERROR + DECLARE_FSM_STATE(ERROR_STATE); + DECLARE_FSM_ACTION(ERROR_STATE, ON_DONE); + + // private messages + class MessageON_INIT: public Message { + public: + MessageON_INIT(TrackingManager::Listener *l, void *c) : Message(ON_INIT), + listener(l), context(c) {} + TrackingManager::Listener *listener; + void *context; + }; + + class MessageON_LIBUSB_EVENT : public Message { + public: + MessageON_LIBUSB_EVENT(bool attach, libusb_device* dev) : Message(attach ? ON_ATTACH : ON_DETACH), device(dev) + { } + libusb_device* device; + }; + + class MessageON_NEW_TASK : public Message { + public: + MessageON_NEW_TASK(std::shared_ptr& newTask) : Message(ON_NEW_TASK), task(newTask) + { } + std::shared_ptr task; + }; + + class MessageON_REMOVE_TASKS : public Message { + public: + MessageON_REMOVE_TASKS(void * owner, bool completeTasks) : Message(ON_REMOVE_TASKS), mOwner(owner), mCompleteTasks(completeTasks) + { } + void* mOwner; + bool mCompleteTasks; + }; + + Status processMessage(const Message &, int prio = Dispatcher::PRIORITY_IDLE); + + private: + + Status loadBufferToDevice(libusb_device* device, unsigned char* buffer, size_t size); + Status loadFileToDevice(libusb_device* device, const char* fileName); + Status init(TrackingManager::Listener*, void*); + void done(); + + TrackingManager::Listener* mListener; + std::map mLibUsbDeviceToTrackingDeviceMap; + libusb_context * mContext; + + std::string mFwFileName; + std::shared_ptr mUsbPlugListener; + std::list> mCompleteQ; + std::mutex mCompleteQMutex; + Event mEvent; + std::map mTrackingDeviceInfoMap; + }; +} diff --git a/third-party/libtm/libtm/src/Message.cpp b/third-party/libtm/libtm/src/Message.cpp new file mode 100644 index 0000000000..65d1ccaecc --- /dev/null +++ b/third-party/libtm/libtm/src/Message.cpp @@ -0,0 +1,500 @@ +/******************************************************************************* +INTEL CORPORATION PROPRIETARY INFORMATION +Copyright(c) 2017 Intel Corporation. All Rights Reserved. +*******************************************************************************/ + +#include +#include +#include "Message.h" +#include "Common.h" + +/** +* @brief This function initilize the message request header with needed length and message ID +* +* @param[in] message_request - message request buffer. +* @param[in] dwLength - message request length. +* @param[in] wMessageID - message ID. +* @return None +*/ +void init_message_request_header(IN unsigned char * message_request, IN uint32_t dwLength, IN uint16_t wMessageID) +{ + ((bulk_message_request_header*)message_request)->dwLength = dwLength; + ((bulk_message_request_header*)message_request)->wMessageID = wMessageID; +} + + +/** +* @brief This function prints all supported request/response messages +* +* @param[in] message - message buffer. +* @return None +*/ +void print_message(IN unsigned char * message) +{ + bulk_message_request_header* message_header = ((bulk_message_request_header*)message); + unsigned int i = 0; + unsigned int j = 0; + + printf("message->header.dwLength = %d\n", message_header->dwLength); + printf("message->header.wMessageID = 0x%X\n", message_header->wMessageID); + + switch (message_header->wMessageID) + { + case (DEV_GET_DEVICE_INFO): + { + if (message_header->dwLength > sizeof(bulk_message_request_get_device_info)) + { + /* bulk_message_response_get_device_info */ + bulk_message_response_get_device_info* message_response = ((bulk_message_response_get_device_info*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + if (message_response->header.wStatus == 0) + { + printf("message->bDeviceType = 0x%X\n", message_response->message.bDeviceType); + printf("message->bHwVersion = 0x%X\n", message_response->message.bHwVersion); + printf("message->bStatus = 0x%X\n", message_response->message.bStatus); + printf("message->wReserved = 0x%X\n", message_response->message.wReserved); + printf("message->dwRomVersion = 0x%X\n", message_response->message.dwRomVersion); + printf("message->dwFWVersion = 0x%X\n", message_response->message.dwFWVersion); + printf("message->dwCalibrationVersion = 0x%X\n", message_response->message.dwCalibrationVersion); + printf("message->llExtendedStatus = %lu\n", message_response->message.llExtendedStatus); + } + } + break; + } + + case (DEV_GET_TIME): + { + if (message_header->dwLength > sizeof(bulk_message_request_get_time)) + { + /* bulk_message_response_get_time */ + bulk_message_response_get_time* message_response = ((bulk_message_response_get_time*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + if (message_response->header.wStatus == 0) + { + printf("message->llNanoseconds = %lu\n", message_response->llNanoseconds); + } + } + break; + } + + case (DEV_GET_SUPPORTED_RAW_STREAMS): + { + if (message_header->dwLength > sizeof(bulk_message_request_get_supported_raw_streams)) + { + /* bulk_message_response_get_supported_raw_streams */ + bulk_message_response_get_supported_raw_streams* message_response = ((bulk_message_response_get_supported_raw_streams*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + if (message_response->header.wStatus == 0) + { + printf("message->wNumSupportedStreams = %d\n", message_response->wNumSupportedStreams); + if (message_response->wNumSupportedStreams > 0) + { + for (i = 0; i < message_response->wNumSupportedStreams; i++) + { + printf("message->stream[%d].bSensorID = 0x%X\n", i, message_response->stream[i].bSensorID); + printf("message->stream[%d].wFramesPerSecond = %d\n", i, message_response->stream[i].wFramesPerSecond); + printf("message->stream[%d].wWidth = %d\n", i, message_response->stream[i].wWidth); + printf("message->stream[%d].wHeight = %d\n", i, message_response->stream[i].wHeight); + printf("message->stream[%d].wReserved = %d\n", i, message_response->stream[i].wReserved); + } + } + } + } + break; + } + + case (DEV_RAW_STREAMS_CONTROL): + { + if (message_header->dwLength > sizeof(bulk_message_response_raw_streams_control)) + { + /* bulk_message_request_raw_streams_control */ + bulk_message_request_raw_streams_control* message_request = ((bulk_message_request_raw_streams_control*)message); + printf("message->wNumEnabledStreams = %d\n", message_request->wNumEnabledStreams); + if (message_request->wNumEnabledStreams > 0) + { + for (i = 0; i < message_request->wNumEnabledStreams; i++) + { + printf("message->stream[%d].bSensorID = 0x%X\n", i, message_request->stream[i].bSensorID); + printf("message->stream[%d].wFramesPerSecond = %d\n", i, message_request->stream[i].wFramesPerSecond); + printf("message->stream[%d].wWidth = %d\n", i, message_request->stream[i].wWidth); + printf("message->stream[%d].wHeight = %d\n", i, message_request->stream[i].wHeight); + printf("message->stream[%d].wReserved = %d\n", i, message_request->stream[i].wReserved); + } + } + } + else + { + /* bulk_message_response_raw_streams_control */ + bulk_message_response_raw_streams_control* message_response = ((bulk_message_response_raw_streams_control*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + } + + break; + } + + case (DEV_GET_CAMERA_INTRINSICS): + { + if (message_header->dwLength == sizeof(bulk_message_request_get_camera_intrinsics)) + { + /* bulk_message_request_get_camera_intrinsics */ + bulk_message_request_get_camera_intrinsics* message_request = ((bulk_message_request_get_camera_intrinsics*)message); + printf("message->bCameraID = 0x%X\n", message_request->bCameraID); + } + else + { + /* bulk_message_response_get_camera_intrinsics */ + bulk_message_response_get_camera_intrinsics* message_response = ((bulk_message_response_get_camera_intrinsics*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + if (message_response->header.wStatus == 0) + { + printf("message->intrinsics.dwWidth = %d\n", message_response->intrinsics.dwWidth); + printf("message->intrinsics.dwHeight = %d\n", message_response->intrinsics.dwHeight); + printf("message->intrinsics.flPpx = %f\n", message_response->intrinsics.flPpx); + printf("message->intrinsics.flPpy = %f\n", message_response->intrinsics.flPpy); + printf("message->intrinsics.flFx = %f\n", message_response->intrinsics.flFx); + printf("message->intrinsics.flFy = %f\n", message_response->intrinsics.flFy); + printf("message->intrinsics.dwDistortionModel = %d\n", message_response->intrinsics.dwDistortionModel); + for (i = 0; i < 5; i++) + { + printf("message->intrinsics.flCoeffs[%d] = %f\n", i, message_response->intrinsics.flCoeffs[i]); + } + } + } + break; + } + + case (DEV_GET_MOTION_INTRINSICS): + { + if (message_header->dwLength == sizeof(bulk_message_request_get_motion_intrinsics)) + { + /* bulk_message_request_get_motion_module_intrinsics */ + bulk_message_request_get_motion_intrinsics* message_request = ((bulk_message_request_get_motion_intrinsics*)message); + printf("message->bMotionID = 0x%X\n", message_request->bMotionID); + } + else + { + /* bulk_message_response_get_motion_module_intrinsics */ + bulk_message_response_get_motion_intrinsics* message_response = ((bulk_message_response_get_motion_intrinsics*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + if (message_response->header.wStatus == 0) + { + for (i = 0; i < 3; i++) + { + for (j = 0; j < 4; j++) + { + printf("message->intrinsics.flData[%d][%d] = %f\n", i, j, message_response->intrinsics.flData[i][j]); + } + } + + for (i = 0; i < 3; i++) + { + printf("message->intrinsics.flNoiseVariances[%d] = %f\n", i, message_response->intrinsics.flNoiseVariances[i]); + } + + for (i = 0; i < 3; i++) + { + printf("message->intrinsics.flBiasVariances[%d] = %f\n", i, message_response->intrinsics.flBiasVariances[i]); + } + } + } + break; + } + + case (DEV_GET_EXTRINSICS): + { + if (message_header->dwLength == sizeof(bulk_message_request_get_extrinsics)) + { + /* bulk_message_request_get_motion_module_intrinsics */ + bulk_message_request_get_extrinsics* message_request = ((bulk_message_request_get_extrinsics*)message); + printf("message->bSensorID = 0x%X\n", message_request->bSensorID); + printf("message->bReferenceSensorID = 0x%X\n", message_request->bReferenceSensorID); + } + else + { + /* bulk_message_response_get_extrinsics */ + bulk_message_response_get_extrinsics* message_response = ((bulk_message_response_get_extrinsics*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + if (message_response->header.wStatus == 0) + { + for (i = 0; i < 9; i++) + { + printf("message->extrinsics.flRotation[%d] = %f\n", i, message_response->extrinsics.flRotation[i]); + } + + for (i = 0; i < 3; i++) + { + printf("message->extrinsics.flTranslation[%d] = %f\n", i, message_response->extrinsics.flTranslation[i]); + } + } + } + break; + } + + case (DEV_SET_CAMERA_INTRINSICS): + { + if (message_header->dwLength == sizeof(bulk_message_request_set_camera_intrinsics)) + { + /* bulk_message_request_set_camera_intrinsics */ + bulk_message_request_set_camera_intrinsics* message_request = ((bulk_message_request_set_camera_intrinsics*)message); + printf("message->bCameraID = 0x%X\n", message_request->bCameraID); + printf("message->bReserved = %d\n", message_request->bReserved); + printf("message->intrinsics.dwWidth = %d\n", message_request->intrinsics.dwWidth); + printf("message->intrinsics.dwHeight = %d\n", message_request->intrinsics.dwHeight); + printf("message->intrinsics.flPpx = %f\n", message_request->intrinsics.flPpx); + printf("message->intrinsics.flPpy = %f\n", message_request->intrinsics.flPpy); + printf("message->intrinsics.flFx = %f\n", message_request->intrinsics.flFx); + printf("message->intrinsics.flFy = %f\n", message_request->intrinsics.flFy); + printf("message->intrinsics.dwDistortionModel = %d\n", message_request->intrinsics.dwDistortionModel); + for (i = 0; i < 5; i++) + { + printf("message->intrinsics.flCoeffs[%d] = %f\n", i, message_request->intrinsics.flCoeffs[i]); + } + } + else + { + /* bulk_message_response_set_camera_intrinsics */ + bulk_message_response_set_camera_intrinsics* message_response = ((bulk_message_response_set_camera_intrinsics*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + } + break; + } + + case (DEV_SET_MOTION_INTRINSICS): + { + if (message_header->dwLength == sizeof(bulk_message_request_set_motion_intrinsics)) + { + /* bulk_message_request_set_camera_intrinsics */ + bulk_message_request_set_motion_intrinsics* message_request = ((bulk_message_request_set_motion_intrinsics*)message); + printf("message->bMotionID = 0x%X\n", message_request->bMotionID); + printf("message->bReserved = %d\n", message_request->bReserved); + for (i = 0; i < 3; i++) + { + for (j = 0; j < 4; j++) + { + printf("message->intrinsics.flData[%d][%d] = %f\n", i, j, message_request->intrinsics.flData[i][j]); + } + } + + for (i = 0; i < 3; i++) + { + printf("message->intrinsics.flNoiseVariances[%d] = %f\n", i, message_request->intrinsics.flNoiseVariances[i]); + } + + for (i = 0; i < 3; i++) + { + printf("message->intrinsics.flBiasVariances[%d] = %f\n", i, message_request->intrinsics.flBiasVariances[i]); + } + } + else + { + /* bulk_message_response_set_motion_intrinsics */ + bulk_message_response_set_motion_intrinsics* message_response = ((bulk_message_response_set_motion_intrinsics*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + } + break; + } + + case (DEV_SET_EXTRINSICS): + { + if (message_header->dwLength == sizeof(bulk_message_request_set_extrinsics)) + { + /* bulk_message_request_set_extrinsics */ + bulk_message_request_set_extrinsics* message_request = ((bulk_message_request_set_extrinsics*)message); + printf("message->bSensorID = 0x%X\n", message_request->bSensorID); + printf("message->bReferenceSensorID = %d\n", message_request->bReferenceSensorID); + for (i = 0; i < 9; i++) + { + printf("message->extrinsics.flRotation[%d] = %f\n", i, message_request->extrinsics.flRotation[i]); + } + + for (i = 0; i < 3; i++) + { + printf("message->extrinsics.flTranslation[%d] = %f\n", i, message_request->extrinsics.flTranslation[i]); + } + } + else + { + /* bulk_message_response_set_extrinsics */ + bulk_message_response_set_extrinsics* message_response = ((bulk_message_response_set_extrinsics*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + } + break; + } + + case (DEV_LOG_CONTROL): + { + if (message_header->dwLength == sizeof(bulk_message_request_log_control)) + { + /* bulk_message_request_log_control */ + bulk_message_request_log_control* message_request = ((bulk_message_request_log_control*)message); + printf("message->bVerbosity = %d\n", message_request->bVerbosity); + printf("message->bLogMode = 0x%X\n", message_request->bLogMode); + } + else + { + /* bulk_message_response_log_control */ + bulk_message_response_log_control* message_response = ((bulk_message_response_log_control*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + } + break; + } + + case (DEV_GET_POSE): + { + if (message_header->dwLength > sizeof(bulk_message_request_get_pose)) + { + /* bulk_message_response_get_pose & interrupt_message_get_pose */ + bulk_message_response_get_pose* message_response = ((bulk_message_response_get_pose*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + if (message_response->header.wStatus == 0) + { + printf("message->pose.flX = %f\n", message_response->pose.flX); + printf("message->pose.flY = %f\n", message_response->pose.flY); + printf("message->pose.flZ = %f\n", message_response->pose.flZ); + printf("message->pose.flQi = %f\n", message_response->pose.flQi); + printf("message->pose.flQj = %f\n", message_response->pose.flQj); + printf("message->pose.flQk = %f\n", message_response->pose.flQk); + printf("message->pose.flQr = %f\n", message_response->pose.flQr); + printf("message->pose.flVx = %f\n", message_response->pose.flVx); + printf("message->pose.flVy = %f\n", message_response->pose.flVy); + printf("message->pose.flVz = %f\n", message_response->pose.flVz); + printf("message->pose.flVAX = %f\n", message_response->pose.flVAX); + printf("message->pose.flVAY = %f\n", message_response->pose.flVAY); + printf("message->pose.flVAZ = %f\n", message_response->pose.flVAZ); + printf("message->pose.flAx = %f\n", message_response->pose.flAx); + printf("message->pose.flAy = %f\n", message_response->pose.flAy); + printf("message->pose.flAz = %f\n", message_response->pose.flAz); + printf("message->pose.flAAX = %f\n", message_response->pose.flAAX); + printf("message->pose.flAAY = %f\n", message_response->pose.flAAY); + printf("message->pose.flAAZ = %f\n", message_response->pose.flAAZ); + printf("message->pose.llNanoseconds = %lu\n", message_response->pose.llNanoseconds); + } + } + break; + } + + case (SLAM_GET_OCCUPANCY_MAP_TILES): + { + if (message_header->dwLength > sizeof(bulk_message_request_get_occupancy_map_tiles)) + { + /* bulk_message_response_get_occupancy_map_tiles */ + bulk_message_response_get_occupancy_map_tiles* message_response = ((bulk_message_response_get_occupancy_map_tiles*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + if (message_response->header.wStatus == 0) + { + printf("message->bAccuracy = %d\n", message_response->map_tiles.bAccuracy); + printf("message->wTileCount = %d\n", message_response->map_tiles.wTileCount); + + if (message_response->map_tiles.wTileCount > 0) + { + for (i = 0; i < message_response->map_tiles.wTileCount; i++) + { + printf("message->tiles[%d].dwX = %d\n", i, message_response->map_tiles.tiles[i].dwX); + printf("message->tiles[%d].dwY = %d\n", i, message_response->map_tiles.tiles[i].dwY); + printf("message->tiles[%d].dwZ = %d\n", i, message_response->map_tiles.tiles[i].dwZ); + } + } + } + } + break; + } + + case (SLAM_GET_LOCALIZATION_DATA): + { + if (message_header->dwLength > sizeof(bulk_message_request_get_localization_data)) + { + /* bulk_message_response_get_localization_data */ + bulk_message_response_get_localization_data* message_response = ((bulk_message_response_get_localization_data*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + if (message_response->header.wStatus == 0) + { + int localizationDataSize = message_response->header.dwLength - offsetof(bulk_message_response_get_localization_data, message); + if (localizationDataSize > 0) + { + printf("message->bLocalizationData:\n"); + print_data(&message_response->message.bLocalizationData[0], localizationDataSize); + } + } + } + break; + } + + case (SLAM_SET_LOCALIZATION_DATA): + { + if (message_header->dwLength > sizeof(bulk_message_response_set_localization_data)) + { + /* bulk_message_request_set_localization_data - Printing bLocalizationData is TBD*/ + /* bulk_message_request_set_localization_data* message_request = ((bulk_message_request_set_localization_data*)message); */ + /* Printing bLocalizationData is TBD */ + } + else + { + /* bulk_message_response_set_localization_data */ + bulk_message_response_set_localization_data* message_response = ((bulk_message_response_set_localization_data*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + } + break; + } + + case (SLAM_SET_6DOF_INTERRUPT_RATE): + { + if (message_header->dwLength == sizeof(bulk_message_request_set_6dof_interrupt_rate)) + { + /* bulk_message_request_set_6dof_interrupt_rate */ + bulk_message_request_set_6dof_interrupt_rate* message_request = ((bulk_message_request_set_6dof_interrupt_rate*)message); + printf("message->bInterruptRate = %d\n", message_request->message.bInterruptRate); + } + else + { + /* bulk_message_response_set_6dof_interrupt_rate */ + bulk_message_response_set_6dof_interrupt_rate* message_response = ((bulk_message_response_set_6dof_interrupt_rate*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + } + break; + } + + case (SLAM_6DOF_CONTROL): + { + if (message_header->dwLength == sizeof(bulk_message_request_6dof_control)) + { + /* bulk_message_request_6dof_control */ + bulk_message_request_6dof_control* message_request = ((bulk_message_request_6dof_control*)message); + printf("message->bEnable = %d\n", message_request->bEnable); + } + else + { + /* bulk_message_response_6dof_control */ + bulk_message_response_6dof_control* message_response = ((bulk_message_response_6dof_control*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + } + break; + } + + case (SLAM_OCCUPANCY_MAP_CONTROL): + { + if (message_header->dwLength > sizeof(bulk_message_response_occupancy_map_control)) + { + /* bulk_message_request_occupancy_map_control */ + bulk_message_request_occupancy_map_control* message_request = ((bulk_message_request_occupancy_map_control*)message); + printf("message->bEnable = %d\n", message_request->message.bEnable); + } + else + { + /* bulk_message_response_occupancy_map_control */ + bulk_message_response_occupancy_map_control* message_response = ((bulk_message_response_occupancy_map_control*)message); + printf("message->header->wStatus = %d\n", message_response->header.wStatus); + } + break; + } + + case (DEV_GET_AND_CLEAR_EVENT_LOG): /* Fall through */ + default: + { + printf("Print unsupported\n"); + break; + } + + } + printf("\n"); + +} diff --git a/third-party/libtm/libtm/src/Message.h b/third-party/libtm/libtm/src/Message.h new file mode 100644 index 0000000000..f958d92fb7 --- /dev/null +++ b/third-party/libtm/libtm/src/Message.h @@ -0,0 +1,1876 @@ +// 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_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 + +namespace perc +{ + /** + * @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_SET_GEO_LOCATION = 0x001A, + DEV_FLUSH = 0x001B, + 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, + + /* 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_RESET_LOCALIZATION_DATA = 0x1008, + SLAM_GET_LOCALIZATION_DATA_STREAM = 0x1009, + SLAM_SET_STATIC_NODE = 0x100A, + SLAM_GET_STATIC_NODE = 0x100B, + + /* Controller messages */ + CONTROLLER_POSE_CONTROL = 0x2002, + CONTROLLER_STATUS_CHANGE_EVENT = 0x2003, + CONTROLLER_DEVICE_CONNECT = 0x2004, + CONTROLLER_DEVICE_DISCOVERY_EVENT = 0x2005, + CONTROLLER_DEVICE_DISCONNECT = 0x2006, + CONTROLLER_READ_ASSOCIATED_DEVICES = 0x2007, + CONTROLLER_WRITE_ASSOCIATED_DEVICES = 0x2008, + CONTROLLER_DEVICE_DISCONNECTED_EVENT = 0x2009, + CONTROLLER_DEVICE_CONNECTED_EVENT = 0x200A, + CONTROLLER_RSSI_TEST_CONTROL = 0x200B, + CONTROLLER_SEND_DATA = 0x200C, + CONTROLLER_START_CALIBRATION = 0x200D, + CONTROLLER_CALIBRATION_STATUS_EVENT = 0x200E, + CONTROLLER_DEVICE_LED_INTENSITY_EVENT = 0xF000, + + /* Error messages */ + DEV_ERROR = 0x8000, + SLAM_ERROR = 0x9000, + CONTROLLER_ERROR = 0xA000, + + /* Message IDs are 16-bits */ + MAX_MESSAGE_ID = 0xFFFF, + } BULK_MESSAGE_ID; + + + /** + * @brief Defines all bulk message return statuses + */ + enum class MESSAGE_STATUS { + 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 */ + NO_BLUETOOTH = 0x0016, /* The device doesn't have bluetooth, so the command failed */ + SLAM_NO_DICTIONARY = 0x9001, /* No relocalization dictionary was loaded */ + }; + + /** + * @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 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 Defines all controller calibration error codes + */ + typedef enum { + CONTROLLER_CALIBRATION_STATUS_SUCCEEDED = 0, /**< No error has occurred. */ + CONTROLLER_CALIBRATION_STATUS_VALIDATION_FAILURE = 1, /**< Validation failure has occurred. */ + CONTROLLER_CALIBRATION_STATUS_FLASH_ACCESS_FAILURE = 2, /**< Flash access failure has occurred. */ + CONTROLLER_CALIBRATION_STATUS_IMU_FAILURE = 3, /**< IMU failure has occurred. */ + CONTROLLER_CALIBRATION_STATUS_INTERNAL_FAILURE = 4, /**< A fatal internal error has occurred. */ + } CONTROLLER_CALIBRATION_STATUS_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 */ + 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 */ + uint32_t dwCentralAppVersionBuild; /**< Build part of the Build Central firmware version */ + + } 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, Controller = 6 */ + /**< 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: NONE = 0, MODIFIED_BROWN_CONRADY = 1, INVERSE_BROWN_CONRADY = 2, FTHETA = 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; + + + /** + * @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 */ + } 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 or controller - 0x0 = HMD, 0x1 - controller 1, 0x2 - controller 2 */ + } 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 Set Geo Location Message + * + * Sets the geographical location (e.g. GPS data). This data can be later used by the algorithms to correct IMU readings. + */ + typedef struct { + bulk_message_request_header header; /**< Message request header: dwLength = 32 bytes, wMessageID = DEV_SET_GEO_LOCATION */ + uint16_t wReserved; /**< Reserved = 0 */ + double_t dfLatitude; /**< Latitude in degrees */ + double_t dfLongitude; /**< Longitude in degrees */ + double_t dfAltitude; /**< Altitude in meters above the WGS 84 reference ellipsoid */ + } bulk_message_request_set_geo_location; + + typedef struct { + bulk_message_response_header header; /**< Message response header: dwLendth = 8, wMessageID = DEV_SET_GEO_LOCATION */ + } bulk_message_response_set_geo_location; + + + /** + * @brief Bulk Flush Message + * + * Sends in addition to the standard response over the commands endpoint, a response message over all the device-to-host endpoints - events & streams endpoints. + * This command shall be the first command requested by the host in order to "flush" any possible messages left from a previous session that wasn't terminated properly (e.g. host app crashed or timed-out) + */ + typedef struct { + bulk_message_request_header header; /**< Message request header: dwLength = 16 bytes, wMessageID = DEV_FLUSH */ + uint16_t wReserved; /**< Reserved = 0 */ + uint64_t ddwToken; /**< 64 bit token that will be received in the response */ + } bulk_message_request_flush; + + typedef struct { + bulk_message_response_header header; /**< Message response header: dwLendth = 16, wMessageID = DEV_FLUSH */ + uint64_t ddwToken; /**< 64 bit token from the request */ + } bulk_message_response_flush; + + + /** + * @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. + * on DEV_LOCK_CONFIGURATION - The locking is done in software by the firmware managing a lock bits in each configuration table metadata. + * on DEV_LOCK_EEPROM - 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_configuration; + + typedef struct { + bulk_message_response_header header; /**< Message response header: dwLendth = 8, wMessageID = DEV_LOCK_CONFIGURATION */ + } bulk_message_response_lock_configuration; + + + /** + * @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 Get Occupancy Map Tiles Message + * + * Returns new real world tiles detected by the SLAM middleware (tiles reported earlier in the session will not be returned). + * Tiles are represented in location relative to the device's initial position, + * If relocalization data was set, this pose is relative to the relocalization database. + */ + typedef struct { + uint32_t dwX; /**< X value of first tile, in meters (relative to initial position) */ + uint32_t dwY; /**< Y value of first tile, in meters (relative to initial position) */ + uint32_t dwW; /**< W value of first tile, in meters (relative to initial position) or 0 if value unknown */ + } map_tiles; + + typedef struct { + uint8_t bAccuracy; /**< Accuracy of occupancy map calculation: 0x0 - low accuracy, 0x1 - medium accuracy, 0x2 - high accuracy */ + uint8_t bReserved; /**< Reserved = 0 */ + uint16_t wTileCount; /**< Number of tiles in the following array */ + uint64_t llNanoseconds; /**< Timestamp of tile sample, measured in nanoseconds since device system initialization */ + map_tiles tiles[]; /**< Occupancy map tiles variable sized array */ + } occupancy_map_tiles; + + typedef struct { + bulk_message_request_header header; /**< Message request header: dwLength = 6 bytes, wMessageID = SLAM_GET_OCCUPANCY_MAP_TILES */ + } bulk_message_request_get_occupancy_map_tiles; + + typedef struct { + bulk_message_response_header header; /**< Message response header: wMessageID = SLAM_GET_OCCUPANCY_MAP_TILES */ + occupancy_map_tiles map_tiles; /**< Occupancy map tiles */ + } bulk_message_response_get_occupancy_map_tiles; + + + /** + * @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 reset Localization Data Message + * + * Resets the localization data + */ + typedef struct { + bulk_message_request_header header; /**< Message request header: dwLength = 8 bytes, wMessageID = SLAM_RESET_LOCALIZATION_DATA */ + uint8_t bFlag; /**< 0 - Reset all localization data, 1 - Reset only the map by its bMapIndex */ + uint8_t bReserved; /**< Reserved = 0 */ + } bulk_message_request_reset_localization_data; + + typedef struct { + bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = SLAM_RESET_LOCALIZATION_DATA */ + } bulk_message_response_reset_localization_data; + + + /** + * @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; + + /** + * @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 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, Controller = 6 */ + /**< 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 controller stream metadata + */ + typedef struct + { + uint32_t dwMetadataLength; /**< Metadata length in bytes (0 bytes) */ + uint32_t dwFrameLength; /**< Length of frame below (8 bytes) */ + uint8_t bEventID; /**< Event ID - button, trackpad or battery (vendor specific), supported values 0-63 */ + uint8_t bInstanceId; /**< Instance of the sensor in case of multiple sensors */ + uint8_t bSensorData[CONTROLLER_SENSOR_DATA_SIZE]; /**< Sensor data that is pass-through from the controller firmware */ + } bulk_message_controller_stream_metadata; + + + /** + * @brief Bulk raw controller stream message + * + * Specific frame metadata and data for sensor IDs - controller + */ + typedef struct + { + bulk_message_raw_stream_header rawStreamHeader; + bulk_message_controller_stream_metadata metadata; + } bulk_message_controller_stream; + + + /** + * @brief Bulk raw rssi stream metadata + */ + typedef struct + { + uint32_t dwMetadataLength; /**< Metadata length in bytes (0 bytes) */ + uint32_t dwFrameLength; /**< Length of frame below (8 bytes) */ + float_t flSignalStrength; /**< Sampled signal strength (dB), a value closer to 0 indicates a stronger signal */ + } bulk_message_rssi_stream_metadata; + + + /** + * @brief Bulk raw rssi stream message + * + * Specific frame metadata and data for sensor IDs - BLE Signal Strength (sent during a BLE RSSI test) + */ + typedef struct + { + bulk_message_raw_stream_header rawStreamHeader; + bulk_message_rssi_stream_metadata metadata; + } bulk_message_rssi_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 radians/sec */ + float_t flVy; /**< Y value of velocimeter, in radians/sec */ + float_t flVz; /**< Z value of velocimeter, in radians/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 Bulk Controller pose Control Message + * + * Enables / disables pose calculation for the controllers. + * If no CONTROLLER_POSE_CONTROL command was called before the host calls to DEV_START, the default value used shall be "Disable Controllers 6DoF". + */ + typedef struct { + bulk_message_request_header header; /**< Message request header: dwLength = 9 bytes, wMessageID = CONTROLLER_POSE_CONTROL */ + uint8_t bEnable; /**< 0x00 - Disable 6DoF, 0x01 - Enable 6DoF */ + uint8_t bMode; /**< 0x00 - Normal Mode, 0x01 - Fast Playback */ + uint8_t bNumControllers; /**< Number of controllers to be tracked. Only values of 1 or 2 supported. Ignored if bEnable == 0x0 */ + } bulk_message_request_controller_pose_control; + + typedef struct { + bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = CONTROLLER_POSE_CONTROL */ + } bulk_message_response_controller_pose_control; + + + /** + * @brief Bulk Controller Device Connect Message + * + * Connect the controller to the device. + */ + typedef struct { + bulk_message_request_header header; /**< Message request header: dwLength = 15 bytes, wMessageID = CONTROLLER_DEVICE_CONNECT */ + uint16_t wTimeout; /**< Connect timeout, in milliseconds */ + uint8_t bMacAddress[MAC_ADDRESS_SIZE]; /**< MAC address of controller to be connected */ + uint8_t bAddressType; /**< BLE address type (as received in advertisement) */ + } bulk_message_request_controller_device_connect; + + typedef struct { + bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = CONTROLLER_DEVICE_CONNECT */ + uint8_t bControllerID; /**< Assigned Controller identifier (1 or 2) */ + } bulk_message_response_controller_device_connect; + + + /** + * @brief Bulk Controller Device Disconnect Message + * + * Disconnect the controller from the device. + */ + typedef struct { + bulk_message_request_header header; /**< Message request header: dwLength = 7 bytes, wMessageID = CONTROLLER_DEVICE_DISCONNECT */ + uint8_t bControllerID; /**< Controller identifier (1 or 2) */ + } bulk_message_request_controller_device_disconnect; + + typedef struct { + bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = CONTROLLER_DEVICE_DISCONNECT */ + } bulk_message_response_controller_device_disconnect; + + + /** + * @brief Bulk Controller Read Associated Devices Message + * + * Reads the associated devices from the EEPROM. + */ + typedef struct { + bulk_message_request_header header; /**< Message request header: dwLength = 6 bytes, wMessageID = CONTROLLER_READ_ASSOCIATED_DEVICES */ + } bulk_message_request_controller_read_associated_devices; + + typedef struct { + bulk_message_response_header header; /**< Message response header: dwLength = 22 bytes, wMessageID = CONTROLLER_READ_ASSOCIATED_DEVICES */ + uint8_t bMacAddress1[MAC_ADDRESS_SIZE]; /**< MAC address of controller 1, set to all zeros if controller is not setup */ + uint8_t bAddressType1; /**< Controller 1 MAC address type. (0 = public, 1 = random static) */ + uint8_t bMacAddress2[MAC_ADDRESS_SIZE]; /**< MAC address of controller 2, set to all zeros if controller is not setup */ + uint8_t bAddressType2; /**< Controller 2 MAC address type. (0 = public, 1 = random static) */ + } bulk_message_response_controller_read_associated_devices; + + + /** + * @brief Bulk Controller Write Associated Devices Message + * + * Writes the associated devices to the EEPROM. + */ + typedef struct { + bulk_message_request_header header; /**< Message request header: dwLength = 20 bytes, wMessageID = CONTROLLER_WRITE_ASSOCIATED_DEVICES */ + uint8_t bMacAddress1[MAC_ADDRESS_SIZE]; /**< MAC address of controller 1, set to all zeros if controller is not setup */ + uint8_t bAddressType1; /**< Controller 1 MAC address type. (0 = public, 1 = random static) */ + uint8_t bMacAddress2[MAC_ADDRESS_SIZE]; /**< MAC address of controller 2, set to all zeros if controller is not setup */ + uint8_t bAddressType2; /**< Controller 2 MAC address type. (0 = public, 1 = random static) */ + } bulk_message_request_controller_write_associated_devices; + + typedef struct { + bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = CONTROLLER_WRITE_ASSOCIATED_DEVICES */ + } bulk_message_response_controller_write_associated_devices; + + + /** + * @brief Bulk Controller Send Data Message + * + * Sends opaque data to the controller. The vendor's controller code is responsible for interpreting the data. + * Example data could be a command to set a LED on the controller or set a vibration pattern on a haptic device. + */ + typedef struct { + bulk_message_request_header header; /**< Message request header: dwLength = 18 bytes, wMessageID = CONTROLLER_SEND_DATA */ + uint8_t bControllerID; /**< Controller identifier (1 or 2) */ + uint8_t bCommandID; /**< Command to be sent to the controller (vendor specific) - values 0-63 supported */ + uint8_t bControllerData[]; /**< Controller data to be sent. Data format is vendor specific */ + } bulk_message_request_controller_send_data; + + typedef struct { + bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = CONTROLLER_SEND_DATA */ + } bulk_message_response_controller_send_data; + + + /** + * @brief Bulk Controller Send Data Message + * + * Start the controller calibration process + * When the controller receives the indication to start calibration, it starts reading its gyroscope output for 30 seconds. + * The controller averages the readings over the first 25 seconds and uses this value as the gyro bias. + * The controller then averages the readings over the next 5 seconds and compares the value to the gyro bias from the first 25 seconds. + * If the values are the same (up to TBD noise value), then the gyro bias is stored to flash and the controller sends the "controller calibration status" event with success. + * Otherwise, the controller reports a failure. + */ + typedef struct { + bulk_message_request_header header; /**< Message request header: dwLength = 8 bytes, wMessageID = CONTROLLER_START_CALIBRATION */ + uint8_t bControllerID; /**< Controller identifier (1 or 2) */ + uint8_t reserved; /**< Reserved = 0 */ + } bulk_message_request_controller_start_calibration; + + typedef struct { + bulk_message_response_header header; /**< Message response header: dwLength = 10 bytes, wMessageID = CONTROLLER_START_CALIBRATION */ + } bulk_message_response_controller_start_calibration; + + + /** + * @brief Bulk Controller RSSI Test Control Message + * + * Start or Stop the RSSI test + */ + typedef struct { + bulk_message_request_header header; /**< Message request header: dwLength = 8 bytes, wMessageID = CONTROLLER_RSSI_TEST_CONTROL */ + uint8_t bControllerID; /**< Controller identifier (1 or 2) */ + uint8_t bTestControl; /**< 1 to start the test, 0 to stop the test */ + } bulk_message_request_controller_rssi_test_control; + + typedef struct { + bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = CONTROLLER_RSSI_TEST_CONTROL */ + } bulk_message_response_controller_rssi_test_control; + + + /** + * @brief Bulk Controller Central FW Update Message + * + * Updates the FW image on the BLE central device + */ + typedef struct { + bulk_message_request_header header; /**< Message request header: dwLength = 6 + image length in bytes, wMessageID = CONTROLLER_CENTRAL_FW_UPDATE */ + uint8_t bUpdateImage[]; /**< New Central FW image to be updated */ + } bulk_message_request_controller_central_fw_update; + + typedef struct { + bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = CONTROLLER_CENTRAL_FW_UPDATE */ + } bulk_message_response_controller_central_fw_update; + + + /** + * @brief Bulk Controller Controller-FW Update Message + * + * Updates the FW image on a connected controller device + */ + typedef struct { + bulk_message_request_header header; /**< Message request header: dwLength = 12 + image length in bytes, wMessageID = CONTROLLER_CONTROLLER_FW_UPDATE */ + uint8_t bMacAddress[MAC_ADDRESS_SIZE]; /**< MAC address of controller to be updated */ + uint8_t bUpdateImage[]; /**< New Controller FW image to be updated */ + } bulk_message_request_controller_controller_fw_update; + + typedef struct { + bulk_message_response_header header; /**< Message response header: dwLength = 8 bytes, wMessageID = CONTROLLER_CONTROLLER_FW_UPDATE */ + } bulk_message_response_controller_controller_fw_update; + + + /** + * @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 or controller - 0x0 = HMD, 0x1 - controller 1, 0x2 - controller 2 */ + 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, Controller = 6 */ + /**< 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 raw controller stream metadata + */ + typedef struct + { + uint32_t dwMetadataLength; /**< Metadata length in bytes (0 bytes) */ + uint32_t dwFrameLength; /**< Length of frame below (8 bytes) */ + uint8_t bEventID; /**< Event ID - button, trackpad or battery (vendor specific), supported values 0-63 */ + uint8_t bInstanceId; /**< Instance of the sensor in case of multiple sensors */ + uint8_t bSensorData[CONTROLLER_SENSOR_DATA_SIZE]; /**< Sensor data that is pass-through from the controller firmware */ + } interrupt_message_controller_stream_metadata; + + + /** + * @brief Interrupt raw controller stream message + * + * Specific frame metadata and data for sensor IDs - controller + */ + typedef struct + { + interrupt_message_raw_stream_header rawStreamHeader; + interrupt_message_controller_stream_metadata metadata; + } interrupt_message_controller_stream; + + + /** + * @brief Interrupt controller status changed message + * + * The Controller Pose algorithm shall provide estimated accuracy of the tracking state + */ + typedef struct { + interrupt_message_header header; /**< Interrupt message header: dwLength = 8 bytes, wMessageID = CONTROLLER_STATUS_CHANGE_EVENT */ + uint16_t wStatus; /**< Status: 0x0 - Failed, 0x1 - Low, 0x2 - Medium, 0x3 - High */ + } interrupt_message_controller_status_change; + + + /** + * @brief Interrupt controller device discovery event message + * + * The Controller Pose algorithm shall indicate on discovered controllers + */ + typedef struct { + uint16_t wManufacturerId; /**< Identifier of the controller manufacturer */ + uint8_t bVendorData; /**< vendor specific data copied from the controller advertisement. */ + /**< The least significant bit is reserved for pairing status (set if controller is in pairing mode) */ + uint8_t bProtocolVersion; /**< BLE protocol version supported by the controller */ + uint8_t bAppVersionMajor; /**< Major number of the app version */ + uint8_t bAppVersionMinor; /**< Minor number of the app version */ + uint8_t bAppVersionPatch; /**< Patch number of the app version */ + uint8_t bSoftdeviceVersion; /**< soft device version */ + uint8_t bBootloaderVersionMajor; /**< Major number of the boot loader version */ + uint8_t bBootloaderVersionMinor; /**< Minor number of the boot loader version */ + uint8_t bBootloaderVersionPatch; /**< Patch number of the boot loader version */ + } controller_discovery_info; + + typedef struct { + interrupt_message_header header; /**< Interrupt message header: dwLength = 25 bytes, wMessageID = CONTROLLER_DEVICE_DISCOVERY_EVENT */ + uint8_t bMacAddress[MAC_ADDRESS_SIZE]; /**< Discovered device byte array of MAC address */ + uint8_t bAddressType; /**< Discovered device address type */ + uint8_t bReserved; /**< Reserved = 0 */ + controller_discovery_info info; /**< Discovered controller versions */ + } interrupt_message_controller_device_discovery; + + + /** + * @brief Interrupt controller connected event message + * + * The Controller algorithm shall indicate on connect + */ + typedef struct { + uint8_t bProtocolVersion; /**< BLE protocol version supported by the controller */ + uint16_t wManufacturerId; /**< Identifier of the controller manufacturer */ + uint8_t bAppVersionMajor; /**< Major number of the app version */ + uint8_t bAppVersionMinor; /**< Minor number of the app version */ + uint8_t bAppVersionPatch; /**< Patch number of the app version */ + uint8_t bSoftdeviceVersion; /**< soft device version */ + uint8_t bBootloaderVersionMajor; /**< Major number of the boot loader version */ + uint8_t bBootloaderVersionMinor; /**< Minor number of the boot loader version */ + uint8_t bBootloaderVersionPatch; /**< Patch number of the boot loader version */ + } controller_connected_info; + + typedef struct { + interrupt_message_header header; /**< Interrupt message header: dwLength = 17 bytes, wMessageID = CONTROLLER_DEVICE_CONNECTED_EVENT */ + uint16_t wStatus; /**< Connection status: SUCCESS - connection succeeded */ + /**< Connection status: TIMEOUT - connection timed out */ + /**< Connection status: INCOMPATIBLE - connection succeeded but controller version is incompatible with TM2 version */ + uint8_t bControllerID; /**< Connected controller identifier (1 or 2) */ + controller_connected_info info; /**< Connected controller versions */ + } interrupt_message_controller_connected; + + + /** + * @brief Interrupt controller disconnected event message + * + * The Controller algorithm shall indicate on disconnect + */ + typedef struct { + interrupt_message_header header; /**< Interrupt message header: dwLength = 7 bytes, wMessageID = CONTROLLER_DEVICE_DISCONNECTED_EVENT */ + uint8_t bControllerID; /**< Disconnected controller identifier (1 or 2) */ + } interrupt_message_controller_disconnected; + + + /** + * @brief Interrupt Controller led intensity + */ + typedef struct + { + interrupt_message_raw_stream_header rawStreamHeader; /**< Interrupt message header: wMessageID = CONTROLLER_DEVICE_LED_INTENSITY_EVENT */ + uint32_t packetType; /**< Packet type */ + uint8_t ledId; /**< Controller Led identifier (1 or 2) */ + uint32_t intensity; /**< Controller Led intensity [0-100] */ + } interrupt_message_controller_led_intensity; + + /** + * @brief Interrupt controller calibration status event message + * + * This event is sent by the controller when the calibration process is finished or failed + */ + typedef struct { + interrupt_message_header header; /**< Interrupt message header: dwLength = 9 bytes, wMessageID = CONTROLLER_CALIBRATION_STATUS_EVENT */ + uint16_t wStatus; /**< Calibration status: 0x0 - calibration succeeded */ + /**< Calibration status: 0x1 - validation failed */ + /**< Calibration status: 0x2 - flash access failure */ + /**< Calibration status: 0x3 - IMU failure */ + /**< Calibration status: 0x4 - internal error */ + uint8_t bControllerID; /**< Calibrated controller identifier (1 or 2) */ + } interrupt_message_controller_calibration_status; + + + /** + * @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 Controller error message + * + * Controller error code message + */ + typedef struct { + interrupt_message_header header; /**< Interrupt message header: dwLength = 8 bytes, wMessageID = CONTROLLER_ERROR */ + uint16_t wStatus; /**< Status: rc_E_ERROR_NONE = 0 - No error has occurred. */ + /**< rc_E_ERROR_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. */ + /**< rc_E_ERROR_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. */ + /**< rc_E_ERROR_OTHER = 3 - A fatal internal error has occurred. */ + } interrupt_message_controller_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 firmware update stream message + * + * Response after sending firmware update command using bulk_message_large_stream + */ + typedef struct { + interrupt_message_header header; /**< Interrupt message header: dwLength = 8 bytes, wMessageID = DEV_FIRMWARE_UPDATE */ + uint16_t wStatus; /**< Status */ + uint8_t bMacAddress[MAC_ADDRESS_SIZE]; /**< MAC address of updated device. All zeros for central FW update */ + uint8_t bProgress; /**< Progress counter (percentage of update complete) */ + } interrupt_message_fw_update_stream; + + typedef struct { + uint8_t bMacAddress[MAC_ADDRESS_SIZE]; /**< MAC address of updated device. Shall be all zeros for Central Firmware update */ + uint8_t bAddressType; /**< BLE address type (as received in advertisement) */ + uint8_t bNumFiles; /**< Number of firmware update files sent below (n) */ + uint32_t dwFileSize[MAX_FW_UPDATE_FILE_COUNT]; /**< Length (in bytes) of the files */ + uint8_t bContcatenatedFiles[]; /**< Length of Concatenation of all files to be used for firmware update (length of this field is dwFileSize1 +...+ dwFileSize n) */ + } message_fw_update_request; + + /** + * @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) + + + /** + * @brief This function initilize the message request header with needed length and message ID + * + * @param[in] message_request - message request buffer. + * @param[in] dwLength - message request length. + * @param[in] wMessageID - message ID. + * @return None + */ + void init_message_request_header(IN unsigned char * message_request, IN uint32_t dwLength, IN uint16_t wMessageID); + + + /** + * @brief This function prints all supported request/response messages + * + * @param[in] message - message buffer. + * @return None + */ + void print_message(IN unsigned char * message); +} +#ifdef _WIN32 +#pragma warning (pop) +#endif diff --git a/third-party/libtm/libtm/src/Protocol.cpp b/third-party/libtm/libtm/src/Protocol.cpp new file mode 100644 index 0000000000..17f39b67d5 --- /dev/null +++ b/third-party/libtm/libtm/src/Protocol.cpp @@ -0,0 +1,363 @@ +/******************************************************************************* +INTEL CORPORATION PROPRIETARY INFORMATION +Copyright(c) 2017 Intel Corporation. All Rights Reserved. +*******************************************************************************/ + +#include +#include "Protocol.h" +#include "Common.h" + +#define TIMEOUT_MS 5000 + +/* Bulk Endpoint Protocol */ +#define BULK_IN_ENDPOINT 0x81 +#define BULK_OUT_ENDPOINT 0x01 +#define BULK_IN_MAX_TRANSFER_SIZE 1024 +#define BULK_OUT_MAX_TRANSFER_SIZE 1024 + +/* Control Endpoint Protocol */ +#define CONTROL_IN_MAX_TRANSFER_SIZE 1024 +#define CONTROL_OUT_MAX_TRANSFER_SIZE 1024 +#define CONTROL_IN_REQUEST_TYPE (LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_INTERFACE) +#define CONTROL_OUT_REQUEST_TYPE (LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_INTERFACE) +#define CONTROL_REQUEST_1 0x01 +#define CONTROL_REQUEST_2 0x02 + +/* Interrupt Endpoint Protocol */ +#define INTERRUPT_IN_ENDPOINT 0x82 +#define INTERRUPT_OUT_ENDPOINT 0x02 +#define INTERRUPT_IN_MAX_TRANSFER_SIZE 1024 +#define INTERRUPT_OUT_MAX_TRANSFER_SIZE 1024 + + +/* Description: This function uses synchronous bulk transfers to write message to the device */ +/* Parameters: IN device_handle - USB device handle */ +/* IN host_data_out - Host buffer with the message to send */ +/* IN host_data_out_len - Host buffer length */ +/* OUT host_data_out_bytes_sent - Actual buffer size transferred */ +/* Returns: Zero on success, libusb_error code on failure */ +int bulk_transfer_write_message(IN libusb_device_handle * device_handle, IN unsigned char * host_data_out, IN unsigned int host_data_out_len, OUT int * host_data_out_bytes_sent) +{ + int result = 0; + + *host_data_out_bytes_sent = 0; + + /* Write message to the device */ + result = libusb_bulk_transfer(device_handle, BULK_OUT_ENDPOINT, host_data_out, host_data_out_len, host_data_out_bytes_sent, TIMEOUT_MS); + if (result >= 0) + { + printf("Message sent via bulk transfer:\n"); + print_data(host_data_out, *host_data_out_bytes_sent); + } + else + { + fprintf(stderr, "Error sending message via bulk transfer (%s)\n", libusb_error_name(result)); + } + + return result; +} + + +/* Description: This function uses synchronous bulk transfers to receive message from the device */ +/* Parameters: IN device_handle - USB device handle */ +/* OUT host_data_in - Host buffer to save the message */ +/* OUT host_data_in_bytes_received - Actual buffer size received */ +/* Returns: Zero on success, libusb_error code on failure */ +int bulk_transfer_read_message(IN libusb_device_handle * device_handle, OUT unsigned char * host_data_in, OUT int * host_data_in_bytes_received) +{ + int result = 0; + + *host_data_in_bytes_received = 0; + + /* Read message from the device */ + result = libusb_bulk_transfer(device_handle, BULK_IN_ENDPOINT, host_data_in, BULK_IN_MAX_TRANSFER_SIZE, host_data_in_bytes_received, TIMEOUT_MS); + if (result >= 0) + { + if (*host_data_in_bytes_received > 0) + { + printf("Message received via bulk transfer:\n"); + print_data(host_data_in, *host_data_in_bytes_received); + } + else + { + fprintf(stderr, "No message received via bulk transfer (%d)\n", result); + return LIBUSB_ERROR_OTHER; + } + } + else + { + fprintf(stderr, "Error receiving message via bulk transfer (%s)\n", libusb_error_name(result)); + } + + return result; +} + + + +/* Description: This function uses synchronous bulk transfers to write data to the device and receive data from the device */ +/* Parameters: IN device_handle - USB device handle */ +/* IN host_data_out - Host buffer with the message to send */ +/* IN host_data_out_len - Host buffer length */ +/* OUT host_data_out_bytes_sent - Actual buffer size transferred */ +/* OUT host_data_in - Host buffer to save the message */ +/* OUT host_data_in_bytes_received - Actual buffer size received */ +/* Returns: Zero on success, libusb_error code on failure */ +int bulk_transfer_exchange_message(IN libusb_device_handle *device_handle, IN unsigned char * host_data_out, IN unsigned int host_data_out_len, OUT int * host_data_out_bytes_sent, OUT unsigned char * host_data_in, OUT int * host_data_in_bytes_received) +{ + int result = 0; + + *host_data_out_bytes_sent = 0; + *host_data_in_bytes_received = 0; + + /* Write data to the device */ + result = libusb_bulk_transfer(device_handle, BULK_OUT_ENDPOINT, host_data_out, host_data_out_len, host_data_out_bytes_sent, TIMEOUT_MS); + if (result >= 0) + { + printf("Message sent via bulk transfer:\n"); + print_data(host_data_out, *host_data_out_bytes_sent); + + /* Read data from the device */ + result = libusb_bulk_transfer(device_handle, BULK_IN_ENDPOINT, host_data_in, BULK_IN_MAX_TRANSFER_SIZE, host_data_in_bytes_received, TIMEOUT_MS); + if (result >= 0) + { + if (*host_data_in_bytes_received > 0) + { + printf("Message received via bulk transfer:\n"); + print_data(host_data_in, *host_data_in_bytes_received); + } + else + { + fprintf(stderr, "No message received via bulk transfer (%d)\n", result); + result = LIBUSB_ERROR_OTHER; + } + } + else + { + fprintf(stderr, "Error receiving message via bulk transfer (%s)\n", libusb_error_name(result)); + } + } + else + { + fprintf(stderr, "Error sending message via bulk transfer (%s)\n", libusb_error_name(result)); + } + + return result; +} + + +/* Description: This function uses synchronous control transfers to write data to the device */ +/* Parameters: IN device_handle - USB device handle */ +/* IN host_data_out - Host buffer with the message to send */ +/* IN host_data_out_len - Host buffer length */ +/* OUT host_data_out_bytes_sent - Actual buffer size transferred */ +/* Returns: Zero on success, libusb_error code on failure */ +int control_transfer_write_message(IN libusb_device_handle *device_handle, IN unsigned char * host_data_out, IN unsigned int host_data_out_len, OUT int * host_data_out_bytes_sent) +{ + int result = 0; + + *host_data_out_bytes_sent = 0; + + /* Write data to the device */ + result = libusb_control_transfer(device_handle, CONTROL_OUT_REQUEST_TYPE, CONTROL_REQUEST_1, 0, INTERFACE_NUMBER, host_data_out, host_data_out_len, TIMEOUT_MS); + if (result >= 0) + { + *host_data_out_bytes_sent = result; + printf("Message sent via control transfer:\n"); + print_data(host_data_out, *host_data_out_bytes_sent); + } + else + { + fprintf(stderr, "Error sending data via control transfer (%s)\n", libusb_error_name(result)); + } + + return result; +} + + +/* Description: This function uses synchronous control transfers to receive data from the device */ +/* Parameters: IN device_handle - USB device handle */ +/* OUT host_data_in - Host buffer to save the message */ +/* OUT host_data_in_bytes_received - Actual buffer size received */ +/* Returns: Zero on success, libusb_error code on failure */ +int control_transfer_read_message(IN libusb_device_handle *device_handle, OUT unsigned char * host_data_in, OUT int * host_data_in_bytes_received) +{ + int result = 0; + + *host_data_in_bytes_received = 0; + + /* Read message from the device */ + result = libusb_control_transfer(device_handle, CONTROL_IN_REQUEST_TYPE, CONTROL_REQUEST_2, 0, INTERFACE_NUMBER, host_data_in, CONTROL_IN_MAX_TRANSFER_SIZE, TIMEOUT_MS); + if (result >= 0) + { + *host_data_in_bytes_received = result; + printf("Message received via control transfer:\n"); + print_data(host_data_in, *host_data_in_bytes_received); + } + else + { + fprintf(stderr, "Error receiving message via control transfer (%s)\n", libusb_error_name(result)); + } + + return result; +} + + +/* Description: This function uses synchronous control transfers to write data to the device and receive data from the device */ +/* Parameters: IN device_handle - USB device handle */ +/* IN host_data_out - Host buffer with the message to send */ +/* IN host_data_out_len - Host buffer length */ +/* OUT host_data_out_bytes_sent - Actual buffer size transferred */ +/* OUT host_data_in - Host buffer to save the message */ +/* OUT host_data_in_bytes_received - Actual buffer size received */ +/* Returns: Zero on success, libusb_error code on failure */ +int control_transfer_exchange_message(IN libusb_device_handle *device_handle, IN unsigned char * host_data_out, IN unsigned int host_data_out_len, OUT int * host_data_out_bytes_sent, OUT unsigned char * host_data_in, OUT int * host_data_in_bytes_received) +{ + int result = 0; + + *host_data_out_bytes_sent = 0; + *host_data_in_bytes_received = 0; + + /* Send message to the device */ + result = libusb_control_transfer(device_handle, CONTROL_OUT_REQUEST_TYPE, CONTROL_REQUEST_1, 0, INTERFACE_NUMBER, host_data_out, host_data_out_len, TIMEOUT_MS); + if (result >= 0) + { + *host_data_out_bytes_sent = result; + printf("Message sent via control transfer:\n"); + print_data(host_data_out, *host_data_out_bytes_sent); + + /* Read message from the device */ + result = libusb_control_transfer(device_handle, CONTROL_IN_REQUEST_TYPE, CONTROL_REQUEST_2, 0, INTERFACE_NUMBER, host_data_in, CONTROL_IN_MAX_TRANSFER_SIZE, TIMEOUT_MS); + if (result >= 0) + { + *host_data_in_bytes_received = result; + printf("Message received via control transfer:\n"); + print_data(host_data_in, *host_data_in_bytes_received); + } + else + { + fprintf(stderr, "Error receiving message via control transfer (%s)\n", libusb_error_name(result)); + } + } + else + { + fprintf(stderr, "Error sending data via control transfer (%s)\n", libusb_error_name(result)); + } + + return result; +} + + + +/* Description: This function uses synchronous interrupt transfers to write data to the device */ +/* Parameters: IN device_handle - USB device handle */ +/* IN host_data_out - Host buffer with the message to send */ +/* IN host_data_out_len - Host buffer length */ +/* OUT host_data_out_bytes_sent - Actual buffer size transferred */ +/* Returns: Zero on success, libusb_error code on failure */ +int interrupt_transfer_write_message(IN libusb_device_handle *device_handle, IN unsigned char * host_data_out, IN unsigned int host_data_out_len, OUT int * host_data_out_bytes_sent) +{ + int result = 0; + + *host_data_out_bytes_sent = 0; + + /* Send message to the device */ + result = libusb_interrupt_transfer(device_handle, INTERRUPT_OUT_ENDPOINT, host_data_out, host_data_out_len, host_data_out_bytes_sent, TIMEOUT_MS); + if (result >= 0) + { + printf("Message sent via interrupt transfer:\n"); + print_data(host_data_out, *host_data_out_bytes_sent); + } + else + { + fprintf(stderr, "Error sending message via interrupt transfer (%s)\n", libusb_error_name(result)); + } + + return result; +} + + +/* Description: This function uses synchronous interrupt transfers to receive data from the device */ +/* Parameters: IN device_handle - USB device handle */ +/* OUT host_data_in - Host buffer to save the message */ +/* OUT host_data_in_bytes_received - Actual buffer size received */ +/* Returns: Zero on success, libusb_error code on failure */ +int interrupt_transfer_read_message(IN libusb_device_handle *device_handle, OUT unsigned char * host_data_in, OUT int * host_data_in_bytes_received) +{ + int result = 0; + + *host_data_in_bytes_received = 0; + + /* Read message from the device */ + result = libusb_interrupt_transfer(device_handle, INTERRUPT_IN_ENDPOINT, host_data_in, INTERRUPT_OUT_MAX_TRANSFER_SIZE, host_data_in_bytes_received, TIMEOUT_MS); + if (result >= 0) + { + if (*host_data_in_bytes_received > 0) + { + printf("Message received via interrupt transfer:\n"); + print_data(host_data_in, *host_data_in_bytes_received); + } + else + { + fprintf(stderr, "No message received in interrupt transfer (%s)\n", libusb_error_name(result)); + result = LIBUSB_ERROR_OTHER; + } + } + else + { + fprintf(stderr, "Error receiving message via interrupt transfer (%s)\n", libusb_error_name(result)); + } + + return result; +} + + +/* Description: This function uses synchronous interrupt transfers to write data to the device and receive data from the device */ +/* Parameters: IN device_handle - USB device handle */ +/* IN host_data_out - Host buffer with the message to send */ +/* IN host_data_out_len - Host buffer length */ +/* OUT host_data_out_bytes_sent - Actual buffer size transferred */ +/* OUT host_data_in - Host buffer to save the message */ +/* OUT host_data_in_bytes_received - Actual buffer size received */ +/* Returns: Zero on success, libusb_error code on failure */ +int interrupt_transfer_exchange_message(IN libusb_device_handle *device_handle, IN unsigned char * host_data_out, IN unsigned int host_data_out_len, OUT int * host_data_out_bytes_sent, OUT unsigned char * host_data_in, OUT int * host_data_in_bytes_received) +{ + int result = 0; + + *host_data_out_bytes_sent = 0; + *host_data_in_bytes_received = 0; + + /* Send message to the device */ + result = libusb_interrupt_transfer(device_handle, INTERRUPT_OUT_ENDPOINT, host_data_out, host_data_out_len, host_data_out_bytes_sent, TIMEOUT_MS); + if (result >= 0) + { + printf("Message sent via interrupt transfer:\n"); + print_data(host_data_out, *host_data_out_bytes_sent); + + /* Read message from the device */ + result = libusb_interrupt_transfer(device_handle, INTERRUPT_IN_ENDPOINT, host_data_in, INTERRUPT_OUT_MAX_TRANSFER_SIZE, host_data_in_bytes_received, TIMEOUT_MS); + if (result >= 0) + { + if (*host_data_in_bytes_received > 0) + { + printf("Message received via interrupt transfer:\n"); + print_data(host_data_in, *host_data_in_bytes_received); + } + else + { + fprintf(stderr, "No message received in interrupt transfer (%s)\n", libusb_error_name(result)); + result = LIBUSB_ERROR_OTHER; + } + } + else + { + fprintf(stderr, "Error receiving message via interrupt transfer (%s)\n", libusb_error_name(result)); + } + } + else + { + fprintf(stderr, "Error sending message via interrupt transfer (%s)\n", libusb_error_name(result)); + } + + return result; +} + diff --git a/third-party/libtm/libtm/src/Protocol.h b/third-party/libtm/libtm/src/Protocol.h new file mode 100644 index 0000000000..1dc28c7ce3 --- /dev/null +++ b/third-party/libtm/libtm/src/Protocol.h @@ -0,0 +1,31 @@ +/******************************************************************************* +INTEL CORPORATION PROPRIETARY INFORMATION +Copyright(c) 2017 Intel Corporation. All Rights Reserved. +*******************************************************************************/ + +#ifndef __TM2_PROTOCOL_H +#define __TM2_PROTOCOL_H + +#include +#include "Common.h" + +#define INTERFACE_NUMBER 0 + +/* Synchronous device I/O */ +int bulk_transfer_write_message(IN libusb_device_handle * device_handle, IN unsigned char * host_data_out, IN unsigned int host_data_out_len, OUT int * host_data_out_bytes_sent); +int bulk_transfer_read_message(IN libusb_device_handle * device_handle, OUT unsigned char * host_data_in, OUT int * host_data_in_bytes_received); +int bulk_transfer_exchange_message(IN libusb_device_handle *device_handle, IN unsigned char * host_data_out, IN unsigned int host_data_out_len, OUT int * host_data_out_bytes_sent, OUT unsigned char * host_data_in, OUT int * host_data_in_bytes_received); + +int control_transfer_write_message(IN libusb_device_handle *device_handle, IN unsigned char * host_data_out, IN unsigned int host_data_out_len, OUT int * host_data_out_bytes_sent); +int control_transfer_read_message(IN libusb_device_handle *device_handle, OUT unsigned char * host_data_in, OUT int * host_data_in_bytes_received); +int control_transfer_exchange_message(IN libusb_device_handle *device_handle, IN unsigned char * host_data_out, IN unsigned int host_data_out_len, OUT int * host_data_out_bytes_sent, OUT unsigned char * host_data_in, OUT int * host_data_in_bytes_received); + +int interrupt_transfer_write_message(IN libusb_device_handle *device_handle, IN unsigned char * host_data_out, IN unsigned int host_data_out_len, OUT int * host_data_out_bytes_sent); +int interrupt_transfer_read_message(IN libusb_device_handle *device_handle, OUT unsigned char * host_data_in, OUT int * host_data_in_bytes_received); +int interrupt_transfer_exchange_message(IN libusb_device_handle *device_handle, IN unsigned char * host_data_out, IN unsigned int host_data_out_len, OUT int * host_data_out_bytes_sent, OUT unsigned char * host_data_in, OUT int * host_data_in_bytes_received); + +/* Asynchronous device I/O */ +/* TBD */ + +#endif // __TM2_PROTOCOL_H + diff --git a/third-party/libtm/libtm/src/UsbPlugListener.cpp b/third-party/libtm/libtm/src/UsbPlugListener.cpp new file mode 100644 index 0000000000..b42d765425 --- /dev/null +++ b/third-party/libtm/libtm/src/UsbPlugListener.cpp @@ -0,0 +1,147 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#define LOG_TAG "UsbListener" +#define LOG_NDEBUG 0 /* Enable LOGV */ + +#include "UsbPlugListener.h" +#include +#include +#include "Manager.h" + +using namespace std::chrono; + +typedef enum { + USB_1_0 = 0x0100, // USB 1.0 (Low Speed 1.5 Mbps) + USB_1_1 = 0x0110, // USB 1.1 (Full Speed 12 Mbps) + USB_2_0 = 0x0200, // USB 2.0 (High Speed 480 Mbps) + USB_2_1 = 0x0210, // USB 2.1 (High Speed 480 Mbps) + USB_3_0 = 0x0300, // USB 3.0 (Super Speed 5.0 Gbps) + USB_3_1 = 0x0310, // USB 3.1 (Super Speed + 10.0 Gbps) +} USB_SPECIFICIATION_VERSION; + +perc::UsbPlugListener::UsbPlugListener(Owner& owner) : mOwner(owner), mMessage(0) +{ + // schedule the listener itself for every 100 milliseconds + mOwner.dispatcher().scheduleTimer(this, ENUMERATE_TIMEOUT_NSEC, mMessage); +} + +void perc::UsbPlugListener::onTimeout(uintptr_t timerId, const Message & msg) +{ + EnumerateDevices(); + + /* Reschedule the listener itself for every 100 milliseconds */ + mOwner.dispatcher().scheduleTimer(this, ENUMERATE_TIMEOUT_NSEC, mMessage); +} + +bool perc::UsbPlugListener::identifyDevice(libusb_device_descriptor* desc) +{ + if ((desc->idProduct == TM2_DEV_PID) && (desc->idVendor == TM2_DEV_VID) && (desc->bcdUSB >= USB_2_0)) + { + return true; + } + + return false; +} + +bool perc::UsbPlugListener::identifyUDFDevice(libusb_device_descriptor* desc) +{ + if ((desc->idVendor == TM2_UDF_DEV_VID) && (desc->idProduct == TM2_UDF_DEV_PID) && (desc->bcdUSB >= USB_2_0)) + { + return true; + } + + return false; +} + +perc::UsbPlugListener::~UsbPlugListener() +{ + // todo +} + +const char* perc::UsbPlugListener::usbSpeed(uint16_t bcdUSB) +{ + switch (bcdUSB) + { + case USB_1_0: return "USB 1.0 (Low Speed 1.5 Mbps)"; + case USB_1_1: return "USB 1.1 (Full Speed 12 Mbps)"; + case USB_2_0: return "USB 2.0 (High Speed 480 Mbps)"; + case USB_2_1: return "USB 2.1 (High Speed 480 Mbps)"; /* USB 3.0 device connected to USB 2.0 HUB */ + case USB_3_0: return "USB 3.0 (Super Speed 5.0 Gbps)"; + case USB_3_1: return "USB 3.1 (Super Speed+ 10.0 Gbps)"; + } + return "Unknown USB speed"; +} + + +void perc::UsbPlugListener::EnumerateDevices() +{ + libusb_device **list = NULL; + int rc = 0; + ssize_t count = 0; + + count = libusb_get_device_list(NULL, &list); + + high_resolution_clock::time_point t1 = high_resolution_clock::now(); + + // mark all devices as not found to track removed devices + for (auto it = mDeviceToPortMap.begin(); it != mDeviceToPortMap.end(); it++) + { + it->second = false; + } + + for (ssize_t idx = 0; idx < count; ++idx) + { + libusb_device *device = list[idx]; + libusb_device_descriptor desc = { 0 }; + + rc = libusb_get_device_descriptor(device, &desc); + + LOGV("%d: VID 0x%04X, PID 0x%04X, bcdUSB 0x%x, iSerialNumber %d", idx, desc.idVendor, desc.idProduct, desc.bcdUSB, desc.iSerialNumber); + + if ((identifyDevice(&desc) == true) || (identifyUDFDevice(&desc) == true)) + { + std::lock_guard lk(mDeviceToPortMapLock); + DeviceToPortMap devicePort(device); + Status st = Status::SUCCESS; + + if (mDeviceToPortMap.find(devicePort) == mDeviceToPortMap.end()) + { + LOGD("Found USB device %s on port %d: VID 0x%04X, PID 0x%04X, %s",(desc.idVendor == TM2_UDF_DEV_VID)?"Movidius":"T250", devicePort.portChain[0], desc.idVendor, desc.idProduct, usbSpeed(desc.bcdUSB)); + st = mOwner.onAttach(device); + } + + // just mark the device as found in this list + if (st == Status::SUCCESS) + { + mDeviceToPortMap[devicePort] = true; + } + } + } + + // look for unplugged devices (==not found in the list) + std::list devicesToDelete; + for (auto item : mDeviceToPortMap) + { + if (item.second == false) // mark the device for deletion + { + devicesToDelete.push_back(item.first); + } + } + + for (auto devicePort : devicesToDelete) // ... and delete it + { + // send message to client handler ON_DETACH + std::lock_guard lock(mDeviceToPortMapLock); + mDeviceToPortMap.erase(devicePort); + mOwner.onDetach(devicePort.libusbDevice); + } + + high_resolution_clock::time_point t2 = high_resolution_clock::now(); + auto duration = duration_cast(t2 - t1).count(); + + LOGV("Devices connected: %d. Time: %d micro-seconds", mDeviceToPortMap.size(), duration); + + // free the list + libusb_free_device_list(list, 1); +} diff --git a/third-party/libtm/libtm/src/UsbPlugListener.h b/third-party/libtm/libtm/src/UsbPlugListener.h new file mode 100644 index 0000000000..d44f0c51e5 --- /dev/null +++ b/third-party/libtm/libtm/src/UsbPlugListener.h @@ -0,0 +1,109 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include +#include +#include +#include "Dispatcher.h" +#include "Fsm.h" +#include "Device.h" +#include "TrackingCommon.h" +#include "TrackingData.h" +#include +#include +#include + +#define TM2_DEV_VID 0x8087 +#define TM2_DEV_PID 0x0AF3 +#define TM2_UDF_DEV_VID 0x03E7 +#define TM2_UDF_DEV_PID 0x2150 +#define ENUMERATE_TIMEOUT_NSEC 500000000 + + +namespace perc +{ + class UsbPlugListener : public EventHandler + { + public: + class Owner { + public: + virtual ~Owner() {}; + virtual Status onAttach(libusb_device*) = 0; + virtual void onDetach(libusb_device*) = 0; + virtual libusb_context* context() = 0; + virtual Dispatcher& dispatcher() = 0; + }; + + public: + UsbPlugListener(Owner& owner); + ~UsbPlugListener(); + + bool identifyDevice(libusb_device_descriptor* desc); + bool identifyUDFDevice(libusb_device_descriptor* desc); + + // [interface] EventHandler + virtual void onTimeout(uintptr_t timerId, const Message &msg); + static const char *usbSpeed(uint16_t bcdUSB); + + private: + void EnumerateDevices(); + Message mMessage; + Owner& mOwner; + class DeviceToPortMap + { + public: + DeviceToPortMap() : libusbDevice(NULL), portChainDepth(0), portChain{0} {}; + DeviceToPortMap(libusb_device* _libusbDevice) : portChain{ 0 } { + libusbDevice = _libusbDevice; + portChainDepth = libusb_get_port_numbers(libusbDevice, portChain, MAX_USB_TREE_DEPTH); + }; + + + libusb_device* libusbDevice; + uint32_t portChainDepth; + uint8_t portChain[MAX_USB_TREE_DEPTH]; + + bool operator<(const DeviceToPortMap &ref) const + { + if (this->libusbDevice < ref.libusbDevice) + { + return true; + } + else if (this->libusbDevice > ref.libusbDevice) + { + return false; + } + + /* this->libusbDevice == ref.libusbDevice */ + if (this->portChainDepth < ref.portChainDepth) + { + return true; + } + else if (this->portChainDepth > ref.portChainDepth) + { + return false; + } + + /* this->libusbDevice == ref.libusbDevice */ + /* this->portChainDepth == ref.portChainDepth */ + for (uint32_t i = 0; i < this->portChainDepth; i++) + { + if (this->portChain[i] < ref.portChain[i]) + { + return true; + } + else if (this->portChain[i] > ref.portChain[i]) + { + return false; + } + } + + return false; + } + + }; + std::map mDeviceToPortMap; + std::mutex mDeviceToPortMapLock; + }; +} diff --git a/third-party/libtm/libtm/src/Version.h.in b/third-party/libtm/libtm/src/Version.h.in new file mode 100644 index 0000000000..2333df2db7 --- /dev/null +++ b/third-party/libtm/libtm/src/Version.h.in @@ -0,0 +1,33 @@ +// INTEL CORPORATION PROPRIETARY INFORMATION +// This software is supplied under the terms of a license agreement or nondisclosure +// agreement with Intel Corporation and may not be copied or disclosed except in +// accordance with the terms of that agreement +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +// Version.h file is an auto-generated file made by CMake from file Version.h.in. +// Changes to Version.h are likely to be overwritten and lost + +#pragma once +#define LIBTM_VERSION_MAJOR @LIBTM_VERSION_MAJOR@ +#define LIBTM_VERSION_MINOR @LIBTM_VERSION_MINOR@ +#define LIBTM_VERSION_PATCH @LIBTM_VERSION_PATCH@ +#define LIBTM_VERSION_BUILD @LIBTM_VERSION_BUILD@ + +#define LIBTM_API_VERSION_MAJOR @LIBTM_API_VERSION_MAJOR@ +#define LIBTM_API_VERSION_MINOR @LIBTM_API_VERSION_MINOR@ + +#define LIBTM_VERSION_BUILD_POS 0 +#define LIBTM_VERSION_BUILD_MSK 0xFFFFFFFF +#define LIBTM_VERSION_PATCH_POS 32 +#define LIBTM_VERSION_PATCH_MSK 0xFFFF +#define LIBTM_VERSION_MINOR_POS 48 +#define LIBTM_VERSION_MINOR_MSK 0xFF +#define LIBTM_VERSION_MAJOR_POS 56 +#define LIBTM_VERSION_MAJOR_MSK 0xFF + +#define LIBTM_VERSION (((uint64_t)(LIBTM_VERSION_MAJOR & LIBTM_VERSION_MAJOR_MSK) << LIBTM_VERSION_MAJOR_POS) | \ + ((uint64_t)(LIBTM_VERSION_MINOR & LIBTM_VERSION_MINOR_MSK) << LIBTM_VERSION_MINOR_POS) | \ + ((uint64_t)(LIBTM_VERSION_PATCH & LIBTM_VERSION_PATCH_MSK) << LIBTM_VERSION_PATCH_POS) | \ + ((uint64_t)(LIBTM_VERSION_BUILD & LIBTM_VERSION_BUILD_MSK) << LIBTM_VERSION_BUILD_POS)) + +#define LIBTM_BRANCH @GIT_BRANCH@ diff --git a/third-party/libtm/libtm/src/infra/Dispatcher.cpp b/third-party/libtm/libtm/src/infra/Dispatcher.cpp new file mode 100644 index 0000000000..340892bc51 --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Dispatcher.cpp @@ -0,0 +1,409 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#define LOG_TAG "Dispatcher" +#define LOG_NDEBUG 1 // controls LOGV only +#include "Log.h" +#include "Dispatcher.h" + + +namespace perc { +// ---------------------------------------------------------------------------- + Dispatcher::Dispatcher() : mThreadId(), m_Handlers(), m_HandlersGuard(), m_MessagesGuard(), m_Timers(), m_TimersGuard(), m_HoldersGuard(), m_Holders() +{ + bool ret = mPoller.add(Poller::event(mEvent.handle(), Poller::READ_MASK, this)); + if (ret != true) + { + ASSERT(ret); + } +} + +// ---------------------------------------------------------------------------- +Dispatcher::~Dispatcher() +{ + processExit(); + { + std::lock_guard guard(m_MessagesGuard); + for (int i = 0; i < PRIORITY_MAX; i++) { + Holder *holder = (Holder *)m_Messages[i].GetHead(); + while (holder) { + do { + m_Messages[i].RemoveHead(); + delete holder; + } while ((holder = (Holder *)m_Messages[i].GetHead()) != 0); + } + } + } + { + std::lock_guard guard(m_HandlersGuard); + m_Handlers.clear(); + } + { + std::lock_guard guard(m_TimersGuard); + Holder *holder = (Holder *)m_Timers.GetHead(); + while (holder) { + do { + m_Timers.RemoveHead(); + delete holder; + } while ((holder = (Holder *)m_Timers.GetHead()) != 0); + } + } + mPoller.remove(mEvent.handle()); +} + +// ---------------------------------------------------------------------------- +// DISPATCHER LOOP +// +// Returns: +// >0 - the total number of timers, I/Oevents and messages that were dispatched in single call +// 0 - if the elapsed without dispatching any handlers +// -1 - if an error occurs +int Dispatcher::handleEvents(nsecs_t timeout) +{ + if (mExitPending) { + LOGV("handleEvents(): processExit"); + processExit(); + return -1; + } + int ret = 0; + mThreadId = std::this_thread::get_id(); + LOGV("handleEvents(): Poller::poll()"); + Poller::event event; + int n = mPoller.poll(event, calculatePollTimeout(timeout)); + LOGV("handleEvents(): Poller::poll() ret %d", n); + if (n > 0) { + // process message queues: complete number of messages from message list that was signaled by event + if (event.handle == mEvent.handle()) { + LOGV("handleEvents(): processMessages"); + ret += processMessages(); + } + else { + // process file descriptor + LOGV("handleEvents(): processEvent fd %d, revents %x", event.handle, event.mask); + ret += processEvents(event); + } + } + else if (n == -1) { + LOGE("handleEvents(): Poller::poll() ret %d", n); + return -1; + } + // process timers + ret += processTimers(); + return ret; +} + +// ---------------------------------------------------------------------------- +void Dispatcher::endEventsLoop() +{ +// TRACE(""); + mExitPending = true; + wakeup(); +} + +// ---------------------------------------------------------------------------- +int Dispatcher::registerHandler(EventHandler *handler, Handle fd, unsigned long mask, void *act) +{ + if (mExitPending) return -1; + ASSERT(handler); + ASSERT(mThreadId == std::this_thread::get_id()); + int res = -1; + HandlerHolder holder(handler, fd, mask, act); + if (mPoller.add(holder.Event)) { + std::lock_guard guard(m_HandlersGuard); + if (m_Handlers.count(fd) == 0) { + // adding new one + m_Handlers.emplace(fd, holder); + } + else { + // modifying old one + m_Handlers[fd] = holder; + } + res = 0; + } + return res; +} + +// ---------------------------------------------------------------------------- +int Dispatcher::registerHandler(EventHandler *handler) +{ + if (mExitPending) return -1; + ASSERT(handler); + std::lock_guard guard(m_HoldersGuard); + EmbeddedList::Iterator it(m_Holders); + Holder *h = (Holder *)it.Current(); + // check if handler exists in a list + while(h) { + if (h->Handler == handler) + return -1; + h = (Holder *)it.Next(); + } + Holder *holder = new Holder(handler); + if (!holder) return -1; + m_Holders.AddTail(holder); + return 0; +} + +// ---------------------------------------------------------------------------- +void Dispatcher::removeHandle(Handle fd) +{ + ASSERT(mThreadId == std::this_thread::get_id()); + mPoller.remove(fd); + std::lock_guard guard(m_HandlersGuard); + if (m_Handlers.count(fd) > 0) { + m_Handlers.erase(fd); + } +} + +// ---------------------------------------------------------------------------- +void Dispatcher::cancelTimer (uintptr_t timerId) +{ + ASSERT(timerId); + ASSERT(mThreadId == std::this_thread::get_id()); + std::lock_guard guard(m_TimersGuard); + HolderTimer *holder = (HolderTimer *)timerId; + // check if trying to cancel timer from callback + if (holder->Uptime) { + m_Timers.Remove(holder); + delete holder; + } +} + +// ---------------------------------------------------------------------------- +int Dispatcher::removeHandler(EventHandler *handler, unsigned int mask) +{ + ASSERT(handler); + ASSERT(mThreadId == std::this_thread::get_id()); + int removedHandlers = 0; + + if (mask & MESSAGES_MASK) { + std::lock_guard guard(m_MessagesGuard); + for (int i = 0; i < PRIORITY_MAX; i++) { + EmbeddedList::Iterator it(m_Messages[i]); + Holder *holder = (Holder *)it.Current(); + while (holder) { + Holder *curr = holder; + holder = (Holder *)it.Next(); + if (curr->Handler == handler) { + m_Messages[i].Remove(curr); + delete curr; + removedHandlers++; + } + } + } + } + if (mask & HANDLES_MASK) { + std::lock_guard guard(m_HandlersGuard); + for (auto pair : m_Handlers) { + const HandlerHolder &holder = pair.second; + if (holder.Handler == handler) { + Handle fd = pair.first; + mPoller.remove(fd); + m_Handlers.erase(fd); + removedHandlers++; + } + } + } + if (mask & TIMERS_MASK) { + std::lock_guard guard(m_TimersGuard); + EmbeddedList::Iterator it(m_Timers); + Holder *holder = (Holder *)it.Current(); + while (holder) { + Holder *curr = holder; + holder = (Holder *)it.Next(); + if (curr->Handler == handler) { + m_Timers.Remove(curr); + delete curr; + removedHandlers++; + } + } + } + if (mask & EXIT_MASK) { + std::lock_guard guard(m_HoldersGuard); + EmbeddedList::Iterator it(m_Holders); + Holder *holder = (Holder *)it.Current(); + while (holder) { + Holder *curr = holder; + holder = (Holder *)it.Next(); + if (curr->Handler == handler) { + m_Holders.Remove(curr); + delete curr; + removedHandlers++; + break; + } + } + } + + return removedHandlers; +} + +// ---------------------------------------------------------------------------- +int Dispatcher::putMessage(Holder *holder, int priority) +{ + if (mExitPending) return -1; + ASSERT(holder); + if (priority >= PRIORITY_MAX) priority = PRIORITY_MAX - 1; + if (priority < 0) priority = 0; + std::lock_guard guard(m_MessagesGuard); + m_Messages[priority].AddTail(holder); + // wake up running loop + if (!wakeup()) { + m_Messages[priority].Remove(holder); + delete holder; + return -1; + } + return 0; +} + +// ---------------------------------------------------------------------------- +uintptr_t Dispatcher::putTimer(EventHandler *handler, nsecs_t delay, Message *msg, int priority) +{ + // sanity check + if ((mExitPending == true) || (msg == NULL)) + { + return 0; + } + + ASSERT(handler && delay != 0); + // TODO: optimization: implement free list with local allocator + HolderTimer *holder = new HolderTimer(handler, msg, delay); + if (!holder) + { + delete msg; + return 0; + } + std::lock_guard guard(m_TimersGuard); + EmbeddedList::Iterator it(m_Timers); + HolderTimer *curr = (HolderTimer *)it.Current(); + if (!curr) { + m_Timers.AddHead(holder); + } + else { + do { + if (holder->Uptime < curr->Uptime) { + m_Timers.AddBefore(curr, holder); + break; + } + } while ((curr = (HolderTimer *)it.Next()) != 0); + if (!curr) { + m_Timers.AddTail(holder); + } + } + // reschedule timers by wake up running loop if was performed from another context + if (mThreadId != std::this_thread::get_id()) + wakeup(); + return (uintptr_t)holder; +} + +// ---------------------------------------------------------------------------- +int Dispatcher::processMessages() +{ + mEvent.reset(); + // normalize wake-up events counter and real messages number before callback + int cnt = 0; + for (int i = 0; i < PRIORITY_MAX; i++) { + cnt += m_Messages[i].Size(); + } + int cntMsgs = cnt; + while (cnt) { + int priority = 0; + for (int i = (PRIORITY_MAX - 1); i >= 0; i--) { + if (m_Messages[i].Size()) { + priority = i; + break; + } + } + m_MessagesGuard.lock(); + Holder *holder = (Holder *)m_Messages[priority].RemoveHead(); + m_MessagesGuard.unlock(); + if (holder) { + holder->complete(); + delete holder; + } + else { + break; + } + cnt--; + } + return cntMsgs; +} + +// ---------------------------------------------------------------------------- +int Dispatcher::processEvents(Poller::event &event) +{ + m_HandlersGuard.lock(); + auto it = m_Handlers.find(event.handle); + if (it != m_Handlers.end()) { + const HandlerHolder &holder = it->second; + m_HandlersGuard.unlock(); + holder.Handler->onEvent(holder.Event.handle, event.mask, holder.Event.act); + return 1; + } + else { + mPoller.remove(event.handle); + m_HandlersGuard.unlock(); + } + return 0; +} + +// ---------------------------------------------------------------------------- +int Dispatcher::processTimers() +{ + int cnt = 0; + HolderTimer* timer; + m_TimersGuard.lock(); + timer = (HolderTimer *)m_Timers.GetHead(); + if (timer) { + // work with head only, user can add new timers in a callback + do { + nsecs_t now = systemTime(); + if (timer->Uptime <= now) { + m_Timers.RemoveHead (); + m_TimersGuard.unlock(); + timer->complete(); + delete timer; + m_TimersGuard.lock(); + cnt++; + } + else { + break; + } + } while ((timer = (HolderTimer *)m_Timers.GetHead()) != 0); + } + m_TimersGuard.unlock(); + return cnt; +} + +// ---------------------------------------------------------------------------- +int Dispatcher::processExit() +{ + m_HoldersGuard.lock(); + Holder *holder; + while (holder = (Holder *)m_Holders.RemoveHead()) { + m_HoldersGuard.unlock(); + holder->Handler->onExit(); + delete holder; + m_HoldersGuard.lock(); + } + m_HoldersGuard.unlock(); + return 0; +} + +// ---------------------------------------------------------------------------- +int Dispatcher::calculatePollTimeout(nsecs_t timeout) +{ + // calculate next wake-up time according to timers queue or user timeout + std::lock_guard guard(m_TimersGuard); + HolderTimer *timer = (HolderTimer *)m_Timers.GetHead(); + if (timer) { + int t1 = toMillisecondTimeoutDelay(systemTime(), timer->Uptime); + int t2 = ns2ms(timeout); + return ((unsigned long)t2 == INFINITE)? t1 : (t1 > t2 ? t2 : t1); + } + + // No timer in timer queue, returning timeout input in msec + return ns2ms(timeout); +} + + +// ---------------------------------------------------------------------------- +} // namespace perc diff --git a/third-party/libtm/libtm/src/infra/Dispatcher.h b/third-party/libtm/libtm/src/infra/Dispatcher.h new file mode 100644 index 0000000000..1a5317f6e0 --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Dispatcher.h @@ -0,0 +1,251 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "Utils.h" +#include "Event.h" +#include "Fence.h" +#include "EmbeddedList.h" +#include "EventHandler.h" +#include "Poller.h" + +namespace perc { +// ---------------------------------------------------------------------------- +/// +/// @class Dispatcher +/// +/// @brief Provides a event demultiplexing mechanism for messages, timers +/// and file descriptors events. +/// +// ---------------------------------------------------------------------------- +class Dispatcher +{ +public: + Dispatcher(); + ~Dispatcher(); + + // Message notification mechanism + enum + { + PRIORITY_IDLE = 0, // Lowest priority + PRIORITY_NORMAL, + PRIORITY_HIGH, + PRIORITY_MAX, + }; + + // = Post/Send message + // + // User can call to this function from any running context. + // Call to these functions will trigger callback. + // Post message to via the Dispatcher's notification mechanism, don't wait for execution. + // This method will internally copy message (using copy constructor) and free after dispatching. + template + int postMessage(EventHandler *, const T&, int priority = Dispatcher::PRIORITY_IDLE); + + // Send message to via the Dispatcher's notification mechanism and blocking wait for execution. + // User SHOULD call to this function from different running context not one. + // This function waits to the message processing by client and returns after returns + // from callback. + // parameter is transferred by reference to callback - user may indicate return parameters if needed. + // Don't call to this function in the same running context. + template + int sendMessage(EventHandler *, const T&, int priority = Dispatcher::PRIORITY_IDLE); + + // = Register/remove handler for I/O and OS events. + // + // A handler can be associated with multiple handles. + // A handle cannot be associated with multiple handlers. + // User can call to this function from any running context. + // Call to these functions will trigger callback, + // according to parameter + int registerHandler(EventHandler *, Handle fd, unsigned long mask = Poller::READ_MASK, void *act = 0); + + // Register handler for Dispatcher exit processing, callback should be called + // during Disptcher deactivation. + int registerHandler(EventHandler *); + + // Remove handle from dispatcher and it's associated handler. + // User SHOULD call these functions from dispatcher context only (from callbacks)!!! + void removeHandle(Handle fd); + + // Remove all messages (and release them), handles and timers (cancel them) associated with this handler + // according to mask parameters. + // + // User SHOULD call these functions from dispatcher context only (from callbacks)!!! + enum + { + MESSAGES_MASK = (1<<0), + HANDLES_MASK = (1<<1), + TIMERS_MASK = (1<<2), + EXIT_MASK = (1<<3), + ALL_MASK = MESSAGES_MASK | HANDLES_MASK | TIMERS_MASK | EXIT_MASK, + }; + int removeHandler(EventHandler *, unsigned int mask = Dispatcher::ALL_MASK); + + // = Timers. + // + // @params: delayMs Time interval in nanoseconds after which the timer will expire. + // @return or 0 in case of error. + template + uintptr_t scheduleTimer (EventHandler *, nsecs_t delay, const T&, int priority = Dispatcher::PRIORITY_IDLE); + + // Cancel timer associated with this . + // User SHOULD call these functions from dispatcher context only (from callbacks)!!! + void cancelTimer (uintptr_t timerId); + + // Main dispatch function + // + // Returns: + // >0 - the total number of timers, I/Oevents and messages that were dispatched in single call + // 0 - if the elapsed without dispatching any handlers + // -1 - if an error occurs + int handleEvents(nsecs_t timeout = INFINITE); + void endEventsLoop(); + +protected: + + // holder of messages base class + class Holder : public EmbeddedListElement + { + public: + Holder(EventHandler *h) : Handler(h) {} + virtual ~Holder() {} + virtual void complete() {} + EventHandler *Handler; + }; + +private: + std::thread::id mThreadId; + Event mEvent; + Poller mPoller; + bool mExitPending = false; + + int putMessage(Holder *, int priority); + uintptr_t putTimer (EventHandler *, nsecs_t delay, Message *, int priority); + int processMessages(); + int processEvents(Poller::event &); + int processTimers(); + int processExit(); + + /** + * @brief This function calculates the next wake-up time according to timers queue or user timeout + * + * @param[in] timeout - poll timeout in nsec. + * @return Next wake up time in msec. + */ + int calculatePollTimeout(nsecs_t); + bool wakeup() { return mEvent.signal(); } + + class HolderPost : public Holder + { + public: + HolderPost(EventHandler *h, Message *m) : Holder(h), Msg(m) {} + virtual ~HolderPost() { delete Msg; } + virtual void complete() { Handler->onMessage(*Msg); } + Message *Msg; + + /* Disallow these operations */ + HolderPost &operator=(const HolderPost &); + HolderPost(const HolderPost &); + }; + + class HolderSend : public Holder + { + public: + HolderSend(EventHandler *h, const Message &m, Fence &w) : + Holder(h), Msg(m), Waiter(w) {} + virtual ~HolderSend() { Waiter.notify(); } + virtual void complete() { Handler->onMessage(Msg); } + const Message &Msg; + Fence &Waiter; // sync wait lock handle + }; + EmbeddedList m_Messages[PRIORITY_MAX]; + std::mutex m_MessagesGuard; + + // I/O HANDLERS + struct HandlerHolder + { + EventHandler *Handler; + Poller::event Event; + HandlerHolder() : Handler(NULL) {} + HandlerHolder(EventHandler *handler, Handle fd, unsigned long mask, void *act) : + Handler(handler), Event(fd, mask, act) {} + + }; + std::unordered_map m_Handlers; + std::mutex m_HandlersGuard; + + // TIMERS + class HolderTimer : public HolderPost + { + public: + HolderTimer(EventHandler *h, Message *m, nsecs_t d) : HolderPost(h, m) + { Uptime = systemTime() + d; } + virtual void complete() { Uptime = 0; Handler->onTimeout((uintptr_t)this, *Msg); } + nsecs_t Uptime; + }; + EmbeddedList m_Timers; + std::mutex m_TimersGuard; + + // HANDLERS + EmbeddedList m_Holders; + std::mutex m_HoldersGuard; + + // Disallow these operations. + Dispatcher &operator=(const Dispatcher &); + Dispatcher(const Dispatcher &); +}; + + +// ---------------------------------------------------------------------------- +// Template functions MUST be implemented in header file with minimal code size +template +int Dispatcher::postMessage(EventHandler *handler, const T &msg, int priority) +{ + T::IS_DERIVED_FROM_Message; + if (!handler) return -1; + + T *m = new T(msg); // use copy constructor to create message clone + if (!m) return -ENOMEM; + + Holder *holder = new HolderPost(handler, m); + if (!holder) + { + delete m; + return -ENOMEM; + } + return putMessage(holder, priority); +} + +// ---------------------------------------------------------------------------- +template +int Dispatcher::sendMessage(EventHandler *handler, const T &msg, int priority) +{ + T::IS_DERIVED_FROM_Message; + ASSERT(mThreadId != std::this_thread::get_id()); + // use message reference + Fence f; + Holder *holder = new HolderSend(handler, msg, f); + if (!holder ) + return -ENOMEM; + + if (putMessage(holder, priority) < 0) + return -1; + + // wake for message completion or reset by dispatcher + f.wait(); + return 0; +} + +// ---------------------------------------------------------------------------- +template +uintptr_t Dispatcher::scheduleTimer (EventHandler *handler, nsecs_t delay, const T &msg, int priority) +{ + T::IS_DERIVED_FROM_Message; + return putTimer(handler, delay, new T(msg), priority); +} + + +// ---------------------------------------------------------------------------- +} // namespace perc diff --git a/third-party/libtm/libtm/src/infra/EmbeddedList.h b/third-party/libtm/libtm/src/infra/EmbeddedList.h new file mode 100644 index 0000000000..5e18777097 --- /dev/null +++ b/third-party/libtm/libtm/src/infra/EmbeddedList.h @@ -0,0 +1,187 @@ +/******************************************************************************* +INTEL CORPORATION PROPRIETARY INFORMATION +Copyright(c) 2017 Intel Corporation. All Rights Reserved. +*******************************************************************************/ + +#pragma once + + +namespace perc { +// ---------------------------------------------------------------------------- +/// +/// @class EmbeddedList +/// +/// @brief Doubly linked list. +/// +/// @note This implementation of a doubly linked list does not require +/// use of dynamically allocated memory. Instead, each class +/// that is a potential list element must inherite from a class +/// . All of the list functions operate on these +/// embedded element. +/// +// ---------------------------------------------------------------------------- +class EmbeddedListElement +{ +public: + EmbeddedListElement *Next () const {return m_EmbeddedListElementNext; } + void Next (EmbeddedListElement *Element) { m_EmbeddedListElementNext = Element; } + EmbeddedListElement *Prev () const {return m_EmbeddedListElementPrev; } + void Prev (EmbeddedListElement *Element) { m_EmbeddedListElementPrev = Element; } +private: + EmbeddedListElement *m_EmbeddedListElementNext; + EmbeddedListElement *m_EmbeddedListElementPrev; +}; + +class EmbeddedList +{ +public: + + EmbeddedList () : Head (0), Tail (0), m_Size(0) {} + + // Check Head + EmbeddedListElement *GetHead () + { + return Head; + } + + // Inserts an element onto the top of the list + void AddHead (EmbeddedListElement *Element) + { + //ASSERT (Element); + Element->Prev (0); + Element->Next (Head); + + // if first element, the tail also points to this element + if (!Head) + Tail = Element; + // more than one + else + // poin to the new head + Head->Prev (Element); + Head = Element; + m_Size++; + } + + // Takes a element off from the top of the list + EmbeddedListElement *RemoveHead () + { + EmbeddedListElement *element = Head; + if (element) + { + // if only one element + if (Tail == Head) + Tail = 0; + else + element->Next ()->Prev (0); + Head = element->Next (); + m_Size--; + } + return element; + } + + // Inserts a element onto the end of the list + void AddTail (EmbeddedListElement *Element) + { + //ASSERT (Element); + Element->Prev (Tail); + Element->Next (0); + + // if first element, the tail also points to this element + if (!Tail) + Head = Element; + // more than one + else + // poin to the new tail + Tail->Next (Element); + Tail = Element; + m_Size++; + } + + // Takes a element off from the end of the list + EmbeddedListElement *RemoveTail () + { + EmbeddedListElement *element = Tail; + if (element) + { + // if only one element + if (Tail == Head) + Head = 0; + else + element->Prev ()->Next (0); + Tail = element->Prev (); + m_Size--; + } + return element; + } + + // Inserts a element onto the end of the list + void AddBefore (EmbeddedListElement *ElementCurr, EmbeddedListElement *ElementNew) + { + //ASSERT (ElementCurr && ElementNew); + if (ElementCurr == Head) + AddHead (ElementNew); + else + { + ElementNew->Prev (ElementCurr->Prev ()); + ElementCurr->Prev (ElementNew); + ElementNew->Next (ElementCurr); + ElementNew->Prev ()->Next (ElementNew); + m_Size++; + } + } + + // Takes a element off from the list + int Remove (EmbeddedListElement *Element) + { + //ASSERT (Element); + if (Element == Head) + RemoveHead (); + else if (Element == Tail) + RemoveTail (); + else + { + Element->Next ()->Prev (Element->Prev ()); + Element->Prev ()->Next (Element->Next ()); + m_Size--; + } + return 0; + } + + /// @class Iterator + class Iterator + { + const EmbeddedList &m_List; + EmbeddedListElement *m_CurrentNode; + public: + Iterator (const EmbeddedList &List) : m_List (List) { Reset (); } + + // Reset the Iterator so that GetNext will give you the first list element + void Reset () + { + m_CurrentNode = m_List.Head; + } + + // Return the next list element (i.e. advance iterator and return next list element) + EmbeddedListElement *Next () + { + // move the iterator one step forward + if (m_CurrentNode) + m_CurrentNode = m_CurrentNode->Next (); + return m_CurrentNode; + } + + // Return the current list element + EmbeddedListElement *Current () const { return m_CurrentNode; } + }; + + int Size() const { return m_Size; } + +private: + EmbeddedListElement *Head; + EmbeddedListElement *Tail; + int m_Size; +}; + + +// ---------------------------------------------------------------------------- +} // namespace perc diff --git a/third-party/libtm/libtm/src/infra/Event.h b/third-party/libtm/libtm/src/infra/Event.h new file mode 100644 index 0000000000..549014bc13 --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Event.h @@ -0,0 +1,30 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include "TrackingCommon.h" +#include "Log.h" + +namespace perc +{ + class Event + { + public: + Event(); + + inline Handle handle() { return mEvent; } + + int signal(); + + int reset(); + + ~Event(); + + private: + Handle mEvent; + + void operator= (const Event &) = delete; + Event(const Event &) = delete; + }; + +} // namespace perc diff --git a/third-party/libtm/libtm/src/infra/EventHandler.h b/third-party/libtm/libtm/src/infra/EventHandler.h new file mode 100644 index 0000000000..bc5601ed52 --- /dev/null +++ b/third-party/libtm/libtm/src/infra/EventHandler.h @@ -0,0 +1,60 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include "TrackingCommon.h" + +namespace perc { +// ---------------------------------------------------------------------------- +/// +/// @class Message +/// +/// @brief Declare a base class for all messages that can be posted to Dispatcher. +/// User MUST manage allocation policy if needed by defining copy constructor +// and appropriate destructor. and its derives will be copied internally and +/// SHOULD be freed after dispatching callback by Dispatcher. +/// +// ---------------------------------------------------------------------------- +class Message +{ +public: + Message(int type, int param = 0) : Type(type), Param(param), Result(-1) {} + virtual ~Message() {} + int Type; + int Param; + mutable int Result; + + // replace mechanism that generate compilation error + // if derived class was not inherited from class + enum { IS_DERIVED_FROM_Message = true }; +}; + +// ---------------------------------------------------------------------------- +/// +/// @class EventHandler +/// +/// @brief This base class defines the interface for receiving the +/// results of sync and asynchronous operations. +/// Subclasses of this class will fill in appropriate methods. +/// @note +/// +// ---------------------------------------------------------------------------- +class EventHandler +{ +public: + virtual ~EventHandler() {} + + // = Completion callbacks + virtual void onMessage(const Message &) {} + virtual void onEvent(Handle fd, unsigned long mask, void *act) {} + virtual void onTimeout(uintptr_t timerId, const Message &) {} + virtual void onExit() {} + +protected: + // Hide the constructor + EventHandler() {} +}; + + +// ---------------------------------------------------------------------------- +} // namespace perc diff --git a/third-party/libtm/libtm/src/infra/Event_lin.cpp b/third-party/libtm/libtm/src/infra/Event_lin.cpp new file mode 100644 index 0000000000..30071e4394 --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Event_lin.cpp @@ -0,0 +1,35 @@ +/******************************************************************************* +INTEL CORPORATION PROPRIETARY INFORMATION +Copyright(c) 2017 Intel Corporation. All Rights Reserved. +*******************************************************************************/ + +#include "Event.h" +#include +# include + +perc::Event::Event() +{ + // LINUX: NONBLOCK + mEvent = ::eventfd(0, EFD_NONBLOCK); + + ASSERT(mEvent != ILLEGAL_HANDLE); +} + +perc::Event::~Event() +{ + ::close(mEvent); +} + +int perc::Event::reset() +{ + ASSERT(mEvent != ILLEGAL_HANDLE); + + eventfd_t cnt; + return !::eventfd_read(mEvent, &cnt); +} + +int perc::Event::signal() { + ASSERT(mEvent != ILLEGAL_HANDLE); + + return !::eventfd_write(mEvent, 1); +} \ No newline at end of file diff --git a/third-party/libtm/libtm/src/infra/Event_win.cpp b/third-party/libtm/libtm/src/infra/Event_win.cpp new file mode 100644 index 0000000000..2d90e5a110 --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Event_win.cpp @@ -0,0 +1,35 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#include "Event.h" + +perc::Event::Event() +{ + // WINDOWS: manual reset event + mEvent = CreateEvent( + NULL, // default security attributes + TRUE, // manual-reset event + FALSE, // initial state is nonsignaled + NULL // Unnamed event + ); + + ASSERT(mEvent != ILLEGAL_HANDLE); +} + +perc::Event::~Event() +{ + CloseHandle(mEvent); +} + +int perc::Event::reset() +{ + ASSERT(mEvent != ILLEGAL_HANDLE); + return ResetEvent(mEvent); +} + +int perc::Event::signal() +{ + ASSERT(mEvent != ILLEGAL_HANDLE); + + return SetEvent(mEvent); +} \ No newline at end of file diff --git a/third-party/libtm/libtm/src/infra/Fence.h b/third-party/libtm/libtm/src/infra/Fence.h new file mode 100644 index 0000000000..678cdaea1c --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Fence.h @@ -0,0 +1,37 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include +#include + + +namespace perc { + +class Fence +{ +public: + Fence(){} + + inline void notify() { + std::unique_lock l(m); + fired = true; + cv.notify_one(); + } + + inline void wait() { + std::unique_lock l(m); + cv.wait(l, [this]{ return fired; }); + fired = false; + } + +private: + std::mutex m; + std::condition_variable cv; + bool fired = false; + // Prevent assignment and initialization. + void operator= (const Fence &); + Fence (const Fence &); +}; + +} // namespace perc diff --git a/third-party/libtm/libtm/src/infra/Fsm.cpp b/third-party/libtm/libtm/src/infra/Fsm.cpp new file mode 100644 index 0000000000..cb13adf17a --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Fsm.cpp @@ -0,0 +1,393 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#define LOG_TAG "Fsm" + +#include "Fsm.h" + +namespace perc { +// -[statics]------------------------------------------------------------------ +// Default NONE transition definition. +const FsmTransition s_FsmTransitionNone = +{ + FSM_TRANSITION_NONE_NAME, + FSM_TRANSITION_NONE, + FSM_EVENT_NONE, + FSM_STATE_FINAL, + 0, + 0, + FSM_TRANSITION_AFTER_MAX_TIMEOUT +}; + +// Default FINAL state definition. +const FsmState s_FsmStateFinal = +{ + FSM_STATE_FINAL_NAME, + FSM_STATE_FINAL, + 0, + 0, + (FsmTransition *)&s_FsmTransitionNone +}; + + +// ---------------------------------------------------------------------------- +Fsm::Fsm() : + m_pFsm(0), + m_CurrStateId(FSM_STATE_FINAL), + m_Owner(NULL), + m_Dispatcher(NULL), + m_Name(""), + m_SelfEvent(NULL) +{ +} + +// ---------------------------------------------------------------------------- +Fsm::~Fsm() +{ + done(); +} + +// ---------------------------------------------------------------------------- +int +Fsm::init (const FsmState * const *pFsm, + void *Owner, + Dispatcher *pDispatcher, + const char *Name) +{ + // User MUST define FSM definition!!! + ASSERT (pFsm); + + // Save context parameters + m_pFsm = pFsm; + m_Owner = Owner; + m_Dispatcher = pDispatcher; + m_Name = Name; + + // Check if Dispatcher was set + if (!dispatcher ()) + { + LOGW("engine not found, can't schedule after transitions!"); + } + + // Init Initial state and call to initial state entry function. + // Initial state always first into the states list! + resetSelfEvent(); + int ret_code = InitNewState (m_pFsm[0]->Type); + if (FSM_CONTEXT_STATUS_OK != ret_code) + { + ASSERT(!m_SelfEvent); + logRetCode(ret_code, m_pFsm[m_CurrStateId], Message(FSM_EVENT_NONE)); + return ret_code; + } + // Process self event if exist + return processSelfEvent(); +} + +// ---------------------------------------------------------------------------- +int +Fsm::done () +{ + if (!m_pFsm) return FSM_CONTEXT_STATUS_ERROR; + + CancelAfterTransitions (); + + // Reset FSM context parameters. + m_pFsm = 0; + m_Owner = 0; + m_Dispatcher = 0; + + return FSM_CONTEXT_STATUS_OK; +} + +// ---------------------------------------------------------------------------- +int +Fsm::fireEvent (const Message &pEvent) +{ + ASSERT (m_pFsm); + + resetSelfEvent(); + + // Find transition that wait for this event + const FsmState *state = m_pFsm[m_CurrStateId]; + int transition_id = FSM_TRANSITION_NONE; + int ret_code = FSM_CONTEXT_STATUS_ERROR; + const FsmTransition *transition = 0; + + if ((ret_code = FindTransition (&transition_id, pEvent)) != FSM_CONTEXT_STATUS_OK) + goto exit_; + else + transition = &state->TransitionList[transition_id]; + + ASSERT (transition); + + // Check if transition internal - in this case only call to transition action and return + if (transition->NewState == FSM_STATE_SAME) + { + CallTransitionAction (transition, pEvent); + ret_code = FSM_CONTEXT_STATUS_OK; + goto exit_; + } + + // Transition is not internal - we must to do some work... + // Call to state exit function. + DoneCurrState (); + + // Call to transition action. + CallTransitionAction (transition, pEvent); + + // Go to new state, that may be final... + ret_code = InitNewState (transition->NewState); + +exit_: + if (FSM_CONTEXT_STATUS_OK != ret_code) + { + ASSERT(!m_SelfEvent); + logRetCode(ret_code, state, pEvent); + return ret_code; + } + // Process self event if exist + return processSelfEvent(); +} + +// -[Private functions]-------------------------------------------------------- +int +Fsm::InitNewState (int StateType) +{ + // Check if new state - final state + if (StateType == FSM_STATE_FINAL) + return FSM_CONTEXT_STATUS_STATE_FINAL; + + // Find new state definition + for (int state_id = 0; m_pFsm[state_id]->Type != FSM_STATE_FINAL; state_id++) + { + if (m_pFsm[state_id]->Type == StateType) + { + // Remember new state index. + m_CurrStateId = state_id; + + CallStateEntry (); + + // Schedule After transitions timers via engine object. + ScheduleAfterTransitions (); + + return FSM_CONTEXT_STATUS_OK; + } + } + return FSM_CONTEXT_STATUS_STATE_NOT_FOUND; +} + +// ---------------------------------------------------------------------------- +int +Fsm::DoneCurrState () +{ + // Cancel after transitions timers + CancelAfterTransitions (); + + // Call to old state exit function + CallStateExit (); + + return FSM_CONTEXT_STATUS_OK; +} + +// ---------------------------------------------------------------------------- +int +Fsm::FindTransition (int *pTransitionId, const Message &pEvent) +{ + const FsmTransition *transition_list = m_pFsm[m_CurrStateId]->TransitionList; + int ret_code = FSM_CONTEXT_STATUS_TRANSITION_NOT_FOUND; + + // = Find transition that wait for this event type + // + if (pEvent.Type == FSM_EVENT_TIMEOUT) + { + int tid = pEvent.Param; + + ASSERT (transition_list[tid].Type == FSM_TRANSITION_AFTER || + transition_list[tid].Type == FSM_TRANSITION_INTERNAL_AFTER); + + // Check transition Guard function + if (CallTransitionGuard (&transition_list[tid], pEvent)) + { + // Save found transition + *pTransitionId = tid; + ret_code = FSM_CONTEXT_STATUS_OK; + } + else + ret_code = FSM_CONTEXT_STATUS_EVENT_NOT_HANDLED; + } + else + { + for (int i = 0; transition_list[i].Type != FSM_TRANSITION_NONE; i++) + { + // Check if this transition wait for current event. + if (transition_list[i].EventType == pEvent.Type) + { + ret_code = FSM_CONTEXT_STATUS_EVENT_NOT_HANDLED; + + if (CallTransitionGuard (&transition_list[i], pEvent)) + { + // Save found transition + *pTransitionId = i; + return FSM_CONTEXT_STATUS_OK; + } + } + } + } + return ret_code; +} + +// ---------------------------------------------------------------------------- +int +Fsm::ScheduleAfterTransitions () +{ + if (!dispatcher ()) return -1; + + const FsmState *state = m_pFsm[m_CurrStateId]; + const FsmTransition *transition_list = state->TransitionList; + + // Find after transitions and schedule timers for them. + for (int tid = 0; transition_list[tid].Type != FSM_TRANSITION_NONE; tid++) + { + // Check if this after transition + if (transition_list[tid].TimeOut != FSM_TRANSITION_AFTER_MAX_TIMEOUT) + { + uintptr_t timerId = dispatcher ()->scheduleTimer(this, ms2ns(transition_list[tid].TimeOut), Message(FSM_EVENT_TIMEOUT, tid)); + if (!timerId) + { + LOGE("[%s]:invalid timer id, can't schedule more!", state->Name); + } + } + } + return 0; +} + +// ---------------------------------------------------------------------------- +int Fsm::CancelAfterTransitions() +{ + if (!dispatcher()) return -1; + + // Cancel timer for all after transitions that was scheduled. + dispatcher()->removeHandler(this, Dispatcher::TIMERS_MASK); + return 0; +} + +// ---------------------------------------------------------------------------- +bool Fsm::CallTransitionGuard(const FsmTransition *pTransition, const Message &pEvent) +{ + bool ret = true; + + if (pTransition->Guard) + ret = pTransition->Guard(this, pEvent); + + LOGV("[%s]:%s[%s]:guard%s %d", + m_pFsm[m_CurrStateId]->Name, + TransitionType (pTransition->Type), + pTransition->Name, + pTransition->Guard ? "()": "(none)", + ret); + return ret; +} + +// ---------------------------------------------------------------------------- +void Fsm::CallTransitionAction(const FsmTransition *pTransition, const Message &pEvent) +{ + LOGV("[%s]:%s[%s]:action%s", + m_pFsm[m_CurrStateId]->Name, + TransitionType (pTransition->Type), + pTransition->Name, + pTransition->Action ? "()": "(none)"); + if (pTransition->Action) + pTransition->Action(this, pEvent); +} + +// ---------------------------------------------------------------------------- +void Fsm::CallStateEntry() +{ + const FsmState *state = m_pFsm[m_CurrStateId]; + LOGV("[%s]:entry%s", + state->Name, + state->Entry ? "()": "(none)"); + if (state->Entry) + state->Entry(this); +} + +// ---------------------------------------------------------------------------- +void Fsm::CallStateExit() +{ + const FsmState *state = m_pFsm[m_CurrStateId]; + LOGV("[%s]:exit%s", + state->Name, + state->Exit ? "()": "(none)"); + if (state->Exit) + state->Exit(this); +} + +// ---------------------------------------------------------------------------- +// A utility static method that translates FSM status to a string. +const char* +Fsm::statusName (int Status) +{ + switch (Status) + { + case FSM_CONTEXT_STATUS_ERROR: return "Error"; + case FSM_CONTEXT_STATUS_OK: return "OK"; + case FSM_CONTEXT_STATUS_TRANSITION_NOT_FOUND: return "Transition not found"; + case FSM_CONTEXT_STATUS_EVENT_NOT_HANDLED: return "Event not handled"; + case FSM_CONTEXT_STATUS_STATE_FINAL: return "State final"; + case FSM_CONTEXT_STATUS_STATE_NOT_FOUND: return "State not found"; + } + return "Unknown status"; +} + +// ---------------------------------------------------------------------------- +// A utility static method that translates FSM transition to a string. +const char* +Fsm::TransitionType (int Transition) +{ + switch (Transition) + { + case FSM_TRANSITION_NONE: return "TN"; + case FSM_TRANSITION: return "T"; + case FSM_TRANSITION_AFTER: return "TA"; + case FSM_TRANSITION_INTERNAL: return "TI"; + case FSM_TRANSITION_INTERNAL_AFTER: return "TIA"; + } + return "T?"; +} + +// ---------------------------------------------------------------------------- +// Debug print of FSM status +void +Fsm::logRetCode (int retCode, const FsmState *state, const Message &pEvent) +{ + if (FSM_CONTEXT_STATUS_STATE_FINAL == retCode) + { + LOGD("final state reached"); + } + else + { + if (FSM_CONTEXT_STATUS_EVENT_NOT_HANDLED == retCode) + { + LOGW("[%s]:event[%d] not handled", state->Name, pEvent.Type); + } + else if (FSM_CONTEXT_STATUS_TRANSITION_NOT_FOUND == retCode) + { + LOGW("[%s]:no appropriate transition for this event[%d]", + state->Name, pEvent.Type); + } + else if (FSM_CONTEXT_STATUS_STATE_NOT_FOUND == retCode) + { + LOGW("[%s]:no appropriate state found for this event[%d]", + state->Name, pEvent.Type); + } + else + { + LOGE("[%s]:undefined status error - %d, event[%d]", + state->Name, retCode, pEvent.Type); + } + } +} + + +// ---------------------------------------------------------------------------- +} // namespace perc diff --git a/third-party/libtm/libtm/src/infra/Fsm.h b/third-party/libtm/libtm/src/infra/Fsm.h new file mode 100644 index 0000000000..556593d15e --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Fsm.h @@ -0,0 +1,332 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include "Log.h" +#include "Dispatcher.h" + + +namespace perc { +// -[Defines]------------------------------------------------------------------ +/// C++ helpers macros that transform static FSM functions interface to class methods. +// Put these macros into header file (protected/private) section. +#define ACTION(Class, State, Event) Class::sAction_s ## State ## _e ## Event +#define GUARD(Class, State, Event) Class::sGuard_s ## State ## _e ## Event +#define ENTRY(Class, State) Class::sEntry_s ## State +#define EXIT(Class, State) Class::sExit_s ## State + +#define DECLARE_FSM_ACTION(State, Event) \ + static void sAction_s##State##_e##Event(Fsm *, const Message &); \ + void Action_s##State##_e##Event(const Message &) + +#define DECLARE_FSM_GUARD(State, Event) \ + static bool sGuard_s##State##_e##Event(Fsm *, const Message &); \ + bool Guard_s##State##_e##Event(const Message &) + +#define DECLARE_FSM_STATE_ENTRY(State) \ + static void sEntry_s##State(Fsm *); \ + void Entry_s##State() + +#define DECLARE_FSM_STATE_EXIT(State) \ + static void sExit_s##State(Fsm *); \ + void Exit_s##State() + +// Put these macros into source file and define body. +#define DEFINE_FSM_ACTION(Class, State, Event, Msg) \ + void ACTION(Class, State, Event)(Fsm *pContext, const Message &Msg) \ + { Class* pThis = reinterpret_cast (pContext->owner()); pThis->Action_s ## State ## _e ## Event(Msg); } \ + void Class::Action_s ## State ## _e ## Event(const Message &Msg) + +#define DEFINE_FSM_GUARD(Class, State, Event, Msg) \ + bool GUARD(Class, State, Event)(Fsm *pContext, const Message &Msg) \ + { Class* pThis = reinterpret_cast (pContext->owner()); return pThis->Guard_s##State##_e##Event(Msg); } \ + bool Class::Guard_s##State##_e##Event(const Message &Msg) + +#define DEFINE_FSM_STATE_ENTRY(Class, State) \ + void ENTRY(Class, State)(Fsm *pContext) \ + { Class* pThis = reinterpret_cast (pContext->owner()); pThis->Entry_s##State(); } \ + void Class::Entry_s##State() + +#define DEFINE_FSM_STATE_EXIT(Class, State) \ + void EXIT(Class, State)(Fsm *pContext) \ + { Class* pThis = reinterpret_cast (pContext->owner()); pThis->Exit_s##State(); } \ + void Class::Exit_s##State() + + +// -[FSM Event]---------------------------------------------------------------- +/// +/// Event types definitions. +/// +/// @note: event types - this is a user defined type exclude timeout event type. +/// User must define application specific events like this (offset from USER_DEFINED): +/// - #define EVENT_TYPE_0 (0 + FSM_EVENT_USER_DEFINED) +/// - #define EVENT_TYPE_1 (1 + FSM_EVENT_USER_DEFINED) +/// - ... +/// +#define FSM_EVENT_NONE ((char)-1) +#define FSM_EVENT_TIMEOUT 0 +#define FSM_EVENT_USER_DEFINED 1 + + +// -[FSM Transition]----------------------------------------------------------- +/// +/// Internal FSM transition type definitions. +/// +/// @note: Only for Infra::Fsm internal use. +/// +#define FSM_TRANSITION_NONE_NAME "NONE" +#define FSM_TRANSITION_NONE ((char)-1) +#define FSM_TRANSITION 0 +#define FSM_TRANSITION_AFTER 1 +#define FSM_TRANSITION_INTERNAL 2 +#define FSM_TRANSITION_INTERNAL_AFTER 3 +#define FSM_TRANSITION_UNCONDITIONAL 4 + +// Define max timeout of after transition. +#define FSM_TRANSITION_AFTER_MAX_TIMEOUT ((unsigned long)~0) + +// Guard and Action function prototypes. +class Fsm; +class FsmEvent; +typedef bool (*FsmTransitionGuard) (Fsm *, const Message &); +typedef void (*FsmTransitionAction)(Fsm *, const Message &); + + +// -[Infra:FSM State]---------------------------------------------------------- +/// +/// FSM state type definitions. +/// +/// @note: !!! WARNING - Forbidden defining state with typeid - (-1) +/// User must define application specific states like this(offset from USER_DEFINED): +/// - #define STATE_0 (0 + FSM_STATE_USER_DEFINED) +/// - #define STATE_1 (1 + FSM_STATE_USER_DEFINED) +/// - ... +/// +#define FSM_STATE_FINAL_NAME "FINAL" +#define FSM_STATE_FINAL ((char)-1) ///< !!! Don't use this define for user defined states +#define FSM_STATE_SAME 0 +#define FSM_STATE_USER_DEFINED 1 + +// State entry/exit functions prototypes. +typedef void (*FsmStateEntry)(Fsm *pFsmContext); +typedef void (*FsmStateExit) (Fsm *pFsmContext); + + +// -[FSM Definition]----------------------------------------------------------- +/// Transition definition +typedef struct FsmTransition_T +{ + const char *Name; + char Type; + char EventType; + char NewState; + FsmTransitionGuard Guard; + FsmTransitionAction Action; + unsigned long TimeOut; +} FsmTransition; + +/// Default NONE transition definition. +extern const FsmTransition s_FsmTransitionNone; + + +/// State definition +typedef struct FsmState_T +{ + const char *Name; + char Type; + FsmStateEntry Entry; + FsmStateExit Exit; + FsmTransition *TransitionList; +} FsmState; + +/// Final state default definitions +extern const FsmState s_FsmStateFinal; + + +/// Internal use: FSM loggings definition macros +# define FSM_NAME(_name) _name, + +/// Transition list definition macros +#define TRANSITION_INTERNAL(_event, _guard, _action) \ + { FSM_NAME (#_event) FSM_TRANSITION_INTERNAL, _event, FSM_STATE_SAME, _guard, _action, FSM_TRANSITION_AFTER_MAX_TIMEOUT }, + +#define TRANSITION_INTERNAL_AFTER(_guard, _action, _timeout) \ + { FSM_NAME ("INTERNAL_AFTER") FSM_TRANSITION_INTERNAL_AFTER, FSM_EVENT_TIMEOUT, FSM_STATE_SAME, _guard, _action, _timeout }, + +#define TRANSITION(_event, _guard, _action, _new_state_type) \ + { FSM_NAME (#_event) FSM_TRANSITION, _event, _new_state_type, _guard, _action, FSM_TRANSITION_AFTER_MAX_TIMEOUT }, + +#define TRANSITION_AFTER(_guard, _action, _new_state_type, _timeout) \ + { FSM_NAME ("AFTER") FSM_TRANSITION_AFTER, FSM_EVENT_TIMEOUT, _new_state_type, _guard, _action, _timeout }, + + +/// State definition macros +#define DECLARE_FSM_STATE(_state) \ + static const FsmState s_Fsm##_s##_state; \ + static const FsmTransition s_Fsm##_s##_state##_##TransitionList[] + +#define DEFINE_FSM_STATE_BEGIN(Class, _state) \ + const FsmTransition Class::s_Fsm##_s##_state##_##TransitionList[] = { + +#define DEFINE_FSM_STATE_END(Class, _state, _entry, _exit) \ + { FSM_NAME (FSM_TRANSITION_NONE_NAME) FSM_TRANSITION_NONE, FSM_EVENT_NONE, FSM_STATE_FINAL, 0, 0, FSM_TRANSITION_AFTER_MAX_TIMEOUT } }; \ + const FsmState Class::s_Fsm##_s##_state = \ + { FSM_NAME (#_state) _state, _entry, _exit, (FsmTransition *)Class::s_Fsm##_s##_state##_##TransitionList }; + + +/// Macros for defining FSM. +#define FSM(_fsm) s_Fsm##_##_fsm + +#define DEFINE_FSM_BEGIN(Class, _fsm) \ + const FsmState * const Class::FSM(_fsm)[] = { + +#define STATE(Class, _state) \ + &Class::s_Fsm##_s##_state, + +#define DEFINE_FSM_END() \ + &s_FsmStateFinal \ + }; + +#define DECLARE_FSM(_fsm) \ + static const FsmState * const FSM(_fsm)[]; + + +// ---------------------------------------------------------------------------- +/// +/// @class Fsm +/// +/// @brief FSM context class. +/// +// ---------------------------------------------------------------------------- +/// FSM engine handle event return codes. +#define FSM_CONTEXT_STATUS_ERROR (-1) +#define FSM_CONTEXT_STATUS_OK 0 +#define FSM_CONTEXT_STATUS_TRANSITION_NOT_FOUND 1 +#define FSM_CONTEXT_STATUS_UNCOND_TRANSITION_NOT_FOUND 2 +#define FSM_CONTEXT_STATUS_EVENT_NOT_HANDLED 3 +#define FSM_CONTEXT_STATUS_STATE_FINAL 4 +#define FSM_CONTEXT_STATUS_STATE_NOT_FOUND 5 + +#define FSM_CONTEXT_DEFAULT_NAME "Fsm" +#define FSM_CONTEXT_TIMER_LIST_LEN 8 + +class Fsm : public EventHandler +{ +public: + + Fsm (); + virtual ~Fsm (); + + /// Explicit init function. + int init (const FsmState * const *pFsm, + void *Owner = 0, + Dispatcher *pDispatcher = 0, + const char *Name = FSM_CONTEXT_DEFAULT_NAME); + + /// + /// FSM context main function - handle user events and run defined state machine. + /// Before calling this function user MUST call Init() function. + /// User MUST call this function from well defined (single) context. + /// + /// @param pEvent Event to proceed. + /// + /// @return See Infra FSM engine handle event return codes. + /// In case of successful event handling function returns - FSM_CONTEXT_STATUS_OK. + /// In another case function returns error code that user MUST check and process. + /// + /// @note May enter recursively !!! + /// + int fireEvent(const Message &); + + int done(); + + // = Get/Set methods + // + /// Get User param method. + void *owner() const { return m_Owner; } + void owner(void *pOwner) { m_Owner = pOwner; } + + // = Self event mechanism + // + // Call to function will enable recursive message processing without + // main dispatcher loop. User SHOULD call once to this function in function, + // and transitions only. + void selfEvent(const Message &selfEvent) + { + ASSERT(!m_SelfEvent); + m_SelfEvent = new Message(selfEvent); + ASSERT(m_SelfEvent); + } + + /// Utility + int getCurrentState (void) const + { + if (m_pFsm) + return m_pFsm[m_CurrStateId]->Type; + return FSM_STATE_FINAL; + } + + static const char *statusName (int Status); + +protected: + + // = Interface: EventHandler + // + void onMessage(const Message &msg) { fireEvent (msg); } + void onTimeout(uintptr_t timerId, const Message &msg) { fireEvent (msg); } + + // Get the dispatcher associated with this handler + Dispatcher *dispatcher() const { return m_Dispatcher; } + +private: + /// FSM context parameters. + const FsmState * const *m_pFsm; + void *m_Owner; + Dispatcher *m_Dispatcher; + int m_CurrStateId; + const char *m_Name; + + /// Self event + Message *m_SelfEvent; + void resetSelfEvent() { m_SelfEvent = 0; } + int processSelfEvent() + { + int ret = FSM_CONTEXT_STATUS_OK; + if (m_SelfEvent) + { + Message *savedSelfEvent = m_SelfEvent; + ret = fireEvent (*m_SelfEvent); + delete savedSelfEvent; + } + return ret; + } + + // = Internal functions + // + int InitNewState(int StateType); + int DoneCurrState(); + static const char *TransitionType(int Transition); + void logRetCode (int retCode, const FsmState *state, const Message &); + + /// + /// This function returns id of transition that wait for this event type in OUT parameter. + /// Checks guard function of transition. In case that we reached last state's defined transition + /// this function returns error - INFRA_FSM_TRANSITION_NONE. + /// + int FindTransition(int /*OUT*/ *, const Message &); + + /// Init after transition list: schedule timer via engine object + int ScheduleAfterTransitions(); + + /// Cancel all timers and reset after transition list + int CancelAfterTransitions(); + + bool CallTransitionGuard(const FsmTransition *, const Message &); + void CallTransitionAction(const FsmTransition *, const Message &); + void CallStateEntry(); + void CallStateExit(); +}; + + +// ---------------------------------------------------------------------------- +} // namespace perc diff --git a/third-party/libtm/libtm/src/infra/Log.cpp b/third-party/libtm/libtm/src/infra/Log.cpp new file mode 100644 index 0000000000..a4ec7288fd --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Log.cpp @@ -0,0 +1,349 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#include "Log.h" +#include "Utils.h" +#include +#include +#include +#include +#include +#include "../include/TrackingData.h" + +#ifdef _WIN32 +#include +#include +#include +#endif + +#ifdef __linux__ +#include +#include +#include +#include +#include +#include +#include +#define gettid() syscall(SYS_gettid) +#endif + +using namespace perc; + +/* Define TIME_OF_DAY_LOG_HEADER to enable time of day (HH:MM:SS.mmm) log */ +/* Undefine TIME_OF_DAY_LOG_HEADER to enable epoch time log */ +#define TIME_OF_DAY_LOG_HEADER + +#define MAX_LOG_CONTAINERS 2 + +// Default priority mask - ALL enabled +static int s_PriorityMask = /*LOG_PRI2MASK(LOG_VERBOSE) | */ +LOG_PRI2MASK(LOG_DEBUG) | +LOG_PRI2MASK(LOG_TRACE) | +LOG_PRI2MASK(LOG_INFO) | +LOG_PRI2MASK(LOG_WARN) | +LOG_PRI2MASK(LOG_ERROR); + +class AdvancedLog : public TrackingData::Log { +public: + AdvancedLog() : rolledOver(0) {}; + + uint8_t rolledOver; +}; + + +class LogConfiguration { +public: + uint32_t outputMode; + uint8_t verbosityMask; + uint8_t rolloverMode; +} ; + +class logManager { +public: + logManager() : activeContainer(0) + { + loadTimestamp = systemTime(); + configuration[LogSourceHost].verbosityMask = s_PriorityMask; + configuration[LogSourceHost].rolloverMode = true; + configuration[LogSourceHost].outputMode = LogOutputModeScreen; + }; + + nsecs_t loadTimestamp; + + std::mutex configurationMutex; + LogConfiguration configuration[LogSourceMax]; + + std::atomic activeContainer; + std::mutex logContainerMutex[MAX_LOG_CONTAINERS]; + AdvancedLog logContainer[MAX_LOG_CONTAINERS]; +}; + +logManager gLogManager; + +FILE *gStream = NULL; + +const char* logPrioritySign[] = { "U" ,/* LOG_UNKNOWN */ +"T" ,/* LOG_DEFAULT */ +"V" ,/* LOG_VERBOSE */ +"D" ,/* LOG_DEBUG */ +"T" ,/* LOG_TRACE */ +"I" ,/* LOG_INFO */ +"W" ,/* LOG_WARN */ +"E" ,/* LOG_ERROR */ +"F" ,/* LOG_FATAL */ +"S" ,/* LOG_SILENT */ }; + +const LogVerbosityLevel prio2verbosity[] = {None, None, Verbose, Debug, Trace, Info, Warning, Error, Error, None}; + +// LOG_FATAL is always enabled by default +#define isPriorityEnabled(prio) (LOG_FATAL == prio || (gLogManager.configuration[LogSourceHost].verbosityMask & LOG_PRI2MASK(prio))) + +void __perc_Log_write(int prio, const char *tag, const char *text) +{ + if (!isPriorityEnabled(prio)) + return; + + fprintf(stdout, "%s", text); + +} + + void __perc_Log_Save(void* deviceId, int prio, const char *tag, int line, int payloadLen, char* payload) +{ + std::lock_guard guardLogContainer(gLogManager.logContainerMutex[gLogManager.activeContainer]); + AdvancedLog* logContainer = &gLogManager.logContainer[gLogManager.activeContainer]; + uint32_t entries = logContainer->entries; + + if ((gLogManager.configuration[LogSourceHost].rolloverMode == 0) && (logContainer->rolledOver == 1)) + { + printf("rolled over - stopped saving prints on container %d, entries = %d...\n", (uint32_t)gLogManager.activeContainer, entries); + return; + } + + logContainer->entry[entries].timeStamp = systemTime() - gLogManager.loadTimestamp; + + HostLocalTime localTime = getLocalTime(); + + logContainer->entry[entries].localTimeStamp.year = localTime.year; + logContainer->entry[entries].localTimeStamp.month = localTime.month; + logContainer->entry[entries].localTimeStamp.dayOfWeek = localTime.dayOfWeek; + logContainer->entry[entries].localTimeStamp.day = localTime.day; + logContainer->entry[entries].localTimeStamp.hour = localTime.hour; + logContainer->entry[entries].localTimeStamp.minute = localTime.minute; + logContainer->entry[entries].localTimeStamp.second = localTime.second; + logContainer->entry[entries].localTimeStamp.milliseconds = localTime.milliseconds; + + logContainer->entry[entries].lineNumber = line; + + if (tag != NULL) + { + perc::copy(&logContainer->entry[entries].moduleID, tag, MAX_LOG_BUFFER_MODULE_SIZE); + } + + logContainer->entry[entries].verbosity = prio2verbosity[prio]; + logContainer->entry[entries].deviceID = (uint64_t)deviceId; + logContainer->entry[entries].threadID = getThreadId(); + + snprintf(logContainer->entry[entries].payload, payloadLen + 1, "%s", payload); + logContainer->entry[entries].payloadSize = payloadLen; + + if (entries == (MAX_LOG_BUFFER_ENTRIES-1)) + { + printf("entries %d, setting rollover = 1...\n", logContainer->entries); + logContainer->rolledOver = 1; + } + + logContainer->entries = (entries + 1) % MAX_LOG_BUFFER_ENTRIES; + +} + + +#ifdef __linux__ +int __perc_Log_print_header(char * buf, int bufSize, int prio, const char *tag, const char* deviceId) +{ + struct timeval tv; + int headerLen; + static struct timeval loadTime = { 0 }; + + gettimeofday(&tv, NULL); + +#ifdef TIME_OF_DAY_LOG_HEADER + + // Round to nearest millisec + int millisec = lrint(tv.tv_usec / 1000.0); + if (millisec >= 1000) + { + millisec -= 1000; + tv.tv_sec++; + } + + struct tm* tm_info = localtime(&tv.tv_sec); // Transform time to ASCII + int timeLen = 8; // Length of adding HH:MM:SS + strftime(buf, timeLen + 2, "%H:%M:%S", tm_info); // Adding HH:MM:SS/0 to buffer + headerLen = snprintf(buf + timeLen, bufSize - timeLen, ".%03d [%lu] [%s] %s%s: ", millisec, gettid(), logPrioritySign[prio], tag, deviceId) + timeLen; + +#else + if (loadTime.tv_sec == 0) + { + loadTime = tv; + } + + unsigned long currentTime = ((tv.tv_sec * 1000000) + tv.tv_usec) - ((loadTime.tv_sec * 1000000) + loadTime.tv_usec); + headerLen = snprintf(buf, bufSize, "%014lu [%lu] [%s] %s: ", currentTime, gettid(), logPrioritySign[prio], tag); + +#endif //TIME_OF_DAY_LOG_HEADER + + return headerLen; +} +#elif _WIN32 +int __perc_Log_print_header(char * buf, int bufSize, int prio, const char *tag, const char* deviceId) +{ + int headerLen = 0; + +#ifdef TIME_OF_DAY_LOG_HEADER + + SYSTEMTIME lt; + GetLocalTime(<); + + headerLen = snprintf(buf, bufSize, "%02d:%02d:%02d:%03d [%06lu] [%s] %s%s: ", lt.wHour, lt.wMinute, lt.wSecond, lt.wMilliseconds, GetCurrentThreadId(), logPrioritySign[prio], tag, deviceId); + +#else + + static unsigned long long loadTime = 0; + FILETIME currentFileTime; + GetSystemTimeAsFileTime(¤tFileTime); + unsigned long long currentTime = (((ULONGLONG)currentFileTime.dwHighDateTime << 32) | (ULONGLONG)currentFileTime.dwLowDateTime) / 10000; + + if (loadTime == 0) + { + loadTime = currentTime; + } + + headerLen = snprintf(buf, bufSize, "%011llu [%06lu] [%s] %s: ", currentTime - loadTime, GetCurrentThreadId(), logPrioritySign[prio], tag); + +#endif // TIME_OF_DAY_LOG_HEADER + + return headerLen; + +} +#endif + +void __perc_Log_print(void* deviceId, int prio, const char *tag, int line, const char *fmt, ...) +{ + if (isPriorityEnabled(prio)) { + uint8_t outputMode; + uint8_t verbosity; + uint8_t rolloverMode; + int headerLen = 0; + int payloadLen = 0; + + va_list ap; + va_start(ap, fmt); + + __perc_Log_Get_Configuration(LogSourceHost, &outputMode, &verbosity, &rolloverMode); + + char buf[BUFSIZ * 4]; + + if (outputMode == LogOutputModeScreen) + { + char deviceBuf[30] = { 0 }; + if (deviceId != NULL) + { + short device = ((uintptr_t)deviceId & 0xFFFF); + snprintf(deviceBuf, sizeof(deviceBuf), "-%04hX", device); + } + + headerLen = __perc_Log_print_header(buf, sizeof(buf), prio, tag, deviceBuf); + + ASSERT((size_t)headerLen < sizeof(buf)); + payloadLen = vsnprintf(buf + headerLen, sizeof(buf) - headerLen, fmt, ap); + fprintf(stdout, "%s\n", buf); + } + else + { + payloadLen = vsnprintf(buf, sizeof(buf), fmt, ap); + __perc_Log_Save(deviceId, prio, tag, line, payloadLen, buf); + } + + va_end(ap); + } +} + +void __perc_Log_Set_Configuration(uint8_t source, uint8_t outputMode, uint8_t verbosity, uint8_t rolloverMode) +{ + std::lock_guard guardLogConfiguration(gLogManager.configurationMutex); + + gLogManager.configuration[source].outputMode = outputMode; + gLogManager.configuration[source].rolloverMode = rolloverMode; + + uint8_t verbosityMask = 0; + switch (verbosity) + { + case Trace: + verbosityMask |= LOG_PRI2MASK(LOG_TRACE); + case Verbose: + verbosityMask |= LOG_PRI2MASK(LOG_VERBOSE); + case Debug: + verbosityMask |= LOG_PRI2MASK(LOG_DEBUG); + case Warning: + verbosityMask |= LOG_PRI2MASK(LOG_WARN); + case Info: + verbosityMask |= LOG_PRI2MASK(LOG_INFO); + case Error: + verbosityMask |= LOG_PRI2MASK(LOG_ERROR); + case None: + verbosityMask |= 0; + } + + gLogManager.configuration[source].verbosityMask = verbosityMask; +} + +void __perc_Log_Get_Configuration(uint8_t source, uint8_t* outputMode, uint8_t* verbosity, uint8_t* rolloverMode) +{ + std::lock_guard guardLogConfiguration(gLogManager.configurationMutex); + + *outputMode = gLogManager.configuration[source].outputMode; + *verbosity = gLogManager.configuration[source].verbosityMask; + *rolloverMode = gLogManager.configuration[source].rolloverMode; +} + + +void __perc_Log_Get_Log(void* log) +{ + TrackingData::Log* logOutput = (TrackingData::Log*)log; + uint32_t entryIndex = 0; + + uint8_t activeContainer = gLogManager.activeContainer; + + gLogManager.activeContainer ^= 1; + + { + std::lock_guard guardLogContainer(gLogManager.logContainerMutex[activeContainer]); + AdvancedLog* logContainer = &gLogManager.logContainer[activeContainer]; + + if (logContainer->rolledOver == 1) + { + for (uint32_t i = logContainer->entries; i < MAX_LOG_BUFFER_ENTRIES; i++, entryIndex++) + { + logOutput->entry[entryIndex] = logContainer->entry[i]; + } + + logOutput->entries = MAX_LOG_BUFFER_ENTRIES; + } + else + { + logOutput->entries = logContainer->entries; + } + + for (uint32_t i = 0; i < logContainer->entries; i++, entryIndex++) + { + logOutput->entry[entryIndex] = logContainer->entry[i]; + } + + logOutput->maxEntries = MAX_LOG_BUFFER_ENTRIES; + + logContainer->entries = 0; + logContainer->rolledOver = 0; + + //printf("got log on container %d - %d entries (new container %d)\n", activeContainer, logOutput->entries, (uint32_t)gLogManager.activeContainer); + } +} diff --git a/third-party/libtm/libtm/src/infra/Log.h b/third-party/libtm/libtm/src/infra/Log.h new file mode 100644 index 0000000000..8c90407072 --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Log.h @@ -0,0 +1,132 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include +#include +#include +#include + +// Log priority values, in ascending priority order. +// Note: This definition is using the same priority order as android log subsystem. +typedef enum LogPriority { + LOG_UNKNOWN = 0, + LOG_DEFAULT, + LOG_VERBOSE, + LOG_DEBUG, + LOG_TRACE, + LOG_INFO, + LOG_WARN, + LOG_ERROR, + LOG_FATAL, + LOG_SILENT, +#define LOG_PRI2MASK(_pri) (1 << _pri) +} LogPriority; + +enum LogSource { + LogSourceHost = 0x0000, /**< Host Log Configuration */ + LogSourceFW = 0x0001, /**< FW Log Configuration */ + LogSourceMax = 0x0002, +}; + +// Normally we strip LOGV (VERBOSE messages) from release builds. +// You can modify this (for example with "#define LOG_NDEBUG 0" at the top of your source file) to change that behavior. +#ifndef NDEBUG +#define NDEBUG +#endif + +#ifndef LOG_NDEBUG +#ifdef NDEBUG +#define LOG_NDEBUG 1 +#else +#define LOG_NDEBUG 0 +#endif +#endif + +#ifndef LOG_TAG +#define LOG_TAG NULL +#endif + +// Macro to send a verbose log message using the current LOG_TAG. +#ifndef LOGV +#if LOG_NDEBUG +#define LOGV(...) ((void)0) +#else +#define LOGV(...) ((void)LOG(NULL, LOG_VERBOSE, LOG_TAG, __LINE__, __VA_ARGS__)) +#endif +#endif + +#ifndef DEVICELOGV +#if LOG_NDEBUG +#define DEVICELOGV(...) ((void)0) +#else +#define DEVICELOGV(...) ((void)LOG(this, LOG_VERBOSE, LOG_TAG, __LINE__, __VA_ARGS__)) +#endif +#endif + +// Macros to send a priority log message using the current LOG_TAG. +#ifndef LOGD +#define LOGD(...) ((void)LOG(NULL, LOG_DEBUG, LOG_TAG, __LINE__, __VA_ARGS__)) +#endif + +#ifndef LOGT +#define LOGT(...) ((void)LOG(NULL, LOG_TRACE, LOG_TAG, __LINE__, __VA_ARGS__)) +#endif + +#ifndef LOGI +#define LOGI(...) ((void)LOG(NULL, LOG_INFO, LOG_TAG, __LINE__, __VA_ARGS__)) +#endif + +#ifndef LOGW +#define LOGW(...) ((void)LOG(NULL, LOG_WARN, LOG_TAG, __LINE__, __VA_ARGS__)) +#endif + +#ifndef LOGE +#define LOGE(...) ((void)LOG(NULL, LOG_ERROR, LOG_TAG, __LINE__, __VA_ARGS__)) +#endif + +#ifndef LOGF +#define LOGF(...) ((void)LOG(NULL, LOG_FATAL, LOG_TAG, __LINE__, __VA_ARGS__)) +#endif + +#ifndef DEVICELOGD +#define DEVICELOGD(...) ((void)LOG(this, LOG_DEBUG, LOG_TAG, __LINE__, __VA_ARGS__)) +#endif + +#ifndef DEVICELOGT +#define DEVICELOGT(...) ((void)LOG(this, LOG_TRACE, LOG_TAG, __LINE__, __VA_ARGS__)) +#endif + +#ifndef DEVICELOGI +#define DEVICELOGI(...) ((void)LOG(this, LOG_INFO, LOG_TAG, __LINE__, __VA_ARGS__)) +#endif + +#ifndef DEVICELOGW +#define DEVICELOGW(...) ((void)LOG(this, LOG_WARN, LOG_TAG, __LINE__, __VA_ARGS__)) +#endif + +#ifndef DEVICELOGE +#define DEVICELOGE(...) ((void)LOG(this, LOG_ERROR, LOG_TAG, __LINE__, __VA_ARGS__)) +#endif + +#ifndef DEVICELOGF +#define DEVICELOGF(...) ((void)LOG(this, LOG_FATAL, LOG_TAG, __LINE__, __VA_ARGS__)) +#endif + +#ifndef LOG +#define LOG(_deviceId, _prio, _tag, _line, ...) \ + __perc_Log_print(_deviceId, _prio, _tag, _line, __VA_ARGS__) +#endif + +#define ASSERT assert +#define UNUSED(_var) (void)(_var) + +// platform depended print declaration +void __perc_Log_write(int prio, const char *tag, const char *text); +void __perc_Log_print(void* deviceId, int prio, const char *tag, int line, const char *fmt, ...); +int __perc_Log_print_header(char * buf, int bufSize, int prio, const char *tag, const char* deviceId); +void __perc_Log_Save(void* deviceId, int prio, int payloadLen, char* payload); +void __perc_Log_Set_Configuration(uint8_t source, uint8_t outputMode, uint8_t verbosity, uint8_t rolloverMode); +void __perc_Log_Get_Configuration(uint8_t source, uint8_t* outputMode, uint8_t* verbosityMask, uint8_t* rolloverMode); +void __perc_Log_Get_Log(void* log); + diff --git a/third-party/libtm/libtm/src/infra/Poller.h b/third-party/libtm/libtm/src/infra/Poller.h new file mode 100644 index 0000000000..5811e67344 --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Poller.h @@ -0,0 +1,60 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include +#include +#include +#include +#include "TrackingCommon.h" + +namespace perc { + + class Poller + { + public: + + struct event + { + Handle handle; + unsigned long mask; + void *act; + event() : + handle(ILLEGAL_HANDLE), mask(Poller::NULL_MASK), act(0) {} + event(Handle h, unsigned long m, void *a) : + handle(h), mask(m), act(a) {} + }; + + Poller(); + ~Poller(); + bool add(const Poller::event &); + bool remove(Handle); + + // POLLING LOOP + // + // Returns: + // >0 - the total number of events + // 0 - if the elapsed without dispatching any handle + // -1 - if an error occurs + int poll(Poller::event &, unsigned long timeoutMs); + + public: + + static const int NULL_MASK; + static const int READ_MASK; + static const int WRITE_MASK; + static const int EXCEPT_MASK; + static const int ALL_MASK; + + + private: + + struct CheshireCat; + std::unique_ptr mData; + + // Prevent assignment and initialization. + void operator= (const Poller &) = delete; + Poller(const Poller &) = delete; + }; + +} // namespace perc diff --git a/third-party/libtm/libtm/src/infra/Poller_lin.cpp b/third-party/libtm/libtm/src/infra/Poller_lin.cpp new file mode 100644 index 0000000000..90050cf34e --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Poller_lin.cpp @@ -0,0 +1,112 @@ +/******************************************************************************* +INTEL CORPORATION PROPRIETARY INFORMATION +Copyright(c) 2017 Intel Corporation. All Rights Reserved. +*******************************************************************************/ + +#define LOG_TAG "infra/Poller" +#define LOG_NDEBUG 1 // controls LOGV only +#include "Log.h" +#include "Utils.h" +#include "Poller.h" +#include "TrackingCommon.h" +#include +#include + +namespace perc +{ + + struct Poller::CheshireCat + { + std::unordered_map mEvents; + std::mutex mEventsGuard; + int mEpoll; + }; + + const int Poller::READ_MASK = EPOLLIN | EPOLLRDNORM; + const int Poller::NULL_MASK = 0; + const int Poller::WRITE_MASK = EPOLLOUT | EPOLLWRNORM; + const int Poller::EXCEPT_MASK = EPOLLPRI | EPOLLERR | EPOLLHUP | EPOLLRDHUP; + const int Poller::ALL_MASK = READ_MASK | WRITE_MASK | EXCEPT_MASK; + + Poller::Poller() : mData(new CheshireCat()) { + mData->mEpoll = ::epoll_create(1); + ASSERT(mData->mEpoll != -1); + } + + Poller::~Poller() + { + ::close(mData->mEpoll); + } + + bool Poller::add(const Poller::event &evt) + { + int res = -1; + if (evt.handle != -1) { + struct epoll_event e; + e.events = evt.mask; + e.data.fd = evt.handle; + // synchronized section + std::lock_guard guard(mData->mEventsGuard); + if (mData->mEvents.count(evt.handle) == 0) { + // adding new one + res = ::epoll_ctl(mData->mEpoll, EPOLL_CTL_ADD, evt.handle, &e); + if (!res) + mData->mEvents.emplace(evt.handle, evt); + } + else { + // modifying old one + res = ::epoll_ctl(mData->mEpoll, EPOLL_CTL_MOD, evt.handle, &e); + if (!res) + mData->mEvents[evt.handle] = evt; + } + } + return res == 0; + } + + bool Poller::remove(Handle handle) + { + if (handle != -1) { + //ASSERT(::pthread_equal(tid, ::pthread_self())); + std::lock_guard guard(mData->mEventsGuard); + if (mData->mEvents.count(handle) > 0) { + int res = ::epoll_ctl(mData->mEpoll, EPOLL_CTL_DEL, handle, 0); + mData->mEvents.erase(handle); + return res == 0; + } + } + return false; + } + + int Poller::poll(Poller::event &evt, unsigned long timeoutMs) + { + struct epoll_event e; + int timeoutAbs = (int)timeoutMs == INFINITE ? INFINITE : ns2ms(systemTime()) + (int)timeoutMs; + again: + int n = ::epoll_wait(mData->mEpoll, &e, 1, (int)timeoutMs); + if (n > 0) { + std::lock_guard guard(mData->mEventsGuard); + LOGV("poll: e.data.fd %d", e.data.fd); + if (mData->mEvents.count(e.data.fd) > 0) { + evt = mData->mEvents[e.data.fd]; + } + else { + ::epoll_ctl(mData->mEpoll, EPOLL_CTL_DEL, e.data.fd, 0); + int cur = ns2ms(systemTime()); + if ((int)timeoutMs != INFINITE) { + if (cur < (int)timeoutAbs) + timeoutMs = timeoutAbs - cur; + else + return 0; + } + goto again; + } + } + else if (n == -1) { + // TODO: add logs or clear errors + LOGE("poll: epoll_wait error %d", errno); + } + // timeout + return n; + } + +} // namespace perc diff --git a/third-party/libtm/libtm/src/infra/Poller_win.cpp b/third-party/libtm/libtm/src/infra/Poller_win.cpp new file mode 100644 index 0000000000..91b450b973 --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Poller_win.cpp @@ -0,0 +1,152 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#define LOG_TAG "Poller" +#define LOG_NDEBUG 1 // controls LOGV only +#include "Log.h" +#include "Utils.h" +#include "Poller.h" +#include "TrackingCommon.h" + + +namespace perc +{ + + struct Poller::CheshireCat + { + std::unordered_map mEvents; + std::mutex mEventsGuard; + + std::mutex mAddRemoveMutex; + std::vector mHandles; + HANDLE mAddRemoveDoneEvent; + }; + + const int Poller::READ_MASK = 0; + const int Poller::NULL_MASK = 0; + const int Poller::WRITE_MASK = 0; + const int Poller::EXCEPT_MASK = 0; + const int Poller::ALL_MASK = 0; + + + Poller::Poller() : mData(new CheshireCat()) + { + mData->mAddRemoveDoneEvent = CreateEvent( + NULL, // default security attributes + TRUE, // manual-reset event + FALSE, // initial state is nonsignaled + NULL // Unnamed event + ); + ASSERT(mData->mAddRemoveDoneEvent != ILLEGAL_HANDLE); + + mData->mHandles.push_back(mData->mAddRemoveDoneEvent); + } + + Poller::~Poller() + { + CloseHandle(mData->mAddRemoveDoneEvent); + } + + bool Poller::add(const Poller::event &evt) + { + // Take Add remove/mutex + std::lock_guard guardAddRemove(mData->mAddRemoveMutex); + + // Notify poll thread to exit and release mEventsGuard + SetEvent(mData->mAddRemoveDoneEvent); + + // Take mEventsGuard - wait for poll thread to release the mutex + std::lock_guard guardEvents(mData->mEventsGuard); + + ResetEvent(mData->mAddRemoveDoneEvent); + + //check if this handle already exists + auto it = std::find(mData->mHandles.begin(), mData->mHandles.end(), evt.handle); + + // we are already at maximum events + if (mData->mHandles.size() >= MAXIMUM_WAIT_OBJECTS && it == mData->mHandles.end()) + return false; + + if (it == mData->mHandles.end()) + mData->mHandles.push_back(evt.handle); + + // Add or modify event in events map + mData->mEvents[evt.handle] = evt; + + return true; + } + + bool Poller::remove(Handle handle) + { + // Take Add remove/mutex + std::lock_guard guardAddRemove(mData->mAddRemoveMutex); + + // Notify poll thread to exit and release mEventsGuard + SetEvent(mData->mAddRemoveDoneEvent); + + // Take mEventsGuard - wait for poll thread to release the mutex + std::lock_guard guardEvents(mData->mEventsGuard); + + ResetEvent(mData->mAddRemoveDoneEvent); + + //check if this handle already exists + auto it = std::find(mData->mHandles.begin(), mData->mHandles.end(), handle); + + if (it == mData->mHandles.end()) + return false; + + // Remove the handle from vector + mData->mHandles.erase(it); + + // remove the evt from events map + mData->mEvents.erase(handle); + + return true; + } + + int Poller::poll(Poller::event &evt, unsigned long timeoutMs) + { + int timeoutAbs = (timeoutMs == INFINITE)? INFINITE : ns2ms(systemTime()) + (int)timeoutMs; + + while (true) + { + int exitReason; + { + // Take events guard, to wait safely on vector of handles + std::lock_guard guardEvents(mData->mEventsGuard); + + exitReason = WaitForMultipleObjects((DWORD)mData->mHandles.size(), + mData->mHandles.data(), + FALSE, + timeoutMs); + + int eventIndex = exitReason - WAIT_OBJECT_0; + if ((exitReason < 0) || ((unsigned int)eventIndex >= mData->mHandles.size())) + { + return 0; + } + else if (eventIndex != 0) // zero is special case - it is our private event + { + evt = mData->mEvents[mData->mHandles[eventIndex]]; + return 1; + } + } // here we release the events guard, so Add/remove function may continue + + // WAIT_OBJECT_0 means add/remove function called, lets wait for it completion by just waiting for the lock + { + std::lock_guard guardAddRemove(mData->mAddRemoveMutex); + + // we are going to a second iteration - update timeout + if (timeoutMs != INFINITE) + { + auto nowMs = ns2ms(systemTime()); + if (nowMs >= timeoutAbs) + timeoutMs = 0; + else + timeoutMs = timeoutAbs - nowMs; + } + } + } + return 0; + } +} // namespace perc diff --git a/third-party/libtm/libtm/src/infra/Semaphore.h b/third-party/libtm/libtm/src/infra/Semaphore.h new file mode 100644 index 0000000000..46ed9f4cfe --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Semaphore.h @@ -0,0 +1,38 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once +#include "Utils.h" +#include +/* +* Wrapper class for pthread semaphore implementation. +*/ + + +namespace perc +{ + + class Semaphore + { + public: + Semaphore(unsigned int initValue = 0); + + int get(nsecs_t timeoutNs = -1); + int put(); + + ~Semaphore(); + + private: + + struct CheshireCat; + + std::unique_ptr mData; + + // = Prevent assignment and initialization. + void operator= (const Semaphore &) = delete; + Semaphore(const Semaphore &) = delete; + + }; + + +} // namespace perc diff --git a/third-party/libtm/libtm/src/infra/Semaphore_lin.cpp b/third-party/libtm/libtm/src/infra/Semaphore_lin.cpp new file mode 100644 index 0000000000..7fdfc94353 --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Semaphore_lin.cpp @@ -0,0 +1,49 @@ +/******************************************************************************* +INTEL CORPORATION PROPRIETARY INFORMATION +Copyright(c) 2017 Intel Corporation. All Rights Reserved. +*******************************************************************************/ + +#include "Semaphore.h" +#include "Utils.h" +#include + +namespace perc +{ + + struct Semaphore::CheshireCat + { + ::sem_t m_Semaphore; + }; + + int Semaphore::get(nsecs_t timeoutNs) + { + if (timeoutNs == -1) + return ::sem_wait(&mData->m_Semaphore); + if (timeoutNs == 0) + return ::sem_trywait(&mData->m_Semaphore); + nsecs_t nsecsTime = systemTime() + timeoutNs; + struct timespec absTimeout; + absTimeout.tv_sec = ns2s(nsecsTime); + absTimeout.tv_nsec = nsecsTime % 1000000000; + return ::sem_timedwait(&mData->m_Semaphore, &absTimeout); + } + + + Semaphore::Semaphore(unsigned int initValue) : mData(new CheshireCat()) + { + ::sem_init(&mData->m_Semaphore, 0, initValue); + } + + Semaphore::~Semaphore() + { + ::sem_destroy(&mData->m_Semaphore); + } + + int Semaphore::put() + { + return ::sem_post(&mData->m_Semaphore); + } + + + +} // namespace perc diff --git a/third-party/libtm/libtm/src/infra/Semaphore_win.cpp b/third-party/libtm/libtm/src/infra/Semaphore_win.cpp new file mode 100644 index 0000000000..8d7abe9bba --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Semaphore_win.cpp @@ -0,0 +1,61 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#include "Semaphore.h" +#include "Utils.h" +#include +#include + +#define MAX_SEMAPHORE_COUNT 128 + +namespace perc { + + struct Semaphore::CheshireCat + { + HANDLE m_Semaphore; + }; + + int Semaphore::get(nsecs_t timeoutNs) + { + DWORD timeoutMs; + + timeoutMs = ns2ms(timeoutNs); + + auto ret = WaitForSingleObject(mData->m_Semaphore, timeoutMs); + + if (ret == WAIT_OBJECT_0) // well this is useless , since WAIT_OBJECT_0 is equal to 0, but at least this is clear; + return 0; + + return -1; + } + + + Semaphore::Semaphore(unsigned int initValue) : mData(new CheshireCat()) + { + mData->m_Semaphore = CreateSemaphore( + NULL, // default security attributes + initValue, // initial count + MAX_SEMAPHORE_COUNT, // maximum count + NULL); + + assert(mData->m_Semaphore != NULL); + } + + Semaphore::~Semaphore() + { + CloseHandle(mData->m_Semaphore); + } + + int Semaphore::put() + { + int ret = ReleaseSemaphore( + mData->m_Semaphore, // handle to semaphore + 1, // increase count by one + NULL); // not interested in previous count + + return !ret; // return zero on success; same as in linux + } + + + +} // namespace perc diff --git a/third-party/libtm/libtm/src/infra/Utils.cpp b/third-party/libtm/libtm/src/infra/Utils.cpp new file mode 100644 index 0000000000..e65a941dd9 --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Utils.cpp @@ -0,0 +1,163 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#define LOG_TAG "Utils" +#include "Utils.h" +#include +#include +#include + +#ifdef __linux__ +#include +#include +#include +#include +#include +#include +#define gettid() syscall(SYS_gettid) +#else +#include +#include +#endif + +nsecs_t systemTime() +{ +#ifdef _WIN32 + /* + auto start = std::chrono::high_resolution_clock::now(); + std::chrono::time_point time_point_ns(start); + return time_point_ns.time_since_epoch().count();*/ + LARGE_INTEGER StartingTime = { 0 }; + LARGE_INTEGER Frequency = { 0 }; + + QueryPerformanceFrequency(&Frequency); + QueryPerformanceCounter(&StartingTime); + + StartingTime.QuadPart *= 1000000LL; // convert to microseconds + StartingTime.QuadPart /= Frequency.QuadPart; + StartingTime.QuadPart *= 1000; // Convert to nano seconds + + return StartingTime.QuadPart; + +#else + struct timespec ts; + ts.tv_sec = ts.tv_nsec = 0; + clock_gettime(CLOCK_MONOTONIC, &ts); + return ((nsecs_t)(ts.tv_sec)) * 1000000000LL + ts.tv_nsec; +#endif +} + + +HostLocalTime getLocalTime() +{ + HostLocalTime localTime; + +#ifdef _WIN32 + SYSTEMTIME lt; + GetLocalTime(<); + + localTime.year = lt.wYear; + localTime.month = lt.wMonth; + localTime.dayOfWeek = lt.wDayOfWeek; + localTime.day = lt.wDay; + localTime.hour = lt.wHour; + localTime.minute = lt.wMinute; + localTime.second = lt.wSecond; + localTime.milliseconds = lt.wMilliseconds; +#else + struct timeval tv; + gettimeofday(&tv, NULL); + + // Round to nearest millisec + int millisec = lrint(tv.tv_usec / 1000.0); + if (millisec >= 1000) + { + millisec -= 1000; + tv.tv_sec++; + } + + struct tm* tm_info = localtime(&tv.tv_sec); // Transform time to ASCII + + localTime.year = tm_info->tm_year; + localTime.month = tm_info->tm_mon; + localTime.dayOfWeek = tm_info->tm_wday; + localTime.day = tm_info->tm_mday; + localTime.hour = tm_info->tm_hour; + localTime.minute = tm_info->tm_min; + localTime.second = tm_info->tm_sec; + localTime.milliseconds = millisec; +#endif + + return localTime; +} + + +uint64_t bytesSwap(uint64_t val) +{ +#ifdef _WIN32 + return _byteswap_uint64(val); +#else + return htobe64(val); +#endif +} + +uint32_t getThreadId(void) +{ +#ifdef _WIN32 + return GetCurrentThreadId(); +#else + return gettid(); +#endif +} + + +void perc::copy(void* dst, void const* src, size_t size) +{ +#ifdef _WIN32 + memcpy_s(dst, size, src, size); +#else + auto from = reinterpret_cast(src); + std::copy(from, from + size, reinterpret_cast(dst)); +#endif +} + + +size_t perc::stringLength(const char* string, size_t maxSize) +{ +#ifdef _WIN32 + return strnlen_s(string, maxSize); +#else + return strnlen(string, maxSize); +#endif +} + +bool setProcessPriorityToRealtime() +{ +#ifdef _WIN32 + HANDLE pid = GetCurrentProcess(); + if (!SetPriorityClass(pid, REALTIME_PRIORITY_CLASS)) + { + LOGE("Error: Failed to set process priority to runtime"); + return false; + } + // Display priority class + DWORD dwPriClass = GetPriorityClass(pid); + LOGD("Setting process priority to 0x%X", dwPriClass); +#else + const int MAX_NICENESS = -20; + + id_t pid = getpid(); + int ret = setpriority(PRIO_PROCESS, pid, MAX_NICENESS); + if (ret == -1) + { + LOGE("Error: Failed to set process priority - to set priority rerun with sudo\n"); + return true; + } + // Display priority class + ret = getpriority(PRIO_PROCESS, pid); + LOGD("Setting process priority to 0x%X", ret); +#endif + + return true; +} + diff --git a/third-party/libtm/libtm/src/infra/Utils.h b/third-party/libtm/libtm/src/infra/Utils.h new file mode 100644 index 0000000000..8807aa93d7 --- /dev/null +++ b/third-party/libtm/libtm/src/infra/Utils.h @@ -0,0 +1,84 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#pragma once + +#include +#include "Log.h" + +#ifdef _WIN32 +#include +#else +#include +#define INFINITE (-1) +#endif + +class HostLocalTime { +public: + HostLocalTime() : year(0), month(0), dayOfWeek(0), day(0), hour(0), minute(0), second(0), milliseconds(0) {} + uint16_t year; + uint16_t month; + uint16_t dayOfWeek; + uint16_t day; + uint16_t hour; + uint16_t minute; + uint16_t second; + uint16_t milliseconds; +}; + +HostLocalTime getLocalTime(); + +#ifdef __cplusplus +extern "C" { +#endif + typedef int64_t nsecs_t; + + inline nsecs_t s2ns(nsecs_t sec) + { + return (sec < 0) ? INFINITE : sec * 1000000000; + } + + inline nsecs_t ms2ns(nsecs_t ms) + { + return (ms < 0) ? INFINITE : ms * 1000000; + } + + inline int ns2s(nsecs_t ns) + { + return (ns < 0) ? INFINITE : (int)(ns / 1000000000); + } + + inline int ns2ms(nsecs_t ns) + { + return (ns < 0) ? INFINITE : (int)(ns / 1000000); + } + + nsecs_t systemTime(); + + uint64_t bytesSwap(uint64_t val); + + uint32_t getThreadId(void); + + inline int toMillisecondTimeoutDelay(nsecs_t referenceTime, nsecs_t timeoutTime) + { + int timeoutDelayMsec = 0; + + if (timeoutTime > referenceTime) + { + timeoutDelayMsec = (int)(((uint64_t)(timeoutTime - referenceTime) + 999999LL) / 1000000LL); + } + + return timeoutDelayMsec; + } + + bool setProcessPriorityToRealtime(); + + namespace perc + { + void copy(void* dst, void const* src, size_t size); + size_t stringLength(const char* string, size_t maxSize); + } + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/third-party/libtm/libtm/src/version.rc.in b/third-party/libtm/libtm/src/version.rc.in new file mode 100644 index 0000000000..53ea77fa81 --- /dev/null +++ b/third-party/libtm/libtm/src/version.rc.in @@ -0,0 +1,93 @@ +// Microsoft Visual C++ generated resource script. +// +#include "windows.h" +#include "fw.h" +#include "CentralAppFw.h" +#include "CentralBlFw.h" + +///////////////////////////////////////////////////////////////////////////// +// Hebrew (Israel) resources + +#if !defined(AFX_RESOURCE_DLL) || defined(AFX_TARG_HEB) +LANGUAGE LANG_HEBREW, SUBLANG_DEFAULT + +#ifdef APSTUDIO_INVOKED +///////////////////////////////////////////////////////////////////////////// +// +// TEXTINCLUDE +// + +1 TEXTINCLUDE +BEGIN + "resource.h\0" +END + +2 TEXTINCLUDE +BEGIN + "\0" +END + +3 TEXTINCLUDE +BEGIN + "\r\n" + "\0" +END + +#endif // APSTUDIO_INVOKED + + +///////////////////////////////////////////////////////////////////////////// +// +// Version +// +#define FILE_VERSION "@LIBTM_VERSION_MAJOR@.@LIBTM_VERSION_MINOR@.@LIBTM_VERSION_PATCH@.@LIBTM_VERSION_BUILD@" +#define PRODUCT_VERSION FILE_VERSION ", FW " FW_VERSION ", Central APP " CENTRAL_APP_VERSION ", Central BL " CENTRAL_BL_VERSION " " + +VS_VERSION_INFO VERSIONINFO + FILEVERSION @LIBTM_VERSION_MAJOR@,@LIBTM_VERSION_MINOR@,@LIBTM_VERSION_PATCH@,0 + PRODUCTVERSION @LIBTM_VERSION_MAJOR@,@LIBTM_VERSION_MINOR@,@LIBTM_VERSION_PATCH@,0 + FILEFLAGSMASK 0x3fL +#ifdef _DEBUG + FILEFLAGS 0x1L +#else + FILEFLAGS 0x0L +#endif + FILEOS 0x40004L + FILETYPE VFT_DLL + FILESUBTYPE 0x0L +BEGIN + BLOCK "StringFileInfo" + BEGIN + BLOCK "040004b0" + BEGIN + VALUE "CompanyName", "Intel(r)" + VALUE "FileDescription", "TM2 library" + VALUE "FileVersion", "@LIBTM_VERSION_MAJOR@.@LIBTM_VERSION_MINOR@.@LIBTM_VERSION_PATCH@.@LIBTM_VERSION_BUILD@" + VALUE "InternalName", "version.rc" + VALUE "LegalCopyright", "Copyright (C) 2017" + VALUE "OriginalFilename", "tm.dll" + VALUE "ProductName", "TM2 library" + VALUE "ProductVersion", PRODUCT_VERSION + END + END + BLOCK "VarFileInfo" + BEGIN + VALUE "Translation", 0x400, 1200 + END +END + +#endif // Hebrew (Israel) resources +///////////////////////////////////////////////////////////////////////////// + + + +#ifndef APSTUDIO_INVOKED +///////////////////////////////////////////////////////////////////////////// +// +// Generated from the TEXTINCLUDE 3 resource. +// + + +///////////////////////////////////////////////////////////////////////////// +#endif // not APSTUDIO_INVOKED + diff --git a/third-party/libtm/resources/CMakeLists.txt b/third-party/libtm/resources/CMakeLists.txt new file mode 100644 index 0000000000..6cee35aa58 --- /dev/null +++ b/third-party/libtm/resources/CMakeLists.txt @@ -0,0 +1,320 @@ +cmake_minimum_required(VERSION 2.8) +project(resources) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + +set(FW_ARTIFACTORY_DIR "http://realsense-hw-public.s3.amazonaws.com/Releases/TM2/FW/target") +set(CENTRAL_APP_ARTIFACTORY_DIR "http://realsense-hw-public.s3.amazonaws.com/Releases/TM2/FW/app") +set(CENTRAL_BL_ARTIFACTORY_DIR "http://realsense-hw-public.s3.amazonaws.com/Releases/TM2/FW/bl") + +set(FW_OUTPUT_FILE "${LIBTM_SRC_DIR}/fw.h") +set(CENTRAL_APP_OUTPUT_FILE "${LIBTM_SRC_DIR}/CentralAppFw.h") +set(CENTRAL_BL_OUTPUT_FILE "${LIBTM_SRC_DIR}/CentralBlFw.h") + +# Convert HEX to DEC +function(from_hex HEX DEC) + string(SUBSTRING "${HEX}" 0 -1 HEX) + string(TOUPPER "${HEX}" HEX) + set(_res 0) + string(LENGTH "${HEX}" _strlen) + + while(_strlen GREATER 0) + math(EXPR _res "${_res} * 16") + string(SUBSTRING "${HEX}" 0 1 NIBBLE) + string(SUBSTRING "${HEX}" 1 -1 HEX) + if(NIBBLE STREQUAL "A") + math(EXPR _res "${_res} + 10") + elseif(NIBBLE STREQUAL "B") + math(EXPR _res "${_res} + 11") + elseif(NIBBLE STREQUAL "C") + math(EXPR _res "${_res} + 12") + elseif(NIBBLE STREQUAL "D") + math(EXPR _res "${_res} + 13") + elseif(NIBBLE STREQUAL "E") + math(EXPR _res "${_res} + 14") + elseif(NIBBLE STREQUAL "F") + math(EXPR _res "${_res} + 15") + else() + math(EXPR _res "${_res} + ${NIBBLE}") + endif() + + string(LENGTH "${HEX}" _strlen) + endwhile() + + set(${DEC} ${_res} PARENT_SCOPE) +endfunction() + +# Creates hex buffer from binary file - binary file must be 4 bytes aligned +function(readBinHeaderField input_bin_file offset limit output_hex_buffer) + #message(STATUS "Reading BIN header from ${input_bin_file} offset ${offset} limit ${limit}") + + file(READ ${input_bin_file} buffer LIMIT ${limit} OFFSET ${offset} HEX) + string(TOUPPER "${buffer}" buffer) + string(REGEX REPLACE "([0-9A-F][0-9A-F])([0-9A-F][0-9A-F])([0-9A-F][0-9A-F])([0-9A-F][0-9A-F])" "\\4\\3\\2\\1" buffer ${buffer}) + from_hex("${buffer}" buffer) + + # Copy ready buffer to output buffer + set(${output_hex_buffer} "${buffer}" PARENT_SCOPE) +endfunction() + +# Creates hex buffer from binary file - binary file must be 4 bytes aligned +function(bin2h input_bin_file offset output_hex_buffer) + message(STATUS "Creating HEX buffer from ${input_bin_file} offset ${offset}") + + # Read hex data from file + file(READ ${input_bin_file} buffer OFFSET ${offset} HEX) + + # Move all buffer to upper case + string(TOUPPER "${buffer}" buffer) + + # Convert every 4 bytes from AABBCCDD to 0xDDCCBBAA + string(REGEX REPLACE "([0-9A-F][0-9A-F])([0-9A-F][0-9A-F])([0-9A-F][0-9A-F])([0-9A-F][0-9A-F])" "0x\\4\\3\\2\\1," buffer ${buffer}) + + # Add new line to every 16 columns + string(REGEX REPLACE "(0x........,0x........,0x........,0x........,0x........,0x........,0x........,0x........,0x........,0x........,0x........,0x........,0x........,0x........,0x........,0x........,)" "\\1\\n" buffer ${buffer}) + + # Copy ready buffer to output buffer + set(${output_hex_buffer} "${buffer}" PARENT_SCOPE) +endfunction() + + +if(FW_VERSION) + message(STATUS "--------------------------------------------------------------------------------------------------------------------------------------------------------------") + + if (FW_SOURCE MATCHES "Remote") + file(REMOVE ${LIBTM_RESOURCES_DIR}/target.mvcmd) + message(STATUS "Downloading FW ${FW_VERSION} from '${FW_ARTIFACTORY_DIR}/${FW_VERSION}/target-${FW_VERSION}.mvcmd'") + file(DOWNLOAD "${FW_ARTIFACTORY_DIR}/${FW_VERSION}/target-${FW_VERSION}.mvcmd" "${LIBTM_RESOURCES_DIR}/target.mvcmd" TIMEOUT 60 STATUS status LOG log) + message(STATUS "Downloading FW ${FW_VERSION} completed - status ${status}") + + list (FIND status "\"No error\"" _index) + if (${_index} EQUAL -1) + message(FATAL_ERROR "Download error ${FW_ARTIFACTORY_DIR}/${FW_VERSION}/target-${FW_VERSION}.mvcmd" ) + endif() + elseif (FW_SOURCE MATCHES "Local") + if(NOT EXISTS "${LIBTM_RESOURCES_DIR}/target.mvcmd") + message(FATAL_ERROR "File ${LIBTM_RESOURCES_DIR}/target.mvcmd is missing" ) + else() + message(STATUS "Downloading FW ${FW_VERSION} skipped") + endif() + endif() + + if (FW_SOURCE MATCHES "Remote" OR FW_SOURCE MATCHES "Local") + file(REMOVE ${FW_OUTPUT_FILE}) + message(STATUS "Converting FW version ${FW_VERSION} ${LIBTM_RESOURCES_DIR}/target.mvcmd to ${FW_OUTPUT_FILE}") + + # Create empty output file + file(WRITE ${FW_OUTPUT_FILE} "") + file(APPEND ${FW_OUTPUT_FILE} "/*******************************************************************************\n") + file(APPEND ${FW_OUTPUT_FILE} "INTEL CORPORATION PROPRIETARY INFORMATION\n") + file(APPEND ${FW_OUTPUT_FILE} "Copyright(c) 2017 Intel Corporation. All Rights Reserved.\n") + file(APPEND ${FW_OUTPUT_FILE} "*******************************************************************************/\n\n") + file(APPEND ${FW_OUTPUT_FILE} "#ifndef target_h\n") + file(APPEND ${FW_OUTPUT_FILE} "#define target_h\n\n") + file(APPEND ${FW_OUTPUT_FILE} "#define FW_VERSION \"${FW_VERSION}\"\n\n") + file(APPEND ${FW_OUTPUT_FILE} "static uint32_t target_hex [] = {\n") + + bin2h(${LIBTM_RESOURCES_DIR}/target.mvcmd 0 fw_ready_buffer) + + file(APPEND ${FW_OUTPUT_FILE} "${fw_ready_buffer}\n") + file(APPEND ${FW_OUTPUT_FILE} "};\n") + file(APPEND ${FW_OUTPUT_FILE} "#endif\n") + else() + if (NOT EXISTS "${FW_OUTPUT_FILE}") + message(FATAL_ERROR "File ${FW_OUTPUT_FILE} is missing" ) + else() + message(STATUS "Already created ${FW_OUTPUT_FILE}") + endif() + endif() +endif(FW_VERSION) + + +if(CENTRAL_APP_VERSION) + message(STATUS "--------------------------------------------------------------------------------------------------------------------------------------------------------------") + + if (FW_SOURCE MATCHES "Remote") + file(REMOVE ${LIBTM_RESOURCES_DIR}/central_app.bin) + message(STATUS "Downloading Central App ${CENTRAL_APP_VERSION} from '${CENTRAL_APP_ARTIFACTORY_DIR}/${CENTRAL_APP_VERSION}/central_app-${CENTRAL_APP_VERSION}.bin'") + file(DOWNLOAD "${CENTRAL_APP_ARTIFACTORY_DIR}/${CENTRAL_APP_VERSION}/central_app-${CENTRAL_APP_VERSION}.bin" "${LIBTM_RESOURCES_DIR}/central_app.bin" TIMEOUT 60 STATUS status LOG log) + message(STATUS "Downloading Central App ${CENTRAL_APP_VERSION} completed - status ${status}") + list (FIND status "\"No error\"" _index) + if (${_index} EQUAL -1) + message(FATAL_ERROR "Download error ${CENTRAL_APP_ARTIFACTORY_DIR}/${CENTRAL_APP_VERSION}/central_app-${CENTRAL_APP_VERSION}.bin" ) + endif() + elseif (FW_SOURCE MATCHES "Local") + if(NOT EXISTS "${LIBTM_RESOURCES_DIR}/central_app.bin") + message(FATAL_ERROR "File ${LIBTM_RESOURCES_DIR}/central_app.bin is missing" ) + else() + message(STATUS "Downloading Central App ${CENTRAL_APP_VERSION} skipped") + endif() + endif() + + if (FW_SOURCE MATCHES "Remote" OR FW_SOURCE MATCHES "Local") + file(REMOVE ${CENTRAL_APP_OUTPUT_FILE}) + message(STATUS "Converting Central App version ${CENTRAL_APP_VERSION} ${LIBTM_RESOURCES_DIR}/central_app.bin to ${CENTRAL_APP_OUTPUT_FILE}") + + STRING(REGEX REPLACE "^([0-9]+)\\.[0-9]+\\.[0-9]+\\.[0-9]+" "\\1" central_app_major_version "${CENTRAL_APP_VERSION}") + STRING(REGEX REPLACE "^[0-9]+\\.([0-9]+)\\.[0-9]+\\.[0-9]+" "\\1" central_app_minor_version "${CENTRAL_APP_VERSION}") + STRING(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+)\\.[0-9]+" "\\1" central_app_patch_version "${CENTRAL_APP_VERSION}") + STRING(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.[0-9]+\\.([0-9]+)" "\\1" central_app_build_version "${CENTRAL_APP_VERSION}") + + # Central APP Header and data format: + # Header: + # uint32 header_size; + # uint32 data_size; + # uint32 file_format_version; // defines the format of both header and data + # uint32 version_size; + # uint8 version[] + # Data: + # uint8 data[]; + + # Read 1 byte of file_format_version and abort if not supported + readBinHeaderField(${LIBTM_RESOURCES_DIR}/central_app.bin 8 1 central_app_format_ver) + if (NOT ${central_app_format_ver} EQUAL 1) + message(FATAL_ERROR "Central FW File format (${central_app_format_ver}) is not supported" ) + endif() + + # Read 4 bytes of header_size + readBinHeaderField(${LIBTM_RESOURCES_DIR}/central_app.bin 0 4 central_app_header_size) + + # Read 4 bytes of data_size + readBinHeaderField(${LIBTM_RESOURCES_DIR}/central_app.bin 4 4 central_app_data_size) + + # Read 4 bytes of version_size + readBinHeaderField(${LIBTM_RESOURCES_DIR}/central_app.bin 12 4 central_app_version_size) + if (NOT ${central_app_version_size} EQUAL 3 AND NOT ${central_app_version_size} EQUAL 7) + message(FATAL_ERROR "Central FW version size (${central_app_version_size}) is not supported" ) + endif() + + # Read 1 byte of major,minor,patch versions + readBinHeaderField(${LIBTM_RESOURCES_DIR}/central_app.bin 16 1 central_app_version_major) + readBinHeaderField(${LIBTM_RESOURCES_DIR}/central_app.bin 17 1 central_app_version_minor) + readBinHeaderField(${LIBTM_RESOURCES_DIR}/central_app.bin 18 1 central_app_version_patch) + + # Check if need to read read 4 bytes of build version + if (${central_app_version_size} EQUAL 7) + readBinHeaderField(${LIBTM_RESOURCES_DIR}/central_app.bin 19 4 central_app_version_build) + endif() + + # Check if internal central FW version is compatible with file name + if (NOT ${central_app_version_major} EQUAL ${central_app_major_version} OR + NOT ${central_app_version_minor} EQUAL ${central_app_minor_version} OR + NOT ${central_app_version_patch} EQUAL ${central_app_patch_version} OR + NOT ${central_app_version_build} EQUAL ${central_app_build_version}) + message(FATAL_ERROR "Wrong Central FW version (${central_app_version_major}.${central_app_version_minor}.${central_app_version_patch}.${central_app_version_build}) VS (${central_app_major_version}.${central_app_minor_version}.${central_app_patch_version}.${central_app_build_version})") + endif() + + message(STATUS "Central Header:") + message(STATUS "- header_size = ${central_app_header_size}") + message(STATUS "- data_size = ${central_app_data_size}") + message(STATUS "- file_format_version = ${central_app_format_ver}") + message(STATUS "- version_size = ${central_app_version_size}") + message(STATUS "- version = ${central_app_version_major}.${central_app_version_minor}.${central_app_version_patch}.${central_app_version_build}") + + # Create empty output file + file(WRITE ${CENTRAL_APP_OUTPUT_FILE} "") + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "/*******************************************************************************\n") + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "INTEL CORPORATION PROPRIETARY INFORMATION\n") + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "Copyright(c) 2017 Intel Corporation. All Rights Reserved.\n") + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "*******************************************************************************/\n\n") + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "#ifndef CentralAppFw_h\n") + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "#define CentralAppFw_h\n\n") + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "#define CENTRAL_APP_VERSION \"${CENTRAL_APP_VERSION}\"\n") + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "#define CENTRAL_APP_SIZE ${central_app_data_size}\n\n") + + + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "namespace CentralAppFw {\n\n") + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "const int Version[] = { ${central_app_major_version}, ${central_app_minor_version}, ${central_app_patch_version}, ${central_app_build_version} };\n") + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "const uint32_t Buffer [] = {\n") + + bin2h(${LIBTM_RESOURCES_DIR}/central_app.bin ${central_app_header_size} central_app_ready_buffer) + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "${central_app_ready_buffer}\n") + + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "};\n") + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "}\n") + file(APPEND ${CENTRAL_APP_OUTPUT_FILE} "#endif\n") + else() + if (NOT EXISTS "${CENTRAL_APP_OUTPUT_FILE}") + message(FATAL_ERROR "File ${CENTRAL_APP_OUTPUT_FILE} is missing" ) + else() + message(STATUS "Already created ${CENTRAL_APP_OUTPUT_FILE}") + endif() + endif() + +endif(CENTRAL_APP_VERSION) + + +if(CENTRAL_BL_VERSION) + message(STATUS "--------------------------------------------------------------------------------------------------------------------------------------------------------------") + + if (FW_SOURCE MATCHES "Remote") + file(REMOVE ${LIBTM_RESOURCES_DIR}/central_bl.bin) + message(STATUS "Downloading Central BL ${CENTRAL_BL_VERSION} from '${CENTRAL_BL_ARTIFACTORY_DIR}/${CENTRAL_BL_VERSION}/central_bl-${CENTRAL_BL_VERSION}.bin'") + file(DOWNLOAD "${CENTRAL_BL_ARTIFACTORY_DIR}/${CENTRAL_BL_VERSION}/central_bl-${CENTRAL_BL_VERSION}.bin" "${LIBTM_RESOURCES_DIR}/central_bl.bin" TIMEOUT 60 STATUS status LOG log) + message(STATUS "Downloading Central BL ${CENTRAL_BL_VERSION} completed - status ${status}") + list (FIND status "\"No error\"" _index) + if (${_index} EQUAL -1) + message(FATAL_ERROR "Download error ${CENTRAL_BL_ARTIFACTORY_DIR}/${CENTRAL_BL_VERSION}/central_bl-${CENTRAL_BL_VERSION}.bin" ) + endif() + elseif (FW_SOURCE MATCHES "Local") + if(NOT EXISTS "${LIBTM_RESOURCES_DIR}/central_bl.bin") + message(FATAL_ERROR "File ${LIBTM_RESOURCES_DIR}/central_bl.bin is missing" ) + else() + message(STATUS "Downloading Central BL ${CENTRAL_BL_VERSION} skipped") + endif() + endif() + + if (FW_SOURCE MATCHES "Remote" OR FW_SOURCE MATCHES "Local") + file(REMOVE ${CENTRAL_BL_OUTPUT_FILE}) + message(STATUS "Converting Central BL version ${CENTRAL_BL_VERSION} ${LIBTM_RESOURCES_DIR}/central_bl.bin to ${CENTRAL_BL_OUTPUT_FILE}") + + STRING(REGEX REPLACE "^([0-9]+)\\.[0-9]+\\.[0-9]+\\.[0-9]+" "\\1" central_bl_major_version "${CENTRAL_BL_VERSION}") + STRING(REGEX REPLACE "^[0-9]+\\.([0-9]+)\\.[0-9]+\\.[0-9]+" "\\1" central_bl_minor_version "${CENTRAL_BL_VERSION}") + STRING(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+)\\.[0-9]+" "\\1" central_bl_patch_version "${CENTRAL_BL_VERSION}") + STRING(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.[0-9]+\\.([0-9]+)" "\\1" central_bl_build_version "${CENTRAL_BL_VERSION}") + + # Create empty output file + file(WRITE ${CENTRAL_BL_OUTPUT_FILE} "") + file(APPEND ${CENTRAL_BL_OUTPUT_FILE} "/*******************************************************************************\n") + file(APPEND ${CENTRAL_BL_OUTPUT_FILE} "INTEL CORPORATION PROPRIETARY INFORMATION\n") + file(APPEND ${CENTRAL_BL_OUTPUT_FILE} "Copyright(c) 2017 Intel Corporation. All Rights Reserved.\n") + file(APPEND ${CENTRAL_BL_OUTPUT_FILE} "*******************************************************************************/\n\n") + file(APPEND ${CENTRAL_BL_OUTPUT_FILE} "#ifndef CentralBlFw_h\n") + file(APPEND ${CENTRAL_BL_OUTPUT_FILE} "#define CentralBlFw_h\n\n") + file(APPEND ${CENTRAL_BL_OUTPUT_FILE} "#define CENTRAL_BL_VERSION \"${CENTRAL_BL_VERSION}\"\n\n") + + file(APPEND ${CENTRAL_BL_OUTPUT_FILE} "namespace CentralBlFw {\n\n") + file(APPEND ${CENTRAL_BL_OUTPUT_FILE} "const int Version[] = { ${central_bl_major_version}, ${central_bl_minor_version}, ${central_bl_patch_version}, ${central_bl_build_version} };\n") + file(APPEND ${CENTRAL_BL_OUTPUT_FILE} "const uint32_t Buffer [] = {\n") + + bin2h(${LIBTM_RESOURCES_DIR}/central_bl.bin 3 central_bl_ready_buffer) + file(APPEND ${CENTRAL_BL_OUTPUT_FILE} "${central_bl_ready_buffer}\n") + + file(APPEND ${CENTRAL_BL_OUTPUT_FILE} "};\n") + file(APPEND ${CENTRAL_BL_OUTPUT_FILE} "}\n") + file(APPEND ${CENTRAL_BL_OUTPUT_FILE} "#endif\n") + else() + if (NOT EXISTS "${CENTRAL_BL_OUTPUT_FILE}") + message(FATAL_ERROR "File ${CENTRAL_BL_OUTPUT_FILE} is missing" ) + else() + message(STATUS "Already created ${CENTRAL_BL_OUTPUT_FILE}") + endif() + endif() + +endif(CENTRAL_BL_VERSION) + + +# Update versions.log & remote_versions.log file +set(FWVERSIONS "FW [${FW_VERSION}]\nCentral App [${CENTRAL_APP_VERSION}]\nCentral BL [${CENTRAL_BL_VERSION}]\n") +set(ALLVERSIONS "Host [${HOST_VERSION}]\nFW [${FW_VERSION}]\nCentral App [${CENTRAL_APP_VERSION}]\nCentral BL [${CENTRAL_BL_VERSION}]\n") + +if (FW_SOURCE MATCHES "Remote") + file(WRITE ${LIBTM_RESOURCES_DIR}/remote_versions.log ${FWVERSIONS}) + file(WRITE ${LIBTM_ROOT}/versions.log ${ALLVERSIONS}) +elseif (FW_SOURCE MATCHES "Local") + file(WRITE ${LIBTM_ROOT}/versions.log Host ${ALLVERSIONS}) +elseif (FW_SOURCE MATCHES "Internal") + file(WRITE ${LIBTM_ROOT}/versions.log ${ALLVERSIONS}) +else() + message(FATAL_ERROR "Bad FW source - ${FW_SOURCE}" ) +endif() diff --git a/third-party/libtm/tools/CMakeLists.txt b/third-party/libtm/tools/CMakeLists.txt new file mode 100644 index 0000000000..fe553ecb9a --- /dev/null +++ b/third-party/libtm/tools/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 2.8) +project(libtm_samples) + +message(STATUS "--------------------------------------------------------------------------------------------------------------------------------------------------------------") +message(STATUS "Building all projects of ${PROJECT_NAME}") + +add_subdirectory(libtm_util) diff --git a/third-party/libtm/tools/libtm_util/CMakeLists.txt b/third-party/libtm/tools/libtm_util/CMakeLists.txt new file mode 100644 index 0000000000..6d0ef4c82e --- /dev/null +++ b/third-party/libtm/tools/libtm_util/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 2.8) + +project(libtm_util) +message(STATUS "Building project ${PROJECT_NAME}") + +if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +endif() + +if (UNIX) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") +endif (UNIX) + +# Specify include files search directories for the compiler +include_directories(${LIBTM_INCLUDE_DIR}) +include_directories(${LIBTM_SRC_DIR}) +include_directories(${INFRA_INCLUDE_DIR}) + +set(SOURCE_FILE libtm_util.cpp) + +# Add source Files +set(SOURCE_FILES + ${SOURCE_FILE} +) + +# Link all source files to a single binary named hello_perc +add_executable(${PROJECT_NAME} + ${SOURCE_FILES} +) + +# Link hello_perc binary against pthread and libtm libraries +target_link_libraries(${PROJECT_NAME} + tm +) + +set_target_properties (${PROJECT_NAME} PROPERTIES FOLDER Tools) + +install( + TARGETS + ${PROJECT_NAME} + RUNTIME DESTINATION + ${CMAKE_INSTALL_PREFIX}/bin +) + diff --git a/third-party/libtm/tools/libtm_util/libtm_util.cpp b/third-party/libtm/tools/libtm_util/libtm_util.cpp new file mode 100644 index 0000000000..e1555a3b04 --- /dev/null +++ b/third-party/libtm/tools/libtm_util/libtm_util.cpp @@ -0,0 +1,4304 @@ +#define LOG_TAG "libtmutil" +#include "Log.h" +#include "TrackingManager.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "Version.h" +#include "Utils.h" +#include "fw.h" + +#define LIBTM_UTIL_VERSION_MAJOR 1 +#define LIBTM_UTIL_VERSION_MINOR 0 + +#define MAX_RUN_TIME_SEC 86400 /* 24 hours */ +#define MIN_RUN_TIME_SEC 2 +#define START_STREAM_TIME_SEC 10 +#define STOP_STREAM_TIME_SEC 2 +#define RESET_DEVICE_TIME_SEC 5 +#define MAX_SUPPORTED_RAW_STREAMS 20 +#define MAX_START_STOP_LOOP_COUNT 1 +#define WAIT_FOR_DEVICE_SEC 1 +#define WAIT_FOR_CONTROLLERS_MSEC 100 +#define MAX_WAIT_FOR_CONTROLLERS_MSEC (3000 * WAIT_FOR_CONTROLLERS_MSEC) +#define MAX_WAIT_FOR_CALIBRATED_CONTROLLERS_MSEC (600 * WAIT_FOR_CONTROLLERS_MSEC) +#define WAIT_FOR_LOCALIZATION_SEC 1 +#define MAX_WAIT_FOR_LOCALIZATION_SEC (WAIT_FOR_LOCALIZATION_SEC * 10) +#define MAX_FIRST_POSE_DELAY_MSEC 1200 +#define MAX_LOCALIZATION_MAP_SIZE 104857600 /* 100 MB*/ +#define CONTROLLER_CONNECT_TIMEOUT_MSEC 16000 +#define MAX_WAIT_FOR_LOG_MSEC 500 +#define MIN_WAIT_FOR_LOG_MSEC 4 +#define WAIT_FOR_LOG_MULTIPLIER_STEP 5 +#define MAX_WAIT_FOR_IMAGE_MSEC 100 +#define TEMPERATURE_THRESHOLD_OVERRIDE_KEY 0xC10F +#define MAX_WAIT_FOR_STATIC_NODE_MSEC 10 +#define MAX_FRAME_LATENCY_MSEC 100 + +/* Max float accuracy */ +typedef std::numeric_limits flt; + +std::ostringstream poseCSV; +std::ostringstream poseTUM; +std::ostringstream videoCSV; +std::ostringstream gyroCSV; +std::ostringstream velocimeterCSV; +std::ostringstream accelerometerCSV; +std::ostringstream controllerCSV; +std::ostringstream rssiCSV; +char gFileHeaderName[80]; + +using namespace std; +shared_ptr gManagerInstance; + +using namespace perc; + + +const std::string statusToString(perc::Status status) +{ + switch (status) + { + case Status::SUCCESS: return "SUCCESS"; + case Status::COMMON_ERROR: return "COMMON_ERROR"; + case Status::FEATURE_UNSUPPORTED: return "FEATURE_UNSUPPORTED"; + case Status::ERROR_PARAMETER_INVALID: return "ERROR_PARAMETER_INVALID"; + case Status::INIT_FAILED: return "INIT_FAILED"; + case Status::ALLOC_FAILED: return "ALLOC_FAILED"; + case Status::ERROR_USB_TRANSFER: return "ERROR_USB_TRANSFER"; + case Status::ERROR_EEPROM_VERIFY_FAIL: return "ERROR_EEPROM_VERIFY_FAIL"; + case Status::ERROR_FW_INTERNAL: return "ERROR_FW_INTERNAL"; + case Status::BUFFER_TOO_SMALL: return "BUFFER_TOO_SMALL"; + case Status::NOT_SUPPORTED_BY_FW: return "NOT_SUPPORTED_BY_FW"; + case Status::DEVICE_BUSY: return "DEVICE_BUSY"; + case Status::TIMEOUT: return "TIMEOUT"; + case Status::TABLE_NOT_EXIST: return "TABLE_NOT_EXIST"; + case Status::TABLE_LOCKED: return "TABLE_LOCKED"; + case Status::DEVICE_STOPPED: return "DEVICE_STOPPED"; + case Status::TEMPERATURE_WARNING: return "TEMPERATURE_WARNING"; + case Status::TEMPERATURE_STOP: return "TEMPERATURE_STOP"; + case Status::CRC_ERROR: return "CRC_ERROR"; + case Status::INCOMPATIBLE: return "INCOMPATIBLE"; + case Status::SLAM_NO_DICTIONARY: return "SLAM_NO_DICTIONARY"; + case Status::NO_CALIBRATION_DATA: return "NO_CALIBRATION_DATA"; + case Status::SLAM_LOCALIZATION_DATA_SET_SUCCESS: return "SLAM_LOCALIZATION_DATA_SET_SUCCESS"; + case Status::SLAM_ERROR_CODE_VISION: return "SLAM_ERROR_CODE_VISION"; + case Status::SLAM_ERROR_CODE_SPEED: return "SLAM_ERROR_CODE_SPEED"; + case Status::SLAM_ERROR_CODE_OTHER: return "SLAM_ERROR_CODE_OTHER"; + case Status::CONTROLLER_CALIBRATION_VALIDATION_FAILURE: return "CONTROLLER_CALIBRATION_VALIDATION_FAILURE"; + case Status::CONTROLLER_CALIBRATION_FLASH_ACCESS_FAILURE: return "CONTROLLER_CALIBRATION_FLASH_ACCESS_FAILURE"; + case Status::CONTROLLER_CALIBRATION_IMU_FAILURE: return "CONTROLLER_CALIBRATION_IMU_FAILURE"; + case Status::CONTROLLER_CALIBRATION_INTERNAL_FAILURE: return "CONTROLLER_CALIBRATION_INTERNAL_FAILURE"; + default: return "UNKNOWN STATUS"; + } +} + +const char* fwLogVerbosityLevel(LogVerbosityLevel level) +{ + switch (level) + { + case Error: return "E"; + case Info: return "I"; + case Warning: return "W"; + case Debug: return "D"; + case Verbose: return "V"; + case Trace: return "T"; + } + return "X"; +} + +class CalibrationInfo { +public: + CalibrationInfo() : accelerometerBiasX(0), accelerometerBiasY(0), accelerometerBiasZ(0) { + memset(accelerometerScale, 0, sizeof(accelerometerScale)); + } + float accelerometerScale[3][3]; + float accelerometerBiasX; + float accelerometerBiasY; + float accelerometerBiasZ; +}; + +class UtilTimeStamps { +public: + class Loadtime { + public: + unsigned long long fw; + unsigned long long host; + unsigned long long hostCorrelated; + } loadTime; + + UtilTimeStamps(TrackingData::TimestampedData timeStampData) + { + loadTime.fw = timeStampData.timestamp; + loadTime.hostCorrelated = timeStampData.systemTimestamp; + }; + + void setTime(TrackingData::TimestampedData timeStampData) + { + hostCurrentSystemTime = systemTime(); + if (loadTime.host == 0) + { + loadTime.host = hostCurrentSystemTime; + } + hostTimeStamp = hostCurrentSystemTime - loadTime.host; + hostCorrelatedTimeStamp = timeStampData.systemTimestamp - loadTime.hostCorrelated; + fwTimeStamp = timeStampData.timestamp; + arrivalTimeStamp = timeStampData.arrivalTimeStamp; + latency = hostCurrentSystemTime - timeStampData.systemTimestamp; + } + + unsigned long long fwTimeStamp; + unsigned long long hostTimeStamp; + unsigned long long hostCorrelatedTimeStamp; + unsigned long long arrivalTimeStamp; + unsigned long long hostCurrentSystemTime; + unsigned long long latency; +}; + +enum LocalizationType +{ + LocalizationTypeGet = 0, + LocalizationTypeSet = 1, + LocalizationTypeReset = 2, + LocalizationTypeMax = 3, +}; + +enum ControllerBurnState +{ + ControllerBurnStateNotStarted = 0, /* Controller FW burn not started */ + ControllerBurnStateProcessing = 1, /* Controller FW burn is currently in process */ + ControllerBurnStateDone = 2, /* Controller FW burn is done */ + ControllerBurnStateMax = 3, +}; + +enum ControllerBurnConfigure +{ + ControllerBurnDisabled = 0, /* Don't burn controller on discovery */ + ControllerBurnOnNewVersion = 1, /* Burn controller on discovery if image version is newer than discovery version */ + ControllerBurnForce = 2, /* Burn controller on discovery */ + ControllerBurnMax = 3, +}; + +class ArgumentConfigurarion { +public: + ArgumentConfigurarion() : inputFilename(""), controllerDataFilename(""), velocimeterFilename(""), nodeFilename(""), temperature{0}, maxLoop(MAX_START_STOP_LOOP_COUNT), startStreamTime(START_STREAM_TIME_SEC), + resetLoop(0), statistics(false), jtag(false), errorCheck(false), errorExit(false), videoFile(false), videoCount(0), gyroCount(0), velocimeterCount(0), accelerometerCount(0), sixdofCount(0), + controllersCount(0), setExposure(false), verifyConfiguration(true), geoLocationEnabled(false), gpioEnabled(0), gpioControlBitMask(0), mode("live"), stereoMode(false), tumFormat(0) + { + for (uint8_t i = 0; i < LogSourceMax; i++) + { + logConfiguration[i].setLog = false; + logConfiguration[i].logOutputMode = LogOutputModeScreen; + logConfiguration[i].logRolloverMode = false; + logConfiguration[i].logVerbosity = None; + } + + for (uint8_t i = 0; i < VideoProfileMax; i++) + { + video[i].enabled = false; + video[i].outputEnabled = false; + } + + for (uint8_t i = 0; i < GyroProfileMax; i++) + { + gyro[i].enabled = false; + gyro[i].outputEnabled = false; + gyro[i].fps = 0; + } + + for (uint8_t i = 0; i < AccelerometerProfileMax; i++) + { + accelerometer[i].enabled = false; + accelerometer[i].outputEnabled = false; + accelerometer[i].fps = 0; + } + + for (uint8_t i = 0; i < SixDofProfileMax; i++) + { + sixdof[i] = false; + } + + for (uint8_t i = 0; i < ProfileTypeMax; i++) + { + memset(controllers[i].macAddress, 0, MAC_ADDRESS_SIZE); + controllers[i].controllerID = ProfileTypeMax; + controllers[i].needed = false; + } + + for (uint8_t i = 0; i < ProfileTypeMax; i++) + { + rssi[i].time = 0; + } + + for (uint8_t i = 0; i < LocalizationTypeMax; i++) + { + localization[i].enabled = false; + localization[i].filename = ""; + } + + geoLocation.set(0, 0, 0); + } + + std::string inputFilename; + std::string controllerDataFilename; + std::string velocimeterFilename; + std::string nodeFilename; + std::string recordFilename; + std::string mode; + bool stereoMode; + uint32_t maxLoop; + uint32_t startStreamTime; + + class Localization { + public: + std::fstream file; + std::string filename; + bool enabled; + }; + + Localization localization[LocalizationTypeMax]; + + TrackingData::GeoLocalization geoLocation; + bool geoLocationEnabled; + + uint8_t gpioControlBitMask; + bool gpioEnabled; + + class Enable { + public: + bool enabled; + bool outputEnabled; + uint16_t width; + uint16_t height; + uint16_t fps; + }; + + class ControllerBurn { + public: + ControllerBurn() : configure(ControllerBurnDisabled), state(ControllerBurnStateNotStarted) {} + ControllerBurnConfigure configure; + ControllerBurnState state; + }; + + class Controller { + public: + uint8_t macAddress[MAC_ADDRESS_SIZE]; + uint8_t controllerID; + ControllerBurn burn; + bool calibrate; + bool needed; + }; + + class Rssi { + public: + uint32_t time; + }; + + ProfileType findController(uint8_t* macAddress) + { + for (uint32_t i = Controller1; i < ProfileTypeMax; i++) + { + if (memcmp(macAddress, controllers[i].macAddress, MAC_ADDRESS_SIZE) == 0) + { + return (ProfileType)i; + } + } + + return ProfileTypeMax; + } + + Enable video[VideoProfileMax]; + uint8_t videoCount; + + Enable gyro[GyroProfileMax]; + uint8_t gyroCount; + + Enable velocimeter[VelocimeterProfileMax]; + uint8_t velocimeterCount; + + Enable accelerometer[AccelerometerProfileMax]; + uint8_t accelerometerCount; + + bool sixdof[SixDofProfileMax]; + uint8_t sixdofCount; + + std::string controllerFWFile; + std::string controllerFWFileType; + Controller controllers[ProfileTypeMax]; + uint8_t controllersCount; + + Rssi rssi[ProfileTypeMax]; + uint8_t rssiCount; + + class Temperature { + public: + class Threshold { + public: + float_t temperature; + bool set; + }; + + bool check; + Threshold threshold[TemperatureSensorMax]; + }; + + Temperature temperature; + + uint32_t resetLoop; + bool statistics; + bool jtag; + bool videoFile; + bool errorCheck; + bool errorExit; + uint8_t tumFormat; + + class LogConfiguration + { + public: + bool setLog; + bool logRolloverMode; + LogOutputMode logOutputMode; + LogVerbosityLevel logVerbosity; + }; + + LogConfiguration logConfiguration[LogSourceMax]; + + bool setExposure; + + TrackingData::Exposure exposure; + + bool verifyConfiguration; +}; +ArgumentConfigurarion gConfiguration; + +class SensorStatistics { +public: + SensorStatistics() : frames(0), frameRate(0), outputMode(0), enabled(0) {} + + void reset(void) + { + frames = 0; + frameRate = 0; + totalLatency = 0; + nanFrame = 0; + enabled = 0; + outputMode = 0; + frameDrops = 0; + frameReorder = 0; + prevFrameId = 0; + prevFrameTimeStamp = 0; + } + + uint64_t frames; /* Number of frames */ + uint64_t frameDrops; /* Number of frame drops */ + uint64_t frameReorder; /* Number of frame reorder */ + uint16_t frameRate; /* Frame per second */ + uint64_t totalLatency; /* total Latency (Motion->Host) in NanoSec - for average latency - divide with frames */ + uint64_t nanFrame; /* pose NAN frame received */ + uint8_t enabled; /* Enabled/Disabled on FW */ + uint8_t outputMode; /* For Video/Gyro/Accelerometer: 0x0 - send sensor outputs to the internal middlewares only */ + /* 0x1 - Send this sensor outputs also to the host over the USB interface */ + /* For Pose: 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 */ + uint32_t prevFrameId; /* Previous frame ID - for drop check */ + int64_t prevFrameTimeStamp; /* Previous frame TimeStamp - for drop check */ +}; + +class GyroStatistics : public SensorStatistics {}; +class VelocimeterStatistics : public SensorStatistics {}; +class VideoStatistics : public SensorStatistics {}; +class AccelerometerStatistics : public SensorStatistics {}; +class PoseStatistics : public SensorStatistics { +public: + void reset(void) + { + SensorStatistics::reset(); + trackerConfidence = 0; + } + + uint32_t trackerConfidence; +}; +class ControllerStatistics : public SensorStatistics { +public: + std::atomic isConnected; + std::atomic isCalibrated; + uint32_t FWProgress; +}; +class RssiStatistics : public SensorStatistics {}; +class LocalizationStatistics : public SensorStatistics { +public: + void reset(void) + { + SensorStatistics::reset(); + isCompleted = false; + fileSize = 0; + } + + bool isCompleted; + uint64_t fileSize; +}; + +class ControllerDiscoveryEventStatistics { +public: + ControllerDiscoveryEventStatistics() : frames(0) + { + memset(macAddress, 0, MAC_ADDRESS_SIZE); + } + + ControllerDiscoveryEventStatistics(uint8_t* _macAddress) : frames(0) + { + perc::copy(macAddress, _macAddress, MAC_ADDRESS_SIZE); + } + + uint8_t macAddress[MAC_ADDRESS_SIZE]; /* Controller Mac Address */ + uint64_t frames; /* Number of frames */ +}; + +class ErrorStatistics { +public: + ErrorStatistics() : count(0) { + } + + void reset(void) + { + count = 0; + } + + uint32_t count; /* Number of errors */ +}; + +class LedStatistics { +public: + LedStatistics() : count(0) { + } + + void reset(void) + { + count = 0; + } + + uint32_t count; /* Number of led events */ +}; + +class StatusStatistics { +public: + StatusStatistics() : count(0) { + } + + void reset(void) + { + count = 0; + } + + uint32_t count; /* Number of statuses */ +}; + +class RunTimeStatistics { +public: + RunTimeStatistics() : startTimeMsec(0), stopTimeMsec(0), diffMsec(0), startToken(0), stopToken(0) {} + + void reset(void) + { + startTimeMsec = 0; + stopTimeMsec = 0; + diffMsec = 0; + startToken = 0; + stopToken = 0; + } + + void start(void) + { + if (startToken == stopToken) + { + startTimeMsec = ns2ms(systemTime()); + stopTimeMsec = 0; + diffMsec = 0; + startToken++; + } + + /* Else already started, do nothing */ + + } + + void stop(void) + { + if (startToken == (stopToken + 1)) + { + stopTimeMsec = ns2ms(systemTime()); + diffMsec = (stopTimeMsec - startTimeMsec); + stopToken++; + } + + /* Else already stopped, do nothing */ + } + + uint32_t startTimeMsec; + uint32_t stopTimeMsec; + uint32_t diffMsec; + uint32_t startToken; + uint32_t stopToken; +}; + +class Statistics { +public: + Statistics() {} + ~Statistics() + { + this->reset(); + } + + /* Reset all Statistics fields */ + void reset(void) + { + for (uint8_t i = 0; i < VideoProfileMax; i++) + { + video[i].reset(); + } + + for (uint8_t i = 0; i < GyroProfileMax; i++) + { + gyro[i].reset(); + } + + for (uint8_t i = 0; i < VelocimeterProfileMax; i++) + { + velocimeter[i].reset(); + } + + for (uint8_t i = 0; i < AccelerometerProfileMax; i++) + { + accelerometer[i].reset(); + } + + for (uint8_t i = 0; i < SixDofProfileMax; i++) + { + pose[i].reset(); + } + + for (uint8_t i = Controller1; i < ProfileTypeMax; i++) + { + controller[i].reset(); + } + + for (uint8_t i = Controller1; i < ProfileTypeMax; i++) + { + rssi[i].reset(); + } + + for (uint8_t i = LocalizationTypeGet; i < LocalizationTypeMax; i++) + { + localization[i].reset(); + } + + for (uint8_t i = 0; i < ProfileTypeMax; i++) + { + runTime[i].reset(); + } + + controllerDiscoveryEventVector.clear(); + + error.reset(); + status.reset(); + led.reset(); + } + + void print() + { + LOGD("Statistics"); + LOGD("--------------------------+-------------------+--------+---------+--------+------------+----------+----------+--------"); + LOGD(" Type | Info | Frames | Enabled | Output | Latency | Run Time | Expected | Actual "); + LOGD(" | | PerSec | | Mode | AVG (mSec) | (mSec) | Frames | Frames "); + LOGD("--------------------------+-------------------+--------+---------+--------+------------+----------+----------+--------"); + + for (uint8_t i = 0; i < VideoProfileMax; i++) + { + LOGD(" Video[%01d] | %-17s | %-6d | 0x%01X | 0x%01X | %-10" PRId64" | %-8d | %-8d | %d", i, (i > VideoProfile1) ? "Low Exposure " : "High Exposure", + video[i].frameRate, video[i].enabled, video[i].outputMode, (video[i].frames > 0) ? (video[i].totalLatency / video[i].frames) : 0, runTime[HMD].diffMsec, (video[i].frameRate * video[i].enabled * video[i].outputMode * runTime[HMD].diffMsec) / 1000, video[i].frames); + } + + for (uint8_t i = 0; i < GyroProfileMax; i++) + { + LOGD(" Gyro[%01d] | %-17s | %-6d | 0x%01X | 0x%01X | %-10" PRId64" | %-8d | %-8d | %d", i, (i > GyroProfile0) ? ((i > GyroProfile1) ? "Controller 2" : "Controller 1") : "HMD", + gyro[i].frameRate, gyro[i].enabled, gyro[i].outputMode, (gyro[i].frames > 0) ? (gyro[i].totalLatency / gyro[i].frames) : 0, runTime[i].diffMsec, (gyro[i].frameRate * gyro[i].enabled * gyro[i].outputMode * runTime[i].diffMsec) / 1000, gyro[i].frames); + } + + for (uint8_t i = 0; i < AccelerometerProfileMax; i++) + { + LOGD(" Accelerometer[%01d] | %-17s | %-6d | 0x%01X | 0x%01X | %-10" PRId64" | %-8d | %-8d | %d", i, (i > AccelerometerProfile0) ? ((i > AccelerometerProfile1) ? "Controller 2" : "Controller 1") : "HMD", + accelerometer[i].frameRate, accelerometer[i].enabled, accelerometer[i].outputMode, (accelerometer[i].frames > 0) ? (accelerometer[i].totalLatency / accelerometer[i].frames) : 0, runTime[i].diffMsec, (accelerometer[i].frameRate * accelerometer[i].enabled * accelerometer[i].outputMode * runTime[i].diffMsec) / 1000, accelerometer[i].frames); + } + + for (uint8_t i = 0; i < VelocimeterProfileMax; i++) + { + LOGD(" Velocimeter[%01d] | External Sensor | | 0x%01X | 0x%01X | %-10" PRId64" | %-8d | | %d", i, + velocimeter[i].enabled, velocimeter[i].outputMode, (velocimeter[i].frames > 0) ? (velocimeter[i].totalLatency / velocimeter[i].frames) : 0, runTime[HMD].diffMsec, velocimeter[i].frames); + } + + for (uint8_t i = 0; i < SixDofProfileMax; i++) + { + uint32_t poseRunTimeMsec = 0; + + if (runTime[i].diffMsec > MAX_FIRST_POSE_DELAY_MSEC) + { + poseRunTimeMsec = (runTime[i].diffMsec - MAX_FIRST_POSE_DELAY_MSEC); + } + + LOGD(" Pose[%01d] | %-17s | %-6d | 0x%01X | 0x%01X | %-10" PRId64" | %-8d | %-8d | %d", i, (i > SixDofProfile0) ? ((i > SixDofProfile1) ? "Controller 2" : "Controller 1") : "HMD", + pose[i].frameRate, pose[i].enabled, pose[i].outputMode, (pose[i].frames > 0) ? (pose[i].totalLatency / pose[i].frames) : 0, runTime[i].diffMsec, (pose[i].frameRate * pose[i].enabled * pose[i].outputMode * poseRunTimeMsec) / 1000, pose[i].frames); + } + + for (auto it = controllerDiscoveryEventVector.begin(); it != controllerDiscoveryEventVector.end(); it++) + { + LOGD(" ControllerDiscoveryEvent | %02X:%02X:%02X:%02X:%02X:%02X | | | | | | | %d", + (*it).get()->macAddress[0], (*it).get()->macAddress[1], (*it).get()->macAddress[2], (*it).get()->macAddress[3], (*it).get()->macAddress[4], (*it).get()->macAddress[5], (*it).get()->frames); + } + + if (controllerDiscoveryEventVector.size() == 0) + { + LOGD(" ControllerDiscoveryEvent | All controllers | | | | | | | 0"); + } + + for (uint8_t i = Controller1; i < ProfileTypeMax; i++) + { + LOGD(" Controller[%01d] | Controller %01d | | | | | | | %d", i, i, controller[i].frames); + } + + for (uint8_t i = Controller1; i < ProfileTypeMax; i++) + { + LOGD(" Rssi[%01d] | Controller %01d | | | | | | | %d", i, i, rssi[i].frames); + } + + LOGD(" Errors | All error types | | | | | | | %d", error.count); + LOGD(" Statuses | All status types | | | | | | | %d", status.count); + LOGD(" Led Event | Led Event | | | | | | | %d", led.count); + LOGD("--------------------------+-------------------+--------+---------+--------+------------+----------+----------+--------"); + } + + /* Check frame count and latency for video/gyro/accelerometer/6dof and check error events */ + bool check() + { + bool checkerFailed = false; + for (uint8_t i = 0; i < VideoProfileMax; i++) + { + /* Check if video sensor started */ + if ((video[i].enabled) && (video[i].outputMode) && (runTime[HMD].diffMsec == 0)) + { + LOGE("Error: Enabled video[%d] didn't start", HMD); + checkerFailed = true; + } + else + { + /* Check if received min frames of runtime - 1 sec */ + if (video[i].frames < (video[i].enabled * video[i].outputMode * video[i].frameRate * (runTime[HMD].diffMsec / 1000 - 1))) + { + LOGE("Error: Got too few video[%d] frames [Min Expected %d] [Actual %d]", i, (video[i].enabled * video[i].outputMode * video[i].frameRate * (runTime[HMD].diffMsec / 1000 - 1)), video[i].frames); + checkerFailed = true; + } + + if (video[i].frames > 0) + { + /* Check if received max frames of runtime + 1 sec */ + if (video[i].frames > (video[i].enabled * video[i].outputMode * video[i].frameRate * (runTime[HMD].diffMsec / 1000 + 1))) + { + LOGE("Error: Got too many video[%d] frames [Max Expected %d] [Actual %d]", i, (video[i].enabled * video[i].outputMode * video[i].frameRate * (runTime[HMD].diffMsec / 1000 + 1)), video[i].frames); + checkerFailed = true; + } + + /* Check latency less than 100 msec */ + if ((video[i].totalLatency / video[i].frames) > MAX_FRAME_LATENCY_MSEC) + { + LOGE("Error: video[%d] frame motion->host latency is too high (%d)", i, (video[i].totalLatency / video[i].frames)); + checkerFailed = true; + } + + /* Check frame drops */ + if (video[i].frameDrops > 0) + { + LOGE("Error: video[%d] frame drops occurred (%d)", i, video[i].frameDrops); + checkerFailed = true; + } + + /* Check frame reorder */ + if (video[i].frameReorder > 0) + { + LOGE("Error: video[%d] frame reorder occurred (%d)", i, video[i].frameReorder); + checkerFailed = true; + } + } + } + } + + for (uint8_t i = 0; i < GyroProfileMax; i++) + { + /* Check if gyro sensor started */ + if ((gyro[i].enabled) && (gyro[i].outputMode) && (runTime[i].diffMsec == 0)) + { + LOGE("Error: Enabled gyro[%d] didn't start", i); + checkerFailed = true; + } + else + { + /* Check if received min frames of runtime - 1 sec */ + if (gyro[i].frames < (gyro[i].enabled * gyro[i].outputMode * gyro[i].frameRate * (runTime[i].diffMsec / 1000 - 1))) + { + LOGE("Error: Got too few gyro[%d] frames [Min Expected %d] [Actual %d]", i, (gyro[i].enabled * gyro[i].outputMode * gyro[i].frameRate * (runTime[i].diffMsec / 1000 - 1)), gyro[i].frames); + checkerFailed = true; + } + + if (gyro[i].frames > 0) + { + /* Check if received max frames of runtime + 1 sec */ + if (gyro[i].frames > (gyro[i].enabled * gyro[i].outputMode * gyro[i].frameRate * (runTime[i].diffMsec / 1000 + 1))) + { + LOGE("Error: Got too many gyro[%d] frames [Max Expected %d] [Actual %d]", i, (gyro[i].enabled * gyro[i].outputMode * gyro[i].frameRate * (runTime[i].diffMsec / 1000 + 1)), gyro[i].frames); + checkerFailed = true; + } + + /* Check latency less than 100 msec */ + if ((gyro[i].totalLatency / gyro[i].frames) > MAX_FRAME_LATENCY_MSEC) + { + LOGE("Error: gyro[%d] frame motion->host latency is too high (%d)", i, (gyro[i].totalLatency / gyro[i].frames)); + checkerFailed = true; + } + + /* Check frame drops */ + if (gyro[i].frameDrops > 0) + { + LOGE("Error: gyro[%d] frame drops occurred (%d)", i, gyro[i].frameDrops); + checkerFailed = true; + } + + /* Check frame reorder */ + if (gyro[i].frameReorder > 0) + { + LOGE("Error: gyro[%d] frame reorder occurred (%d)", i, gyro[i].frameReorder); + checkerFailed = true; + } + } + } + } + + for (uint8_t i = 0; i < AccelerometerProfileMax; i++) + { + /* Check if accelerometer sensor started */ + if ((accelerometer[i].enabled) && (accelerometer[i].outputMode) && (runTime[i].diffMsec == 0)) + { + LOGE("Error: Enabled accelerometer[%d] didn't start", i); + checkerFailed = true; + } + else + { + /* Check if received min frames of runtime - 1 sec */ + if (accelerometer[i].frames < (accelerometer[i].enabled * accelerometer[i].outputMode * accelerometer[i].frameRate * (runTime[i].diffMsec / 1000 - 1))) + { + LOGE("Error: Got too few accelerometer[%d] frames [Min Expected %d] [Actual %d]", i, (accelerometer[i].enabled * accelerometer[i].outputMode * accelerometer[i].frameRate * (runTime[i].diffMsec / 1000 - 1)), accelerometer[i].frames); + checkerFailed = true; + } + + if (accelerometer[i].frames > 0) + { + /* Check if received max frames of runtime + 1 sec */ + if (accelerometer[i].frames > (accelerometer[i].enabled * accelerometer[i].outputMode * accelerometer[i].frameRate * (runTime[i].diffMsec / 1000 + 1))) + { + LOGE("Error: Got too many accelerometer[%d] frames [Max Expected %d] [Actual %d]", i, (accelerometer[i].enabled * accelerometer[i].outputMode * accelerometer[i].frameRate * (runTime[i].diffMsec / 1000 + 1)), accelerometer[i].frames); + checkerFailed = true; + } + + /* Check latency less than 100 msec */ + if ((accelerometer[i].totalLatency / accelerometer[i].frames) > MAX_FRAME_LATENCY_MSEC) + { + LOGE("Error: accelerometer[%d] frame motion->host latency is too high (%d)", i, (accelerometer[i].totalLatency / accelerometer[i].frames)); + checkerFailed = true; + } + + /* Check frame drops */ + if (accelerometer[i].frameDrops > 0) + { + LOGE("Error: accelerometer[%d] frame drops occurred (%d)", i, accelerometer[i].frameDrops); + checkerFailed = true; + } + + /* Check frame reorder */ + if (accelerometer[i].frameReorder > 0) + { + LOGE("Error: accelerometer[%d] frame reorder occurred (%d)", i, accelerometer[i].frameReorder); + checkerFailed = true; + } + } + } + } + + for (uint8_t i = 0; i < SixDofProfileMax; i++) + { + uint32_t poseRunTimeMsecMin = 0; + uint32_t poseRunTimeMsecMax = 0; + uint32_t poseRunTimeMsec = 0; + + if (runTime[i].diffMsec > MAX_FIRST_POSE_DELAY_MSEC) + { + poseRunTimeMsecMin = ((runTime[i].diffMsec - 500) - MAX_FIRST_POSE_DELAY_MSEC); + poseRunTimeMsecMax = ((runTime[i].diffMsec + 500) - MAX_FIRST_POSE_DELAY_MSEC); + poseRunTimeMsec = (runTime[i].diffMsec - MAX_FIRST_POSE_DELAY_MSEC); + } + + /* Check if pose started */ + if ((pose[i].enabled) && (pose[i].outputMode) && (runTime[i].diffMsec == 0)) + { + LOGE("Error: Enabled pose[%d] didn't start", i); + checkerFailed = true; + } + else + { + /* Check if received min frames of runtime - 500 msec */ + if (pose[i].frames < ((pose[i].enabled * pose[i].outputMode * ((std::min)((uint16_t)100, pose[i].frameRate)) * poseRunTimeMsecMin) / 1000)) + { + LOGE("Error: Got too few pose[%d] frames [Expected %d] [Actual %d]", i, ((pose[i].enabled * pose[i].outputMode * pose[i].frameRate * poseRunTimeMsec) / 1000), pose[i].frames); + checkerFailed = true; + } + + if (pose[i].frames > 0) + { + /* Check if received max frames of runtime + 500 msec */ + if (pose[i].frames > ((pose[i].enabled * pose[i].outputMode * ((std::max)((uint16_t)100, pose[i].frameRate)) * poseRunTimeMsecMax) / 1000)) + { + LOGE("Error: Got too many pose[%d] frames [Expected %d] [Actual %d]", i, ((pose[i].enabled * pose[i].outputMode * pose[i].frameRate * poseRunTimeMsec) / 1000), pose[i].frames); + checkerFailed = true; + } + + /* Check latency less than 100 msec */ + if ((pose[i].totalLatency / pose[i].frames) > MAX_FRAME_LATENCY_MSEC) + { + LOGE("Error: pose[%d] frame motion->host latency is too high (%d)", i, (pose[i].totalLatency / pose[i].frames)); + checkerFailed = true; + } + + /* Check frame drops */ + if (pose[i].frameDrops > 0) + { + LOGE("Error: pose[%d] frame drops occurred (%d)", i, pose[i].frameDrops); + checkerFailed = true; + } + + /* Check frame reorder */ + if (pose[i].frameReorder > 0) + { + LOGE("Error: pose[%d] frame reorder occurred (%d)", i, pose[i].frameReorder); + checkerFailed = true; + } + + /* Check NAN frame */ + if (pose[i].nanFrame > 0) + { + LOGE("Error: pose[%d] NAN frame received (%d)", i, pose[i].nanFrame); + checkerFailed = true; + } + } + } + } + + /* Check if got error events */ + if (error.count > 0) + { + LOGE("Error: Got %d error events", error.count); + checkerFailed = true; + } + + return checkerFailed; + } + + /* Statistics comparison overload */ + bool operator == (const Statistics &ref) const + { + for (uint8_t i = 0; i < VideoProfileMax; i++) + { + if (this->video[i].frames != ref.video[i].frames) + { + return false; + } + } + + for (uint8_t i = 0; i < GyroProfileMax; i++) + { + if (this->gyro[i].frames != ref.gyro[i].frames) + { + return false; + } + } + + for (uint8_t i = 0; i < VelocimeterProfileMax; i++) + { + if (this->velocimeter[i].frames != ref.velocimeter[i].frames) + { + return false; + } + } + + for (uint8_t i = 0; i < AccelerometerProfileMax; i++) + { + if (this->accelerometer[i].frames != ref.accelerometer[i].frames) + { + return false; + } + } + + for (uint8_t i = 0; i < SixDofProfileMax; i++) + { + if (this->pose[i].frames != ref.pose[i].frames) + { + return false; + } + } + + for (uint8_t i = Controller1; i < ProfileTypeMax; i++) + { + if (this->controller[i].frames != ref.controller[i].frames) + { + return false; + } + } + + for (uint8_t i = Controller1; i < ProfileTypeMax; i++) + { + if (this->rssi[i].frames != ref.rssi[i].frames) + { + return false; + } + } + + for (uint8_t i = LocalizationTypeGet; i < LocalizationTypeMax; i++) + { + if (this->localization[i].frames != ref.localization[i].frames) + { + return false; + } + } + + return true; + } + + /* Increase error count */ + void inc(void) + { + error.count++; + if (gConfiguration.errorExit == true) + { + LOGE("Error event occurred"); + exit(EXIT_FAILURE); + } + } + + /* Increase led count */ + void inc(TrackingData::ControllerLedEventFrame& ledFrame) + { + led.count++; + } + + /* Increase status count */ + void inc(TrackingData::StatusEventFrame& statusFrame) + { + status.count++; + } + + /* Increase gyro frame count */ + void inc(TrackingData::GyroFrame& gyroFrame) + { + gyro[gyroFrame.sensorIndex].frames++; + gyro[gyroFrame.sensorIndex].totalLatency += ns2ms(systemTime() - gyroFrame.systemTimestamp); + + int64_t offset = (int64_t)gyroFrame.frameId - (int64_t)gyro[gyroFrame.sensorIndex].prevFrameId; + if (gyro[gyroFrame.sensorIndex].prevFrameTimeStamp != 0) + { + if (offset > 1) + { + gyro[gyroFrame.sensorIndex].frameDrops++; + if (gConfiguration.errorExit == true) + { + LOGE("Gyro[%d] frame drops occurred - %lld missing frames [FrameId %d-%d], time diff = %d (msec)", gyroFrame.sensorIndex, offset-1, gyro[gyroFrame.sensorIndex].prevFrameId+1, gyroFrame.frameId-1, ns2ms(gyroFrame.timestamp - gyro[gyroFrame.sensorIndex].prevFrameTimeStamp)); + exit(EXIT_FAILURE); + } + } + else if (offset < 1) + { + gyro[gyroFrame.sensorIndex].frameReorder++; + if (gConfiguration.errorExit == true) + { + LOGE("Gyro[%d] frame reorder occurred - prev frameId = %d, new frameId = %d, time diff = %d (msec)", gyroFrame.sensorIndex, gyro[gyroFrame.sensorIndex].prevFrameId, gyroFrame.frameId, ns2ms(gyroFrame.timestamp - gyro[gyroFrame.sensorIndex].prevFrameTimeStamp)); + exit(EXIT_FAILURE); + } + } + } + + gyro[gyroFrame.sensorIndex].prevFrameTimeStamp = gyroFrame.timestamp; + gyro[gyroFrame.sensorIndex].prevFrameId = gyroFrame.frameId; + } + + /* Increase velocimeter frame count */ + void inc(TrackingData::VelocimeterFrame& velocimeterFrame) + { + velocimeter[velocimeterFrame.sensorIndex].frames++; + velocimeter[velocimeterFrame.sensorIndex].totalLatency += ns2ms(systemTime() - velocimeterFrame.systemTimestamp); + + int64_t offset = (int64_t)velocimeterFrame.frameId - (int64_t)velocimeter[velocimeterFrame.sensorIndex].prevFrameId; + if (velocimeter[velocimeterFrame.sensorIndex].prevFrameTimeStamp != 0) + { + if (offset > 1) + { + velocimeter[velocimeterFrame.sensorIndex].frameDrops++; + if (gConfiguration.errorExit == true) + { + LOGE("Velocimeter[%d] frame drops occurred - %lld missing frames [FrameId %d-%d], time diff = %d (msec)", velocimeterFrame.sensorIndex, offset-1, velocimeter[velocimeterFrame.sensorIndex].prevFrameId+1, velocimeterFrame.frameId-1, ns2ms(velocimeterFrame.timestamp - velocimeter[velocimeterFrame.sensorIndex].prevFrameTimeStamp)); + exit(EXIT_FAILURE); + } + } + else if (offset < 1) + { + velocimeter[velocimeterFrame.sensorIndex].frameReorder++; + if (gConfiguration.errorExit == true) + { + LOGE("Velocimeter[%d] frame reorder occurred - prev frameId = %d, new frameId = %d, time diff = %d (msec)", velocimeterFrame.sensorIndex, velocimeter[velocimeterFrame.sensorIndex].prevFrameId, velocimeterFrame.frameId, ns2ms(velocimeterFrame.timestamp - velocimeter[velocimeterFrame.sensorIndex].prevFrameTimeStamp)); + exit(EXIT_FAILURE); + } + } + } + + velocimeter[velocimeterFrame.sensorIndex].prevFrameTimeStamp = velocimeterFrame.timestamp; + velocimeter[velocimeterFrame.sensorIndex].prevFrameId = velocimeterFrame.frameId; + } + + /* Increase accelerometer frame count */ + void inc(TrackingData::AccelerometerFrame& accelerometerFrame) + { + accelerometer[accelerometerFrame.sensorIndex].frames++; + accelerometer[accelerometerFrame.sensorIndex].totalLatency += ns2ms(systemTime() - accelerometerFrame.systemTimestamp); + + int64_t offset = (int64_t)accelerometerFrame.frameId - (int64_t)accelerometer[accelerometerFrame.sensorIndex].prevFrameId; + if (accelerometer[accelerometerFrame.sensorIndex].prevFrameTimeStamp != 0) + { + if (offset > 1) + { + accelerometer[accelerometerFrame.sensorIndex].frameDrops++; + if (gConfiguration.errorExit == true) + { + LOGE("Accelerometer[%d] frame drops occurred - %lld missing frames [FrameId %d-%d], time diff = %d (msec)", accelerometerFrame.sensorIndex, offset-1, accelerometer[accelerometerFrame.sensorIndex].prevFrameId+1, accelerometerFrame.frameId-1, ns2ms(accelerometerFrame.timestamp - accelerometer[accelerometerFrame.sensorIndex].prevFrameTimeStamp)); + exit(EXIT_FAILURE); + } + + } + else if (offset < 1) + { + accelerometer[accelerometerFrame.sensorIndex].frameReorder++; + if (gConfiguration.errorExit == true) + { + LOGE("Accelerometer[%d] frame reorder occurred - prev frameId = %d, new frameId = %d, time diff = %d (msec)", accelerometerFrame.sensorIndex, accelerometer[accelerometerFrame.sensorIndex].prevFrameId, accelerometerFrame.frameId, ns2ms(accelerometerFrame.timestamp - accelerometer[accelerometerFrame.sensorIndex].prevFrameTimeStamp)); + exit(EXIT_FAILURE); + } + } + } + + accelerometer[accelerometerFrame.sensorIndex].prevFrameTimeStamp = accelerometerFrame.timestamp; + accelerometer[accelerometerFrame.sensorIndex].prevFrameId = accelerometerFrame.frameId; + } + + /* Increase pose frame count */ + void inc(TrackingData::PoseFrame& poseFrame) + { + pose[poseFrame.sourceIndex].frames++; + pose[poseFrame.sourceIndex].totalLatency += ns2ms(systemTime() - poseFrame.systemTimestamp); + + if ((gConfiguration.errorCheck == true) && isnan(poseFrame.translation.x) && (poseFrame.trackerConfidence != 0)) + { + pose[poseFrame.sourceIndex].nanFrame++; + if (gConfiguration.errorExit == true) + { + LOGE("Got NAN Pose[%u] (%" PRId64 "): Timestamp %" PRId64 ", Translation[%f, %f, %f], TrackerConfidence = 0x%X", poseFrame.sourceIndex, pose[poseFrame.sourceIndex].frames, poseFrame.timestamp, poseFrame.translation.x, poseFrame.translation.y, poseFrame.translation.z, poseFrame.trackerConfidence); + exit(EXIT_FAILURE); + } + } + + /* Pose must arrive every 5 msec */ + int64_t offset = (poseFrame.timestamp - pose[poseFrame.sourceIndex].prevFrameTimeStamp) / 5000000; + if (pose[poseFrame.sourceIndex].prevFrameTimeStamp != 0) + { + if (offset > 5) + { + pose[poseFrame.sourceIndex].frameDrops++; + if (gConfiguration.errorExit == true) + { + LOGE("Pose[%d] frame drops occurred - %lld missing frames, prev pose time = %lld, new pose time = %lld, time diff = %d (msec)", poseFrame.sourceIndex, offset-1, pose[poseFrame.sourceIndex].prevFrameTimeStamp, poseFrame.timestamp, ns2ms(poseFrame.timestamp - pose[poseFrame.sourceIndex].prevFrameTimeStamp)); + exit(EXIT_FAILURE); + } + } + else if (offset < 0) + { + pose[poseFrame.sourceIndex].frameReorder++; + if (gConfiguration.errorExit == true) + { + LOGE("Pose[%d] frame reorder occurred - prev pose time = %lld, new pose time = %lld, time diff = %d (msec)", poseFrame.sourceIndex, pose[poseFrame.sourceIndex].prevFrameTimeStamp, poseFrame.timestamp, ns2ms(poseFrame.timestamp - pose[poseFrame.sourceIndex].prevFrameTimeStamp)); + exit(EXIT_FAILURE); + } + } + } + + pose[poseFrame.sourceIndex].prevFrameTimeStamp = poseFrame.timestamp; + pose[poseFrame.sourceIndex].trackerConfidence = poseFrame.trackerConfidence; + } + + /* Increase video (left/right FishEye) frame count */ + void inc(TrackingData::VideoFrame& videoFrame) + { + video[videoFrame.sensorIndex].frames++; + video[videoFrame.sensorIndex].totalLatency += ns2ms(systemTime() - videoFrame.systemTimestamp); + + int64_t offset = (int64_t)videoFrame.frameId - (int64_t)video[videoFrame.sensorIndex].prevFrameId; + if (video[videoFrame.sensorIndex].prevFrameTimeStamp != 0) + { + if (offset > 1) + { + video[videoFrame.sensorIndex].frameDrops++; + if (gConfiguration.errorExit == true) + { + LOGE("Video[%d] frame drops occurred - %lld missing frames [FrameId %d-%d], time diff = %d (msec)", videoFrame.sensorIndex, offset-1, video[videoFrame.sensorIndex].prevFrameId+1, videoFrame.frameId-1, ns2ms(videoFrame.timestamp - video[videoFrame.sensorIndex].prevFrameTimeStamp)); + exit(EXIT_FAILURE); + } + } + else if (offset < 1) + { + video[videoFrame.sensorIndex].frameReorder++; + if (gConfiguration.errorExit == true) + { + LOGE("Video[%d] frame reorder occurred - prev frameId = %d, new frameId = %d, time diff = %d (msec)", videoFrame.sensorIndex, video[videoFrame.sensorIndex].prevFrameId, videoFrame.frameId, ns2ms(videoFrame.timestamp - video[videoFrame.sensorIndex].prevFrameTimeStamp)); + exit(EXIT_FAILURE); + } + } + } + + video[videoFrame.sensorIndex].prevFrameTimeStamp = videoFrame.timestamp; + video[videoFrame.sensorIndex].prevFrameId = videoFrame.frameId; + } + + /* Increase controller frame count */ + void inc(TrackingData::ControllerFrame& controllerFrame) + { + controller[controllerFrame.sensorIndex].frames++; + controller[controllerFrame.sensorIndex].totalLatency += ns2ms(systemTime() - controllerFrame.systemTimestamp); + } + + /* Add new controller discovery event statistics and increase controller discovery event frame count */ + void inc(TrackingData::ControllerDiscoveryEventFrame& frame) + { + auto it = std::find_if(controllerDiscoveryEventVector.begin(), controllerDiscoveryEventVector.end(), [&frame](std::shared_ptr v) -> bool + { return !memcmp(v->macAddress, frame.macAddress, MAC_ADDRESS_SIZE);}); + + if (it == controllerDiscoveryEventVector.end()) + { + std::shared_ptr statistics(new ControllerDiscoveryEventStatistics(frame.macAddress)); + statistics->frames++; + controllerDiscoveryEventVector.push_back(statistics); + } + else + { + (*it)->frames++; + } + } + + /* Increase Rssi frame count */ + void inc(TrackingData::RssiFrame& rssiFrame) + { + rssi[rssiFrame.sensorIndex].frames++; + rssi[rssiFrame.sensorIndex].totalLatency += ns2ms(systemTime() - rssiFrame.systemTimestamp); + } + + /* Increase Localization Get/Set frame count */ + void inc(TrackingData::LocalizationDataFrame& localizationFrame) + { + LocalizationType type = LocalizationTypeGet; + + if (localizationFrame.buffer == NULL) + { + type = LocalizationTypeSet; + } + + localization[type].frames++; + localization[type].fileSize += localizationFrame.length; + + if (localizationFrame.moreData == false) + { + localization[type].isCompleted = true; + } + + } + + /* Check if controller mac address is already connected */ + bool isConnected(uint8_t* macAddress) + { + auto it = std::find_if(controllerDiscoveryEventVector.begin(), controllerDiscoveryEventVector.end(), [macAddress](std::shared_ptr v) -> bool + { + return !memcmp(v->macAddress, macAddress, MAC_ADDRESS_SIZE); } + ); + + return (it != controllerDiscoveryEventVector.end()); + } + + /* Configure Video sensor rate and outpurMode for statistics calculations */ + void configure(TrackingData::VideoProfile profile, uint8_t enabled, uint8_t outputMode) + { + video[profile.sensorIndex].frameRate = profile.fps; + video[profile.sensorIndex].enabled = enabled; + video[profile.sensorIndex].outputMode = outputMode; + } + + /* Configure Gyro sensor rate and outpurMode for statistics calculations */ + void configure(TrackingData::GyroProfile profile, uint8_t enabled, uint8_t outputMode) + { + gyro[profile.sensorIndex].frameRate = profile.fps; + gyro[profile.sensorIndex].enabled = enabled; + gyro[profile.sensorIndex].outputMode = outputMode; + } + + /* Configure Velocimeter sensor rate and outpurMode for statistics calculations */ + void configure(TrackingData::VelocimeterProfile profile, uint8_t enabled, uint8_t outputMode) + { + velocimeter[profile.sensorIndex].frameRate = profile.fps; + velocimeter[profile.sensorIndex].enabled = enabled; + velocimeter[profile.sensorIndex].outputMode = outputMode; + } + + /* Configure Accelerometer sensor rate and outpurMode for statistics calculations */ + void configure(TrackingData::AccelerometerProfile profile, uint8_t enabled, uint8_t outputMode) + { + accelerometer[profile.sensorIndex].frameRate = profile.fps; + accelerometer[profile.sensorIndex].enabled = enabled; + accelerometer[profile.sensorIndex].outputMode = outputMode; + } + + /* Configure 6dof rate and outpurMode for statistics calculations */ + void configure(TrackingData::SixDofProfile profile, uint8_t enabled, uint16_t frameRate) + { + pose[profile.profileType].frameRate = frameRate; + pose[profile.profileType].enabled = enabled; + pose[profile.profileType].outputMode = enabled; + } + + RunTimeStatistics runTime[ProfileTypeMax]; + ErrorStatistics error; + StatusStatistics status; + LedStatistics led; + VideoStatistics video[VideoProfileMax]; + GyroStatistics gyro[GyroProfileMax]; + VelocimeterStatistics velocimeter[VelocimeterProfileMax]; + AccelerometerStatistics accelerometer[AccelerometerProfileMax]; + PoseStatistics pose[SixDofProfileMax]; + ControllerStatistics controller[ProfileTypeMax]; + RssiStatistics rssi[ProfileTypeMax]; + LocalizationStatistics localization[LocalizationTypeMax]; + std::vector> controllerDiscoveryEventVector; + + std::mutex videoFrameListMutex; + std::list videoList; +}; + +std::atomic_bool gIsDisposed(false); +TrackingDevice* gDevice; +Statistics gStatistics; +CalibrationInfo calibrationInfo; +uint32_t resetCount = 0; + +void updateControllerFW(TrackingData::ControllerDiscoveryEventFrame& message, ProfileType controllerId) +{ + struct FwFileHeader + { + uint32_t headerSize; + uint32_t dataSize; + uint32_t fileFormatVersion; + uint32_t versionSize; + uint8_t version[7]; /* 3 bytes for major,minor,patch, 4 byte for build number */ + }; + + std::ifstream file(gConfiguration.controllerFWFile, std::ios::binary); + if (!file) + { + LOGE("Error: controller fw file %s doesn't exists", gConfiguration.controllerFWFile.c_str()); + return; + } + + uint32_t headerSize = 0; + if (!file.read((char*)&headerSize, sizeof(headerSize))) + { + LOGE("Error: Failed to read controller file %s", gConfiguration.controllerFWFile.c_str()); + return; + } + + file.seekg(0); + + if (headerSize == 0) + { + LOGE("Error: Invalid controller fw file header size"); + return; + } + std::vector headerArr(headerSize, 0); + FwFileHeader* header = (FwFileHeader*)headerArr.data(); + if (!file.read((char*)header, headerSize)) + { + LOGE("Error: Failed to read header in controller file %s", gConfiguration.controllerFWFile.c_str()); + return; + } + + TrackingData::Version discoverdVersion; + if (gConfiguration.controllerFWFileType == "app") + { + discoverdVersion = message.app; + } + else if (gConfiguration.controllerFWFileType == "bl") + { + discoverdVersion = message.bootLoader; + } + else + { + LOGE("Invalid image type input for burning controller fw: %s", gConfiguration.controllerFWFileType); + return; + } + + if ((header->versionSize != 3) && (header->versionSize != 7)) + { + LOGE("Unsupported BLE image version size (%d)", header->versionSize); + return; + } + + /* Burning controller image only if image and discovery versions are different or configured to force burn */ + if (header->version[0] != discoverdVersion.major || + header->version[1] != discoverdVersion.minor || + header->version[2] != discoverdVersion.patch || + gConfiguration.controllers[controllerId].burn.configure == ControllerBurnForce) + { + LOGD("Updating controller FW %s application with new version %u.%u.%u", (gConfiguration.controllerFWFileType == "app")?"Application":"Boot Loader", header->version[0], header->version[1], header->version[2]); + + std::vector imageArr(header->dataSize, 0); + file.read((char*)imageArr.data(), header->dataSize); + + TrackingData::ControllerFW fw(message.macAddress, header->dataSize, imageArr.data(), message.addressType); + auto status = gDevice->ControllerFWUpdate(fw); + if (status != Status::SUCCESS) + { + LOGE("Failed to update controller %s firmware: %s (0x%X)", gConfiguration.controllerFWFileType.c_str(), statusToString(status).c_str(), status); + return; + } + + /* Burn succeeded, no need to burn this controller again */ + gConfiguration.controllers[controllerId].burn.state = ControllerBurnStateProcessing; + return; + } + + LOGD("Controller %d versions are equal [%u.%u.%u], skipping controller FW update", controllerId, header->version[0], header->version[1], header->version[2]); + + /* Versions are equal, no need to burn this controller */ + gConfiguration.controllers[controllerId].burn.state = ControllerBurnStateDone; + return; +} + +class CommonListener : public TrackingManager::Listener, public TrackingDevice::Listener +{ +public: + + // [TrackingManager::Listener] + virtual void onStateChanged(TrackingManager::EventType state, TrackingDevice* device, TrackingData::DeviceInfo deviceInfo) + { + switch (state) + { + case TrackingManager::ATTACH: + gDevice = device; + LOGD("Device (0x%p) Attached - Serial Number %llx", device, (deviceInfo.serialNumber >> 16)); + break; + + case TrackingManager::DETACH: + LOGD("Device (0x%p) Detached - Serial Number %llx", device, (deviceInfo.serialNumber >> 16)); + gDevice = NULL; + break; + } + }; + + virtual void onError(Status error, TrackingDevice* dev) override + { + gStatistics.inc(); + LOGE("Error occurred while connecting device: %p Error: %s (0x%X)", dev, statusToString(error).c_str(), error); + } + + virtual void onPoseFrame(TrackingData::PoseFrame& pose) + { + gStatistics.inc(pose); + + LOGV("Got Pose[%u] (%" PRId64 "): Timestamp %" PRId64 ", Translation[%f, %f, %f], TrackerConfidence = 0x%X", + pose.sourceIndex, gStatistics.pose[pose.sourceIndex].frames, pose.timestamp, pose.translation.x, pose.translation.y, pose.translation.z, pose.trackerConfidence); + + if (gConfiguration.statistics == true) + { + static UtilTimeStamps timeStamp(pose); + timeStamp.setTime(pose); + + poseCSV << std::fixed << (unsigned int)(pose.sourceIndex) << "," << gStatistics.pose[pose.sourceIndex].frames << "," + << timeStamp.hostCurrentSystemTime << "," << pose.systemTimestamp << "," << timeStamp.fwTimeStamp << "," << timeStamp.latency << "," + << pose.translation.x << "," << pose.translation.y << "," << pose.translation.z << "," + << pose.velocity.x << "," << pose.velocity.y << "," << pose.velocity.z << "," + << pose.acceleration.x << "," << pose.acceleration.y << "," << pose.acceleration.z << "," + << pose.rotation.i << "," << pose.rotation.j << "," << pose.rotation.k << "," << pose.rotation.r << "," + << pose.angularVelocity.x << "," << pose.angularVelocity.y << "," << pose.angularVelocity.z << "," + << pose.angularAcceleration.x << "," << pose.angularAcceleration.y << "," << pose.angularAcceleration.z << "," + << pose.trackerConfidence << "," << pose.mapperConfidence << "\n"; + } + + switch (gConfiguration.tumFormat) + { + case 1: + /* TUM */ + poseTUM << std::fixed << static_cast(pose.timestamp)/1000000000 << " " << pose.translation.x << " " << pose.translation.y << " " << pose.translation.z << " " << pose.rotation.i << " " << pose.rotation.j << " " << pose.rotation.k << " " << pose.rotation.r << "\n"; + break; + case 2: + /* TUMX */ + poseTUM << std::fixed << static_cast(pose.timestamp)/1000000000 << " " << pose.translation.x << " " << pose.translation.y << " " << pose.translation.z << " " << pose.rotation.i << " " << pose.rotation.j << " " << pose.rotation.k << " " << pose.rotation.r << " " << (unsigned int)(pose.sourceIndex) << "\n"; + break; + default: + break; + } + }; + + virtual void onVideoFrame(TrackingData::VideoFrame& frame) + { + gStatistics.inc(frame); + static UtilTimeStamps timeStamp(frame); + timeStamp.setTime(frame); + + LOGV("Got Video[%u] frame (%" PRId64 "): Timestamp %" PRId64 ", FrameId = %u, Exposure = %u, Gain = %f, FrameLength = %u, size %dx%d pixels", frame.sensorIndex, gStatistics.video[frame.sensorIndex].frames, frame.timestamp, + frame.frameId, frame.exposuretime, frame.gain, frame.frameLength, frame.profile.width, frame.profile.height); + + if (((frame.frameLength < (uint32_t)(frame.profile.height * frame.profile.stride))) || (((frame.frameLength < (uint32_t)(frame.profile.height * frame.profile.width))))) + { + LOGE("Error: Video[%u] frame (%" PRId64 ") size mismatch: FrameId = %u, FrameLength = %u, Width = %u, Height = %u, Stride = %u", + frame.sensorIndex, gStatistics.video[frame.sensorIndex].frames, frame.frameId, frame.frameLength, frame.profile.width, frame.profile.height, frame.profile.stride); + } + + if (gConfiguration.statistics == true) + { + videoCSV << std::fixed << (unsigned int)(frame.sensorIndex) << "," << gStatistics.video[frame.sensorIndex].frames << "," + << timeStamp.hostCurrentSystemTime << "," << frame.systemTimestamp << "," << timeStamp.fwTimeStamp << "," << timeStamp.arrivalTimeStamp << "," << timeStamp.latency << "," + << frame.frameId << "," << frame.frameLength << "," << frame.exposuretime << "," << frame.gain << "," << frame.profile.stride << "," << frame.profile.width << "," << frame.profile.height << "," << frame.profile.pixelFormat << "\n"; + } + + if ((gConfiguration.videoFile == true) && (frame.frameLength == (frame.profile.width * frame.profile.height))) + { + TrackingData::VideoFrame* newFrame = new TrackingData::VideoFrame(frame); + auto imageSize = (newFrame->profile.width * newFrame->profile.height); + uint8_t* data = (uint8_t*)malloc(frame.frameLength); + newFrame->data = data; + perc::copy((uint8_t*)newFrame->data, frame.data, frame.frameLength); + gStatistics.videoFrameListMutex.lock(); + gStatistics.videoList.push_back(newFrame); + gStatistics.videoFrameListMutex.unlock(); + } + }; + + virtual void onAccelerometerFrame(TrackingData::AccelerometerFrame& message) + { + gStatistics.inc(message); + + auto calibratedAccelerationX = ((message.acceleration.x * calibrationInfo.accelerometerScale[0][0]) - calibrationInfo.accelerometerBiasX); + auto calibratedAccelerationY = ((message.acceleration.y * calibrationInfo.accelerometerScale[1][1]) - calibrationInfo.accelerometerBiasY); + auto calibratedAccelerationZ = ((message.acceleration.z * calibrationInfo.accelerometerScale[2][2]) - calibrationInfo.accelerometerBiasZ); + auto magnitude = sqrt((calibratedAccelerationX * calibratedAccelerationX) + (calibratedAccelerationY * calibratedAccelerationY) + (calibratedAccelerationZ * calibratedAccelerationZ)); + + LOGV("Got Accelerometer[%u] frame (%" PRId64 "): Timestamp %" PRId64 ", FrameID = %d, Temperature = %.0f, Acceleration[%f, %f, %f], Calibrated Acceleration[%f, %f, %f], Magnitude = %f", + message.sensorIndex, gStatistics.accelerometer[message.sensorIndex].frames, message.timestamp, message.frameId, message.temperature, message.acceleration.x, message.acceleration.y, message.acceleration.z, + calibratedAccelerationX, calibratedAccelerationY, calibratedAccelerationZ, magnitude); + + if (gConfiguration.statistics == true) + { + static UtilTimeStamps timeStamp(message); + timeStamp.setTime(message); + + accelerometerCSV << std::fixed << unsigned(message.sensorIndex) << "," << gStatistics.accelerometer[message.sensorIndex].frames << "," + << timeStamp.hostCurrentSystemTime << "," << message.systemTimestamp << "," << timeStamp.fwTimeStamp << "," << timeStamp.arrivalTimeStamp << "," << timeStamp.latency << "," + << message.frameId << "," << message.temperature << "," << message.acceleration.x << "," << message.acceleration.y << "," << message.acceleration.z << "," + << calibratedAccelerationX << "," << calibratedAccelerationY << "," << calibratedAccelerationZ << "," << magnitude << "\n"; + } + }; + + virtual void onGyroFrame(TrackingData::GyroFrame& message) + { + gStatistics.inc(message); + + LOGV("Got Gyro[%u] frame (%" PRId64 "): Timestamp %" PRId64 ", FrameID = %d, Temperature = %.0f, AngularVelocity[%f, %f, %f]", message.sensorIndex, gStatistics.gyro[message.sensorIndex].frames, + message.timestamp, message.frameId, message.temperature, message.angularVelocity.x, message.angularVelocity.y, message.angularVelocity.z); + + if (gConfiguration.statistics == true) + { + static UtilTimeStamps timeStamp(message); + timeStamp.setTime(message); + + gyroCSV << std::fixed << unsigned(message.sensorIndex) << "," << gStatistics.gyro[message.sensorIndex].frames << "," + << timeStamp.hostCurrentSystemTime << "," << message.systemTimestamp << "," << timeStamp.fwTimeStamp << "," << timeStamp.arrivalTimeStamp << "," << timeStamp.latency << "," + << message.frameId << "," << message.temperature << "," << message.angularVelocity.x << "," << message.angularVelocity.y << "," << message.angularVelocity.z << "\n"; + } + }; + + virtual void onVelocimeterFrame(TrackingData::VelocimeterFrame& message) + { + gStatistics.inc(message); + + LOGV("Got Velocimeter[%u] frame (%" PRId64 "): Timestamp %" PRId64 ", FrameID = %d, Temperature = %.0f, AngularVelocity[%f, %f, %f]", message.sensorIndex, gStatistics.velocimeter[message.sensorIndex].frames, + message.timestamp, message.frameId, message.temperature, message.angularVelocity.x, message.angularVelocity.y, message.angularVelocity.z); + + if (gConfiguration.statistics == true) + { + static UtilTimeStamps timeStamp(message); + timeStamp.setTime(message); + + velocimeterCSV << std::fixed << unsigned(message.sensorIndex) << "," << gStatistics.velocimeter[message.sensorIndex].frames << "," + << timeStamp.hostCurrentSystemTime << "," << message.systemTimestamp << "," << timeStamp.fwTimeStamp << "," << timeStamp.arrivalTimeStamp << "," << timeStamp.latency << "," + << message.frameId << "," << message.temperature << "," << message.angularVelocity.x << "," << message.angularVelocity.y << "," << message.angularVelocity.z << "\n"; + } + }; + + virtual void onControllerDiscoveryEventFrame(TrackingData::ControllerDiscoveryEventFrame& message) + { + //LOGD("Got Controller Discovery Event: on MacAddress [%02X:%02X:%02X:%02X:%02X:%02X], AddressType [0x%X], Manufacturer ID [0x%X], Vendor Data [0x%X], App Version [%u.%u.%u], Boot Loader Version [%u.%u.%u], Soft Device Version [%u], Protocol Version [%u]", + // message.macAddress[0], message.macAddress[1], message.macAddress[2], message.macAddress[3], message.macAddress[4], message.macAddress[5], + // message.addressType, + // message.manufacturerId, + // message.vendorData, + // message.app.major, message.app.minor, message.app.patch, + // message.bootLoader.major, message.bootLoader.minor, message.bootLoader.patch, + // message.softDevice.major, + // message.protocol.major); + + if (gConfiguration.controllersCount > 0) + { + ProfileType controllerId = ProfileTypeMax; + + /* Check if controller is in argument list and we didn't send connect request yet */ + if (((controllerId = gConfiguration.findController(message.macAddress)) != ProfileTypeMax) && (gStatistics.isConnected(message.macAddress) == 0)) + { + /* Burning controller FW */ + if (gConfiguration.controllers[controllerId].burn.configure > ControllerBurnDisabled) + { + + switch (gConfiguration.controllers[controllerId].burn.state) + { + case ControllerBurnStateNotStarted: + /* Burn not started yet, initiating burn process */ + LOGD("Start updating FW of controller %d MacAddress %02X:%02X:%02X:%02X:%02X:%02X", controllerId, message.macAddress[0], message.macAddress[1], message.macAddress[2], message.macAddress[3], message.macAddress[4], message.macAddress[5]); + updateControllerFW(message, controllerId); + + /* If update controller FW succeeded, skipping connection attempt until next discovery event */ + /* If update controller FW failed, return and try to burn again on next discovery */ + return; + break; + + case ControllerBurnStateProcessing: + /* Burn is already in progress, exiting */ + LOGD("FW Update is already in progress on controller %d MacAddress %02X:%02X:%02X:%02X:%02X:%02X", controllerId, message.macAddress[0], message.macAddress[1], message.macAddress[2], message.macAddress[3], message.macAddress[4], message.macAddress[5]); + return; + break; + + case ControllerBurnStateDone: + /* Burn is done, continue to controller connect */ + LOGD("Controller FW is updated on controller %d MacAddress %02X:%02X:%02X:%02X:%02X:%02X", controllerId, message.macAddress[0], message.macAddress[1], message.macAddress[2], message.macAddress[3], message.macAddress[4], message.macAddress[5]); + break; + } + } + + Status status = Status::SUCCESS; + uint8_t controllerId = ProfileTypeMax; + + /* Found our controller, trying to connect */ + LOGD("Connecting to Controller with MacAddress = %02X:%02X:%02X:%02X:%02X:%02X, AddressType [0x%X]", message.macAddress[0], message.macAddress[1], message.macAddress[2], message.macAddress[3], message.macAddress[4], message.macAddress[5], message.addressType); + + TrackingData::ControllerDeviceConnect device(message.macAddress, CONTROLLER_CONNECT_TIMEOUT_MSEC, message.addressType); + status = gDevice->ControllerConnect(device, controllerId); + if (status != Status::SUCCESS) + { + LOGE("Error: Failed connecting controller MacAddress = %02X:%02X:%02X:%02X:%02X:%02X , Status %s (0x%X)", + message.macAddress[0], message.macAddress[1], message.macAddress[2], message.macAddress[3], message.macAddress[4], message.macAddress[5], statusToString(status).c_str(), status); + } + else + { + gStatistics.inc(message); + } + } + } + else + { + LOGE("Error: Got Controller DiscoveryEvent on MacAddress = %02X:%02X:%02X:%02X:%02X:%02X while controllers are disabled", + message.macAddress[0], message.macAddress[1], message.macAddress[2], message.macAddress[3], message.macAddress[4], message.macAddress[5]); + } + }; + + virtual void onControllerConnectedEventFrame(TrackingData::ControllerConnectedEventFrame& message) + { + if ((message.controllerId >= ProfileTypeMax) || (message.controllerId < Controller1)) + { + LOGE("Error: Got Bad Controller Connected Event: Status = %s (0x%X), ControllerID = %d, ManufactorId = 0x%X, Protocol = %u, App = %u.%u.%u, SoftDevice = %u, BootLoader = %u.%u.%u", + statusToString(message.status).c_str(), message.status, message.controllerId, message.manufacturerId, message.protocol.major, message.app.major, message.app.minor, message.app.build, message.softDevice.major, message.bootLoader.major, message.bootLoader.minor, message.bootLoader.build); + return; + } + + LOGD("Got Controller[%d] Connected Event with status = %s (0x%X)", message.controllerId, statusToString(message.status).c_str(), message.status); + + if (message.status == Status::SUCCESS) + { + gStatistics.controller[message.controllerId].isConnected = true; + + if (gConfiguration.controllers[message.controllerId].calibrate == true) + { + LOGD("Start calibrating controller %d", message.controllerId); + Status status = gDevice->ControllerStartCalibration(message.controllerId); + if (status != Status::SUCCESS) + { + LOGE("Error: Failed calibrating controller %d, Status %s (0x%X)", message.controllerId, statusToString(status).c_str(), status); + gStatistics.runTime[message.controllerId].start(); + } + } + else + { + gStatistics.runTime[message.controllerId].start(); + } + } + else + { + LOGE("Error: Failed connecting to Controller[%d] - status = %s (0x%X)", message.controllerId, statusToString(message.status).c_str(), message.status); + } + }; + + virtual void onControllerDisconnectedEventFrame(TrackingData::ControllerDisconnectedEventFrame& message) + { + if ((message.controllerId >= ProfileTypeMax) || (message.controllerId < Controller1)) + { + LOGE("Error: Got Bad Controller Disconnected Event on ControllerID = %d", message.controllerId); + return; + } + + LOGD("Got Controller Disconnected Event on ControllerID = %d", message.controllerId); + gStatistics.controller[message.controllerId].isConnected = false; + gStatistics.controller[message.controllerId].isCalibrated = false; + gStatistics.runTime[message.controllerId].stop(); + }; + + + virtual void onControllerCalibrationEventFrame(TrackingData::ControllerCalibrationEventFrame& message) + { + if ((message.controllerId >= ProfileTypeMax) || (message.controllerId < Controller1)) + { + LOGE("Error: Got Bad Controller Calibration Event on ControllerID = %d", message.controllerId); + return; + } + + if (message.status != Status::SUCCESS) + { + LOGE("Error: Got Controller Calibration Event on ControllerID = %d, Status %s (0x%X)", message.controllerId, statusToString(message.status).c_str(), message.status); + return; + } + + LOGD("Got Controller[%d] Calibration Event with status %s (0x%X)", message.controllerId, statusToString(message.status).c_str(), message.status); + + gStatistics.controller[message.controllerId].isCalibrated = true; + gStatistics.runTime[message.controllerId].start(); + }; + + virtual void onControllerFrame(TrackingData::ControllerFrame& message) + { + gStatistics.inc(message); + + if (gStatistics.controller[message.sensorIndex].isConnected == true) + { + LOGD("Got Controller[%u] frame: Timestamp %" PRId64 ", EventId = 0x%X, InstanceId = 0x%X, SensorData = %02X:%02X:%02X:%02X:%02X:%02X", message.sensorIndex, message.timestamp, + message.eventId, message.instanceId, message.sensorData[0], message.sensorData[1], message.sensorData[2], message.sensorData[3], message.sensorData[4], message.sensorData[5]); + } + else + { + LOGE("Error: Got Controller[%u] frame on disconnected controller: Timestamp %" PRId64 ", EventId = 0x%X, InstanceId = 0x%X, SensorData = %02X:%02X:%02X:%02X:%02X:%02X", message.sensorIndex, message.timestamp, + message.eventId, message.instanceId, message.sensorData[0], message.sensorData[1], message.sensorData[2], message.sensorData[3], message.sensorData[4], message.sensorData[5]); + } + + if (gConfiguration.statistics == true) + { + static UtilTimeStamps timeStamp(message); + timeStamp.setTime(message); + + controllerCSV << std::fixed << unsigned(message.sensorIndex) << "," << gStatistics.controller[message.sensorIndex].frames << "," + << timeStamp.hostCurrentSystemTime << "," << message.systemTimestamp << "," << message.timestamp << "," << timeStamp.fwTimeStamp << "," << message.arrivalTimeStamp << "," << timeStamp.arrivalTimeStamp << "," << timeStamp.latency << "," + << unsigned(message.eventId) << "," << unsigned(message.instanceId) << "," + << unsigned(message.sensorData[0]) << "," << unsigned(message.sensorData[1]) << "," << unsigned(message.sensorData[2]) << "," + << unsigned(message.sensorData[3]) << "," << unsigned(message.sensorData[4]) << "," << unsigned(message.sensorData[5]) << "\n"; + } + }; + + virtual void onRssiFrame(TrackingData::RssiFrame& message) + { + gStatistics.inc(message); + + LOGV("Got Rssi[%u] frame (%" PRId64 "): Timestamp %" PRId64 ", FrameID = %d, SignalStrength = %.0f", message.sensorIndex, gStatistics.rssi[message.sensorIndex].frames, message.timestamp, message.frameId, message.signalStrength); + + if (gConfiguration.statistics == true) + { + static UtilTimeStamps timeStamp(message); + timeStamp.setTime(message); + + rssiCSV << std::fixed << unsigned(message.sensorIndex) << "," << gStatistics.rssi[message.sensorIndex].frames << "," + << timeStamp.hostCurrentSystemTime << "," << message.systemTimestamp << "," << timeStamp.fwTimeStamp << "," << timeStamp.arrivalTimeStamp << "," << timeStamp.latency << "," + << message.frameId << "," << message.signalStrength << "\n"; + } + }; + + virtual void onLocalizationDataEventFrame(TrackingData::LocalizationDataFrame& message) + { + gStatistics.inc(message); + + /* On Get localization map, saving to output file */ + if (message.length > 0) + { + LOGD("Got Localization Data frame: status = %s (0x%X), moredata = %s, chunkIndex = %d, length = %d", statusToString(message.status).c_str(), message.status, (message.moreData) ? "True" : "False", message.chunkIndex, message.length); + + std::fstream & mapFile = gConfiguration.localization[LocalizationTypeGet].file; + string filename = gConfiguration.localization[LocalizationTypeGet].filename; + if (mapFile.is_open()) + { + mapFile.write((const char*)message.buffer, message.length); + } + else + { + LOGE("Error: Can't write localization data to file %s", filename.c_str()); + } + } + else + { + LOGD("Got Set Localization Data frame complete: status = %s (0x%X)", statusToString(message.status).c_str(), message.status); + } + }; + + virtual void onFWUpdateEvent(OUT TrackingData::ControllerFWEventFrame& frame) override + { + ProfileType controllerId = gConfiguration.findController(frame.macAddress); + if (controllerId != ProfileTypeMax) + { + gStatistics.controller[controllerId].FWProgress = frame.progress; + LOGV("Got controller %d FW update Mac Address [%02X:%02X:%02X:%02X:%02X:%02X], progress %u%%, status 0x%X", controllerId, + frame.macAddress[0], frame.macAddress[1], frame.macAddress[2], frame.macAddress[3], frame.macAddress[4], frame.macAddress[5], frame.progress, frame.status); + + if (frame.progress == 100) + { + gConfiguration.controllers[controllerId].burn.state = ControllerBurnStateDone; + } + } + } + + virtual void onStatusEvent(OUT TrackingData::StatusEventFrame& frame) override + { + gStatistics.inc(frame); + LOGD("Got status = %s (0x%X)", statusToString(frame.status).c_str(), frame.status); + } + + virtual void onControllerLedEvent(OUT TrackingData::ControllerLedEventFrame& frame) override + { + gStatistics.inc(frame); + LOGD("Got Controller[%d] Led[%d] intensity %" PRId64 " Event", frame.controllerId, frame.ledId, frame.intensity); + } +}; + +void handleThreadFunction() +{ + while (!gIsDisposed) + { + if (!gManagerInstance) + { + std::this_thread::yield(); + continue; + } + gManagerInstance->handleEvents(); + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } +} + + +void saveImage(TrackingData::VideoFrame* frame) +{ + std::ostringstream fwTimeStamp; + fwTimeStamp << std::setw(16) << std::setfill('0') << frame->timestamp; + std::string fileHeaderName(gFileHeaderName); + std::ofstream save_file("Image_" + fileHeaderName + "_Libtm_" + std::to_string(LIBTM_VERSION_MAJOR) + "_" + std::to_string(LIBTM_VERSION_MINOR) + "_" + std::to_string(LIBTM_VERSION_PATCH) + + "_FW_" + FW_VERSION + + "_FW_TimeStamp_" + fwTimeStamp.str() + + "_FE" + std::to_string(frame->sensorIndex) + + "_FrameID_" + std::to_string(frame->frameId) + + ".pgm", std::ofstream::binary); + + std::string pgm_header = "P5 " + std::to_string(frame->profile.width) + " " + std::to_string(frame->profile.height) + " 255\n"; + save_file.write(pgm_header.c_str(), pgm_header.size()); + save_file.write((char*)frame->data, frame->profile.height * frame->profile.stride); + save_file.close(); +} + +void imageThreadFunction() +{ + uint32_t frameCount = 0; + + LOGD("Starting Image thread"); + + while (true) + { + if (!gManagerInstance) + { + std::this_thread::yield(); + continue; + } + + while (gStatistics.videoList.size()) + { + gStatistics.videoFrameListMutex.lock(); + auto frame = gStatistics.videoList.front(); + gStatistics.videoList.pop_front(); + gStatistics.videoFrameListMutex.unlock(); + saveImage(frame); + + frameCount++; + free((void*)frame->data); + } + + if(gIsDisposed) + { + while (gStatistics.videoList.size()) + { + gStatistics.videoFrameListMutex.lock(); + auto frame = gStatistics.videoList.front(); + gStatistics.videoList.pop_front(); + gStatistics.videoFrameListMutex.unlock(); + + saveImage(frame); + + frameCount++; + free((void*)frame->data); + } + + LOGD("Closing Image thread - saved %d images (PGM)", frameCount); + return; + } + } // end of while +} + +void hostLogThreadFunction() +{ + TrackingData::Log log; + FILE *logThreadStream = stdout; + uint32_t waitForLogMsec = MAX_WAIT_FOR_LOG_MSEC; + + std::string fileHeaderName(gFileHeaderName); + std::string fileName = "Host_Log_" + fileHeaderName + "_Libtm_" + std::to_string(LIBTM_VERSION_MAJOR) + "_" + std::to_string(LIBTM_VERSION_MINOR) + "_" + std::to_string(LIBTM_VERSION_PATCH) + "_FW_" + FW_VERSION + ".log"; + + auto rc = fopen_s(&logThreadStream, fileName.c_str(), "w"); + if (rc != 0) + { + LOGE("Error while opening file %s", fileName.c_str()); + return; + } + + LOGD("Starting Host Log thread"); + fprintf(logThreadStream, "Host log: Libtm %u.%u.%u.%u, FW %s\n\n", LIBTM_VERSION_MAJOR, LIBTM_VERSION_MINOR, LIBTM_VERSION_PATCH, LIBTM_VERSION_BUILD, FW_VERSION); + + while (true) + { + if (!gManagerInstance) + { + std::this_thread::yield(); + continue; + } + + gManagerInstance.get()->getHostLog(&log); + + if (log.entries > 0) + { + fprintf(logThreadStream, "Received %d Host log entries from last %d msec (max entries %d)\n", log.entries, waitForLogMsec, log.maxEntries); + } + + for (uint32_t i = 0; i < log.entries; i++) + { + char deviceId[30] = { 0 }; + short device = ((uintptr_t)log.entry[i].deviceID & 0xFFFF); + if (device != 0) + { + snprintf(deviceId, sizeof(deviceId), "-%04hX", device); + } + + fprintf(logThreadStream, "%02d:%02d:%02d:%03d [%06x] [%s] %s%s(%d): %s\n", + log.entry[i].localTimeStamp.hour, log.entry[i].localTimeStamp.minute, log.entry[i].localTimeStamp.second, log.entry[i].localTimeStamp.milliseconds, + log.entry[i].threadID, + fwLogVerbosityLevel(log.entry[i].verbosity), + log.entry[i].moduleID, + deviceId, + log.entry[i].lineNumber, + log.entry[i].payload); + + } + if (log.entries > 0) + { + fprintf(logThreadStream, "\n"); + } + + if ((log.entries > ((log.maxEntries * 2) / 5)) && (waitForLogMsec > MIN_WAIT_FOR_LOG_MSEC)) + { + waitForLogMsec = waitForLogMsec / WAIT_FOR_LOG_MULTIPLIER_STEP; + } + else if ((log.entries < ((log.maxEntries * 1) / 5)) && (waitForLogMsec < MAX_WAIT_FOR_LOG_MSEC)) + { + waitForLogMsec = waitForLogMsec * WAIT_FOR_LOG_MULTIPLIER_STEP; + } + + if (gIsDisposed) // Object 0 = Stop event + { + if (logThreadStream != NULL) + { + fclose(logThreadStream); + LOGD("Host logs saved to %s", fileName.c_str()); + } + + LOGD("Closing Host Log thread"); + return; + } + } // end of while +} + +void fwLogThreadFunction() +{ + TrackingData::Log log; + FILE *logThreadStream = stdout; + uint32_t waitForLogMsec = MAX_WAIT_FOR_LOG_MSEC; + Status status = Status::SUCCESS; + char deviceBuf[30] = { 0 };\ + + std::string fileHeaderName(gFileHeaderName); + std::string fileName = "FW_Log_" + fileHeaderName + "_Libtm_" + std::to_string(LIBTM_VERSION_MAJOR) + "_" + std::to_string(LIBTM_VERSION_MINOR) + "_" + std::to_string(LIBTM_VERSION_PATCH) + "_FW_" + FW_VERSION + ".log"; + + if (gConfiguration.logConfiguration[LogSourceFW].logOutputMode == LogOutputModeBuffer) + { + auto rc = fopen_s(&logThreadStream, fileName.c_str(), "w"); + if (rc != 0) + { + LOGE("Error while opening file %s", fileName.c_str()); + return; + } + } + + LOGD("Starting Log thread"); + if (logThreadStream != stdout) + { + fprintf(logThreadStream, "FW log: Libtm %u.%u.%u.%u, FW %s\n\n", LIBTM_VERSION_MAJOR, LIBTM_VERSION_MINOR, LIBTM_VERSION_PATCH, LIBTM_VERSION_BUILD, FW_VERSION); + } + + while (true) + { + if (!gManagerInstance) + { + std::this_thread::yield(); + continue; + } + + if (gDevice == NULL) + { + continue; + } + + short device = ((uintptr_t)gDevice & 0xFFFF); + + status = gDevice->GetFWLog(log); + if (status == Status::SUCCESS) + { + if (logThreadStream != stdout) + { + if (log.entries > 0) + { + fprintf(logThreadStream, "Received %d FW log entries from last %d msec (max entries %d)\n", log.entries, waitForLogMsec, log.maxEntries); + } + + for (uint32_t i = 0; i < log.entries; i++) + { + short device = ((uintptr_t)log.entry[i].deviceID & 0xFFFF); + + fprintf(logThreadStream, "%02d:%02d:%02d:%03d Device-%04hX: %" PRId64 " [%s] [%02d] [0x%X] [%s](%d): %s\n", + log.entry[i].localTimeStamp.hour, log.entry[i].localTimeStamp.minute, log.entry[i].localTimeStamp.second, log.entry[i].localTimeStamp.milliseconds, + device, + log.entry[i].timeStamp, + fwLogVerbosityLevel(log.entry[i].verbosity), + log.entry[i].threadID, + log.entry[i].functionSymbol, + log.entry[i].moduleID, + log.entry[i].lineNumber, + log.entry[i].payload + ); + } + if (log.entries > 0) + { + fprintf(logThreadStream, "\n"); + } + } + + if ((log.entries > ((log.maxEntries * 2) / 5)) && (waitForLogMsec > MIN_WAIT_FOR_LOG_MSEC)) + { + waitForLogMsec = waitForLogMsec / WAIT_FOR_LOG_MULTIPLIER_STEP; + } + else if ((log.entries < ((log.maxEntries * 1) / 5)) && (waitForLogMsec < MAX_WAIT_FOR_LOG_MSEC)) + { + waitForLogMsec = waitForLogMsec * WAIT_FOR_LOG_MULTIPLIER_STEP; + } + } + else + { + fprintf(logThreadStream, "Device-%04hX: Failed to receive FW log entries from last %d (msec)\n", device, waitForLogMsec); + log.entries = 0; + } + + if (gIsDisposed) // Object 0 = Stop event + { + if ((logThreadStream != NULL) && (gConfiguration.logConfiguration[LogSourceFW].logOutputMode == LogOutputModeBuffer)) + { + fclose(logThreadStream); + LOGD("FW logs saved to %s", fileName.c_str()); + } + + LOGD("Closing Log thread"); + return; + } + } // end of while +} + +void showArguments() +{ + printf("libtm_util version %d.%d\n", LIBTM_UTIL_VERSION_MAJOR, LIBTM_UTIL_VERSION_MINOR); + printf("Usage: libtm_util [-h] [-y] [-reset (optional)] [-time