diff --git a/RWR/src/.gitignore b/RWR/src/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..7727a19d096dd1bd016e62837eb19fce3dc0f5b6 --- /dev/null +++ b/RWR/src/.gitignore @@ -0,0 +1,45 @@ +**/build/ +**/devel/ +**/logs/ +**/.catkin_tools/ +**/.catkin_workspace +**/~.bashrc + +# Prerequisites +*.d + +# Compiled Object files +**.slo +**.lo +**.o +**.obj + +# Precompiled Headers +**.gch +**.pch + +# Compiled Dynamic libraries +**.so +**.dylib +**.dll + +# Fortran module files +**.mod +**.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +#vscode +**.vscode + +#valgrind +**.log.txt diff --git a/RWR/src/read_event_pub/include/event_array_sub.h b/RWR/src/read_event_pub/include/event_array_sub.h deleted file mode 100644 index d8d6a37aeb07bde0785b873244f9fe9abd306b47..0000000000000000000000000000000000000000 --- a/RWR/src/read_event_pub/include/event_array_sub.h +++ /dev/null @@ -1,47 +0,0 @@ -#ifndef EVENT_ARRAY_SUB_H_ -#define EVENT_ARRAY_SUB_H_ - -#define EVENT_ARRAY_PUBLISHERS 3 -#define EVENT_ARRAY_SUBSCRIBERS 1 - -#include <ros/ros.h> -#include <dvs_msgs/Event.h> -#include <dvs_msgs/EventArray.h> -#include <dvs_msgs/stats.h> - -namespace event_array_sub { - -class EventArraySub -{ - public: - EventArraySub(ros::NodeHandle& nh); - ~EventArraySub(); - - private: - ros::NodeHandle nh_; - - void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg); - - void publishEventStats(); - - ros::Subscriber event_sub_; - - struct EventDetection { - ros::Publisher events_[2]; - ros::Publisher dirLR_; - float xy_means_[1]; - size_t xy_variance[1]; - bool traject; - size_t msgSz; - }; - size_t votesLeft; - size_t votesRight; - size_t trajCounter; - - EventDetection event_detection_stats_; - uint8_t counter, nCounter; -}; - -} //namespace - -#endif //EVENT_ARRAY_SUB_H_ \ No newline at end of file diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/CMakeLists.txt b/RWR/src/read_event_pub/src/dvs_clustering_tracking/CMakeLists.txt new file mode 120000 index 0000000000000000000000000000000000000000..581e61db89fce59006b1ceb2d208d9f3e5fbcb5e --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/CMakeLists.txt @@ -0,0 +1 @@ +/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/.gitignore b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/.gitignore new file mode 100755 index 0000000000000000000000000000000000000000..0d20b6487c61e7d1bde93acf4a14b7a89083a16d --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/.gitignore @@ -0,0 +1 @@ +*.pyc diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/CMakeLists.txt b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/CMakeLists.txt new file mode 100755 index 0000000000000000000000000000000000000000..2bb0702122e813d14f91db8e7bbecc9f84124da1 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 2.8.3) +project(catkin_simple) + +find_package(catkin REQUIRED) + +catkin_package( + CATKIN_DEPENDS catkin + CFG_EXTRAS catkin_simple-extras.cmake +) diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/README.md b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/README.md new file mode 100755 index 0000000000000000000000000000000000000000..5b9f13a781e0de2619c69fb5a3681171ff63bddf --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/README.md @@ -0,0 +1,123 @@ +# catkin simple + +This `catkin` package is designed to make the `CMakeLists.txt` of other `catkin` packages simpler. + +## CMakeLists.txt Example + +Here is an example of a package `foo` which depends on other catkin packages: + +```cmake +cmake_minimum_required(VERSION 2.8.3) +project(foo) + +find_package(catkin_simple REQUIRED) + +catkin_simple() + +cs_add_library(my_lib src/my_lib.cpp) + +cs_add_executable(my_exec src/main.cpp) +target_link_libraries(my_exec my_lib) + +cs_install() + +cs_install_scripts(scripts/my_script.py) + +cs_export() +``` + +Lets break this down, line by line. + +First is the standard CMake header: + +```cmake +cmake_minimum_required(VERSION 2.8.3) +project(foo) +``` + +Which defines the minimum CMake version and the name of this project. + +Next comes the `find_package` of `catkin_simple`: + +```cmake +find_package(catkin_simple REQUIRED) +``` + +This is just like `find_package` for any other `catkin` package. This command is required. + +### catkin_simple() + +Then you invoke `catkin_simple`: + +```cmake +catkin_simple() +``` + +This macro call gathers your `build_depend`'s from the `package.xml` of your package, then does a `find_package(...)` on each of them. + +By default, `find_package(...)` is called with the `QUIET` option and without the `REQUIRED` option. That way, if any of the build_depend's are not `catkin` packages then they are simply ignored. This means that if you depend on a `caktin` package which is not on your system, `catkin_simple` will not warn you about this, because there is no way to know if it is a missing `catkin` package or a system dependency. If this is not the desired behaviour, calling `catkin_simple(ALL_DEPS_REQUIRED)` will call `find_package(...)` on each dependency *with* the `REQUIRED` option. + +Packages which are successfully found and identified to be `catkin` packages are added to a list of "catkin build dependencies" for your package. This list of build dependencies is passed to `find_package(catkin REQUIRED COMPONENTS ...)`. This command is required. + +Next, this macro adds the local `include` folder and any `catkin` include directories to the include path with CMake's `include_directories(...)` macro, but the local `include` folder is only added if it exists. + +Finally, this macro will discover and build any ROS messages, services, and actions which reside in the `msg`, `srv`, action `action` folders, respectively. The automatic discovery and building of messages/services is only done if your package `build_depend`'s on `message_generation`, and message generation will complain if your package does not run_depend on `message_runtime`. + +This macro discovers and builds [dynamic_reconfigure](http://wiki.ros.org/dynamic_reconfigure) config files from the `cfg` folder. The automatic discovery only works if your package `build_depend`'s on `dynamic_reconfigure`, and dynamic reconfigure will complain if your package does not run_depend on `dynamic_reconfigure`. + +### cs_add_library() + +Next we create a library: + +```cmake +cs_add_library(my_lib src/my_lib.cpp) +``` + +This call does a few things, first it calls directly through to the normal CMake macro `add_library`, then it calls `target_link_libraries(my_lib ${catkin_LIBRARIES})` to link your new library against any catkin libraries you have build depended on in your package.xml. Finally it does some bookkeeping so that your library target can be implicitly used later. + +### cs_add_executable() + +Next we add a new executable: + +```cmake +cs_add_executable(my_exec src/main.cpp) +target_link_libraries(my_exec my_lib) +``` + +This works just like `cs_add_library`, but it calls CMake's `add_executable(...)` instead. + +Notice that here we have to explicitly call `target_link_libraries` for linking against our library, this is because there is no way to enforce order of target creation. The executable is still automatically linked against the catkin libraries. + +### cs_install() + +Next we install everything: + +```cmake +cs_install() + +cs_install_scripts(scripts/my_script.py) +``` + +The first macro call creates an installation rule for any libraries and executables you created with `cs_` prefixed commands. That call can also take zero to many additional targets you wish to install which were created without the `cs_` prefixed commands. This command is optional. + +The second macro call creates an installation rule for the given scripts, installing them to `${prefix}/lib/${pkg_name}/`. This command is optional. + +### cs_export() + +Finally, we export everything: + +```cmake +cs_export() +``` + +This command calls `catkin_package(...)` under the hood, extending that call with any libraries created and catkin_depends found automatically with `catkin_simple`. You can also pass in your own arguments to `catkin_package(...)` through this command. This command is required. + +## Known Limitations + +There are several known assumptions and incorrect behaviors in `catkin_simple` which are a result of the trade-off of correctness for convenience. + +* There is no warning when a catkin package is not found during `find_package`. +* There is over linking, as all libraries of all dependencies are linked against all targets indiscriminately. +* Assumes that the `include` folder is meant to be in the include path. +* If new .msg or .srv files are added, they will not be detected until you force CMake to run again +* All targets have a target dependency on any downstream message generation, which results in sub-optimal parallelization of targets, as there are unnecessary dependencies created. diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/cmake/catkin_simple-extras.cmake.em b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/cmake/catkin_simple-extras.cmake.em new file mode 100755 index 0000000000000000000000000000000000000000..989f9f97d177853eb94a0e6b2e77d683c95ee323 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/cmake/catkin_simple-extras.cmake.em @@ -0,0 +1,226 @@ +# Generated from: catkin_simple/cmake/catkin_simple-extras.cmake.em + +if(_CATKIN_SIMPLE_EXTRAS_INCLUDED_) + return() +endif() +set(_CATKIN_SIMPLE_EXTRAS_INCLUDED_ TRUE) + +include(CMakeParseArguments) + +@[if DEVELSPACE]@ +# cmake dir in develspace +set(catkin_simple_CMAKE_DIR "@(CMAKE_CURRENT_SOURCE_DIR)/cmake") +@[else]@ +# cmake dir in installspace +set(catkin_simple_CMAKE_DIR "@(PKG_CMAKE_DIR)") +@[end if]@ + +macro(catkin_simple) + # Arguments + # ALL_DEPS_REQUIRED -- Add the "REQUIRED" flag when calling + # FIND_PACKAGE() for each dependency + cmake_parse_arguments(cs_args "ALL_DEPS_REQUIRED" "" "" ${ARGN}) + + if(TARGET ${PROJECT_NAME}_package) + message(WARNING "Could not create target '${${PROJECT_NAME}_package}' for project ${PROJECT_NAME}, as it already exists.") + endif() + add_custom_target(${PROJECT_NAME}_package) + set(${PROJECT_NAME}_TARGETS) + set(${PROJECT_NAME}_LIBRARIES) + + find_package(catkin REQUIRED) + # call catkin_package_xml() if it has not been called before + if(NOT _CATKIN_CURRENT_PACKAGE) + catkin_package_xml() + endif() + + set(${PROJECT_NAME}_CATKIN_BUILD_DEPENDS) + set(${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS) + foreach(dep ${${PROJECT_NAME}_BUILD_DEPENDS}) + # If this flag is defined, add the "REQUIRED" flag + # to all FIND_PACKAGE calls + if(cs_args_ALL_DEPS_REQUIRED) + find_package(${dep} REQUIRED) + else() + find_package(${dep} QUIET) + endif() + + if(${dep}_FOUND_CATKIN_PROJECT) + list(APPEND ${PROJECT_NAME}_CATKIN_BUILD_DEPENDS ${dep}) + list(APPEND ${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS ${${dep}_EXPORTED_TARGETS}) + endif() + endforeach() + + # Let find_package(catkin ...) do the heavy lifting + find_package(catkin REQUIRED COMPONENTS ${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS}) + + # add include directory if available + set(${PROJECT_NAME}_LOCAL_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include) + if(NOT IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_INCLUDE_DIR}) + set(${PROJECT_NAME}_LOCAL_INCLUDE_DIR) + endif() + include_directories(${${PROJECT_NAME}_LOCAL_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) + + # perform action/msg/srv generation if necessary + if(message_generation_FOUND_CATKIN_PROJECT) + set(${PROJECT_NAME}_DO_MESSAGE_GENERATION FALSE) + # add action files if available + set(${PROJECT_NAME}_LOCAL_ACTION_DIR ${CMAKE_CURRENT_SOURCE_DIR}/action) + if(NOT IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_ACTION_DIR}) + set(${PROJECT_NAME}_LOCAL_ACTION_DIR) + endif() + if(${PROJECT_NAME}_LOCAL_ACTION_DIR) + add_action_files(DIRECTORY action) + set(${PROJECT_NAME}_DO_MESSAGE_GENERATION TRUE) + endif() + + # add message files if available + set(${PROJECT_NAME}_LOCAL_MSG_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg) + if(NOT IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_MSG_DIR}) + set(${PROJECT_NAME}_LOCAL_MSG_DIR) + endif() + if(${PROJECT_NAME}_LOCAL_MSG_DIR) + add_message_files(DIRECTORY msg) + set(${PROJECT_NAME}_DO_MESSAGE_GENERATION TRUE) + endif() + + # add service files if available + set(${PROJECT_NAME}_LOCAL_SRV_DIR ${CMAKE_CURRENT_SOURCE_DIR}/srv) + if(NOT IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_SRV_DIR}) + set(${PROJECT_NAME}_LOCAL_SRV_DIR) + endif() + if(${PROJECT_NAME}_LOCAL_SRV_DIR) + add_service_files(DIRECTORY srv) + set(${PROJECT_NAME}_DO_MESSAGE_GENERATION TRUE) + endif() + + # generate messages if necessary + if(${PROJECT_NAME}_DO_MESSAGE_GENERATION) + # identify all build dependencies which contain messages + set(${PROJECT_NAME}_MSG_PACKAGES) + foreach(dep ${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS}) + set(${PROJECT_NAME}_MSG_PACKAGE_FILE ${${dep}_DIR}/${dep}-msg-paths.cmake) + if(EXISTS ${${PROJECT_NAME}_MSG_PACKAGE_FILE}) + list(APPEND ${PROJECT_NAME}_MSG_PACKAGES ${dep}) + endif() + endforeach() + generate_messages(DEPENDENCIES ${${PROJECT_NAME}_MSG_PACKAGES}) + # add additional exported targets coming from generate_messages() + list(INSERT ${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS 0 ${${PROJECT_NAME}_EXPORTED_TARGETS}) + endif() + endif() + + # generate dynamic reconfigure files + if(dynamic_reconfigure_FOUND_CATKIN_PROJECT) + set(${PROJECT_NAME}_LOCAL_CFG_DIR ${CMAKE_CURRENT_SOURCE_DIR}/cfg) + if(IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_CFG_DIR}) + # create a list containing all the cfg files + file(GLOB ${PROJECT_NAME}_LOCAL_CFG_FILES RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${${PROJECT_NAME}_LOCAL_CFG_DIR}/*.cfg") + if(${PROJECT_NAME}_LOCAL_CFG_FILES) + generate_dynamic_reconfigure_options(${${PROJECT_NAME}_LOCAL_CFG_FILES}) + # add build dep on gencfg + list(APPEND ${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS ${PROJECT_NAME}_gencfg) + endif() + endif() + endif() +endmacro() + +macro(cs_add_targets_to_package) + add_dependencies(${PROJECT_NAME}_package ${ARGN}) + list(APPEND ${PROJECT_NAME}_TARGETS ${ARGN}) +endmacro() + +macro(cs_add_executable _target) + if(${_target} STREQUAL ${PROJECT_NAME}_package) + message(WARNING "Could not create executable with name '${_target}' as '${PROJECT_NAME}_package' is reserved for the top level target name for this project.") + endif() + cmake_parse_arguments(cs_add_executable_args "NO_AUTO_LINK;NO_AUTO_DEP" "" "" ${ARGN}) + add_executable(${_target} ${cs_add_executable_args_UNPARSED_ARGUMENTS}) + if(NOT cs_add_executable_args_NO_AUTO_LINK) + target_link_libraries(${_target} ${catkin_LIBRARIES}) + endif() + if(NOT cs_add_executable_args_NO_AUTO_DEP) + if(NOT "${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS}" STREQUAL "") + add_dependencies(${_target} ${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS}) + endif() + endif() + cs_add_targets_to_package(${_target}) +endmacro() + +macro(cs_add_library _target) + if(${_target} STREQUAL ${PROJECT_NAME}_package) + message(WARNING "Could not create library with name '${_target}' as '${PROJECT_NAME}_package' is reserved for the top level target name for this project.") + endif() + cmake_parse_arguments(cs_add_library "NO_AUTO_LINK;NO_AUTO_DEP;NO_AUTO_EXPORT" "" "" ${ARGN}) + add_library(${_target} ${cs_add_library_UNPARSED_ARGUMENTS}) + if(NOT cs_add_library_NO_AUTO_LINK) + target_link_libraries(${_target} ${catkin_LIBRARIES}) + endif() + if(NOT cs_add_library_NO_AUTO_DEP) + if(NOT "${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS}" STREQUAL "") + add_dependencies(${_target} ${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS}) + endif() + endif() + if(NOT cs_add_library_NO_AUTO_EXPORT) + list(APPEND ${PROJECT_NAME}_LIBRARIES ${_target}) + endif() + cs_add_targets_to_package(${_target}) +endmacro() + +macro(cs_install) + # Install targets (exec's and lib's) + foreach(_target ${${PROJECT_NAME}_TARGETS}) + get_target_property(${_target}_type ${_target} TYPE) + message(STATUS "Marking ${${_target}_type} \"${_target}\" of package \"${PROJECT_NAME}\" for installation") + endforeach() + install(TARGETS ${${PROJECT_NAME}_TARGETS} ${ARGN} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + if(EXISTS ${${PROJECT_NAME}_LOCAL_INCLUDE_DIR}) + # Install include directory + message(STATUS "Marking HEADER FILES in \"include\" folder of package \"${PROJECT_NAME}\" for installation") + install(DIRECTORY ${${PROJECT_NAME}_LOCAL_INCLUDE_DIR}/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" + PATTERN ".svn" EXCLUDE + ) + endif() + # Install shared content located in commonly used folders + set(_shared_content_folders launch rviz urdf meshes maps worlds Media param) + foreach(_folder ${_shared_content_folders}) + if(IS_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${_folder}) + message(STATUS "Marking SHARED CONTENT FOLDER \"${_folder}\" of package \"${PROJECT_NAME}\" for installation") + install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${_folder}/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${_folder} + ) + endif() + endforeach() +endmacro() + +macro(cs_install_scripts) + install(PROGRAMS ${ARGN} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +endmacro() + +macro(cs_export) + cmake_parse_arguments(CS_PROJECT + "" "" "INCLUDE_DIRS;LIBRARIES;CATKIN_DEPENDS;DEPENDS;CFG_EXTRAS" + ${ARGN}) + + set(${PROJECT_NAME}_CATKIN_RUN_DEPENDS) + foreach(dep ${${PROJECT_NAME}_RUN_DEPENDS}) + find_package(${dep} QUIET) + if(${dep}_FOUND_CATKIN_PROJECT) + list(APPEND ${PROJECT_NAME}_CATKIN_RUN_DEPENDS ${dep}) + endif() + endforeach() + + catkin_package( + INCLUDE_DIRS ${${PROJECT_NAME}_LOCAL_INCLUDE_DIR} ${CS_PROJECT_INCLUDE_DIRS} + LIBRARIES ${${PROJECT_NAME}_LIBRARIES} ${CS_PROJECT_LIBRARIES} + CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_RUN_DEPENDS} ${CS_PROJECT_CATKIN_DEPENDS} + DEPENDS ${CS_PROJECT_DEPENDS} + CFG_EXTRAS ${CS_PROJECT_CFG_EXTRAS} + ) +endmacro() diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/package.xml b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/package.xml new file mode 100755 index 0000000000000000000000000000000000000000..3f6e0705bd9aa528a701c6f58f1493ddd9d3a3aa --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/package.xml @@ -0,0 +1,17 @@ +<?xml version="1.0"?> +<package> + <name>catkin_simple</name> + <version>0.1.1</version> + <description>catkin, simpler</description> + + <maintainer email="william@osrfoundation.org">William Woodall</maintainer> + <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer> + <license>BSD</license> + + <author email="william@osrfoundation.org">William Woodall</author> + <author email="dthomas@osrfoundation.org">Dirk Thomas</author> + + <buildtool_depend>catkin</buildtool_depend> + <run_depend>catkin</run_depend> + +</package> \ No newline at end of file diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/.gitignore b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/.gitignore new file mode 100755 index 0000000000000000000000000000000000000000..a0f00082c8e5d428fcf98979e38e626b810213b7 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/.gitignore @@ -0,0 +1 @@ +CMakeLists.txt diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/bar/include/bar/hello.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/bar/include/bar/hello.h new file mode 100755 index 0000000000000000000000000000000000000000..9bb085a0d88259b574ffaa9c30c531fb1a74577f --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/bar/include/bar/hello.h @@ -0,0 +1 @@ +void hello(); diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/bar/msg/HeaderString.msg b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/bar/msg/HeaderString.msg new file mode 100755 index 0000000000000000000000000000000000000000..9741bdaf0f0e186423e0c129003e74a7157f78a3 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/bar/msg/HeaderString.msg @@ -0,0 +1,2 @@ +Header header +string data diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/bar/package.xml b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/bar/package.xml new file mode 100755 index 0000000000000000000000000000000000000000..d98d6e91f9f39143ee50ba2dc003916d9070a883 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/bar/package.xml @@ -0,0 +1,18 @@ +<?xml version="1.0"?> +<package> + <name>bar</name> + <version>0.1.0</version> + <description>The bar package</description> + <maintainer email="william@osrfoundation.org">William Woodall</maintainer> + <license>BSD</license> + + <buildtool_depend>catkin</buildtool_depend> + <buildtool_depend>catkin_simple</buildtool_depend> + <run_depend>boost</run_depend> + <run_depend>message_runtime</run_depend> + <run_depend>std_msgs</run_depend> + <build_depend>boost-dev</build_depend> + <build_depend>message_generation</build_depend> + <build_depend>std_msgs</build_depend> + +</package> \ No newline at end of file diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/bar/src/hello.cpp b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/bar/src/hello.cpp new file mode 100755 index 0000000000000000000000000000000000000000..3aed965185104fabf446f5da25072b4a976ed3dc --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/bar/src/hello.cpp @@ -0,0 +1,16 @@ +#include <bar/hello.h> +#include <bar/HeaderString.h> + +#include <iostream> + +#include <boost/thread.hpp> + + +inline void print_hello() { + std::cout << "Hello "; +} + +void hello() { + boost::thread t(print_hello); + t.join(); +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/baz/include/baz/world.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/baz/include/baz/world.h new file mode 100755 index 0000000000000000000000000000000000000000..766f82ccccef2a5a704d56e725024ef6140040f7 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/baz/include/baz/world.h @@ -0,0 +1,12 @@ +#include <iostream> + +#include <boost/thread.hpp> + +void print_world() { + std::cout << "world!" << std::endl; +} + +void world() { + boost::thread t(print_world); + t.join(); +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/baz/package.xml b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/baz/package.xml new file mode 100755 index 0000000000000000000000000000000000000000..db0518e0139d96d4c3e37755da1b7e4a76667093 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/baz/package.xml @@ -0,0 +1,13 @@ +<?xml version="1.0"?> +<package> + <name>baz</name> + <version>0.1.0</version> + <description>The baz package</description> + <maintainer email="william@osrfoundation.org">William Woodall</maintainer> + <license>BSD</license> + + <buildtool_depend>catkin</buildtool_depend> + <run_depend>boost</run_depend> + <build_depend>boost-dev</build_depend> + +</package> \ No newline at end of file diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/catkin_simple b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/catkin_simple new file mode 120000 index 0000000000000000000000000000000000000000..a8a4f8c2127bf74725cf14800dde7ea8aeec5c5c --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/catkin_simple @@ -0,0 +1 @@ +../../.. \ No newline at end of file diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/foo/package.xml b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/foo/package.xml new file mode 100755 index 0000000000000000000000000000000000000000..16121e46ae9f02784a0cf3e32f50df9713286a41 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/foo/package.xml @@ -0,0 +1,18 @@ +<?xml version="1.0"?> +<package> + <name>foo</name> + <version>0.1.0</version> + <description>The foo package</description> + <maintainer email="william@osrfoundation.org">William Woodall</maintainer> + <license>BSD</license> + + <buildtool_depend>catkin</buildtool_depend> + <buildtool_depend>catkin_simple</buildtool_depend> + <run_depend>bar</run_depend> + <build_depend>bar</build_depend> + <run_depend>baz</run_depend> + <build_depend>baz</build_depend> + <run_depend>boost</run_depend> + <build_depend>boost-dev</build_depend> + +</package> \ No newline at end of file diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/foo/src/main.cpp b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/foo/src/main.cpp new file mode 100755 index 0000000000000000000000000000000000000000..3f59650f51d846a90d0e267c8120118258c5a37d --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/catkin_simple/test/scenarios/hello_world/foo/src/main.cpp @@ -0,0 +1,9 @@ +#include <bar/hello.h> +#include <baz/world.h> + +int main(void) { + hello(); + world(); + + return 0; +} \ No newline at end of file diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/88-retina.rules b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/88-retina.rules new file mode 100755 index 0000000000000000000000000000000000000000..4fb8b2b79ddc19991dadd4abca5e7173fb063a6c --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/88-retina.rules @@ -0,0 +1,2 @@ +SUBSYSTEM=="usb|usb_device", ATTR{idVendor}=="152a", ATTR{idProduct}=="8400", MODE="0666", GROUP="plugdev" + diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/88-retinaDAVIS.rules b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/88-retinaDAVIS.rules new file mode 100755 index 0000000000000000000000000000000000000000..d50c20159e38b32a6e2f227d631dfe7a66f92487 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/88-retinaDAVIS.rules @@ -0,0 +1,2 @@ +SUBSYSTEM=="usb|usb_device", ATTR{idVendor}=="152a", ATTR{idProduct}=="840d", MODE="0666", GROUP="plugdev" + diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/CMakeLists.txt b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/CMakeLists.txt new file mode 100755 index 0000000000000000000000000000000000000000..500ecd864eaccdc6c821095df35a74c1ca3347da --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 2.8.3) +project(dvs_driver) + +# search for everything we need to build the package +find_package(catkin) + +# since we need eigen and boost search them as well +find_package(Boost REQUIRED COMPONENTS thread) + +include(FindPkgConfig) +pkg_check_modules(LIBUSB1 REQUIRED libusb-1.0) + +# export the dependencis of this package for who ever depends on us +catkin_package( + INCLUDE_DIRS include ${LIBUSB1_INCLUDE_DIRS} + DEPENDS libusb-1.0 + LIBRARIES dvs_driver +) + +# tell catkin where to find the headers for this project +include_directories( + include + ${LIBUSB1_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +# make the library +add_library(dvs_driver + src/dvs_driver.cpp +) + +target_link_libraries(dvs_driver + ${LIBUSB1_LIBRARIES} + ${Boost_LIBRARIES} +) + +install(TARGETS dvs_driver + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/include/dvs_driver/dvs_driver.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/include/dvs_driver/dvs_driver.h new file mode 100755 index 0000000000000000000000000000000000000000..458cbe1d24db64bc722b1bcfdea927bae168cf7a --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/include/dvs_driver/dvs_driver.h @@ -0,0 +1,138 @@ +// This file is part of DVS-ROS - the RPG DVS ROS Package +// +// DVS-ROS is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// DVS-ROS is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with DVS-ROS. If not, see <http://www.gnu.org/licenses/>. + +#ifndef DVS_DRIVER_H_ +#define DVS_DRIVER_H_ + +#include <libusb-1.0/libusb.h> +#include <boost/thread.hpp> +#include <vector> +#include <iostream> +#include <map> +#include <string> +#include <stdio.h> + +namespace dvs { + +// DVS USB interface +#define DVS128_VID 0x152A +#define DVS128_PID 0x8400 //this is for DVS +//#define DVS128_PID 0x840D //this is for DAVIS +#define DVS128_DID_TYPE 0x00 + +#define USB_IO_ENDPOINT 0x86 + +// DVS data decoding masks +#define DVS128_POLARITY_SHIFT 0 +#define DVS128_POLARITY_MASK 0x0001 +#define DVS128_Y_ADDR_SHIFT 8 +#define DVS128_Y_ADDR_MASK 0x007F +#define DVS128_X_ADDR_SHIFT 1 +#define DVS128_X_ADDR_MASK 0x007F +#define DVS128_SYNC_EVENT_MASK 0x8000 + +// USB vendor requests +#define VENDOR_REQUEST_START_TRANSFER 0xB3 +#define VENDOR_REQUEST_STOP_TRANSFER 0xB4 +#define VENDOR_REQUEST_SEND_BIASES 0xB8 +#define VENDOR_REQUEST_SET_SYNC_ENABLED 0xBE +#define VENDOR_REQUEST_RESET_TIMESTAMPS 0xBB + +struct Event { + uint16_t x, y; + bool polarity; + uint64_t timestamp; +}; + +class DVS_Driver { +public: + DVS_Driver(std::string dvs_serial_number = "", bool master = true); + ~DVS_Driver(); + + std::vector<Event> get_events(); + + bool change_parameters(uint32_t cas, uint32_t injGnd, uint32_t reqPd, uint32_t puX, + uint32_t diffOff, uint32_t req, uint32_t refr, uint32_t puY, + uint32_t diffOn, uint32_t diff, uint32_t foll, uint32_t Pr); + + void callback(struct libusb_transfer *transfer); + + void resetTimestamps(); + + inline std::string get_camera_id() { + return camera_id; + } + +private: + bool change_parameter(std::string parameter, uint32_t value); + bool send_parameters(); + + bool open_device(std::string dvs_serial_number = ""); + void close_device(); + + boost::mutex event_buffer_mutex; + boost::mutex device_mutex; + boost::thread* thread; + void run(); + + void event_translator(uint8_t *buffer, size_t bytesSent); + + // USB handle and buffer + libusb_device_handle *device_handle; + struct libusb_transfer *transfer; + unsigned char *buffer; + + // event buffer + std::vector<dvs::Event> event_buffer; + + // buffers + static const uint32_t bufferNumber = 8; + static const uint32_t bufferSize = 4096; + + uint64_t wrapAdd; + uint64_t lastTimestamp; + + class Parameter { + public: + Parameter(uint32_t min = 0, uint32_t max = 0, uint32_t value = 0) : + _min(min), _max(max), _value(value) {} + + uint32_t get_value() { return _value; } + + bool set_value(uint32_t value) { + if (value >= _min && value <= _max) { + _value = value; + return true; + } + else { + return false; + } + } + private: + uint32_t _min; + uint32_t _max; + uint32_t _value; + + }; + + // parameters + std::map<std::string, Parameter> parameters; + + // camera name + std::string camera_id; +}; + +} // namespace +#endif diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/install.sh b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/install.sh new file mode 100755 index 0000000000000000000000000000000000000000..9dcf4ea992baaa9e891927efd0f487a33ee52af9 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/install.sh @@ -0,0 +1,9 @@ +#!/bin/bash +echo "Copying udev rule (needs root privileges)." +sudo cp 88-retina.rules /etc/udev/rules.d/ + +echo "Reloading udev rules." +sudo udevadm control --reload-rules +sudo udevadm trigger + +echo "Done!" diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/installDAVIS.sh b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/installDAVIS.sh new file mode 100755 index 0000000000000000000000000000000000000000..bcb89810c9a6cab1bd8d6def740acf2591b1b406 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/installDAVIS.sh @@ -0,0 +1,9 @@ +#!/bin/bash +echo "Copying udev rule (needs root privileges)." +sudo cp 88-retinaDAVIS.rules /etc/udev/rules.d/ + +echo "Reloading udev rules." +sudo udevadm control --reload-rules +sudo udevadm trigger + +echo "Done!" diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/package.xml b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/package.xml new file mode 100755 index 0000000000000000000000000000000000000000..f0541b2e2d5ed35fba4567ca5b4aa0d893bdd632 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/package.xml @@ -0,0 +1,13 @@ +<?xml version="1.0"?> +<package> + <name>dvs_driver</name> + <version>0.0.0</version> + <description>The dvs_driver package</description> + <maintainer email="mueggler@ifi.uzh.ch">Elias Mueggler</maintainer> + <license>GNU GPL</license> + + <buildtool_depend>catkin</buildtool_depend> + + <export> + </export> +</package> diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/src/dvs_driver.cpp b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/src/dvs_driver.cpp new file mode 100755 index 0000000000000000000000000000000000000000..90b5c6152a66a1bb9df128df3d7d76bd4c698eb5 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_driver/src/dvs_driver.cpp @@ -0,0 +1,410 @@ +// This file is part of DVS-ROS - the RPG DVS ROS Package +// +// DVS-ROS is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// DVS-ROS is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with DVS-ROS. If not, see <http://www.gnu.org/licenses/>. + +#include "dvs_driver/dvs_driver.h" + +namespace dvs { + +extern "C" { + +static void callback_wrapper(struct libusb_transfer *transfer) +{ + DVS_Driver *dvs_driver = (DVS_Driver *)transfer->user_data; + + dvs_driver->callback(transfer); +} + +} // extern "C" + +DVS_Driver::DVS_Driver(std::string dvs_serial_number, bool master) { + // initialize parameters (min, max, value) + parameters.insert(std::pair<std::string, Parameter>("cas", Parameter(0, 16777215, 1992))); + parameters.insert(std::pair<std::string, Parameter>("injGnd", Parameter(0, 16777215, 1108364))); + parameters.insert(std::pair<std::string, Parameter>("reqPd", Parameter(0, 16777215, 16777215))); + parameters.insert(std::pair<std::string, Parameter>("puX", Parameter(0, 16777215, 8159221))); + parameters.insert(std::pair<std::string, Parameter>("diffOff", Parameter(0, 16777215, 132))); + parameters.insert(std::pair<std::string, Parameter>("req", Parameter(0, 16777215, 309590))); + parameters.insert(std::pair<std::string, Parameter>("refr", Parameter(0, 16777215, 969))); + parameters.insert(std::pair<std::string, Parameter>("puY", Parameter(0, 16777215, 16777215))); + parameters.insert(std::pair<std::string, Parameter>("diffOn", Parameter(0, 16777215, 209996))); + parameters.insert(std::pair<std::string, Parameter>("diff", Parameter(0, 16777215, 13125))); + parameters.insert(std::pair<std::string, Parameter>("foll", Parameter(0, 16777215, 271))); + parameters.insert(std::pair<std::string, Parameter>("Pr", Parameter(0, 16777215, 217))); + + wrapAdd = 0; + lastTimestamp = 0; + + libusb_init(NULL); + + device_mutex.lock(); + if (open_device(dvs_serial_number)) { + std::cout << "Opened DVS with the following identification: " << camera_id << std::endl; + } + else { + if (dvs_serial_number.empty()) { + std::cout << "Could not find any DVS..." << std::endl; + } + else { + std::cout << "Could not find DVS with serial number " << dvs_serial_number << "..." << std::endl; + } + } + device_mutex.unlock(); + + // put into slave mode? + if (!master) { + std::cout << "Setting camera (" << camera_id << ") as slave!" << std::endl; + device_mutex.lock(); + + unsigned char enabled[1]; + enabled[0] = 0; + + libusb_control_transfer(device_handle, LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_SET_SYNC_ENABLED, 0, 0, enabled, sizeof(enabled), 0); + device_mutex.unlock(); + } + + thread = new boost::thread(boost::bind(&DVS_Driver::run, this)); +} + +DVS_Driver::~DVS_Driver() { + device_mutex.lock(); + close_device(); + device_mutex.unlock(); +} + +bool DVS_Driver::open_device(std::string dvs_serial_number) { + + // get device list + libusb_device **list = NULL; + ssize_t count = libusb_get_device_list(NULL, &list); + + // iterate over all USB devices + for (size_t idx = 0; idx < count; ++idx) { + libusb_device *device = list[idx]; + libusb_device_descriptor desc = {0}; + + int rc = libusb_get_device_descriptor(device, &desc); + assert(rc == 0); + +// printf("Vendor:Device = %04x:%04x\n", desc.idVendor, desc.idProduct); + + if (DVS128_VID == desc.idVendor && DVS128_PID == desc.idProduct) { + + // Open device to see its serial number + int e = libusb_open(device, &device_handle); + if (e) { + printf("Error opening device... Error code: %d", e); + continue; + } + + unsigned char sSerial[256]; + + e = libusb_get_string_descriptor_ascii(device_handle, desc.iSerialNumber, sSerial, sizeof(sSerial)); + if (e < 0) { + std::cout << "libusb: get error code: " << e << std::endl; + libusb_close(device_handle); + } + + if (std::string(reinterpret_cast<char*>(sSerial)) == dvs_serial_number || dvs_serial_number.empty()) { + + // claim interface + libusb_claim_interface(device_handle, 0); + + // figure out precise product + unsigned char sProduct[256]; + e = libusb_get_string_descriptor_ascii(device_handle, desc.iProduct, sProduct, sizeof(sProduct)); + if (e < 0) { + std::cout << "libusb: get error code: " << e << std::endl; + libusb_close(device_handle); + } + + camera_id = std::string(reinterpret_cast<char*>(sProduct)) + "-" + std::string(reinterpret_cast<char*>(sSerial)); + + // alloc transfer and setup + transfer = libusb_alloc_transfer(0); + + transfer->length = (int) bufferSize; + buffer = new unsigned char[bufferSize]; + transfer->buffer = buffer; + + transfer->dev_handle = device_handle; + transfer->endpoint = USB_IO_ENDPOINT; + transfer->type = LIBUSB_TRANSFER_TYPE_BULK; + transfer->callback = callback_wrapper; + transfer->user_data = this; + transfer->timeout = 0; + transfer->flags = LIBUSB_TRANSFER_FREE_BUFFER; + + // libusb_submit_transfer(transfer); + int status = libusb_submit_transfer(transfer); + if (status != LIBUSB_SUCCESS) { + std::cout << "Unable to submit libusb transfer: " << status << std::endl; + + // The transfer buffer is freed automatically here thanks to + // the LIBUSB_TRANSFER_FREE_BUFFER flag set above. + libusb_free_transfer(transfer); + } + + return true; + } + + libusb_close(device_handle); + } + } + return false; +} + +void DVS_Driver::close_device() { + // stop thread + thread->join(); + + // close transfer + int status = libusb_cancel_transfer(transfer); + if (status != LIBUSB_SUCCESS && status != LIBUSB_ERROR_NOT_FOUND) { + std::cout << "Unable to cancel libusb transfer: " << status << std::endl; + } + + libusb_close(device_handle); + libusb_exit(NULL); +} + +void DVS_Driver::run() { + timeval te; + te.tv_sec = 0; + te.tv_usec = 1000000; + + libusb_control_transfer(device_handle, + LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_START_TRANSFER, 0, 0, NULL, 0, 0); + + try { + int completed; + while(true) { + device_mutex.lock(); + libusb_handle_events_timeout(NULL, &te); + device_mutex.unlock(); + } + } + catch(boost::thread_interrupted const& ) { + //clean resources + std::cout << "Worker thread interrupted!" << std::endl; + } + + libusb_control_transfer(device_handle, + LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_STOP_TRANSFER, 0, 0, NULL, 0, 0); +} + +void DVS_Driver::callback(struct libusb_transfer *transfer) { + if (transfer->status == LIBUSB_TRANSFER_COMPLETED) { + // Handle data. + event_buffer_mutex.lock(); + event_translator(transfer->buffer, (size_t) transfer->actual_length); + event_buffer_mutex.unlock(); + } + + if (transfer->status != LIBUSB_TRANSFER_CANCELLED && transfer->status != LIBUSB_TRANSFER_NO_DEVICE) { + // Submit transfer again. + if (libusb_submit_transfer(transfer) == LIBUSB_SUCCESS) { + return; + } + } + + // Cannot recover (cancelled, no device, or other critical error). + // Signal this by adjusting the counter, free and exit. + libusb_free_transfer(transfer); +} + +void DVS_Driver::event_translator(uint8_t *buffer, size_t bytesSent) { + // Truncate off any extra partial event. + bytesSent &= (size_t) ~0x03; + + for (size_t i = 0; i < bytesSent; i += 4) { + if ((buffer[i + 3] & 0x80) == 0x80) { + // timestamp bit 15 is one -> wrap: now we need to increment + // the wrapAdd, uses only 14 bit timestamps + wrapAdd += 0x4000; + + // Detect big timestamp wrap-around. + if (wrapAdd == 0) { + // Reset lastTimestamp to zero at this point, so we can again + // start detecting overruns of the 32bit value. + lastTimestamp = 0; + } + } + else if ((buffer[i + 3] & 0x40) == 0x40) { + // timestamp bit 14 is one -> wrapAdd reset: this firmware + // version uses reset events to reset timestamps + wrapAdd = 0; + lastTimestamp = 0; + } + else { + // address is LSB MSB (USB is LE) + uint16_t addressUSB = le16toh(*((uint16_t * ) (&buffer[i]))); + + // same for timestamp, LSB MSB (USB is LE) + // 15 bit value of timestamp in 1 us tick + uint16_t timestampUSB = le16toh(*((uint16_t * ) (&buffer[i + 2]))); + + // Expand to 32 bits. (Tick is 1µs already.) + uint64_t timestamp = timestampUSB + wrapAdd; + + // Check monotonicity of timestamps. + if (timestamp < lastTimestamp) { + std::cout << "DVS128: non-monotonic time-stamp detected: lastTimestamp=" << lastTimestamp << ", timestamp=" << timestamp << "." << std::endl; + } + + lastTimestamp = timestamp; + + // Special Trigger Event (MSB is set) + if ((addressUSB & DVS128_SYNC_EVENT_MASK) != 0) { + std::cout << "got sync event on camera " << camera_id << "at " << timestamp << std::endl; + } + else { + // Invert x values (flip along the x axis). + uint16_t x = (uint16_t) (127 - ((uint16_t) ((addressUSB >> DVS128_X_ADDR_SHIFT) & DVS128_X_ADDR_MASK))); + uint16_t y = (uint16_t) (127 - ((uint16_t) ((addressUSB >> DVS128_Y_ADDR_SHIFT) & DVS128_Y_ADDR_MASK))); + bool polarity = (((addressUSB >> DVS128_POLARITY_SHIFT) & DVS128_POLARITY_MASK) == 0) ? (1) : (0); + + // std::cout << "Event: <x, y, t, p> = <" << x << ", " << y << ", " << timestamp << ", " << polarity << ">" << std::endl; + Event e; + e.x = x; + e.y = y; + e.timestamp = timestamp; + e.polarity = polarity; + event_buffer.push_back(e); + } + } + } +} + +std::vector<Event> DVS_Driver::get_events() { + event_buffer_mutex.lock(); + std::vector<Event> buffer_copy = event_buffer; + event_buffer.clear(); + event_buffer_mutex.unlock(); + return buffer_copy; +} + +void DVS_Driver::resetTimestamps() { + device_mutex.lock(); + libusb_control_transfer(device_handle, LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_RESET_TIMESTAMPS, 0, 0, NULL, 0, 0); + device_mutex.unlock(); +} + +bool DVS_Driver::change_parameter(std::string parameter, uint32_t value) { + // does parameter exist? + if (parameters.find(parameter) != parameters.end()) { + // did it change? (only if within range) + if (parameters[parameter].set_value(value)) { + return true; + } + else + return false; + } + else + return false; +} + +bool DVS_Driver::change_parameters(uint32_t cas, uint32_t injGnd, uint32_t reqPd, uint32_t puX, + uint32_t diffOff, uint32_t req, uint32_t refr, uint32_t puY, + uint32_t diffOn, uint32_t diff, uint32_t foll, uint32_t Pr) { + change_parameter("cas", cas); + change_parameter("injGnd", injGnd); + change_parameter("reqPd", reqPd); + change_parameter("puX", puX); + change_parameter("diffOff", diffOff); + change_parameter("req", req); + change_parameter("refr", refr); + change_parameter("puY", puY); + change_parameter("diffOn", diffOn); + change_parameter("diff", diff); + change_parameter("foll", foll); + change_parameter("Pr", Pr); + + return send_parameters(); +} + +bool DVS_Driver::send_parameters() { + uint8_t biases[12 * 3]; + + uint32_t cas = parameters["cas"].get_value(); + biases[0] = (uint8_t) (cas >> 16); + biases[1] = (uint8_t) (cas >> 8); + biases[2] = (uint8_t) (cas >> 0); + + uint32_t injGnd = parameters["injGnd"].get_value(); + biases[3] = (uint8_t) (injGnd >> 16); + biases[4] = (uint8_t) (injGnd >> 8); + biases[5] = (uint8_t) (injGnd >> 0); + + uint32_t reqPd = parameters["reqPd"].get_value(); + biases[6] = (uint8_t) (reqPd >> 16); + biases[7] = (uint8_t) (reqPd >> 8); + biases[8] = (uint8_t) (reqPd >> 0); + + uint32_t puX = parameters["puX"].get_value(); + biases[9] = (uint8_t) (puX >> 16); + biases[10] = (uint8_t) (puX >> 8); + biases[11] = (uint8_t) (puX >> 0); + + uint32_t diffOff = parameters["diffOff"].get_value(); + biases[12] = (uint8_t) (diffOff >> 16); + biases[13] = (uint8_t) (diffOff >> 8); + biases[14] = (uint8_t) (diffOff >> 0); + + uint32_t req = parameters["req"].get_value(); + biases[15] = (uint8_t) (req >> 16); + biases[16] = (uint8_t) (req >> 8); + biases[17] = (uint8_t) (req >> 0); + + uint32_t refr = parameters["refr"].get_value(); + biases[18] = (uint8_t) (refr >> 16); + biases[19] = (uint8_t) (refr >> 8); + biases[20] = (uint8_t) (refr >> 0); + + uint32_t puY = parameters["puY"].get_value(); + biases[21] = (uint8_t) (puY >> 16); + biases[22] = (uint8_t) (puY >> 8); + biases[23] = (uint8_t) (puY >> 0); + + uint32_t diffOn = parameters["diffOn"].get_value(); + biases[24] = (uint8_t) (diffOn >> 16); + biases[25] = (uint8_t) (diffOn >> 8); + biases[26] = (uint8_t) (diffOn >> 0); + + uint32_t diff = parameters["diff"].get_value(); + biases[27] = (uint8_t) (diff >> 16); + biases[28] = (uint8_t) (diff >> 8); + biases[29] = (uint8_t) (diff >> 0); + + uint32_t foll = parameters["foll"].get_value(); + biases[30] = (uint8_t) (foll >> 16); + biases[31] = (uint8_t) (foll >> 8); + biases[32] = (uint8_t) (foll >> 0); + + uint32_t Pr = parameters["Pr"].get_value(); + biases[33] = (uint8_t) (Pr >> 16); + biases[34] = (uint8_t) (Pr >> 8); + biases[35] = (uint8_t) (Pr >> 0); + + device_mutex.lock(); + libusb_control_transfer(device_handle, LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_SEND_BIASES, 0, 0, biases, sizeof(biases), 0); + device_mutex.unlock(); +} + +} // namespace diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/.vscode/settings.json b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/.vscode/settings.json new file mode 100644 index 0000000000000000000000000000000000000000..c925ebcc2f19d9a7598fae22842622ffddaf96d2 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "C_Cpp.intelliSenseEngineFallback": "Disabled" +} \ No newline at end of file diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/CMakeLists.txt b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/CMakeLists.txt new file mode 100755 index 0000000000000000000000000000000000000000..0861ef29d18a32462ffb803d609a4470a1eb0f51 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 2.8.3) +project(dvs_meanshift) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + cv_bridge + dvs_msgs + image_transport + pcl_ros + roscpp + rospy + sensor_msgs + std_msgs +) + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED) +find_package(OpenCV REQUIRED) +find_package(PCL 1.7 REQUIRED) + +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES dvs_meanshift + CATKIN_DEPENDS cv_bridge dvs_msgs image_transport pcl_ros roscpp rospy sensor_msgs std_msgs + DEPENDS OpenCV boost +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(dvs_meanshift ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(dvs_meanshift_node src/dvs_meanshift_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(dvs_meanshift_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(dvs_meanshift_node +# ${catkin_LIBRARIES} +# ) + +add_executable(dvs_meanshift src/meanshift.cpp src/meanshift_node.cpp src/color_meanshift.cpp src/segment.cpp src/image_reconst.cpp) +target_link_libraries(dvs_meanshift ${catkin_LIBRARIES} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${OpenCV_LIBS}) +add_dependencies(dvs_meanshift ${catkin_EXPORTED_TARGETS} ) diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/dvs_meanshift/color_meanshift.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/dvs_meanshift/color_meanshift.h new file mode 100755 index 0000000000000000000000000000000000000000..bd20d1b0b4edec2b1411e8887c4b0920d30b29ed --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/dvs_meanshift/color_meanshift.h @@ -0,0 +1,18 @@ +/* + * color_meanshift.h + * + * Created on: Nov 2, 2016 + * Author: fran + */ + +#ifndef COLOR_MEANSHIFT_H_ +#define COLOR_MEANSHIFT_H_ + +#include <ros/ros.h> +#include <opencv2/core/core.hpp> + +void colorMeanShiftFilt(double *newPixelsPtr, const double *pixelsPtr, int mPixels, int maxPixelDim, const double *imagePtr, int numRows, int numCols, float spaceDivider, char *kernelFun, int maxIterNum, float tolFun); +//void colorMeanShiftFilt(double *newPixelsPtr, const double *pixelsPtr, int mPixels, int maxPixelDim, const double *imagePtr, int numRows, int numCols, float spaceDivider, char *kernelFun, int maxIterNum, float tolFun, double *newImagePtr); +void meanshiftCluster_Gaussian(cv::Mat dataPts, std::vector<double> *clusterCenterX, std::vector<double> *clusterCenterY, std::vector<double> *clusterCenterZ, std::vector<int> *point2Clusters, double bandwidth); + +#endif /* COLOR_MEANSHIFT_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/dvs_meanshift/meanshift.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/dvs_meanshift/meanshift.h new file mode 100755 index 0000000000000000000000000000000000000000..d8197cfb2d7c0da436138b2e78ad5397a1879ee9 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/dvs_meanshift/meanshift.h @@ -0,0 +1,151 @@ +/* + * dvs_meanshift.h + * + * Created on: Nov 2, 2016 + * Author: fran + */ + +#ifndef MEANSHIFT_H_ +#define MEANSHIFT_H_ + +#include <ros/ros.h> +#include <iostream> +#include <image_transport/image_transport.h> +#include <cv_bridge/cv_bridge.h> +#include <sensor_msgs/CameraInfo.h> +#include <sensor_msgs/Image.h> +#include <sensor_msgs/image_encodings.h> + +#include <opencv2/core/core.hpp> +#include <opencv2/imgproc/imgproc.hpp> +#include <opencv2/photo/photo.hpp> +#include <opencv2/highgui/highgui.hpp> +#include <opencv2/video/video.hpp> + +#include <dvs_msgs/Event.h> +#include <dvs_msgs/EventArray.h> +#include "graph3d/segment.h" + +#define TEST 0 +#define KALMAN_FILTERING 1 +#define BG_FILTERING 1 + + +#define DVSW 240 //240 +#define DVSH 180 //180 + +//#define DVSW 240 +//#define DVSH 180 + +namespace dvs_meanshift +{ + +class Meanshift { +public: + Meanshift(ros::NodeHandle & nh, ros::NodeHandle nh_private); + virtual ~Meanshift(); + +private: + ros::NodeHandle nh_; + + void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg); + void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg); + void eventsCallback_simple(const dvs_msgs::EventArray::ConstPtr& msg); + + //void assignClusterColor(std::vector<int> * positionClusterColor, int numClusters, std::vector<int> matches, std::vector<int> oldPositions); + void assignClusterColor(std::vector<int> * positionClusterColor, int numClusters, std::vector<int> matches, std::vector<int> oldPositions, std::vector<int> activeTrajectories); + //void createKalmanFilter(); + void createKalmanFilter(cv::KalmanFilter *kf, cv::Mat *state, cv::Mat *meas); + + int lastPositionClusterColor; + + cv::Mat camera_matrix_, dist_coeffs_; + + ros::Subscriber event_sub_; + ros::Subscriber camera_info_sub_; + + image_transport::Publisher image_pub_; + image_transport::Publisher image_segmentation_pub_; + + + //DEBUG + image_transport::Publisher image_debug1_pub_; + image_transport::Publisher image_debug2_pub_; + //NOT DEBUG + + + cv::Mat last_image_; + bool used_last_image_; + + enum DisplayMethod + { + GRAYSCALE, RED_BLUE + } display_method_; + + //Meanshift params + //computing image calibration + double *outputFeatures; + int mPixels, maxPixelDim; + double *pixelsPtr; + double *imagePtr; + //double *positionsPtr; + double *initializationPtr; + int numRows, numCols, maxIterNum; + float spaceDivider, timeDivider; + char kernelFun[8]; + float tolFun; + + //Params for graph3d segmentation + static const int MAX_NUM_CLUSTERS = 256; + static const int MAX_NUM_TRAJECTORY_POINTS=16; + std::vector<cv::Vec3b> RGBColors; + std::vector<cv::Point> *allTrajectories; //It is an array of vectors of Points for storing trajectories (a maximum of 8 points should be stored) + std::vector<int> counterTrajectories; //It stores the last position occupied for each trajectory in allTrajectories + + float sigma; + int k; + int min_region; + int num_components; + + //Clusters + std::vector<double> prev_clustCentX, prev_clustCentY, prev_clustCentZ; + std::vector<int> prev_positionClusterColor; + std::vector<int> clustColor; + std::vector<int> prev_activeTrajectories; + + //Trajectories + //cv::Mat trajectories=cv::Mat(DVSH, DVSW, CV_8UC3); + cv::Mat trajectories; + + //Kalman filter parameters + //cv::KalmanFilter kf; + //cv::Mat state; + //cv::Mat meas; + + std::vector<cv::KalmanFilter> vector_of_kf; + std::vector<cv::Mat> vector_of_state; + std::vector<cv::Mat> vector_of_meas; + + //bool foundBlobs; + std::vector<bool> vector_of_foundBlobs; + int notFoundBlobsCount; + std::vector<bool> foundTrajectory; + //bool foundTrajectory; + int selectedTrajectory; + //double ticks; + std::vector<double> vector_of_ticks; + + //frame of times + cv::Mat BGAFframe; + + //Variance stuff + std::vector<uint64_t> preVariance(MAX_NUM_CLUSTERS-1, 0); + std::vector<uint32_t> counterFor(MAX_NUM_CLUSTERS-1, 0); + std::vector<uint32_t> counterBar(MAX_NUM_CLUSTERS-1, 0); + //Robot movement + ros::Publisher dirUB; +}; + +} // namespace + +#endif // MEANSHIFT_H_ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/convolve.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/convolve.h new file mode 100755 index 0000000000000000000000000000000000000000..6c2a77339823de1835642bc9df0f8bd0754448c5 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/convolve.h @@ -0,0 +1,69 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* convolution */ + +#ifndef CONVOLVE_H +#define CONVOLVE_H + +#include <vector> +#include <algorithm> +#include <cmath> +#include "graph3d/image.h" + +/* convolve src with mask. dst is flipped! */ +static void convolve_even(image<float> *src, image<float> *dst, + std::vector<float> &mask) { + int width = src->width(); + int height = src->height(); + int len = mask.size(); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + float sum = mask[0] * imRef(src, x, y); + for (int i = 1; i < len; i++) { + sum += mask[i] * + (imRef(src, std::max(x-i,0), y) + + imRef(src, std::min(x+i, width-1), y)); + } + imRef(dst, y, x) = sum; + } + } +} + +/* convolve src with mask. dst is flipped! */ +static void convolve_odd(image<float> *src, image<float> *dst, + std::vector<float> &mask) { + int width = src->width(); + int height = src->height(); + int len = mask.size(); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + float sum = mask[0] * imRef(src, x, y); + for (int i = 1; i < len; i++) { + sum += mask[i] * + (imRef(src, std::max(x-i,0), y) - + imRef(src, std::min(x+i, width-1), y)); + } + imRef(dst, y, x) = sum; + } + } +} + +#endif diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/disjoint-set.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/disjoint-set.h new file mode 100755 index 0000000000000000000000000000000000000000..061b68c8a4f382c96c6c0bf5b1fe08ce12c81f5f --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/disjoint-set.h @@ -0,0 +1,79 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef DISJOINT_SET +#define DISJOINT_SET + +// disjoint-set forests using union-by-rank and path compression (sort of). + +typedef struct { + int rank; + int p; + int size; +} uni_elt; + +class universe { +public: + universe(int elements); + ~universe(); + int find(int x); + void join(int x, int y); + int size(int x) const { return elts[x].size; } + int num_sets() const { return num; } + +private: + uni_elt *elts; + int num; +}; + +universe::universe(int elements) { + elts = new uni_elt[elements]; + num = elements; + for (int i = 0; i < elements; i++) { + elts[i].rank = 0; + elts[i].size = 1; + elts[i].p = i; + } +} + +universe::~universe() { + delete [] elts; +} + +int universe::find(int x) { + int y = x; + while (y != elts[y].p) + y = elts[y].p; + elts[x].p = y; + return y; +} + +void universe::join(int x, int y) { + if (elts[x].rank > elts[y].rank) { + elts[y].p = x; + elts[x].size += elts[y].size; + } else { + elts[x].p = y; + elts[y].size += elts[x].size; + if (elts[x].rank == elts[y].rank) + elts[y].rank++; + } + num--; +} + +#endif diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/filter.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/filter.h new file mode 100755 index 0000000000000000000000000000000000000000..438c5acadb0374b669769b45a282051ab1fc4c79 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/filter.h @@ -0,0 +1,100 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* simple filters */ + +#ifndef FILTER_H +#define FILTER_H + +#include <vector> +#include <cmath> +#include "graph3d/image.h" +#include "graph3d/misc.h" +#include "graph3d/convolve.h" +#include "graph3d/imconv.h" + +#define WIDTH 4.0 + +/* normalize mask so it integrates to one */ +static void normalize(std::vector<float> &mask) { + int len = mask.size(); + float sum = 0; + for (int i = 1; i < len; i++) { + sum += fabs(mask[i]); + } + sum = 2*sum + fabs(mask[0]); + for (int i = 0; i < len; i++) { + mask[i] /= sum; + } +} + +/* make filters */ +#define MAKE_FILTER(name, fun) \ +static std::vector<float> make_ ## name (float sigma) { \ + sigma = std::max(sigma, 0.01F); \ + int len = (int)ceil(sigma * WIDTH) + 1; \ + std::vector<float> mask(len); \ + for (int i = 0; i < len; i++) { \ + mask[i] = fun; \ + } \ + return mask; \ +} + +MAKE_FILTER(fgauss, exp(-0.5*square(i/sigma))); + +/* convolve image with gaussian filter */ +static image<float> *smooth(image<float> *src, float sigma) { + std::vector<float> mask = make_fgauss(sigma); + normalize(mask); + + image<float> *tmp = new image<float>(src->height(), src->width(), false); + image<float> *dst = new image<float>(src->width(), src->height(), false); + convolve_even(src, tmp, mask); + convolve_even(tmp, dst, mask); + + delete tmp; + return dst; +} + +/* convolve image with gaussian filter */ +image<float> *smooth(image<uchar> *src, float sigma) { + image<float> *tmp = imageUCHARtoFLOAT(src); + image<float> *dst = smooth(tmp, sigma); + delete tmp; + return dst; +} + +/* compute laplacian */ +static image<float> *laplacian(image<float> *src) { + int width = src->width(); + int height = src->height(); + image<float> *dst = new image<float>(width, height); + + for (int y = 1; y < height-1; y++) { + for (int x = 1; x < width-1; x++) { + float d2x = imRef(src, x-1, y) + imRef(src, x+1, y) - + 2*imRef(src, x, y); + float d2y = imRef(src, x, y-1) + imRef(src, x, y+1) - + 2*imRef(src, x, y); + imRef(dst, x, y) = d2x + d2y; + } + } + return dst; +} + +#endif diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/image.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/image.h new file mode 100755 index 0000000000000000000000000000000000000000..c542465dc10b5029753e2758288b34338015e006 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/image.h @@ -0,0 +1,101 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* a simple image class */ + +#ifndef IMAGE_H +#define IMAGE_H + +#include <cstring> + +template <class T> +class image { + public: + /* create an image */ + image(const int width, const int height, const bool init = true); + + /* delete an image */ + ~image(); + + /* init an image */ + void init(const T &val); + + /* copy an image */ + image<T> *copy() const; + + /* get the width of an image. */ + int width() const { return w; } + + /* get the height of an image. */ + int height() const { return h; } + + /* image data. */ + T *data; + + /* row pointers. */ + T **access; + + private: + int w, h; +}; + +/* use imRef to access image data. */ +#define imRef(im, x, y) (im->access[y][x]) + +/* use imPtr to get pointer to image data. */ +#define imPtr(im, x, y) &(im->access[y][x]) + +template <class T> +image<T>::image(const int width, const int height, const bool init) { + w = width; + h = height; + data = new T[w * h]; // allocate space for image data + access = new T*[h]; // allocate space for row pointers + + // initialize row pointers + for (int i = 0; i < h; i++) + access[i] = data + (i * w); + + if (init) + memset(data, 0, w * h * sizeof(T)); +} + +template <class T> +image<T>::~image() { + delete [] data; + delete [] access; +} + +template <class T> +void image<T>::init(const T &val) { + T *ptr = imPtr(this, 0, 0); + T *end = imPtr(this, w-1, h-1); + while (ptr <= end) + *ptr++ = val; +} + + +template <class T> +image<T> *image<T>::copy() const { + image<T> *im = new image<T>(w, h, false); + memcpy(im->data, data, w * h * sizeof(T)); + return im; +} + +#endif + diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/imconv.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/imconv.h new file mode 100755 index 0000000000000000000000000000000000000000..ba78bcc622bf39c8e04dbb9fabf9a22fb71c8edd --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/imconv.h @@ -0,0 +1,177 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* image conversion */ + +#ifndef CONV_H +#define CONV_H + +#include <climits> +#include "graph3d/image.h" +#include "graph3d/imutil.h" +#include "graph3d/misc.h" + +#define RED_WEIGHT 0.299 +#define GREEN_WEIGHT 0.587 +#define BLUE_WEIGHT 0.114 + +static image<uchar> *imageRGBtoGRAY(image<rgb> *input) { + int width = input->width(); + int height = input->height(); + image<uchar> *output = new image<uchar>(width, height, false); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + imRef(output, x, y) = (uchar) + (imRef(input, x, y).r * RED_WEIGHT + + imRef(input, x, y).g * GREEN_WEIGHT + + imRef(input, x, y).b * BLUE_WEIGHT); + } + } + return output; +} + +static image<rgb> *imageGRAYtoRGB(image<uchar> *input) { + int width = input->width(); + int height = input->height(); + image<rgb> *output = new image<rgb>(width, height, false); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + imRef(output, x, y).r = imRef(input, x, y); + imRef(output, x, y).g = imRef(input, x, y); + imRef(output, x, y).b = imRef(input, x, y); + } + } + return output; +} + +static image<float> *imageUCHARtoFLOAT(image<uchar> *input) { + int width = input->width(); + int height = input->height(); + image<float> *output = new image<float>(width, height, false); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + imRef(output, x, y) = imRef(input, x, y); + } + } + return output; +} + +static image<float> *imageINTtoFLOAT(image<int> *input) { + int width = input->width(); + int height = input->height(); + image<float> *output = new image<float>(width, height, false); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + imRef(output, x, y) = imRef(input, x, y); + } + } + return output; +} + +static image<uchar> *imageFLOATtoUCHAR(image<float> *input, + float min, float max) { + int width = input->width(); + int height = input->height(); + image<uchar> *output = new image<uchar>(width, height, false); + + if (max == min) + return output; + + float scale = UCHAR_MAX / (max - min); + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + uchar val = (uchar)((imRef(input, x, y) - min) * scale); + imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX); + } + } + return output; +} + +static image<uchar> *imageFLOATtoUCHAR(image<float> *input) { + float min, max; + min_max(input, &min, &max); + return imageFLOATtoUCHAR(input, min, max); +} + +static image<long> *imageUCHARtoLONG(image<uchar> *input) { + int width = input->width(); + int height = input->height(); + image<long> *output = new image<long>(width, height, false); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + imRef(output, x, y) = imRef(input, x, y); + } + } + return output; +} + +static image<uchar> *imageLONGtoUCHAR(image<long> *input, long min, long max) { + int width = input->width(); + int height = input->height(); + image<uchar> *output = new image<uchar>(width, height, false); + + if (max == min) + return output; + + float scale = UCHAR_MAX / (float)(max - min); + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + uchar val = (uchar)((imRef(input, x, y) - min) * scale); + imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX); + } + } + return output; +} + +static image<uchar> *imageLONGtoUCHAR(image<long> *input) { + long min, max; + min_max(input, &min, &max); + return imageLONGtoUCHAR(input, min, max); +} + +static image<uchar> *imageSHORTtoUCHAR(image<short> *input, + short min, short max) { + int width = input->width(); + int height = input->height(); + image<uchar> *output = new image<uchar>(width, height, false); + + if (max == min) + return output; + + float scale = UCHAR_MAX / (float)(max - min); + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + uchar val = (uchar)((imRef(input, x, y) - min) * scale); + imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX); + } + } + return output; +} + +static image<uchar> *imageSHORTtoUCHAR(image<short> *input) { + short min, max; + min_max(input, &min, &max); + return imageSHORTtoUCHAR(input, min, max); +} + +#endif diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/imutil.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/imutil.h new file mode 100755 index 0000000000000000000000000000000000000000..f59c65d5dc5b9b7a749c44918321a0702c75a523 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/imutil.h @@ -0,0 +1,66 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* some image utilities */ + +#ifndef IMUTIL_H +#define IMUTIL_H + +#include "graph3d/image.h" +#include "graph3d/misc.h" + +/* compute minimum and maximum value in an image */ +template <class T> +void min_max(image<T> *im, T *ret_min, T *ret_max) { + int width = im->width(); + int height = im->height(); + + T min = imRef(im, 0, 0); + T max = imRef(im, 0, 0); + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + T val = imRef(im, x, y); + if (min > val) + min = val; + if (max < val) + max = val; + } + } + + *ret_min = min; + *ret_max = max; +} + +/* threshold image */ +template <class T> +image<uchar> *threshold(image<T> *src, int t) { + int width = src->width(); + int height = src->height(); + image<uchar> *dst = new image<uchar>(width, height); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + imRef(dst, x, y) = (imRef(src, x, y) >= t); + } + } + + return dst; +} + +#endif + diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/misc.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/misc.h new file mode 100755 index 0000000000000000000000000000000000000000..4e4a43961ef134a232badaf3c7ca1052b2fbca0e --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/misc.h @@ -0,0 +1,65 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* random stuff */ + +#ifndef MISC_H +#define MISC_H + +#include <cmath> + +#ifndef M_PI +#define M_PI 3.141592653589793 +#endif + +typedef unsigned char uchar; + +typedef struct { uchar r, g, b; } rgb; + +inline bool operator==(const rgb &a, const rgb &b) { + return ((a.r == b.r) && (a.g == b.g) && (a.b == b.b)); +} + +template <class T> +inline T abs(const T &x) { return (x > 0 ? x : -x); }; + +template <class T> +inline int sign(const T &x) { return (x >= 0 ? 1 : -1); }; + +template <class T> +inline T square(const T &x) { return x*x; }; + +template <class T> +inline T bound(const T &x, const T &min, const T &max) { + return (x < min ? min : (x > max ? max : x)); +} + +template <class T> +inline bool check_bound(const T &x, const T&min, const T &max) { + return ((x < min) || (x > max)); +} + +inline int vlib_round(float x) { return (int)(x + 0.5F); } + +inline int vlib_round(double x) { return (int)(x + 0.5); } + +inline double gaussian(double val, double sigma) { + return exp(-square(val/sigma)/2)/(sqrt(2*M_PI)*sigma); +} + +#endif diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/pnmfile.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/pnmfile.h new file mode 100755 index 0000000000000000000000000000000000000000..acc98bebd5f11ab19299bb102aa23fc40b3e0cfa --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/pnmfile.h @@ -0,0 +1,211 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* basic image I/O */ + +#ifndef PNM_FILE_H +#define PNM_FILE_H + +#include <cstdlib> +#include <climits> +#include <cstring> +#include <fstream> +#include "graph3d/image.h" +#include "graph3d/misc.h" + +#define BUF_SIZE 256 + +class pnm_error { }; + +static void read_packed(unsigned char *data, int size, std::ifstream &f) { + unsigned char c = 0; + + int bitshift = -1; + for (int pos = 0; pos < size; pos++) { + if (bitshift == -1) { + c = f.get(); + bitshift = 7; + } + data[pos] = (c >> bitshift) & 1; + bitshift--; + } +} + +static void write_packed(unsigned char *data, int size, std::ofstream &f) { + unsigned char c = 0; + + int bitshift = 7; + for (int pos = 0; pos < size; pos++) { + c = c | (data[pos] << bitshift); + bitshift--; + if ((bitshift == -1) || (pos == size-1)) { + f.put(c); + bitshift = 7; + c = 0; + } + } +} + +/* read PNM field, skipping comments */ +static void pnm_read(std::ifstream &file, char *buf) { + char doc[BUF_SIZE]; + char c; + + file >> c; + while (c == '#') { + file.getline(doc, BUF_SIZE); + file >> c; + } + file.putback(c); + + file.width(BUF_SIZE); + file >> buf; + file.ignore(); +} + +static image<uchar> *loadPBM(const char *name) { + char buf[BUF_SIZE]; + + /* read header */ + std::ifstream file(name, std::ios::in | std::ios::binary); + pnm_read(file, buf); + if (strncmp(buf, "P4", 2)) + throw pnm_error(); + + pnm_read(file, buf); + int width = atoi(buf); + pnm_read(file, buf); + int height = atoi(buf); + + /* read data */ + image<uchar> *im = new image<uchar>(width, height); + for (int i = 0; i < height; i++) + read_packed(imPtr(im, 0, i), width, file); + + return im; +} + +static void savePBM(image<uchar> *im, const char *name) { + int width = im->width(); + int height = im->height(); + std::ofstream file(name, std::ios::out | std::ios::binary); + + file << "P4\n" << width << " " << height << "\n"; + for (int i = 0; i < height; i++) + write_packed(imPtr(im, 0, i), width, file); +} + +static image<uchar> *loadPGM(const char *name) { + char buf[BUF_SIZE]; + + /* read header */ + std::ifstream file(name, std::ios::in | std::ios::binary); + pnm_read(file, buf); + if (strncmp(buf, "P5", 2)) + throw pnm_error(); + + pnm_read(file, buf); + int width = atoi(buf); + pnm_read(file, buf); + int height = atoi(buf); + + pnm_read(file, buf); + if (atoi(buf) > UCHAR_MAX) + throw pnm_error(); + + /* read data */ + image<uchar> *im = new image<uchar>(width, height); + file.read((char *)imPtr(im, 0, 0), width * height * sizeof(uchar)); + + return im; +} + +static void savePGM(image<uchar> *im, const char *name) { + int width = im->width(); + int height = im->height(); + std::ofstream file(name, std::ios::out | std::ios::binary); + + file << "P5\n" << width << " " << height << "\n" << UCHAR_MAX << "\n"; + file.write((char *)imPtr(im, 0, 0), width * height * sizeof(uchar)); +} + +static image<rgb> *loadPPM(const char *name) { + char buf[BUF_SIZE], doc[BUF_SIZE]; + + /* read header */ + std::ifstream file(name, std::ios::in | std::ios::binary); + pnm_read(file, buf); + if (strncmp(buf, "P6", 2)) + throw pnm_error(); + + pnm_read(file, buf); + int width = atoi(buf); + pnm_read(file, buf); + int height = atoi(buf); + + pnm_read(file, buf); + if (atoi(buf) > UCHAR_MAX) + throw pnm_error(); + + /* read data */ + image<rgb> *im = new image<rgb>(width, height); + file.read((char *)imPtr(im, 0, 0), width * height * sizeof(rgb)); + + return im; +} + +static void savePPM(image<rgb> *im, const char *name) { + int width = im->width(); + int height = im->height(); + std::ofstream file(name, std::ios::out | std::ios::binary); + + file << "P6\n" << width << " " << height << "\n" << UCHAR_MAX << "\n"; + file.write((char *)imPtr(im, 0, 0), width * height * sizeof(rgb)); +} + +template <class T> +void load_image(image<T> **im, const char *name) { + char buf[BUF_SIZE]; + + /* read header */ + std::ifstream file(name, std::ios::in | std::ios::binary); + pnm_read(file, buf); + if (strncmp(buf, "VLIB", 9)) + throw pnm_error(); + + pnm_read(file, buf); + int width = atoi(buf); + pnm_read(file, buf); + int height = atoi(buf); + + /* read data */ + *im = new image<T>(width, height); + file.read((char *)imPtr((*im), 0, 0), width * height * sizeof(T)); +} + +template <class T> +void save_image(image<T> *im, const char *name) { + int width = im->width(); + int height = im->height(); + std::ofstream file(name, std::ios::out | std::ios::binary); + + //file << "VLIB\n" << width << " " << height << "\n"; + file.write((char *)imPtr(im, 0, 0), width * height * sizeof(T)); +} + +#endif diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/segment-graph.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/segment-graph.h new file mode 100755 index 0000000000000000000000000000000000000000..ec97b7f59d059fc4ce5750123caa6716bfacf23c --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/segment-graph.h @@ -0,0 +1,83 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef SEGMENT_GRAPH +#define SEGMENT_GRAPH + +#include <algorithm> +#include <cmath> +#include "graph3d/disjoint-set.h" + +// threshold function +#define THRESHOLD(size, c) (c/size) + +typedef struct { + float w; + int a, b; +} edge; + +bool operator<(const edge &a, const edge &b) { + return a.w < b.w; +} + +/* + * Segment a graph + * + * Returns a disjoint-set forest representing the segmentation. + * + * num_vertices: number of vertices in graph. + * num_edges: number of edges in graph + * edges: array of edges. + * c: constant for treshold function. + */ +universe *segment_graph(int num_vertices, int num_edges, edge *edges, + float c) { + // sort edges by weight + std::sort(edges, edges + num_edges); + + // make a disjoint-set forest + universe *u = new universe(num_vertices); + + // init thresholds + float *threshold = new float[num_vertices]; + for (int i = 0; i < num_vertices; i++) + threshold[i] = THRESHOLD(1,c); + + // for each edge, in non-decreasing weight order... + for (int i = 0; i < num_edges; i++) { + edge *pedge = &edges[i]; + + // components conected by this edge + int a = u->find(pedge->a); + int b = u->find(pedge->b); + if (a != b) { + if ((pedge->w <= threshold[a]) && + (pedge->w <= threshold[b])) { + u->join(a, b); + a = u->find(a); + threshold[a] = pedge->w + THRESHOLD(u->size(a), c); + } + } + } + + // free up + delete threshold; + return u; +} + +#endif diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/segment-image.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/segment-image.h new file mode 100755 index 0000000000000000000000000000000000000000..6079e799f8194dfcdb27758f8d4a14d6f9bd122f --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/segment-image.h @@ -0,0 +1,153 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef SEGMENT_IMAGE +#define SEGMENT_IMAGE + +#include <cstdlib> +#include "graph3d/image.h" +#include "graph3d/misc.h" +#include "graph3d/filter.h" +#include "graph3d/segment-graph.h" + +// random color +rgb random_rgb(){ + rgb c; + double r; + + c.r = (uchar)random(); + c.g = (uchar)random(); + c.b = (uchar)random(); + + return c; +} + +// dissimilarity measure between pixels +static inline float diff(image<float> *r, image<float> *g, image<float> *b, + int x1, int y1, int x2, int y2) { + return sqrt(square(imRef(r, x1, y1)-imRef(r, x2, y2)) + + square(imRef(g, x1, y1)-imRef(g, x2, y2)) + + square(imRef(b, x1, y1)-imRef(b, x2, y2))); +} + +/* + * Segment an image + * + * Returns a color image representing the segmentation. + * + * im: image to segment. + * sigma: to smooth the image. + * c: constant for treshold function. + * min_size: minimum component size (enforced by post-processing stage). + * num_ccs: number of connected components in the segmentation. + */ +image<rgb> *segment_image(image<rgb> *im, float sigma, float c, int min_size, int *num_ccs) { + int width = im->width(); + int height = im->height(); + + image<float> *r = new image<float>(width, height); + image<float> *g = new image<float>(width, height); + image<float> *b = new image<float>(width, height); + + // smooth each color channel + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + imRef(r, x, y) = imRef(im, x, y).r; + imRef(g, x, y) = imRef(im, x, y).g; + imRef(b, x, y) = imRef(im, x, y).b; + } + } + image<float> *smooth_r = smooth(r, sigma); + image<float> *smooth_g = smooth(g, sigma); + image<float> *smooth_b = smooth(b, sigma); + delete r; + delete g; + delete b; + + // build graph + edge *edges = new edge[width*height*4]; + int num = 0; + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + if (x < width-1) { + edges[num].a = y * width + x; + edges[num].b = y * width + (x+1); + edges[num].w = diff(smooth_r, smooth_g, smooth_b, x, y, x+1, y); + num++; + } + + if (y < height-1) { + edges[num].a = y * width + x; + edges[num].b = (y+1) * width + x; + edges[num].w = diff(smooth_r, smooth_g, smooth_b, x, y, x, y+1); + num++; + } + + if ((x < width-1) && (y < height-1)) { + edges[num].a = y * width + x; + edges[num].b = (y+1) * width + (x+1); + edges[num].w = diff(smooth_r, smooth_g, smooth_b, x, y, x+1, y+1); + num++; + } + + if ((x < width-1) && (y > 0)) { + edges[num].a = y * width + x; + edges[num].b = (y-1) * width + (x+1); + edges[num].w = diff(smooth_r, smooth_g, smooth_b, x, y, x+1, y-1); + num++; + } + } + } + delete smooth_r; + delete smooth_g; + delete smooth_b; + + // segment + universe *u = segment_graph(width*height, num, edges, c); + + // post process small components + for (int i = 0; i < num; i++) { + int a = u->find(edges[i].a); + int b = u->find(edges[i].b); + if ((a != b) && ((u->size(a) < min_size) || (u->size(b) < min_size))) + u->join(a, b); + } + delete [] edges; + *num_ccs = u->num_sets(); + + image<rgb> *output = new image<rgb>(width, height); + + // pick random colors for each component + rgb *colors = new rgb[width*height]; + for (int i = 0; i < width*height; i++) + colors[i] = random_rgb(); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + int comp = u->find(y * width + x); + imRef(output, x, y) = colors[comp]; + } + } + + delete [] colors; + delete u; + + return output; +} + +#endif diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/segment.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/segment.h new file mode 100755 index 0000000000000000000000000000000000000000..3696ec7e5f4c688630a7c65438ac2d71c5bf48fe --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/graph3d/segment.h @@ -0,0 +1,19 @@ +/* + * segment.h + * + * Created on: Nov 3, 2016 + * Author: fran + */ + +#ifndef SEGMENT_H_ +#define SEGMENT_H_ + +#include <cv_bridge/cv_bridge.h> +#include <opencv2/core/core.hpp> +#include <opencv2/imgproc/imgproc.hpp> + + +void imadjust(const cv::Mat & src, cv::Mat & dst); +void im2segment(cv::Mat input, cv::Mat cv_image_output, float sigma, float k, int min_size, int *num_ccs); + +#endif /* SEGMENT_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/image_reconstruct/image_reconst.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/image_reconstruct/image_reconst.h new file mode 100755 index 0000000000000000000000000000000000000000..c9a12e6cb957d2f5184e1a27b119976d27d156b6 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/include/image_reconstruct/image_reconst.h @@ -0,0 +1,35 @@ +/* + * image_reconst.h + * + * Created on: Nov 3, 2016 + * Author: fran + */ + +#ifndef IMAGE_RECONST_H_ +#define IMAGE_RECONST_H_ + +#include <opencv2/core/core.hpp> +#include <opencv2/opencv.hpp> +//#include <opencv2/core/core_c.h> +#include "opencv2/highgui/highgui_c.h" +//#include "opencv2/core/core.hpp" + + +void dst(double *btest, double *bfinal, int h, int w); +void idst(double *btest, double *bfinal, int h, int w); +void transpose(double *mat, double *mat_t, int h, int w); + +void getGradientX(cv::Mat &img, cv::Mat &gx); +void getGradientY(cv::Mat &img, cv::Mat &gy); +void lapx(cv::Mat &gx, cv::Mat &gxx); +void lapy(cv::Mat &gy, cv::Mat &gyy); + +void poisson_solver(cv::Mat &img, cv::Mat &gxx, cv::Mat &gyy, cv::Mat &result); +void poisson_solver_jacobi(cv::Mat &img, cv::Mat &gxx, cv::Mat &gyy, cv::Mat &result); + + +//IPL Image versions (legacy) +void lapx( const IplImage *img, IplImage *gxx); +void lapy( const IplImage *img, IplImage *gyy); +void poisson_solver(const IplImage *img, IplImage *gxx , IplImage *gyy, cv::Mat &result); +#endif /* IMAGE_RECONST_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/launch/dvs_mono.launch b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/launch/dvs_mono.launch new file mode 100755 index 0000000000000000000000000000000000000000..07ee3e77b014e886dc38e111e162bb0223923097 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/launch/dvs_mono.launch @@ -0,0 +1,39 @@ +<launch> + <!-- camera driver --> + <node name="dvs_ros_driver" pkg="dvs_ros_driver" type="dvs_ros_driver" output = "screen"> + <rosparam command="load" file="$(find dvs_ros_driver)/config/fast.yaml" /> + </node> + + <!-- visualization --> + <node name="dvs_meanshift" pkg="dvs_meanshift" type="dvs_meanshift" output = "screen"> + <!-- <param name="display_method" value="grayscale"/> --> + <param name="display_method" value="red-blue"/> + <remap from="events" to="/dvs/events" /> + <remap from="camera_info" to="/dvs/camera_info" /> + </node> + + <!-- display --> + <node name="image_view" pkg="image_view" type="image_view" output = "screen"> + <remap from="image" to="dvs_rendering"/> + </node> + + <node name="image_view2" pkg="image_view" type="image_view" output = "screen"> + <remap from="image" to="dvs_segmentation"/> + </node> + + <node name="image_view3" pkg="image_view" type="image_view" output = "screen"> + <remap from="image" to="dvs_debug1"/> + </node> + + <node name="image_view4" pkg="image_view" type="image_view" output = "screen"> + <remap from="image" to="dvs_debug2"/> + </node> + + <!-- <node name="image_view2" pkg="image_view" type="image_view" output = "screen"> --> + <!-- <remap from="image" to="dvs_undistorted"/> --> + <!-- </node> --> + + <!-- configure --> + <!-- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" /> --> + +</launch> diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/launch/dvs_segmentation.launch b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/launch/dvs_segmentation.launch new file mode 100755 index 0000000000000000000000000000000000000000..061b64d88577938e5274e0814db008d730b3740d --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/launch/dvs_segmentation.launch @@ -0,0 +1,31 @@ +<launch> + <!-- camera driver --> + <node name="dvs_ros_driver" pkg="dvs_ros_driver" type="dvs_ros_driver" output = "screen"> + <rosparam command="load" file="$(find dvs_ros_driver)/config/fast.yaml" /> + </node> + + <!-- visualization --> + <node name="dvs_meanshift" pkg="dvs_meanshift" type="dvs_meanshift" output = "screen"> + <!-- <param name="display_method" value="grayscale"/> --> + <param name="display_method" value="red-blue"/> + <remap from="events" to="/dvs/events" /> + <remap from="camera_info" to="/dvs/camera_info" /> + </node> + + <!-- display --> + <node name="rqt_image_view" pkg="rqt_image_view" type="rqt_image_view" output = "screen"> + <remap from="image" to="dvs_rendering"/> + </node> + + <node name="image_view2" pkg="rqt_image_view" type="rqt_image_view" output = "screen"> + <remap from="image" to="dvs_segmentation"/> + </node> + + <!-- <node name="image_view3" pkg="image_view" type="image_view" output = "screen"> --> + <!-- <remap from="image" to="dvs_debug1"/> --> + <!-- </node> --> + + <!-- configure --> + <!-- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" /> --> + +</launch> diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/package.xml b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/package.xml new file mode 100755 index 0000000000000000000000000000000000000000..05e2835bf6f75312d4d6c7d91b5ae1dba83d411f --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/package.xml @@ -0,0 +1,66 @@ +<?xml version="1.0"?> +<package> + <name>dvs_meanshift</name> + <version>0.0.0</version> + <description>The dvs_meanshift package</description> + + <!-- One maintainer tag required, multiple allowed, one person per tag --> + <!-- Example: --> + <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> + <maintainer email="fran@todo.todo">fran</maintainer> + + + <!-- One license tag required, multiple allowed, one license per tag --> + <!-- Commonly used license strings: --> + <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --> + <license>TODO</license> + + + <!-- Url tags are optional, but mutiple are allowed, one per tag --> + <!-- Optional attribute type can be: website, bugtracker, or repository --> + <!-- Example: --> + <!-- <url type="website">http://wiki.ros.org/dvs_meanshift</url> --> + + + <!-- Author tags are optional, mutiple are allowed, one per tag --> + <!-- Authors do not have to be maintianers, but could be --> + <!-- Example: --> + <!-- <author email="jane.doe@example.com">Jane Doe</author> --> + + + <!-- The *_depend tags are used to specify dependencies --> + <!-- Dependencies can be catkin packages or system dependencies --> + <!-- Examples: --> + <!-- Use build_depend for packages you need at compile time: --> + <!-- <build_depend>message_generation</build_depend> --> + <!-- Use buildtool_depend for build tool packages: --> + <!-- <buildtool_depend>catkin</buildtool_depend> --> + <!-- Use run_depend for packages you need at runtime: --> + <!-- <run_depend>message_runtime</run_depend> --> + <!-- Use test_depend for packages you need only for testing: --> + <!-- <test_depend>gtest</test_depend> --> + <buildtool_depend>catkin</buildtool_depend> + <build_depend>cv_bridge</build_depend> + <build_depend>dvs_msgs</build_depend> + <build_depend>image_transport</build_depend> + <build_depend>pcl_ros</build_depend> + <build_depend>roscpp</build_depend> + <build_depend>rospy</build_depend> + <build_depend>sensor_msgs</build_depend> + <build_depend>std_msgs</build_depend> + <run_depend>cv_bridge</run_depend> + <run_depend>dvs_msgs</run_depend> + <run_depend>image_transport</run_depend> + <run_depend>pcl_ros</run_depend> + <run_depend>roscpp</run_depend> + <run_depend>rospy</run_depend> + <run_depend>sensor_msgs</run_depend> + <run_depend>std_msgs</run_depend> + + + <!-- The export tag contains other, unspecified, tags --> + <export> + <!-- Other tools can request additional information be placed here --> + + </export> +</package> \ No newline at end of file diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/color_meanshift.cpp b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/color_meanshift.cpp new file mode 100755 index 0000000000000000000000000000000000000000..f1c8d2c021ec0a1f9ad62c933c3d28579b26d853 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/color_meanshift.cpp @@ -0,0 +1,739 @@ +#include <cstring> +#include <cmath> +#include <cstdlib> +#include "dvs_meanshift/color_meanshift.h" + +#define max(x,y) ((x)>(y)?(x):(y)) +#define min(x,y) ((x)<(y)?(x):(y)) + +#define _2D_to_linear(row, col, maxCol) ((row)*(maxCol)+(col)) +//#define _3D_to_linear(row, col, color, maxRow, maxCol) ((maxRow)*(maxCol)*(color)+(row)*(maxCol)+(col)) + +//#define _2D_to_linear(row, col, maxRow) ((col)*(maxRow)+(row)) +//#define _3D_to_linear(row, col, color, maxRow, maxCol) ((maxRow)*(maxCol)*(color)+(col)*(maxRow)+(row)) +//void colorMeanShiftFilt_helper_C(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) + +/*void colorMeanShiftFilt(double *newPixelsPtr, const double *pixelsPtr, int mPixels, int maxPixelDim, const double *imagePtr, int numRows, int numCols, float spaceDivider, char *kernelFun, int maxIterNum, float tolFun, double *newImagePtr) +{ + // Copy the original pixel values to the new pixel matrix + memcpy(newPixelsPtr, pixelsPtr, maxPixelDim*mPixels*sizeof(double)); + + // Copy the original image to a new image + memcpy(newImagePtr, imagePtr, mPixels*sizeof(double)); + + // Help variables + double x,y; + int rX, rY; + int neighX, neighY; + bool bRepeat=false; + + double *delta = (double *) calloc(maxPixelDim,sizeof(double)); + double *dX = (double *) calloc(maxPixelDim,sizeof(double)); + double *neighVec = (double *) calloc(maxPixelDim,sizeof(double)); + double deltaGrad2; + + // Create an array that indicates if we reached the end of the optimization for each pixel + double *optimizedPointArray=new double[mPixels]; + + for (int iterNum=0; iterNum<maxIterNum; iterNum++) + { + bRepeat=false; + + for (int pointNum=0; pointNum<mPixels; pointNum++) + { + if (optimizedPointArray[pointNum]==1) // The optimization for this pixel has reached the threshold + continue; + + x=newPixelsPtr[_2D_to_linear(0, pointNum, maxPixelDim)]*spaceDivider; + y=newPixelsPtr[_2D_to_linear(1, pointNum, maxPixelDim)]*spaceDivider; + + // Reset the delta variables + delta[0]=0; delta[1]=0; delta[2]=0; + dX[0]=0; dX[1]=0; dX[2]=0; + + //ROS_ERROR("y = %d, x = %d, pointNum = %d, spaceDivider = %g", (int)floor(y), (int)floor(x), pointNum, spaceDivider); + + double sumF=0; + double dist2; + + + // Find the neighbors around point (rX,rY) + //for (neighX=max(0, floor(x)-1-spaceDivider); neighX<min(numCols, ceil(x)+spaceDivider); neighX++) + for (neighX=max(0, floor(x)-spaceDivider); neighX<min(numCols, ceil(x)+spaceDivider); neighX++) + { + //for (neighY=max(0,floor(y)-1-spaceDivider); neighY<min(numRows, ceil(y)+spaceDivider); neighY++) + for (neighY=max(0,floor(y)-spaceDivider); neighY<min(numRows, ceil(y)+spaceDivider); neighY++) + { + neighVec[0]=(neighX)/spaceDivider; + neighVec[1]=(neighY)/spaceDivider; + neighVec[2]=newImagePtr[neighY*numCols+neighX]; + + + delta[0]=newPixelsPtr[_2D_to_linear(0,pointNum,maxPixelDim)]-neighVec[0]; + delta[1]=newPixelsPtr[_2D_to_linear(1,pointNum,maxPixelDim)]-neighVec[1]; + delta[2]=newPixelsPtr[_2D_to_linear(2,pointNum,maxPixelDim)]-neighVec[2]; + + dist2 = delta[0]*delta[0] + delta[1]*delta[1] + delta[2]*delta[2]; + + double kres=(exp(-dist2/2.0)); + + dX[0] += kres*neighVec[0]; + dX[1] += kres*neighVec[1]; + dX[2] += kres*neighVec[2]; + + sumF+=kres; + + //ROS_ERROR("newImagePtr[%d, %d]= %g", (int)(neighX), (int)(neighY), neighVec[2]); + } + } + + deltaGrad2=0; + + + dX[0]=(sumF==0)?0:dX[0]/sumF; //check for NaNs + dX[1]=(sumF==0)?0:dX[1]/sumF; //check for NaNs + dX[2]=(sumF==0)?0:dX[2]/sumF; //check for NaNs + + double tmp0=newPixelsPtr[_2D_to_linear(0, pointNum, maxPixelDim)]-dX[0]; + double tmp1=newPixelsPtr[_2D_to_linear(1, pointNum, maxPixelDim)]-dX[1]; + double tmp2=newPixelsPtr[_2D_to_linear(2, pointNum, maxPixelDim)]-dX[2]; + + //double tmp1=newPixelsPtr[_2D_to_linear(i, pointNum, mPixels)]-dX[i]; + deltaGrad2 = tmp0*tmp0 + tmp1*tmp1 + tmp2*tmp2; + newPixelsPtr[_2D_to_linear(0, pointNum, maxPixelDim)]=dX[0]; + newPixelsPtr[_2D_to_linear(1, pointNum, maxPixelDim)]=dX[1]; + newPixelsPtr[_2D_to_linear(2, pointNum, maxPixelDim)]=dX[2]; + + //ROS_ERROR("newImagePtr[%d, %d]= %g", (int)pointNum/numCols, (int)pointNum%numCols, dX[0]); + + // Find the corresponding row and column of the point + newImagePtr[pointNum]=dX[2]; + + //ROS_ERROR("newImagePtr[%d, %d] = %g", pointCol, pointRow, dX[0]); + + if (deltaGrad2<tolFun) + optimizedPointArray[pointNum]=1; + else + bRepeat=true; + + } // end pointNum loop + + if (bRepeat==false) + break; + + } // end for(iterNum) loop + + // CLEAN-UP + //delete[] newImagePtr; + delete[] optimizedPointArray; + delete(delta); + delete(dX); + delete(neighVec); +}*/ + +/*void colorMeanShiftFilt(double *newPixelsPtr, const double *pixelsPtr, int mPixels, int maxPixelDim, const double *imagePtr, int numRows, int numCols, float spaceDivider, char *kernelFun, int maxIterNum, float tolFun) +{ + // Copy the original pixel values to the new pixel matrix + memcpy(newPixelsPtr, pixelsPtr, maxPixelDim*mPixels*sizeof(double)); + + // Copy the original image to a new image + double *newImagePtr=new double[mPixels]; + memcpy(newImagePtr, imagePtr, mPixels*sizeof(double)); + + // Help variables + double x,y; + int neighX, neighY; + bool bRepeat=false; + + double *delta = (double *) calloc(maxPixelDim,sizeof(double)); + double *dX = (double *) calloc(maxPixelDim,sizeof(double)); + double *neighVec = (double *) calloc(maxPixelDim,sizeof(double)); + double deltaGrad2; + + // Create an array that indicates if we reached the end of the optimization for each pixel + double *optimizedPointArray=new double[mPixels]; + + for (int iterNum=0; iterNum<maxIterNum; iterNum++) + { + bRepeat=false; + for (int pointNum=0; pointNum<mPixels; pointNum++) + { + if (optimizedPointArray[pointNum]==1) // The optimization for this pixel has reached the threshold + continue; + + x=newPixelsPtr[_2D_to_linear(0, pointNum, mPixels)]*spaceDivider; + y=newPixelsPtr[_2D_to_linear(1, pointNum, mPixels)]*spaceDivider; + + delta[0]=0; delta[1]=0; delta[2]=0; + dX[0]=0; dX[1]=0; dX[2]=0; + + //ROS_ERROR("y = %d, x = %d, pointNum = %d, spaceDivider = %g", (int)floor(y), (int)floor(x), pointNum, spaceDivider); + + double sumF=0; + double dist2, kres; + + // Find the neighbors around point (rX,rY) + for (neighX=max(0, floor(x)-1-spaceDivider); neighX<min(numCols, ceil(x)+spaceDivider); neighX++) + { + for (neighY=max(0,floor(y)-1-spaceDivider); neighY<min(numRows, ceil(y)+spaceDivider); neighY++) + { + neighVec[0]=(neighX+1)/spaceDivider; + neighVec[1]=(neighY+1)/spaceDivider; + neighVec[2]=newImagePtr[neighY*numCols+neighX]; + + delta[0]=newPixelsPtr[_2D_to_linear(0,pointNum,mPixels)]-neighVec[0]; + delta[1]=newPixelsPtr[_2D_to_linear(1,pointNum,mPixels)]-neighVec[1]; + delta[2]=newPixelsPtr[_2D_to_linear(2,pointNum,mPixels)]-neighVec[2]; + + dist2 = delta[0]*delta[0]+delta[1]*delta[1]+delta[2]*delta[2]; + + kres =(exp(-dist2/2.0)); + + dX[0]+= kres*neighVec[0]; + dX[1]+= kres*neighVec[1]; + dX[2]+= kres*neighVec[2]; + sumF +=kres; + } + } + + // Find the corresponding row and column of the point + int pointRow = pointNum/numCols; + int pointCol = pointNum%numCols; + + dX[0]=(sumF==0)?0:dX[0]/sumF; //check for NaNs + dX[1]=(sumF==0)?0:dX[1]/sumF; //check for NaNs + dX[2]=(sumF==0)?0:dX[2]/sumF; //check for NaNs + //double tmp1=newPixelsPtr[_2D_to_linear(i, pointNum, maxPixelDim)]-dX[i]; + double tmp1=newPixelsPtr[_2D_to_linear(0, pointNum, mPixels)]-dX[0]; + double tmp2=newPixelsPtr[_2D_to_linear(1, pointNum, mPixels)]-dX[1]; + double tmp3=newPixelsPtr[_2D_to_linear(2, pointNum, mPixels)]-dX[2]; + + deltaGrad2=tmp1*tmp1+tmp2*tmp2+tmp3*tmp3; + newPixelsPtr[_2D_to_linear(0, pointNum, mPixels)]=dX[0]; + newPixelsPtr[_2D_to_linear(1, pointNum, mPixels)]=dX[1]; + newPixelsPtr[_2D_to_linear(2, pointNum, mPixels)]=dX[2]; + //newImagePtr[pointRow*numCols+pointCol]=dX[2]; + newImagePtr[pointNum]=dX[2]; + + if (deltaGrad2<tolFun) + optimizedPointArray[pointNum]=1; + else + bRepeat=true; + + } // end pointNum loop + + if (bRepeat==false) + break; + + } // end for(iterNum) loop + + + // CLEAN-UP + delete[] newImagePtr; + delete[] optimizedPointArray; + delete(delta); + delete(dX); + delete(neighVec); +}*/ + +/*void meanshiftCluster_Gaussian(cv::Mat dataPts, std::vector<double> *clusterCenterX, std::vector<double> *clusterCenterY, std::vector<double> *clusterCenterZ, std::vector<int> *point2Clusters, double bandwidth) +{ + //Initialization + int numPts = dataPts.cols; + int numDim = dataPts.rows; + int numClust = 0; + double bandSq = bandwidth*bandwidth; + cv::Range rng = cv::Range(1,numPts); + + cv::Mat onesAux = cv::Mat::ones(1, dataPts.cols, dataPts.type()); + + std::vector<int> initPtInds; + for (int i = rng.start; i <= rng.end; i++) initPtInds.push_back(i); + double stopThresh = 1E-3*bandwidth; //when mean has converged + int numInitPts = numPts; //track if a points been seen already + cv::Mat beenVisitedFlag = cv::Mat(1, numPts, CV_32S, cv::Scalar::all(0)); //number of points to posibaly use as initilization points + cv::Mat clusterVotes; //used to resolve conflicts on cluster membership + + double lambda = 10.; + double myMeanX, myMeanY, myMeanZ, myOldMeanX, myOldMeanY, myOldMeanZ, totalWeightX, totalWeightY, totalWeightZ; + int stInd; + + //std::vector<double> clusterCenterX, clusterCenterY, clusterCenterZ; + + int tempInd; + + while (numInitPts>0) + { + tempInd = (rand()%numInitPts); //pick a random seed point + stInd = initPtInds[tempInd]; //use this point as start of mean + myMeanX = dataPts.at<double>(cv::Point(stInd, 0)); //intilize mean to this points location + myMeanY = dataPts.at<double>(cv::Point(stInd, 1)); + myMeanZ = dataPts.at<double>(cv::Point(stInd, 2)); + cv::Mat thisClusterVotes = cv::Mat(1, numPts, CV_32S, cv::Scalar::all(0)); //used to resolve conflicts on cluster membership + //ROS_ERROR_STREAM("Before getting into while myMean = ["<<myMeanX<<", "<<myMeanY<<", "<<myMeanZ<<"]"); + cv::Mat_<double> myMean = ( cv::Mat_<double>(3, 1) << myMeanX, myMeanY, myMeanZ); + + + + while(true) + { + cv::Mat dataX, dataY, dataZ; + dataX.push_back(dataPts.row(0)); + dataY.push_back(dataPts.row(1)); + dataZ.push_back(dataPts.row(2)); + + cv::Mat diffX = dataX - myMeanX; + cv::Mat diffY = dataY - myMeanY; + cv::Mat diffZ = dataZ - myMeanZ; + + ROS_ERROR_STREAM("Original = "<<diffX); + ROS_ERROR_STREAM("onesAux.rows = "<<onesAux.rows<<" cols = "<<onesAux.cols); + ROS_ERROR_STREAM("dataPts.rows = "<<dataPts.rows<<" cols = "<<dataPts.cols); + ROS_ERROR_STREAM("myMean.rows = "<<myMean.rows<<" cols = "<<myMean.cols); + + cv::Mat diff = dataPts - myMean*onesAux; + diffX.push_back(diff.row(0)); + diffY.push_back(diff.row(1)); + diffZ.push_back(diff.row(2)); + + ROS_ERROR_STREAM("Matricial "<<diff.row(0)); + exit(0); + + cv::Mat diffXX = diffX.mul(diffX); + cv::Mat diffYY = diffY.mul(diffY); + cv::Mat diffZZ = diffZ.mul(diffZ); + + cv::Mat sqDistToAll = diffXX + diffYY + diffZZ; //dist squared from mean to all points still active + cv::Mat inInds = (sqDistToAll < bandSq); // Puts 255 wherever it is true + //ROS_ERROR_STREAM("This is the size "<<inInds.size()); + //ROS_ERROR_STREAM("This is the size "<<thisClusterVotes.size()); + //inInds.convertTo(inInds, CV_8UC1); + //cv::add(thisClusterVotes, cv::Mat::ones(1, numPts, CV_32S), thisClusterVotes, inInds); //add a vote for all the in points belonging to this cluster + cv::add(thisClusterVotes, cv::Scalar(1), thisClusterVotes, inInds); //add a vote for all the in points belonging to this cluster + //ROS_ERROR_STREAM("thisclusterVotes = "<<inInds); + //ROS_ERROR_STREAM("thisclusterVotes = "<<thisClusterVotes); + //ROS_ERROR_STREAM("thisclusterVotes = "<<sqDistToAll); + //exit(0); + + cv::Mat selectedDataX, selectedDataY, selectedDataZ; + //cv::bitwise_and(dataX, cv::Scalar(255), selectedDataX, inInds); + //cv::bitwise_and(dataY, cv::Scalar(255), selectedDataY, inInds); + //cv::bitwise_and(dataZ, cv::Scalar(255), selectedDataZ, inInds); + //ROS_ERROR_STREAM("Before: dataX ="<<dataX); + dataX.setTo(0., inInds==0); + dataY.setTo(0., inInds==0); + dataZ.setTo(0., inInds==0); + //ROS_ERROR_STREAM("inInds ="<<inInds); + //ROS_ERROR_STREAM("After: dataX ="<<dataX); + + double numOfOnes = (double) (cv::sum(inInds)[0])/255; //inInds is active if inInds[i]==255 + //ROS_ERROR_STREAM("Num of Ones ="<<numOfOnes); + //ROS_ERROR_STREAM("inInds ="<<inInds); + //exit(0); + myOldMeanX = myMeanX; myOldMeanY = myMeanY; myOldMeanZ = myMeanZ; + myMeanX = cv::sum(dataX)[0]/numOfOnes; + myMeanY = cv::sum(dataY)[0]/numOfOnes; + myMeanZ = cv::sum(dataZ)[0]/numOfOnes; + + + //ROS_ERROR_STREAM("inInds="<<inInds); + //ROS_ERROR_STREAM("After: dataX="<<dataX); + + diffX = dataX - myMeanX; + diffY = dataY - myMeanY; + diffZ = dataZ - myMeanZ; + + diffXX = diffXX.mul(diffXX); + diffYY = diffYY.mul(diffYY); + diffZZ = diffZZ.mul(diffZZ); + + cv::Mat weightX, weightY, weightZ; + cv::exp(diffXX/lambda, weightX); + cv::exp(diffYY/lambda, weightY); + cv::exp(diffZZ/lambda, weightZ); + //cv::Mat wDataX = weightX.mul(selectedDataX); + //cv::Mat wDataY = weightY.mul(selectedDataY); + //cv::Mat wDataZ = weightZ.mul(selectedDataZ); + + weightX.setTo(0., inInds==0); //Again, because the exp and the x - u make invalid values non-zero (inInds) + weightY.setTo(0., inInds==0); + weightZ.setTo(0., inInds==0); + + cv::Mat wDataX = weightX.mul(dataX); + cv::Mat wDataY = weightY.mul(dataY); + cv::Mat wDataZ = weightZ.mul(dataZ); + totalWeightX = cv::sum(weightX)[0]; + totalWeightY = cv::sum(weightY)[0]; + totalWeightZ = cv::sum(weightZ)[0]; + myMeanX = cv::sum(wDataX)[0]/totalWeightX; + myMeanY = cv::sum(wDataY)[0]/totalWeightY; + myMeanZ = cv::sum(wDataZ)[0]/totalWeightZ; + + //cv::add(beenVisitedFlag, cv::Scalar(1.f), beenVisitedFlag, inInds); + beenVisitedFlag.setTo(1, inInds); + //ROS_ERROR_STREAM("myMean = ["<<myMeanX<<", "<<myMeanY<<", "<<myMeanZ<<"]"); + //exit(0); + // if mean doesn't move much stop this cluster + if((myMeanX-myOldMeanX)*(myMeanX-myOldMeanX) + (myMeanY-myOldMeanY)*(myMeanY-myOldMeanY) + (myMeanZ-myOldMeanZ)*(myMeanZ-myOldMeanZ) < stopThresh) + { + //check for merge possibilities + int mergeWith = -1; + double distToOther; + for(int cN = 0; cN<numClust; cN++) //Careful!! cN goes from 1 to numClust!!! + { + double distToOther = (myMeanX - (*clusterCenterX)[cN])*(myMeanX - (*clusterCenterX)[cN]) + (myMeanY - (*clusterCenterY)[cN])*(myMeanY - (*clusterCenterY)[cN]) + (myMeanZ - (*clusterCenterZ)[cN])*(myMeanZ - (*clusterCenterZ)[cN]); //distance from posible new clust max to old clust max + //ROS_ERROR_STREAM("Distance to others "<<distToOther<<" and cN "<<cN); + //ROS_ERROR_STREAM("myMean = ["<<myMeanX<<", "<<myMeanY<<", "<<myMeanZ<<"]"); + //ROS_ERROR_STREAM("clusterCenter = ["<<clusterCenterX[cN]<<", "<<clusterCenterY[cN]<<", "<<clusterCenterZ[cN]<<"]"); + if(distToOther < bandwidth/2) //if its within bandwidth/2 merge new and old + { + mergeWith = cN; + break; + } + } + + if(mergeWith > -1) + { + (*clusterCenterX)[mergeWith] = 0.5*(myMeanX+(*clusterCenterX)[mergeWith]); + (*clusterCenterY)[mergeWith] = 0.5*(myMeanY+(*clusterCenterY)[mergeWith]); + (*clusterCenterZ)[mergeWith] = 0.5*(myMeanZ+(*clusterCenterZ)[mergeWith]); + + cv::Mat newClusterVotes = cv::Mat(clusterVotes.row(mergeWith)) + thisClusterVotes; + newClusterVotes.copyTo(clusterVotes.row(mergeWith)); + } + else + { + numClust = numClust+1; //increment clusters + (*clusterCenterX).push_back(myMeanX); //record the mean + (*clusterCenterY).push_back(myMeanY); + (*clusterCenterZ).push_back(myMeanZ); + + clusterVotes.push_back(thisClusterVotes); // add the new row for the new cluster + } + break; + } + } + + std::vector<int> newInitPtInds; //we can initialize with any of the points not yet visited + for(int i=0; i<numPts; i++) + if (beenVisitedFlag.at<int>(cv::Point(i, 0)) == 0) + newInitPtInds.push_back(i); + initPtInds = newInitPtInds; + numInitPts = initPtInds.size(); //number of active points in set + + //ROS_ERROR_STREAM("Num. of values still un clustered "<<numInitPts); + //ROS_ERROR_STREAM("beenVisitedFlag "<<beenVisitedFlag); + //for(int i=0; i<initPtInds.size(); ++i) + // std::cerr<<initPtInds[i] << ' '; + } + //exit(0); + //[val,data2cluster] = max(clusterVotes,[],1); //a point belongs to the cluster with the most votes + + cv::Mat TclusterVotes = clusterVotes.t(); + + double min, max; + for(int i=0; i<TclusterVotes.rows; i++) + { + cv::minMaxIdx(cv::Mat(TclusterVotes.row(i)), &min, &max); + (*point2Clusters).push_back((int)max); + } + + //ROS_ERROR_STREAM("Inside "<<(*point2Clusters).size()); + //ROS_ERROR_STREAM("Number of clusters " <<numClust<<"and "<<clusterCenterX.size()); +}*/ + + + +void meanshiftCluster_Gaussian(cv::Mat dataPts, std::vector<double> *clusterCenterX, std::vector<double> *clusterCenterY, std::vector<double> *clusterCenterZ, std::vector<int> *point2Clusters, double bandwidth) +{ + //Initialization + int numPts = dataPts.cols; + int numDim = dataPts.rows; + int numClust = 0; + double bandSq = bandwidth*bandwidth; + cv::Range rng = cv::Range(0,numPts-1); + + cv::Mat onesAux = cv::Mat::ones(1, dataPts.cols, dataPts.type()); + + std::vector<int> initPtInds; + for (int i = rng.start; i <= rng.end; i++) initPtInds.push_back(i); + double stopThresh = 1E-3*bandwidth; //when mean has converged + int numInitPts = numPts; //track if a points been seen already + cv::Mat beenVisitedFlag = cv::Mat(1, numPts, CV_32S, cv::Scalar::all(0)); //number of points to posibaly use as initilization points + cv::Mat clusterVotes; //used to resolve conflicts on cluster membership + + double lambda = 10.; + double myMeanX, myMeanY, myMeanZ, myOldMeanX, myOldMeanY, myOldMeanZ, totalWeightX, totalWeightY, totalWeightZ; + int stInd; + + //std::vector<double> clusterCenterX, clusterCenterY, clusterCenterZ; + + + int tempInd; + + while (numInitPts>0) + { + //ROS_ERROR_STREAM("iniPtInds"); + //for(int i=0; i<initPtInds.size(); i++) + // std::cerr<<initPtInds[i]<<" "; + //std::cerr<<std::endl; + + + tempInd = (rand()%numInitPts); //pick a random seed point + //ROS_ERROR_STREAM("numInitPts ="<<numInitPts); + + //std::cin.ignore(); + stInd = initPtInds[tempInd]; //use this point as start of mean + /**********REMOVE THIS******************************/ + //stInd = initPtInds[numInitPts-1]; + /**********REMOVE THIS******************************/ + myMeanX = dataPts.at<double>(cv::Point(stInd, 0)); //intilize mean to this points location + myMeanY = dataPts.at<double>(cv::Point(stInd, 1)); + myMeanZ = dataPts.at<double>(cv::Point(stInd, 2)); + cv::Mat thisClusterVotes = cv::Mat(1, numPts, CV_32S, cv::Scalar::all(0)); //used to resolve conflicts on cluster membership + //ROS_ERROR_STREAM("Before getting into while myMean = ["<<myMeanX<<", "<<myMeanY<<", "<<myMeanZ<<"]"); + cv::Mat_<double> myMean = ( cv::Mat_<double>(3, 1) << myMeanX, myMeanY, myMeanZ); + + + while(true) + { + cv::Mat myDataPts; + dataPts.copyTo(myDataPts); + + //ROS_ERROR_STREAM("Original = "<<diffX); + //ROS_ERROR_STREAM("onesAux.rows = "<<onesAux.rows<<" cols = "<<onesAux.cols); + //ROS_ERROR_STREAM("dataPts.rows = "<<dataPts.rows<<" cols = "<<dataPts.cols); + //ROS_ERROR_STREAM("myMean.rows = "<<myMean.rows<<" cols = "<<myMean.cols); + + cv::Mat diff = myDataPts - myMean*onesAux; + cv::Mat diffdiff = diff.mul(diff); + cv::Mat sqDistToAll = diffdiff.row(0) +diffdiff.row(1) + diffdiff.row(2); //dist squared from mean to all points still active + + cv::Mat inInds = (sqDistToAll < bandSq); // Puts 255 wherever it is true + cv::add(thisClusterVotes, cv::Scalar(1), thisClusterVotes, inInds); //add a vote for all the in points belonging to this cluster + cv::Mat mask3 = cv::repeat(inInds==0, 3, 1); + + //ROS_ERROR_STREAM("BEFORE myMean = "<<myMean); + //ROS_ERROR_STREAM("sqDistToAll = "<<sqDistToAll); + //ROS_ERROR_STREAM("diffX = "<<diff.row(0)); + //ROS_ERROR_STREAM("diffdiffX = "<<diffdiff.row(0)); + //ROS_ERROR_STREAM("thisClusterVotes = "<<thisClusterVotes); + //ROS_ERROR_STREAM("dataPts = "<<dataPts); + + myDataPts.setTo(0, mask3); + double numOfOnes = (double) (cv::sum(inInds)[0])/255; //inInds is active if inInds[i]==255 + + myOldMeanX = myMeanX; myOldMeanY = myMeanY; myOldMeanZ = myMeanZ; + + myMeanX = cv::sum(myDataPts.row(0))[0]/numOfOnes; + myMeanY = cv::sum(myDataPts.row(1))[0]/numOfOnes; + myMeanZ = cv::sum(myDataPts.row(2))[0]/numOfOnes; + + myMean = ( cv::Mat_<double>(3, 1) << myMeanX, myMeanY, myMeanZ); + + //ROS_ERROR_STREAM("MIDDLE myMean = "<<myMean); + + diff = myDataPts - myMean*onesAux; + diffdiff = diff.mul(diff); + + cv::Mat weight; + cv::exp(diffdiff/lambda, weight); + + weight.setTo(0., mask3); //Again, because the exp and the x - u make invalid values non-zero (inInds) + cv::Mat wData = weight.mul(myDataPts); + + totalWeightX = cv::sum(weight.row(0))[0]; + totalWeightY = cv::sum(weight.row(1))[0]; + totalWeightZ = cv::sum(weight.row(2))[0]; + myMeanX = cv::sum(wData.row(0))[0]/totalWeightX; + myMeanY = cv::sum(wData.row(1))[0]/totalWeightY; + myMeanZ = cv::sum(wData.row(2))[0]/totalWeightZ; + + myMean = ( cv::Mat_<double>(3, 1) << myMeanX, myMeanY, myMeanZ); + //ROS_ERROR_STREAM("AFTER: myMean = "<<myMeanX<<","<<myMeanY<<","<<myMeanZ); + //exit(0); + + beenVisitedFlag.setTo(1, inInds); + //ROS_ERROR_STREAM("beenVisitedFlag = "<<beenVisitedFlag); + + // if mean doesn't move much stop this cluster + //ROS_ERROR_STREAM("Norm = "<<(myMeanX-myOldMeanX)*(myMeanX-myOldMeanX) + (myMeanY-myOldMeanY)*(myMeanY-myOldMeanY) + (myMeanZ-myOldMeanZ)*(myMeanZ-myOldMeanZ)); + if((myMeanX-myOldMeanX)*(myMeanX-myOldMeanX) + (myMeanY-myOldMeanY)*(myMeanY-myOldMeanY) + (myMeanZ-myOldMeanZ)*(myMeanZ-myOldMeanZ) < stopThresh*stopThresh) + { + //check for merge posibilities + //ROS_ERROR_STREAM("Dentro!! "); + int mergeWith = -1; + double distToOther; + for(int cN = 0; cN<numClust; cN++) //Careful!! cN goes from 1 to numClust!!! + { + double distToOther = (myMeanX - (*clusterCenterX)[cN])*(myMeanX - (*clusterCenterX)[cN]) + (myMeanY - (*clusterCenterY)[cN])*(myMeanY - (*clusterCenterY)[cN]) + (myMeanZ - (*clusterCenterZ)[cN])*(myMeanZ - (*clusterCenterZ)[cN]); //distance from posible new clust max to old clust max + //ROS_ERROR_STREAM("Dentro!! "); + //ROS_ERROR_STREAM("distToOther " <<distToOther); + if(distToOther < (bandwidth/2)*(bandwidth/2)) //if its within bandwidth/2 merge new and old + { + mergeWith = cN; + break; + } + } + + if(mergeWith > -1) + { + //ROS_ERROR_STREAM("Merging cluster with #"<<mergeWith); + //ROS_ERROR_STREAM("NumClust = "<<numClust); + //exit(0); + + (*clusterCenterX)[mergeWith] = 0.5*(myMeanX+(*clusterCenterX)[mergeWith]); + (*clusterCenterY)[mergeWith] = 0.5*(myMeanY+(*clusterCenterY)[mergeWith]); + (*clusterCenterZ)[mergeWith] = 0.5*(myMeanZ+(*clusterCenterZ)[mergeWith]); + + cv::Mat newClusterVotes = cv::Mat(clusterVotes.row(mergeWith)) + thisClusterVotes; + newClusterVotes.copyTo(clusterVotes.row(mergeWith)); + + //ROS_ERROR_STREAM("clusterVotes = "<<clusterVotes); + //exit(0); + } + else + { + //ROS_ERROR_STREAM("Creating new cluster"); + (*clusterCenterX).push_back(myMeanX); //record the mean + (*clusterCenterY).push_back(myMeanY); + (*clusterCenterZ).push_back(myMeanZ); + numClust = numClust+1; //increment clusters + + clusterVotes.push_back(thisClusterVotes); // add the new row for the new cluster + + //ROS_ERROR_STREAM("clusterVotes = "<<clusterVotes); + //ROS_ERROR_STREAM("ClusterCenterX = "<<(*clusterCenterX)[numClust-1]); + //ROS_ERROR_STREAM("ClusterCenterY = "<<(*clusterCenterY)[numClust-1]); + //ROS_ERROR_STREAM("ClusterCenterZ = "<<(*clusterCenterZ)[numClust-1]); + //exit(0); + } + break; + } + } + + std::vector<int> newInitPtInds; //we can initialize with any of the points not yet visited + for(int i=0; i<numPts; i++) + if (beenVisitedFlag.at<int>(cv::Point(i, 0)) == 0) + newInitPtInds.push_back(i); + initPtInds = newInitPtInds; + numInitPts = initPtInds.size(); //number of active points in set + } + + cv::Mat TclusterVotes = clusterVotes.t(); + cv::Point minLoc; + cv::Point maxLoc; + double min, max; + for(int i=0; i<TclusterVotes.rows; i++) + { + cv::minMaxLoc(cv::Mat(TclusterVotes.row(i)), &min, &max, &minLoc, &maxLoc); + (*point2Clusters).push_back(maxLoc.x); + } + + //ROS_ERROR_STREAM("Number of clusters " <<numClust<<" and "<<(*clusterCenterX).size()); + //ROS_ERROR_STREAM("Tclusters rows " <<TclusterVotes.rows<<" cols "<<TclusterVotes.cols); +} + +void colorMeanShiftFilt(double *newPixelsPtr, const double *pixelsPtr, int mPixels, int maxPixelDim, const double *imagePtr, int numRows, int numCols, float spaceDivider, char *kernelFun, int maxIterNum, float tolFun) +{ + // Copy the original pixel values to the new pixel matrix + memcpy(newPixelsPtr, pixelsPtr, maxPixelDim*mPixels*sizeof(double)); + + // Copy the original image to a new image + double *newImagePtr=new double[mPixels]; + memcpy(newImagePtr, imagePtr, mPixels*sizeof(double)); + + // Help variables + double x,y,val; + int neighX, neighY; + bool bRepeat=false; + + double *delta = (double *) calloc(maxPixelDim,sizeof(double)); + double *dX = (double *) calloc(maxPixelDim,sizeof(double)); + double *neighVec = (double *) calloc(maxPixelDim,sizeof(double)); + double deltaGrad2; + + // Create an array that indicates if we reached the end of the optimization for each pixel + double *optimizedPointArray=new double[mPixels]; + + for (int iterNum=0; iterNum<maxIterNum; iterNum++) + { + bRepeat=false; + + cv::Mat XX=cv::Mat(numRows, numCols, CV_64F, newPixelsPtr); + cv::Mat YY=cv::Mat(numRows, numCols, CV_64F, newPixelsPtr+mPixels); + cv::Mat MM=cv::Mat(numRows, numCols, CV_64F, newPixelsPtr+2*mPixels); + + for (int pointNum=0; pointNum<mPixels; pointNum++) + { + if (optimizedPointArray[pointNum]==1) // The optimization for this pixel has reached the threshold + continue; + + x=newPixelsPtr[_2D_to_linear(0, pointNum, mPixels)]; + y=newPixelsPtr[_2D_to_linear(1, pointNum, mPixels)]; + val=newPixelsPtr[_2D_to_linear(2, pointNum, mPixels)]; + + //ROS_ERROR("y = %d, x = %d, pointNum = %d, spaceDivider = %g", (int)floor(y), (int)floor(x), pointNum, spaceDivider); + double sumF; + + int x_coord = max(0, floor(x*spaceDivider)-1-spaceDivider); + int y_coord = max(0, floor(y*spaceDivider)-1-spaceDivider); + int width = min(2*spaceDivider+1, numCols-x_coord); + int height= min(2*spaceDivider+1, numRows-y_coord); + + cv::Mat submatXX = XX(cv::Rect(x_coord, y_coord, width, height)).clone(); + cv::Mat submatYY = YY(cv::Rect(x_coord, y_coord, width, height)).clone(); + cv::Mat submatMM = MM(cv::Rect(x_coord, y_coord, width, height)).clone(); + + cv::Mat delta_XX, delta_YY, delta_MM, dist2_matrix, kres_matrix; + delta_XX = x - submatXX; + delta_YY = y - submatYY; + delta_MM = val - submatMM; + dist2_matrix = delta_XX.mul(delta_XX) + delta_YY.mul(delta_YY) + delta_MM.mul(delta_MM); + cv::exp(dist2_matrix/(-2.0), kres_matrix); //kres =(exp(-dist2/2.0)); + + dX[0] = cv::sum(kres_matrix.mul(submatXX))[0];//cv::sum() returns a cvScalar, to get the value we need .val(0) + dX[1] = cv::sum(kres_matrix.mul(submatYY))[0]; + dX[2] = cv::sum(kres_matrix.mul(submatMM))[0]; + sumF = cv::sum(kres_matrix)[0]; + + // Find the corresponding row and column of the point + + dX[0]=(sumF==0)?0:dX[0]/sumF; //check for NaNs + dX[1]=(sumF==0)?0:dX[1]/sumF; //check for NaNs + dX[2]=(sumF==0)?0:dX[2]/sumF; //check for NaNs + + double tmp1=newPixelsPtr[_2D_to_linear(0, pointNum, mPixels)]-dX[0]; + double tmp2=newPixelsPtr[_2D_to_linear(1, pointNum, mPixels)]-dX[1]; + double tmp3=newPixelsPtr[_2D_to_linear(2, pointNum, mPixels)]-dX[2]; + + deltaGrad2=tmp1*tmp1+tmp2*tmp2+tmp3*tmp3; + newPixelsPtr[_2D_to_linear(0, pointNum, mPixels)]=dX[0]; + newPixelsPtr[_2D_to_linear(1, pointNum, mPixels)]=dX[1]; + newPixelsPtr[_2D_to_linear(2, pointNum, mPixels)]=dX[2]; + + newImagePtr[pointNum]=dX[2]; + + if (deltaGrad2<tolFun) + optimizedPointArray[pointNum]=1; + else + bRepeat=true; + + } // end pointNum loop + + if (bRepeat==false) + break; + + } // end for(iterNum) loop + + + // CLEAN-UP + delete[] newImagePtr; + delete[] optimizedPointArray; + delete(delta); + delete(dX); + delete(neighVec); +} + + + diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/image_reconst.cpp b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/image_reconst.cpp new file mode 100755 index 0000000000000000000000000000000000000000..1d58720914bc49f62bbc0ce2dc8895e7d4052710 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/image_reconst.cpp @@ -0,0 +1,568 @@ +/* + * image_reconst.cpp + * + * Created on: Nov 3, 2016 + * Author: fran + */ + +/* +######################### Poisson Image Editing ############################ + +Copyright (C) 2012 Siddharth Kherada +Copyright (C) 2006-2012 Natural User Interface Group + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. " + +############################################################################## +*/ + +#include <iostream> +#include <stdio.h> +#include <stdlib.h> +#include <complex> +#include "image_reconstruct/image_reconst.h" +#include <math.h> +#include <omp.h> + +//using namespace std; +//using namespace cv; + +#define pi 3.1416 + +#define ATF at<float> +#define AT3F at<Vec3f> +#define ATB at<uchar> +#define AT3B at<Vec3b> + +/////////////////////Poisson Equation Solver by FFT///////////////////// +void dst(double *btest, double *bfinal, int h, int w) +{ + + unsigned long int idx; + + cv::Mat temp = cv::Mat(2 * h + 2, 1, CV_32F); + cv::Mat res = cv::Mat(h, 1, CV_32F); + + int p = 0; + + for (int i = 0; i<w; i++) + { + temp.ATF(0, 0) = 0.0; + + for (int j = 0, r = 1; j<h; j++, r++) + { + idx = j*w + i; + temp.ATF(r, 0) = (float)btest[idx]; + } + + temp.ATF(h + 1, 0) = 0.0; + + for (int j = h - 1, r = h + 2; j >= 0; j--, r++) + { + idx = j*w + i; + temp.ATF(r, 0) = (float)(-1 * btest[idx]); + } + + cv::Mat planes[] = { cv::Mat_<float>(temp), cv::Mat::zeros(temp.size(), CV_32F) }; + + cv::Mat complex1; + merge(planes, 2, complex1); + + dft(complex1, complex1, 0, 0); + + cv::Mat planes1[] = { cv::Mat::zeros(complex1.size(), CV_32F), cv::Mat::zeros(complex1.size(), CV_32F) }; + + split(complex1, planes1); + + std::complex<double> two_i = std::sqrt(std::complex<double>(-1)); + + double fac = -2 * imag(two_i); + + for (int c = 1, z = 0; c<h + 1; c++, z++) + { + res.ATF(z, 0) = (float)(planes1[1].ATF(c, 0) / fac); + } + + for (int q = 0, z = 0; q<h; q++, z++) + { + idx = q*w + p; + bfinal[idx] = res.ATF(z, 0); + } + p++; + } + +} + +void idst(double *btest, double *bfinal, int h, int w) +{ + int nn = h + 1; + dst(btest, bfinal, h, w); + #pragma omp parallel for + for (int i = 0; i<h; i++) + for (int j = 0; j<w; j++) + { + unsigned long int idx = i*w + j; + bfinal[idx] = (double)(2 * bfinal[idx]) / nn; + } + +} + +void transpose(double *mat, double *mat_t, int h, int w) +{ + + cv::Mat tmp = cv::Mat(h, w, CV_32FC1); + int p = 0; + + #pragma omp parallel for + for (int i = 0; i < h; i++) + { + for (int j = 0; j < w; j++) + { + + unsigned long int idx = i*(w)+j; + tmp.ATF(i, j) = (float)(mat[idx]); + } + } + cv::Mat tmp_t = tmp.t(); + + unsigned long int idx; + for (int i = 0; i < tmp_t.size().height; i++) + for (int j = 0; j<tmp_t.size().width; j++) + { + idx = i*tmp_t.size().width + j; + mat_t[idx] = tmp_t.ATF(i, j); + } + +} + + +void getGradientX(cv::Mat &img, cv::Mat &gx) +{ + int height = img.rows; + int width = img.cols; + cv::Mat cat = repeat(img, 1, 2); + /*cat.col(width) = 0;*/ + cv::Mat roimat = cat(cv::Rect(1, 0, width, height)); + gx = roimat - img; + gx.col(width - 1) = 0; +} + +// calculate vertical gradient, gy(i, j) = img(i, j+1) - img(i, j) +void getGradientY(cv::Mat &img, cv::Mat &gy) +{ + int height = img.rows; + int width = img.cols; + cv::Mat cat = repeat(img, 2, 1); + /*cat.row(height) = 0;*/ + cv::Mat roimat = cat(cv::Rect(0, 1, width, height)); + gy = roimat - img; + gy.row(height - 1) = 0; +} + +// calculate horizontal gradient, gxx(i+1, j) = gx(i+1, j) - gx(i, j) +void lapx(cv::Mat &gx, cv::Mat &gxx) +{ + int height = gx.rows; + int width = gx.cols; + cv::Mat cat = repeat(gx, 1, 2); + /*cat.col(width - 1) = 0;*/ + cv::Mat roi = cat(cv::Rect(width - 1, 0, width, height)); + gxx = gx - roi; + gxx.col(0) = 0; +} + +// calculate vertical gradient, gyy(i, j+1) = gy(i, j+1) - gy(i, j) +void lapy(cv::Mat &gy, cv::Mat &gyy) +{ + int height = gy.rows; + int width = gy.cols; + cv::Mat cat = repeat(gy, 2, 1); + /*cat.row(height - 1) = 0;*/ + cv::Mat roi = cat(cv::Rect(0, height - 1, width, height)); + gyy = gy - roi; + gyy.row(0) = 0; +} + + + +void poisson_solver(cv::Mat &img, cv::Mat &gxx, cv::Mat &gyy, cv::Mat &result){ + + int w = img.cols; + int h = img.rows; + int channel = img.channels(); + + unsigned long int idx; + + cv::Mat lap = gxx + gyy; + + cv::Mat bound(img); + bound(cv::Rect(1, 1, w - 2, h - 2)) = 0; + double *dir_boundary = new double[h*w]; + +#pragma omp parallel for + for (int i = 1; i < h - 1; i++) + for (int j = 1; j < w - 1; j++) + { + unsigned long int idx = i*w + j; + if ( i == 1 || i == h - 2 || j == 1 || j == w - 2 ) + dir_boundary[idx] = (int)bound.ATF(i, (j + 1)) + (int)bound.ATF(i, (j - 1)) + (int)bound.ATF(i - 1, j) + (int)bound.ATF(i + 1, j); + else dir_boundary[idx] = 0; + + } + + + cv::Mat diff(h, w, CV_32FC1); + +#pragma omp parallel for + for (int i = 0; i<h; i++) + { + for (int j = 0; j<w; j++) + { + unsigned long int idx = i*w + j; + diff.ATF(i, j) = (float)(lap.ATF(i, j) - dir_boundary[idx]); + } + } + + double *btemp = new double[(h - 2)*(w - 2)]; +#pragma omp parallel for + for (int i = 0; i < h - 2; i++) + { + for (int j = 0; j < w - 2; j++) + { + unsigned long int idx = i*(w - 2) + j; + btemp[idx] = diff.ATF(i + 1, j + 1); + + } + } + + double *bfinal = new double[(h - 2)*(w - 2)]; + double *bfinal_t = new double[(h - 2)*(w - 2)]; + double *denom = new double[(h - 2)*(w - 2)]; + double *fres = new double[(h - 2)*(w - 2)]; + double *fres_t = new double[(h - 2)*(w - 2)]; + + + dst(btemp, bfinal, h - 2, w - 2); + + transpose(bfinal, bfinal_t, h - 2, w - 2); + + dst(bfinal_t, bfinal, w - 2, h - 2); + + transpose(bfinal, bfinal_t, w - 2, h - 2); + + int cx = 1; + int cy = 1; + + for (int i = 0; i < w - 2; i++, cy++) + { + for (int j = 0, cx = 1; j < h - 2; j++, cx++) + { + idx = j*(w - 2) + i; + denom[idx] = (float)2 * cos(pi*cy / ((double)(w - 1))) - 2 + 2 * cos(pi*cx / ((double)(h - 1))) - 2; + + } + } + + for (idx = 0; (int)idx < (w - 2)*(h - 2); idx++) + { + bfinal_t[idx] = bfinal_t[idx] / denom[idx]; + } + + + idst(bfinal_t, fres, h - 2, w - 2); + + transpose(fres, fres_t, h - 2, w - 2); + + idst(fres_t, fres, w - 2, h - 2); + + transpose(fres, fres_t, w - 2, h - 2); + + + img.convertTo(result, CV_8UC1); + + + cv::Mat tmp = cv::Mat(h-2, w-2, CV_64F, fres_t); + double min, max; + cv::minMaxLoc(tmp, &min, &max); + + + #pragma omp parallel for + for (int i = 0; i < h - 2; i++) + { + for (int j = 0; j < w - 2; j++) + { + unsigned long int idx = i*(w - 2) + j; + //if (fres_t[idx] < 0.0) + // result.ATB(i+1, j+1) = 0; + //else if (fres_t[idx] > 255.0) + // result.ATB(i+1, j+1) = 255; + //else + // result.ATB(i+1, j+1) = (int)fres_t[idx]; + result.ATB(i+1,j+1) = (int) (255*(fres_t[idx]-min)/(max-min)); + } + } + +} + +void poisson_solver_jacobi(cv::Mat &img, cv::Mat &gxx, cv::Mat &gyy, cv::Mat &result) { + int w = img.cols; + int h = img.rows; + int channel = img.channels(); + + unsigned long int idx; + + cv::Mat lap = gxx + gyy; + + cv::Mat bound(img); + bound(cv::Rect(1, 1, w - 2, h - 2)) = 0; + double *dir_boundary = new double[h*w]; + + #pragma omp parallel for + for (int i = 1; i < h - 1; i++) + for (int j = 1; j < w - 1; j++) + { + idx = i*w + j; + if (i == 1 || i == h - 2 || j == 1 || j == w - 2) + dir_boundary[idx] = (int)bound.ATF(i, (j + 1)) + (int)bound.ATF(i, (j - 1)) + (int)bound.ATF(i - 1, j) + (int)bound.ATF(i + 1, j); + else dir_boundary[idx] = 0; + + } + + + cv::Mat diff(h, w, CV_32FC1); + #pragma omp parallel for + for (int i = 0; i < h; i++) + { + for (int j = 0; j < w; j++) + { + idx = i*w + j; + diff.ATF(i, j) = (float)(-lap.ATF(i, j) + dir_boundary[idx]); + } + } + + double *gtest = new double[(h - 2)*(w - 2)]; + #pragma omp parallel for + for (int i = 0; i < h - 2; i++) + { + for (int j = 0; j < w - 2; j++) + { + idx = i*(w - 2) + j; + gtest[idx] = diff.ATF(i + 1, j + 1); + + } + } + //Iteration begins + cv::Mat U = cv::Mat::zeros(img.size(), CV_32FC3); + int k = 0; + while (k <= 3000){ + #pragma omp parallel for + for (int i = 1; i < h - 1; i++) + { + for (int j = 1; j < w - 1; j++) + { + U.ATF(i, j) = (float)((U.ATF(i + 1, j) + U.ATF(i, j + 1) + U.ATF(i - 1, j) + U.ATF(i, j - 1) + diff.ATF(i, j)) / 4.0); + } + } + k++; + } + + img.convertTo(result, CV_8UC1); + #pragma omp parallel for + for (int i = 1; i < h - 1; i++) + { + for (int j = 1; j < w - 1; j++) + { + if (U.ATF(i,j) < 0.0) + result.ATB(i , j ) = 0; + else if (U.ATF(i, j) > 255.0) + result.ATB(i, j) = 255; + else + result.ATB(i , j ) = (int)U.ATF(i, j); + } + } +} + + +//IplImage versions + +void lapx( const IplImage *img, IplImage *gxx) +{ + int w = img->width; + int h = img->height; + int channel = img->nChannels; + + cvZero( gxx ); + for(int i=0;i<h;i++) + for(int j=0;j<w-1;j++) + for(int c=0;c<channel;++c) + { + CV_IMAGE_ELEM(gxx,float,i,(j+1)*channel+c) = + (float)CV_IMAGE_ELEM(img,float,i,(j+1)*channel+c) - (float)CV_IMAGE_ELEM(img,float,i,j*channel+c); + } +} +void lapy( const IplImage *img, IplImage *gyy) +{ + int w = img->width; + int h = img->height; + int channel = img->nChannels; + + cvZero( gyy ); + for(int i=0;i<h-1;i++) + for(int j=0;j<w;j++) + for(int c=0;c<channel;++c) + { + CV_IMAGE_ELEM(gyy,float,i+1,j*channel+c) = + (float)CV_IMAGE_ELEM(img,float,(i+1),j*channel+c) - (float)CV_IMAGE_ELEM(img,float,i,j*channel+c); + + } +} + +void poisson_solver(const IplImage *img, IplImage *gxx , IplImage *gyy, cv::Mat &result) +{ + + int w = img->width; + int h = img->height; + int channel = img->nChannels; + + unsigned long int idx,idx1; + + IplImage *lap = cvCreateImage(cvGetSize(img), 32, 1); + + for(int i =0;i<h;i++) + for(int j=0;j<w;j++) + CV_IMAGE_ELEM(lap,float,i,j)=CV_IMAGE_ELEM(gyy,float,i,j)+CV_IMAGE_ELEM(gxx,float,i,j); + + //cv::Mat bound(img); + cv::Mat bound = cv::cvarrToMat(img); + + for(int i =1;i<h-1;i++) + for(int j=1;j<w-1;j++) + { + bound.at<uchar>(i,j) = 0.0; + } + + double *f_bp = new double[h*w]; + + + for(int i =1;i<h-1;i++) + for(int j=1;j<w-1;j++) + { + idx=i*w + j; + f_bp[idx] = -4*(int)bound.at<uchar>(i,j) + (int)bound.at<uchar>(i,(j+1)) + (int)bound.at<uchar>(i,(j-1)) + + (int)bound.at<uchar>(i-1,j) + (int)bound.at<uchar>(i+1,j); + } + + + cv::Mat diff = cv::Mat(h,w,CV_32FC1); + for(int i =0;i<h;i++) + { + for(int j=0;j<w;j++) + { + idx = i*w+j; + diff.at<float>(i,j) = (CV_IMAGE_ELEM(lap,float,i,j) - f_bp[idx]); + } + } + double *gtest = new double[(h-2)*(w-2)]; + for(int i = 0 ; i < h-2;i++) + { + for(int j = 0 ; j < w-2; j++) + { + idx = i*(w-2) + j; + gtest[idx] = diff.at<float>(i+1,j+1); + + } + } + ///////////////////////////////////////////////////// Find DST ///////////////////////////////////////////////////// + + double *gfinal = new double[(h-2)*(w-2)]; + double *gfinal_t = new double[(h-2)*(w-2)]; + double *denom = new double[(h-2)*(w-2)]; + double *f3 = new double[(h-2)*(w-2)]; + double *f3_t = new double[(h-2)*(w-2)]; + double *img_d = new double[(h)*(w)]; + + dst(gtest,gfinal,h-2,w-2); + + transpose(gfinal,gfinal_t,h-2,w-2); + + dst(gfinal_t,gfinal,w-2,h-2); + + transpose(gfinal,gfinal_t,w-2,h-2); + + int cx=1; + int cy=1; + + for(int i = 0 ; i < w-2;i++,cy++) + { + for(int j = 0,cx = 1; j < h-2; j++,cx++) + { + idx = j*(w-2) + i; + denom[idx] = (float) 2*cos(pi*cy/( (double) (w-1))) - 2 + 2*cos(pi*cx/((double) (h-1))) - 2; + + } + } + + for(idx = 0 ; idx < (w-2)*(h-2) ;idx++) + { + gfinal_t[idx] = gfinal_t[idx]/denom[idx]; + } + + + idst(gfinal_t,f3,h-2,w-2); + + transpose(f3,f3_t,h-2,w-2); + + idst(f3_t,f3,w-2,h-2); + + transpose(f3,f3_t,w-2,h-2); + + for(int i = 0 ; i < h;i++) + { + for(int j = 0 ; j < w; j++) + { + idx = i*w + j; + img_d[idx] = (double)CV_IMAGE_ELEM(img,uchar,i,j); + } + } + for(int i = 1 ; i < h-1;i++) + { + for(int j = 1 ; j < w-1; j++) + { + idx = i*w + j; + img_d[idx] = 0.0; + } + } + int id1,id2; + for(int i = 1,id1=0 ; i < h-1;i++,id1++) + { + for(int j = 1,id2=0 ; j < w-1; j++,id2++) + { + idx = i*w + j; + idx1= id1*(w-2) + id2; + img_d[idx] = f3_t[idx1]; + } + } + + for(int i = 0 ; i < h;i++) + { + for(int j = 0 ; j < w; j++) + { + idx = i*w + j; + if(img_d[idx] < 0.0) + result.at<uchar>(i,j) = 0; + else if(img_d[idx] > 255.0) + result.at<uchar>(i,j) = 255.0; + else + result.at<uchar>(i,j) = img_d[idx]; + } + } + +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/meanshift.cpp b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/meanshift.cpp new file mode 100755 index 0000000000000000000000000000000000000000..5eca46e166d382493bda860047c9154564fe669f --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/meanshift.cpp @@ -0,0 +1,1684 @@ + +#include "dvs_meanshift/meanshift.h" +#include "dvs_meanshift/color_meanshift.h" +#include "image_reconstruct/image_reconst.h" +#include <time.h> + +//remove this include, just for debugging +#include "graph3d/pnmfile.h" +#include <std_msgs/Float32.h> + +#define min(x,y) ((x)<(y)?(x):(y)) + +double epsilon = 1E-10; +int counterGlobal = 0; +const int MAX_DISTANCE = 500; //actually the real distance is sqrt(MAX_DISTANCE) + +bool firstevent = true; +double firsttimestamp; + +//std::ofstream foutX("/home/fran/0_tmp/kk/shapes_rotation_totalevents.txt"); +//std::ofstream fouttotalproc("/home/fran/0_tmp/kk/shapes_rotation_totalprocessing_f.txt"); +//std::ofstream foutX("/home/fran/0_tmp/kk/shapes_rotation_2_totalevents.txt"); +//std::ofstream fouttotalproc("/home/fran/0_tmp/kk/shapes_rotation_2_totalprocessing_f.txt"); +//std::ofstream foutX("/home/fran/0_tmp/kk/shapes_translation_totalevents.txt"); +//std::ofstream fouttotalproc("/home/fran/0_tmp/kk/shapes_translation_totalprocessing_f.txt"); +//std::ofstream foutX("/home/fran/0_tmp/kk/shapes_translation_2_totalevents.txt"); +//std::ofstream fouttotalproc("/home/fran/0_tmp/kk/shapes_translation_2_totalprocessing_f.txt"); +//std::ofstream foutX("/home/fran/0_tmp/kk/shapes_6dof_totalevents.txt"); +//std::ofstream fouttotalproc("/home/fran/0_tmp/kk/shapes_6dof_totalprocessing_f.txt"); +//std::ofstream foutX("/home/fran/0_tmp/kk/shapes_6dof_2_totalevents.txt"); +//std::ofstream fouttotalproc("/home/fran/0_tmp/kk/shapes_6dof_2_totalprocessing_f.txt"); +//std::ofstream foutnumclust("/home/fran/0_tmp/kk/shapes_translation_numclusters_f.txt"); +//std::ofstream foutnumclust("/home/fran/0_tmp/kk/shapes_rotation_numclusters_f.txt"); +//std::ofstream foutnumclust("/home/fran/0_tmp/kk/shapes_6dof_numclusters_f.txt"); +//std::ofstream foutnumclust("/home/fran/0_tmp/kk/shapes_6dof_2_numclusters_f.txt"); +//std::ofstream foutnumclust("/home/fran/0_tmp/kk/shapes_rotation_2_numclusters_f.txt"); +//std::ofstream foutnumclust("/home/fran/0_tmp/kk/shapes_translation_2_numclusters_f.txt"); + +#if KALMAN_FILTERING +std::ofstream foutX("/home/fran/0_tmp/kk/traj.txt"); +//std::ofstream foutX_estim("/home/fran/0_tmp/kk/filt_traj.txt"); +//std::ofstream foutT("/home/fran/0_tmp/kk/timetraj.txt"); +#endif + +namespace dvs_meanshift { + +void writeMatToFile(cv::Mat & m, const char* filename) +{ + std::ofstream fout(filename); + + if(!fout) + { + std::cout<<"File Not Opened"<<std::endl; return; + } + + for(int i=0; i<m.rows; i++) + { + for(int j=0; j<m.cols; j++) + { + fout<<m.at<double>(i,j)<<"\t"; + } + fout<<std::endl; + } + fout.close(); +} + +class Parallel_pixel_cosine: public cv::ParallelLoopBody +{ + +public: + + Parallel_pixel_cosine(cv::Mat imgg) : img(imgg) + { + } + + void operator() (const cv::Range &r) const + { + for(int j=r.start; j<r.end; ++j) + { + double* current = const_cast<double*>(img.ptr<double>(j)); + double* last = current + img.cols; + + for (; current != last; ++current) + *current = (double) cos((double) *current); + } + } + +private: + cv::Mat img; +}; + +void meshgrid(const cv::Mat &xgv, const cv::Mat &ygv, cv::Mat1i &X, cv::Mat1i &Y) +{ + cv::repeat(xgv.reshape(1,1), ygv.total(), 1, X); + cv::repeat(ygv.reshape(1,1).t(), 1, xgv.total(), Y); +} + +// helper function (maybe that goes somehow easier) + void meshgridTest(const cv::Range &xgv, const cv::Range &ygv, cv::Mat1i &X, cv::Mat1i &Y) +{ + std::vector<int> t_x, t_y; + for (int i = xgv.start; i <= xgv.end; i++) t_x.push_back(i); + for (int i = ygv.start; i <= ygv.end; i++) t_y.push_back(i); + + meshgrid(cv::Mat(t_x), cv::Mat(t_y), X, Y); +} + +void findClosestCluster(std::vector<int> * clustID, std::vector<double> clustX, std::vector<double> clustY,std::vector<double>clustX_old,std::vector<double> clustY_old) +{ + + //for (int i = 0; i<clustX.size(); i++) + // ROS_ERROR_STREAM("ClusterCenter("<<i+1<<",:)=["<<clustX.at(i)<<","<<clustY.at(i)<<"];"); + //for (int i = 0; i<clustX_old.size(); i++) + // ROS_ERROR_STREAM("prev_ClusterCenter("<<i+1<<",:)=["<<clustX_old.at(i)<<","<<clustY_old.at(i)<<"];"); + + std::vector<double> distvalue (clustX.size()); + double tmp; + + for(int i=0; i<clustX.size(); i++) + { + distvalue[i]=MAX_DISTANCE; + (*clustID).push_back(-1); //no match when clustID is -1 + for(int j=0; j<clustX_old.size(); j++) + { + tmp= (clustX[i]*DVSW-clustX_old[j]*DVSW)*(clustX[i]*DVSW-clustX_old[j]*DVSW) + (clustY[i]*DVSH-clustY_old[j]*DVSH)*(clustY[i]*DVSH-clustY_old[j]*DVSH); + if(tmp<distvalue[i]) + { + distvalue[i]=tmp; + (*clustID)[i]=j; + //ROS_ERROR_STREAM("Cluster "<<i+1<<" same than old cluster "<<j+1<<". Distance = "<<distvalue[i]); + } + } + } +} + +/* +void Meanshift::assignClusterColor(std::vector<int> * positionClusterColor, int numClusters, std::vector<int> matches, std::vector<int> oldPositions) +{ + + if (matches.size()==0) //no mask --> assing all new colors + { + for(int i=0; i<numClusters; i++) + { + (*positionClusterColor).push_back(lastPositionClusterColor); + lastPositionClusterColor = lastPositionClusterColor+1; + } + } + else + { + for(int i=0; i<numClusters; i++) + { + if(matches[i]==-1) //No match with previous clusters --> new cluster, new color + { + (*positionClusterColor).push_back(lastPositionClusterColor); + lastPositionClusterColor = lastPositionClusterColor+1; + } + else //Match with previous cluster --> assign color of previous cluster + { + (*positionClusterColor).push_back(oldPositions[matches[i]]); + } + } + + } +}*/ + +void Meanshift::assignClusterColor(std::vector<int> * positionClusterColor, int numClusters, std::vector<int> matches, std::vector<int> oldPositions, std::vector<int> activeTrajectories) +{ + + if (matches.size()==0) //no mask --> assing all new colors + { + for(int i=0; i<numClusters; i++) + { + (*positionClusterColor).push_back(lastPositionClusterColor); + lastPositionClusterColor = lastPositionClusterColor+1; + } + } + else + { + for(int i=0; i<numClusters; i++) + { + if(matches[i]==-1) //No match with previous clusters --> new cluster, new color + { + //Check if the new position is being used + while ( activeTrajectories.end() != find (activeTrajectories.begin(), activeTrajectories.end(), (lastPositionClusterColor & (MAX_NUM_CLUSTERS-1)))) + lastPositionClusterColor++; + + (*positionClusterColor).push_back(lastPositionClusterColor); + lastPositionClusterColor = lastPositionClusterColor+1; + } + else //Match with previous cluster --> assign color of previous cluster + { + (*positionClusterColor).push_back(oldPositions[matches[i]]); + } + } + + } +} + +Meanshift::Meanshift(ros::NodeHandle & nh, ros::NodeHandle nh_private) : nh_(nh) +{ + used_last_image_ = false; + + // get parameters of display method + std::string display_method_str; + nh_private.param<std::string>("display_method", display_method_str, ""); + display_method_ = (display_method_str == std::string("grayscale")) ? GRAYSCALE : RED_BLUE; + + // setup subscribers and publishers + //event_sub_ = nh_.subscribe("events", 1, &Meanshift::eventsCallback, this); + + //Change this back to events + event_sub_ = nh_.subscribe("/AADC_AudiTT/camera_front/events", 1, &Meanshift::eventsCallback_simple, this); + camera_info_sub_ = nh_.subscribe("camera_info", 1, &Meanshift::cameraInfoCallback, this); + + image_transport::ImageTransport it_(nh_); + + image_pub_ = it_.advertise("dvs_rendering", 1); + image_segmentation_pub_ = it_.advertise("dvs_segmentation", 1); + + dirUB = nh_.advertise<std_msgs::String>("/obj_det", 1); + + + for(int i = 0; i < MAX_NUM_CLUSTERS ; ++i) + { + preVariance[i] = 0; + counterFor[i] = 0; + counterBar[i] = 0; + } + + //DEBUGGING + image_debug1_pub_ = it_.advertise("dvs_debug1", 1); + image_debug2_pub_ = it_.advertise("dvs_debug2", 1); + //NOT DEBUGGING + + //Initializing params for color meanshift + //computing image calibration + numRows=DVSH; numCols=DVSW; + mPixels=numRows*numCols; maxPixelDim=3; + + spaceDivider=7; timeDivider = 5; + maxIterNum=1; + tolFun=0.0001; + kernelFun[0] = 'g'; kernelFun[1] = 'a'; kernelFun[2] = 'u'; kernelFun[3] = 's'; + kernelFun[4] = 's'; kernelFun[5] = 'i'; kernelFun[6] = 'a'; kernelFun[7] = 'n'; + + //Initialize pointers + outputFeatures = NULL; imagePtr = NULL; pixelsPtr = NULL; + initializationPtr=new double[maxPixelDim*mPixels/4]; + + /*for (int i = 0; i < numRows; i++) + for (int j = 0; j < numCols; j++) + { + initializationPtr[i*numCols + j]=j/spaceDivider;//x + initializationPtr[mPixels+i*numCols+j]=i/spaceDivider;//y + initializationPtr[2*mPixels+i*numCols+j]=0;//img + + }*/ + + //Write xposition, yposition, and image in the same vector + for (int i = 0; i < numRows/2; i++) + for (int j = 0; j < numCols/2; j++) + { + initializationPtr[i*numCols/2 + j]=j/spaceDivider;//x + initializationPtr[mPixels/4+i*numCols/2+j]=i/spaceDivider;//y + initializationPtr[2*mPixels/4+i*numCols/2+j]=0;//img + + } + + //Initialize params for graph3d segmentation + sigma = 0.75; + k = 500; + min_region = (int)(numRows*numCols*0.01); + num_components = 0; + + //Initialization of random seed + srand(time(NULL));//Random seed initialization + //asm("rdtsc\n" + // "mov edi, eax\n" + // "call srand"); + + //Create first the RGB values and then assign it to the clusters + //(hopefully, this will keep some stability in the cluster colors along consecutive frames) + //Assume we won't have more than 50 clusters + for(int i=0; i<MAX_NUM_CLUSTERS; i++) + { + RGBColors.push_back(cv::Vec3b((uchar)random(), (uchar)random(), (uchar)random())); + //counterTrajectories.push_back(0);//initialize counter trajectories to all zeros + } + counterTrajectories=std::vector<int>(MAX_NUM_CLUSTERS,0); + + lastPositionClusterColor=0; + + //Initializing trajectories + trajectories = cv::Mat(numRows, numCols, CV_8UC3); + trajectories = cv::Scalar(128,128,128); + + //Initialize trajectory matrix + allTrajectories = new std::vector<cv::Point>[MAX_NUM_CLUSTERS]; + for(int i = 0; i < MAX_NUM_CLUSTERS; i++) + { + allTrajectories[i] = std::vector<cv::Point>(MAX_NUM_TRAJECTORY_POINTS); + } + prev_activeTrajectories=std::vector<int>(MAX_NUM_CLUSTERS,0); + + //Init Kalman filter + //createKalmanFilter(); + vector_of_kf=std::vector<cv::KalmanFilter>(MAX_NUM_CLUSTERS); + vector_of_meas=std::vector<cv::Mat>(MAX_NUM_CLUSTERS); + vector_of_state=std::vector<cv::Mat>(MAX_NUM_CLUSTERS); + for(int i=0; i<vector_of_kf.size(); i++) + createKalmanFilter(&(vector_of_kf[i]), &(vector_of_state[i]), &(vector_of_meas[i])); + + selectedTrajectory = -1; + //foundBlobs = false; + vector_of_foundBlobs=std::vector<bool>(MAX_NUM_CLUSTERS,false); + notFoundBlobsCount = 0; + //foundTrajectory=false; + foundTrajectory = std::vector<bool>(MAX_NUM_CLUSTERS, false); + //ticks = 0; + vector_of_ticks=std::vector<double> (MAX_NUM_CLUSTERS,0.0); + + //BG activity filtering + BGAFframe = cv::Mat(numRows, numCols, CV_32FC1); + BGAFframe =cv::Scalar(0.); +} + +Meanshift::~Meanshift() +{ + delete [] initializationPtr; + +#if KALMAN_FILTERING + foutX.close(); + //foutX_estim.close(); + //foutT.close(); +#endif + image_pub_.shutdown(); + image_segmentation_pub_.shutdown(); + image_debug1_pub_.shutdown(); +} + +void Meanshift::cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg) +{ + camera_matrix_ = cv::Mat(3, 3, CV_64F); + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + camera_matrix_.at<double>(cv::Point(i, j)) = msg->K[i+j*3]; + + dist_coeffs_ = cv::Mat(msg->D.size(), 1, CV_64F); + for (int i = 0; i < msg->D.size(); i++) + dist_coeffs_.at<double>(i) = msg->D[i]; +} + + +void Meanshift::createKalmanFilter(cv::KalmanFilter *kf, cv::Mat *state, cv::Mat *meas) +{ + /* + * This implementation takes 6 states position, velocity, and dimensions of the bounding box + int stateSize = 6; + int measSize = 4; + int contrSize = 0; + + unsigned int type = CV_32F; + + //cv::KalmanFilter kf(stateSize, measSize, contrSize, type); + kf.init(stateSize, measSize, contrSize, type); + + cv::Mat state(stateSize, 1, type); // [x,y,v_x,v_y,w,h] + cv::Mat meas(measSize, 1, type); // [z_x,z_y,z_w,z_h] + // [E_x,E_y,E_v_x,E_v_y,E_w,E_h] + + // Transition State Matrix A + // Note: set dT at each processing step! + // [ 1 0 dT 0 0 0 ] + // [ 0 1 0 dT 0 0 ] + // [ 0 0 1 0 0 0 ] + // [ 0 0 0 1 0 0 ] + // [ 0 0 0 0 1 0 ] + // [ 0 0 0 0 0 1 ] + cv::setIdentity(kf.transitionMatrix); + + // Measure Matrix H + // [ 1 0 0 0 0 0 ] + // [ 0 1 0 0 0 0 ] + // [ 0 0 0 0 1 0 ] + // [ 0 0 0 0 0 1 ] + kf.measurementMatrix = cv::Mat::zeros(measSize, stateSize, type); + kf.measurementMatrix.at<float>(0) = 1.0f; + kf.measurementMatrix.at<float>(7) = 1.0f; + kf.measurementMatrix.at<float>(16) = 1.0f; + kf.measurementMatrix.at<float>(23) = 1.0f; + + // Process Noise Covariance Matrix Q + // [ Ex 0 0 0 0 0 ] + // [ 0 Ey 0 0 0 0 ] + // [ 0 0 Ev_x 0 0 0 ] + // [ 0 0 0 Ev_y 0 0 ] + // [ 0 0 0 0 Ew 0 ] + // [ 0 0 0 0 0 Eh ] + //cv::setIdentity(kf.processNoiseCov, cv::Scalar(1e-2)); + kf.processNoiseCov.at<float>(0) = 1e-2; + kf.processNoiseCov.at<float>(7) = 1e-2; + kf.processNoiseCov.at<float>(14) = 5.0f; + kf.processNoiseCov.at<float>(21) = 5.0f; + kf.processNoiseCov.at<float>(28) = 1e-2; + kf.processNoiseCov.at<float>(35) = 1e-2; + + // Measures Noise Covariance Matrix R + cv::setIdentity(kf.measurementNoiseCov, cv::Scalar(1e-1)); + */ + + + /* + * This implementation is only for one kalman filter + */ + int stateSize = 4; + int measSize = 2; + int contrSize = 0; + + unsigned int type = CV_32F; + + kf->init(stateSize, measSize, contrSize, type); + + //cv::Mat state(stateSize, 1, type); // [x,y,v_x,v_y] + //cv::Mat meas(measSize, 1, type); // [z_x,z_y] + state->create(stateSize,1,type); + meas->create(measSize,1,type); + // [E_x,E_y,E_v_x,E_v_y] + + + // Transition State Matrix A + // Note: set dT at each processing step! + // [ 1 0 dT 0 ] + // [ 0 1 0 dT] + // [ 0 0 1 0 ] + // [ 0 0 0 1 ] + cv::setIdentity(kf->transitionMatrix); + + // Measure Matrix H + // [ 1 0 0 0 ] + // [ 0 1 0 0 ] + kf->measurementMatrix = cv::Mat::zeros(measSize, stateSize, type); + kf->measurementMatrix.at<float>(0) = 1.0f; + kf->measurementMatrix.at<float>(5) = 1.0f; + + // Process Noise Covariance Matrix Q + // [ Ex 0 0 0 ] + // [ 0 Ey 0 0 ] + // [ 0 0 Ev_x 0 ] + // [ 0 0 0 Ev_y ] + //cv::setIdentity(kf.processNoiseCov, cv::Scalar(1e-2)); + kf->processNoiseCov.at<float>(0) = 1e-2;//original values + kf->processNoiseCov.at<float>(5) = 1e-2; + kf->processNoiseCov.at<float>(10) = 5.0f; + kf->processNoiseCov.at<float>(15) = 5.0f; + + kf->processNoiseCov.at<float>(0) = 1e-2; + kf->processNoiseCov.at<float>(5) = 1e-2; + kf->processNoiseCov.at<float>(10) = 7.0f; + kf->processNoiseCov.at<float>(15) = 7.0f; + + // Measures Noise Covariance Matrix R + cv::setIdentity(kf->measurementNoiseCov, cv::Scalar(1e-1)); +} + + + +void Meanshift::eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg) +{ + // only create image if at least one subscriber + if (image_pub_.getNumSubscribers() > 0) + { + cv_bridge::CvImage cv_image; + + if (display_method_ == RED_BLUE) + { + cv_image.encoding = "bgr8"; + + if (last_image_.rows == msg->height && last_image_.cols == msg->width) + { + last_image_.copyTo(cv_image.image); + used_last_image_ = true; + } + else + { + cv_image.image = cv::Mat(msg->height, msg->width, CV_8UC3); + cv_image.image = cv::Scalar(128, 128, 128); + } + + for (int i = 0; i < msg->events.size(); ++i) + { + const int x = msg->events[i].x; + const int y = msg->events[i].y; + + cv_image.image.at<cv::Vec3b>(cv::Point(x, y)) = ( + msg->events[i].polarity == true ? cv::Vec3b(255, 0, 0) : cv::Vec3b(0, 0, 255)); + } + } + else + { + cv_image.encoding = "mono8"; + cv_image.image = cv::Mat(msg->height, msg->width, CV_8U); + cv_image.image = cv::Scalar(128); + + cv::Mat on_events = cv::Mat(msg->height, msg->width, CV_8U); + on_events = cv::Scalar(0); + + cv::Mat off_events = cv::Mat(msg->height, msg->width, CV_8U); + off_events = cv::Scalar(0); + + // count events per pixels with polarity + for (int i = 0; i < msg->events.size(); ++i) + { + const int x = msg->events[i].x; + const int y = msg->events[i].y; + + if (msg->events[i].polarity == 1) + on_events.at<uint8_t>(cv::Point(x, y))++; + else + off_events.at<uint8_t>(cv::Point(x, y))++; + } + + // scale image + cv::normalize(on_events, on_events, 0, 128, cv::NORM_MINMAX, CV_8UC1); + cv::normalize(off_events, off_events, 0, 127, cv::NORM_MINMAX, CV_8UC1); + + cv_image.image += on_events; + cv_image.image -= off_events; + } + image_pub_.publish(cv_image.toImageMsg()); + } + + if (image_segmentation_pub_.getNumSubscribers() > 0) + { + cv_bridge::CvImage cv_segments; + //cv_bridge::CvImage cv_debug1; + //cv_bridge::CvImage cv_debug2; + + + //Doing color meanshift + cv::Mat timestamp_matrix = cv::Mat(numRows, numCols, CV_64F); + timestamp_matrix = cv::Scalar(0.); + cv::Mat timestamp_matrix_out; + + //Reading time of first event and saving it + //ROS_ERROR("Reading values from timestamp_matrix_out: %d", (int) msg->events.size()); + uint64_t first_timestamp = msg->events[0].ts.toNSec(); + // count events per pixels with polarity + for (int i = 0; i < msg->events.size(); ++i) + { + const int x = msg->events[i].x; + const int y = msg->events[i].y; + + double event_timestamp = (0.001*(double)(msg->events[i].ts.toNSec()-first_timestamp));//now in usecs + + if (msg->events[i].polarity == 1) + timestamp_matrix.at<double>(cv::Point(x, y))= event_timestamp*0.001; + else + timestamp_matrix.at<double>(cv::Point(x, y))=-event_timestamp*0.001; + } + + //First create the interface and + //cv_debug1.image =result_image; + //cv::Mat img_hist_equalized; + //cv::equalizeHist(result_image, img_hist_equalized); //equalize the histogram + //cv_debug1.image = img_hist_equalized; + + + + //POISSON SOLVER IMPLEMENTATION + /*----------------------------------------------------------------------------* / + cv::Mat debugIm1= cv::Mat(numRows, numCols, CV_8UC1); + + double min, max; + cv::minMaxLoc(timestamp_matrix, &min, &max); + + cv::normalize(timestamp_matrix, debugIm1, 0, 255, cv::NORM_MINMAX, CV_8UC1); + + // Here, we are assuming that our image is a gradient already (gx and gy) + cv::Mat gxx = cv::Mat(numRows, numCols, CV_64F); + cv::Mat gyy = cv::Mat(numRows, numCols, CV_64F); + + cv::Mat tmp; + debugIm1.convertTo(tmp, CV_64F); + tmp = (tmp/255)*(max-min)+min; + + + lapx(tmp, gxx); + lapy(tmp, gyy); + + cv::Mat lapfnc = cv::Mat(numRows, numCols, CV_64F); + cv::Mat fcos = cv::Mat(numRows, numCols, CV_64F); + cv::Mat denom = cv::Mat(numRows, numCols, CV_64F); + cv::Mat output = cv::Mat(numRows, numCols, CV_64F); + lapfnc = gxx+gyy; + cv::dct(lapfnc, fcos); + + cv::Mat1i XX, YY; + cv::Mat XX_bis, YY_bis; + meshgridTest(cv::Range(0,numCols-1), cv::Range(0, numRows-1), XX, YY); + + XX.convertTo(XX_bis, CV_64F); YY.convertTo(YY_bis, CV_64F); + XX_bis = CV_PI*XX_bis/numCols; YY_bis=CV_PI*YY_bis/numRows; + + parallel_for_(cv::Range(0,XX_bis.rows), Parallel_pixel_cosine(XX_bis)); + parallel_for_(cv::Range(0,YY_bis.rows), Parallel_pixel_cosine(YY_bis)); + + denom = 2*XX_bis-2 + 2*YY_bis-2; + + double first_elem =fcos.at<double>(cv::Point(0, 0)); + cv::divide(fcos, denom, fcos); + fcos.at<double>(cv::Point(0, 0))=first_elem; // remove the Inf + + cv::idct(fcos, output); + double min_bis, max_bis; + cv::minMaxLoc(output, &min_bis, &max_bis); + output = output - min_bis; + cv::Mat result_image, output_norm; + cv::normalize(output, output_norm, 0, 255, cv::NORM_MINMAX, CV_8UC1); + + cv::medianBlur(output_norm,result_image,3); + /*----------------------------------------------------------------------------*/ + //Alternatively, one can use only the time + cv::Mat result_image; + cv::normalize(timestamp_matrix, result_image, 0, 255, cv::NORM_MINMAX, CV_8UC1); + /*----------------------------------------------------------------------------*/ + + //cv::Mat kk1; + //result_image.convertTo(kk1, CV_64F); + //writeMatToFile(kk1,"/home/fran/kk1.txt"); + + //cv_debug1.encoding = "mono8"; + //cv_debug1.image = result_image; + //image_debug1_pub_.publish(cv_debug1.toImageMsg()); + + cv::Mat temporalIm, temporalImSubsampled; + result_image.convertTo(temporalIm, CV_64F); + temporalIm = temporalIm/timeDivider; + cv::pyrDown(temporalIm, temporalImSubsampled, cv::Size(numCols/2, numRows/2)); + //Initialize positions (only once) + outputFeatures=new double[maxPixelDim*mPixels/4]; + pixelsPtr=new double[maxPixelDim*mPixels/4]; + imagePtr=new double[mPixels/4]; + + memcpy(pixelsPtr, initializationPtr, maxPixelDim*mPixels/4*sizeof(double)); //copy event the 0s image + memcpy(pixelsPtr+2*mPixels/4, (void*)(temporalImSubsampled.data), mPixels/4*sizeof(double)); //copy event the 0s image + memcpy(imagePtr, (void*)(temporalImSubsampled.data), mPixels/4*sizeof(double)); //copy event the 0s image + + //extract the features + colorMeanShiftFilt(outputFeatures, pixelsPtr, mPixels/4, maxPixelDim, imagePtr, \ + numRows/2, numCols/2, spaceDivider, kernelFun, maxIterNum, tolFun); + + cv::Mat meanshiftIm; + cv::Mat meanshiftIm_small = cv::Mat(numRows/2, numCols/2, CV_64F, outputFeatures+2*mPixels/4); + meanshiftIm_small = meanshiftIm_small*timeDivider; + cv::pyrUp(meanshiftIm_small, meanshiftIm, cv::Size(numCols, numRows)); + + //cv::Mat kk = cv::Mat(numRows, numCols, CV_64F, imagePtr); + //kk = kk*timeDivider; + //writeMatToFile(kk,"/home/fran/imagePtr.txt"); + + //do the segmentation +// cv::Mat filtered_im = cv::Mat(numRows, numCols, CV_64F, outputFeatures+2*mPixels); +// filtered_im = filtered_im*timeDivider; + cv::Mat meanshiftImNormalized; + //meanshift_im.convertTo(meanshift_im_normalized,CV_8UC1); + cv::normalize(meanshiftIm, meanshiftImNormalized, 0, 255, cv::NORM_MINMAX, CV_8UC1); + + //cv_debug2.encoding = "mono8"; + //cv_debug2.image = meanshift_im_normalized; + //image_debug2_pub_.publish(cv_debug2.toImageMsg()); + + //meanshift_im_normalized= cv::imread("/home/fran/mug.pgm", CV_LOAD_IMAGE_GRAYSCALE); + + //tmp has the values that are non-zero + + cv_segments.encoding = "bgr8"; + cv_segments.image =cv::Mat(numRows, numCols, CV_8UC3); + //cv_segments.image =cv::Mat(128, 128, CV_8UC3); + im2segment(meanshiftImNormalized, cv_segments.image, sigma, k, min_region, &num_components); + image_segmentation_pub_.publish(cv_segments.toImageMsg()); + + //cv::Mat mask; + //mask = (abs(timestamp_matrix) > 1E-6); // Puts 255 wherever it is true + //mask.convertTo(mask, CV_64F); + //writeMatToFile(mask,"/home/fran/mask.txt"); + + //cv::Mat finalOutput; + //cv::bitwise_and(cv_segments.image, cv::Scalar(255,255,255), finalOutput, mask); + //cv_segments.image = finalOutput; + //image_debug2_pub_.publish(cv_segments.toImageMsg()); + //------------------------------------------------------------------------------------- + + delete[] imagePtr; + delete [] outputFeatures; + delete [] pixelsPtr; + } +} + +/*void Meanshift::eventsCallback_simple(const dvs_msgs::EventArray::ConstPtr& msg) +{ + // only create image if at least one subscriber + if (image_pub_.getNumSubscribers() > 0) + { + cv_bridge::CvImage cv_image; + + if (display_method_ == RED_BLUE) + { + cv_image.encoding = "bgr8"; + + if (last_image_.rows == msg->height && last_image_.cols == msg->width) + { + last_image_.copyTo(cv_image.image); + used_last_image_ = true; + } + else + { + cv_image.image = cv::Mat(msg->height, msg->width, CV_8UC3); + cv_image.image = cv::Scalar(128, 128, 128); + } + + for (int i = 0; i < msg->events.size(); ++i) + { + const int x = msg->events[i].x; + const int y = msg->events[i].y; + + cv_image.image.at<cv::Vec3b>(cv::Point(x, y)) = ( + msg->events[i].polarity == true ? cv::Vec3b(255, 0, 0) : cv::Vec3b(0, 0, 255)); + } + } + else + { + cv_image.encoding = "mono8"; + cv_image.image = cv::Mat(msg->height, msg->width, CV_8U); + cv_image.image = cv::Scalar(128); + + cv::Mat on_events = cv::Mat(msg->height, msg->width, CV_8U); + on_events = cv::Scalar(0); + + cv::Mat off_events = cv::Mat(msg->height, msg->width, CV_8U); + off_events = cv::Scalar(0); + + // count events per pixels with polarity + for (int i = 0; i < msg->events.size(); ++i) + { + const int x = msg->events[i].x; + const int y = msg->events[i].y; + + if (msg->events[i].polarity == 1) + on_events.at<uint8_t>(cv::Point(x, y))++; + else + off_events.at<uint8_t>(cv::Point(x, y))++; + } + + // scale image + cv::normalize(on_events, on_events, 0, 128, cv::NORM_MINMAX, CV_8UC1); + cv::normalize(off_events, off_events, 0, 127, cv::NORM_MINMAX, CV_8UC1); + + cv_image.image += on_events; + cv_image.image -= off_events; + } + image_pub_.publish(cv_image.toImageMsg()); + } + + if (image_segmentation_pub_.getNumSubscribers() > 0) + { + cv_bridge::CvImage cv_segments; + + //Doing color meanshift + cv::Mat diffTimestampMatrix = cv::Mat(numRows, numCols, CV_64F, cv::Scalar::all(0.)); + cv::Mat timestampMatrix = cv::Mat(numRows, numCols, CV_64F, cv::Scalar::all(0.)); + + uint64_t first_timestamp = msg->events[0].ts.toNSec(); + double final_timestamp = (1E-6*(double)(msg->events[(msg->events.size())-1].ts.toNSec()-first_timestamp)); + + // count events per pixels with polarity + cv::Mat data=cv::Mat(3, msg->events.size(), CV_64F, cv::Scalar::all(0.)); + int counter = 0; + + //std::ofstream foutX("/home/fran/xpos.txt"); + //std::ofstream foutY("/home/fran/ypos.txt"); + //std::ofstream foutTime("/home/fran/timestamp.txt"); + + for (int i = 0; i < msg->events.size(); i++) + { + const int x = msg->events[i].x; + const int y = msg->events[i].y; + + double event_timestamp = (1E-6*(double)(msg->events[i].ts.toNSec()-first_timestamp));//now in usecs + + //if (timestampMatrix.at<double>(cv::Point(x,y))> epsilon) + // diffTimestampMatrix.at<double>(cv::Point(x, y)) = event_timestamp -timestampMatrix.at<double>(cv::Point(x, y)); + //timestampMatrix.at<double>(cv::Point(x, y))= event_timestamp; + + if (event_timestamp < 15) + { + data.at<double>(cv::Point(i, 0))= (double)x/numCols; + data.at<double>(cv::Point(i, 1))= (double)y/numRows; + data.at<double>(cv::Point(i, 2))= event_timestamp/final_timestamp;//normalized + counter++; + } + else + { + //ROS_ERROR_STREAM("I'm using "<<counter<<" out of "<<msg->events.size()); + break; + } + //foutX<<x<<"\t"; foutY<<y<<"\t"; foutTime<<event_timestamp<<"\t"; + } + //foutX.close(); foutY.close(); foutTime.close(); + +/* + cv::Mat data=cv::Mat(3, 66, CV_64F, cv::Scalar::all(0.)); + int counter = 0; + std::ifstream foutX("/home/fran/xpos.txt"); + std::ifstream foutY("/home/fran/ypos.txt"); + std::ifstream foutTime("/home/fran/timestamp.txt"); + final_timestamp = 9.963; + for (int i = 0; i < 66; i++) + { + double x, y, event_timestamp; + foutX>>x; foutY>>y; foutTime>>event_timestamp; + data.at<double>(cv::Point(i, 0))= x/numCols; + data.at<double>(cv::Point(i, 1))= y/numRows; + data.at<double>(cv::Point(i, 2))= event_timestamp/final_timestamp; + } + foutX.close(); foutY.close(); foutTime.close(); +* / + + //---------------------------------------------------------------------------- + //Alternatively, one can use only the time + //cv::Mat result_image; + //cv::normalize(diffTimestampMatrix, result_image, 0, 255, cv::NORM_MINMAX, CV_8UC1); + //---------------------------------------------------------------------------- + + cv::Mat clusterCenters; + cv::Mat segmentation=cv::Mat(numRows, numCols, CV_8UC3); + segmentation = cv::Scalar(128,128,128); + + std::vector<double> clustCentX, clustCentY, clustCentZ; + std::vector<int> point2Clusters; + double bandwidth = 0.2; + std::vector<cv::Vec3b> RGBColors; + cv::RNG rng(12345); + + //ROS_ERROR_STREAM("Just before meanshift "<<msg->events.size()); + meanshiftCluster_Gaussian(data, &clustCentX, &clustCentY, &clustCentZ, &point2Clusters, bandwidth); + + //Now, draw the different segments with a color + for(int i=0; i<clustCentX.size(); i++) + RGBColors.push_back(cv::Vec3b((uchar)random(), (uchar)random(), (uchar)random())); + + counter =0; + for (int i = 0; i < msg->events.size(); i++) + { + const int x = msg->events[i].x; + const int y = msg->events[i].y; + double ts = (1E-6*(double)(msg->events[i].ts.toNSec()-first_timestamp));//now in usecs + + if(ts<15) + { + segmentation.at<cv::Vec3b>(cv::Point(x,y))=RGBColors[point2Clusters[i]]; + counter++; + } + else + { + break; + } + //ROS_ERROR_STREAM("segmentation["<<x<<","<<y<<"] = "<<RGBColors[point2Clusters[i]]); + } + + cv_segments.encoding = "bgr8"; + cv_segments.image =segmentation; + image_segmentation_pub_.publish(cv_segments.toImageMsg()); + //std::cin.ignore(); + + //Saving RGB image + //if (counterGlobal>20) + //{ + // char filename[80]; + // sprintf(filename,"/home/fran/RGB_%0d.png", counterGlobal+1); + // cv::imwrite(filename , segmentation); + // exit(0); + //} + //counterGlobal++; + //------------------------------------------------------------------------------------- + + } +}*/ + +void Meanshift::eventsCallback_simple(const dvs_msgs::EventArray::ConstPtr& msg) +{ + // only create image if at least one subscriber + if (image_pub_.getNumSubscribers() > 0) + { + cv_bridge::CvImage cv_image; + + if (display_method_ == RED_BLUE) + { + cv_image.encoding = "bgr8"; + + if (last_image_.rows == msg->height && last_image_.cols == msg->width) + { + last_image_.copyTo(cv_image.image); + used_last_image_ = true; + } + else + { + cv_image.image = cv::Mat(msg->height, msg->width, CV_8UC3); + cv_image.image = cv::Scalar(128, 128, 128); + } + + for (int i = 0; i < msg->events.size(); ++i) + { + const int x = msg->events[i].x; + const int y = msg->events[i].y; + + cv_image.image.at<cv::Vec3b>(cv::Point(x, y)) = ( + msg->events[i].polarity == true ? cv::Vec3b(255, 0, 0) : cv::Vec3b(0, 0, 255)); + } + + //Write the total number of events + //foutX<<msg->events.size()<<std::endl; + } + else + { + cv_image.encoding = "mono8"; + cv_image.image = cv::Mat(msg->height, msg->width, CV_8U); + cv_image.image = cv::Scalar(128); + + cv::Mat on_events = cv::Mat(msg->height, msg->width, CV_8U); + on_events = cv::Scalar(0); + + cv::Mat off_events = cv::Mat(msg->height, msg->width, CV_8U); + off_events = cv::Scalar(0); + + // count events per pixels with polarity + for (int i = 0; i < msg->events.size(); ++i) + { + const int x = msg->events[i].x; + const int y = msg->events[i].y; + + if (msg->events[i].polarity == 1) + on_events.at<uint8_t>(cv::Point(x, y))++; + else + off_events.at<uint8_t>(cv::Point(x, y))++; + } + + // scale image + cv::normalize(on_events, on_events, 0, 128, cv::NORM_MINMAX, CV_8UC1); + cv::normalize(off_events, off_events, 0, 127, cv::NORM_MINMAX, CV_8UC1); + + cv_image.image += on_events; + cv_image.image -= off_events; + } + image_pub_.publish(cv_image.toImageMsg()); + } + +/* + // only create image if at least one subscriber + if (image_debug1_pub_.getNumSubscribers() > 0) + { + cv_bridge::CvImage cv_image; + + if (display_method_ == RED_BLUE) + { + cv_image.encoding = "bgr8"; + + if (last_image_.rows == msg->height && last_image_.cols == msg->width) + { + last_image_.copyTo(cv_image.image); + used_last_image_ = true; + } + else + { + cv_image.image = cv::Mat(msg->height, msg->width, CV_8UC3); + cv_image.image = cv::Scalar(128, 128, 128); + } + + if(firstevent) + { + firsttimestamp = (1E-6*(double)(msg->events[0].ts.toNSec())); + firstevent = false; + } + + double maxTs; + int posx, posy; + double usTime = 16.0; + double ts; + for (int i = 0; i < msg->events.size(); ++i) + { + const int x = msg->events[i].x; + const int y = msg->events[i].y; + ts = (1E-6*(double)(msg->events[i].ts.toNSec())) - firsttimestamp; +#if BG_FILTERING + //BGAFframe.at<float>(cv::Point(x,y))=(float)(1E-6*(double)(msg->events[i].ts.toNSec())); + BGAFframe.at<float>(cv::Point(x,y))=0.; + maxTs = -1; + //ROS_ERROR_STREAM("NEW EVENT "<<x<<","<<y<<":"<<ts); + for(int ii=-1; ii<=1; ii++) + for(int jj=-1; jj<=1; jj++) + { + posx = x + ii; + posy = y + jj; + if(posx<0) + posx = 0; + if(posy<0) + posy=0; + if(posx>numRows-1) + posx = numRows-1; + if(posy>numCols-1) + posy = numCols-1; + + if(BGAFframe.at<float>(cv::Point(posx,posy)) > maxTs) + maxTs = BGAFframe.at<float>(cv::Point(posx,posy)); + + //ROS_ERROR_STREAM(posx<<","<<posy<<":"<<BGAFframe.at<float>(cv::Point(posx,posy))); + } + //ROS_ERROR_STREAM("maxTs: "<<maxTs); + BGAFframe.at<float>(cv::Point(x,y))=ts; + //ROS_ERROR_STREAM(BGAFframe.at<float>(cv::Point(x,y)) - maxTs); + + if(BGAFframe.at<float>(cv::Point(x,y)) >= (maxTs + usTime)) + { + continue; + //ROS_ERROR_STREAM("HERE"); + } + +#endif + cv_image.image.at<cv::Vec3b>(cv::Point(x, y)) = ( + msg->events[i].polarity == true ? cv::Vec3b(255, 0, 0) : cv::Vec3b(0, 0, 255)); + } + //ROS_ERROR_STREAM("DONE--------------------------------------------"); + + //Write the total number of events + //foutX<<msg->events.size()<<std::endl; + } + image_debug1_pub_.publish(cv_image.toImageMsg()); + } +*/ + + + if (image_segmentation_pub_.getNumSubscribers() > 0) + { + cv_bridge::CvImage cv_segments; + + //Doing color meanshift + //cv::Mat diffTimestampMatrix = cv::Mat(numRows, numCols, CV_64F, cv::Scalar::all(0.)); + //cv::Mat timestampMatrix = cv::Mat(numRows, numCols, CV_64F, cv::Scalar::all(0.)); + + uint64_t first_timestamp = msg->events[0].ts.toNSec(); + double final_timestamp = (1E-6*(double)(msg->events[(msg->events.size())-1].ts.toNSec()-first_timestamp)); + std::vector<bool> activeEvents(msg->events.size()); + + int counterIn = 0; + int counterOut = 0; + + int beginEvent = 0; + int packet; + if(DVSW==240) + packet = 1800; + else + packet = 1500; + //ROS_ERROR_STREAM("Before while"); + + + while(beginEvent < msg->events.size()) + { + //if(msg->events.size()>15e3) + //ROS_ERROR_STREAM("BEGIN packet "<<beginEvent); + //SELECT SMALL PACKETS OF MAXIMUM 1500 events + counterIn = 0; + counterOut = 0; + //ROS_ERROR_STREAM("Computing events ["<<beginEvent<<","<<min(beginEvent+packet, msg->events.size())<<"]"); + //ROS_ERROR_STREAM("SIZE of current package is "<<msg->events.size()<<"];"); + + // count events per pixels with polarity + cv::Mat data=cv::Mat(3, min(packet, msg->events.size()-beginEvent), CV_64F, cv::Scalar::all(0.)); + + //std::ofstream foutX("/home/fran/xpos.txt"); + //std::ofstream foutY("/home/fran/ypos.txt"); + //std::ofstream foutTime("/home/fran/timestamp.txt"); + + //Filter events + //cv::Mat BGAFframe = cv::Mat(numRows, numCols, CV_32FC1); + //frame =cv::Scalar(-1); + + if(firstevent) + { + firsttimestamp = (1E-6*(double)(msg->events[10].ts.toNSec())); + firstevent = false; + } + + double maxTs; + int posx, posy; + double usTime = 10.0; + double ts; + + + + for (int i = beginEvent; i < min(beginEvent+packet, msg->events.size()); i++) + { + //SELECT SMALL PACKETS OF MAXIMUM 1000 events + const int x = msg->events[counterIn].x; + const int y = msg->events[counterIn].y; + + double event_timestamp = (1E-6*(double)(msg->events[counterIn].ts.toNSec()-first_timestamp));//now in usecs + + ts = (1E-6*(double)(msg->events[i].ts.toNSec())) - firsttimestamp; +#if BG_FILTERING + //BGAFframe.at<float>(cv::Point(x,y))=(float)(1E-6*(double)(msg->events[i].ts.toNSec())); + BGAFframe.at<float>(cv::Point(x,y))=0.; + maxTs = -1; + //calculate maximum in a 3x3 neighborhood + for(int ii=-1; ii<=1; ii++) + for(int jj=-1; jj<=1; jj++) + { + posx = x + ii; + posy = y + jj; + if(posx<0) + posx = 0; + if(posy<0) + posy=0; + if(posx>numRows-1) + posx = numRows-1; + if(posy>numCols-1) + posy = numCols-1; + + if(BGAFframe.at<float>(cv::Point(posx,posy)) > maxTs) + maxTs = BGAFframe.at<float>(cv::Point(posx,posy)); + } + + BGAFframe.at<float>(cv::Point(x,y))=ts; + //if nothing happened in usTime, remove the event + if(BGAFframe.at<float>(cv::Point(x,y)) >= (maxTs + usTime)) + { + activeEvents.at(counterIn)=false; + counterIn++; + } + else + { +#endif + data.at<double>(cv::Point(counterOut, 0))= (double)x/numCols; + data.at<double>(cv::Point(counterOut, 1))= (double)y/numRows; + //data.at<double>(cv::Point(counter, 2))= event_timestamp/final_timestamp;//normalized + double tau = 10000; + data.at<double>(cv::Point(counterOut, 2))= exp(-(final_timestamp-event_timestamp)/tau);//normalized + activeEvents.at(counterIn)=true; + counterIn++; + counterOut++; +#if BG_FILTERING + } +#endif + + + + //foutX<<x<<"\t"; foutY<<y<<"\t"; foutTime<<event_timestamp<<"\t"; + } + double last_timestamp = (1E-6*(double)(msg->events[counterIn-1].ts.toNSec()));//now in usecs + //foutX.close(); foutY.close(); foutTime.close(); + + cv::Mat clusterCenters; + cv::Mat segmentation=cv::Mat(numRows, numCols, CV_8UC3); + segmentation = cv::Scalar(128,128,128); + + cv::Mat traj = cv::Mat(numRows, numCols, CV_8UC3); + traj = cv::Scalar(128,128,128); + + std::vector<double> clustCentX, clustCentY, clustCentZ; + std::vector<int> point2Clusters; + std::vector<int> positionClusterColor; + std::vector<int> assign_matches; + + + //double bandwidth = 0.05; + double bandwidth = 0.15; + //double bandwidth = 0.15; + //double bandwidth = 0.20; + //double bandwidth = 0.25; + //double bandwidth = 0.50; + //cv::RNG rng(12345); + +/* if(counterGlobal==0) + { + //clock_t start, end; + //double elapsed; + //start = clock(); + //clustCentZ_old = clustCentZ; + + meanshiftCluster_Gaussian(data, &clustCentX, &clustCentY, &clustCentZ, &point2Clusters, bandwidth); + + assignClusterColor(&positionClusterColor, clustCentX.size(), assign_matches, prev_positionClusterColor); //assign new colors to clusters + + prev_clustCentX = clustCentX; + prev_clustCentY = clustCentY; + prev_positionClusterColor = positionClusterColor; + //std::vector<int> checkvector (clustCentX.size(), -1); + //ROS_ERROR_STREAM("Total number of clusters is "<<clustCentX.size()); + + + //end = clock(); + //elapsed = ((double) (end - start)) / CLOCKS_PER_SEC; + //ROS_ERROR_STREAM("Num. packets = "<<data.cols<<" elapsed time = "<<elapsed<<". Time/packet = "<<elapsed/data.cols); + + //Moved to initialization + //Now, draw the different segments with a color + //for(int i=0; i<clustCentX.size(); i++) + // RGBColors.push_back(cv::Vec3b((uchar)random(), (uchar)random(), (uchar)random())); + + counter =0; + for (int i = beginEvent; i < min(beginEvent+packet, msg->events.size()); i++) + { + const int x = msg->events[counter].x; + const int y = msg->events[counter].y; + double ts = (1E-6*(double)(msg->events[counter].ts.toNSec()-first_timestamp));//now in usecs + + //if(ts<15) + //{ + //segmentation.at<cv::Vec3b>(cv::Point(x,y))=RGBColors[point2Clusters[counter]]; + //segmentation.at<cv::Vec3b>(cv::Point(x,y))=RGBColors[(positionClusterColor[point2Clusters[counter]])%MAX_NUM_CLUSTERS]; + segmentation.at<cv::Vec3b>(cv::Point(x,y))=RGBColors[(positionClusterColor[point2Clusters[counter]])&MAX_NUM_CLUSTERS]; //cheaper than % (mod operation) + //} + //else + // break; + counter++; + } + } + else + { + +*/ + + //fouttotalproc<<min(packet, msg->events.size()-beginEvent)<<std::endl; + + meanshiftCluster_Gaussian(data, &clustCentX, &clustCentY, &clustCentZ, &point2Clusters, bandwidth); + + //Assign new color or use color from previous clusters? + //if(counterGlobal>0)//not the first time + findClosestCluster(&assign_matches, clustCentX, clustCentY, prev_clustCentX, prev_clustCentY); //no match when clustID is -1 + + //assignClusterColor(&positionClusterColor, clustCentX.size(), assign_matches, prev_positionClusterColor); //assign new colors to clusters + assignClusterColor(&positionClusterColor, clustCentX.size(), assign_matches, prev_positionClusterColor, prev_activeTrajectories); //assign new colors to clusters + + //foutnumclust<<clustCentX.size()<<std::endl; + + int tmpColorPos; + float estimClustCenterX=-1., estimClustCenterY=-1.; + //std::vector<int> activeTrajectories(MAX_NUM_CLUSTERS,0); + std::vector<int> activeTrajectories; + + if(point2Clusters.size()>400) //update positions only when there is enough change (>25%) + { + for(int i=0; i<clustCentX.size(); i++) + { + estimClustCenterX=-1., estimClustCenterY=-1.; + + tmpColorPos = (positionClusterColor[i])&(MAX_NUM_CLUSTERS-1); + + //Check if the new position is being used + /*int idx = 0; + for (int idx=0; idx<prev_activeTrajectories.size(); idx++) + if(tmpColorPos == prev_activeTrajectories[idx]) + break;*/ + std::vector<int>::iterator it; + it = find (prev_activeTrajectories.begin(), prev_activeTrajectories.end(), tmpColorPos); + + if(it==prev_activeTrajectories.end())//if the element tmpcolorpos is not found in the vector + { + //Initialize the trajectory (just the counter) + //allTrajectories[tmpColorPos] = std::vector<cv::Point>(MAX_NUM_TRAJECTORY_POINTS); + counterTrajectories[tmpColorPos]=0; + +#if KALMAN_FILTERING + foundTrajectory[tmpColorPos] = false; + createKalmanFilter(&(vector_of_kf[tmpColorPos]), &(vector_of_state[tmpColorPos]), &(vector_of_meas[tmpColorPos])); + vector_of_foundBlobs[tmpColorPos]=false; + vector_of_ticks[tmpColorPos]=0.0; +#endif + } + + /* + if(prev_activeTrajectories[tmpColorPos]==0) //TODO: when the cluster is tracked all the time, it is active when I go back with &MAX_NUM_CLUSTERS + { + //Initialize the trajectory (just the counter) + //allTrajectories[tmpColorPos] = std::vector<cv::Point>(MAX_NUM_TRAJECTORY_POINTS); + counterTrajectories[tmpColorPos]=0; + }*/ + +#if KALMAN_FILTERING + if(counterTrajectories[tmpColorPos]>25 & !foundTrajectory[tmpColorPos]) + { + //selectedTrajectory = tmpColorPos; + foundTrajectory[tmpColorPos] = true; + } + //if(foundTrajectory[tmpColorPos] & selectedTrajectory == tmpColorPos) //More than 25 points in the trajectory, start w/ KF + if(foundTrajectory[tmpColorPos]) //More than 25 points in the trajectory, start w/ KF + { + //double precTick = ticks; + //ticks = (double) cv::getTickCount(); + //double dT = (ticks - precTick) / cv::getTickFrequency(); //seconds + + /* In case we use only 1 kalman filter + if((last_timestamp - ticks) > 1) + { + double precTick = ticks; + ticks = last_timestamp; + double dT = (ticks - precTick)/1000; //seconds + + if (foundBlobs) + { + // >>>> Matrix A + kf.transitionMatrix.at<float>(2) = dT; + kf.transitionMatrix.at<float>(7) = dT; + // <<<< Matrix A + state = kf.predict(); + estimClustCenterX = state.at<float>(0); + estimClustCenterY = state.at<float>(1); + } + + meas.at<float>(0) = clustCentX[i]*DVSW; + meas.at<float>(1) = clustCentY[i]*DVSH; //[z_x, z_y] + + if (!foundBlobs) // First detection! + { + // >>>> Initialization + kf.errorCovPre.at<float>(0) = 1; // px + kf.errorCovPre.at<float>(5) = 1; // px + kf.errorCovPre.at<float>(10) = 2; + kf.errorCovPre.at<float>(15) = 2; + + state.at<float>(0) = meas.at<float>(0); + state.at<float>(1) = meas.at<float>(1); + state.at<float>(2) = 0; + state.at<float>(3) = 0; //[z_x, z_y, v_x, v_y] + // <<<< Initialization + + kf.statePost = state; + + foundBlobs = true; + } + else + kf.correct(meas); // Kalman Correction + + if(estimClustCenterX>=0.) //initialized to -1 + { + //ROS_ERROR_STREAM("Estimated point = ["<<estimClustCenterX<<", "<<estimClustCenterY<<"];"); + //ROS_ERROR_STREAM("Measured point = ["<<clustCentX[i]<<", "<<clustCentY[i]<<"];"); + cv::circle(trajectories, cv::Point(clustCentX[i]*DVSW, clustCentY[i]*DVSH), 2, cv::Scalar( 0, 0, 255 ),-1,8); + cv::circle(trajectories, cv::Point(estimClustCenterX, estimClustCenterY), 2, cv::Scalar( 255, 0, 0),-1,8); + foutX<<clustCentX[i]*DVSW<<" "<<clustCentY[i]*DVSH<<std::endl; + foutX_estim<<estimClustCenterX<<" "<<estimClustCenterY<<std::endl; + foutT<<dT<<std::endl; + } + } + */ + + if((last_timestamp - vector_of_ticks[tmpColorPos]) > 1) + { + double precTick = vector_of_ticks[tmpColorPos]; + vector_of_ticks[tmpColorPos] = last_timestamp; + double dT = (vector_of_ticks[tmpColorPos] - precTick)/1000; //seconds + //if (foundBlobs) + if(vector_of_foundBlobs[tmpColorPos]) + { + // >>>> Matrix A + vector_of_kf[tmpColorPos].transitionMatrix.at<float>(2) = dT; + vector_of_kf[tmpColorPos].transitionMatrix.at<float>(7) = dT; + // <<<< Matrix A + vector_of_state[tmpColorPos] = vector_of_kf[tmpColorPos].predict(); + estimClustCenterX = vector_of_state[tmpColorPos].at<float>(0); + estimClustCenterY = vector_of_state[tmpColorPos].at<float>(1); + } + vector_of_meas[tmpColorPos].at<float>(0) = clustCentX[i]*DVSW; + vector_of_meas[tmpColorPos].at<float>(1) = clustCentY[i]*DVSH; //[z_x, z_y] + + + //if (!foundBlobs) // First detection! + if(!vector_of_foundBlobs[tmpColorPos]) + { + // >>>> Initialization + vector_of_kf[tmpColorPos].errorCovPre.at<float>(0) = 1; // px + vector_of_kf[tmpColorPos].errorCovPre.at<float>(5) = 1; // px + vector_of_kf[tmpColorPos].errorCovPre.at<float>(10) = 2; + vector_of_kf[tmpColorPos].errorCovPre.at<float>(15) = 2; + + vector_of_state[tmpColorPos].at<float>(0) = vector_of_meas[tmpColorPos].at<float>(0); + vector_of_state[tmpColorPos].at<float>(1) = vector_of_meas[tmpColorPos].at<float>(1); + vector_of_state[tmpColorPos].at<float>(2) = 0; + vector_of_state[tmpColorPos].at<float>(3) = 0; //[z_x, z_y, v_x, v_y] + // <<<< Initialization + + vector_of_kf[tmpColorPos].statePost = vector_of_state[tmpColorPos]; + + //foundBlobs = true; + vector_of_foundBlobs[tmpColorPos]=true; + } + else + vector_of_kf[tmpColorPos].correct(vector_of_meas[tmpColorPos]); // Kalman Correction + + if(estimClustCenterX>=0.) //initialized to -1 + { + //ROS_ERROR_STREAM("Estimated difference = ["<<abs(estimClustCenterX/DVSW -clustCentX[i]) <<", "<<abs(estimClustCenterY/DVSH-clustCentY[i])<<"];"); + //ROS_ERROR_STREAM("Estimated point = ["<<estimClustCenterX<<", "<<estimClustCenterY<<"];"); + //ROS_ERROR_STREAM("Measured point = ["<<clustCentX[i]<<", "<<clustCentY[i]<<"];"); + //cv::circle(trajectories, cv::Point(clustCentX[i]*DVSW, clustCentY[i]*DVSH), 2, cv::Scalar( 0, 0, 255 ),-1,8); + //cv::circle(trajectories, cv::Point(estimClustCenterX, estimClustCenterY), 2, cv::Scalar( 255, 0, 0),-1,8); + + //foutX<<tmpColorPos<<" "<<clustCentX[i]*DVSW<<" "<<clustCentY[i]*DVSH<<" "<<estimClustCenterX<<" "<<estimClustCenterY<<std::endl; + + //foutX_estim<<estimClustCenterX<<" "<<estimClustCenterY<<std::endl; + //foutT<<dT<<std::endl; + //foutX<<"Estimated difference = ["<<abs(estimClustCenterX/DVSW -clustCentX[i]) <<", "<<abs(estimClustCenterY/DVSH-clustCentY[i])<<"];"<<std::endl; + + // !!!!!!CAREFUL **************************************************************************************** + //ACTIVATE THIS !!!!! + //clustCentX[i] = estimClustCenterX/DVSW; + //clustCentY[i] = estimClustCenterY/DVSH; + // CAREFUL **************************************************************************************** + } + } + } +#endif + + cv::Point end(clustCentX[i]*DVSW, clustCentY[i]*DVSH); +/* +#if KALMAN_FILTERING + //frame, event, colorCluster, X,Y, filtX, filtY + foutX<<counterGlobal+1<<" "<<(double)(1E-6*(msg->events[beginEvent].ts.toNSec()-msg->events[0].ts.toNSec()))<<" " <<tmpColorPos<<" "<<clustCentX[i]*DVSW<<" "<<clustCentY[i]*DVSH<<" "<<estimClustCenterX<<" "<<estimClustCenterY<<std::endl; +#endif +*/ + + allTrajectories[tmpColorPos][(counterTrajectories[tmpColorPos]) & (MAX_NUM_TRAJECTORY_POINTS-1)]=end; + counterTrajectories[tmpColorPos]++; + //activeTrajectories[tmpColorPos]=1; + activeTrajectories.push_back(tmpColorPos); + } + + prev_clustCentX = clustCentX; + prev_clustCentY = clustCentY; + prev_positionClusterColor = positionClusterColor; + prev_activeTrajectories = activeTrajectories; + + trajectories = cv::Scalar(128,128,128); + int first=1; + for(int i=0; i<activeTrajectories.size(); i++) + { + int tmpval = activeTrajectories[i]; + if (counterTrajectories[tmpval]>1) + { + if(counterTrajectories[tmpval]<=MAX_NUM_TRAJECTORY_POINTS) + { + for(int j=1; j<counterTrajectories[tmpval]; j++) //instead of % I use &(power of 2 -1): it is more efficient + cv::line( trajectories, allTrajectories[tmpval][j-1], allTrajectories[tmpval][j], cv::Scalar( RGBColors[tmpval].val[0], RGBColors[tmpval].val[1], RGBColors[tmpval].val[2]), 2, 1); + } + else + { + int j=1; + int pos = counterTrajectories[tmpval]; + while(j<MAX_NUM_TRAJECTORY_POINTS) + { + cv::line( trajectories, allTrajectories[tmpval][pos & (MAX_NUM_TRAJECTORY_POINTS-1)], allTrajectories[tmpval][(pos+1)&(MAX_NUM_TRAJECTORY_POINTS-1)], cv::Scalar( RGBColors[tmpval].val[0], RGBColors[tmpval].val[1], RGBColors[tmpval].val[2]), 2, 1); + pos = pos+1; + j++; + } + } + } + + } + } + + trajectories.copyTo(segmentation); //Always keep trajectories + +/* +#if TEST + char filename[80]; + sprintf(filename,"/home/fran/0_tmp/kk/RGB_%0d.png", counterGlobal+1); + cv::imwrite(filename , segmentation); + + sprintf(filename,"/home/fran/0_tmp/kk/RGB_%0d.txt", counterGlobal+1); + std::ofstream foutallT(filename); + + for(int k=0; k<activeTrajectories.size(); k++) + if(counterTrajectories[activeTrajectories[k]]>0) + { + foutallT<<"ClusterID "<<activeTrajectories[k]<<" with "<<counterTrajectories[activeTrajectories[k]]<<" elems: = ["; + for(int j=0; j<counterTrajectories[activeTrajectories[k]]; j++) //instead of % I use &(power of 2 -1): it is more efficient + { + foutallT<<"("<<allTrajectories[activeTrajectories[k]][j].x<<", "<<allTrajectories[activeTrajectories[k]][j].y<<"), "; + } + foutallT<<"];\n"; + } + + foutallT.close(); +#endif +*/ + +#if TEST + char filename1[80]; + sprintf(filename1,"/home/fran/0_tmp/kk/rgb_%0d.txt", counterGlobal+1); + std::ofstream foutX(filename1); +#endif + counterIn =0; + counterOut=0; + + std::vector<uint64_t> xVar(MAX_NUM_CLUSTERS-1, 0); + std::vector<uint64_t> yVar(MAX_NUM_CLUSTERS-1, 0); + std::vector<uint64_t> xMean(MAX_NUM_CLUSTERS-1, 0); + std::vector<uint64_t> xMean(MAX_NUM_CLUSTERS-1, 0); + std::vector<uint64_t> numEvents2Cluster(MAX_NUM_CLUSTERS-1, 0); + std::vector<uint64_t> Variance(MAX_NUM_CLUSTERS-1, 0); + + + for (int i = beginEvent; i < min(beginEvent+packet, msg->events.size()); i++) + { + if(activeEvents.at(counterIn)) //Label it only if it is not noise + { + const int x = msg->events[counterIn].x; + const int y = msg->events[counterIn].y; + double ts = (1E-6*(double)(msg->events[counterIn].ts.toNSec()-first_timestamp));//now in usecs + + //VARIANCE CALUCLATION + int idx = positionClusterColor[point2Clusters[counterOut]])&(MAX_NUM_CLUSTERS-1) + numEvents2Cluster[idx] = numEvents2Cluster[idx] + 1; + + xMean[idx] = xMean[idx] + x; + yMean[idx] = yMean[idx] + y; + + //xVar[idx] = xVar[idx] + ((x - clustCentX[idx]) * (x - clustCentX[idx])); + //yVar[idx] = yVar[idx] + ((x - clustCentY[idx]) * (x - clustCentY[idx])); + + //xVar = xVar + ((msg->events[i].x - avg_x) * (msg->events[i].x - avg_x)); + + + segmentation.at<cv::Vec3b>(cv::Point(x,y))=RGBColors[(positionClusterColor[point2Clusters[counterOut]])&(MAX_NUM_CLUSTERS-1)]; //cheaper than % (mod operation) +#if TEST + //foutX<<x<<" "<<y<<" "<<ts<<" "<<msg->events[counter].polarity<<'\n'; + //foutX<<x<<'\t'<<'\t'<<y<<'\t'<<ts<<'\t'<<(int)(msg->events[counter].polarity)<<'\t'<<(int)((positionClusterColor[point2Clusters[counter]])&(MAX_NUM_CLUSTERS-1))<<'\n'; +#endif + counterOut++; + } + //} + //else + //{ + // break; + //} + counterIn++; + } + counterIn = 0; + for (int i = beginEvent; i < min(beginEvent+packet, msg->events.size()); i++) + { + const int x = msg->events[counterIn].x; + const int y = msg->events[counterIn].y; + + if(activeEvents.at(counterIn) && (numEvents2Cluster[i] != 0)) //Label it only if it is not noise + { + xMean[i] = xMean[i] / numEvents2Cluster[i]; + yMean[i] = yMean[i] / numEvents2Cluster[i]; + } + else + { + counterIn++; + } + + } + counterIn = 0; + for (int i = beginEvent; i < min(beginEvent+packet, msg->events.size()); i++) + { + const int x = msg->events[counterIn].x; + const int y = msg->events[counterIn].y; + + if(activeEvents.at(counterIn) && (numEvents2Cluster[i] != 0)) //Label it only if it is not noise + { + xVar[idx] = xVar[idx] + ((x - xMean[i]) * (x - xMean[i])); + yVar[idx] = yVar[idx] + ((y - yMean[i]) * (y - yMean[i])); + } + else + { + counterIn++; + } + } + + + + std_msgs::String dir; + for(int i = 0 ; i <= MAX_NUM_CLUSTERS-1; ++i) + { + if(numEvents2Cluster[i] != 0) + { + xVar[i] = xVar[i] / numEvents2Cluster[i]; + yVar[i] = yVar[i] / numEvents2Cluster[i]; + Variance[i] = (xVar[i] + yVar[i] / 2); + + Variance[i] + 10 > preVariance[i] ? counterFor[i]++ : counterBar[i]++; + + if(counterBar[i] > 10) + { + counterFor[i] = 0; + counterBar[i] = 0; + } + + if(counterFor[i] > 5) + { + if(clustCentY[i]*DVSH < 75) + dir.data = "above"; + else if (clustCentY[i]*DVSH > 45) + dir.data = "below"; + else + dir.data = "none"; + } + } + + preVariance[i] = Variance[i]; + + dirUB.publish(dir); + } + } + + char filename2[80]; + sprintf(filename2,"/home/fran/0_tmp/kk/rgb_%0d.png", counterGlobal+1); + cv::imwrite(filename2, segmentation); + + +// } + + + cv_segments.encoding = "bgr8"; + cv_segments.image = segmentation; + image_segmentation_pub_.publish(cv_segments.toImageMsg()); + + //std::cin.ignore(); + + beginEvent +=packet; + +//#if TEST + //char filename[80]; + //sprintf(filename,"/home/fran/0_tmp/kk/rgb_%0d.jpg", counterGlobal+1); + //cv::imwrite(filename , segmentation); + //sprintf(filename,"/home/fran/0_tmp/kk/cc_%0d.txt", counterGlobal+1); + //std::ofstream foutcc(filename); + //for (int i = 0; i<clustCentX.size(); i++) + // foutcc<<"ClusterCenter("<<i+1<<",:)=["<<clustCentX.at(i)*DVSW<<","<<clustCentY.at(i)*DVSH<<"];"<<std::endl; + //foutcc.close(); +//#endif + //Saving RGB image +/* + if (counterGlobal>0) + { + //std::cin.ignore(); + //for (int i = 0; i<clustCentX.size(); i++) + // ROS_ERROR_STREAM("ClusterCenter("<<i+1<<",:)=["<<clustCentX.at(i)<<","<<clustCentY.at(i)<<","<<clustCentZ.at(i)<<"];"); + + //std::cout<<"clustperevent=["<<point2Clusters[0]; + //for (int i = 1; i<point2Clusters.size(); i++) + // std::cout<<","<<point2Clusters[i]; + //std::cout<<"];"<<std::endl; + + char filename[80]; + sprintf(filename,"/home/fran/0_tmp/kk/RGB_%0d.png", counterGlobal+1); + cv::imwrite(filename , segmentation); + //exit(0); + + //char filename[80]; + //sprintf(filename,"/home/fran/0_tmp/kk/RGB_%0d.txt", counterGlobal+1); + //std::ofstream foutX(filename); + //foutX<<counter<<"\n"; + //foutX.close(); + } +*/ + + + /*if(msg->events.size()>15e3) + { + char filename[80]; + sprintf(filename,"/home/fran/0_tmp/kk/RGB_%0d.png", counterGlobal+1); + cv::imwrite(filename , segmentation); + exit(0); + }*/ + counterGlobal++; + } + } +} + + + + +} // namespace diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/meanshift_node.cpp b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/meanshift_node.cpp new file mode 100755 index 0000000000000000000000000000000000000000..879b219b3532f680d1f1d3c5af0841e88bca02c1 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/meanshift_node.cpp @@ -0,0 +1,17 @@ + + +#include "dvs_meanshift/meanshift.h" + +int main(int argc, char* argv[]) +{ + ros::init(argc, argv, "dvs_meanshift"); + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + + dvs_meanshift::Meanshift meanshift(nh, nh_private); + + ros::spin(); + + return 0; +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/segment.cpp b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/segment.cpp new file mode 100755 index 0000000000000000000000000000000000000000..e64a0c835ac32a837dde0a02cc290e48c746bfb2 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/dvs_meanshift/src/segment.cpp @@ -0,0 +1,171 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#include <cstdio> +#include <cstdlib> +#include "graph3d/segment.h" +#include "graph3d/image.h" +#include "graph3d/misc.h" +#include "graph3d/pnmfile.h" +#include "graph3d/segment-image.h" + + +/*#include <cstdio> +#include <cstdlib> +#include <cv_bridge/cv_bridge.h> +#include <opencv2/core/core.hpp> +#include <opencv2/imgproc/imgproc.hpp> +#include "graph3d/image.h" +#include "graph3d/misc.h" +#include "graph3d/pnmfile.h" +#include "graph3d/segment-image.h" + + +int main(int argc, char **argv) { + if (argc != 6) { + fprintf(stderr, "usage: %s sigma k min input(ppm) output(ppm)\n", argv[0]); + return 1; + } + + float sigma = atof(argv[1]); + float k = atof(argv[2]); + int min_size = atoi(argv[3]); + + printf("loading input image.\n"); + image<rgb> *input = loadPPM(argv[4]); + + printf("processing\n"); + int num_ccs; + image<rgb> *seg = segment_image(input, sigma, k, min_size, &num_ccs); + savePPM(seg, argv[5]); + + + printf("got %d components\n", num_ccs); + printf("done! uff...thats hard work.\n"); + + return 0; +}*/ + + + +//void imadjust(const cv::Mat & src, cv::Mat & dst, int tol = 1, cv::Vec2i in = cv::Vec2i(0, 255), cv::Vec2i out = cv::Vec2i(0, 255)) +void imadjust(const cv::Mat & src, cv::Mat & dst) +{ + // src : input CV_8UC1 image + // dst : output CV_8UC1 imge + // tol : tolerance, from 0 to 100. + // in : src image bounds + // out : dst image buonds + + int in[2]; in[0]=0; in[1]=255; + int out[2]; out[0]=0; out[1]=255; + int tol =1; + + + + dst = src.clone(); + + tol = std::max(0, std::min(100, tol)); + + if (tol > 0) + { + // Compute in and out limits + + // Histogram + std::vector<int> hist(256, 0); + for (int r = 0; r < src.rows; ++r) { + for (int c = 0; c < src.cols; ++c) { + hist[src.at<uint8_t>(cv::Point(r,c))]++; + } + } + + // Cumulative histogram + std::vector<int> cum = hist; + for (int i = 1; i < hist.size(); ++i) { + cum[i] = cum[i - 1] + hist[i]; + } + + // Compute bounds + int total = src.rows * src.cols; + int low_bound = total * tol / 100; + int upp_bound = total * (100-tol) / 100; + in[0] = distance(cum.begin(), lower_bound(cum.begin(), cum.end(), low_bound)); + in[1] = distance(cum.begin(), lower_bound(cum.begin(), cum.end(), upp_bound)); + + } + + // Stretching + float scale = float(out[1] - out[0]) / float(in[1] - in[0]); + for (int r = 0; r < dst.rows; ++r) + { + for (int c = 0; c < dst.cols; ++c) + { + int vs = std::max(src.at<uint8_t>(cv::Point(r, c)) - in[0], 0); + + + int vd = std::min(int(vs * scale + 0.5f) + out[0], out[1]); + dst.at<uint8_t>(cv::Point(r, c)) = cv::saturate_cast<uchar>(vd); + } + } +} + +//This function is just an interface to call the segment_image function +void im2segment(cv::Mat input, cv::Mat cv_image_output, float sigma, float k, int min_size, int *num_ccs) +{ + + image<rgb> *inputIm=new image<rgb>(input.cols, input.rows); + + + for (int y = 0; y < input.rows; y++) { + for (int x = 0; x < input.cols; x++) { + imRef(inputIm, x, y).r = input.at<uint8_t>(cv::Point(x, y)); + imRef(inputIm, x, y).g = input.at<uint8_t>(cv::Point(x, y)); + imRef(inputIm, x, y).b = input.at<uint8_t>(cv::Point(x, y)); + } + } + + std::cout<<"I'm here!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<std::endl; + savePPM(inputIm, "/home/fran/input_image.ppm"); + + image<rgb> *seg = segment_image(inputIm, sigma, k, min_size, num_ccs); + + //cv_image_output.encoding = "bgr8"; + //cv_image_output.image =cv::Mat(input.rows, input.cols, CV_8UC3); + + for (int y = 0; y < input.rows; y++) { + for (int x = 0; x < input.cols; x++) { + //output.at<uint8_t>(cv::Point(x, y))=imRef(seg, x, y); + cv_image_output.at<cv::Vec3b>(cv::Point(x, y)) = ( \ + cv::Vec3b(imRef(seg,x,y).b, imRef(seg,x,y).g, imRef(seg,x,y).r)); + } + } + + + /*image<uchar> *inputTmp=new image<uchar>(input.cols, input.rows);; + for (int y = 0; y < input.rows; y++) { + for (int x = 0; x < input.cols; x++) { + imRef(inputTmp, x, y) = input.at<uint8_t>(cv::Point(x, y)); + } + } + savePGM(inputTmp, "/home/fran/input_image.ppm"); + */ + + //printf("got %d components\n", num_ccs); + //printf("done! uff...thats hard work.\n"); +} + diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.cproject b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.cproject new file mode 100755 index 0000000000000000000000000000000000000000..98ae393212dab9a1c62f98389dff3cdb598daba3 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.cproject @@ -0,0 +1,67 @@ +<?xml version="1.0" encoding="UTF-8" standalone="no"?> +<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage"> + <storageModule moduleId="org.eclipse.cdt.core.settings"> + <cconfiguration id="cdt.managedbuild.toolchain.gnu.base.1733434747"> + <storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.1733434747" moduleId="org.eclipse.cdt.core.settings" name="Default"> + <externalSettings/> + <extensions> + <extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/> + <extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> + <extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/> + <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> + <extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> + <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> + </extensions> + </storageModule> + <storageModule moduleId="cdtBuildSystem" version="4.0.0"> + <configuration artifactName="${ProjName}" buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.base.1733434747" name="Default" parent="org.eclipse.cdt.build.core.emptycfg"> + <folderInfo id="cdt.managedbuild.toolchain.gnu.base.1733434747.361979371" name="/" resourcePath=""> + <toolChain id="cdt.managedbuild.toolchain.gnu.base.1216366819" name="cdt.managedbuild.toolchain.gnu.base" superClass="cdt.managedbuild.toolchain.gnu.base"> + <targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.894778321" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/> + <builder enableAutoBuild="true" id="cdt.managedbuild.target.gnu.builder.base.714995375" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="cdt.managedbuild.target.gnu.builder.base"/> + <tool id="cdt.managedbuild.tool.gnu.archiver.base.1646610619" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/> + <tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.2073536060" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base"/> + <tool id="cdt.managedbuild.tool.gnu.c.compiler.base.1181955887" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base"> + <option id="gnu.c.compiler.option.preprocessor.def.symbols.617236572" name="Defined symbols (-D)" superClass="gnu.c.compiler.option.preprocessor.def.symbols" useByScannerDiscovery="false" valueType="definedSymbols"> + <listOptionValue builtIn="false" value="_DEFAULT_SOURCE=1"/> + <listOptionValue builtIn="false" value="_XOPEN_SOURCE=700"/> + </option> + <option id="gnu.c.compiler.option.include.paths.10561483" name="Include paths (-I)" superClass="gnu.c.compiler.option.include.paths" useByScannerDiscovery="false" valueType="includePath"> + <listOptionValue builtIn="false" value="/usr/include/libusb-1.0"/> + </option> + <inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1645890299" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/> + </tool> + <tool id="cdt.managedbuild.tool.gnu.c.linker.base.226870374" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"> + <inputType id="cdt.managedbuild.tool.gnu.c.linker.input.749058961" superClass="cdt.managedbuild.tool.gnu.c.linker.input"> + <additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/> + <additionalInput kind="additionalinput" paths="$(LIBS)"/> + </inputType> + </tool> + <tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.1405901520" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base"/> + <tool id="cdt.managedbuild.tool.gnu.assembler.base.1784132232" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base"> + <inputType id="cdt.managedbuild.tool.gnu.assembler.input.115896780" superClass="cdt.managedbuild.tool.gnu.assembler.input"/> + </tool> + </toolChain> + </folderInfo> + </configuration> + </storageModule> + <storageModule moduleId="org.eclipse.cdt.core.externalSettings"/> + </cconfiguration> + </storageModule> + <storageModule moduleId="cdtBuildSystem" version="4.0.0"> + <project id="libcaer.null.970779615" name="libcaer"/> + </storageModule> + <storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/> + <storageModule moduleId="refreshScope" versionNumber="2"> + <configuration configurationName="Default"> + <resource resourceType="PROJECT" workspacePath="/libcaer"/> + </configuration> + </storageModule> + <storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/> + <storageModule moduleId="scannerConfiguration"> + <autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/> + <scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.1733434747;cdt.managedbuild.toolchain.gnu.base.1733434747.361979371;cdt.managedbuild.tool.gnu.c.compiler.base.1181955887;cdt.managedbuild.tool.gnu.c.compiler.input.1645890299"> + <autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/> + </scannerConfigBuildInfo> + </storageModule> +</cproject> diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.gitignore b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.gitignore new file mode 100755 index 0000000000000000000000000000000000000000..7b08def56d1c871eea014d5477322b260502f056 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.gitignore @@ -0,0 +1,11 @@ +/build/ +CMakeCache.txt +CMakeFiles/ +Makefile +cmake_install.cmake +/install_manifest.txt +/libcaer.pc +*~ +*.swp +*.swo +*.sh diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.project b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.project new file mode 100755 index 0000000000000000000000000000000000000000..c74470cdcfeca5192085fd752b3d2b9306e64873 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.project @@ -0,0 +1,26 @@ +<?xml version="1.0" encoding="UTF-8"?> +<projectDescription> + <name>libcaer</name> + <comment></comment> + <projects> + </projects> + <buildSpec> + <buildCommand> + <name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name> + <arguments> + </arguments> + </buildCommand> + <buildCommand> + <name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name> + <triggers>full,incremental,</triggers> + <arguments> + </arguments> + </buildCommand> + </buildSpec> + <natures> + <nature>org.eclipse.cdt.core.cnature</nature> + <nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature> + <nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature> + <nature>org.eclipse.cdt.codan.core.codanNature</nature> + </natures> +</projectDescription> diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.settings/language.settings.xml b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.settings/language.settings.xml new file mode 100755 index 0000000000000000000000000000000000000000..f5dd61dd9bdc79d20c46daa9f3dd097bb37ff426 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.settings/language.settings.xml @@ -0,0 +1,12 @@ +<?xml version="1.0" encoding="UTF-8" standalone="no"?> +<project> + <configuration id="cdt.managedbuild.toolchain.gnu.base.1733434747" name="Default"> + <extension point="org.eclipse.cdt.core.LanguageSettingsProvider"> + <provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> + <provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/> + <provider-reference id="org.eclipse.cdt.managedbuilder.core.GCCBuildCommandParser" ref="shared-provider"/> + <provider-reference id="org.eclipse.cdt.managedbuilder.core.GCCBuiltinSpecsDetector" ref="shared-provider"/> + <provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/> + </extension> + </configuration> +</project> diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.settings/org.eclipse.cdt.codan.core.prefs b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.settings/org.eclipse.cdt.codan.core.prefs new file mode 100755 index 0000000000000000000000000000000000000000..d2a09ffdd1bf058a5ef23d7623296ae42fedabe3 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/.settings/org.eclipse.cdt.codan.core.prefs @@ -0,0 +1,73 @@ +eclipse.preferences.version=1 +onBuild=true +org.eclipse.cdt.codan.checkers.errnoreturn=Warning +org.eclipse.cdt.codan.checkers.errnoreturn.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},implicit\=>false} +org.eclipse.cdt.codan.checkers.errreturnvalue=Error +org.eclipse.cdt.codan.checkers.errreturnvalue.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.checkers.nocommentinside=Error +org.eclipse.cdt.codan.checkers.nocommentinside.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.checkers.nolinecomment=-Error +org.eclipse.cdt.codan.checkers.nolinecomment.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.checkers.noreturn=Error +org.eclipse.cdt.codan.checkers.noreturn.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},implicit\=>false} +org.eclipse.cdt.codan.internal.checkers.AbstractClassCreation=Error +org.eclipse.cdt.codan.internal.checkers.AbstractClassCreation.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.AmbiguousProblem=Error +org.eclipse.cdt.codan.internal.checkers.AmbiguousProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.AssignmentInConditionProblem=Warning +org.eclipse.cdt.codan.internal.checkers.AssignmentInConditionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.AssignmentToItselfProblem=Error +org.eclipse.cdt.codan.internal.checkers.AssignmentToItselfProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.CaseBreakProblem=Warning +org.eclipse.cdt.codan.internal.checkers.CaseBreakProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},no_break_comment\=>"no break",last_case_param\=>false,empty_case_param\=>false} +org.eclipse.cdt.codan.internal.checkers.CatchByReference=Warning +org.eclipse.cdt.codan.internal.checkers.CatchByReference.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},unknown\=>false,exceptions\=>()} +org.eclipse.cdt.codan.internal.checkers.CircularReferenceProblem=Error +org.eclipse.cdt.codan.internal.checkers.CircularReferenceProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.ClassMembersInitialization=Warning +org.eclipse.cdt.codan.internal.checkers.ClassMembersInitialization.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},skip\=>true} +org.eclipse.cdt.codan.internal.checkers.FieldResolutionProblem=-Error +org.eclipse.cdt.codan.internal.checkers.FieldResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.FunctionResolutionProblem=Error +org.eclipse.cdt.codan.internal.checkers.FunctionResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.InvalidArguments=Error +org.eclipse.cdt.codan.internal.checkers.InvalidArguments.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.InvalidTemplateArgumentsProblem=Error +org.eclipse.cdt.codan.internal.checkers.InvalidTemplateArgumentsProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.LabelStatementNotFoundProblem=Error +org.eclipse.cdt.codan.internal.checkers.LabelStatementNotFoundProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.MemberDeclarationNotFoundProblem=Error +org.eclipse.cdt.codan.internal.checkers.MemberDeclarationNotFoundProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.MethodResolutionProblem=Error +org.eclipse.cdt.codan.internal.checkers.MethodResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.NamingConventionFunctionChecker=-Info +org.eclipse.cdt.codan.internal.checkers.NamingConventionFunctionChecker.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},pattern\=>"^[a-z]",macro\=>true,exceptions\=>()} +org.eclipse.cdt.codan.internal.checkers.NonVirtualDestructorProblem=Warning +org.eclipse.cdt.codan.internal.checkers.NonVirtualDestructorProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.OverloadProblem=Error +org.eclipse.cdt.codan.internal.checkers.OverloadProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.RedeclarationProblem=Error +org.eclipse.cdt.codan.internal.checkers.RedeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.RedefinitionProblem=Error +org.eclipse.cdt.codan.internal.checkers.RedefinitionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.ReturnStyleProblem=Warning +org.eclipse.cdt.codan.internal.checkers.ReturnStyleProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.ScanfFormatStringSecurityProblem=Warning +org.eclipse.cdt.codan.internal.checkers.ScanfFormatStringSecurityProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.StatementHasNoEffectProblem=Warning +org.eclipse.cdt.codan.internal.checkers.StatementHasNoEffectProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},macro\=>true,exceptions\=>()} +org.eclipse.cdt.codan.internal.checkers.SuggestedParenthesisProblem=Warning +org.eclipse.cdt.codan.internal.checkers.SuggestedParenthesisProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},paramNot\=>false} +org.eclipse.cdt.codan.internal.checkers.SuspiciousSemicolonProblem=Warning +org.eclipse.cdt.codan.internal.checkers.SuspiciousSemicolonProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},else\=>false,afterelse\=>false} +org.eclipse.cdt.codan.internal.checkers.TypeResolutionProblem=-Error +org.eclipse.cdt.codan.internal.checkers.TypeResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +org.eclipse.cdt.codan.internal.checkers.UnusedFunctionDeclarationProblem=Warning +org.eclipse.cdt.codan.internal.checkers.UnusedFunctionDeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},macro\=>true} +org.eclipse.cdt.codan.internal.checkers.UnusedStaticFunctionProblem=Warning +org.eclipse.cdt.codan.internal.checkers.UnusedStaticFunctionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},macro\=>true} +org.eclipse.cdt.codan.internal.checkers.UnusedVariableDeclarationProblem=Warning +org.eclipse.cdt.codan.internal.checkers.UnusedVariableDeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},macro\=>true,exceptions\=>("@(\#)","$Id")} +org.eclipse.cdt.codan.internal.checkers.VariableResolutionProblem=Error +org.eclipse.cdt.codan.internal.checkers.VariableResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}} +useParentScope=false diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/CMakeLists.txt b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/CMakeLists.txt new file mode 100755 index 0000000000000000000000000000000000000000..18ea7e6ab8876b11330a0331ae0d3f5dc436eb72 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/CMakeLists.txt @@ -0,0 +1,223 @@ +# Init CMake (require at least version 2.6) +CMAKE_MINIMUM_REQUIRED(VERSION 2.6) + +# General build settings +IF (NOT CMAKE_BUILD_TYPE) + SET(CMAKE_BUILD_TYPE "Release" CACHE STRING "Possible build types: None Debug Release RelWithDebInfo MinSizeRel") +ENDIF() + +IF (NOT ENABLE_OPENCV) + SET(ENABLE_OPENCV 0 CACHE BOOL "Enable support for frame enhancements using OpenCV") +ENDIF() + +# Project name and version +PROJECT(libcaer C CXX) +SET(PROJECT_VERSION_MAJOR 2) +SET(PROJECT_VERSION_MINOR 0) +SET(PROJECT_VERSION_PATCH 0) +SET(PROJECT_VERSION_NOREV ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}.${PROJECT_VERSION_PATCH}) +IF (NOT PROJECT_REVISION) + # EXECUTE_PROCESS(COMMAND svnversion OUTPUT_VARIABLE PROJECT_REVISION OUTPUT_STRIP_TRAILING_WHITESPACE) + EXECUTE_PROCESS(COMMAND git rev-parse HEAD OUTPUT_VARIABLE PROJECT_REVISION OUTPUT_STRIP_TRAILING_WHITESPACE) +ENDIF() +SET(PROJECT_VERSION ${PROJECT_VERSION_NOREV}-r${PROJECT_REVISION}) + +# Define installation paths. +INCLUDE(GNUInstallDirs) + +# Set compiler info +SET(CC_CLANG FALSE) +SET(CC_GCC FALSE) +SET(CC_ICC FALSE) +SET(CC_MSVC FALSE) + +IF ("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang") + SET(CC_CLANG TRUE) +ELSEIF ("${CMAKE_C_COMPILER_ID}" STREQUAL "GNU") + SET(CC_GCC TRUE) +ELSEIF ("${CMAKE_C_COMPILER_ID}" STREQUAL "Intel") + SET(CC_ICC TRUE) +ELSEIF ("${CMAKE_C_COMPILER_ID}" STREQUAL "MSVC") + SET(CC_MSVC TRUE) +ENDIF() + +# Set operating system info +SET(OS_UNIX FALSE) +SET(OS_LINUX FALSE) +SET(OS_MACOSX FALSE) +SET(OS_WINDOWS FALSE) + +IF (UNIX) + SET(OS_UNIX TRUE) + ADD_DEFINITIONS(-DOS_UNIX=1) +ENDIF() + +IF (UNIX AND "${CMAKE_SYSTEM_NAME}" MATCHES "Linux") + SET(OS_LINUX TRUE) + ADD_DEFINITIONS(-DOS_LINUX=1) +ENDIF() + +IF (UNIX AND APPLE AND "${CMAKE_SYSTEM_NAME}" MATCHES "Darwin") + SET(OS_MACOSX TRUE) + ADD_DEFINITIONS(-DOS_MACOSX=1) +ENDIF() + +IF (WIN32 AND "${CMAKE_SYSTEM_NAME}" MATCHES "Windows") + SET(OS_WINDOWS TRUE) + ADD_DEFINITIONS(-DOS_WINDOWS=1) +ENDIF() + +# Test if we are on a big-endian architecture +INCLUDE(TestBigEndian) +TEST_BIG_ENDIAN(SYSTEM_BIGENDIAN) + +# Check size of various types +INCLUDE(CheckTypeSize) +CHECK_TYPE_SIZE("size_t" SIZEOF_SIZE_T) +CHECK_TYPE_SIZE("void *" SIZEOF_VOID_PTR) + +IF (NOT "${SIZEOF_VOID_PTR}" STREQUAL "${SIZEOF_SIZE_T}") + MESSAGE(SEND_ERROR "Size of void * and size_t must be the same!") +ENDIF() + +# Check threads support (almost nobody implements C11 threads yet!) +FIND_PACKAGE(Threads) +SET(HAVE_PTHREADS FALSE) +SET(HAVE_WIN32_THREADS FALSE) + +IF (OS_UNIX AND DEFINED "CMAKE_USE_PTHREADS_INIT") + IF (${CMAKE_USE_PTHREADS_INIT}) + SET(HAVE_PTHREADS TRUE) + ADD_DEFINITIONS(-DHAVE_PTHREADS=1) + ENDIF() +ENDIF() + +IF (OS_WINDOWS AND DEFINED "CMAKE_USE_WIN32_THREADS_INIT") + IF (${CMAKE_USE_WIN32_THREADS_INIT}) + SET(HAVE_WIN32_THREADS TRUE) + ADD_DEFINITIONS(-DHAVE_WIN32_THREADS=1) + ENDIF() +ENDIF() + +# Add system defines for header features +IF (OS_UNIX AND HAVE_PTHREADS) + # POSIX system (Unix, Linux, MacOS X) + ADD_DEFINITIONS(-D_XOPEN_SOURCE=700) + ADD_DEFINITIONS(-D_DEFAULT_SOURCE=1) + + IF (OS_MACOSX) + ADD_DEFINITIONS(-D_DARWIN_C_SOURCE=1) + ENDIF() +ENDIF() + +# C11 standard needed (atomics, threads) +IF (CC_GCC OR CC_CLANG) + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +ENDIF() + +# Enable all warnings for GCC / Clang +IF (CC_GCC OR CC_CLANG) + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -pedantic -Wall -Wextra") + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pedantic -Wall -Wextra") + + IF (CC_GCC) + # Enable all useful warnings manually in GCC. + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wunused -Wundef -Wformat=2 -Winit-self -Wuninitialized") + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wpointer-arith -Wcast-qual -Wcast-align -Wwrite-strings") + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wstrict-prototypes -Wmissing-prototypes -Wredundant-decls") + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wmissing-declarations -Wnested-externs -Wstack-protector") + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wshadow -Wbad-function-cast -Wfloat-equal") + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wconversion -Wstrict-overflow=2") + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wjump-misses-init -Wunsuffixed-float-constants -Wdouble-promotion") + ENDIF() + + IF (CC_CLANG) + # Enable all warnings in Clang easily. + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Weverything -Wno-packed -Wno-padded -Wno-unreachable-code-break") + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-disabled-macro-expansion -Wno-reserved-id-macro -Wno-vla") + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-parentheses-equality -Wno-covered-switch-default") + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-used-but-marked-unused -Wno-cast-align") + ENDIF() +ENDIF() + +# Search for external libraries with pkg-config +INCLUDE(FindPkgConfig) + +# Basic USB device support +PKG_CHECK_MODULES(LIBUSB1 REQUIRED libusb-1.0>=1.0.17) +SET(LIBCAER_INCDIRS ${LIBUSB1_INCLUDE_DIRS}) +SET(LIBCAER_LIBDIRS ${LIBUSB1_LIBRARY_DIRS}) +SET(LIBCAER_LIBS ${LIBUSB1_LIBRARIES}) + +IF (ENABLE_OPENCV) + # Require new OpenCV 3.1 at least. + PKG_CHECK_MODULES(OPENCV3 REQUIRED opencv>=3.1) + + SET(LIBCAER_INCDIRS ${LIBCAER_INCDIRS} ${OPENCV3_INCLUDE_DIRS}) + SET(LIBCAER_LIBDIRS ${LIBCAER_LIBDIRS} ${OPENCV3_LIBRARY_DIRS}) + SET(LIBCAER_LIBS ${LIBCAER_LIBS} ${OPENCV3_LIBRARIES}) +ENDIF() + +# Threads support +SET(LIBCAER_LIBS ${LIBCAER_LIBS} ${CMAKE_THREAD_LIBS_INIT}) + +# Add local directory to include paths +SET(LIBCAER_INCDIRS ${LIBCAER_INCDIRS} ${CMAKE_SOURCE_DIR}/include/) + +INCLUDE_DIRECTORIES(${LIBCAER_INCDIRS}) +LINK_DIRECTORIES(${LIBCAER_LIBDIRS}) + +# Set full RPATH +SET(CMAKE_INSTALL_RPATH ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}) +SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) +IF (OS_MACOSX) + SET(CMAKE_MACOSX_RPATH TRUE) +ENDIF() + +# Subdirectories +ADD_SUBDIRECTORY(include) +ADD_SUBDIRECTORY(src) + +# Generate pkg-config file +FOREACH (LIB ${CMAKE_THREAD_LIBS_INIT}) + SET(PRIVATE_LIBS "${LIB} ${PRIVATE_LIBS}") +ENDFOREACH() + +IF (ENABLE_OPENCV) + # Use different pkg-config file to specify OpenCV support + CONFIGURE_FILE(libcaer_opencv.pc.in libcaer.pc @ONLY) +ELSE() + CONFIGURE_FILE(libcaer.pc.in libcaer.pc @ONLY) +ENDIF() + +INSTALL(FILES ${CMAKE_BINARY_DIR}/libcaer.pc DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig) + +# Automatic documentation generation support +FIND_PACKAGE(Doxygen) + +IF (DOXYGEN_FOUND) + ADD_SUBDIRECTORY(docs) +ENDIF() + +# Print info summary for debug purposes +MESSAGE(STATUS "Project version is: ${PROJECT_VERSION}") +MESSAGE(STATUS "Compiler is Clang: ${CC_CLANG}") +MESSAGE(STATUS "Compiler is GCC: ${CC_GCC}") +MESSAGE(STATUS "Compiler is IntelCC: ${CC_ICC}") +MESSAGE(STATUS "Compiler is MS VisualC: ${CC_MSVC}") +MESSAGE(STATUS "OS is Unix: ${OS_UNIX}") +MESSAGE(STATUS "OS is Linux: ${OS_LINUX}") +MESSAGE(STATUS "OS is MacOS X: ${OS_MACOSX}") +MESSAGE(STATUS "OS is Windows: ${OS_WINDOWS}") +MESSAGE(STATUS "System is big-endian: ${SYSTEM_BIGENDIAN}") +MESSAGE(STATUS "Thread support is PThreads: ${HAVE_PTHREADS}") +MESSAGE(STATUS "Thread support is Win32 Threads: ${HAVE_WIN32_THREADS}") +MESSAGE(STATUS "C flags are: ${CMAKE_C_FLAGS}") +MESSAGE(STATUS "CXX flags are: ${CMAKE_CXX_FLAGS}") +MESSAGE(STATUS "Include directories are: ${LIBCAER_INCDIRS}") +MESSAGE(STATUS "Library directories are: ${LIBCAER_LIBDIRS}") +MESSAGE(STATUS "Linked libraries are: ${LIBCAER_LIBS}") +MESSAGE(STATUS "Compiled source files are: ${LIBCAER_SRC_FILES}") +MESSAGE(STATUS "Final install libdir is: ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") +MESSAGE(STATUS "Final install includedir is: ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}/${CMAKE_PROJECT_NAME}") diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/COPYING b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/COPYING new file mode 100755 index 0000000000000000000000000000000000000000..f09927e6bed70b65257fc743369ba46b3f97f2ce --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/COPYING @@ -0,0 +1,23 @@ +Copyright (c) 2014-2015, Luca Longinotti, iniLabs Ltd. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/ChangeLog b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/ChangeLog new file mode 100755 index 0000000000000000000000000000000000000000..2b00a657ab7553a75c62e9b0c21b9ccde9a40b25 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/ChangeLog @@ -0,0 +1,95 @@ +Release 1.0.3 - 28.03.2016 +INCOMPATIBLE CHANGES +- davis_fx2.c: removed specific FX2 code, now uses same backend as FX3 + for sending all configuration values, due to new firmware version 3 + for FX2 DAVIS small-board cameras. A firmware update is required, + please follow the instructions at http://inilabs.com/support/reflashing/. + +NEW FEATURES +- frame.h: add function caerFrameEventGetPixelArrayCGFormat() to + Frame Event, allows to get a copy of the pixels array in the + standard computer graphics format (pixel (0, 0) in upper left + corner) efficently. +- davis.h: add DAVIS_CONFIG_APS_ADC_TEST_MODE parameter to APS module. + +Release 1.0.2 - 08.01.2016 +IMCOMPATIBLE CHANGES +- frame.h: rename channel number to color channels and add color + filter information, to better specify the information needed + to reconstruct color frames. The 'enum caer_frame_event_color_channels' + and 'enum caer_frame_event_color_filter' hold the possible values. + This change is backwards-compatible with older frame format! +- davis.h, dvs128.h: changed info data structure's integers to be + signed for better cross-language compatibility. Also 'deviceID' + can now be compared directly with 'sourceID' from event packets. +- davis.h: changed type of 'apsColorFilter' in info data structure + to take advantage of new 'enum caer_frame_event_color_filter'. + +NEW FEATURES +- common.h: add generic functions for event packet copying, with + support for packet size reductions (copy only valid). +- libcaer.h: add generic clear, get and set functions for bitfields, + and use them in all the event functions to avoid errors. +- config.h: add device configuration event type, for tracking + of device configuration changes in the event stream. +- log.c: add timezone information to log message time information. +- dvs128_simple.cpp, davis_simple.cpp: add C++11 example variants. +- Enable more Clang compiler warnings. + +BUG FIXES +- davis_common.c: only update the ROI size information if ROI + updates actually came in from the device. +- Fix and improve documentation, especially for the frame event. +- Fix off-by-one in event's GetEvent() function's warning log message. +- device.c: mark global function pointer arrays static. +- common.h, frame.h: fix compilation of inline functions in + strict C++11 mode. + + +Release 1.0.1 - 06.11.2015 +INCOMPATIBLE CHANGES +- Requires firmware version 2 and logic revision 7449. +- usb.h: improved caerDeviceDataStart() API to also allow for + notification of data acquisition shutdown, to be able to react + to abnormal shutdown cases, like when a device is unplugged. +- frame.h: changed in-memory format for easier handling and compatibility + with the other event formats (all-in-one memory block). +- davis.h: rename DAVIS240 APSOVERFLOWLEVEL to APSOVERFLOWLEVELBN. + +NEW FEATURES +- Headers are C++ compatible and stand-alone now. +- MacOS X support. +- Added pkg-config file for library. +- Full API documentation (see docs/ for PDF). +- Various cmake/build improvements. Support out-of-tree builds. +- Add iterator macros for EventPackets. +- frame.h: added ROI region ID tracking, as well as ROI position. + Added caerFrameEventGetExposureLength() and caerFrameEventGetTimestamp(), + which is defined as the median of the exposure times. + Added caerFrameEventPacketGetPixelsSize() and caerFrameEventPacketGetPixelsMaxIndex() + for EventPacket-level size information. + Added caerFrameEventGetPixelsSize() and caerFrameEventGetPixelsMaxIndex() + for Event-level size information. +- log.h: added caerLogFileDescriptorsSet() to allow logging to + up to two different file descriptors (defaults to STDERR only). +- davis.h: added chipID check macros. +- davis.h: added DAVIS_CONFIG_APS_SNAPSHOT to take one frame snapshots. +- dvs128.h: added DVS128_CONFIG_DVS_TS_MASTER to select timestamp + master or slave behavior (timestamp synchronization). +- davis_common.c: added support for outputting only reset read or + signal read frames, for debugging purposes. +- c11threads_posix.h: C11-compatible thread abstraction, for + both Linux and MacOS X, based on POSIX Threads. + +BUG FIXES +- Relaxed atomic operations memory constraints for better + performance on weakly-ordered architectures (like ARM). +- log.c: call tzset() before localtime_r(), as per standard. +- davis_common.c: improved Region of Interest support (APS). +- davis_fx2.c: keep GlobalShutter flag correctly in-sync. +- davis_fx3.c: fixed support for new FX3 devices. +- packetContainer.h: handle container being NULL by returning NULL. + + +Release 1.0.0 - 02.10.2015 +- Initial release. diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/README b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/README new file mode 100755 index 0000000000000000000000000000000000000000..5d0931029d853df9da2ed00f5b868b5dc571517b --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/README @@ -0,0 +1,56 @@ +libcaer +==== + +Minimal C library to access, configure and get data out of AER sensors, +such as the Dynamic Vision Sensor (DVS) or DAVIS cameras. + +REQUIREMENTS: + +cmake >= 2.6 +gcc >= 4.9 or clang >= 3.6 +libusb >= 1.0.17 + +Please make sure that you have the various development packages installed +for the above dependencies. They are usually called PKG-dev or PKG-devel. + +INSTALLATION: + +1) configure: + +$ cmake -DCMAKE_INSTALL_PREFIX=/usr . + +2) build: + +$ make + +3) install: + +$ make install + +DOCUMENTATION: + +The API documentation for a release can be found at docs/libcaer_api_manual.pdf. +Also check the examples/ directory and the iniLabs Support website. + +For the development tree, you can generate the documentation using: + +$ make doc - to generate docs/latex/ and docs/html/ documentation files. +$ make pdf - to generate a PDF from the LaTeX sources at docs/latex/refman.pdf. + +USAGE: + +See examples/ directory. Usual usage is (simplified): + +h = caerDeviceOpen(TYPE); +caerDeviceSendDefaultConfig(h); +caerDeviceDataStart(h); + +loop: + c = caerDeviceDataGet(h); + work with c (container) and its event packets + +caerDeviceDataStop(h); +caerDeviceClose(&h); + +All configuration parameters and event types are specified in the +public headers and documented there. diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/docs/.gitignore b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/docs/.gitignore new file mode 100755 index 0000000000000000000000000000000000000000..f5133a9754202a173c4d4d24bdecccc16506cef5 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/docs/.gitignore @@ -0,0 +1,3 @@ +/libcaer_api.doxy +/html/ +/latex/ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/docs/CMakeLists.txt b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/docs/CMakeLists.txt new file mode 100755 index 0000000000000000000000000000000000000000..2ad5c8a96a58c992c8abe9418314acab8a8f1ff1 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/docs/CMakeLists.txt @@ -0,0 +1,48 @@ +IF (NOT DOXYGEN_FOUND) + # Only support call from main cmake. + RETURN() +ENDIF() + +# Doxygen directories and configuration. HTML and Latex are generated. +SET(DOXYGEN_OUTPUT_DIR ${CMAKE_BINARY_DIR}/docs/) +SET(DOXYGEN_CONFIG ${DOXYGEN_OUTPUT_DIR}/libcaer_api.doxy) +SET(DOXYGEN_LATEX_OUTPUT_DIR ${DOXYGEN_OUTPUT_DIR}/latex/) +SET(DOXYGEN_LATEX_OUTPUT ${DOXYGEN_LATEX_OUTPUT_DIR}/refman.tex) +SET(DOXYGEN_HTML_OUTPUT_DIR ${DOXYGEN_OUTPUT_DIR}/html/) +SET(DOXYGEN_HTML_OUTPUT ${DOXYGEN_HTML_OUTPUT_DIR}/index.html) + +CONFIGURE_FILE(libcaer_api.doxy.in libcaer_api.doxy @ONLY) + +ADD_CUSTOM_COMMAND( + OUTPUT ${DOXYGEN_LATEX_OUTPUT} ${DOXYGEN_HTML_OUTPUT} + COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_CONFIG} + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/include/ + DEPENDS ${DOXYGEN_CONFIG}) + +# Generate documentation with 'make doc'. +ADD_CUSTOM_TARGET(doc DEPENDS ${DOXYGEN_LATEX_OUTPUT} ${DOXYGEN_HTML_OUTPUT}) + +# Automatically generate PDF from Latex. +FIND_PACKAGE(LATEX) + +IF (NOT LATEX_COMPILER) + MESSAGE(STATUS "latex command LATEX_COMPILER not found, but usually required by Doxygen.") +ENDIF() + +IF (NOT MAKEINDEX_COMPILER) + MESSAGE(STATUS "makeindex command MAKEINDEX_COMPILER not found, but usually required by Doxygen.") +ENDIF() + +IF (NOT DVIPS_CONVERTER) + MESSAGE(STATUS "dvips command DVIPS_CONVERTER not found, but usually required by Doxygen.") +ENDIF() + +# Generate PDF documentation. +ADD_CUSTOM_COMMAND( + OUTPUT ${DOXYGEN_LATEX_OUTPUT_DIR}/refman.pdf + COMMAND make + WORKING_DIRECTORY ${DOXYGEN_LATEX_OUTPUT_DIR} + DEPENDS ${DOXYGEN_LATEX_OUTPUT}) + +# Generate PDF from Latex with 'make pdf'. +ADD_CUSTOM_TARGET(pdf DEPENDS ${DOXYGEN_LATEX_OUTPUT_DIR}/refman.pdf) diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/docs/libcaer_api.doxy.in b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/docs/libcaer_api.doxy.in new file mode 100755 index 0000000000000000000000000000000000000000..df70e7b8048e7dafda678d0afd96e15508bfd5bb --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/docs/libcaer_api.doxy.in @@ -0,0 +1,2362 @@ +# Doxyfile 1.8.9.1 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a double hash (##) is considered a comment and is placed in +# front of the TAG it is preceding. +# +# All text after a single hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists, items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (\" \"). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all text +# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv +# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv +# for the list of possible encodings. +# The default value is: UTF-8. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by +# double-quotes, unless you are using Doxywizard) that should identify the +# project for which the documentation is generated. This name is used in the +# title of most generated pages and in a few other places. +# The default value is: My Project. + +PROJECT_NAME = "@PROJECT_NAME@" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. This +# could be handy for archiving the generated documentation or if some version +# control system is used. + +PROJECT_NUMBER = "@PROJECT_VERSION@" + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer a +# quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = + +# With the PROJECT_LOGO tag one can specify a logo or an icon that is included +# in the documentation. The maximum height of the logo should not exceed 55 +# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy +# the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path +# into which the generated documentation will be written. If a relative path is +# entered, it will be relative to the location where doxygen was started. If +# left blank the current directory will be used. + +OUTPUT_DIRECTORY = @DOXYGEN_OUTPUT_DIR@ + +# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub- +# directories (in 2 levels) under the output directory of each output format and +# will distribute the generated files over these directories. Enabling this +# option can be useful when feeding doxygen a huge amount of source files, where +# putting all generated files in the same directory would otherwise causes +# performance problems for the file system. +# The default value is: NO. + +CREATE_SUBDIRS = NO + +# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII +# characters to appear in the names of generated files. If set to NO, non-ASCII +# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode +# U+3044. +# The default value is: NO. + +ALLOW_UNICODE_NAMES = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese, +# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States), +# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian, +# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages), +# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian, +# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian, +# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish, +# Ukrainian and Vietnamese. +# The default value is: English. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member +# descriptions after the members that are listed in the file and class +# documentation (similar to Javadoc). Set to NO to disable this. +# The default value is: YES. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief +# description of a member or function before the detailed description +# +# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. +# The default value is: YES. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator that is +# used to form the text in various listings. Each string in this list, if found +# as the leading text of the brief description, will be stripped from the text +# and the result, after processing the whole list, is used as the annotated +# text. Otherwise, the brief description is used as-is. If left blank, the +# following values are used ($name is automatically replaced with the name of +# the entity):The $name class, The $name widget, The $name file, is, provides, +# specifies, contains, represents, a, an and the. + +ABBREVIATE_BRIEF = + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# doxygen will generate a detailed section even if there is only a brief +# description. +# The default value is: NO. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. +# The default value is: NO. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path +# before files name in the file list and in the header files. If set to NO the +# shortest path that makes the file name unique will be used +# The default value is: YES. + +FULL_PATH_NAMES = YES + +# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. +# Stripping is only done if one of the specified strings matches the left-hand +# part of the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the path to +# strip. +# +# Note that you can specify absolute paths here, but also relative paths, which +# will be relative from the directory where doxygen is started. +# This tag requires that the tag FULL_PATH_NAMES is set to YES. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the +# path mentioned in the documentation of a class, which tells the reader which +# header file to include in order to use a class. If left blank only the name of +# the header file containing the class definition is used. Otherwise one should +# specify the list of include paths that are normally passed to the compiler +# using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but +# less readable) file names. This can be useful is your file systems doesn't +# support long names like on DOS, Mac, or CD-ROM. +# The default value is: NO. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the +# first line (until the first dot) of a Javadoc-style comment as the brief +# description. If set to NO, the Javadoc-style will behave just like regular Qt- +# style comments (thus requiring an explicit @brief command for a brief +# description.) +# The default value is: NO. + +JAVADOC_AUTOBRIEF = NO + +# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first +# line (until the first dot) of a Qt-style comment as the brief description. If +# set to NO, the Qt-style will behave just like regular Qt-style comments (thus +# requiring an explicit \brief command for a brief description.) +# The default value is: NO. + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a +# multi-line C++ special comment block (i.e. a block of //! or /// comments) as +# a brief description. This used to be the default behavior. The new default is +# to treat a multi-line C++ comment block as a detailed description. Set this +# tag to YES if you prefer the old behavior instead. +# +# Note that setting this tag to YES also means that rational rose comments are +# not recognized any more. +# The default value is: NO. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the +# documentation from any documented member that it re-implements. +# The default value is: YES. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new +# page for each member. If set to NO, the documentation of a member will be part +# of the file/class/namespace that contains it. +# The default value is: NO. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen +# uses this value to replace tabs by spaces in code fragments. +# Minimum value: 1, maximum value: 16, default value: 4. + +TAB_SIZE = 4 + +# This tag can be used to specify a number of aliases that act as commands in +# the documentation. An alias has the form: +# name=value +# For example adding +# "sideeffect=@par Side Effects:\n" +# will allow you to put the command \sideeffect (or @sideeffect) in the +# documentation, which will result in a user-defined paragraph with heading +# "Side Effects:". You can put \n's in the value part of an alias to insert +# newlines. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding "class=itcl::class" +# will allow you to use the command class in the itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources +# only. Doxygen will then generate output that is more tailored for C. For +# instance, some of the names that are used will be different. The list of all +# members will be omitted, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_FOR_C = YES + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or +# Python sources only. Doxygen will then generate output that is more tailored +# for that language. For instance, namespaces will be presented as packages, +# qualified scopes will look different, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources. Doxygen will then generate output that is tailored for Fortran. +# The default value is: NO. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for VHDL. +# The default value is: NO. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given +# extension. Doxygen has a built-in mapping, but you can override or extend it +# using this tag. The format is ext=language, where ext is a file extension, and +# language is one of the parsers supported by doxygen: IDL, Java, Javascript, +# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran: +# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran: +# Fortran. In the later case the parser tries to guess whether the code is fixed +# or free formatted code, this is the default for Fortran type files), VHDL. For +# instance to make doxygen treat .inc files as Fortran files (default is PHP), +# and .f files as C (default is Fortran), use: inc=Fortran f=C. +# +# Note: For files without extension you can use no_extension as a placeholder. +# +# Note that for custom extensions you also need to set FILE_PATTERNS otherwise +# the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments +# according to the Markdown format, which allows for more readable +# documentation. See http://daringfireball.net/projects/markdown/ for details. +# The output of markdown processing is further processed by doxygen, so you can +# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in +# case of backward compatibilities issues. +# The default value is: YES. + +MARKDOWN_SUPPORT = YES + +# When enabled doxygen tries to link words that correspond to documented +# classes, or namespaces to their corresponding documentation. Such a link can +# be prevented in individual cases by putting a % sign in front of the word or +# globally by setting AUTOLINK_SUPPORT to NO. +# The default value is: YES. + +AUTOLINK_SUPPORT = YES + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should set this +# tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); +# versus func(std::string) {}). This also make the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. +# The default value is: NO. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. +# The default value is: NO. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: +# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen +# will parse them like normal C++ but will assume all classes use public instead +# of private inheritance when no explicit protection keyword is present. +# The default value is: NO. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate +# getter and setter methods for a property. Setting this option to YES will make +# doxygen to replace the get and set methods by a property in the documentation. +# This will only work if the methods are indeed getting or setting a simple +# type. If this is not the case, or you want to show the methods anyway, you +# should set this option to NO. +# The default value is: YES. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. +# The default value is: NO. + +DISTRIBUTE_GROUP_DOC = YES + +# Set the SUBGROUPING tag to YES to allow class member groups of the same type +# (for instance a group of public functions) to be put as a subgroup of that +# type (e.g. under the Public Functions section). Set it to NO to prevent +# subgrouping. Alternatively, this can be done per class using the +# \nosubgrouping command. +# The default value is: YES. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions +# are shown inside the group in which they are included (e.g. using \ingroup) +# instead of on a separate page (for HTML and Man pages) or section (for LaTeX +# and RTF). +# +# Note that this feature does not work in combination with +# SEPARATE_MEMBER_PAGES. +# The default value is: NO. + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions +# with only public data fields or simple typedef fields will be shown inline in +# the documentation of the scope in which they are defined (i.e. file, +# namespace, or group documentation), provided this scope is documented. If set +# to NO, structs, classes, and unions are shown on a separate page (for HTML and +# Man pages) or section (for LaTeX and RTF). +# The default value is: NO. + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or +# enum is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically be +# useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. +# The default value is: NO. + +TYPEDEF_HIDES_STRUCT = NO + +# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This +# cache is used to resolve symbols given their name and scope. Since this can be +# an expensive process and often the same symbol appears multiple times in the +# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small +# doxygen will become slower. If the cache is too large, memory is wasted. The +# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range +# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 +# symbols. At the end of a run doxygen will report the cache usage and suggest +# the optimal cache size from a speed point of view. +# Minimum value: 0, maximum value: 9, default value: 0. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in +# documentation are documented, even if no documentation was available. Private +# class members and static file members will be hidden unless the +# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. +# Note: This will also disable the warnings about undocumented members that are +# normally produced when WARNINGS is set to YES. +# The default value is: NO. + +EXTRACT_ALL = NO + +# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will +# be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIVATE = NO + +# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal +# scope will be included in the documentation. +# The default value is: NO. + +EXTRACT_PACKAGE = NO + +# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be +# included in the documentation. +# The default value is: NO. + +EXTRACT_STATIC = YES + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined +# locally in source files will be included in the documentation. If set to NO, +# only classes defined in header files are included. Does not have any effect +# for Java sources. +# The default value is: YES. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. If set to YES, local methods, +# which are defined in the implementation section but not in the interface are +# included in the documentation. If set to NO, only methods in the interface are +# included. +# The default value is: NO. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base name of +# the file that contains the anonymous namespace. By default anonymous namespace +# are hidden. +# The default value is: NO. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all +# undocumented members inside documented classes or files. If set to NO these +# members will be included in the various overviews, but no documentation +# section is generated. This option has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. If set +# to NO, these classes will be included in the various overviews. This option +# has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend +# (class|struct|union) declarations. If set to NO, these declarations will be +# included in the documentation. +# The default value is: NO. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any +# documentation blocks found inside the body of a function. If set to NO, these +# blocks will be appended to the function's detailed documentation block. +# The default value is: NO. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation that is typed after a +# \internal command is included. If the tag is set to NO then the documentation +# will be excluded. Set it to YES to include the internal documentation. +# The default value is: NO. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file +# names in lower-case letters. If set to YES, upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. +# The default value is: system dependent. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with +# their full class and namespace scopes in the documentation. If set to YES, the +# scope will be hidden. +# The default value is: NO. + +HIDE_SCOPE_NAMES = NO + +# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will +# append additional text to a page's title, such as Class Reference. If set to +# YES the compound reference will be hidden. +# The default value is: NO. + +HIDE_COMPOUND_REFERENCE= NO + +# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of +# the files that are included by a file in the documentation of that file. +# The default value is: YES. + +SHOW_INCLUDE_FILES = YES + +# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each +# grouped member an include statement to the documentation, telling the reader +# which file to include in order to use the member. +# The default value is: NO. + +SHOW_GROUPED_MEMB_INC = NO + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include +# files with double quotes in the documentation rather than with sharp brackets. +# The default value is: NO. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the +# documentation for inline members. +# The default value is: YES. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the +# (detailed) documentation of file and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. +# The default value is: YES. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief +# descriptions of file, namespace and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. Note that +# this will also influence the order of the classes in the class list. +# The default value is: NO. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the +# (brief and detailed) documentation of class members so that constructors and +# destructors are listed first. If set to NO the constructors will appear in the +# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. +# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief +# member documentation. +# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting +# detailed member documentation. +# The default value is: NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy +# of group names into alphabetical order. If set to NO the group names will +# appear in their defined order. +# The default value is: NO. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by +# fully-qualified names, including namespaces. If set to NO, the class list will +# be sorted only by class name, not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the alphabetical +# list. +# The default value is: NO. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper +# type resolution of all parameters of a function it will reject a match between +# the prototype and the implementation of a member function even if there is +# only one candidate or it is obvious which candidate to choose by doing a +# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still +# accept a match between prototype and implementation in such cases. +# The default value is: NO. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo +# list. This list is created by putting \todo commands in the documentation. +# The default value is: YES. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test +# list. This list is created by putting \test commands in the documentation. +# The default value is: YES. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug +# list. This list is created by putting \bug commands in the documentation. +# The default value is: YES. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO) +# the deprecated list. This list is created by putting \deprecated commands in +# the documentation. +# The default value is: YES. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional documentation +# sections, marked by \if <section_label> ... \endif and \cond <section_label> +# ... \endcond blocks. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the +# initial value of a variable or macro / define can have for it to appear in the +# documentation. If the initializer consists of more lines than specified here +# it will be hidden. Use a value of 0 to hide initializers completely. The +# appearance of the value of individual variables and macros / defines can be +# controlled using \showinitializer or \hideinitializer command in the +# documentation regardless of this setting. +# Minimum value: 0, maximum value: 10000, default value: 30. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at +# the bottom of the documentation of classes and structs. If set to YES, the +# list will mention the files that were used to generate the documentation. +# The default value is: YES. + +SHOW_USED_FILES = YES + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This +# will remove the Files entry from the Quick Index and from the Folder Tree View +# (if specified). +# The default value is: YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces +# page. This will remove the Namespaces entry from the Quick Index and from the +# Folder Tree View (if specified). +# The default value is: YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command command input-file, where command is the value of the +# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided +# by doxygen. Whatever the program writes to standard output is used as the file +# version. For an example see the documentation. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. To create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. You can +# optionally specify a file name after the option, if omitted DoxygenLayout.xml +# will be used as the name of the layout file. +# +# Note that if you run doxygen from a directory containing a file called +# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE +# tag is left empty. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files containing +# the reference definitions. This must be a list of .bib files. The .bib +# extension is automatically appended if omitted. This requires the bibtex tool +# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info. +# For LaTeX the style of the bibliography can be controlled using +# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the +# search path. See also \cite for info how to create references. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# Configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated to +# standard output by doxygen. If QUIET is set to YES this implies that the +# messages are off. +# The default value is: NO. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES +# this implies that the warnings are on. +# +# Tip: Turn warnings on while writing the documentation. +# The default value is: YES. + +WARNINGS = YES + +# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate +# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag +# will automatically be disabled. +# The default value is: YES. + +WARN_IF_UNDOCUMENTED = YES + +# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some parameters +# in a documented function, or documenting parameters that don't exist or using +# markup commands wrongly. +# The default value is: YES. + +WARN_IF_DOC_ERROR = YES + +# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that +# are documented, but have no documentation for their parameters or return +# value. If set to NO, doxygen will only warn about wrong or incomplete +# parameter documentation, but not about the absence of documentation. +# The default value is: NO. + +WARN_NO_PARAMDOC = NO + +# The WARN_FORMAT tag determines the format of the warning messages that doxygen +# can produce. The string should contain the $file, $line, and $text tags, which +# will be replaced by the file and line number from which the warning originated +# and the warning text. Optionally the format may contain $version, which will +# be replaced by the version of the file (if it could be obtained via +# FILE_VERSION_FILTER) +# The default value is: $file:$line: $text. + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning and error +# messages should be written. If left blank the output is written to standard +# error (stderr). + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# Configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag is used to specify the files and/or directories that contain +# documented source files. You may enter file names like myfile.cpp or +# directories like /usr/src/myproject. Separate the files or directories with +# spaces. +# Note: If this tag is empty the current directory is searched. + +INPUT = + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses +# libiconv (or the iconv built into libc) for the transcoding. See the libiconv +# documentation (see: http://www.gnu.org/software/libiconv) for the list of +# possible encodings. +# The default value is: UTF-8. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank the +# following patterns are tested:*.c, *.cc, *.cxx, *.cpp, *.c++, *.java, *.ii, +# *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp, +# *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown, +# *.md, *.mm, *.dox, *.py, *.f90, *.f, *.for, *.tcl, *.vhd, *.vhdl, *.ucf, +# *.qsf, *.as and *.js. + +FILE_PATTERNS = + +# The RECURSIVE tag can be used to specify whether or not subdirectories should +# be searched for input files as well. +# The default value is: NO. + +RECURSIVE = YES + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. +# The default value is: NO. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories use the pattern */test/* + +EXCLUDE_SYMBOLS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or directories +# that contain example code fragments that are included (see the \include +# command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank all +# files are included. + +EXAMPLE_PATTERNS = + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude commands +# irrespective of the value of the RECURSIVE tag. +# The default value is: NO. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or directories +# that contain images that are to be included in the documentation (see the +# \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command: +# +# <filter> <input-file> +# +# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the +# name of an input file. Doxygen will then use the output that the filter +# program writes to standard output. If FILTER_PATTERNS is specified, this tag +# will be ignored. +# +# Note that the filter must not add or remove lines; it is applied before the +# code is scanned, but not when the output code is generated. If lines are added +# or removed, the anchors will not be placed correctly. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: pattern=filter +# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how +# filters are used. If the FILTER_PATTERNS tag is empty or if none of the +# patterns match the file name, INPUT_FILTER is applied. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will also be used to filter the input files that are used for +# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). +# The default value is: NO. + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and +# it is also possible to disable source filtering for a specific pattern using +# *.ext= (so without naming a filter). +# This tag requires that the tag FILTER_SOURCE_FILES is set to YES. + +FILTER_SOURCE_PATTERNS = + +# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that +# is part of the input, its contents will be placed on the main page +# (index.html). This can be useful if you have a project on for instance GitHub +# and want to reuse the introduction page also for the doxygen output. + +USE_MDFILE_AS_MAINPAGE = + +#--------------------------------------------------------------------------- +# Configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will be +# generated. Documented entities will be cross-referenced with these sources. +# +# Note: To get rid of all source code in the generated output, make sure that +# also VERBATIM_HEADERS is set to NO. +# The default value is: NO. + +SOURCE_BROWSER = NO + +# Setting the INLINE_SOURCES tag to YES will include the body of functions, +# classes and enums directly into the documentation. +# The default value is: NO. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any +# special comment blocks from generated source code fragments. Normal C, C++ and +# Fortran comments will always remain visible. +# The default value is: YES. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES then for each documented +# function all documented functions referencing it will be listed. +# The default value is: NO. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES then for each documented function +# all documented entities called/used by that function will be listed. +# The default value is: NO. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set +# to YES then the hyperlinks from functions in REFERENCES_RELATION and +# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will +# link to the documentation. +# The default value is: YES. + +REFERENCES_LINK_SOURCE = YES + +# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the +# source code will show a tooltip with additional information such as prototype, +# brief description and links to the definition and documentation. Since this +# will make the HTML file larger and loading of large files a bit slower, you +# can opt to disable this feature. +# The default value is: YES. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +SOURCE_TOOLTIPS = YES + +# If the USE_HTAGS tag is set to YES then the references to source code will +# point to the HTML generated by the htags(1) tool instead of doxygen built-in +# source browser. The htags tool is part of GNU's global source tagging system +# (see http://www.gnu.org/software/global/global.html). You will need version +# 4.8.6 or higher. +# +# To use it do the following: +# - Install the latest version of global +# - Enable SOURCE_BROWSER and USE_HTAGS in the config file +# - Make sure the INPUT points to the root of the source tree +# - Run doxygen as normal +# +# Doxygen will invoke htags (and that will in turn invoke gtags), so these +# tools must be available from the command line (i.e. in the search path). +# +# The result: instead of the source browser generated by doxygen, the links to +# source code will now point to the output of htags. +# The default value is: NO. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a +# verbatim copy of the header file for each class for which an include is +# specified. Set to NO to disable this. +# See also: Section \class. +# The default value is: YES. + +VERBATIM_HEADERS = YES + +#--------------------------------------------------------------------------- +# Configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all +# compounds will be generated. Enable this if the project contains a lot of +# classes, structs, unions or interfaces. +# The default value is: YES. + +ALPHABETICAL_INDEX = YES + +# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in +# which the alphabetical index list will be split. +# Minimum value: 1, maximum value: 20, default value: 5. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all classes will +# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag +# can be used to specify a prefix (or a list of prefixes) that should be ignored +# while generating the index headers. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output +# The default value is: NO. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each +# generated HTML page (for example: .htm, .php, .asp). +# The default value is: .html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a user-defined HTML header file for +# each generated HTML page. If the tag is left blank doxygen will generate a +# standard header. +# +# To get valid HTML the header file that includes any scripts and style sheets +# that doxygen needs, which is dependent on the configuration options used (e.g. +# the setting GENERATE_TREEVIEW). It is highly recommended to start with a +# default header using +# doxygen -w html new_header.html new_footer.html new_stylesheet.css +# YourConfigFile +# and then modify the file new_header.html. See also section "Doxygen usage" +# for information on how to generate the default header that doxygen normally +# uses. +# Note: The header is subject to change so you typically have to regenerate the +# default header when upgrading to a newer version of doxygen. For a description +# of the possible markers and block names see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each +# generated HTML page. If the tag is left blank doxygen will generate a standard +# footer. See HTML_HEADER for more information on how to generate a default +# footer and what special commands can be used inside the footer. See also +# section "Doxygen usage" for information on how to generate the default footer +# that doxygen normally uses. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style +# sheet that is used by each HTML page. It can be used to fine-tune the look of +# the HTML output. If left blank doxygen will generate a default style sheet. +# See also section "Doxygen usage" for information on how to generate the style +# sheet that doxygen normally uses. +# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as +# it is more robust and this tag (HTML_STYLESHEET) will in the future become +# obsolete. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_STYLESHEET = + +# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined +# cascading style sheets that are included after the standard style sheets +# created by doxygen. Using this option one can overrule certain style aspects. +# This is preferred over using HTML_STYLESHEET since it does not replace the +# standard style sheet and is therefore more robust against future updates. +# Doxygen will copy the style sheet files to the output directory. +# Note: The order of the extra style sheet files is of importance (e.g. the last +# style sheet in the list overrules the setting of the previous ones in the +# list). For an example see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that the +# files will be copied as-is; there are no commands or markers available. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen +# will adjust the colors in the style sheet and background images according to +# this color. Hue is specified as an angle on a colorwheel, see +# http://en.wikipedia.org/wiki/Hue for more information. For instance the value +# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 +# purple, and 360 is red again. +# Minimum value: 0, maximum value: 359, default value: 220. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors +# in the HTML output. For a value of 0 the output will use grayscales only. A +# value of 255 will produce the most vivid colors. +# Minimum value: 0, maximum value: 255, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the +# luminance component of the colors in the HTML output. Values below 100 +# gradually make the output lighter, whereas values above 100 make the output +# darker. The value divided by 100 is the actual gamma applied, so 80 represents +# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not +# change the gamma. +# Minimum value: 40, maximum value: 240, default value: 80. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting this +# to NO can help when comparing the output of multiple runs. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_TIMESTAMP = NO + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_SECTIONS = NO + +# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries +# shown in the various tree structured indices initially; the user can expand +# and collapse entries dynamically later on. Doxygen will expand the tree to +# such a level that at most the specified number of entries are visible (unless +# a fully collapsed tree already exceeds this amount). So setting the number of +# entries 1 will produce a full collapsed tree by default. 0 is a special value +# representing an infinite number of entries and will result in a full expanded +# tree by default. +# Minimum value: 0, maximum value: 9999, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_INDEX_NUM_ENTRIES = 100 + +# If the GENERATE_DOCSET tag is set to YES, additional index files will be +# generated that can be used as input for Apple's Xcode 3 integrated development +# environment (see: http://developer.apple.com/tools/xcode/), introduced with +# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a +# Makefile in the HTML output directory. Running make will produce the docset in +# that directory and running make install will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at +# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_DOCSET = NO + +# This tag determines the name of the docset feed. A documentation feed provides +# an umbrella under which multiple documentation sets from a single provider +# (such as a company or product suite) can be grouped. +# The default value is: Doxygen generated docs. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# This tag specifies a string that should uniquely identify the documentation +# set bundle. This should be a reverse domain-name style string, e.g. +# com.mycompany.MyDocSet. Doxygen will append .docset to the name. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. +# The default value is: org.doxygen.Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher. +# The default value is: Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three +# additional HTML index files: index.hhp, index.hhc, and index.hhk. The +# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop +# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on +# Windows. +# +# The HTML Help Workshop contains a compiler that can convert all HTML output +# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML +# files are now used as the Windows 98 help format, and will replace the old +# Windows help format (.hlp) on all Windows platforms in the future. Compressed +# HTML files also contain an index, a table of contents, and you can search for +# words in the documentation. The HTML workshop also contains a viewer for +# compressed HTML files. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_HTMLHELP = NO + +# The CHM_FILE tag can be used to specify the file name of the resulting .chm +# file. You can add a path in front of the file if the result should not be +# written to the html output directory. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_FILE = + +# The HHC_LOCATION tag can be used to specify the location (absolute path +# including file name) of the HTML help compiler (hhc.exe). If non-empty, +# doxygen will try to run the HTML help compiler on the generated index.hhp. +# The file has to be specified with full path. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +HHC_LOCATION = + +# The GENERATE_CHI flag controls if a separate .chi index file is generated +# (YES) or that it should be included in the master .chm file (NO). +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +GENERATE_CHI = NO + +# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc) +# and project file content. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_INDEX_ENCODING = + +# The BINARY_TOC flag controls whether a binary table of contents is generated +# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it +# enables the Previous and Next buttons. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members to +# the table of contents of the HTML help documentation and to the tree view. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that +# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help +# (.qch) of the generated HTML documentation. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify +# the file name of the resulting .qch file. The path specified is relative to +# the HTML output folder. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help +# Project output. For more information please see Qt Help Project / Namespace +# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace). +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt +# Help Project output. For more information please see Qt Help Project / Virtual +# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual- +# folders). +# The default value is: doc. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_VIRTUAL_FOLDER = doc + +# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom +# filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's filter section matches. Qt Help Project / Filter Attributes (see: +# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_SECT_FILTER_ATTRS = + +# The QHG_LOCATION tag can be used to specify the location of Qt's +# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the +# generated .qhp file. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be +# generated, together with the HTML files, they form an Eclipse help plugin. To +# install this plugin and make it available under the help contents menu in +# Eclipse, the contents of the directory containing the HTML and XML files needs +# to be copied into the plugins directory of eclipse. The name of the directory +# within the plugins directory should be the same as the ECLIPSE_DOC_ID value. +# After copying Eclipse needs to be restarted before the help appears. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the Eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have this +# name. Each documentation set should have its own identifier. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# If you want full control over the layout of the generated HTML pages it might +# be necessary to disable the index and replace it with your own. The +# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top +# of each HTML page. A value of NO enables the index and the value YES disables +# it. Since the tabs in the index contain the same information as the navigation +# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. If the tag +# value is set to YES, a side panel will be generated containing a tree-like +# index structure (just like the one that is generated for HTML Help). For this +# to work a browser that supports JavaScript, DHTML, CSS and frames is required +# (i.e. any modern browser). Windows users are probably better off using the +# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can +# further fine-tune the look of the index. As an example, the default style +# sheet generated by doxygen has an example that shows how to put an image at +# the root of the tree instead of the PROJECT_NAME. Since the tree basically has +# the same information as the tab index, you could consider setting +# DISABLE_INDEX to YES when enabling this option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_TREEVIEW = NO + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that +# doxygen will group on one line in the generated HTML documentation. +# +# Note that a value of 0 will completely suppress the enum values from appearing +# in the overview section. +# Minimum value: 0, maximum value: 20, default value: 4. +# This tag requires that the tag GENERATE_HTML is set to YES. + +ENUM_VALUES_PER_LINE = 4 + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used +# to set the initial width (in pixels) of the frame in which the tree is shown. +# Minimum value: 0, maximum value: 1500, default value: 250. +# This tag requires that the tag GENERATE_HTML is set to YES. + +TREEVIEW_WIDTH = 250 + +# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to +# external symbols imported via tag files in a separate window. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of LaTeX formulas included as images in +# the HTML documentation. When you change the font size after a successful +# doxygen run you need to manually remove any form_*.png images from the HTML +# output directory to force them to be regenerated. +# Minimum value: 8, maximum value: 50, default value: 10. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are not +# supported properly for IE 6.0, but are supported on all modern browsers. +# +# Note that when changing this option you need to delete any form_*.png files in +# the HTML output directory before the changes have effect. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see +# http://www.mathjax.org) which uses client side Javascript for the rendering +# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX +# installed or if you want to formulas look prettier in the HTML output. When +# enabled you may also need to install MathJax separately and configure the path +# to it using the MATHJAX_RELPATH option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +USE_MATHJAX = NO + +# When MathJax is enabled you can set the default output format to be used for +# the MathJax output. See the MathJax site (see: +# http://docs.mathjax.org/en/latest/output.html) for more details. +# Possible values are: HTML-CSS (which is slower, but has the best +# compatibility), NativeMML (i.e. MathML) and SVG. +# The default value is: HTML-CSS. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_FORMAT = HTML-CSS + +# When MathJax is enabled you need to specify the location relative to the HTML +# output directory using the MATHJAX_RELPATH option. The destination directory +# should contain the MathJax.js script. For instance, if the mathjax directory +# is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax +# Content Delivery Network so you can quickly see the result without installing +# MathJax. However, it is strongly recommended to install a local copy of +# MathJax from http://www.mathjax.org before deployment. +# The default value is: http://cdn.mathjax.org/mathjax/latest. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest + +# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax +# extension names that should be enabled during MathJax rendering. For example +# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_EXTENSIONS = + +# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces +# of code that will be used on startup of the MathJax code. See the MathJax site +# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an +# example see the documentation. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_CODEFILE = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box for +# the HTML output. The underlying search engine uses javascript and DHTML and +# should work on any modern browser. Note that when using HTML help +# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) +# there is already a search function so this one should typically be disabled. +# For large projects the javascript based search engine can be slow, then +# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to +# search using the keyboard; to jump to the search box use <access key> + S +# (what the <access key> is depends on the OS and browser, but it is typically +# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down +# key> to jump into the search results window, the results can be navigated +# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel +# the search. The filter options can be selected when the cursor is inside the +# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys> +# to select a filter and <Enter> or <escape> to activate or cancel the filter +# option. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +SEARCHENGINE = YES + +# When the SERVER_BASED_SEARCH tag is enabled the search engine will be +# implemented using a web server instead of a web client using Javascript. There +# are two flavors of web server based searching depending on the EXTERNAL_SEARCH +# setting. When disabled, doxygen will generate a PHP script for searching and +# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing +# and searching needs to be provided by external tools. See the section +# "External Indexing and Searching" for details. +# The default value is: NO. +# This tag requires that the tag SEARCHENGINE is set to YES. + +SERVER_BASED_SEARCH = NO + +# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP +# script for searching. Instead the search results are written to an XML file +# which needs to be processed by an external indexer. Doxygen will invoke an +# external search engine pointed to by the SEARCHENGINE_URL option to obtain the +# search results. +# +# Doxygen ships with an example indexer (doxyindexer) and search engine +# (doxysearch.cgi) which are based on the open source search engine library +# Xapian (see: http://xapian.org/). +# +# See the section "External Indexing and Searching" for details. +# The default value is: NO. +# This tag requires that the tag SEARCHENGINE is set to YES. + +EXTERNAL_SEARCH = NO + +# The SEARCHENGINE_URL should point to a search engine hosted by a web server +# which will return the search results when EXTERNAL_SEARCH is enabled. +# +# Doxygen ships with an example indexer (doxyindexer) and search engine +# (doxysearch.cgi) which are based on the open source search engine library +# Xapian (see: http://xapian.org/). See the section "External Indexing and +# Searching" for details. +# This tag requires that the tag SEARCHENGINE is set to YES. + +SEARCHENGINE_URL = + +# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed +# search data is written to a file for indexing by an external tool. With the +# SEARCHDATA_FILE tag the name of this file can be specified. +# The default file is: searchdata.xml. +# This tag requires that the tag SEARCHENGINE is set to YES. + +SEARCHDATA_FILE = searchdata.xml + +# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the +# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is +# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple +# projects and redirect the results back to the right project. +# This tag requires that the tag SEARCHENGINE is set to YES. + +EXTERNAL_SEARCH_ID = + +# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen +# projects other than the one defined by this configuration file, but that are +# all added to the same external search index. Each project needs to have a +# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of +# to a relative location where the documentation can be found. The format is: +# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ... +# This tag requires that the tag SEARCHENGINE is set to YES. + +EXTRA_SEARCH_MAPPINGS = + +#--------------------------------------------------------------------------- +# Configuration options related to the LaTeX output +#--------------------------------------------------------------------------- + +# If the GENERATE_LATEX tag is set to YES, doxygen will generate LaTeX output. +# The default value is: YES. + +GENERATE_LATEX = YES + +# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: latex. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_OUTPUT = latex + +# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be +# invoked. +# +# Note that when enabling USE_PDFLATEX this option is only used for generating +# bitmaps for formulas in the HTML output, but not in the Makefile that is +# written to the output directory. +# The default file is: latex. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_CMD_NAME = latex + +# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate +# index for LaTeX. +# The default file is: makeindex. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +MAKEINDEX_CMD_NAME = makeindex + +# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX +# documents. This may be useful for small projects and may help to save some +# trees in general. +# The default value is: NO. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +COMPACT_LATEX = NO + +# The PAPER_TYPE tag can be used to set the paper type that is used by the +# printer. +# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x +# 14 inches) and executive (7.25 x 10.5 inches). +# The default value is: a4. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +PAPER_TYPE = a4 + +# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names +# that should be included in the LaTeX output. To get the times font for +# instance you can specify +# EXTRA_PACKAGES=times +# If left blank no extra packages will be included. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +EXTRA_PACKAGES = + +# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the +# generated LaTeX document. The header should contain everything until the first +# chapter. If it is left blank doxygen will generate a standard header. See +# section "Doxygen usage" for information on how to let doxygen write the +# default header to a separate file. +# +# Note: Only use a user-defined header if you know what you are doing! The +# following commands have a special meaning inside the header: $title, +# $datetime, $date, $doxygenversion, $projectname, $projectnumber, +# $projectbrief, $projectlogo. Doxygen will replace $title with the empty +# string, for the replacement values of the other commands the user is referred +# to HTML_HEADER. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_HEADER = + +# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the +# generated LaTeX document. The footer should contain everything after the last +# chapter. If it is left blank doxygen will generate a standard footer. See +# LATEX_HEADER for more information on how to generate a default footer and what +# special commands can be used inside the footer. +# +# Note: Only use a user-defined footer if you know what you are doing! +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_FOOTER = + +# The LATEX_EXTRA_STYLESHEET tag can be used to specify additional user-defined +# LaTeX style sheets that are included after the standard style sheets created +# by doxygen. Using this option one can overrule certain style aspects. Doxygen +# will copy the style sheet files to the output directory. +# Note: The order of the extra style sheet files is of importance (e.g. the last +# style sheet in the list overrules the setting of the previous ones in the +# list). +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_EXTRA_STYLESHEET = + +# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the LATEX_OUTPUT output +# directory. Note that the files will be copied as-is; there are no commands or +# markers available. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_EXTRA_FILES = + +# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is +# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will +# contain links (just like the HTML output) instead of page references. This +# makes the output suitable for online browsing using a PDF viewer. +# The default value is: YES. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +PDF_HYPERLINKS = YES + +# If the USE_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate +# the PDF file directly from the LaTeX files. Set this option to YES, to get a +# higher quality PDF documentation. +# The default value is: YES. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +USE_PDFLATEX = YES + +# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode +# command to the generated LaTeX files. This will instruct LaTeX to keep running +# if errors occur, instead of asking the user for help. This option is also used +# when generating formulas in HTML. +# The default value is: NO. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_BATCHMODE = NO + +# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the +# index chapters (such as File Index, Compound Index, etc.) in the output. +# The default value is: NO. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_HIDE_INDICES = NO + +# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source +# code with syntax highlighting in the LaTeX output. +# +# Note that which sources are shown also depends on other settings such as +# SOURCE_BROWSER. +# The default value is: NO. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_SOURCE_CODE = NO + +# The LATEX_BIB_STYLE tag can be used to specify the style to use for the +# bibliography, e.g. plainnat, or ieeetr. See +# http://en.wikipedia.org/wiki/BibTeX and \cite for more info. +# The default value is: plain. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_BIB_STYLE = plain + +#--------------------------------------------------------------------------- +# Configuration options related to the RTF output +#--------------------------------------------------------------------------- + +# If the GENERATE_RTF tag is set to YES, doxygen will generate RTF output. The +# RTF output is optimized for Word 97 and may not look too pretty with other RTF +# readers/editors. +# The default value is: NO. + +GENERATE_RTF = NO + +# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: rtf. +# This tag requires that the tag GENERATE_RTF is set to YES. + +RTF_OUTPUT = rtf + +# If the COMPACT_RTF tag is set to YES, doxygen generates more compact RTF +# documents. This may be useful for small projects and may help to save some +# trees in general. +# The default value is: NO. +# This tag requires that the tag GENERATE_RTF is set to YES. + +COMPACT_RTF = NO + +# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will +# contain hyperlink fields. The RTF file will contain links (just like the HTML +# output) instead of page references. This makes the output suitable for online +# browsing using Word or some other Word compatible readers that support those +# fields. +# +# Note: WordPad (write) and others do not support links. +# The default value is: NO. +# This tag requires that the tag GENERATE_RTF is set to YES. + +RTF_HYPERLINKS = NO + +# Load stylesheet definitions from file. Syntax is similar to doxygen's config +# file, i.e. a series of assignments. You only have to provide replacements, +# missing definitions are set to their default value. +# +# See also section "Doxygen usage" for information on how to generate the +# default style sheet that doxygen normally uses. +# This tag requires that the tag GENERATE_RTF is set to YES. + +RTF_STYLESHEET_FILE = + +# Set optional variables used in the generation of an RTF document. Syntax is +# similar to doxygen's config file. A template extensions file can be generated +# using doxygen -e rtf extensionFile. +# This tag requires that the tag GENERATE_RTF is set to YES. + +RTF_EXTENSIONS_FILE = + +# If the RTF_SOURCE_CODE tag is set to YES then doxygen will include source code +# with syntax highlighting in the RTF output. +# +# Note that which sources are shown also depends on other settings such as +# SOURCE_BROWSER. +# The default value is: NO. +# This tag requires that the tag GENERATE_RTF is set to YES. + +RTF_SOURCE_CODE = NO + +#--------------------------------------------------------------------------- +# Configuration options related to the man page output +#--------------------------------------------------------------------------- + +# If the GENERATE_MAN tag is set to YES, doxygen will generate man pages for +# classes and files. +# The default value is: NO. + +GENERATE_MAN = NO + +# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. A directory man3 will be created inside the directory specified by +# MAN_OUTPUT. +# The default directory is: man. +# This tag requires that the tag GENERATE_MAN is set to YES. + +MAN_OUTPUT = man + +# The MAN_EXTENSION tag determines the extension that is added to the generated +# man pages. In case the manual section does not start with a number, the number +# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is +# optional. +# The default value is: .3. +# This tag requires that the tag GENERATE_MAN is set to YES. + +MAN_EXTENSION = .3 + +# The MAN_SUBDIR tag determines the name of the directory created within +# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by +# MAN_EXTENSION with the initial . removed. +# This tag requires that the tag GENERATE_MAN is set to YES. + +MAN_SUBDIR = + +# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it +# will generate one additional man file for each entity documented in the real +# man page(s). These additional files only source the real man page, but without +# them the man command would be unable to find the correct page. +# The default value is: NO. +# This tag requires that the tag GENERATE_MAN is set to YES. + +MAN_LINKS = NO + +#--------------------------------------------------------------------------- +# Configuration options related to the XML output +#--------------------------------------------------------------------------- + +# If the GENERATE_XML tag is set to YES, doxygen will generate an XML file that +# captures the structure of the code including all documentation. +# The default value is: NO. + +GENERATE_XML = NO + +# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: xml. +# This tag requires that the tag GENERATE_XML is set to YES. + +XML_OUTPUT = xml + +# If the XML_PROGRAMLISTING tag is set to YES, doxygen will dump the program +# listings (including syntax highlighting and cross-referencing information) to +# the XML output. Note that enabling this will significantly increase the size +# of the XML output. +# The default value is: YES. +# This tag requires that the tag GENERATE_XML is set to YES. + +XML_PROGRAMLISTING = YES + +#--------------------------------------------------------------------------- +# Configuration options related to the DOCBOOK output +#--------------------------------------------------------------------------- + +# If the GENERATE_DOCBOOK tag is set to YES, doxygen will generate Docbook files +# that can be used to generate PDF. +# The default value is: NO. + +GENERATE_DOCBOOK = NO + +# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in +# front of it. +# The default directory is: docbook. +# This tag requires that the tag GENERATE_DOCBOOK is set to YES. + +DOCBOOK_OUTPUT = docbook + +# If the DOCBOOK_PROGRAMLISTING tag is set to YES, doxygen will include the +# program listings (including syntax highlighting and cross-referencing +# information) to the DOCBOOK output. Note that enabling this will significantly +# increase the size of the DOCBOOK output. +# The default value is: NO. +# This tag requires that the tag GENERATE_DOCBOOK is set to YES. + +DOCBOOK_PROGRAMLISTING = NO + +#--------------------------------------------------------------------------- +# Configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- + +# If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an +# AutoGen Definitions (see http://autogen.sf.net) file that captures the +# structure of the code including all documentation. Note that this feature is +# still experimental and incomplete at the moment. +# The default value is: NO. + +GENERATE_AUTOGEN_DEF = NO + +#--------------------------------------------------------------------------- +# Configuration options related to the Perl module output +#--------------------------------------------------------------------------- + +# If the GENERATE_PERLMOD tag is set to YES, doxygen will generate a Perl module +# file that captures the structure of the code including all documentation. +# +# Note that this feature is still experimental and incomplete at the moment. +# The default value is: NO. + +GENERATE_PERLMOD = NO + +# If the PERLMOD_LATEX tag is set to YES, doxygen will generate the necessary +# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI +# output from the Perl module output. +# The default value is: NO. +# This tag requires that the tag GENERATE_PERLMOD is set to YES. + +PERLMOD_LATEX = NO + +# If the PERLMOD_PRETTY tag is set to YES, the Perl module output will be nicely +# formatted so it can be parsed by a human reader. This is useful if you want to +# understand what is going on. On the other hand, if this tag is set to NO, the +# size of the Perl module output will be much smaller and Perl will parse it +# just the same. +# The default value is: YES. +# This tag requires that the tag GENERATE_PERLMOD is set to YES. + +PERLMOD_PRETTY = YES + +# The names of the make variables in the generated doxyrules.make file are +# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful +# so different doxyrules.make files included by the same Makefile don't +# overwrite each other's variables. +# This tag requires that the tag GENERATE_PERLMOD is set to YES. + +PERLMOD_MAKEVAR_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- + +# If the ENABLE_PREPROCESSING tag is set to YES, doxygen will evaluate all +# C-preprocessor directives found in the sources and include files. +# The default value is: YES. + +ENABLE_PREPROCESSING = YES + +# If the MACRO_EXPANSION tag is set to YES, doxygen will expand all macro names +# in the source code. If set to NO, only conditional compilation will be +# performed. Macro expansion can be done in a controlled way by setting +# EXPAND_ONLY_PREDEF to YES. +# The default value is: NO. +# This tag requires that the tag ENABLE_PREPROCESSING is set to YES. + +MACRO_EXPANSION = NO + +# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then +# the macro expansion is limited to the macros specified with the PREDEFINED and +# EXPAND_AS_DEFINED tags. +# The default value is: NO. +# This tag requires that the tag ENABLE_PREPROCESSING is set to YES. + +EXPAND_ONLY_PREDEF = NO + +# If the SEARCH_INCLUDES tag is set to YES, the include files in the +# INCLUDE_PATH will be searched if a #include is found. +# The default value is: YES. +# This tag requires that the tag ENABLE_PREPROCESSING is set to YES. + +SEARCH_INCLUDES = YES + +# The INCLUDE_PATH tag can be used to specify one or more directories that +# contain include files that are not input files but should be processed by the +# preprocessor. +# This tag requires that the tag SEARCH_INCLUDES is set to YES. + +INCLUDE_PATH = + +# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard +# patterns (like *.h and *.hpp) to filter out the header-files in the +# directories. If left blank, the patterns specified with FILE_PATTERNS will be +# used. +# This tag requires that the tag ENABLE_PREPROCESSING is set to YES. + +INCLUDE_FILE_PATTERNS = + +# The PREDEFINED tag can be used to specify one or more macro names that are +# defined before the preprocessor is started (similar to the -D option of e.g. +# gcc). The argument of the tag is a list of macros of the form: name or +# name=definition (no spaces). If the definition and the "=" are omitted, "=1" +# is assumed. To prevent a macro definition from being undefined via #undef or +# recursively expanded use the := operator instead of the = operator. +# This tag requires that the tag ENABLE_PREPROCESSING is set to YES. + +PREDEFINED = + +# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this +# tag can be used to specify a list of macro names that should be expanded. The +# macro definition that is found in the sources will be used. Use the PREDEFINED +# tag if you want to use a different macro definition that overrules the +# definition found in the source code. +# This tag requires that the tag ENABLE_PREPROCESSING is set to YES. + +EXPAND_AS_DEFINED = + +# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will +# remove all references to function-like macros that are alone on a line, have +# an all uppercase name, and do not end with a semicolon. Such function macros +# are typically used for boiler-plate code, and will confuse the parser if not +# removed. +# The default value is: YES. +# This tag requires that the tag ENABLE_PREPROCESSING is set to YES. + +SKIP_FUNCTION_MACROS = YES + +#--------------------------------------------------------------------------- +# Configuration options related to external references +#--------------------------------------------------------------------------- + +# The TAGFILES tag can be used to specify one or more tag files. For each tag +# file the location of the external documentation should be added. The format of +# a tag file without this location is as follows: +# TAGFILES = file1 file2 ... +# Adding location for the tag files is done as follows: +# TAGFILES = file1=loc1 "file2 = loc2" ... +# where loc1 and loc2 can be relative or absolute paths or URLs. See the +# section "Linking to external documentation" for more information about the use +# of tag files. +# Note: Each tag file must have a unique name (where the name does NOT include +# the path). If a tag file is not located in the directory in which doxygen is +# run, you must also specify the path to the tagfile here. + +TAGFILES = + +# When a file name is specified after GENERATE_TAGFILE, doxygen will create a +# tag file that is based on the input files it reads. See section "Linking to +# external documentation" for more information about the usage of tag files. + +GENERATE_TAGFILE = + +# If the ALLEXTERNALS tag is set to YES, all external class will be listed in +# the class index. If set to NO, only the inherited external classes will be +# listed. +# The default value is: NO. + +ALLEXTERNALS = NO + +# If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed +# in the modules index. If set to NO, only the current project's groups will be +# listed. +# The default value is: YES. + +EXTERNAL_GROUPS = YES + +# If the EXTERNAL_PAGES tag is set to YES, all external pages will be listed in +# the related pages index. If set to NO, only the current project's pages will +# be listed. +# The default value is: YES. + +EXTERNAL_PAGES = YES + +# The PERL_PATH should be the absolute path and name of the perl script +# interpreter (i.e. the result of 'which perl'). +# The default file (with absolute path) is: /usr/bin/perl. + +PERL_PATH = /usr/bin/perl + +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- + +# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram +# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to +# NO turns the diagrams off. Note that this option also works with HAVE_DOT +# disabled, but it is recommended to install and use dot, since it yields more +# powerful graphs. +# The default value is: YES. + +CLASS_DIAGRAMS = YES + +# You can define message sequence charts within doxygen comments using the \msc +# command. Doxygen will then run the mscgen tool (see: +# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the +# documentation. The MSCGEN_PATH tag allows you to specify the directory where +# the mscgen tool resides. If left empty the tool is assumed to be found in the +# default search path. + +MSCGEN_PATH = + +# You can include diagrams made with dia in doxygen documentation. Doxygen will +# then run dia to produce the diagram and insert it in the documentation. The +# DIA_PATH tag allows you to specify the directory where the dia binary resides. +# If left empty dia is assumed to be found in the default search path. + +DIA_PATH = + +# If set to YES the inheritance and collaboration graphs will hide inheritance +# and usage relations if the target is undocumented or is not a class. +# The default value is: YES. + +HIDE_UNDOC_RELATIONS = YES + +# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is +# available from the path. This tool is part of Graphviz (see: +# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent +# Bell Labs. The other options in this section have no effect if this option is +# set to NO +# The default value is: NO. + +HAVE_DOT = NO + +# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed +# to run in parallel. When set to 0 doxygen will base this on the number of +# processors available in the system. You can set it explicitly to a value +# larger than 0 to get control over the balance between CPU load and processing +# speed. +# Minimum value: 0, maximum value: 32, default value: 0. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_NUM_THREADS = 0 + +# When you want a differently looking font in the dot files that doxygen +# generates you can specify the font name using DOT_FONTNAME. You need to make +# sure dot is able to find the font, which can be done by putting it in a +# standard location or by setting the DOTFONTPATH environment variable or by +# setting DOT_FONTPATH to the directory containing the font. +# The default value is: Helvetica. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_FONTNAME = Helvetica + +# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of +# dot graphs. +# Minimum value: 4, maximum value: 24, default value: 10. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_FONTSIZE = 10 + +# By default doxygen will tell dot to use the default font as specified with +# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set +# the path where dot can find it using this tag. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_FONTPATH = + +# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for +# each documented class showing the direct and indirect inheritance relations. +# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +CLASS_GRAPH = YES + +# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a +# graph for each documented class showing the direct and indirect implementation +# dependencies (inheritance, containment, and class references variables) of the +# class with other documented classes. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +COLLABORATION_GRAPH = YES + +# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for +# groups, showing the direct groups dependencies. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +GROUP_GRAPHS = YES + +# If the UML_LOOK tag is set to YES, doxygen will generate inheritance and +# collaboration diagrams in a style similar to the OMG's Unified Modeling +# Language. +# The default value is: NO. +# This tag requires that the tag HAVE_DOT is set to YES. + +UML_LOOK = NO + +# If the UML_LOOK tag is enabled, the fields and methods are shown inside the +# class node. If there are many fields or methods and many nodes the graph may +# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the +# number of items for each type to make the size more manageable. Set this to 0 +# for no limit. Note that the threshold may be exceeded by 50% before the limit +# is enforced. So when you set the threshold to 10, up to 15 fields may appear, +# but if the number exceeds 15, the total amount of fields shown is limited to +# 10. +# Minimum value: 0, maximum value: 100, default value: 10. +# This tag requires that the tag HAVE_DOT is set to YES. + +UML_LIMIT_NUM_FIELDS = 10 + +# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and +# collaboration graphs will show the relations between templates and their +# instances. +# The default value is: NO. +# This tag requires that the tag HAVE_DOT is set to YES. + +TEMPLATE_RELATIONS = NO + +# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to +# YES then doxygen will generate a graph for each documented file showing the +# direct and indirect include dependencies of the file with other documented +# files. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +INCLUDE_GRAPH = YES + +# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are +# set to YES then doxygen will generate a graph for each documented file showing +# the direct and indirect include dependencies of the file with other documented +# files. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +INCLUDED_BY_GRAPH = YES + +# If the CALL_GRAPH tag is set to YES then doxygen will generate a call +# dependency graph for every global function or class method. +# +# Note that enabling this option will significantly increase the time of a run. +# So in most cases it will be better to enable call graphs for selected +# functions only using the \callgraph command. +# The default value is: NO. +# This tag requires that the tag HAVE_DOT is set to YES. + +CALL_GRAPH = NO + +# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller +# dependency graph for every global function or class method. +# +# Note that enabling this option will significantly increase the time of a run. +# So in most cases it will be better to enable caller graphs for selected +# functions only using the \callergraph command. +# The default value is: NO. +# This tag requires that the tag HAVE_DOT is set to YES. + +CALLER_GRAPH = NO + +# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical +# hierarchy of all classes instead of a textual one. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +GRAPHICAL_HIERARCHY = YES + +# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the +# dependencies a directory has on other directories in a graphical way. The +# dependency relations are determined by the #include relations between the +# files in the directories. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +DIRECTORY_GRAPH = YES + +# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images +# generated by dot. +# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order +# to make the SVG files visible in IE 9+ (other browsers do not have this +# requirement). +# Possible values are: png, jpg, gif and svg. +# The default value is: png. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_IMAGE_FORMAT = png + +# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to +# enable generation of interactive SVG images that allow zooming and panning. +# +# Note that this requires a modern browser other than Internet Explorer. Tested +# and working are Firefox, Chrome, Safari, and Opera. +# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make +# the SVG files visible. Older versions of IE do not have SVG support. +# The default value is: NO. +# This tag requires that the tag HAVE_DOT is set to YES. + +INTERACTIVE_SVG = NO + +# The DOT_PATH tag can be used to specify the path where the dot tool can be +# found. If left blank, it is assumed the dot tool can be found in the path. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_PATH = + +# The DOTFILE_DIRS tag can be used to specify one or more directories that +# contain dot files that are included in the documentation (see the \dotfile +# command). +# This tag requires that the tag HAVE_DOT is set to YES. + +DOTFILE_DIRS = + +# The MSCFILE_DIRS tag can be used to specify one or more directories that +# contain msc files that are included in the documentation (see the \mscfile +# command). + +MSCFILE_DIRS = + +# The DIAFILE_DIRS tag can be used to specify one or more directories that +# contain dia files that are included in the documentation (see the \diafile +# command). + +DIAFILE_DIRS = + +# When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the +# path where java can find the plantuml.jar file. If left blank, it is assumed +# PlantUML is not used or called during a preprocessing step. Doxygen will +# generate a warning when it encounters a \startuml command in this case and +# will not generate output for the diagram. + +PLANTUML_JAR_PATH = + +# When using plantuml, the specified paths are searched for files specified by +# the !include statement in a plantuml block. + +PLANTUML_INCLUDE_PATH = + +# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes +# that will be shown in the graph. If the number of nodes in a graph becomes +# larger than this value, doxygen will truncate the graph, which is visualized +# by representing a node as a red box. Note that doxygen if the number of direct +# children of the root node in a graph is already larger than +# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that +# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. +# Minimum value: 0, maximum value: 10000, default value: 50. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_GRAPH_MAX_NODES = 50 + +# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs +# generated by dot. A depth value of 3 means that only nodes reachable from the +# root by following a path via at most 3 edges will be shown. Nodes that lay +# further from the root node will be omitted. Note that setting this option to 1 +# or 2 may greatly reduce the computation time needed for large code bases. Also +# note that the size of a graph can be further restricted by +# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. +# Minimum value: 0, maximum value: 1000, default value: 0. +# This tag requires that the tag HAVE_DOT is set to YES. + +MAX_DOT_GRAPH_DEPTH = 0 + +# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent +# background. This is disabled by default, because dot on Windows does not seem +# to support this out of the box. +# +# Warning: Depending on the platform used, enabling this option may lead to +# badly anti-aliased labels on the edges of a graph (i.e. they become hard to +# read). +# The default value is: NO. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_TRANSPARENT = NO + +# Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output +# files in one run (i.e. multiple -o and -T options on the command line). This +# makes dot run faster, but since only newer versions of dot (>1.8.10) support +# this, this feature is disabled by default. +# The default value is: NO. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_MULTI_TARGETS = NO + +# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page +# explaining the meaning of the various boxes and arrows in the dot generated +# graphs. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +GENERATE_LEGEND = YES + +# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate dot +# files that are used to generate the various graphs. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_CLEANUP = YES diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/docs/libcaer_api_manual.pdf b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/docs/libcaer_api_manual.pdf new file mode 100755 index 0000000000000000000000000000000000000000..0c71255bfedd1b3ae02726630988538210902fea Binary files /dev/null and b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/docs/libcaer_api_manual.pdf differ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/.gitignore b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/.gitignore new file mode 100755 index 0000000000000000000000000000000000000000..2cb7ec138f0b889ea8f903af1c0d90a90dd5d4a8 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/.gitignore @@ -0,0 +1,3 @@ +/dvs128_simple +/davis_simple +/*.exe \ No newline at end of file diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/README b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/README new file mode 100755 index 0000000000000000000000000000000000000000..94d4b7740388664ca7bf2eb4179506bd4b7c1eef --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/README @@ -0,0 +1,7 @@ +DVS128 example: +C: gcc -std=c11 -pedantic -Wall -Wextra -O2 -o dvs128_simple dvs128_simple.c -D_POSIX_C_SOURCE=1 -D_BSD_SOURCE=1 -lcaer +C++: g++ -std=c++11 -pedantic -Wall -Wextra -O2 -o dvs128_simple dvs128_simple.cpp -D_POSIX_C_SOURCE=1 -D_BSD_SOURCE=1 -lcaer + +DAVIS example: +C: gcc -std=c11 -pedantic -Wall -Wextra -O2 -o davis_simple davis_simple.c -D_POSIX_C_SOURCE=1 -D_BSD_SOURCE=1 -lcaer +C++: g++ -std=c++11 -pedantic -Wall -Wextra -O2 -o davis_simple davis_simple.cpp -D_POSIX_C_SOURCE=1 -D_BSD_SOURCE=1 -lcaer diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/davis_simple.c b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/davis_simple.c new file mode 100755 index 0000000000000000000000000000000000000000..780951b36e21afe2e47c4bfbb107ddf9074dc1ec --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/davis_simple.c @@ -0,0 +1,140 @@ +#include <libcaer/libcaer.h> +#include <libcaer/devices/davis.h> +#include <stdio.h> +#include <signal.h> +#include <stdatomic.h> + +static atomic_bool globalShutdown = ATOMIC_VAR_INIT(false); + +static void globalShutdownSignalHandler(int signal) { + // Simply set the running flag to false on SIGTERM and SIGINT (CTRL+C) for global shutdown. + if (signal == SIGTERM || signal == SIGINT) { + atomic_store(&globalShutdown, true); + } +} + +int main(void) { + // Install signal handler for global shutdown. +#ifdef _WIN32 + signal(SIGTERM, globalShutdownSignalHandler); + signal(SIGINT, globalShutdownSignalHandler); +#else + struct sigaction shutdownAction; + + shutdownAction.sa_handler = &globalShutdownSignalHandler; + shutdownAction.sa_flags = 0; + sigemptyset(&shutdownAction.sa_mask); + sigaddset(&shutdownAction.sa_mask, SIGTERM); + sigaddset(&shutdownAction.sa_mask, SIGINT); + + if (sigaction(SIGTERM, &shutdownAction, NULL) == -1) { + caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGTERM. Error: %d.", errno); + return (EXIT_FAILURE); + } + + if (sigaction(SIGINT, &shutdownAction, NULL) == -1) { + caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGINT. Error: %d.", errno); + return (EXIT_FAILURE); + } +#endif + + // Open a DAVIS, give it a device ID of 1, and don't care about USB bus or SN restrictions. + caerDeviceHandle davis_handle = caerDeviceOpen(1, CAER_DEVICE_DAVIS_FX2, 0, 0, NULL); + if (davis_handle == NULL) { + return (EXIT_FAILURE); + } + + // Let's take a look at the information we have on the device. + struct caer_davis_info davis_info = caerDavisInfoGet(davis_handle); + + printf("%s --- ID: %d, Master: %d, DVS X: %d, DVS Y: %d, Logic: %d.\n", davis_info.deviceString, + davis_info.deviceID, davis_info.deviceIsMaster, davis_info.dvsSizeX, davis_info.dvsSizeY, + davis_info.logicVersion); + + // Send the default configuration before using the device. + // No configuration is sent automatically! + caerDeviceSendDefaultConfig(davis_handle); + + // Tweak some biases, to increase bandwidth in this case. + struct caer_bias_coarsefine coarseFineBias; + + coarseFineBias.coarseValue = 2; + coarseFineBias.fineValue = 116; + coarseFineBias.enabled = true; + coarseFineBias.sexN = false; + coarseFineBias.typeNormal = true; + coarseFineBias.currentLevelNormal = true; + caerDeviceConfigSet(davis_handle, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_PRBP, + caerBiasCoarseFineGenerate(coarseFineBias)); + + coarseFineBias.coarseValue = 1; + coarseFineBias.fineValue = 33; + coarseFineBias.enabled = true; + coarseFineBias.sexN = false; + coarseFineBias.typeNormal = true; + coarseFineBias.currentLevelNormal = true; + caerDeviceConfigSet(davis_handle, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_PRSFBP, + caerBiasCoarseFineGenerate(coarseFineBias)); + + // Let's verify they really changed! + uint32_t prBias, prsfBias; + caerDeviceConfigGet(davis_handle, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_PRBP, &prBias); + caerDeviceConfigGet(davis_handle, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_PRSFBP, &prsfBias); + + printf("New bias values --- PR-coarse: %d, PR-fine: %d, PRSF-coarse: %d, PRSF-fine: %d.\n", + caerBiasCoarseFineParse(prBias).coarseValue, caerBiasCoarseFineParse(prBias).fineValue, + caerBiasCoarseFineParse(prsfBias).coarseValue, caerBiasCoarseFineParse(prsfBias).fineValue); + + // Now let's get start getting some data from the device. We just loop, no notification needed. + caerDeviceDataStart(davis_handle, NULL, NULL, NULL, NULL, NULL); + + // Let's turn on blocking data-get mode to avoid wasting resources. + caerDeviceConfigSet(davis_handle, CAER_HOST_CONFIG_DATAEXCHANGE, CAER_HOST_CONFIG_DATAEXCHANGE_BLOCKING, true); + + while (!atomic_load_explicit(&globalShutdown, memory_order_relaxed)) { + caerEventPacketContainer packetContainer = caerDeviceDataGet(davis_handle); + if (packetContainer == NULL) { + continue; // Skip if nothing there. + } + + int32_t packetNum = caerEventPacketContainerGetEventPacketsNumber(packetContainer); + + printf("\nGot event container with %d packets (allocated).\n", packetNum); + + for (int32_t i = 0; i < packetNum; i++) { + caerEventPacketHeader packetHeader = caerEventPacketContainerGetEventPacket(packetContainer, i); + if (packetHeader == NULL) { + printf("Packet %d is empty (not present).\n", i); + continue; // Skip if nothing there. + } + + printf("Packet %d of type %d -> size is %d.\n", i, caerEventPacketHeaderGetEventType(packetHeader), + caerEventPacketHeaderGetEventNumber(packetHeader)); + + // Packet 0 is always the special events packet for DVS128, while packet is the polarity events packet. + if (i == POLARITY_EVENT) { + caerPolarityEventPacket polarity = (caerPolarityEventPacket) packetHeader; + + // Get full timestamp and addresses of first event. + caerPolarityEvent firstEvent = caerPolarityEventPacketGetEvent(polarity, 0); + + int32_t ts = caerPolarityEventGetTimestamp(firstEvent); + uint16_t x = caerPolarityEventGetX(firstEvent); + uint16_t y = caerPolarityEventGetY(firstEvent); + bool pol = caerPolarityEventGetPolarity(firstEvent); + + printf("First polarity event - ts: %d, x: %d, y: %d, pol: %d.\n", ts, x, y, pol); + } + } + + caerEventPacketContainerFree(packetContainer); + } + + caerDeviceDataStop(davis_handle); + + caerDeviceClose(&davis_handle); + + printf("Shutdown successful.\n"); + + return (EXIT_SUCCESS); +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/davis_simple.cpp b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/davis_simple.cpp new file mode 100755 index 0000000000000000000000000000000000000000..11fdc72abd8fee16ee9ff99c2627f4bab1c3ac6f --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/davis_simple.cpp @@ -0,0 +1,142 @@ +#include <libcaer/libcaer.h> +#include <libcaer/devices/davis.h> +#include <cstdio> +#include <csignal> +#include <atomic> + +using namespace std; + +static atomic_bool globalShutdown(false); + +static void globalShutdownSignalHandler(int signal) { + // Simply set the running flag to false on SIGTERM and SIGINT (CTRL+C) for global shutdown. + if (signal == SIGTERM || signal == SIGINT) { + globalShutdown.store(true); + } +} + +int main(void) { +#ifdef _WIN32 + signal(SIGTERM, globalShutdownSignalHandler); + signal(SIGINT, globalShutdownSignalHandler); +#else + // Install signal handler for global shutdown. + struct sigaction shutdownAction; + + shutdownAction.sa_handler = &globalShutdownSignalHandler; + shutdownAction.sa_flags = 0; + sigemptyset(&shutdownAction.sa_mask); + sigaddset(&shutdownAction.sa_mask, SIGTERM); + sigaddset(&shutdownAction.sa_mask, SIGINT); + + if (sigaction(SIGTERM, &shutdownAction, NULL) == -1) { + caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGTERM. Error: %d.", errno); + return (EXIT_FAILURE); + } + + if (sigaction(SIGINT, &shutdownAction, NULL) == -1) { + caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGINT. Error: %d.", errno); + return (EXIT_FAILURE); + } +#endif + + // Open a DAVIS, give it a device ID of 1, and don't care about USB bus or SN restrictions. + caerDeviceHandle davis_handle = caerDeviceOpen(1, CAER_DEVICE_DAVIS_FX2, 0, 0, NULL); + if (davis_handle == NULL) { + return (EXIT_FAILURE); + } + + // Let's take a look at the information we have on the device. + struct caer_davis_info davis_info = caerDavisInfoGet(davis_handle); + + printf("%s --- ID: %d, Master: %d, DVS X: %d, DVS Y: %d, Logic: %d.\n", davis_info.deviceString, + davis_info.deviceID, davis_info.deviceIsMaster, davis_info.dvsSizeX, davis_info.dvsSizeY, + davis_info.logicVersion); + + // Send the default configuration before using the device. + // No configuration is sent automatically! + caerDeviceSendDefaultConfig(davis_handle); + + // Tweak some biases, to increase bandwidth in this case. + struct caer_bias_coarsefine coarseFineBias; + + coarseFineBias.coarseValue = 2; + coarseFineBias.fineValue = 116; + coarseFineBias.enabled = true; + coarseFineBias.sexN = false; + coarseFineBias.typeNormal = true; + coarseFineBias.currentLevelNormal = true; + caerDeviceConfigSet(davis_handle, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_PRBP, + caerBiasCoarseFineGenerate(coarseFineBias)); + + coarseFineBias.coarseValue = 1; + coarseFineBias.fineValue = 33; + coarseFineBias.enabled = true; + coarseFineBias.sexN = false; + coarseFineBias.typeNormal = true; + coarseFineBias.currentLevelNormal = true; + caerDeviceConfigSet(davis_handle, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_PRSFBP, + caerBiasCoarseFineGenerate(coarseFineBias)); + + // Let's verify they really changed! + uint32_t prBias, prsfBias; + caerDeviceConfigGet(davis_handle, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_PRBP, &prBias); + caerDeviceConfigGet(davis_handle, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_PRSFBP, &prsfBias); + + printf("New bias values --- PR-coarse: %d, PR-fine: %d, PRSF-coarse: %d, PRSF-fine: %d.\n", + caerBiasCoarseFineParse(prBias).coarseValue, caerBiasCoarseFineParse(prBias).fineValue, + caerBiasCoarseFineParse(prsfBias).coarseValue, caerBiasCoarseFineParse(prsfBias).fineValue); + + // Now let's get start getting some data from the device. We just loop, no notification needed. + caerDeviceDataStart(davis_handle, NULL, NULL, NULL, NULL, NULL); + + // Let's turn on blocking data-get mode to avoid wasting resources. + caerDeviceConfigSet(davis_handle, CAER_HOST_CONFIG_DATAEXCHANGE, CAER_HOST_CONFIG_DATAEXCHANGE_BLOCKING, true); + + while (!globalShutdown.load(memory_order_relaxed)) { + caerEventPacketContainer packetContainer = caerDeviceDataGet(davis_handle); + if (packetContainer == NULL) { + continue; // Skip if nothing there. + } + + int32_t packetNum = caerEventPacketContainerGetEventPacketsNumber(packetContainer); + + printf("\nGot event container with %d packets (allocated).\n", packetNum); + + for (int32_t i = 0; i < packetNum; i++) { + caerEventPacketHeader packetHeader = caerEventPacketContainerGetEventPacket(packetContainer, i); + if (packetHeader == NULL) { + printf("Packet %d is empty (not present).\n", i); + continue; // Skip if nothing there. + } + + printf("Packet %d of type %d -> size is %d.\n", i, caerEventPacketHeaderGetEventType(packetHeader), + caerEventPacketHeaderGetEventNumber(packetHeader)); + + // Packet 0 is always the special events packet for DVS128, while packet is the polarity events packet. + if (i == POLARITY_EVENT) { + caerPolarityEventPacket polarity = (caerPolarityEventPacket) packetHeader; + + // Get full timestamp and addresses of first event. + caerPolarityEvent firstEvent = caerPolarityEventPacketGetEvent(polarity, 0); + + int32_t ts = caerPolarityEventGetTimestamp(firstEvent); + uint16_t x = caerPolarityEventGetX(firstEvent); + uint16_t y = caerPolarityEventGetY(firstEvent); + bool pol = caerPolarityEventGetPolarity(firstEvent); + + printf("First polarity event - ts: %d, x: %d, y: %d, pol: %d.\n", ts, x, y, pol); + } + } + + caerEventPacketContainerFree(packetContainer); + } + + caerDeviceDataStop(davis_handle); + + caerDeviceClose(&davis_handle); + + printf("Shutdown successful.\n"); + + return (EXIT_SUCCESS); +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/dvs128_simple.c b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/dvs128_simple.c new file mode 100755 index 0000000000000000000000000000000000000000..9d21433b9f9a1c60ba0454a3727f67bff0c0bb03 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/dvs128_simple.c @@ -0,0 +1,116 @@ +#include <libcaer/libcaer.h> +#include <libcaer/devices/dvs128.h> +#include <stdio.h> +#include <signal.h> +#include <stdatomic.h> + +static atomic_bool globalShutdown = ATOMIC_VAR_INIT(false); + +static void globalShutdownSignalHandler(int signal) { + // Simply set the running flag to false on SIGTERM and SIGINT (CTRL+C) for global shutdown. + if (signal == SIGTERM || signal == SIGINT) { + atomic_store(&globalShutdown, true); + } +} + +int main(void) { + // Install signal handler for global shutdown. + struct sigaction shutdownAction; + + shutdownAction.sa_handler = &globalShutdownSignalHandler; + shutdownAction.sa_flags = 0; + sigemptyset(&shutdownAction.sa_mask); + sigaddset(&shutdownAction.sa_mask, SIGTERM); + sigaddset(&shutdownAction.sa_mask, SIGINT); + + if (sigaction(SIGTERM, &shutdownAction, NULL) == -1) { + caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGTERM. Error: %d.", errno); + return (EXIT_FAILURE); + } + + if (sigaction(SIGINT, &shutdownAction, NULL) == -1) { + caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGINT. Error: %d.", errno); + return (EXIT_FAILURE); + } + + // Open a DVS128, give it a device ID of 1, and don't care about USB bus or SN restrictions. + caerDeviceHandle dvs128_handle = caerDeviceOpen(1, CAER_DEVICE_DVS128, 0, 0, NULL); + if (dvs128_handle == NULL) { + return (EXIT_FAILURE); + } + + // Let's take a look at the information we have on the device. + struct caer_dvs128_info dvs128_info = caerDVS128InfoGet(dvs128_handle); + + printf("%s --- ID: %d, Master: %d, DVS X: %d, DVS Y: %d, Logic: %d.\n", dvs128_info.deviceString, + dvs128_info.deviceID, dvs128_info.deviceIsMaster, dvs128_info.dvsSizeX, dvs128_info.dvsSizeY, + dvs128_info.logicVersion); + + // Send the default configuration before using the device. + // No configuration is sent automatically! + caerDeviceSendDefaultConfig(dvs128_handle); + + // Tweak some biases, to increase bandwidth in this case. + caerDeviceConfigSet(dvs128_handle, DVS128_CONFIG_BIAS, DVS128_CONFIG_BIAS_PR, 695); + caerDeviceConfigSet(dvs128_handle, DVS128_CONFIG_BIAS, DVS128_CONFIG_BIAS_FOLL, 867); + + // Let's verify they really changed! + uint32_t prBias, follBias; + caerDeviceConfigGet(dvs128_handle, DVS128_CONFIG_BIAS, DVS128_CONFIG_BIAS_PR, &prBias); + caerDeviceConfigGet(dvs128_handle, DVS128_CONFIG_BIAS, DVS128_CONFIG_BIAS_FOLL, &follBias); + + printf("New bias values --- PR: %d, FOLL: %d.\n", prBias, follBias); + + // Now let's get start getting some data from the device. We just loop, no notification needed. + caerDeviceDataStart(dvs128_handle, NULL, NULL, NULL, NULL, NULL); + + // Let's turn on blocking data-get mode to avoid wasting resources. + caerDeviceConfigSet(dvs128_handle, CAER_HOST_CONFIG_DATAEXCHANGE, CAER_HOST_CONFIG_DATAEXCHANGE_BLOCKING, true); + + while (!atomic_load_explicit(&globalShutdown, memory_order_relaxed)) { + caerEventPacketContainer packetContainer = caerDeviceDataGet(dvs128_handle); + if (packetContainer == NULL) { + continue; // Skip if nothing there. + } + + int32_t packetNum = caerEventPacketContainerGetEventPacketsNumber(packetContainer); + + printf("\nGot event container with %d packets (allocated).\n", packetNum); + + for (int32_t i = 0; i < packetNum; i++) { + caerEventPacketHeader packetHeader = caerEventPacketContainerGetEventPacket(packetContainer, i); + if (packetHeader == NULL) { + printf("Packet %d is empty (not present).\n", i); + continue; // Skip if nothing there. + } + + printf("Packet %d of type %d -> size is %d.\n", i, caerEventPacketHeaderGetEventType(packetHeader), + caerEventPacketHeaderGetEventNumber(packetHeader)); + + // Packet 0 is always the special events packet for DVS128, while packet is the polarity events packet. + if (i == POLARITY_EVENT) { + caerPolarityEventPacket polarity = (caerPolarityEventPacket) packetHeader; + + // Get full timestamp and addresses of first event. + caerPolarityEvent firstEvent = caerPolarityEventPacketGetEvent(polarity, 0); + + int32_t ts = caerPolarityEventGetTimestamp(firstEvent); + uint16_t x = caerPolarityEventGetX(firstEvent); + uint16_t y = caerPolarityEventGetY(firstEvent); + bool pol = caerPolarityEventGetPolarity(firstEvent); + + printf("First polarity event - ts: %d, x: %d, y: %d, pol: %d.\n", ts, x, y, pol); + } + } + + caerEventPacketContainerFree(packetContainer); + } + + caerDeviceDataStop(dvs128_handle); + + caerDeviceClose(&dvs128_handle); + + printf("Shutdown successful.\n"); + + return (EXIT_SUCCESS); +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/dvs128_simple.cpp b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/dvs128_simple.cpp new file mode 100755 index 0000000000000000000000000000000000000000..874a9229d119a6ffdf70034aa9244498edf2fb03 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/examples/dvs128_simple.cpp @@ -0,0 +1,118 @@ +#include <libcaer/libcaer.h> +#include <libcaer/devices/dvs128.h> +#include <cstdio> +#include <csignal> +#include <atomic> + +using namespace std; + +static atomic_bool globalShutdown(false); + +static void globalShutdownSignalHandler(int signal) { + // Simply set the running flag to false on SIGTERM and SIGINT (CTRL+C) for global shutdown. + if (signal == SIGTERM || signal == SIGINT) { + globalShutdown.store(true); + } +} + +int main(void) { + // Install signal handler for global shutdown. + struct sigaction shutdownAction; + + shutdownAction.sa_handler = &globalShutdownSignalHandler; + shutdownAction.sa_flags = 0; + sigemptyset(&shutdownAction.sa_mask); + sigaddset(&shutdownAction.sa_mask, SIGTERM); + sigaddset(&shutdownAction.sa_mask, SIGINT); + + if (sigaction(SIGTERM, &shutdownAction, NULL) == -1) { + caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGTERM. Error: %d.", errno); + return (EXIT_FAILURE); + } + + if (sigaction(SIGINT, &shutdownAction, NULL) == -1) { + caerLog(CAER_LOG_CRITICAL, "ShutdownAction", "Failed to set signal handler for SIGINT. Error: %d.", errno); + return (EXIT_FAILURE); + } + + // Open a DVS128, give it a device ID of 1, and don't care about USB bus or SN restrictions. + caerDeviceHandle dvs128_handle = caerDeviceOpen(1, CAER_DEVICE_DVS128, 0, 0, NULL); + if (dvs128_handle == NULL) { + return (EXIT_FAILURE); + } + + // Let's take a look at the information we have on the device. + struct caer_dvs128_info dvs128_info = caerDVS128InfoGet(dvs128_handle); + + printf("%s --- ID: %d, Master: %d, DVS X: %d, DVS Y: %d, Logic: %d.\n", dvs128_info.deviceString, + dvs128_info.deviceID, dvs128_info.deviceIsMaster, dvs128_info.dvsSizeX, dvs128_info.dvsSizeY, + dvs128_info.logicVersion); + + // Send the default configuration before using the device. + // No configuration is sent automatically! + caerDeviceSendDefaultConfig(dvs128_handle); + + // Tweak some biases, to increase bandwidth in this case. + caerDeviceConfigSet(dvs128_handle, DVS128_CONFIG_BIAS, DVS128_CONFIG_BIAS_PR, 695); + caerDeviceConfigSet(dvs128_handle, DVS128_CONFIG_BIAS, DVS128_CONFIG_BIAS_FOLL, 867); + + // Let's verify they really changed! + uint32_t prBias, follBias; + caerDeviceConfigGet(dvs128_handle, DVS128_CONFIG_BIAS, DVS128_CONFIG_BIAS_PR, &prBias); + caerDeviceConfigGet(dvs128_handle, DVS128_CONFIG_BIAS, DVS128_CONFIG_BIAS_FOLL, &follBias); + + printf("New bias values --- PR: %d, FOLL: %d.\n", prBias, follBias); + + // Now let's get start getting some data from the device. We just loop, no notification needed. + caerDeviceDataStart(dvs128_handle, NULL, NULL, NULL, NULL, NULL); + + // Let's turn on blocking data-get mode to avoid wasting resources. + caerDeviceConfigSet(dvs128_handle, CAER_HOST_CONFIG_DATAEXCHANGE, CAER_HOST_CONFIG_DATAEXCHANGE_BLOCKING, true); + + while (!globalShutdown.load(memory_order_relaxed)) { + caerEventPacketContainer packetContainer = caerDeviceDataGet(dvs128_handle); + if (packetContainer == NULL) { + continue; // Skip if nothing there. + } + + int32_t packetNum = caerEventPacketContainerGetEventPacketsNumber(packetContainer); + + printf("\nGot event container with %d packets (allocated).\n", packetNum); + + for (int32_t i = 0; i < packetNum; i++) { + caerEventPacketHeader packetHeader = caerEventPacketContainerGetEventPacket(packetContainer, i); + if (packetHeader == NULL) { + printf("Packet %d is empty (not present).\n", i); + continue; // Skip if nothing there. + } + + printf("Packet %d of type %d -> size is %d.\n", i, caerEventPacketHeaderGetEventType(packetHeader), + caerEventPacketHeaderGetEventNumber(packetHeader)); + + // Packet 0 is always the special events packet for DVS128, while packet is the polarity events packet. + if (i == POLARITY_EVENT) { + caerPolarityEventPacket polarity = (caerPolarityEventPacket) packetHeader; + + // Get full timestamp and addresses of first event. + caerPolarityEvent firstEvent = caerPolarityEventPacketGetEvent(polarity, 0); + + int32_t ts = caerPolarityEventGetTimestamp(firstEvent); + uint16_t x = caerPolarityEventGetX(firstEvent); + uint16_t y = caerPolarityEventGetY(firstEvent); + bool pol = caerPolarityEventGetPolarity(firstEvent); + + printf("First polarity event - ts: %d, x: %d, y: %d, pol: %d.\n", ts, x, y, pol); + } + } + + caerEventPacketContainerFree(packetContainer); + } + + caerDeviceDataStop(dvs128_handle); + + caerDeviceClose(&dvs128_handle); + + printf("Shutdown successful.\n"); + + return (EXIT_SUCCESS); +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/.gitignore b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/.gitignore new file mode 100755 index 0000000000000000000000000000000000000000..6c7aec8ee1fe9855abf1a5633e9e5437306524d3 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/.gitignore @@ -0,0 +1 @@ +/libcaer.h diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/CMakeLists.txt b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/CMakeLists.txt new file mode 100755 index 0000000000000000000000000000000000000000..fdefea8567b71707905450a2c687c677433b4ea2 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/CMakeLists.txt @@ -0,0 +1,10 @@ +CONFIGURE_FILE(libcaer_in.h ${CMAKE_CURRENT_SOURCE_DIR}/libcaer.h @ONLY) + +SET(INC_INSTALL_DIR ${CMAKE_INSTALL_INCLUDEDIR}/${CMAKE_PROJECT_NAME}) +INSTALL(FILES libcaer.h log.h portable_endian.h frame_utils.h DESTINATION ${INC_INSTALL_DIR}) +INSTALL(DIRECTORY events DESTINATION ${INC_INSTALL_DIR}) +INSTALL(DIRECTORY devices DESTINATION ${INC_INSTALL_DIR}) + +IF (ENABLE_OPENCV) + INSTALL(FILES frame_utils_opencv.h DESTINATION ${INC_INSTALL_DIR}) +ENDIF() diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/devices/davis.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/devices/davis.h new file mode 100755 index 0000000000000000000000000000000000000000..89a8d3984ffd83dfebb25a012746e666237a146c --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/devices/davis.h @@ -0,0 +1,1823 @@ +/** + * @file davis.h + * + * DAVIS specific configuration defines and information structures. + */ + +#ifndef LIBCAER_DEVICES_DAVIS_H_ +#define LIBCAER_DEVICES_DAVIS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "usb.h" +#include "../events/polarity.h" +#include "../events/special.h" +#include "../events/frame.h" +#include "../events/imu6.h" + +/** + * Device type definition for iniLabs DAVIS FX2-based boards, like DAVIS240a/b/c. + */ +#define CAER_DEVICE_DAVIS_FX2 1 +/** + * Device type definition for iniLabs DAVIS FX3-based boards, like DAVIS640. + */ +#define CAER_DEVICE_DAVIS_FX3 2 + +/** + * DAVIS240A chip identifier. + * 240x180, no color, no global shutter. + */ +#define DAVIS_CHIP_DAVIS240A 0 +/** + * DAVIS240B chip identifier. + * 240x180, no color, 50 test columns left-side. + */ +#define DAVIS_CHIP_DAVIS240B 1 +/** + * DAVIS240C chip identifier. + * 240x180, no color. + */ +#define DAVIS_CHIP_DAVIS240C 2 +/** + * DAVIS128 chip identifier. + * 128x128, color possible, internal ADC. + */ +#define DAVIS_CHIP_DAVIS128 3 +/** + * DAVIS346A chip identifier. + * 346x260, color possible, internal ADC. + */ +#define DAVIS_CHIP_DAVIS346A 4 +/** + * DAVIS346B chip identifier. + * 346x260, color possible, internal ADC. + */ +#define DAVIS_CHIP_DAVIS346B 5 +/** + * DAVIS640 chip identifier. + * 640x480, color possible, internal ADC. + */ +#define DAVIS_CHIP_DAVIS640 6 +/** + * DAVISRGB chip identifier. + * 640x480 APS, 320x240 DVS, color possible, internal ADC. + */ +#define DAVIS_CHIP_DAVISRGB 7 +/** + * DAVIS208 chip identifier. + * 208x192, special sensitive test pixels, color possible, internal ADC. + */ +#define DAVIS_CHIP_DAVIS208 8 +/** + * DAVIS346C chip identifier. + * 346x260, BSI, color possible, internal ADC. + */ +#define DAVIS_CHIP_DAVIS346C 9 + +//@{ +/** + * Macros to check a chip identifier integer against the known chip types. + * Returns true if a chip identifier matches, false otherwise. + */ +#define IS_DAVIS128(chipID) ((chipID) == DAVIS_CHIP_DAVIS128) +#define IS_DAVIS208(chipID) ((chipID) == DAVIS_CHIP_DAVIS208) +#define IS_DAVIS240A(chipID) ((chipID) == DAVIS_CHIP_DAVIS240A) +#define IS_DAVIS240B(chipID) ((chipID) == DAVIS_CHIP_DAVIS240B) +#define IS_DAVIS240C(chipID) ((chipID) == DAVIS_CHIP_DAVIS240C) +#define IS_DAVIS240(chipID) (IS_DAVIS240A(chipID) || IS_DAVIS240B(chipID) || IS_DAVIS240C(chipID)) +#define IS_DAVIS346A(chipID) ((chipID) == DAVIS_CHIP_DAVIS346A) +#define IS_DAVIS346B(chipID) ((chipID) == DAVIS_CHIP_DAVIS346B) +#define IS_DAVIS346C(chipID) ((chipID) == DAVIS_CHIP_DAVIS346C) +#define IS_DAVIS346(chipID) (IS_DAVIS346A(chipID) || IS_DAVIS346B(chipID) || IS_DAVIS346C(chipID)) +#define IS_DAVIS640(chipID) ((chipID) == DAVIS_CHIP_DAVIS640) +#define IS_DAVISRGB(chipID) ((chipID) == DAVIS_CHIP_DAVISRGB) +//@} + +/** + * Module address: device-side Multiplexer configuration. + * The Multiplexer is responsible for mixing, timestamping and outputting + * (via USB) the various event types generated by the device. It is also + * responsible for timestamp generation and synchronization. + */ +#define DAVIS_CONFIG_MUX 0 +/** + * Module address: device-side DVS configuration. + * The DVS state machine handshakes with the chip's AER bus and gets the + * polarity events from it. It supports various configurable delays, as + * well as advanced filtering capabilities on the polarity events. + */ +#define DAVIS_CONFIG_DVS 1 +/** + * Module address: device-side APS (Frame) configuration. + * The APS (Active-Pixel-Sensor) is responsible for getting the normal, + * synchronous frame from the camera chip. It supports various options + * for very precise timing control, as well as Region of Interest imaging. + */ +#define DAVIS_CONFIG_APS 2 +/** + * Module address: device-side IMU (Inertial Measurement Unit) configuration. + * The IMU module connects to the external IMU chip and sends data on the + * device's movement in space. It can configure various options on the external + * chip, such as accelerometer range or gyroscope refresh rate. + */ +#define DAVIS_CONFIG_IMU 3 +/** + * Module address: device-side External Input (signal detector/generator) configuration. + * The External Input module is used to detect external signals on the external input + * jack and inject an event into the event stream when this happens. It can detect pulses + * of a specific length or rising and falling edges. + * On some systems, a signal generator module is also present, which can generate + * PWM-like pulsed signals with configurable timing. + */ +#define DAVIS_CONFIG_EXTINPUT 4 +/** + * Module address: device-side chip bias configuration. + * Shared with DAVIS_CONFIG_CHIP. + * This state machine is responsible for configuring the chip's bias generator. + */ +#define DAVIS_CONFIG_BIAS 5 +/** + * Module address: device-side chip control configuration. + * Shared with DAVIS_CONFIG_BIAS. + * This state machine is responsible for configuring the chip's internal + * control shift registers, to set special options. + */ +#define DAVIS_CONFIG_CHIP 5 +/** + * Module address: device-side system information. + * The system information module provides various details on the device, such + * as currently installed logic revision or clock speeds. + * All its parameters are read-only. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation for more details on what information is available. + */ +#define DAVIS_CONFIG_SYSINFO 6 +/** + * Module address: device-side USB output configuration. + * The USB output module forwards the data from the device and the FPGA/CPLD to + * the USB chip, usually a Cypress FX2 or FX3. + */ +#define DAVIS_CONFIG_USB 9 + +/** + * Parameter address for module DAVIS_CONFIG_MUX: + * run the Multiplexer state machine, which is responsible for + * mixing the various event types at the device level, timestamping + * them and outputting them via USB or other connectors. + */ +#define DAVIS_CONFIG_MUX_RUN 0 +/** + * Parameter address for module DAVIS_CONFIG_MUX: + * run the Timestamp Generator inside the Multiplexer state machine, + * which will provide microsecond accurate timestamps to the + * events passing through. + */ +#define DAVIS_CONFIG_MUX_TIMESTAMP_RUN 1 +/** + * Parameter address for module DAVIS_CONFIG_MUX: + * reset the Timestamp Generator to zero. This also sends a reset + * pulse to all connected slave devices, resetting their timestamp too. + */ +#define DAVIS_CONFIG_MUX_TIMESTAMP_RESET 2 +/** + * Parameter address for module DAVIS_CONFIG_MUX: + * under normal circumstances, the chip's bias generator is only powered + * up when either the DVS or the APS state machines are running, to save + * power. This flag forces the bias generator to be powered up all the time, + * which may be useful when one wants to shut-down both APS and DVS + * temporarily, but still have a quick and well-defined resume behavior. + */ +#define DAVIS_CONFIG_MUX_FORCE_CHIP_BIAS_ENABLE 3 +/** + * Parameter address for module DAVIS_CONFIG_MUX: + * drop DVS events if the USB output FIFO is full, instead of having + * them pile up at the input FIFOs. + */ +#define DAVIS_CONFIG_MUX_DROP_DVS_ON_TRANSFER_STALL 4 +/** + * Parameter address for module DAVIS_CONFIG_MUX: + * drop APS events if the USB output FIFO is full, instead of having + * them pile up at the input FIFOs. + * This normally should not be enabled to guarantee complete, coherent + * frame events, though small timing differences may cause a reduction + * in observed image quality. + */ +#define DAVIS_CONFIG_MUX_DROP_APS_ON_TRANSFER_STALL 5 +/** + * Parameter address for module DAVIS_CONFIG_MUX: + * drop IMU events if the USB output FIFO is full, instead of having + * them pile up at the input FIFOs. + * This normally should not be enabled to guarantee complete, coherent + * IMU events, and not get incomplete or wrong IMU information. + */ +#define DAVIS_CONFIG_MUX_DROP_IMU_ON_TRANSFER_STALL 6 +/** + * Parameter address for module DAVIS_CONFIG_MUX: + * drop External Input events if the USB output FIFO is full, instead of having + * them pile up at the input FIFOs. + */ +#define DAVIS_CONFIG_MUX_DROP_EXTINPUT_ON_TRANSFER_STALL 7 + +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * read-only parameter, contains the X axis resolution of the + * DVS events returned by the camera. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get proper size information that already + * considers the rotation and orientation settings. + */ +#define DAVIS_CONFIG_DVS_SIZE_COLUMNS 0 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * read-only parameter, contains the Y axis resolution of the + * DVS events returned by the camera. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get proper size information that already + * considers the rotation and orientation settings. + */ +#define DAVIS_CONFIG_DVS_SIZE_ROWS 1 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * read-only parameter, contains information on the orientation + * of the X/Y axes, whether they should be inverted or not on + * the host when parsing incoming events. + * Bit 0: dvsInvertXY + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get proper size information that already + * considers the rotation and orientation settings. + */ +#define DAVIS_CONFIG_DVS_ORIENTATION_INFO 2 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * run the DVS state machine and get polarity events from the chip by + * handshaking with its AER bus. + */ +#define DAVIS_CONFIG_DVS_RUN 3 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * delay capturing the data and acknowledging it on the AER bus for + * the row events (serial AER protocol) by this many LogicClock cycles. + */ +#define DAVIS_CONFIG_DVS_ACK_DELAY_ROW 4 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * delay capturing the data and acknowledging it on the AER bus for + * the column events (serial AER protocol) by this many LogicClock cycles. + */ +#define DAVIS_CONFIG_DVS_ACK_DELAY_COLUMN 5 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * extend the length of the acknowledge on the AER bus for + * the row events (serial AER protocol) by this many LogicClock cycles. + */ +#define DAVIS_CONFIG_DVS_ACK_EXTENSION_ROW 6 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * extend the length of the acknowledge on the AER bus for + * the column events (serial AER protocol) by this many LogicClock cycles. + */ +#define DAVIS_CONFIG_DVS_ACK_EXTENSION_COLUMN 7 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * if the output FIFO for this module is full, stall the AER handshake with + * the chip and wait until it's free again, instead of just continuing + * the handshake and dropping the resulting events. + */ +#define DAVIS_CONFIG_DVS_WAIT_ON_TRANSFER_STALL 8 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * enable row-only event filter, to eliminate spurious row events with no + * following columns events. This can happen on DAVIS240 chips, or + * following the various pixel and background-activity filtering stages, + * which drop column events to achieve their effect. + * This should always be enabled! + */ +#define DAVIS_CONFIG_DVS_FILTER_ROW_ONLY_EVENTS 9 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * enable external AER control. This ensures the chip and the DVS pixel + * array are running, but doesn't do the handshake and leaves the ACK + * pin in high-impedance, to allow for an external system to take + * over the AER communication with the chip. + * DAVIS_CONFIG_DVS_RUN has to be turned off for this to work. + */ +#define DAVIS_CONFIG_DVS_EXTERNAL_AER_CONTROL 10 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * read-only parameter, information about the presence of the + * pixel filter feature. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get this information. + */ +#define DAVIS_CONFIG_DVS_HAS_PIXEL_FILTER 11 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 0, Y axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_0_ROW 12 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 0, X axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_0_COLUMN 13 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 1, Y axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_1_ROW 14 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 1, X axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_1_COLUMN 15 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 2, Y axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_2_ROW 16 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 2, X axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_2_COLUMN 17 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 3, Y axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_3_ROW 18 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 3, X axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_3_COLUMN 19 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 4, Y axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_4_ROW 20 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 4, X axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_4_COLUMN 21 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 5, Y axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_5_ROW 22 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 5, X axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_5_COLUMN 23 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 6, Y axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_6_ROW 24 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 6, X axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_6_COLUMN 25 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 7, Y axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_7_ROW 26 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * the pixel filter completely suppresses up to eight pixels in the + * DVS array, filtering out all events produced by them. + * This is the pixel 7, X axis setting. + */ +#define DAVIS_CONFIG_DVS_FILTER_PIXEL_7_COLUMN 27 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * read-only parameter, information about the presence of the + * background-activity filter feature. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get this information. + */ +#define DAVIS_CONFIG_DVS_HAS_BACKGROUND_ACTIVITY_FILTER 28 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * enable the background-activity filter, which tries to remove events + * caused by transistor leakage, by rejecting uncorrelated events. + */ +#define DAVIS_CONFIG_DVS_FILTER_BACKGROUND_ACTIVITY 29 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * specify the time difference constant for the background-activity + * filter in microseconds. Events that do correlated within this + * time-frame are let through, while others are filtered out. + */ +#define DAVIS_CONFIG_DVS_FILTER_BACKGROUND_ACTIVITY_DELTAT 30 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * read-only parameter, information about the presence of the + * test event generator feature. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get this information. + */ +#define DAVIS_CONFIG_DVS_HAS_TEST_EVENT_GENERATOR 31 +/** + * Parameter address for module DAVIS_CONFIG_DVS: + * enable the test event generator for debugging purposes. + * This generates fake events that appear to originate from all rows + * sequentially, and for each row going through all its columns, + * first with an ON polarity and then with an OFF polarity. + * Both DAVIS_CONFIG_DVS_RUN and DAVIS_CONFIG_DVS_EXTERNAL_AER_CONTROL + * have to be turned off for this to work. + */ +#define DAVIS_CONFIG_DVS_TEST_EVENT_GENERATOR_ENABLE 32 + +/** + * Parameter address for module DAVIS_CONFIG_APS: + * read-only parameter, contains the X axis resolution of the + * APS frames returned by the camera. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get proper size information that already + * considers the rotation and orientation settings. + */ +#define DAVIS_CONFIG_APS_SIZE_COLUMNS 0 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * read-only parameter, contains the Y axis resolution of the + * APS frames returned by the camera. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get proper size information that already + * considers the rotation and orientation settings. + */ +#define DAVIS_CONFIG_APS_SIZE_ROWS 1 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * read-only parameter, contains information on the orientation + * of the X/Y axes, whether they should be inverted or not on + * the host when parsing incoming pixels, as well as if the X + * or Y axes need to be flipped when reading the pixels. + * Bit 2: apsInvertXY + * Bit 1: apsFlipX + * Bit 0: apsFlipY + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get proper size information that already + * considers the rotation and orientation settings. + */ +#define DAVIS_CONFIG_APS_ORIENTATION_INFO 2 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * read-only parameter, contains information on the type of + * color filter present on the device. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get proper color filter information. + */ +#define DAVIS_CONFIG_APS_COLOR_FILTER 3 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * enable the APS module and take intensity images of the scene. + * While this parameter is enabled, frames will be taken continuously. + * To slow down the frame-rate, see DAVIS_CONFIG_APS_FRAME_DELAY. + * To only take snapshots, see DAVIS_CONFIG_APS_SNAPSHOT. + */ +#define DAVIS_CONFIG_APS_RUN 4 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * enable the reset read phase in addition to the signal read, + * to allow for correlated double sampling schemes. This heavily + * improves image quality and should always be turned on. In special + * cases, especially when the camera is perfectly stationary, this + * can be turned off for longer periods of time to achieve a higher + * frame-rate and significantly faster frame capture. + */ +#define DAVIS_CONFIG_APS_RESET_READ 5 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * if the output FIFO for this module is full, stall the APS state machine + * and wait until it's free again, instead of just dropping the pixels as + * they are being read out. This guarantees a complete frame readout, at the + * possible cost of slight timing differences between pixels. If disabled, + * incomplete frames may be transmitted and will then be dropped on the host, + * resulting in lower frame-rates, especially during high DVS traffic. + */ +#define DAVIS_CONFIG_APS_WAIT_ON_TRANSFER_STALL 6 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * read-only parameter, information about the presence of the + * global shutter feature. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get this information. + */ +#define DAVIS_CONFIG_APS_HAS_GLOBAL_SHUTTER 7 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * enable Global Shutter mode instead of Rolling Shutter. + * The Global Shutter eliminates motion artifacts, but + * is noisier than the Rolling Shutter (worse quality). + */ +#define DAVIS_CONFIG_APS_GLOBAL_SHUTTER 8 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * start position on the X axis for Region of Interest 0. + * Must be between 0 and APS_SIZE_X-1, and be smaller + * or equal to DAVIS_CONFIG_APS_END_COLUMN_0 for the ROI + * region to be enabled. Setting it to APS_SIZE_X itself + * deactivates this ROI region completely. + */ +#define DAVIS_CONFIG_APS_START_COLUMN_0 9 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * start position on the Y axis for Region of Interest 0. + * Must be between 0 and APS_SIZE_Y-1, and be smaller + * or equal to DAVIS_CONFIG_APS_END_ROW_0. + */ +#define DAVIS_CONFIG_APS_START_ROW_0 10 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * end position on the X axis for Region of Interest 0. + * Must be between 0 and APS_SIZE_X-1, and be greater + * or equal to DAVIS_CONFIG_APS_START_COLUMN_0. + */ +#define DAVIS_CONFIG_APS_END_COLUMN_0 11 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * end position on the Y axis for Region of Interest 0. + * Must be between 0 and APS_SIZE_Y-1, and be greater + * or equal to DAVIS_CONFIG_APS_START_ROW_0. + */ +#define DAVIS_CONFIG_APS_END_ROW_0 12 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * frame exposure time in microseconds, up to about + * one second maximum. + * Very precise for Global Shutter, slightly less exact for + * Rolling Shutter due to column-based timing constraints. + */ +#define DAVIS_CONFIG_APS_EXPOSURE 13 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * delay between consecutive frames in microseconds, + * up to about one second maximum. + * This can be used to achieve slower frame-rates, down + * to about 1 Hertz. + */ +#define DAVIS_CONFIG_APS_FRAME_DELAY 14 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * column reset settle time in ADCClock cycles. + */ +#define DAVIS_CONFIG_APS_RESET_SETTLE 15 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * column settle time in ADCClock cycles. + */ +#define DAVIS_CONFIG_APS_COLUMN_SETTLE 16 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * row settle time in ADCClock cycles. + */ +#define DAVIS_CONFIG_APS_ROW_SETTLE 17 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * null (between states) settle time in ADCClock cycles. + */ +#define DAVIS_CONFIG_APS_NULL_SETTLE 18 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * read-only parameter, information about the presence of the + * Quadruple Region of Interest feature. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get this information. + */ +#define DAVIS_CONFIG_APS_HAS_QUAD_ROI 19 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * start position on the X axis for Region of Interest 1. + * Must be between 0 and APS_SIZE_X-1, and be smaller + * or equal to DAVIS_CONFIG_APS_END_COLUMN_1 for the ROI + * region to be enabled. Setting it to APS_SIZE_X itself + * deactivates this ROI region completely. + */ +#define DAVIS_CONFIG_APS_START_COLUMN_1 20 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * start position on the Y axis for Region of Interest 1. + * Must be between 0 and APS_SIZE_Y-1, and be smaller + * or equal to DAVIS_CONFIG_APS_END_ROW_1. + */ +#define DAVIS_CONFIG_APS_START_ROW_1 21 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * end position on the X axis for Region of Interest 1. + * Must be between 0 and APS_SIZE_X-1, and be greater + * or equal to DAVIS_CONFIG_APS_START_COLUMN_1. + */ +#define DAVIS_CONFIG_APS_END_COLUMN_1 22 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * end position on the Y axis for Region of Interest 1. + * Must be between 0 and APS_SIZE_Y-1, and be greater + * or equal to DAVIS_CONFIG_APS_START_ROW_1. + */ +#define DAVIS_CONFIG_APS_END_ROW_1 23 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * start position on the X axis for Region of Interest 2. + * Must be between 0 and APS_SIZE_X-1, and be smaller + * or equal to DAVIS_CONFIG_APS_END_COLUMN_2 for the ROI + * region to be enabled. Setting it to APS_SIZE_X itself + * deactivates this ROI region completely. + */ +#define DAVIS_CONFIG_APS_START_COLUMN_2 24 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * start position on the Y axis for Region of Interest 2. + * Must be between 0 and APS_SIZE_Y-1, and be smaller + * or equal to DAVIS_CONFIG_APS_END_ROW_2. + */ +#define DAVIS_CONFIG_APS_START_ROW_2 25 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * end position on the X axis for Region of Interest 2. + * Must be between 0 and APS_SIZE_X-1, and be greater + * or equal to DAVIS_CONFIG_APS_START_COLUMN_2. + */ +#define DAVIS_CONFIG_APS_END_COLUMN_2 26 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * end position on the Y axis for Region of Interest 2. + * Must be between 0 and APS_SIZE_Y-1, and be greater + * or equal to DAVIS_CONFIG_APS_START_ROW_2. + */ +#define DAVIS_CONFIG_APS_END_ROW_2 27 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * start position on the X axis for Region of Interest 3. + * Must be between 0 and APS_SIZE_X-1, and be smaller + * or equal to DAVIS_CONFIG_APS_END_COLUMN_3 for the ROI + * region to be enabled. Setting it to APS_SIZE_X itself + * deactivates this ROI region completely. + */ +#define DAVIS_CONFIG_APS_START_COLUMN_3 28 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * start position on the Y axis for Region of Interest 3. + * Must be between 0 and APS_SIZE_Y-1, and be smaller + * or equal to DAVIS_CONFIG_APS_END_ROW_3. + */ +#define DAVIS_CONFIG_APS_START_ROW_3 29 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * end position on the X axis for Region of Interest 3. + * Must be between 0 and APS_SIZE_X-1, and be greater + * or equal to DAVIS_CONFIG_APS_START_COLUMN_3. + */ +#define DAVIS_CONFIG_APS_END_COLUMN_3 30 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * end position on the Y axis for Region of Interest 3. + * Must be between 0 and APS_SIZE_Y-1, and be greater + * or equal to DAVIS_CONFIG_APS_START_ROW_3. + */ +#define DAVIS_CONFIG_APS_END_ROW_3 31 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * read-only parameter, information about the presence of an + * external ADC to read the pixel values. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get this information. + */ +#define DAVIS_CONFIG_APS_HAS_EXTERNAL_ADC 32 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * read-only parameter, information about the presence of an + * internal, on-chip ADC to read the pixel values. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get this information. + */ +#define DAVIS_CONFIG_APS_HAS_INTERNAL_ADC 33 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * use the internal, on-chip ADC instead of the external one. + * This enables a much faster and more power-efficient readout + * for the frames, and should as such always be preferred. + */ +#define DAVIS_CONFIG_APS_USE_INTERNAL_ADC 34 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * enable sampling of pixel voltage by the internal ADC circuitry. + * Must always be enabled to get proper frame values. + */ +#define DAVIS_CONFIG_APS_SAMPLE_ENABLE 35 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * sample settle time in ADCClock cycles. + */ +#define DAVIS_CONFIG_APS_SAMPLE_SETTLE 36 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * ramp reset time in ADCClock cycles. + */ +#define DAVIS_CONFIG_APS_RAMP_RESET 37 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * only perform a short ramp (half length) during reset reads, given that + * the voltage should always be close to the top of the range. + * This increases the frame-rate, but may have impacts on image quality, + * especially in very bright regions. + */ +#define DAVIS_CONFIG_APS_RAMP_SHORT_RESET 38 +/** + * Parameter address for module DAVIS_CONFIG_APS: + * put all APS pixels into reset, while keeping everything else running. + * This is only useful for testing and characterizing the internal + * ADC, to minimize noise. + */ +#define DAVIS_CONFIG_APS_ADC_TEST_MODE 39 + +// Extra timing settings for DAVISRGB APS module. +/** + * Parameter address for module DAVIS_CONFIG_APS (only for DAVIS RGB chip): + * charge transfer time in ADCClock cycles. + */ +#define DAVISRGB_CONFIG_APS_TRANSFER 50 +/** + * Parameter address for module DAVIS_CONFIG_APS (only for DAVIS RGB chip): + * Rolling Shutter FD settle time in ADCClock cycles. + */ +#define DAVISRGB_CONFIG_APS_RSFDSETTLE 51 +/** + * Parameter address for module DAVIS_CONFIG_APS (only for DAVIS RGB chip): + * Global Shutter PD reset time in ADCClock cycles. + */ +#define DAVISRGB_CONFIG_APS_GSPDRESET 52 +/** + * Parameter address for module DAVIS_CONFIG_APS (only for DAVIS RGB chip): + * Global Shutter Reset Fall time in ADCClock cycles. + */ +#define DAVISRGB_CONFIG_APS_GSRESETFALL 53 +/** + * Parameter address for module DAVIS_CONFIG_APS (only for DAVIS RGB chip): + * Global Shutter Transfer Fall time in ADCClock cycles. + */ +#define DAVISRGB_CONFIG_APS_GSTXFALL 54 +/** + * Parameter address for module DAVIS_CONFIG_APS (only for DAVIS RGB chip): + * Global Shutter FD reset time in ADCClock cycles. + */ +#define DAVISRGB_CONFIG_APS_GSFDRESET 55 + +/** + * Parameter address for module DAVIS_CONFIG_APS: + * takes a snapshot (one frame), like a photo-camera. + * More efficient implementation that just toggling + * the DAVIS_CONFIG_APS_RUN parameter. + * The APS module should not be running prior to calling this, + * as it only makes sense if frames are not being generated + * at the time. Also, DAVIS_CONFIG_APS_FRAME_DELAY should + * be set to zero if only doing snapshots, to ensure a + * quicker readiness for the next one, since the delay is + * always observed after taking a frame. + */ +#define DAVIS_CONFIG_APS_SNAPSHOT 80 + +/** + * Parameter address for module DAVIS_CONFIG_IMU: + * run the IMU state machine to get information about the + * movement and position of the device. This takes the + * IMU chip out of sleep. + */ +#define DAVIS_CONFIG_IMU_RUN 0 +/** + * Parameter address for module DAVIS_CONFIG_IMU: + * put the temperature sensor in standby, disabling it. + */ +#define DAVIS_CONFIG_IMU_TEMP_STANDBY 1 +/** + * Parameter address for module DAVIS_CONFIG_IMU: + * put the accelerometer sensor in standby, disabling it. + */ +#define DAVIS_CONFIG_IMU_ACCEL_STANDBY 2 +/** + * Parameter address for module DAVIS_CONFIG_IMU: + * put the gyroscope sensor in standby, disabling it. + */ +#define DAVIS_CONFIG_IMU_GYRO_STANDBY 3 +/** + * Parameter address for module DAVIS_CONFIG_IMU: + * put the IMU into Cycle Mode. In Cycle Mode, the device + * cycles between sleep mode and waking up to take a single + * sample of data from the accelerometer at a rate determined + * by DAVIS_CONFIG_IMU_LP_WAKEUP. + */ +#define DAVIS_CONFIG_IMU_LP_CYCLE 4 +/** + * Parameter address for module DAVIS_CONFIG_IMU: + * rate at which the IMU takes an accelerometer sample while + * in Cycle Mode (see DAVIS_CONFIG_IMU_LP_CYCLE). + * Valid values are: + * 0 - 1.25 Hz wake-up frequency + * 1 - 5 Hz wake-up frequency + * 2 - 20 Hz wake-up frequency + * 3 - 40 Hz wake-up frequency + */ +#define DAVIS_CONFIG_IMU_LP_WAKEUP 5 +/** + * Parameter address for module DAVIS_CONFIG_IMU: + * this specifies the divider from the Gyroscope Output Rate used + * to generate the Sample Rate for the IMU. + * Valid values are from 0 to 255. + * The Sample Rate is generated like this: + * Sample Rate = Gyroscope Output Rate / (1 + DAVIS_CONFIG_IMU_SAMPLE_RATE_DIVIDER) + * where Gyroscope Output Rate = 8 kHz when DAVIS_CONFIG_IMU_DIGITAL_LOW_PASS_FILTER + * is disabled (set to 0 or 7), and 1 kHz when enabled. + * Note: the accelerometer output rate is 1 kHz. This means that for a Sample Rate + * greater than 1 kHz, the same accelerometer sample may be output multiple times. + */ +#define DAVIS_CONFIG_IMU_SAMPLE_RATE_DIVIDER 6 +/** + * Parameter address for module DAVIS_CONFIG_IMU: + * this configures the digital low-pass filter for both the + * accelerometer and the gyroscope. + * Valid values are from 0 to 7 and have the following meaning: + * 0 - Accel: BW=260Hz, Delay=0ms, FS=1kHz - Gyro: BW=256Hz, Delay=0.98ms, FS=8kHz + * 1 - Accel: BW=184Hz, Delay=2.0ms, FS=1kHz - Gyro: BW=188Hz, Delay=1.9ms, FS=1kHz + * 2 - Accel: BW=94Hz, Delay=3.0ms, FS=1kHz - Gyro: BW=98Hz, Delay=2.8ms, FS=1kHz + * 3 - Accel: BW=44Hz, Delay=4.9ms, FS=1kHz - Gyro: BW=42Hz, Delay=4.8ms, FS=1kHz + * 4 - Accel: BW=21Hz, Delay=8.5ms, FS=1kHz - Gyro: BW=20Hz, Delay=8.3ms, FS=1kHz + * 5 - Accel: BW=10Hz, Delay=13.8ms, FS=1kHz - Gyro: BW=10Hz, Delay=13.4ms, FS=1kHz + * 6 - Accel: BW=5Hz, Delay=19.0ms, FS=1kHz - Gyro: BW=5Hz, Delay=18.6ms, FS=1kHz + * 7 - Accel: RESERVED, FS=1kHz - Gyro: RESERVED, FS=8kHz + */ +#define DAVIS_CONFIG_IMU_DIGITAL_LOW_PASS_FILTER 7 +/** + * Parameter address for module DAVIS_CONFIG_IMU: + * select the full scale range of the accelerometer outputs. + * Valid values are: + * 0 - +- 2 g + * 1 - +- 4 g + * 2 - +- 8 g + * 3 - +- 16 g + */ +#define DAVIS_CONFIG_IMU_ACCEL_FULL_SCALE 8 +/** + * Parameter address for module DAVIS_CONFIG_IMU: + * select the full scale range of the gyroscope outputs. + * Valid values are: + * 0 - +- 250 °/s + * 1 - +- 500 °/s + * 2 - +- 1000 °/s + * 3 - +- 2000 °/s + */ +#define DAVIS_CONFIG_IMU_GYRO_FULL_SCALE 9 + +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * enable the signal detector module. It generates events + * when it sees certain types of signals, such as edges or + * pulses of a defined length, on the IN JACK signal. + * This can be useful to inject events into the event + * stream in response to external stimuli or controls, + * such as turning on a LED lamp. + */ +#define DAVIS_CONFIG_EXTINPUT_RUN_DETECTOR 0 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * send a special EXTERNAL_INPUT_RISING_EDGE event when a + * rising edge is detected (transition from low voltage to high). + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_RISING_EDGES 1 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * send a special EXTERNAL_INPUT_FALLING_EDGE event when a + * falling edge is detected (transition from high voltage to low). + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_FALLING_EDGES 2 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * send a special EXTERNAL_INPUT_PULSE event when a pulse, of + * a specified, configurable polarity and length, is detected. + * See DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY and + * DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH for more details. + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_PULSES 3 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * the polarity the pulse must exhibit to be detected as such. + * '1' means active high; a pulse will start when the signal + * goes from low to high and will continue to be seen as the + * same pulse as long as it stays high. + * '0' means active low; a pulse will start when the signal + * goes from high to low and will continue to be seen as the + * same pulse as long as it stays low. + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY 4 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * the minimal length that a pulse must have to trigger the + * sending of a special event. This is measured in cycles + * at LogicClock frequency (see 'struct caer_davis_info' for + * details on how to get the frequency). + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH 5 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * read-only parameter, information about the presence of the + * signal generator feature. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get this information. + */ +#define DAVIS_CONFIG_EXTINPUT_HAS_GENERATOR 6 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * enable the signal generator module. It generates a + * PWM-like signal based on configurable parameters and + * outputs it on the OUT JACK signal. + */ +#define DAVIS_CONFIG_EXTINPUT_RUN_GENERATOR 7 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * instead of generating a PWM-like signal by using the + * configured parameters, use a signal on the FPGA/CPLD + * that's passed as an input to the External Input module. + * By default this is disabled and tied to ground, but + * it can be useful for customized logic designs. + */ +#define DAVIS_CONFIG_EXTINPUT_GENERATE_USE_CUSTOM_SIGNAL 8 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * polarity of the PWM-like signal to be generated. + * '1' means active high, '0' means active low. + */ +#define DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_POLARITY 9 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * the interval between the start of two consecutive pulses, + * expressed in cycles at LogicClock frequency (see + * 'struct caer_davis_info' for details on how to get the frequency). + * This must be bigger or equal to DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_LENGTH. + * To generate a signal with 50% duty cycle, this would + * have to be exactly double of DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_LENGTH. + */ +#define DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_INTERVAL 10 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * the length a pulse stays active, expressed in cycles at + * LogicClock frequency (see 'struct caer_davis_info' for + * details on how to get the frequency). This must be + * smaller or equal to DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_INTERVAL. + * To generate a signal with 50% duty cycle, this would + * have to be exactly half of DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_INTERVAL. + */ +#define DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_LENGTH 11 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * enables event injection when a rising edge occurs in the + * generated signal; a special event EXTERNAL_GENERATOR_RISING_EDGE + * is emitted into the event stream. + */ +#define DAVIS_CONFIG_EXTINPUT_GENERATE_INJECT_ON_RISING_EDGE 12 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * enables event injection when a falling edge occurs in the + * generated signal; a special event EXTERNAL_GENERATOR_FALLING_EDGE + * is emitted into the event stream. + */ +#define DAVIS_CONFIG_EXTINPUT_GENERATE_INJECT_ON_FALLING_EDGE 13 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * read-only parameter, information about the presence of the + * extra detectors feature. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get this information. + */ +#define DAVIS_CONFIG_EXTINPUT_HAS_EXTRA_DETECTORS 14 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * enable the signal detector module. It generates events + * when it sees certain types of signals, such as edges or + * pulses of a defined length, on the B1P20 input pin. + * This can be useful to inject events into the event + * stream in response to external stimuli or controls, + * such as turning on a LED lamp. + */ +#define DAVIS_CONFIG_EXTINPUT_RUN_DETECTOR1 15 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * send a special EXTERNAL_INPUT1_RISING_EDGE event when a + * rising edge is detected (transition from low voltage to high). + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_RISING_EDGES1 16 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * send a special EXTERNAL_INPUT1_FALLING_EDGE event when a + * falling edge is detected (transition from high voltage to low). + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_FALLING_EDGES1 17 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * send a special EXTERNAL_INPUT1_PULSE event when a pulse, of + * a specified, configurable polarity and length, is detected. + * See DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY1 and + * DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH1 for more details. + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_PULSES1 18 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * the polarity the pulse must exhibit to be detected as such. + * '1' means active high; a pulse will start when the signal + * goes from low to high and will continue to be seen as the + * same pulse as long as it stays high. + * '0' means active low; a pulse will start when the signal + * goes from high to low and will continue to be seen as the + * same pulse as long as it stays low. + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY1 19 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * the minimal length that a pulse must have to trigger the + * sending of a special event. This is measured in cycles + * at LogicClock frequency (see 'struct caer_davis_info' for + * details on how to get the frequency). + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH1 20 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * enable the signal detector module. It generates events + * when it sees certain types of signals, such as edges or + * pulses of a defined length, on the B1P21 input pin. + * This can be useful to inject events into the event + * stream in response to external stimuli or controls, + * such as turning on a LED lamp. + */ +#define DAVIS_CONFIG_EXTINPUT_RUN_DETECTOR2 21 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * send a special EXTERNAL_INPUT2_RISING_EDGE event when a + * rising edge is detected (transition from low voltage to high). + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_RISING_EDGES2 22 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * send a special EXTERNAL_INPUT2_FALLING_EDGE event when a + * falling edge is detected (transition from high voltage to low). + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_FALLING_EDGES2 23 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * send a special EXTERNAL_INPUT2_PULSE event when a pulse, of + * a specified, configurable polarity and length, is detected. + * See DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY2 and + * DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH2 for more details. + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_PULSES2 24 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * the polarity the pulse must exhibit to be detected as such. + * '1' means active high; a pulse will start when the signal + * goes from low to high and will continue to be seen as the + * same pulse as long as it stays high. + * '0' means active low; a pulse will start when the signal + * goes from high to low and will continue to be seen as the + * same pulse as long as it stays low. + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY2 25 +/** + * Parameter address for module DAVIS_CONFIG_EXTINPUT: + * the minimal length that a pulse must have to trigger the + * sending of a special event. This is measured in cycles + * at LogicClock frequency (see 'struct caer_davis_info' for + * details on how to get the frequency). + */ +#define DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH2 26 + +/** + * Parameter address for module DAVIS_CONFIG_SYSINFO: + * read-only parameter, the version of the logic currently + * running on the device's FPGA/CPLD. It usually represents + * a specific SVN revision, at which the logic code was + * synthesized. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get this information. + */ +#define DAVIS_CONFIG_SYSINFO_LOGIC_VERSION 0 +/** + * Parameter address for module DAVIS_CONFIG_SYSINFO: + * read-only parameter, an integer used to identify the different + * types of sensor chips used on the device. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get this information. + */ +#define DAVIS_CONFIG_SYSINFO_CHIP_IDENTIFIER 1 +/** + * Parameter address for module DAVIS_CONFIG_SYSINFO: + * read-only parameter, whether the device is currently a timestamp + * master or slave when synchronizing multiple devices together. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get this information. + */ +#define DAVIS_CONFIG_SYSINFO_DEVICE_IS_MASTER 2 +/** + * Parameter address for module DAVIS_CONFIG_SYSINFO: + * read-only parameter, the frequency in MHz at which the main + * FPGA/CPLD logic is running. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get this information. + */ +#define DAVIS_CONFIG_SYSINFO_LOGIC_CLOCK 3 +/** + * Parameter address for module DAVIS_CONFIG_SYSINFO: + * read-only parameter, the frequency in MHz at which the FPGA/CPLD + * logic related to APS frame grabbing is running. + * This is reserved for internal use and should not be used by + * anything other than libcaer. Please see the 'struct caer_davis_info' + * documentation to get this information. + */ +#define DAVIS_CONFIG_SYSINFO_ADC_CLOCK 4 + +/** + * Parameter address for module DAVIS_CONFIG_USB: + * enable the USB FIFO module, which transfers the data from the + * FPGA/CPLD to the USB chip, to be then sent to the host. + * Turning this off will suppress any USB data communication! + */ +#define DAVIS_CONFIG_USB_RUN 0 +/** + * Parameter address for module DAVIS_CONFIG_USB: + * the time delay after which a packet of data is committed to + * USB, even if it is not full yet (short USB packet). + * The value is in 125µs time-slices, corresponding to how + * USB schedules its operations (a value of 4 for example + * would mean waiting at most 0.5ms until sending a short + * USB packet to the host). + */ +#define DAVIS_CONFIG_USB_EARLY_PACKET_DELAY 1 + +//@{ +/** + * Parameter address for module DAVIS128_CONFIG_BIAS: + * DAVIS128 chip biases. + * Bias configuration values must be generated using the proper + * functions, which are: + * - caerBiasVDACGenerate() for VDAC (voltage) biases. + * - caerBiasCoarseFineGenerate() for coarse-fine (current) biases. + * - caerBiasShiftedSourceGenerate() for shifted-source biases. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DAVIS128_CONFIG_BIAS_APSOVERFLOWLEVEL 0 +#define DAVIS128_CONFIG_BIAS_APSCAS 1 +#define DAVIS128_CONFIG_BIAS_ADCREFHIGH 2 +#define DAVIS128_CONFIG_BIAS_ADCREFLOW 3 +#define DAVIS128_CONFIG_BIAS_LOCALBUFBN 8 +#define DAVIS128_CONFIG_BIAS_PADFOLLBN 9 +#define DAVIS128_CONFIG_BIAS_DIFFBN 10 +#define DAVIS128_CONFIG_BIAS_ONBN 11 +#define DAVIS128_CONFIG_BIAS_OFFBN 12 +#define DAVIS128_CONFIG_BIAS_PIXINVBN 13 +#define DAVIS128_CONFIG_BIAS_PRBP 14 +#define DAVIS128_CONFIG_BIAS_PRSFBP 15 +#define DAVIS128_CONFIG_BIAS_REFRBP 16 +#define DAVIS128_CONFIG_BIAS_READOUTBUFBP 17 +#define DAVIS128_CONFIG_BIAS_APSROSFBN 18 +#define DAVIS128_CONFIG_BIAS_ADCCOMPBP 19 +#define DAVIS128_CONFIG_BIAS_COLSELLOWBN 20 +#define DAVIS128_CONFIG_BIAS_DACBUFBP 21 +#define DAVIS128_CONFIG_BIAS_LCOLTIMEOUTBN 22 +#define DAVIS128_CONFIG_BIAS_AEPDBN 23 +#define DAVIS128_CONFIG_BIAS_AEPUXBP 24 +#define DAVIS128_CONFIG_BIAS_AEPUYBP 25 +#define DAVIS128_CONFIG_BIAS_IFREFRBN 26 +#define DAVIS128_CONFIG_BIAS_IFTHRBN 27 +#define DAVIS128_CONFIG_BIAS_BIASBUFFER 34 +#define DAVIS128_CONFIG_BIAS_SSP 35 +#define DAVIS128_CONFIG_BIAS_SSN 36 +//@} + +//@{ +/** + * Parameter address for module DAVIS128_CONFIG_CHIP: + * DAVIS128 chip configuration. + * These are for expert control and should never be used + * or changed unless for advanced debugging purposes. + * To change the Global Shutter configuration, please use + * DAVIS_CONFIG_APS_GLOBAL_SHUTTER instead. + */ +#define DAVIS128_CONFIG_CHIP_DIGITALMUX0 128 +#define DAVIS128_CONFIG_CHIP_DIGITALMUX1 129 +#define DAVIS128_CONFIG_CHIP_DIGITALMUX2 130 +#define DAVIS128_CONFIG_CHIP_DIGITALMUX3 131 +#define DAVIS128_CONFIG_CHIP_ANALOGMUX0 132 +#define DAVIS128_CONFIG_CHIP_ANALOGMUX1 133 +#define DAVIS128_CONFIG_CHIP_ANALOGMUX2 134 +#define DAVIS128_CONFIG_CHIP_BIASMUX0 135 +#define DAVIS128_CONFIG_CHIP_RESETCALIBNEURON 136 +#define DAVIS128_CONFIG_CHIP_TYPENCALIBNEURON 137 +#define DAVIS128_CONFIG_CHIP_RESETTESTPIXEL 138 +#define DAVIS128_CONFIG_CHIP_AERNAROW 140 +#define DAVIS128_CONFIG_CHIP_USEAOUT 141 +#define DAVIS128_CONFIG_CHIP_GLOBAL_SHUTTER 142 +#define DAVIS128_CONFIG_CHIP_SELECTGRAYCOUNTER 143 +//@} + +//@{ +/** + * Parameter address for module DAVIS208_CONFIG_BIAS: + * DAVIS208 chip biases. + * Bias configuration values must be generated using the proper + * functions, which are: + * - caerBiasVDACGenerate() for VDAC (voltage) biases. + * - caerBiasCoarseFineGenerate() for coarse-fine (current) biases. + * - caerBiasShiftedSourceGenerate() for shifted-source biases. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DAVIS208_CONFIG_BIAS_APSOVERFLOWLEVEL 0 +#define DAVIS208_CONFIG_BIAS_APSCAS 1 +#define DAVIS208_CONFIG_BIAS_ADCREFHIGH 2 +#define DAVIS208_CONFIG_BIAS_ADCREFLOW 3 +#define DAVIS208_CONFIG_BIAS_RESETHIGHPASS 6 +#define DAVIS208_CONFIG_BIAS_REFSS 7 +#define DAVIS208_CONFIG_BIAS_LOCALBUFBN 8 +#define DAVIS208_CONFIG_BIAS_PADFOLLBN 9 +#define DAVIS208_CONFIG_BIAS_DIFFBN 10 +#define DAVIS208_CONFIG_BIAS_ONBN 11 +#define DAVIS208_CONFIG_BIAS_OFFBN 12 +#define DAVIS208_CONFIG_BIAS_PIXINVBN 13 +#define DAVIS208_CONFIG_BIAS_PRBP 14 +#define DAVIS208_CONFIG_BIAS_PRSFBP 15 +#define DAVIS208_CONFIG_BIAS_REFRBP 16 +#define DAVIS208_CONFIG_BIAS_READOUTBUFBP 17 +#define DAVIS208_CONFIG_BIAS_APSROSFBN 18 +#define DAVIS208_CONFIG_BIAS_ADCCOMPBP 19 +#define DAVIS208_CONFIG_BIAS_COLSELLOWBN 20 +#define DAVIS208_CONFIG_BIAS_DACBUFBP 21 +#define DAVIS208_CONFIG_BIAS_LCOLTIMEOUTBN 22 +#define DAVIS208_CONFIG_BIAS_AEPDBN 23 +#define DAVIS208_CONFIG_BIAS_AEPUXBP 24 +#define DAVIS208_CONFIG_BIAS_AEPUYBP 25 +#define DAVIS208_CONFIG_BIAS_IFREFRBN 26 +#define DAVIS208_CONFIG_BIAS_IFTHRBN 27 +#define DAVIS208_CONFIG_BIAS_REGBIASBP 28 +#define DAVIS208_CONFIG_BIAS_REFSSBN 30 +#define DAVIS208_CONFIG_BIAS_BIASBUFFER 34 +#define DAVIS208_CONFIG_BIAS_SSP 35 +#define DAVIS208_CONFIG_BIAS_SSN 36 +//@} + +//@{ +/** + * Parameter address for module DAVIS208_CONFIG_CHIP: + * DAVIS208 chip configuration. + * These are for expert control and should never be used + * or changed unless for advanced debugging purposes. + * To change the Global Shutter configuration, please use + * DAVIS_CONFIG_APS_GLOBAL_SHUTTER instead. + */ +#define DAVIS208_CONFIG_CHIP_DIGITALMUX0 128 +#define DAVIS208_CONFIG_CHIP_DIGITALMUX1 129 +#define DAVIS208_CONFIG_CHIP_DIGITALMUX2 130 +#define DAVIS208_CONFIG_CHIP_DIGITALMUX3 131 +#define DAVIS208_CONFIG_CHIP_ANALOGMUX0 132 +#define DAVIS208_CONFIG_CHIP_ANALOGMUX1 133 +#define DAVIS208_CONFIG_CHIP_ANALOGMUX2 134 +#define DAVIS208_CONFIG_CHIP_BIASMUX0 135 +#define DAVIS208_CONFIG_CHIP_RESETCALIBNEURON 136 +#define DAVIS208_CONFIG_CHIP_TYPENCALIBNEURON 137 +#define DAVIS208_CONFIG_CHIP_RESETTESTPIXEL 138 +#define DAVIS208_CONFIG_CHIP_AERNAROW 140 +#define DAVIS208_CONFIG_CHIP_USEAOUT 141 +#define DAVIS208_CONFIG_CHIP_GLOBAL_SHUTTER 142 +#define DAVIS208_CONFIG_CHIP_SELECTGRAYCOUNTER 143 +#define DAVIS208_CONFIG_CHIP_SELECTPREAMPAVG 145 +#define DAVIS208_CONFIG_CHIP_SELECTBIASREFSS 146 +#define DAVIS208_CONFIG_CHIP_SELECTSENSE 147 +#define DAVIS208_CONFIG_CHIP_SELECTPOSFB 148 +#define DAVIS208_CONFIG_CHIP_SELECTHIGHPASS 149 +//@} + +//@{ +/** + * Parameter address for module DAVIS240_CONFIG_BIAS: + * DAVIS240chip biases. + * Bias configuration values must be generated using the proper + * functions, which are: + * - caerBiasCoarseFineGenerate() for coarse-fine (current) biases. + * - caerBiasShiftedSourceGenerate() for shifted-source biases. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DAVIS240_CONFIG_BIAS_DIFFBN 0 +#define DAVIS240_CONFIG_BIAS_ONBN 1 +#define DAVIS240_CONFIG_BIAS_OFFBN 2 +#define DAVIS240_CONFIG_BIAS_APSCASEPC 3 +#define DAVIS240_CONFIG_BIAS_DIFFCASBNC 4 +#define DAVIS240_CONFIG_BIAS_APSROSFBN 5 +#define DAVIS240_CONFIG_BIAS_LOCALBUFBN 6 +#define DAVIS240_CONFIG_BIAS_PIXINVBN 7 +#define DAVIS240_CONFIG_BIAS_PRBP 8 +#define DAVIS240_CONFIG_BIAS_PRSFBP 9 +#define DAVIS240_CONFIG_BIAS_REFRBP 10 +#define DAVIS240_CONFIG_BIAS_AEPDBN 11 +#define DAVIS240_CONFIG_BIAS_LCOLTIMEOUTBN 12 +#define DAVIS240_CONFIG_BIAS_AEPUXBP 13 +#define DAVIS240_CONFIG_BIAS_AEPUYBP 14 +#define DAVIS240_CONFIG_BIAS_IFTHRBN 15 +#define DAVIS240_CONFIG_BIAS_IFREFRBN 16 +#define DAVIS240_CONFIG_BIAS_PADFOLLBN 17 +#define DAVIS240_CONFIG_BIAS_APSOVERFLOWLEVELBN 18 +#define DAVIS240_CONFIG_BIAS_BIASBUFFER 19 +#define DAVIS240_CONFIG_BIAS_SSP 20 +#define DAVIS240_CONFIG_BIAS_SSN 21 +//@} + +//@{ +/** + * Parameter address for module DAVIS240_CONFIG_CHIP: + * DAVIS240 chip configuration. + * These are for expert control and should never be used + * or changed unless for advanced debugging purposes. + * To change the Global Shutter configuration, please use + * DAVIS_CONFIG_APS_GLOBAL_SHUTTER instead. + * On DAVIS240B cameras, DAVIS240_CONFIG_CHIP_SPECIALPIXELCONTROL + * can be used to enable the test pixel array. + */ +#define DAVIS240_CONFIG_CHIP_DIGITALMUX0 128 +#define DAVIS240_CONFIG_CHIP_DIGITALMUX1 129 +#define DAVIS240_CONFIG_CHIP_DIGITALMUX2 130 +#define DAVIS240_CONFIG_CHIP_DIGITALMUX3 131 +#define DAVIS240_CONFIG_CHIP_ANALOGMUX0 132 +#define DAVIS240_CONFIG_CHIP_ANALOGMUX1 133 +#define DAVIS240_CONFIG_CHIP_ANALOGMUX2 134 +#define DAVIS240_CONFIG_CHIP_BIASMUX0 135 +#define DAVIS240_CONFIG_CHIP_RESETCALIBNEURON 136 +#define DAVIS240_CONFIG_CHIP_TYPENCALIBNEURON 137 +#define DAVIS240_CONFIG_CHIP_RESETTESTPIXEL 138 +#define DAVIS240_CONFIG_CHIP_SPECIALPIXELCONTROL 139 +#define DAVIS240_CONFIG_CHIP_AERNAROW 140 +#define DAVIS240_CONFIG_CHIP_USEAOUT 141 +#define DAVIS240_CONFIG_CHIP_GLOBAL_SHUTTER 142 +//@} + +//@{ +/** + * Parameter address for module DAVIS346_CONFIG_BIAS: + * DAVIS346 chip biases. + * Bias configuration values must be generated using the proper + * functions, which are: + * - caerBiasVDACGenerate() for VDAC (voltage) biases. + * - caerBiasCoarseFineGenerate() for coarse-fine (current) biases. + * - caerBiasShiftedSourceGenerate() for shifted-source biases. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DAVIS346_CONFIG_BIAS_APSOVERFLOWLEVEL 0 +#define DAVIS346_CONFIG_BIAS_APSCAS 1 +#define DAVIS346_CONFIG_BIAS_ADCREFHIGH 2 +#define DAVIS346_CONFIG_BIAS_ADCREFLOW 3 +#define DAVIS346_CONFIG_BIAS_ADCTESTVOLTAGE 4 +#define DAVIS346_CONFIG_BIAS_LOCALBUFBN 8 +#define DAVIS346_CONFIG_BIAS_PADFOLLBN 9 +#define DAVIS346_CONFIG_BIAS_DIFFBN 10 +#define DAVIS346_CONFIG_BIAS_ONBN 11 +#define DAVIS346_CONFIG_BIAS_OFFBN 12 +#define DAVIS346_CONFIG_BIAS_PIXINVBN 13 +#define DAVIS346_CONFIG_BIAS_PRBP 14 +#define DAVIS346_CONFIG_BIAS_PRSFBP 15 +#define DAVIS346_CONFIG_BIAS_REFRBP 16 +#define DAVIS346_CONFIG_BIAS_READOUTBUFBP 17 +#define DAVIS346_CONFIG_BIAS_APSROSFBN 18 +#define DAVIS346_CONFIG_BIAS_ADCCOMPBP 19 +#define DAVIS346_CONFIG_BIAS_COLSELLOWBN 20 +#define DAVIS346_CONFIG_BIAS_DACBUFBP 21 +#define DAVIS346_CONFIG_BIAS_LCOLTIMEOUTBN 22 +#define DAVIS346_CONFIG_BIAS_AEPDBN 23 +#define DAVIS346_CONFIG_BIAS_AEPUXBP 24 +#define DAVIS346_CONFIG_BIAS_AEPUYBP 25 +#define DAVIS346_CONFIG_BIAS_IFREFRBN 26 +#define DAVIS346_CONFIG_BIAS_IFTHRBN 27 +#define DAVIS346_CONFIG_BIAS_BIASBUFFER 34 +#define DAVIS346_CONFIG_BIAS_SSP 35 +#define DAVIS346_CONFIG_BIAS_SSN 36 +//@} + +//@{ +/** + * Parameter address for module DAVIS346_CONFIG_CHIP: + * DAVIS346 chip configuration. + * These are for expert control and should never be used + * or changed unless for advanced debugging purposes. + * To change the Global Shutter configuration, please use + * DAVIS_CONFIG_APS_GLOBAL_SHUTTER instead. + */ +#define DAVIS346_CONFIG_CHIP_DIGITALMUX0 128 +#define DAVIS346_CONFIG_CHIP_DIGITALMUX1 129 +#define DAVIS346_CONFIG_CHIP_DIGITALMUX2 130 +#define DAVIS346_CONFIG_CHIP_DIGITALMUX3 131 +#define DAVIS346_CONFIG_CHIP_ANALOGMUX0 132 +#define DAVIS346_CONFIG_CHIP_ANALOGMUX1 133 +#define DAVIS346_CONFIG_CHIP_ANALOGMUX2 134 +#define DAVIS346_CONFIG_CHIP_BIASMUX0 135 +#define DAVIS346_CONFIG_CHIP_RESETCALIBNEURON 136 +#define DAVIS346_CONFIG_CHIP_TYPENCALIBNEURON 137 +#define DAVIS346_CONFIG_CHIP_RESETTESTPIXEL 138 +#define DAVIS346_CONFIG_CHIP_AERNAROW 140 +#define DAVIS346_CONFIG_CHIP_USEAOUT 141 +#define DAVIS346_CONFIG_CHIP_GLOBAL_SHUTTER 142 +#define DAVIS346_CONFIG_CHIP_SELECTGRAYCOUNTER 143 +#define DAVIS346_CONFIG_CHIP_TESTADC 144 +//@} + +//@{ +/** + * Parameter address for module DAVIS640_CONFIG_BIAS: + * DAVIS640 chip biases. + * Bias configuration values must be generated using the proper + * functions, which are: + * - caerBiasVDACGenerate() for VDAC (voltage) biases. + * - caerBiasCoarseFineGenerate() for coarse-fine (current) biases. + * - caerBiasShiftedSourceGenerate() for shifted-source biases. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DAVIS640_CONFIG_BIAS_APSOVERFLOWLEVEL 0 +#define DAVIS640_CONFIG_BIAS_APSCAS 1 +#define DAVIS640_CONFIG_BIAS_ADCREFHIGH 2 +#define DAVIS640_CONFIG_BIAS_ADCREFLOW 3 +#define DAVIS640_CONFIG_BIAS_ADCTESTVOLTAGE 4 +#define DAVIS640_CONFIG_BIAS_LOCALBUFBN 8 +#define DAVIS640_CONFIG_BIAS_PADFOLLBN 9 +#define DAVIS640_CONFIG_BIAS_DIFFBN 10 +#define DAVIS640_CONFIG_BIAS_ONBN 11 +#define DAVIS640_CONFIG_BIAS_OFFBN 12 +#define DAVIS640_CONFIG_BIAS_PIXINVBN 13 +#define DAVIS640_CONFIG_BIAS_PRBP 14 +#define DAVIS640_CONFIG_BIAS_PRSFBP 15 +#define DAVIS640_CONFIG_BIAS_REFRBP 16 +#define DAVIS640_CONFIG_BIAS_READOUTBUFBP 17 +#define DAVIS640_CONFIG_BIAS_APSROSFBN 18 +#define DAVIS640_CONFIG_BIAS_ADCCOMPBP 19 +#define DAVIS640_CONFIG_BIAS_COLSELLOWBN 20 +#define DAVIS640_CONFIG_BIAS_DACBUFBP 21 +#define DAVIS640_CONFIG_BIAS_LCOLTIMEOUTBN 22 +#define DAVIS640_CONFIG_BIAS_AEPDBN 23 +#define DAVIS640_CONFIG_BIAS_AEPUXBP 24 +#define DAVIS640_CONFIG_BIAS_AEPUYBP 25 +#define DAVIS640_CONFIG_BIAS_IFREFRBN 26 +#define DAVIS640_CONFIG_BIAS_IFTHRBN 27 +#define DAVIS640_CONFIG_BIAS_BIASBUFFER 34 +#define DAVIS640_CONFIG_BIAS_SSP 35 +#define DAVIS640_CONFIG_BIAS_SSN 36 +//@} + +//@{ +/** + * Parameter address for module DAVIS640_CONFIG_CHIP: + * DAVIS640 chip configuration. + * These are for expert control and should never be used + * or changed unless for advanced debugging purposes. + * To change the Global Shutter configuration, please use + * DAVIS_CONFIG_APS_GLOBAL_SHUTTER instead. + */ +#define DAVIS640_CONFIG_CHIP_DIGITALMUX0 128 +#define DAVIS640_CONFIG_CHIP_DIGITALMUX1 129 +#define DAVIS640_CONFIG_CHIP_DIGITALMUX2 130 +#define DAVIS640_CONFIG_CHIP_DIGITALMUX3 131 +#define DAVIS640_CONFIG_CHIP_ANALOGMUX0 132 +#define DAVIS640_CONFIG_CHIP_ANALOGMUX1 133 +#define DAVIS640_CONFIG_CHIP_ANALOGMUX2 134 +#define DAVIS640_CONFIG_CHIP_BIASMUX0 135 +#define DAVIS640_CONFIG_CHIP_RESETCALIBNEURON 136 +#define DAVIS640_CONFIG_CHIP_TYPENCALIBNEURON 137 +#define DAVIS640_CONFIG_CHIP_RESETTESTPIXEL 138 +#define DAVIS640_CONFIG_CHIP_AERNAROW 140 +#define DAVIS640_CONFIG_CHIP_USEAOUT 141 +#define DAVIS640_CONFIG_CHIP_GLOBAL_SHUTTER 142 +#define DAVIS640_CONFIG_CHIP_SELECTGRAYCOUNTER 143 +#define DAVIS640_CONFIG_CHIP_TESTADC 144 +//@} + +//@{ +/** + * Parameter address for module DAVISRGB_CONFIG_BIAS: + * DAVISRGB chip biases. + * Bias configuration values must be generated using the proper + * functions, which are: + * - caerBiasVDACGenerate() for VDAC (voltage) biases. + * - caerBiasCoarseFineGenerate() for coarse-fine (current) biases. + * - caerBiasShiftedSourceGenerate() for shifted-source biases. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DAVISRGB_CONFIG_BIAS_APSCAS 0 +#define DAVISRGB_CONFIG_BIAS_OVG1LO 1 +#define DAVISRGB_CONFIG_BIAS_OVG2LO 2 +#define DAVISRGB_CONFIG_BIAS_TX2OVG2HI 3 +#define DAVISRGB_CONFIG_BIAS_GND07 4 +#define DAVISRGB_CONFIG_BIAS_ADCTESTVOLTAGE 5 +#define DAVISRGB_CONFIG_BIAS_ADCREFHIGH 6 +#define DAVISRGB_CONFIG_BIAS_ADCREFLOW 7 +#define DAVISRGB_CONFIG_BIAS_IFREFRBN 8 +#define DAVISRGB_CONFIG_BIAS_IFTHRBN 9 +#define DAVISRGB_CONFIG_BIAS_LOCALBUFBN 10 +#define DAVISRGB_CONFIG_BIAS_PADFOLLBN 11 +#define DAVISRGB_CONFIG_BIAS_PIXINVBN 13 +#define DAVISRGB_CONFIG_BIAS_DIFFBN 14 +#define DAVISRGB_CONFIG_BIAS_ONBN 15 +#define DAVISRGB_CONFIG_BIAS_OFFBN 16 +#define DAVISRGB_CONFIG_BIAS_PRBP 17 +#define DAVISRGB_CONFIG_BIAS_PRSFBP 18 +#define DAVISRGB_CONFIG_BIAS_REFRBP 19 +#define DAVISRGB_CONFIG_BIAS_ARRAYBIASBUFFERBN 20 +#define DAVISRGB_CONFIG_BIAS_ARRAYLOGICBUFFERBN 22 +#define DAVISRGB_CONFIG_BIAS_FALLTIMEBN 23 +#define DAVISRGB_CONFIG_BIAS_RISETIMEBP 24 +#define DAVISRGB_CONFIG_BIAS_READOUTBUFBP 25 +#define DAVISRGB_CONFIG_BIAS_APSROSFBN 26 +#define DAVISRGB_CONFIG_BIAS_ADCCOMPBP 27 +#define DAVISRGB_CONFIG_BIAS_DACBUFBP 28 +#define DAVISRGB_CONFIG_BIAS_LCOLTIMEOUTBN 30 +#define DAVISRGB_CONFIG_BIAS_AEPDBN 31 +#define DAVISRGB_CONFIG_BIAS_AEPUXBP 32 +#define DAVISRGB_CONFIG_BIAS_AEPUYBP 33 +#define DAVISRGB_CONFIG_BIAS_BIASBUFFER 34 +#define DAVISRGB_CONFIG_BIAS_SSP 35 +#define DAVISRGB_CONFIG_BIAS_SSN 36 +//@} + +//@{ +/** + * Parameter address for module DAVISRGB_CONFIG_CHIP: + * DAVISRGB chip configuration. + * These are for expert control and should never be used + * or changed unless for advanced debugging purposes. + * To change the Global Shutter configuration, please use + * DAVIS_CONFIG_APS_GLOBAL_SHUTTER instead. + */ +#define DAVISRGB_CONFIG_CHIP_DIGITALMUX0 128 +#define DAVISRGB_CONFIG_CHIP_DIGITALMUX1 129 +#define DAVISRGB_CONFIG_CHIP_DIGITALMUX2 130 +#define DAVISRGB_CONFIG_CHIP_DIGITALMUX3 131 +#define DAVISRGB_CONFIG_CHIP_ANALOGMUX0 132 +#define DAVISRGB_CONFIG_CHIP_ANALOGMUX1 133 +#define DAVISRGB_CONFIG_CHIP_ANALOGMUX2 134 +#define DAVISRGB_CONFIG_CHIP_BIASMUX0 135 +#define DAVISRGB_CONFIG_CHIP_RESETCALIBNEURON 136 +#define DAVISRGB_CONFIG_CHIP_TYPENCALIBNEURON 137 +#define DAVISRGB_CONFIG_CHIP_RESETTESTPIXEL 138 +#define DAVISRGB_CONFIG_CHIP_AERNAROW 140 +#define DAVISRGB_CONFIG_CHIP_USEAOUT 141 +#define DAVISRGB_CONFIG_CHIP_SELECTGRAYCOUNTER 143 +#define DAVISRGB_CONFIG_CHIP_TESTADC 144 +#define DAVISRGB_CONFIG_CHIP_ADJUSTOVG1LO 145 +#define DAVISRGB_CONFIG_CHIP_ADJUSTOVG2LO 146 +#define DAVISRGB_CONFIG_CHIP_ADJUSTTX2OVG2HI 147 +//@} + +/** + * DAVIS device-related information. + */ +struct caer_davis_info { + /// Unique device identifier. Also 'source' for events. + int16_t deviceID; + /// Device serial number. + char deviceSerialNumber[8 + 1]; + /// Device USB bus number. + uint8_t deviceUSBBusNumber; + /// Device USB device address. + uint8_t deviceUSBDeviceAddress; + /// Device information string, for logging purposes. + char *deviceString; + /// Logic (FPGA/CPLD) version. + int16_t logicVersion; + /// Whether the device is a time-stamp master or slave. + bool deviceIsMaster; + /// Clock in MHz for main logic (FPGA/CPLD). + int16_t logicClock; + /// Clock in MHz for ADC/APS logic (FPGA/CPLD). + int16_t adcClock; + /// Chip identifier/type. + int16_t chipID; + /// DVS X axis resolution. + int16_t dvsSizeX; + /// DVS Y axis resolution. + int16_t dvsSizeY; + /// Feature test: DVS pixel-level filtering. + bool dvsHasPixelFilter; + /// Feature test: DVS Background Activity filter. + bool dvsHasBackgroundActivityFilter; + /// Feature test: fake event generator (testing/debug). + bool dvsHasTestEventGenerator; + /// APS X axis resolution. + int16_t apsSizeX; + /// APS Y axis resolution. + int16_t apsSizeY; + /// APS color filter type. + enum caer_frame_event_color_filter apsColorFilter; + /// Feature test: APS supports Global Shutter. + bool apsHasGlobalShutter; + /// Feature test: APS supports Quadruple Region-of-Interest readout. + bool apsHasQuadROI; + /// Feature test: APS supports External ADC for getting the image. + bool apsHasExternalADC; + /// Feature test: APS supports Internal (on-chip) ADC for getting the image. + bool apsHasInternalADC; + /// Feature test: External Input module supports Signal-Generation. + bool extInputHasGenerator; + /// Feature test: External Input module supports extra detectors (1 & 2). + bool extInputHasExtraDetectors; +}; + +/** + * Return basic information on the device, such as its ID, its + * resolution, the logic version, and so on. See the 'struct + * caer_davis_info' documentation for more details. + * + * @param handle a valid device handle. + * + * @return a copy of the device information structure if successful, + * an empty structure (all zeros) on failure. + */ +struct caer_davis_info caerDavisInfoGet(caerDeviceHandle handle); + +/** + * On-chip voltage digital-to-analog converter configuration. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +struct caer_bias_vdac { + /// Voltage, between 0 and 63, as a fraction of 1/64th of VDD=3.3V. + uint8_t voltageValue; + /// Current, between 0 and 7, that drives the voltage. + uint8_t currentValue; +}; + +/** + * Transform VDAC bias structure into internal integer representation, + * suited for sending directly to the device via caerDeviceConfigSet(). + * + * @param vdacBias VDAC bias structure. + * + * @return internal integer representation for device configuration. + */ +uint16_t caerBiasVDACGenerate(struct caer_bias_vdac vdacBias); +/** + * Transform internal integer representation, as received by calls to + * caerDeviceConfigGet(), into a VDAC bias structure, for easier + * handling and understanding of the various parameters. + * + * @param vdacBias internal integer representation from device. + * + * @return VDAC bias structure. + */ +struct caer_bias_vdac caerBiasVDACParse(uint16_t vdacBias); + +/** + * On-chip coarse-fine bias current configuration. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +struct caer_bias_coarsefine { + /// Coarse current, from 0 to 7, creates big variations in output current. + uint8_t coarseValue; + /// Fine current, from 0 to 255, creates small variations in output current. + uint8_t fineValue; + /// Whether this bias is enabled or not. + bool enabled; + /// Bias sex: true for 'N' type, false for 'P' type. + bool sexN; + /// Bias type: true for 'Normal', false for 'Cascode'. + bool typeNormal; + /// Bias current level: true for 'Normal, false for 'Low'. + bool currentLevelNormal; +}; + +/** + * Transform coarse-fine bias structure into internal integer representation, + * suited for sending directly to the device via caerDeviceConfigSet(). + * + * @param coarseFineBias coarse-fine bias structure. + * + * @return internal integer representation for device configuration. + */ +uint16_t caerBiasCoarseFineGenerate(struct caer_bias_coarsefine coarseFineBias); +/** + * Transform internal integer representation, as received by calls to + * caerDeviceConfigGet(), into a coarse-fine bias structure, for easier + * handling and understanding of the various parameters. + * + * @param coarseFineBias internal integer representation from device. + * + * @return coarse-fine bias structure. + */ +struct caer_bias_coarsefine caerBiasCoarseFineParse(uint16_t coarseFineBias); + +/** + * Shifted-source bias operating mode. + */ +enum caer_bias_shiftedsource_operating_mode { + /// Standard mode. + SHIFTED_SOURCE = 0, + /// High impedance (driven from outside). + HI_Z = 1, + /// Tied to ground (SSN) or VDD (SSP). + TIED_TO_RAIL = 2, +}; + +/** + * Shifted-source bias voltage level. + */ +enum caer_bias_shiftedsource_voltage_level { + /// Standard mode (200-400mV). + SPLIT_GATE = 0, + /// Higher shifted-source voltage (one cascode). + SINGLE_DIODE = 1, + /// Even higher shifted-source voltage (two cascodes). + DOUBLE_DIODE = 2, +}; + +/** + * On-chip shifted-source bias current configuration. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +struct caer_bias_shiftedsource { + /// Shifted-source bias level, from 0 to 63. + uint8_t refValue; + /// Shifted-source bias current for buffer amplifier, from 0 to 63. + uint8_t regValue; + /// Shifted-source operating mode (see 'enum caer_bias_shiftedsource_operating_mode'). + enum caer_bias_shiftedsource_operating_mode operatingMode; + /// Shifted-source voltage level (see 'enum caer_bias_shiftedsource_voltage_level'). + enum caer_bias_shiftedsource_voltage_level voltageLevel; +}; + +/** + * Transform shifted-source bias structure into internal integer representation, + * suited for sending directly to the device via caerDeviceConfigSet(). + * + * @param shiftedSourceBias shifted-source bias structure. + * + * @return internal integer representation for device configuration. + */ +uint16_t caerBiasShiftedSourceGenerate(struct caer_bias_shiftedsource shiftedSourceBias); +/** + * Transform internal integer representation, as received by calls to + * caerDeviceConfigGet(), into a shifted-source bias structure, for easier + * handling and understanding of the various parameters. + * + * @param shiftedSourceBias internal integer representation from device. + * + * @return shifted-source bias structure. + */ +struct caer_bias_shiftedsource caerBiasShiftedSourceParse(uint16_t shiftedSourceBias); + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_DEVICES_DAVIS_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/devices/dvs128.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/devices/dvs128.h new file mode 100755 index 0000000000000000000000000000000000000000..acf21babdf789b305672649be06e96cb9f91b6b9 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/devices/dvs128.h @@ -0,0 +1,169 @@ +/** + * @file dvs128.h + * + * DVS128 specific configuration defines and information structures. + */ + +#ifndef LIBCAER_DEVICES_DVS128_H_ +#define LIBCAER_DEVICES_DVS128_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "usb.h" +#include "../events/polarity.h" +#include "../events/special.h" + +/** + * Device type definition for iniLabs DVS128. + */ +#define CAER_DEVICE_DVS128 0 + +/** + * Module address: device-side DVS configuration. + */ +#define DVS128_CONFIG_DVS 0 +/** + * Module address: device-side chip bias generator configuration. + */ +#define DVS128_CONFIG_BIAS 1 + +/** + * Parameter address for module DVS128_CONFIG_DVS: + * run the DVS chip and generate polarity event data. + */ +#define DVS128_CONFIG_DVS_RUN 0 +/** + * Parameter address for module DVS128_CONFIG_DVS: + * reset the time-stamp counter of the device. This is a temporary + * configuration switch and will reset itself right away. + */ +#define DVS128_CONFIG_DVS_TIMESTAMP_RESET 1 +/** + * Parameter address for module DVS128_CONFIG_DVS: + * reset the whole DVS pixel array. This is a temporary + * configuration switch and will reset itself right away. + */ +#define DVS128_CONFIG_DVS_ARRAY_RESET 2 +/** + * Parameter address for module DVS128_CONFIG_DVS: + * control if this DVS is a timestamp master device. + * Default is enabled. + */ +#define DVS128_CONFIG_DVS_TS_MASTER 3 + +/** + * Parameter address for module DVS128_CONFIG_BIAS: + * First stage amplifier cascode bias. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DVS128_CONFIG_BIAS_CAS 0 +/** + * Parameter address for module DVS128_CONFIG_BIAS: + * Injected ground bias. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DVS128_CONFIG_BIAS_INJGND 1 +/** + * Parameter address for module DVS128_CONFIG_BIAS: + * Pull down on chip request (AER). + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DVS128_CONFIG_BIAS_REQPD 2 +/** + * Parameter address for module DVS128_CONFIG_BIAS: + * Pull up on request from X arbiter (AER). + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DVS128_CONFIG_BIAS_PUX 3 +/** + * Parameter address for module DVS128_CONFIG_BIAS: + * Off events threshold bias. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DVS128_CONFIG_BIAS_DIFFOFF 4 +/** + * Parameter address for module DVS128_CONFIG_BIAS: + * Pull down for passive load inverters in digital AER pixel circuitry. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DVS128_CONFIG_BIAS_REQ 5 +/** + * Parameter address for module DVS128_CONFIG_BIAS: + * Refractory period bias. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DVS128_CONFIG_BIAS_REFR 6 +/** + * Parameter address for module DVS128_CONFIG_BIAS: + * Pull up on request from Y arbiter (AER). + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DVS128_CONFIG_BIAS_PUY 7 +/** + * Parameter address for module DVS128_CONFIG_BIAS: + * On events threshold bias. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DVS128_CONFIG_BIAS_DIFFON 8 +/** + * Parameter address for module DVS128_CONFIG_BIAS: + * Differential (second stage amplifier) bias. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DVS128_CONFIG_BIAS_DIFF 9 +/** + * Parameter address for module DVS128_CONFIG_BIAS: + * Source follower bias. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DVS128_CONFIG_BIAS_FOLL 10 +/** + * Parameter address for module DVS128_CONFIG_BIAS: + * Photoreceptor bias. + * See 'http://inilabs.com/support/biasing/' for more details. + */ +#define DVS128_CONFIG_BIAS_PR 11 + +/** + * DVS128 device-related information. + */ +struct caer_dvs128_info { + /// Unique device identifier. Also 'source' for events. + int16_t deviceID; + /// Device serial number. + char deviceSerialNumber[8 + 1]; + /// Device USB bus number. + uint8_t deviceUSBBusNumber; + /// Device USB device address. + uint8_t deviceUSBDeviceAddress; + /// Device information string, for logging purposes. + char *deviceString; + /// Logic (FPGA/CPLD) version. + int16_t logicVersion; + /// Whether the device is a time-stamp master or slave. + bool deviceIsMaster; + /// DVS X axis resolution. + int16_t dvsSizeX; + /// DVS Y axis resolution. + int16_t dvsSizeY; +}; + +/** + * Return basic information on the device, such as its ID, its + * resolution, the logic version, and so on. See the 'struct + * caer_dvs128_info' documentation for more details. + * + * @param handle a valid device handle. + * + * @return a copy of the device information structure if successful, + * an empty structure (all zeros) on failure. + */ +struct caer_dvs128_info caerDVS128InfoGet(caerDeviceHandle handle); + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_DEVICES_DVS128_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/devices/usb.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/devices/usb.h new file mode 100755 index 0000000000000000000000000000000000000000..1d2c05ca9a0ee83158859bf165ff3221e3336a76 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/devices/usb.h @@ -0,0 +1,242 @@ +/** + * @file usb.h + * + * Common functions to access, configure and exchange data with + * supported USB devices. Also contains defines for host/USB + * related configuration options. + */ + +#ifndef LIBCAER_DEVICES_USB_H_ +#define LIBCAER_DEVICES_USB_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "../libcaer.h" +#include "../events/packetContainer.h" + +/** + * Reference to an open device on which to operate. + */ +typedef struct caer_device_handle *caerDeviceHandle; + +/** + * Module address: host-side USB configuration. + */ +#define CAER_HOST_CONFIG_USB -1 +/** + * Module address: host-side data exchange (ring-buffer) configuration. + */ +#define CAER_HOST_CONFIG_DATAEXCHANGE -2 +/** + * Module address: host-side event packets generation configuration. + */ +#define CAER_HOST_CONFIG_PACKETS -3 + +/** + * Parameter address for module CAER_HOST_CONFIG_USB: + * set number of buffers used by libusb for asynchronous data transfers + * with the USB device. The default values are usually fine, only change + * them if you're running into I/O limits. + */ +#define CAER_HOST_CONFIG_USB_BUFFER_NUMBER 0 +/** + * Parameter address for module CAER_HOST_CONFIG_USB: + * set size of each buffer used by libusb for asynchronous data transfers + * with the USB device. The default values are usually fine, only change + * them if you're running into I/O limits. + */ +#define CAER_HOST_CONFIG_USB_BUFFER_SIZE 1 + +/** + * Parameter address for module CAER_HOST_CONFIG_DATAEXCHANGE: + * set size of elements that can be held by the thread-safe FIFO + * buffer between the USB data transfer thread and the main thread. + * The default values are usually fine, only change them if you're + * running into lots of dropped/missing packets; you can turn on + * the INFO log level to see when this is the case. + */ +#define CAER_HOST_CONFIG_DATAEXCHANGE_BUFFER_SIZE 0 +/** + * Parameter address for module CAER_HOST_CONFIG_DATAEXCHANGE: + * when calling caerDeviceDataGet(), the function can either be + * blocking, meaning it waits until it has a valid + * EventPacketContainer to return, or not, meaning it returns + * right away. This behavior can be set with this flag. + * Please see the caerDeviceDataGet() documentation for more + * information on its return values. + */ +#define CAER_HOST_CONFIG_DATAEXCHANGE_BLOCKING 1 +/** + * Parameter address for module CAER_HOST_CONFIG_DATAEXCHANGE: + * whether to start all the data producer modules on the device + * (DVS, APS, Mux, ...) automatically when starting the USB + * data transfer thread with caerDeviceDataStart() or not. + * If disabled, be aware you will have to start the right modules + * manually, which can be useful if you need precise control + * over which ones are running at any time. + */ + #define CAER_HOST_CONFIG_DATAEXCHANGE_START_PRODUCERS 2 +/** + * Parameter address for module CAER_HOST_CONFIG_DATAEXCHANGE: + * whether to stop all the data producer modules on the device + * (DVS, APS, Mux, ...) automatically when stopping the USB + * data transfer thread with caerDeviceDataStop() or not. + * If disabled, be aware you will have to stop the right modules + * manually, to halt the data flow, which can be useful if you + * need precise control over which ones are running at any time. + */ + #define CAER_HOST_CONFIG_DATAEXCHANGE_STOP_PRODUCERS 3 + +/** + * Parameter address for module CAER_HOST_CONFIG_PACKETS: + * set the maximum number of events any of a packet container's + * packets may hold before it's made available to the user. + * This is checked for each number of events held in each typed + * EventPacket that is a part of the EventPacketContainer. + */ +#define CAER_HOST_CONFIG_PACKETS_MAX_CONTAINER_PACKET_SIZE 0 +/** + * Parameter address for module CAER_HOST_CONFIG_PACKETS: + * set the time interval between subsequent packet containers. + * The value is in microseconds, and is checked across all + * types of events contained in the EventPacketContainer. + */ +#define CAER_HOST_CONFIG_PACKETS_MAX_CONTAINER_INTERVAL 1 + +/** + * Open a specified USB device, assign an ID to it and return a handle for further usage. + * Various means can be employed to limit the selection of the device. + * + * @param deviceID a unique ID to identify the device from others. Will be used as the + * source for EventPackets being generate from its data. + * @param deviceType type of the device to open. Currently supported are: + * CAER_DEVICE_DVS128, CAER_DEVICE_DAVIS_FX2, CAER_DEVICE_DAVIS_FX3 + * @param busNumberRestrict restrict the search for viable devices to only this USB bus number. + * @param devAddressRestrict restrict the search for viable devices to only this USB device address. + * @param serialNumberRestrict restrict the search for viable devices to only devices which do + * possess the given Serial Number in their USB SerialNumber descriptor. + * + * @return a valid device handle that can be used with the other libcaer functions, + * or NULL on error. Always check for this! + */ +caerDeviceHandle caerDeviceOpen(uint16_t deviceID, uint16_t deviceType, uint8_t busNumberRestrict, + uint8_t devAddressRestrict, const char *serialNumberRestrict); +/** + * Close a previously opened USB device and invalidate its handle. + * + * @param handle pointer to a valid device handle. Will set handle to NULL if closing is + * successful, to prevent further usage of this handle for other operations. + * + * @return true if closing was successful, false on errors. + */ +bool caerDeviceClose(caerDeviceHandle *handle); + +/** + * Send a set of good default configuration settings to the device. + * This avoids users having to set every configuration option each time, + * especially when wanting to get going quickly or just needing to change + * a few settings to get to the desired operating mode. + * + * @param handle a valid device handle. + * + * @return true if sending the configuration was successful, false on errors. + */ +bool caerDeviceSendDefaultConfig(caerDeviceHandle handle); + +/** + * Set a configuration parameter to a given value. + * + * @param handle a valid device handle. + * @param modAddr a module address, used to specify which configuration module + * one wants to update. Negative addresses are used for host-side + * configuration, while positive addresses (including zero) are + * used for device-side configuration. + * @param paramAddr a parameter address, to select a specific parameter to update + * from this particular configuration module. Only positive numbers + * (including zero) are allowed. + * @param param a configuration parameter's new value. + * + * @return true if sending the configuration was successful, false on errors. + */ +bool caerDeviceConfigSet(caerDeviceHandle handle, int8_t modAddr, uint8_t paramAddr, uint32_t param); + +/** + * Get the value of a configuration parameter. + * + * @param handle a valid device handle. + * @param modAddr a module address, used to specify which configuration module + * one wants to query. Negative addresses are used for host-side + * configuration, while positive addresses (including zero) are + * used for device-side configuration. + * @param paramAddr a parameter address, to select a specific parameter to query + * from this particular configuration module. Only positive numbers + * (including zero) are allowed. + * @param param a pointer to an integer, in which to store the configuration + * parameter's current value. The integer will always be either set + * to zero (on failure), or to the current value (on success). + * + * @return true if sending the configuration was successful, false on errors. + */ +bool caerDeviceConfigGet(caerDeviceHandle handle, int8_t modAddr, uint8_t paramAddr, uint32_t *param); + +/** + * Start getting data from the device, setting up the USB data transfer thread + * and starting the data producers (see CAER_HOST_CONFIG_DATAEXCHANGE_START_PRODUCERS). + * Supports notification of new data and shutdown events via user-defined call-backs. + * + * @param handle a valid device handle. + * @param dataNotifyIncrease function pointer, called every time a new piece of data + * available and has been put in the FIFO buffer for consumption. + * dataNotifyUserPtr will be passed as parameter to the function. + * @param dataNotifyDecrease function pointer, called every time a new piece of data + * has been consumed from the FIFO buffer inside caerDeviceDataGet(). + * dataNotifyUserPtr will be passed as parameter to the function. + * @param dataNotifyUserPtr pointer that will be passed to the dataNotifyIncrease and + * dataNotifyDecrease functions. Can be NULL. + * @param dataShutdownNotify function pointer, called on shut-down of the USB data transfer + * thread. This can be used to detect exceptional shut-downs that + * do not come from calling caerDeviceDataStop(), such as when the + * device is disconnected or all USB transfers fail. + * @param dataShutdownUserPtr pointer that will be passed to the dataShutdownNotify + * function. Can be NULL. + * + * @return true if starting the data transfer was successful, false on errors. + */ +bool caerDeviceDataStart(caerDeviceHandle handle, void (*dataNotifyIncrease)(void *ptr), + void (*dataNotifyDecrease)(void *ptr), void *dataNotifyUserPtr, void (*dataShutdownNotify)(void *ptr), + void *dataShutdownUserPtr); + +/** + * Stop getting data from the device, shutting down the USB data transfer thread + * and stopping the data producers (see CAER_HOST_CONFIG_DATAEXCHANGE_STOP_PRODUCERS). + * This normal shut-down will also generate a notification (see caerDeviceDataStart()). + * + * @param handle a valid device handle. + * + * @return true if stopping the data transfer was successful, false on errors. + */ +bool caerDeviceDataStop(caerDeviceHandle handle); + +/** + * Get an event packet container, which contains events of various types generated by + * the device, from the USB data transfer thread for further processing. + * The returned data structures are allocated in memory and will need to be freed. + * The caerEventPacketContainerFree() function can be used to correctly free the full + * container memory. For single caerEventPackets, just use free(). + * This function can be made blocking with the CAER_HOST_CONFIG_DATAEXCHANGE_BLOCKING + * configuration parameter. By default it is non-blocking. + * + * @param handle a valid device handle. + * + * @return a valid event packet container. NULL will be returned on errors, or when + * there is no container available in non-blocking mode. Always check for this! + */ +caerEventPacketContainer caerDeviceDataGet(caerDeviceHandle handle); + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_DEVICES_USB_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/common.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/common.h new file mode 100755 index 0000000000000000000000000000000000000000..39470dab25c156d4e798aa7cd2c0d620a6f08765 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/common.h @@ -0,0 +1,780 @@ +/** + * @file common.h + * + * Common EventPacket header format definition and handling functions. + * Every EventPacket, of any type, has as a first member a common header, + * which describes various properties of the contained events. This allows + * easy parsing of events. See the 'struct caer_event_packet_header' + * documentation for more details. + */ + +#ifndef LIBCAER_EVENTS_COMMON_H_ +#define LIBCAER_EVENTS_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "../libcaer.h" + +/** + * Generic validity mark: + * this bit is used to mark whether an event is still + * valid or not, and can be used to efficiently filter + * out events from a packet. + * The caerXXXEventValidate() and caerXXXEventInvalidate() + * functions should be used to toggle this! + * 0 in the 0th bit of the first byte means invalid, 1 means valid. + * This way zeroing-out an event packet sets all its events to invalid. + * Care must be taken to put the field containing the validity + * mark always as the first member of an event. + */ +//@{ +#define VALID_MARK_SHIFT 0 +#define VALID_MARK_MASK 0x00000001 +//@} + +/** + * 64bit timestamp support: + * since timestamps wrap around after some time, being only 31 bit (32 bit signed int), + * another timestamp at the packet level provides another 31 bit (32 bit signed int), + * to enable the generation of a 62 bit (64 bit signed int) microsecond timestamp which + * is guaranteed to never wrap around (in the next 146'138 years at least). + * The TSOverflow needs to be shifted by 31 thus when constructing such a timestamp. + */ +#define TS_OVERFLOW_SHIFT 31 + +/** + * List of supported event types. + * Each event type has its own integer representation. + * All event types below 100 are reserved for use + * by libcaer and cAER. + * DO NOT USE THEM FOR YOUR OWN EVENT TYPES! + */ +enum caer_default_event_types { + SPECIAL_EVENT = 0, //!< Special events. + POLARITY_EVENT = 1, //!< Polarity (change, DVS) events. + FRAME_EVENT = 2, //!< Frame (intensity, APS) events. + IMU6_EVENT = 3, //!< 6 axes IMU events. + IMU9_EVENT = 4, //!< 9 axes IMU events. + SAMPLE_EVENT = 5, //!< ADC sample events. + EAR_EVENT = 6, //!< Ear (cochlea) events. + CONFIG_EVENT = 7, //!< Device configuration events. + POINT1D_EVENT = 8, //!< 1D measurement events. + POINT2D_EVENT = 9, //!< 2D measurement events. + POINT3D_EVENT = 10, //!< 3D measurement events. + POINT4D_EVENT = 11, //!< 4D measurement events. +}; + +/** + * Size of the EventPacket header. + * This is constant across all supported systems. + */ +#define CAER_EVENT_PACKET_HEADER_SIZE 28 + +/** + * EventPacket header data structure definition. + * The size, also defined in CAER_EVENT_PACKET_HEADER_SIZE, + * must always be constant. The header is common to all + * types of event packets and is always the very first + * member of an event packet data structure. + * Signed integers are used for compatibility with languages that + * do not have unsigned ones, such as Java. + */ +struct caer_event_packet_header { + /// Numerical type ID, unique to each event type (see 'enum caer_default_event_types'). + int16_t eventType; + /// Numerical source ID, unique inside a process, identifies who generated the events. + int16_t eventSource; + /// Size of one event in bytes. + int32_t eventSize; + /// Offset from the start of an event, in bytes, at which the main 32 bit time-stamp can be found. + int32_t eventTSOffset; + /// Overflow counter for the standard 32bit event time-stamp. Used to generate the 64 bit time-stamp. + int32_t eventTSOverflow; + /// Maximum number of events this packet can store. + int32_t eventCapacity; + /// Total number of events present in this packet (valid + invalid). + int32_t eventNumber; + /// Total number of valid events present in this packet. + int32_t eventValid; +}__attribute__((__packed__)); + +/** + * Type for pointer to EventPacket header data structure. + */ +typedef struct caer_event_packet_header *caerEventPacketHeader; + +/** + * Return the numerical event type ID, representing the event type this + * EventPacket is containing. + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * + * @return the numerical event type (see 'enum caer_default_event_types'). + */ +static inline int16_t caerEventPacketHeaderGetEventType(caerEventPacketHeader header) { + return (le16toh(header->eventType)); +} + +/** + * Set the numerical event type ID, representing the event type this + * EventPacket will contain. + * All event types below 100 are reserved for use by libcaer and cAER. + * DO NOT USE THEM FOR YOUR OWN EVENT TYPES! + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * @param eventType the numerical event type (see 'enum caer_default_event_types'). + */ +static inline void caerEventPacketHeaderSetEventType(caerEventPacketHeader header, int16_t eventType) { + if (eventType < 0) { + // Negative numbers (bit 31 set) are not allowed! + caerLog(CAER_LOG_CRITICAL, "EventPacket Header", + "Called caerEventPacketHeaderSetEventType() with negative value!"); + return; + } + + header->eventType = htole16(eventType); +} + +/** + * Get the numerical event source ID, representing the event source + * that generated all the events present in this packet. + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * + * @return the numerical event source ID. + */ +static inline int16_t caerEventPacketHeaderGetEventSource(caerEventPacketHeader header) { + return (le16toh(header->eventSource)); +} + +/** + * Set the numerical event source ID, representing the event source + * that generated all the events present in this packet. + * This ID should be unique at least within a process, if not within + * the whole system, to guarantee correct identification of who + * generated an event later on. + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * @param eventSource the numerical event source ID. + */ +static inline void caerEventPacketHeaderSetEventSource(caerEventPacketHeader header, int16_t eventSource) { + if (eventSource < 0) { + // Negative numbers (bit 31 set) are not allowed! + caerLog(CAER_LOG_CRITICAL, "EventPacket Header", + "Called caerEventPacketHeaderSetEventSource() with negative value!"); + return; + } + + header->eventSource = htole16(eventSource); +} + +/** + * Get the size of a single event, in bytes. + * All events inside an event packet always have the same size. + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * + * @return the event size in bytes. + */ +static inline int32_t caerEventPacketHeaderGetEventSize(caerEventPacketHeader header) { + return (le32toh(header->eventSize)); +} + +/** + * Set the size of a single event, in bytes. + * All events inside an event packet always have the same size. + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * @param eventSize the event size in bytes. + */ +static inline void caerEventPacketHeaderSetEventSize(caerEventPacketHeader header, int32_t eventSize) { + if (eventSize < 0) { + // Negative numbers (bit 31 set) are not allowed! + caerLog(CAER_LOG_CRITICAL, "EventPacket Header", + "Called caerEventPacketHeaderSetEventSize() with negative value!"); + return; + } + + header->eventSize = htole32(eventSize); +} + +/** + * Get the offset, in bytes, to where the field with the main + * 32 bit timestamp is stored. This is useful for generic access + * to the timestamp field, given that different event types might + * have it at different offsets or might even have multiple + * timestamps, in which case this offset references the 'main' + * timestamp, the most representative one. + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * + * @return the event timestamp offset in bytes. + */ +static inline int32_t caerEventPacketHeaderGetEventTSOffset(caerEventPacketHeader header) { + return (le32toh(header->eventTSOffset)); +} + +/** + * Set the offset, in bytes, to where the field with the main + * 32 bit timestamp is stored. This is useful for generic access + * to the timestamp field, given that different event types might + * have it at different offsets or might even have multiple + * timestamps, in which case this offset references the 'main' + * timestamp, the most representative one. + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * @param eventTSOffset the event timestamp offset in bytes. + */ +static inline void caerEventPacketHeaderSetEventTSOffset(caerEventPacketHeader header, int32_t eventTSOffset) { + if (eventTSOffset < 0) { + // Negative numbers (bit 31 set) are not allowed! + caerLog(CAER_LOG_CRITICAL, "EventPacket Header", + "Called caerEventPacketHeaderSetEventTSOffset() with negative value!"); + return; + } + + header->eventTSOffset = htole32(eventTSOffset); +} + +/** + * Get the 32 bit timestamp overflow counter (in microseconds). This is per-packet + * and is used to generate a 64 bit timestamp that never wraps around. + * Since timestamps wrap around after some time, being only 31 bit (32 bit signed int), + * another timestamp at the packet level provides another 31 bit (32 bit signed int), + * to enable the generation of a 62 bit (64 bit signed int) microsecond timestamp which + * is guaranteed to never wrap around (in the next 146'138 years at least). + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * + * @return the packet-level timestamp overflow counter, in microseconds. + */ +static inline int32_t caerEventPacketHeaderGetEventTSOverflow(caerEventPacketHeader header) { + return (le32toh(header->eventTSOverflow)); +} + +/** + * Set the 32 bit timestamp overflow counter (in microseconds). This is per-packet + * and is used to generate a 64 bit timestamp that never wraps around. + * Since timestamps wrap around after some time, being only 31 bit (32 bit signed int), + * another timestamp at the packet level provides another 31 bit (32 bit signed int), + * to enable the generation of a 62 bit (64 bit signed int) microsecond timestamp which + * is guaranteed to never wrap around (in the next 146'138 years at least). + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * @param eventTSOverflow the packet-level timestamp overflow counter, in microseconds. + */ +static inline void caerEventPacketHeaderSetEventTSOverflow(caerEventPacketHeader header, int32_t eventTSOverflow) { + if (eventTSOverflow < 0) { + // Negative numbers (bit 31 set) are not allowed! + caerLog(CAER_LOG_CRITICAL, "EventPacket Header", + "Called caerEventPacketHeaderSetEventTSOverflow() with negative value!"); + return; + } + + header->eventTSOverflow = htole32(eventTSOverflow); +} + +/** + * Get the maximum number of events this packet can store. + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * + * @return the number of events this packet can hold. + */ +static inline int32_t caerEventPacketHeaderGetEventCapacity(caerEventPacketHeader header) { + return (le32toh(header->eventCapacity)); +} + +/** + * Set the maximum number of events this packet can store. + * This is determined at packet allocation time and should + * not be changed during the life-time of the packet. + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * @param eventsCapacity the number of events this packet can hold. + */ +static inline void caerEventPacketHeaderSetEventCapacity(caerEventPacketHeader header, int32_t eventsCapacity) { + if (eventsCapacity < 0) { + // Negative numbers (bit 31 set) are not allowed! + caerLog(CAER_LOG_CRITICAL, "EventPacket Header", + "Called caerEventPacketHeaderSetEventCapacity() with negative value!"); + return; + } + + header->eventCapacity = htole32(eventsCapacity); +} + +/** + * Get the number of events currently stored in this packet, + * considering both valid and invalid events. + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * + * @return the number of events in this packet. + */ +static inline int32_t caerEventPacketHeaderGetEventNumber(caerEventPacketHeader header) { + return (le32toh(header->eventNumber)); +} + +/** + * Set the number of events currently stored in this packet, + * considering both valid and invalid events. + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * @param eventsNumber the number of events in this packet. + */ +static inline void caerEventPacketHeaderSetEventNumber(caerEventPacketHeader header, int32_t eventsNumber) { + if (eventsNumber < 0) { + // Negative numbers (bit 31 set) are not allowed! + caerLog(CAER_LOG_CRITICAL, "EventPacket Header", + "Called caerEventPacketHeaderSetEventNumber() with negative value!"); + return; + } + + header->eventNumber = htole32(eventsNumber); +} + +/** + * Get the number of valid events in this packet, disregarding + * invalid ones (where the invalid mark is set). + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * + * @return the number of valid events in this packet. + */ +static inline int32_t caerEventPacketHeaderGetEventValid(caerEventPacketHeader header) { + return (le32toh(header->eventValid)); +} + +/** + * Set the number of valid events in this packet, disregarding + * invalid ones (where the invalid mark is set). + * + * @param header a valid EventPacket header pointer. Cannot be NULL. + * @param eventsValid the number of valid events in this packet. + */ +static inline void caerEventPacketHeaderSetEventValid(caerEventPacketHeader header, int32_t eventsValid) { + if (eventsValid < 0) { + // Negative numbers (bit 31 set) are not allowed! + caerLog(CAER_LOG_CRITICAL, "EventPacket Header", + "Called caerEventPacketHeaderSetEventValid() with negative value!"); + return; + } + + header->eventValid = htole32(eventsValid); +} + +/** + * Grows an event packet. + * Use free() to reclaim this memory afterwards. + * + * @param packet the current events packet. + * @param eventCapacity the new maximum number of events this packet will hold. + * + * @return a valid event packet handle or NULL on error. + * On success, the old packet handle is to be considered invalid and not to be + * used anymore. On failure, the old packet handle is not touched in any way. + */ +static inline caerEventPacketHeader caerGenericEventPacketGrow(caerEventPacketHeader packet, int32_t newEventCapacity) { + if (packet == NULL || newEventCapacity == 0) { + return (NULL); + } + + int32_t oldEventCapacity = caerEventPacketHeaderGetEventCapacity(packet); + + if (newEventCapacity <= oldEventCapacity) { + caerLog(CAER_LOG_CRITICAL, "Generic Event Packet", + "Called caerGenericEventPacketGrow() with a new capacity value (%" PRIi32 ") that is equal or smaller than the old one (%" PRIi32 "). " + "Only strictly growing an event packet is supported!", newEventCapacity, oldEventCapacity); + return (NULL); + } + + int32_t eventSize = caerEventPacketHeaderGetEventSize(packet); + size_t newEventPacketSize = CAER_EVENT_PACKET_HEADER_SIZE + (size_t) (newEventCapacity * eventSize); + + // Grow memory used to hold events. + packet = (caerEventPacketHeader) realloc(packet, newEventPacketSize); + if (packet == NULL) { + caerLog(CAER_LOG_CRITICAL, "Generic Event Packet", + "Failed to reallocate %zu bytes of memory for growing Event Packet of capacity %" + PRIi32 " to new capacity of %" PRIi32 ". Error: %d.", newEventPacketSize, oldEventCapacity, + newEventCapacity, errno); + return (NULL); + } + + // Zero out new event memory (all events invalid). + memset(((uint8_t *) packet) + CAER_EVENT_PACKET_HEADER_SIZE + (oldEventCapacity * eventSize), 0, + (size_t) ((newEventCapacity - oldEventCapacity) * eventSize)); + + // Update header fields. + caerEventPacketHeaderSetEventCapacity(packet, newEventCapacity); + + return (packet); +} + +/** + * Appends an event packet to another. + * This is a simple append operation, no timestamp reordering is done. + * Please ensure time is monotonically increasing over the two packets! + * Use free() to reclaim this memory afterwards. + * + * @param packet the main events packet. + * @param appendPacket the events packet to append on the main one. + * + * @return a valid event packet handle or NULL on error. + * On success, the old packet handle is to be considered invalid and not to be + * used anymore. On failure, the old packet handle is not touched in any way. + * The appendPacket handle is never touched in any way. + */ +static inline caerEventPacketHeader caerGenericEventPacketAppend(caerEventPacketHeader packet, + caerEventPacketHeader appendPacket) { + if (packet == NULL) { + return (NULL); + } + + // Support appending nothing, the result is the unmodified input. + if (appendPacket == NULL) { + return (packet); + } + + // Check that the two packets are of the same type and size, and have the same TSOverflow epoch. + if ((caerEventPacketHeaderGetEventType(packet) != caerEventPacketHeaderGetEventType(appendPacket)) + || (caerEventPacketHeaderGetEventSize(packet) != caerEventPacketHeaderGetEventSize(appendPacket)) + || (caerEventPacketHeaderGetEventTSOverflow(packet) != caerEventPacketHeaderGetEventTSOverflow(appendPacket))) { + return (NULL); + } + + int32_t packetEventValid = caerEventPacketHeaderGetEventValid(packet); + int32_t packetEventNumber = caerEventPacketHeaderGetEventNumber(packet); + int32_t packetEventCapacity = caerEventPacketHeaderGetEventCapacity(packet); + + int32_t appendPacketEventValid = caerEventPacketHeaderGetEventValid(appendPacket); + int32_t appendPacketEventNumber = caerEventPacketHeaderGetEventNumber(appendPacket); + int32_t appendPacketEventCapacity = caerEventPacketHeaderGetEventCapacity(appendPacket); + + int32_t eventSize = caerEventPacketHeaderGetEventSize(packet); // Is the same! Checked above. + size_t newEventPacketSize = CAER_EVENT_PACKET_HEADER_SIZE + + (size_t) ((packetEventCapacity + appendPacketEventCapacity) * eventSize); + + // Grow memory used to hold events. + packet = (caerEventPacketHeader) realloc(packet, newEventPacketSize); + if (packet == NULL) { + caerLog(CAER_LOG_CRITICAL, "Generic Event Packet", + "Failed to reallocate %zu bytes of memory for appending Event Packet of capacity %" + PRIi32 " to Event Packet of capacity %" PRIi32 ". Error: %d.", newEventPacketSize, + appendPacketEventCapacity, packetEventCapacity, errno); + return (NULL); + } + + // Copy appendPacket event memory at start of free space in packet. + memcpy(((uint8_t *) packet) + CAER_EVENT_PACKET_HEADER_SIZE + (packetEventNumber * eventSize), + ((uint8_t *) appendPacket) + CAER_EVENT_PACKET_HEADER_SIZE, (size_t) (appendPacketEventNumber * eventSize)); + + // Zero out remaining event memory (all events invalid). + memset( + ((uint8_t *) packet) + CAER_EVENT_PACKET_HEADER_SIZE + + ((packetEventNumber + appendPacketEventNumber) * eventSize), 0, + (size_t) (((packetEventCapacity + appendPacketEventCapacity) - (packetEventNumber + appendPacketEventNumber)) + * eventSize)); + + // Update header fields. + caerEventPacketHeaderSetEventValid(packet, (packetEventValid + appendPacketEventValid)); + caerEventPacketHeaderSetEventNumber(packet, (packetEventNumber + appendPacketEventNumber)); + caerEventPacketHeaderSetEventCapacity(packet, (packetEventCapacity + appendPacketEventCapacity)); + + return (packet); +} + +/** + * Get a generic pointer to an event, without having to know what event + * type the packet is containing. + * + * @param headerPtr a valid EventPacket header pointer. Cannot be NULL. + * @param n the index of the returned event. Must be within [0,eventCapacity[ bounds. + * + * @return a generic pointer to the requested event. NULL on error. + */ +static inline void *caerGenericEventGetEvent(caerEventPacketHeader headerPtr, int32_t n) { + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketHeaderGetEventCapacity(headerPtr)) { + caerLog(CAER_LOG_CRITICAL, "Generic Event", + "Called caerGenericEventGetEvent() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ". Negative values are not allowed!", + n, caerEventPacketHeaderGetEventCapacity(headerPtr) - 1); + return (NULL); + } + + // Return a pointer to the specified event. + return (((uint8_t *) headerPtr) + + (CAER_EVENT_PACKET_HEADER_SIZE + U64T(n * caerEventPacketHeaderGetEventSize(headerPtr)))); +} + +/** + * Get the main 32 bit timestamp for a generic event, without having to + * know what event type the packet is containing. + * + * @param eventPtr a generic pointer to an event. Cannot be NULL. + * @param headerPtr a valid EventPacket header pointer. Cannot be NULL. + * + * @return the main 32 bit timestamp of this event. + */ +static inline int32_t caerGenericEventGetTimestamp(void *eventPtr, caerEventPacketHeader headerPtr) { + return (le32toh(*((int32_t *) (((uint8_t *) eventPtr) + U64T(caerEventPacketHeaderGetEventTSOffset(headerPtr)))))); +} + +/** + * Get the main 64 bit timestamp for a generic event, without having to + * know what event type the packet is containing. This takes the + * per-packet timestamp into account too, generating a timestamp + * that doesn't suffer from overflow problems. + * + * @param eventPtr a generic pointer to an event. Cannot be NULL. + * @param headerPtr a valid EventPacket header pointer. Cannot be NULL. + * + * @return the main 64 bit timestamp of this event. + */ +static inline int64_t caerGenericEventGetTimestamp64(void *eventPtr, caerEventPacketHeader headerPtr) { + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(headerPtr)) << TS_OVERFLOW_SHIFT) | U64T(caerGenericEventGetTimestamp(eventPtr, headerPtr)))); +} + +/** + * Check if the given generic event is valid or not. + * + * @param eventPtr a generic pointer to an event. Cannot be NULL. + * + * @return true if the event is valid, false otherwise. + */ +static inline bool caerGenericEventIsValid(void *eventPtr) { + // Look at first byte of event memory's lowest bit. + // This should always work since first event member must contain the valid mark + // and memory is little-endian, so lowest bit must be in first byte of memory. + return (*((uint8_t *) eventPtr) & VALID_MARK_MASK); +} + +/** + * Generic iterator over all events in a packet. + * Returns the current index in the 'caerIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerIteratorElement' variable + * of type EVENT_TYPE. + * + * PACKET_HEADER: a valid EventPacket header pointer. Cannot be NULL. + * EVENT_TYPE: the event pointer type for this EventPacket (ie. caerPolarityEvent or caerFrameEvent). + */ +#define CAER_ITERATOR_ALL_START(PACKET_HEADER, EVENT_TYPE) \ + for (int32_t caerIteratorCounter = 0; \ + caerIteratorCounter < caerEventPacketHeaderGetEventNumber(PACKET_HEADER); \ + caerIteratorCounter++) { \ + EVENT_TYPE caerIteratorElement = (EVENT_TYPE) caerGenericEventGetEvent(PACKET_HEADER, caerIteratorCounter); + +/** + * Generic iterator close statement. + */ +#define CAER_ITERATOR_ALL_END } + +/** + * Generic iterator over only the valid events in a packet. + * Returns the current index in the 'caerIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerIteratorElement' variable + * of type EVENT_TYPE. + * + * PACKET_HEADER: a valid EventPacket header pointer. Cannot be NULL. + * EVENT_TYPE: the event pointer type for this EventPacket (ie. caerPolarityEvent or caerFrameEvent). + */ +#define CAER_ITERATOR_VALID_START(PACKET_HEADER, EVENT_TYPE) \ + for (int32_t caerIteratorCounter = 0; \ + caerIteratorCounter < caerEventPacketHeaderGetEventNumber(PACKET_HEADER); \ + caerIteratorCounter++) { \ + EVENT_TYPE caerIteratorElement = (EVENT_TYPE) caerGenericEventGetEvent(PACKET_HEADER, caerIteratorCounter); \ + if (!caerGenericEventIsValid(caerIteratorElement)) { continue; } // Skip invalid events. + +/** + * Generic iterator close statement. + */ +#define CAER_ITERATOR_VALID_END } + +// Functions for event packet copying. + +/** + * Make a full copy of an event packet (up to eventCapacity). + * + * @param eventPacket an event packet to copy. + * + * @return a full copy of an event packet. + */ +static inline void *caerCopyEventPacket(void *eventPacket) { + // Handle empty event packets. + if (eventPacket == NULL) { + return (NULL); + } + + // Calculate needed memory for new event packet. + caerEventPacketHeader header = (caerEventPacketHeader) eventPacket; + int32_t eventSize = caerEventPacketHeaderGetEventSize(header); + int32_t eventNumber = caerEventPacketHeaderGetEventNumber(header); + int32_t eventCapacity = caerEventPacketHeaderGetEventCapacity(header); + size_t packetMem = CAER_EVENT_PACKET_HEADER_SIZE + (size_t) (eventSize * eventCapacity); + size_t dataMem = CAER_EVENT_PACKET_HEADER_SIZE + (size_t) (eventSize * eventNumber); + + // Allocate memory for new event packet. + caerEventPacketHeader eventPacketCopy = (caerEventPacketHeader) malloc(packetMem); + if (eventPacketCopy == NULL) { + // Failed to allocate memory. + return (NULL); + } + + // Copy the data over. + memcpy(eventPacketCopy, eventPacket, dataMem); + + // Zero out the rest of the packet. + memset(((uint8_t *) eventPacketCopy) + dataMem, 0, packetMem - dataMem); + + return (eventPacketCopy); +} + +/** + * Make a copy of an event packet, sized down to only include the + * currently present events (eventNumber, valid+invalid), and not + * including the possible extra unused events (up to eventCapacity). + * + * @param eventPacket an event packet to copy. + * + * @return a sized down copy of an event packet. + */ +static inline void *caerCopyEventPacketOnlyEvents(void *eventPacket) { + // Handle empty event packets. + if (eventPacket == NULL) { + return (NULL); + } + + // Calculate needed memory for new event packet. + caerEventPacketHeader header = (caerEventPacketHeader) eventPacket; + int32_t eventSize = caerEventPacketHeaderGetEventSize(header); + int32_t eventNumber = caerEventPacketHeaderGetEventNumber(header); + + if (eventNumber == 0) { + // No copy possible if result is empty (capacity=0). + return (NULL); + } + + size_t packetMem = CAER_EVENT_PACKET_HEADER_SIZE + (size_t) (eventSize * eventNumber); + + // Allocate memory for new event packet. + caerEventPacketHeader eventPacketCopy = (caerEventPacketHeader) malloc(packetMem); + if (eventPacketCopy == NULL) { + // Failed to allocate memory. + return (NULL); + } + + // Copy the data over. + memcpy(eventPacketCopy, eventPacket, packetMem); + + // Set the event capacity to the event number, since we only allocated + // memory for that many events. + caerEventPacketHeaderSetEventCapacity(eventPacketCopy, eventNumber); + + return (eventPacketCopy); +} + +/** + * Make a copy of an event packet, sized down to only include the + * currently valid events (eventValid), and discarding everything else. + * + * @param eventPacket an event packet to copy. + * + * @return a copy of an event packet, containing only valid events. + */ +static inline void *caerCopyEventPacketOnlyValidEvents(void *eventPacket) { + // Handle empty event packets. + if (eventPacket == NULL) { + return (NULL); + } + + // Calculate needed memory for new event packet. + caerEventPacketHeader header = (caerEventPacketHeader) eventPacket; + int32_t eventSize = caerEventPacketHeaderGetEventSize(header); + int32_t eventValid = caerEventPacketHeaderGetEventValid(header); + + if (eventValid == 0) { + // No copy possible if result is empty (capacity=0). + return (NULL); + } + + size_t packetMem = CAER_EVENT_PACKET_HEADER_SIZE + (size_t) (eventSize * eventValid); + + // Allocate memory for new event packet. + caerEventPacketHeader eventPacketCopy = (caerEventPacketHeader) malloc(packetMem); + if (eventPacketCopy == NULL) { + // Failed to allocate memory. + return (NULL); + } + + // First copy over the header. + memcpy(eventPacketCopy, eventPacket, CAER_EVENT_PACKET_HEADER_SIZE); + + // Copy the data over. Must check every event for validity! + size_t offset = CAER_EVENT_PACKET_HEADER_SIZE; + + CAER_ITERATOR_VALID_START(header, void *) + memcpy(((uint8_t *) eventPacketCopy) + offset, caerIteratorElement, (size_t) eventSize); + offset += (size_t) eventSize; + } + + // Set the event capacity and the event number to the number of + // valid events, since we only copied those. + caerEventPacketHeaderSetEventCapacity(eventPacketCopy, eventValid); + caerEventPacketHeaderSetEventNumber(eventPacketCopy, eventValid); + + return (eventPacketCopy); +} + +/** + * Cleanup a packet by removing all invalid events, so that + * the total number of events is the number of valid events. + * The packet's capacity doesn't change. + * + * @param eventPacket an event packet to clean. + */ +static inline void caerCleanEventPacket(void *eventPacket) { + // Handle empty event packets. + if (eventPacket == NULL) { + return; + } + + // Calculate needed memory for new event packet. + caerEventPacketHeader header = (caerEventPacketHeader) eventPacket; + int32_t eventSize = caerEventPacketHeaderGetEventSize(header); + int32_t eventValid = caerEventPacketHeaderGetEventValid(header); + int32_t eventNumber = caerEventPacketHeaderGetEventNumber(header); + int32_t eventCapacity = caerEventPacketHeaderGetEventCapacity(header); + + // If we have no invalid events, we're already done. + if (eventValid == eventNumber) { + return; + } + + // Move all valid events close together. Must check every event for validity! + size_t offset = CAER_EVENT_PACKET_HEADER_SIZE; + + CAER_ITERATOR_VALID_START(header, void *) + void *dest = ((uint8_t *) header) + offset; + + if (dest != caerIteratorElement) { + memcpy(dest, caerIteratorElement, (size_t) eventSize); + offset += (size_t) eventSize; + } + } + + // Reset remaining memory, up to capacity, to zero (all events invalid). + memset(((uint8_t *) header) + offset, 0, (size_t) ((eventCapacity - eventValid) * eventSize)); + + // Event capacity remains unchanged, event number shrunk to event valid number. + caerEventPacketHeaderSetEventNumber(header, eventValid); +} + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_EVENTS_COMMON_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/config.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/config.h new file mode 100755 index 0000000000000000000000000000000000000000..c66c07aa2a437c43b27349f23d1dc70cf5f95c22 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/config.h @@ -0,0 +1,332 @@ +/** + * @file config.h + * + * Configuration Events format definition and handling functions. + * This event contains information about the current configuration of + * the device. By having configuration as a standardized event format, + * it becomes host-software agnostic, and it also becomes part of the + * event stream, enabling easy tracking of changes through time, by + * putting them into the event stream at the moment they happen. + * While the resolution of the timestamps for these events is in + * microseconds for compatibility with all other event types, the + * precision is in the order of ~1-20 milliseconds, given that these + * events are generated and injected on the host-side. + */ + +#ifndef LIBCAER_EVENTS_CONFIG_H_ +#define LIBCAER_EVENTS_CONFIG_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "common.h" + +/** + * Shift and mask values for the module address. + * Module address is only 7 bits, since the eighth bit + * is used device-side to differentiate reads from writes. + * Here we can just re-use it for the validity mark. + */ +//@{ +#define MODULE_ADDR_SHIFT 1 +#define MODULE_ADDR_MASK 0x0000007F +//@} + +/** + * Configuration event data structure definition. + * This contains the actual configuration module address, the + * parameter address and the actual parameter content, as + * well as the 32 bit event timestamp. + * Signed integers are used for fields that are to be interpreted + * directly, for compatibility with languages that do not have + * unsigned integer types, such as Java. + */ +struct caer_configuration_event { + /// Configuration module address. First (also) because of valid mark. + uint8_t moduleAddress; + /// Configuration parameter address. + uint8_t parameterAddress; + /// Configuration parameter content (4 bytes). + uint32_t parameter; + /// Event timestamp. + int32_t timestamp; +}__attribute__((__packed__)); + +/** + * Type for pointer to configuration event data structure. + */ +typedef struct caer_configuration_event *caerConfigurationEvent; + +/** + * Configuration event packet data structure definition. + * EventPackets are always made up of the common packet header, + * followed by 'eventCapacity' events. Everything has to + * be in one contiguous memory block. + */ +struct caer_configuration_event_packet { + /// The common event packet header. + struct caer_event_packet_header packetHeader; + /// The events array. + struct caer_configuration_event events[]; +}__attribute__((__packed__)); + +/** + * Type for pointer to configuration event packet data structure. + */ +typedef struct caer_configuration_event_packet *caerConfigurationEventPacket; + +/** + * Allocate a new configuration events packet. + * Use free() to reclaim this memory. + * + * @param eventCapacity the maximum number of events this packet will hold. + * @param eventSource the unique ID representing the source/generator of this packet. + * @param tsOverflow the current timestamp overflow counter value for this packet. + * + * @return a valid ConfigurationEventPacket handle or NULL on error. + */ +caerConfigurationEventPacket caerConfigurationEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, + int32_t tsOverflow); + +/** + * Get the configuration event at the given index from the event packet. + * + * @param packet a valid ConfigurationEventPacket pointer. Cannot be NULL. + * @param n the index of the returned event. Must be within [0,eventCapacity[ bounds. + * + * @return the requested configuration event. NULL on error. + */ +static inline caerConfigurationEvent caerConfigurationEventPacketGetEvent(caerConfigurationEventPacket packet, + int32_t n) { + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketHeaderGetEventCapacity(&packet->packetHeader)) { + caerLog(CAER_LOG_CRITICAL, "Configuration Event", + "Called caerConfigurationEventPacketGetEvent() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ".", + n, caerEventPacketHeaderGetEventCapacity(&packet->packetHeader) - 1); + return (NULL); + } + + // Return a pointer to the specified event. + return (packet->events + n); +} + +/** + * Get the 32bit event timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid ConfigurationEvent pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond timestamp. + */ +static inline int32_t caerConfigurationEventGetTimestamp(caerConfigurationEvent event) { + return (le32toh(event->timestamp)); +} + +/** + * Get the 64bit event timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid ConfigurationEvent pointer. Cannot be NULL. + * @param packet the ConfigurationEventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond timestamp. + */ +static inline int64_t caerConfigurationEventGetTimestamp64(caerConfigurationEvent event, + caerConfigurationEventPacket packet) { + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerConfigurationEventGetTimestamp(event)))); +} + +/** + * Set the 32bit event timestamp, the value has to be in microseconds. + * + * @param event a valid ConfigurationEvent pointer. Cannot be NULL. + * @param timestamp a positive 32bit microsecond timestamp. + */ +static inline void caerConfigurationEventSetTimestamp(caerConfigurationEvent event, int32_t timestamp) { + if (timestamp < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "Configuration Event", + "Called caerConfigurationEventSetTimestamp() with negative value!"); + return; + } + + event->timestamp = htole32(timestamp); +} + +/** + * Check if this configuration event is valid. + * + * @param event a valid ConfigurationEvent pointer. Cannot be NULL. + * + * @return true if valid, false if not. + */ +static inline bool caerConfigurationEventIsValid(caerConfigurationEvent event) { + return (GET_NUMBITS8(event->moduleAddress, VALID_MARK_SHIFT, VALID_MARK_MASK)); +} + +/** + * Validate the current event by setting its valid bit to true + * and increasing the event packet's event count and valid + * event count. Only works on events that are invalid. + * DO NOT CALL THIS AFTER HAVING PREVIOUSLY ALREADY + * INVALIDATED THIS EVENT, the total count will be incorrect. + * + * @param event a valid ConfigurationEvent pointer. Cannot be NULL. + * @param packet the ConfigurationEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerConfigurationEventValidate(caerConfigurationEvent event, caerConfigurationEventPacket packet) { + if (!caerConfigurationEventIsValid(event)) { + SET_NUMBITS8(event->moduleAddress, VALID_MARK_SHIFT, VALID_MARK_MASK, 1); + + // Also increase number of events and valid events. + // Only call this on (still) invalid events! + caerEventPacketHeaderSetEventNumber(&packet->packetHeader, + caerEventPacketHeaderGetEventNumber(&packet->packetHeader) + 1); + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) + 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Configuration Event", + "Called caerConfigurationEventValidate() on already valid event."); + } +} + +/** + * Invalidate the current event by setting its valid bit + * to false and decreasing the number of valid events held + * in the packet. Only works with events that are already + * valid! + * + * @param event a valid ConfigurationEvent pointer. Cannot be NULL. + * @param packet the ConfigurationEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerConfigurationEventInvalidate(caerConfigurationEvent event, caerConfigurationEventPacket packet) { + if (caerConfigurationEventIsValid(event)) { + CLEAR_NUMBITS8(event->moduleAddress, VALID_MARK_SHIFT, VALID_MARK_MASK); + + // Also decrease number of valid events. Number of total events doesn't change. + // Only call this on valid events! + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) - 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Configuration Event", + "Called caerConfigurationEventInvalidate() on already invalid event."); + } +} + +/** + * Get the configuration event's module address. + * + * @param event a valid ConfigurationEvent pointer. Cannot be NULL. + * + * @return configuration module address. + */ +static inline uint8_t caerConfigurationEventGetModuleAddress(caerConfigurationEvent event) { + return U8T(GET_NUMBITS8(event->moduleAddress, MODULE_ADDR_SHIFT, MODULE_ADDR_MASK)); +} + +/** + * Set the configuration event's module address. + * + * @param event a valid ConfigurationEvent pointer. Cannot be NULL. + * @param moduleAddress configuration module address. + */ +static inline void caerConfigurationEventSetModuleAddress(caerConfigurationEvent event, uint8_t moduleAddress) { + CLEAR_NUMBITS8(event->moduleAddress, MODULE_ADDR_SHIFT, MODULE_ADDR_MASK); + SET_NUMBITS8(event->moduleAddress, MODULE_ADDR_SHIFT, MODULE_ADDR_MASK, moduleAddress); +} + +/** + * Get the configuration event's parameter address. + * + * @param event a valid ConfigurationEvent pointer. Cannot be NULL. + * + * @return configuration parameter address. + */ +static inline uint8_t caerConfigurationEventGetParameterAddress(caerConfigurationEvent event) { + return (event->parameterAddress); +} + +/** + * Set the configuration event's parameter address. + * + * @param event a valid ConfigurationEvent pointer. Cannot be NULL. + * @param parameterAddress configuration parameter address. + */ +static inline void caerConfigurationEventSetParameterAddress(caerConfigurationEvent event, uint8_t parameterAddress) { + event->parameterAddress = parameterAddress; +} + +/** + * Get the configuration event's parameter. + * + * @param event a valid ConfigurationEvent pointer. Cannot be NULL. + * + * @return configuration parameter. + */ +static inline uint32_t caerConfigurationEventGetParameter(caerConfigurationEvent event) { + return (le32toh(event->parameter)); +} + +/** + * Set the configuration event's parameter. + * + * @param event a valid ConfigurationEvent pointer. Cannot be NULL. + * @param parameter configuration parameter. + */ +static inline void caerConfigurationEventSetParameter(caerConfigurationEvent event, uint32_t parameter) { + event->parameter = htole32(parameter); +} + +/** + * Iterator over all configuration events in a packet. + * Returns the current index in the 'caerConfigurationIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerConfigurationIteratorElement' variable + * of type caerConfigurationEvent. + * + * CONFIGURATION_PACKET: a valid ConfigurationEventPacket pointer. Cannot be NULL. + */ +#define CAER_CONFIGURATION_ITERATOR_ALL_START(CONFIGURATION_PACKET) \ + for (int32_t caerConfigurationIteratorCounter = 0; \ + caerConfigurationIteratorCounter < caerEventPacketHeaderGetEventNumber(&(CONFIGURATION_PACKET)->packetHeader); \ + caerConfigurationIteratorCounter++) { \ + caerConfigurationEvent caerConfigurationIteratorElement = caerConfigurationEventPacketGetEvent(CONFIGURATION_PACKET, caerConfigurationIteratorCounter); + +/** + * Iterator close statement. + */ +#define CAER_CONFIGURATION_ITERATOR_ALL_END } + +/** + * Iterator over only the valid configuration events in a packet. + * Returns the current index in the 'caerConfigurationIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerConfigurationIteratorElement' variable + * of type caerConfigurationEvent. + * + * CONFIGURATION_PACKET: a valid ConfigurationEventPacket pointer. Cannot be NULL. + */ +#define CAER_CONFIGURATION_ITERATOR_VALID_START(CONFIGURATION_PACKET) \ + for (int32_t caerConfigurationIteratorCounter = 0; \ + caerConfigurationIteratorCounter < caerEventPacketHeaderGetEventNumber(&(CONFIGURATION_PACKET)->packetHeader); \ + caerConfigurationIteratorCounter++) { \ + caerConfigurationEvent caerConfigurationIteratorElement = caerConfigurationEventPacketGetEvent(CONFIGURATION_PACKET, caerConfigurationIteratorCounter); \ + if (!caerConfigurationEventIsValid(caerConfigurationIteratorElement)) { continue; } // Skip invalid configuration events. + +/** + * Iterator close statement. + */ +#define CAER_CONFIGURATION_ITERATOR_VALID_END } + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_EVENTS_CONFIG_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/ear.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/ear.h new file mode 100755 index 0000000000000000000000000000000000000000..1556f8b0ad340d34363a222f228eecae15355dd6 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/ear.h @@ -0,0 +1,334 @@ +/** + * @file ear.h + * + * Ear (Cochlea) Events format definition and handling functions. + * This encodes events from a silicon cochlea chip, containing + * information about which ear (microphone) generated the event, + * as well as which channel was involved and additional information + * on filters and neurons. + */ + +#ifndef LIBCAER_EVENTS_EAR_H_ +#define LIBCAER_EVENTS_EAR_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "common.h" + +/** + * Shift and mask values for the ear event values coming from + * a cochlea: the ear position (up to 16), the channel number + * (up to 2048), the ganglion (up to 256) and the filter (up to 256). + * Bit 0 is the valid mark, see 'common.h' for more details. + */ +//@{ +#define EAR_SHIFT 1 +#define EAR_MASK 0x0000000F +#define CHANNEL_SHIFT 5 +#define CHANNEL_MASK 0x000007FF +#define NEURON_SHIFT 16 +#define NEURON_MASK 0x000000FF +#define FILTER_SHIFT 24 +#define FILTER_MASK 0x000000FF +//@} + +/** + * Ear (cochlea) event data structure definition. + * Contains information on events gotten from a cochlea chip: + * ears, channels, neurons and filters are stored. + * Signed integers are used for fields that are to be interpreted + * directly, for compatibility with languages that do not have + * unsigned integer types, such as Java. + */ +struct caer_ear_event { + /// Event data. First because of valid mark. + uint32_t data; + /// Event timestamp. + int32_t timestamp; +}__attribute__((__packed__)); + +/** + * Type for pointer to ear (cochlea) event data structure. + */ +typedef struct caer_ear_event *caerEarEvent; + +/** + * Ear (cochlea) event packet data structure definition. + * EventPackets are always made up of the common packet header, + * followed by 'eventCapacity' events. Everything has to + * be in one contiguous memory block. + */ +struct caer_ear_event_packet { + /// The common event packet header. + struct caer_event_packet_header packetHeader; + /// The events array. + struct caer_ear_event events[]; +}__attribute__((__packed__)); + +/** + * Type for pointer to ear (cochlea) event packet data structure. + */ +typedef struct caer_ear_event_packet *caerEarEventPacket; + +/** + * Allocate a new ear (cochlea) events packet. + * Use free() to reclaim this memory. + * + * @param eventCapacity the maximum number of events this packet will hold. + * @param eventSource the unique ID representing the source/generator of this packet. + * @param tsOverflow the current timestamp overflow counter value for this packet. + * + * @return a valid EarEventPacket handle or NULL on error. + */ +caerEarEventPacket caerEarEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow); + +/** + * Get the ear (cochlea) event at the given index from the event packet. + * + * @param packet a valid EarEventPacket pointer. Cannot be NULL. + * @param n the index of the returned event. Must be within [0,eventCapacity[ bounds. + * + * @return the requested ear (cochlea) event. NULL on error. + */ +static inline caerEarEvent caerEarEventPacketGetEvent(caerEarEventPacket packet, int32_t n) { + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketHeaderGetEventCapacity(&packet->packetHeader)) { + caerLog(CAER_LOG_CRITICAL, "Ear Event", + "Called caerEarEventPacketGetEvent() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ".", + n, caerEventPacketHeaderGetEventCapacity(&packet->packetHeader) - 1); + return (NULL); + } + + // Return a pointer to the specified event. + return (packet->events + n); +} + +/** + * Get the 32bit event timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid EarEvent pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond timestamp. + */ +static inline int32_t caerEarEventGetTimestamp(caerEarEvent event) { + return (le32toh(event->timestamp)); +} + +/** + * Get the 64bit event timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid EarEvent pointer. Cannot be NULL. + * @param packet the EarEventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond timestamp. + */ +static inline int64_t caerEarEventGetTimestamp64(caerEarEvent event, caerEarEventPacket packet) { + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerEarEventGetTimestamp(event)))); +} + +/** + * Set the 32bit event timestamp, the value has to be in microseconds. + * + * @param event a valid EarEvent pointer. Cannot be NULL. + * @param timestamp a positive 32bit microsecond timestamp. + */ +static inline void caerEarEventSetTimestamp(caerEarEvent event, int32_t timestamp) { + if (timestamp < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "Ear Event", "Called caerEarEventSetTimestamp() with negative value!"); + return; + } + + event->timestamp = htole32(timestamp); +} + +/** + * Check if this ear (cochlea) event is valid. + * + * @param event a valid EarEvent pointer. Cannot be NULL. + * + * @return true if valid, false if not. + */ +static inline bool caerEarEventIsValid(caerEarEvent event) { + return (GET_NUMBITS32(event->data, VALID_MARK_SHIFT, VALID_MARK_MASK)); +} + +/** + * Validate the current event by setting its valid bit to true + * and increasing the event packet's event count and valid + * event count. Only works on events that are invalid. + * DO NOT CALL THIS AFTER HAVING PREVIOUSLY ALREADY + * INVALIDATED THIS EVENT, the total count will be incorrect. + * + * @param event a valid EarEvent pointer. Cannot be NULL. + * @param packet the EarEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerEarEventValidate(caerEarEvent event, caerEarEventPacket packet) { + if (!caerEarEventIsValid(event)) { + SET_NUMBITS32(event->data, VALID_MARK_SHIFT, VALID_MARK_MASK, 1); + + // Also increase number of events and valid events. + // Only call this on (still) invalid events! + caerEventPacketHeaderSetEventNumber(&packet->packetHeader, + caerEventPacketHeaderGetEventNumber(&packet->packetHeader) + 1); + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) + 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Ear Event", "Called caerEarEventValidate() on already valid event."); + } +} + +/** + * Invalidate the current event by setting its valid bit + * to false and decreasing the number of valid events held + * in the packet. Only works with events that are already + * valid! + * + * @param event a valid EarEvent pointer. Cannot be NULL. + * @param packet the EarEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerEarEventInvalidate(caerEarEvent event, caerEarEventPacket packet) { + if (caerEarEventIsValid(event)) { + CLEAR_NUMBITS32(event->data, VALID_MARK_SHIFT, VALID_MARK_MASK); + + // Also decrease number of valid events. Number of total events doesn't change. + // Only call this on valid events! + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) - 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Ear Event", "Called caerEarEventInvalidate() on already invalid event."); + } +} + +/** + * Get the numerical ID of the ear (microphone). + * Usually, 0 is left, 1 is right for 2 ear cochleas. + * For 4 ear cochleas, 0 is front left, 1 is front right, + * 2 is back left and 3 is back right. + * + * @param event a valid EarEvent pointer. Cannot be NULL. + * + * @return the ear (microphone) ID. + */ +static inline uint8_t caerEarEventGetEar(caerEarEvent event) { + return U8T(GET_NUMBITS32(event->data, EAR_SHIFT, EAR_MASK)); +} + +/** + * Set the numerical ID of the ear (microphone). + * Usually, 0 is left, 1 is right for 2 ear cochleas. + * For 4 ear cochleas, 0 is front left, 1 is front right, + * 2 is back left and 3 is back right. + * + * @param event a valid EarEvent pointer. Cannot be NULL. + * @param ear the ear (microphone) ID. + */ +static inline void caerEarEventSetEar(caerEarEvent event, uint8_t ear) { + CLEAR_NUMBITS32(event->data, EAR_SHIFT, EAR_MASK); + SET_NUMBITS32(event->data, EAR_SHIFT, EAR_MASK, ear); +} + +/** + * Get the channel (frequency band) ID. + * The channels count from 0 upward, where 0 is the highest + * frequency channel, while higher numbers are progressively + * lower frequency channels. This is derived from how the actual + * human ear works. + * + * @param event a valid EarEvent pointer. Cannot be NULL. + * + * @return the channel (frequency band) ID. + */ +static inline uint16_t caerEarEventGetChannel(caerEarEvent event) { + return U16T(GET_NUMBITS32(event->data, CHANNEL_SHIFT, CHANNEL_MASK)); +} + +/** + * Set the channel (frequency band) ID. + * The channels count from 0 upward, where 0 is the highest + * frequency channel, while higher numbers are progressively + * lower frequency channels. This is derived from how the actual + * human ear works. + * + * @param event a valid EarEvent pointer. Cannot be NULL. + * @param channel the channel (frequency band) ID. + */ +static inline void caerEarEventSetChannel(caerEarEvent event, uint16_t channel) { + CLEAR_NUMBITS32(event->data, CHANNEL_SHIFT, CHANNEL_MASK); + SET_NUMBITS32(event->data, CHANNEL_SHIFT, CHANNEL_MASK, channel); +} + +static inline uint8_t caerEarEventGetNeuron(caerEarEvent event) { + return U8T(GET_NUMBITS32(event->data, NEURON_SHIFT, NEURON_MASK)); +} + +static inline void caerEarEventSetNeuron(caerEarEvent event, uint8_t neuron) { + CLEAR_NUMBITS32(event->data, NEURON_SHIFT, NEURON_MASK); + SET_NUMBITS32(event->data, NEURON_SHIFT, NEURON_MASK, neuron); +} + +static inline uint8_t caerEarEventGetFilter(caerEarEvent event) { + return U8T(GET_NUMBITS32(event->data, FILTER_SHIFT, FILTER_MASK)); +} + +static inline void caerEarEventSetFilter(caerEarEvent event, uint8_t filter) { + CLEAR_NUMBITS32(event->data, FILTER_SHIFT, FILTER_MASK); + SET_NUMBITS32(event->data, FILTER_SHIFT, FILTER_MASK, filter); +} + +/** + * Iterator over all ear events in a packet. + * Returns the current index in the 'caerEarIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerEarIteratorElement' variable + * of type caerEarEvent. + * + * EAR_PACKET: a valid EarEventPacket pointer. Cannot be NULL. + */ +#define CAER_EAR_ITERATOR_ALL_START(EAR_PACKET) \ + for (int32_t caerEarIteratorCounter = 0; \ + caerEarIteratorCounter < caerEventPacketHeaderGetEventNumber(&(EAR_PACKET)->packetHeader); \ + caerEarIteratorCounter++) { \ + caerEarEvent caerEarIteratorElement = caerEarEventPacketGetEvent(EAR_PACKET, caerEarIteratorCounter); + +/** + * Iterator close statement. + */ +#define CAER_EAR_ITERATOR_ALL_END } + +/** + * Iterator over only the valid ear events in a packet. + * Returns the current index in the 'caerEarIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerEarIteratorElement' variable + * of type caerEarEvent. + * + * EAR_PACKET: a valid EarEventPacket pointer. Cannot be NULL. + */ +#define CAER_EAR_ITERATOR_VALID_START(EAR_PACKET) \ + for (int32_t caerEarIteratorCounter = 0; \ + caerEarIteratorCounter < caerEventPacketHeaderGetEventNumber(&(EAR_PACKET)->packetHeader); \ + caerEarIteratorCounter++) { \ + caerEarEvent caerEarIteratorElement = caerEarEventPacketGetEvent(EAR_PACKET, caerEarIteratorCounter); \ + if (!caerEarEventIsValid(caerEarIteratorElement)) { continue; } // Skip invalid ear events. + +/** + * Iterator close statement. + */ +#define CAER_EAR_ITERATOR_VALID_END } + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_EVENTS_EAR_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/frame.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/frame.h new file mode 100755 index 0000000000000000000000000000000000000000..ed15e6d4aff5165fffe52b9bb869b3cae3a9f5a7 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/frame.h @@ -0,0 +1,1019 @@ +/** + * @file frame.h + * + * Frame Events format definition and handling functions. + * This event type encodes intensity frames, like you would + * get from a normal APS camera. It supports multiple channels + * for color, color filter information, as well as multiple + * Regions of Interest (ROI). + * The (0, 0) pixel is in the upper left corner of the screen, + * like in OpenCV/computer graphics. The pixel array is laid out row by row + * (increasing X axis), going from top to bottom (increasing Y axis). + */ + +#ifndef LIBCAER_EVENTS_FRAME_H_ +#define LIBCAER_EVENTS_FRAME_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "common.h" + +/** + * Shift and mask values for the color channels number, the color + * filter arrangement and the ROI identifier contained in the + * 'info' field of the frame event. + * Multiple channels (RGB for example) are possible, see the + * 'enum caer_frame_event_color_channels'. To understand the original + * color filter arrangement to interpolate color images, see + * the 'enum caer_frame_event_color_filter'. + * Also, up to 128 different Regions of Interest (ROI) can be tracked. + * Bit 0 is the valid mark, see 'common.h' for more details. + */ +//@{ +#define COLOR_CHANNELS_SHIFT 1 +#define COLOR_CHANNELS_MASK 0x00000007 +#define COLOR_FILTER_SHIFT 4 +#define COLOR_FILTER_MASK 0x0000000F +#define ROI_IDENTIFIER_SHIFT 8 +#define ROI_IDENTIFIER_MASK 0x0000007F +//@} + +/** + * List of all frame event color channel identifiers. + * Used to interpret the frame event color channel field. + */ +enum caer_frame_event_color_channels { + GRAYSCALE = 1, //!< Grayscale, one channel only. + RGB = 3, //!< Red Green Blue, 3 color channels. + RGBA = 4, //!< Red Green Blue Alpha, 3 color channels plus transparency. +}; + +/** + * List of all frame event color filter identifiers. + * Used to interpret the frame event color filter field. + */ +enum caer_frame_event_color_filter { + MONO = 0, //!< No color filter present, all light passes. + RGBG = 1, //!< Standard Bayer color filter, 1 red 2 green 1 blue. Variation 1. + GRGB = 2, //!< Standard Bayer color filter, 1 red 2 green 1 blue. Variation 2. + GBGR = 3, //!< Standard Bayer color filter, 1 red 2 green 1 blue. Variation 3. + BGRG = 4, //!< Standard Bayer color filter, 1 red 2 green 1 blue. Variation 4. + RGBW = 5, //!< Modified Bayer color filter, with white (pass all light) instead of extra green. Variation 1. + GRWB = 6, //!< Modified Bayer color filter, with white (pass all light) instead of extra green. Variation 2. + WBGR = 7, //!< Modified Bayer color filter, with white (pass all light) instead of extra green. Variation 3. + BWRG = 8, //!< Modified Bayer color filter, with white (pass all light) instead of extra green. Variation 4. +}; + +/** + * Frame event data structure definition. + * This contains the actual information on the frame (ROI, color channels, + * color filter), several timestamps to signal start and end of capture and + * of exposure, as well as the actual pixels, in a 16 bit normalized format. + * The (0, 0) address is in the upper left corner, like in OpenCV/computer graphics. + * The pixel array is laid out row by row (increasing X axis), going from + * top to bottom (increasing Y axis). + * Signed integers are used for fields that are to be interpreted + * directly, for compatibility with languages that do not have + * unsigned integer types, such as Java. + */ +struct caer_frame_event { + /// Event information (ROI region, color channels, color filter). First because of valid mark. + uint32_t info; + /// Start of Frame (SOF) timestamp. + int32_t ts_startframe; + /// End of Frame (EOF) timestamp. + int32_t ts_endframe; + /// Start of Exposure (SOE) timestamp. + int32_t ts_startexposure; + /// End of Exposure (EOE) timestamp. + int32_t ts_endexposure; + /// X axis length in pixels. + int32_t lengthX; + /// Y axis length in pixels. + int32_t lengthY; + /// X axis position (upper left offset) in pixels. + int32_t positionX; + /// Y axis position (upper left offset) in pixels. + int32_t positionY; + /// Pixel array, 16 bit unsigned integers, normalized to 16 bit depth. + /// The pixel array is laid out row by row (increasing X axis), going + /// from top to bottom (increasing Y axis). + uint16_t pixels[]; +}__attribute__((__packed__)); + +/** + * Type for pointer to frame event data structure. + */ +typedef struct caer_frame_event *caerFrameEvent; + +/** + * Frame event packet data structure definition. + * EventPackets are always made up of the common packet header, + * followed by 'eventCapacity' events. Everything has to + * be in one contiguous memory block. Direct access to the events + * array is not possible for Frame events. To calculate position + * offsets, use the 'eventSize' field in the packet header. + */ +struct caer_frame_event_packet { + /// The common event packet header. + struct caer_event_packet_header packetHeader; +/// All events follow here. Direct access to the events +/// array is not possible. To calculate position, use the +/// 'eventSize' field in the packetHeader. +}__attribute__((__packed__)); + +/** + * Type for pointer to frame event packet data structure. + */ +typedef struct caer_frame_event_packet *caerFrameEventPacket; + +/** + * Allocate a new frame events packet. + * Use free() to reclaim this memory. + * The frame events allocate memory for a maximum sized pixels array, depending + * on the parameters passed to this function, so that every event occupies the + * same amount of memory (constant size). The actual frames inside of it + * might be smaller than that, for example when using ROI, and their actual size + * is stored inside the frame event and should always be queried from there. + * The unused part of a pixels array is guaranteed to be zeros. + * + * @param eventCapacity the maximum number of events this packet will hold. + * @param eventSource the unique ID representing the source/generator of this packet. + * @param tsOverflow the current timestamp overflow counter value for this packet. + * @param maxLengthX the maximum expected X axis size for frames in this packet. + * @param maxLengthY the maximum expected Y axis size for frames in this packet. + * @param maxChannelNumber the maximum expected number of channels for frames in this packet. + * + * @return a valid FrameEventPacket handle or NULL on error. + */ +caerFrameEventPacket caerFrameEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow, + int32_t maxLengthX, int32_t maxLengthY, int16_t maxChannelNumber); + +/** + * Get the frame event at the given index from the event packet. + * + * @param packet a valid FrameEventPacket pointer. Cannot be NULL. + * @param n the index of the returned event. Must be within [0,eventCapacity[ bounds. + * + * @return the requested frame event. NULL on error. + */ +static inline caerFrameEvent caerFrameEventPacketGetEvent(caerFrameEventPacket packet, int32_t n) { + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketHeaderGetEventCapacity(&packet->packetHeader)) { + caerLog(CAER_LOG_CRITICAL, "Frame Event", + "Called caerFrameEventPacketGetEvent() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ".", + n, caerEventPacketHeaderGetEventCapacity(&packet->packetHeader) - 1); + return (NULL); + } + + // Return a pointer to the specified event. + return ((caerFrameEvent) (((uint8_t *) &packet->packetHeader) + + (CAER_EVENT_PACKET_HEADER_SIZE + U64T(n * caerEventPacketHeaderGetEventSize(&packet->packetHeader))))); +} + +/** + * Get the 32bit start of frame capture timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond start of frame timestamp. + */ +static inline int32_t caerFrameEventGetTSStartOfFrame(caerFrameEvent event) { + return (le32toh(event->ts_startframe)); +} + +/** + * Get the 64bit start of frame capture timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param packet the FrameEventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond start of frame timestamp. + */ +static inline int64_t caerFrameEventGetTSStartOfFrame64(caerFrameEvent event, caerFrameEventPacket packet) { + // Even if frames have multiple time-stamps, it's not possible for later time-stamps to + // be in a different TSOverflow period, since in those rare cases the event is dropped. + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerFrameEventGetTSStartOfFrame(event)))); +} + +/** + * Set the 32bit start of frame capture timestamp, the value has to be in microseconds. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param startFrame a positive 32bit microsecond timestamp. + */ +static inline void caerFrameEventSetTSStartOfFrame(caerFrameEvent event, int32_t startFrame) { + if (startFrame < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "Frame Event", "Called caerFrameEventSetTSStartOfFrame() with negative value!"); + return; + } + + event->ts_startframe = htole32(startFrame); +} + +/** + * Get the 32bit end of frame capture timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond end of frame timestamp. + */ +static inline int32_t caerFrameEventGetTSEndOfFrame(caerFrameEvent event) { + return (le32toh(event->ts_endframe)); +} + +/** + * Get the 64bit end of frame capture timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param packet the FrameEventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond end of frame timestamp. + */ +static inline int64_t caerFrameEventGetTSEndOfFrame64(caerFrameEvent event, caerFrameEventPacket packet) { + // Even if frames have multiple time-stamps, it's not possible for later time-stamps to + // be in a different TSOverflow period, since in those rare cases the event is dropped. + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerFrameEventGetTSEndOfFrame(event)))); +} + +/** + * Set the 32bit end of frame capture timestamp, the value has to be in microseconds. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param endFrame a positive 32bit microsecond timestamp. + */ +static inline void caerFrameEventSetTSEndOfFrame(caerFrameEvent event, int32_t endFrame) { + if (endFrame < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "Frame Event", "Called caerFrameEventSetTSEndOfFrame() with negative value!"); + return; + } + + event->ts_endframe = htole32(endFrame); +} + +/** + * Get the 32bit start of exposure timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond start of exposure timestamp. + */ +static inline int32_t caerFrameEventGetTSStartOfExposure(caerFrameEvent event) { + return (le32toh(event->ts_startexposure)); +} + +/** + * Get the 64bit start of exposure timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param packet the FrameEventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond start of exposure timestamp. + */ +static inline int64_t caerFrameEventGetTSStartOfExposure64(caerFrameEvent event, caerFrameEventPacket packet) { + // Even if frames have multiple time-stamps, it's not possible for later time-stamps to + // be in a different TSOverflow period, since in those rare cases the event is dropped. + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerFrameEventGetTSStartOfExposure(event)))); +} + +/** + * Set the 32bit start of exposure timestamp, the value has to be in microseconds. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param startExposure a positive 32bit microsecond timestamp. + */ +static inline void caerFrameEventSetTSStartOfExposure(caerFrameEvent event, int32_t startExposure) { + if (startExposure < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "Frame Event", "Called caerFrameEventSetTSStartOfExposure() with negative value!"); + return; + } + + event->ts_startexposure = htole32(startExposure); +} + +/** + * Get the 32bit end of exposure timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond end of exposure timestamp. + */ +static inline int32_t caerFrameEventGetTSEndOfExposure(caerFrameEvent event) { + return (le32toh(event->ts_endexposure)); +} + +/** + * Get the 64bit end of exposure timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param packet the FrameEventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond end of exposure timestamp. + */ +static inline int64_t caerFrameEventGetTSEndOfExposure64(caerFrameEvent event, caerFrameEventPacket packet) { + // Even if frames have multiple time-stamps, it's not possible for later time-stamps to + // be in a different TSOverflow period, since in those rare cases the event is dropped. + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerFrameEventGetTSEndOfExposure(event)))); +} + +/** + * Set the 32bit end of exposure timestamp, the value has to be in microseconds. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param endExposure a positive 32bit microsecond timestamp. + */ +static inline void caerFrameEventSetTSEndOfExposure(caerFrameEvent event, int32_t endExposure) { + if (endExposure < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "Frame Event", "Called caerFrameEventSetTSEndOfExposure() with negative value!"); + return; + } + + event->ts_endexposure = htole32(endExposure); +} + +/** + * The total length, in microseconds, of the frame exposure time. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return the exposure time in microseconds. + */ +static inline int32_t caerFrameEventGetExposureLength(caerFrameEvent event) { + return (caerFrameEventGetTSEndOfExposure(event) - caerFrameEventGetTSStartOfExposure(event)); +} + +/** + * Get the 32bit event timestamp, in microseconds. + * This is a median of the exposure timestamps. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond timestamp. + */ +static inline int32_t caerFrameEventGetTimestamp(caerFrameEvent event) { + return (caerFrameEventGetTSStartOfExposure(event) + (caerFrameEventGetExposureLength(event) / 2)); +} + +/** + * Get the 64bit event timestamp, in microseconds. + * This is a median of the exposure timestamps. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param packet the FrameEventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond timestamp. + */ +static inline int64_t caerFrameEventGetTimestamp64(caerFrameEvent event, caerFrameEventPacket packet) { + // Even if frames have multiple time-stamps, it's not possible for later time-stamps to + // be in a different TSOverflow period, since in those rare cases the event is dropped. + return (caerFrameEventGetTSStartOfExposure64(event, packet) + (caerFrameEventGetExposureLength(event) / 2)); +} + +/** + * Check if this frame event is valid. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return true if valid, false if not. + */ +static inline bool caerFrameEventIsValid(caerFrameEvent event) { + return (GET_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK)); +} + +/** + * Validate the current event by setting its valid bit to true + * and increasing the event packet's event count and valid + * event count. Only works on events that are invalid. + * DO NOT CALL THIS AFTER HAVING PREVIOUSLY ALREADY + * INVALIDATED THIS EVENT, the total count will be incorrect. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param packet the FrameEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerFrameEventValidate(caerFrameEvent event, caerFrameEventPacket packet) { + if (!caerFrameEventIsValid(event)) { + SET_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK, 1); + + // Also increase number of events and valid events. + // Only call this on (still) invalid events! + caerEventPacketHeaderSetEventNumber(&packet->packetHeader, + caerEventPacketHeaderGetEventNumber(&packet->packetHeader) + 1); + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) + 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Frame Event", "Called caerFrameEventValidate() on already valid event."); + } +} + +/** + * Invalidate the current event by setting its valid bit + * to false and decreasing the number of valid events held + * in the packet. Only works with events that are already + * valid! + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param packet the FrameEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerFrameEventInvalidate(caerFrameEvent event, caerFrameEventPacket packet) { + if (caerFrameEventIsValid(event)) { + CLEAR_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK); + + // Also decrease number of valid events. Number of total events doesn't change. + // Only call this on valid events! + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) - 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Frame Event", "Called caerFrameEventInvalidate() on already invalid event."); + } +} + +/** + * Get the maximum size of the pixels array in bytes, based upon how + * much memory was allocated to it by 'caerFrameEventPacketAllocate()'. + * + * @param packet a valid FrameEventPacket pointer. Cannot be NULL. + * + * @return maximum pixels array size in bytes. + */ +static inline size_t caerFrameEventPacketGetPixelsSize(caerFrameEventPacket packet) { + return ((size_t) caerEventPacketHeaderGetEventSize(&packet->packetHeader) - sizeof(struct caer_frame_event)); +} + +/** + * Get the maximum index into the pixels array, based upon how + * much memory was allocated to it by 'caerFrameEventPacketAllocate()'. + * + * @param packet a valid FrameEventPacket pointer. Cannot be NULL. + * + * @return maximum pixels array index. + */ +static inline size_t caerFrameEventPacketGetPixelsMaxIndex(caerFrameEventPacket packet) { + return (caerFrameEventPacketGetPixelsSize(packet) / sizeof(uint16_t)); +} + +/** + * Get the numerical identifier for the Region of Interest + * (ROI) region, to distinguish between multiple of them. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return numerical ROI identifier. + */ +static inline uint8_t caerFrameEventGetROIIdentifier(caerFrameEvent event) { + return U8T(GET_NUMBITS32(event->info, ROI_IDENTIFIER_SHIFT, ROI_IDENTIFIER_MASK)); +} + +/** + * Set the numerical identifier for the Region of Interest + * (ROI) region, to distinguish between multiple of them. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param roiIdentifier numerical ROI identifier. + */ +static inline void caerFrameEventSetROIIdentifier(caerFrameEvent event, uint8_t roiIdentifier) { + CLEAR_NUMBITS32(event->info, ROI_IDENTIFIER_SHIFT, ROI_IDENTIFIER_MASK); + SET_NUMBITS32(event->info, ROI_IDENTIFIER_SHIFT, ROI_IDENTIFIER_MASK, roiIdentifier); +} + +/** + * Get the identifier for the color filter used by the sensor. + * Useful for interpolating color images. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return color filter identifier. + */ +static inline enum caer_frame_event_color_filter caerFrameEventGetColorFilter(caerFrameEvent event) { + return ((enum caer_frame_event_color_filter) U8T(GET_NUMBITS32(event->info, COLOR_FILTER_SHIFT, COLOR_FILTER_MASK))); +} + +/** + * Set the identifier for the color filter used by the sensor. + * Useful for interpolating color images. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param colorFilter color filter identifier. + */ +static inline void caerFrameEventSetColorFilter(caerFrameEvent event, enum caer_frame_event_color_filter colorFilter) { + CLEAR_NUMBITS32(event->info, COLOR_FILTER_SHIFT, COLOR_FILTER_MASK); + SET_NUMBITS32(event->info, COLOR_FILTER_SHIFT, COLOR_FILTER_MASK, colorFilter); +} + +/** + * Get the actual X axis length for the current frame. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return frame X axis length. + */ +static inline int32_t caerFrameEventGetLengthX(caerFrameEvent event) { + return (le32toh(event->lengthX)); +} + +/** + * Get the actual Y axis length for the current frame. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return frame Y axis length. + */ +static inline int32_t caerFrameEventGetLengthY(caerFrameEvent event) { + return (le32toh(event->lengthY)); +} + +/** + * Get the actual color channels number for the current frame. + * This can be used to store RGB frames for example. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return frame color channels number. + */ +static inline enum caer_frame_event_color_channels caerFrameEventGetChannelNumber(caerFrameEvent event) { + return ((enum caer_frame_event_color_channels) U8T( + GET_NUMBITS32(event->info, COLOR_CHANNELS_SHIFT, COLOR_CHANNELS_MASK))); +} + +/** + * Set the X and Y axes length and the color channels number for a frame, + * while taking into account the maximum amount of memory available + * for the pixel array, as allocated in 'caerFrameEventPacketAllocate()'. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param lengthX the frame's X axis length. + * @param lengthY the frame's Y axis length. + * @param channelNumber the number of color channels for this frame. + * @param packet the FrameEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerFrameEventSetLengthXLengthYChannelNumber(caerFrameEvent event, int32_t lengthX, int32_t lengthY, + enum caer_frame_event_color_channels channelNumber, caerFrameEventPacket packet) { + // Verify lengths and color channels number don't exceed allocated space. + size_t neededMemory = (sizeof(uint16_t) * (size_t) lengthX * (size_t) lengthY * channelNumber); + + if (neededMemory > caerFrameEventPacketGetPixelsSize(packet)) { + caerLog(CAER_LOG_CRITICAL, "Frame Event", + "Called caerFrameEventSetLengthXLengthYChannelNumber() with values that result in requiring %zu bytes, which exceeds the maximum allocated event size of %zu bytes.", + neededMemory, (size_t) caerEventPacketHeaderGetEventSize(&packet->packetHeader)); + return; + } + + event->lengthX = htole32(lengthX); + event->lengthY = htole32(lengthY); + CLEAR_NUMBITS32(event->info, COLOR_CHANNELS_SHIFT, COLOR_CHANNELS_MASK); + SET_NUMBITS32(event->info, COLOR_CHANNELS_SHIFT, COLOR_CHANNELS_MASK, channelNumber); +} + +/** + * Get the maximum valid index into the pixel array, at which + * you can still get valid pixels. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return maximum valid pixels array index. + */ +static inline size_t caerFrameEventGetPixelsMaxIndex(caerFrameEvent event) { + enum caer_frame_event_color_channels channels = caerFrameEventGetChannelNumber(event); + return ((size_t) (caerFrameEventGetLengthX(event) * caerFrameEventGetLengthY(event) * I32T(channels))); +} + +/** + * Get the maximum size of the pixels array in bytes, in which + * you can still get valid pixels. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return maximum valid pixels array size in bytes. + */ +static inline size_t caerFrameEventGetPixelsSize(caerFrameEvent event) { + return (caerFrameEventGetPixelsMaxIndex(event) * sizeof(uint16_t)); +} + +/** + * Get the X axis position offset. + * This is used to place partial frames, like the ones gotten from + * ROI readouts, in the visual space. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return X axis position offset. + */ +static inline int32_t caerFrameEventGetPositionX(caerFrameEvent event) { + return (le32toh(event->positionX)); +} + +/** + * Set the X axis position offset. + * This is used to place partial frames, like the ones gotten from + * ROI readouts, in the visual space. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param positionX X axis position offset. + */ +static inline void caerFrameEventSetPositionX(caerFrameEvent event, int32_t positionX) { + event->positionX = htole32(positionX); +} + +/** + * Get the Y axis position offset. + * This is used to place partial frames, like the ones gotten from + * ROI readouts, in the visual space. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return Y axis position offset. + */ +static inline int32_t caerFrameEventGetPositionY(caerFrameEvent event) { + return (le32toh(event->positionY)); +} + +/** + * Set the Y axis position offset. + * This is used to place partial frames, like the ones gotten from + * ROI readouts, in the visual space. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param positionY Y axis position offset. + */ +static inline void caerFrameEventSetPositionY(caerFrameEvent event, int32_t positionY) { + event->positionY = htole32(positionY); +} + +/** + * Get the pixel value at the specified (X, Y) address. + * (X, Y) are checked against the actual possible values for this frame. + * Different channels are not taken into account! + * The (0, 0) pixel is in the upper left corner, like in OpenCV/computer graphics. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param xAddress X address value (checked). + * @param yAddress Y address value (checked). + * + * @return pixel value (normalized to 16 bit depth). + */ +static inline uint16_t caerFrameEventGetPixel(caerFrameEvent event, int32_t xAddress, int32_t yAddress) { + // Check frame bounds first. + if (yAddress < 0 || yAddress >= caerFrameEventGetLengthY(event)) { + caerLog(CAER_LOG_CRITICAL, "Frame Event", + "Called caerFrameEventGetPixel() with invalid Y address of %" PRIi32 ", should be between 0 and %" PRIi32 ".", + yAddress, caerFrameEventGetLengthY(event) - 1); + return (0); + } + + int32_t xLength = caerFrameEventGetLengthX(event); + + if (xAddress < 0 || xAddress >= xLength) { + caerLog(CAER_LOG_CRITICAL, "Frame Event", + "Called caerFrameEventGetPixel() with invalid X address of %" PRIi32 ", should be between 0 and %" PRIi32".", + xAddress, xLength - 1); + return (0); + } + + // Get pixel value at specified position. + return (le16toh(event->pixels[(yAddress * xLength) + xAddress])); +} + +/** + * Set the pixel value at the specified (X, Y) address. + * (X, Y) are checked against the actual possible values for this frame. + * Different channels are not taken into account! + * The (0, 0) pixel is in the upper left corner, like in OpenCV/computer graphics. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param xAddress X address value (checked). + * @param yAddress Y address value (checked). + * @param pixelValue pixel value (normalized to 16 bit depth). + */ +static inline void caerFrameEventSetPixel(caerFrameEvent event, int32_t xAddress, int32_t yAddress, uint16_t pixelValue) { + // Check frame bounds first. + if (yAddress < 0 || yAddress >= caerFrameEventGetLengthY(event)) { + caerLog(CAER_LOG_CRITICAL, "Frame Event", + "Called caerFrameEventSetPixel() with invalid Y address of %" PRIi32 ", should be between 0 and %" PRIi32 ".", + yAddress, caerFrameEventGetLengthY(event) - 1); + return; + } + + int32_t xLength = caerFrameEventGetLengthX(event); + + if (xAddress < 0 || xAddress >= xLength) { + caerLog(CAER_LOG_CRITICAL, "Frame Event", + "Called caerFrameEventSetPixel() with invalid X address of %" PRIi32 ", should be between 0 and %" PRIi32 ".", + xAddress, xLength - 1); + return; + } + + // Set pixel value at specified position. + event->pixels[(yAddress * xLength) + xAddress] = htole16(pixelValue); +} + +/** + * Get the pixel value at the specified (X, Y) address, taking into + * account the specified channel. + * (X, Y) and the channel number are checked against the actual + * possible values for this frame. + * The (0, 0) pixel is in the upper left corner, like in OpenCV/computer graphics. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param xAddress X address value (checked). + * @param yAddress Y address value (checked). + * @param channel the channel number (checked). + * + * @return pixel value (normalized to 16 bit depth). + */ +static inline uint16_t caerFrameEventGetPixelForChannel(caerFrameEvent event, int32_t xAddress, int32_t yAddress, + uint8_t channel) { + // Check frame bounds first. + if (yAddress < 0 || yAddress >= caerFrameEventGetLengthY(event)) { + caerLog(CAER_LOG_CRITICAL, "Frame Event", + "Called caerFrameEventGetPixelForChannel() with invalid Y address of %" PRIi32 ", should be between 0 and %" PRIi32 ".", + yAddress, caerFrameEventGetLengthY(event) - 1); + return (0); + } + + int32_t xLength = caerFrameEventGetLengthX(event); + + if (xAddress < 0 || xAddress >= xLength) { + caerLog(CAER_LOG_CRITICAL, "Frame Event", + "Called caerFrameEventGetPixelForChannel() with invalid X address of %" PRIi32 ", should be between 0 and %" PRIi32 ".", + xAddress, xLength - 1); + return (0); + } + + uint8_t channelNumber = caerFrameEventGetChannelNumber(event); + + if (channel >= channelNumber) { + caerLog(CAER_LOG_CRITICAL, "Frame Event", + "Called caerFrameEventGetPixelForChannel() with invalid channel number of %" PRIu8 ", should be between 0 and %" PRIu8 ".", + channel, (uint8_t) (channelNumber - 1)); + return (0); + } + + // Get pixel value at specified position. + return (le16toh(event->pixels[(((yAddress * xLength) + xAddress) * channelNumber) + channel])); +} + +/** + * Set the pixel value at the specified (X, Y) address, taking into + * account the specified channel. + * (X, Y) and the channel number are checked against the actual + * possible values for this frame. + * The (0, 0) pixel is in the upper left corner, like in OpenCV/computer graphics. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param xAddress X address value (checked). + * @param yAddress Y address value (checked). + * @param channel the channel number (checked). + * @param pixelValue pixel value (normalized to 16 bit depth). + */ +static inline void caerFrameEventSetPixelForChannel(caerFrameEvent event, int32_t xAddress, int32_t yAddress, + uint8_t channel, uint16_t pixelValue) { + // Check frame bounds first. + if (yAddress < 0 || yAddress >= caerFrameEventGetLengthY(event)) { + caerLog(CAER_LOG_CRITICAL, "Frame Event", + "Called caerFrameEventSetPixelForChannel() with invalid Y address of %" PRIi32 ", should be between 0 and %" PRIi32 ".", + yAddress, caerFrameEventGetLengthY(event) - 1); + return; + } + + int32_t xLength = caerFrameEventGetLengthX(event); + + if (xAddress < 0 || xAddress >= xLength) { + caerLog(CAER_LOG_CRITICAL, "Frame Event", + "Called caerFrameEventSetPixelForChannel() with invalid X address of %" PRIi32 ", should be between 0 and %" PRIi32 ".", + xAddress, xLength - 1); + return; + } + + uint8_t channelNumber = caerFrameEventGetChannelNumber(event); + + if (channel >= channelNumber) { + caerLog(CAER_LOG_CRITICAL, "Frame Event", + "Called caerFrameEventSetPixelForChannel() with invalid channel number of %" PRIu8 ", should be between 0 and %" PRIu8 ".", + channel, (uint8_t) (channelNumber - 1)); + return; + } + + // Set pixel value at specified position. + event->pixels[(((yAddress * xLength) + xAddress) * channelNumber) + channel] = htole16(pixelValue); +} + +/** + * Get the pixel value at the specified (X, Y) address. + * No checks on (X, Y) are performed! + * The (0, 0) pixel is in the upper left corner, like in OpenCV/computer graphics. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param xAddress X address value (unchecked). + * @param yAddress Y address value (unchecked). + * + * @return pixel value (normalized to 16 bit depth). + */ +static inline uint16_t caerFrameEventGetPixelUnsafe(caerFrameEvent event, int32_t xAddress, int32_t yAddress) { + // Get pixel value at specified position. + return (le16toh(event->pixels[(yAddress * caerFrameEventGetLengthX(event)) + xAddress])); +} + +/** + * Set the pixel value at the specified (X, Y) address. + * No checks on (X, Y) are performed! + * The (0, 0) pixel is in the upper left corner, like in OpenCV/computer graphics. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param xAddress X address value (unchecked). + * @param yAddress Y address value (unchecked). + * @param pixelValue pixel value (normalized to 16 bit depth). + */ +static inline void caerFrameEventSetPixelUnsafe(caerFrameEvent event, int32_t xAddress, int32_t yAddress, + uint16_t pixelValue) { + // Set pixel value at specified position. + event->pixels[(yAddress * caerFrameEventGetLengthX(event)) + xAddress] = htole16(pixelValue); +} + +/** + * Get the pixel value at the specified (X, Y) address, taking into + * account the specified channel. + * No checks on (X, Y) and the channel number are performed! + * The (0, 0) pixel is in the upper left corner, like in OpenCV/computer graphics. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param xAddress X address value (unchecked). + * @param yAddress Y address value (unchecked). + * @param channel the channel number (unchecked). + * + * @return pixel value (normalized to 16 bit depth). + */ +static inline uint16_t caerFrameEventGetPixelForChannelUnsafe(caerFrameEvent event, int32_t xAddress, int32_t yAddress, + uint8_t channel) { + enum caer_frame_event_color_channels channels = caerFrameEventGetChannelNumber(event); + // Get pixel value at specified position. + return (le16toh( + event->pixels[(((yAddress * caerFrameEventGetLengthX(event)) + xAddress) * I32T(channels))+ channel])); +} + +/** + * Set the pixel value at the specified (X, Y) address, taking into + * account the specified channel. + * No checks on (X, Y) and the channel number are performed! + * The (0, 0) pixel is in the upper left corner, like in OpenCV/computer graphics. + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * @param xAddress X address value (unchecked). + * @param yAddress Y address value (unchecked). + * @param channel the channel number (unchecked). + * @param pixelValue pixel value (normalized to 16 bit depth). + */ +static inline void caerFrameEventSetPixelForChannelUnsafe(caerFrameEvent event, int32_t xAddress, int32_t yAddress, + uint8_t channel, uint16_t pixelValue) { + enum caer_frame_event_color_channels channels = caerFrameEventGetChannelNumber(event); + // Set pixel value at specified position. + event->pixels[(((yAddress * caerFrameEventGetLengthX(event)) + xAddress) * I32T(channels)) + channel] = htole16( + pixelValue); +} + +/** + * Get a direct reference to the underlying pixels array. + * This can be used to both get and set values. + * No checks at all are performed at any point, nor any + * conversions, use this at your own risk! + * Remember that the 16 bit pixel values are in little-endian! + * The pixel array is laid out row by row (increasing X axis), + * going from top to bottom (increasing Y axis). + * + * @param event a valid FrameEvent pointer. Cannot be NULL. + * + * @return the pixels array (16 bit integers are little-endian). + */ +static inline uint16_t *caerFrameEventGetPixelArrayUnsafe(caerFrameEvent event) { + // Get pixels array. + return (event->pixels); +} + +/** + * Iterator over all frame events in a packet. + * Returns the current index in the 'caerFrameIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerFrameIteratorElement' variable + * of type caerFrameEvent. + * + * FRAME_PACKET: a valid FrameEventPacket pointer. Cannot be NULL. + */ +#define CAER_FRAME_ITERATOR_ALL_START(FRAME_PACKET) \ + for (int32_t caerFrameIteratorCounter = 0; \ + caerFrameIteratorCounter < caerEventPacketHeaderGetEventNumber(&(FRAME_PACKET)->packetHeader); \ + caerFrameIteratorCounter++) { \ + caerFrameEvent caerFrameIteratorElement = caerFrameEventPacketGetEvent(FRAME_PACKET, caerFrameIteratorCounter); + +/** + * Iterator close statement. + */ +#define CAER_FRAME_ITERATOR_ALL_END } + +/** + * Iterator over only the valid frame events in a packet. + * Returns the current index in the 'caerFrameIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerFrameIteratorElement' variable + * of type caerFrameEvent. + * + * FRAME_PACKET: a valid FrameEventPacket pointer. Cannot be NULL. + */ +#define CAER_FRAME_ITERATOR_VALID_START(FRAME_PACKET) \ + for (int32_t caerFrameIteratorCounter = 0; \ + caerFrameIteratorCounter < caerEventPacketHeaderGetEventNumber(&(FRAME_PACKET)->packetHeader); \ + caerFrameIteratorCounter++) { \ + caerFrameEvent caerFrameIteratorElement = caerFrameEventPacketGetEvent(FRAME_PACKET, caerFrameIteratorCounter); \ + if (!caerFrameEventIsValid(caerFrameIteratorElement)) { continue; } // Skip invalid frame events. + +/** + * Iterator close statement. + */ +#define CAER_FRAME_ITERATOR_VALID_END } + +/** + * Reverse iterator over all frame events in a packet. + * Returns the current index in the 'caerFrameIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerFrameIteratorElement' variable + * of type caerFrameEvent. + * + * FRAME_PACKET: a valid FrameEventPacket pointer. Cannot be NULL. + */ +#define CAER_FRAME_REVERSE_ITERATOR_ALL_START(FRAME_PACKET) \ + for (int32_t caerFrameIteratorCounter = caerEventPacketHeaderGetEventNumber(&(FRAME_PACKET)->packetHeader) - 1; \ + caerFrameIteratorCounter >= 0; \ + caerFrameIteratorCounter--) { \ + caerFrameEvent caerFrameIteratorElement = caerFrameEventPacketGetEvent(FRAME_PACKET, caerFrameIteratorCounter); + +/** + * Reverse iterator close statement. + */ +#define CAER_FRAME_REVERSE_ITERATOR_ALL_END } + +/** + * Reverse iterator over only the valid frame events in a packet. + * Returns the current index in the 'caerFrameIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerFrameIteratorElement' variable + * of type caerFrameEvent. + * + * FRAME_PACKET: a valid FrameEventPacket pointer. Cannot be NULL. + */ +#define CAER_FRAME_REVERSE_ITERATOR_VALID_START(FRAME_PACKET) \ + for (int32_t caerFrameIteratorCounter = caerEventPacketHeaderGetEventNumber(&(FRAME_PACKET)->packetHeader) - 1; \ + caerFrameIteratorCounter >= 0; \ + caerFrameIteratorCounter--) { \ + caerFrameEvent caerFrameIteratorElement = caerFrameEventPacketGetEvent(FRAME_PACKET, caerFrameIteratorCounter); \ + if (!caerFrameEventIsValid(caerFrameIteratorElement)) { continue; } // Skip invalid frame events. + +/** + * Reverse iterator close statement. + */ +#define CAER_FRAME_REVERSE_ITERATOR_VALID_END } + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_EVENTS_FRAME_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/imu6.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/imu6.h new file mode 100755 index 0000000000000000000000000000000000000000..08d5b80362e501c668c22659e79f0452d8d02fdf --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/imu6.h @@ -0,0 +1,420 @@ +/** + * @file imu6.h + * + * IMU6 (6 axes) Events format definition and handling functions. + * This contains data coming from the Inertial Measurement Unit + * chip, with the 3-axes accelerometer and 3-axes gyroscope. + * Temperature is also included. + */ + +#ifndef LIBCAER_EVENTS_IMU6_H_ +#define LIBCAER_EVENTS_IMU6_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "common.h" + +/** + * IMU 6-axes event data structure definition. + * This contains accelerometer and gyroscope headings, plus + * temperature. + * The X, Y and Z axes are referred to the camera plane. + * X increases to the right, Y going up and Z towards where + * the lens is pointing. Rotation for the gyroscope is + * counter-clockwise along the increasing axis, for all three axes. + * Floats are in IEEE 754-2008 binary32 format. + * Signed integers are used for fields that are to be interpreted + * directly, for compatibility with languages that do not have + * unsigned integer types, such as Java. + */ +struct caer_imu6_event { + /// Event information. First because of valid mark. + uint32_t info; + /// Event timestamp. + int32_t timestamp; + /// Acceleration in the X axis, measured in g (9.81m/s²). + float accel_x; + /// Acceleration in the Y axis, measured in g (9.81m/s²). + float accel_y; + /// Acceleration in the Z axis, measured in g (9.81m/s²). + float accel_z; + /// Rotation in the X axis, measured in °/s. + float gyro_x; + /// Rotation in the Y axis, measured in °/s. + float gyro_y; + /// Rotation in the Z axis, measured in °/s. + float gyro_z; + /// Temperature, measured in °C. + float temp; +}__attribute__((__packed__)); + +/** + * Type for pointer to IMU 6-axes event data structure. + */ +typedef struct caer_imu6_event *caerIMU6Event; + +/** + * IMU 6-axes event packet data structure definition. + * EventPackets are always made up of the common packet header, + * followed by 'eventCapacity' events. Everything has to + * be in one contiguous memory block. + */ +struct caer_imu6_event_packet { + /// The common event packet header. + struct caer_event_packet_header packetHeader; + /// The events array. + struct caer_imu6_event events[]; +}__attribute__((__packed__)); + +/** + * Type for pointer to IMU 6-axes event packet data structure. + */ +typedef struct caer_imu6_event_packet *caerIMU6EventPacket; + +/** + * Allocate a new IMU 6-axes events packet. + * Use free() to reclaim this memory. + * + * @param eventCapacity the maximum number of events this packet will hold. + * @param eventSource the unique ID representing the source/generator of this packet. + * @param tsOverflow the current timestamp overflow counter value for this packet. + * + * @return a valid IMU6EventPacket handle or NULL on error. + */ +caerIMU6EventPacket caerIMU6EventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow); + +/** + * Get the IMU 6-axes event at the given index from the event packet. + * + * @param packet a valid IMU6EventPacket pointer. Cannot be NULL. + * @param n the index of the returned event. Must be within [0,eventCapacity[ bounds. + * + * @return the requested IMU 6-axes event. NULL on error. + */ +static inline caerIMU6Event caerIMU6EventPacketGetEvent(caerIMU6EventPacket packet, int32_t n) { + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketHeaderGetEventCapacity(&packet->packetHeader)) { + caerLog(CAER_LOG_CRITICAL, "IMU6 Event", + "Called caerIMU6EventPacketGetEvent() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ".", + n, caerEventPacketHeaderGetEventCapacity(&packet->packetHeader) - 1); + return (NULL); + } + + // Return a pointer to the specified event. + return (packet->events + n); +} + +/** + * Get the 32bit event timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond timestamp. + */ +static inline int32_t caerIMU6EventGetTimestamp(caerIMU6Event event) { + return (le32toh(event->timestamp)); +} + +/** + * Get the 64bit event timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * @param packet the IMU6EventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond timestamp. + */ +static inline int64_t caerIMU6EventGetTimestamp64(caerIMU6Event event, caerIMU6EventPacket packet) { + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerIMU6EventGetTimestamp(event)))); +} + +/** + * Set the 32bit event timestamp, the value has to be in microseconds. + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * @param timestamp a positive 32bit microsecond timestamp. + */ +static inline void caerIMU6EventSetTimestamp(caerIMU6Event event, int32_t timestamp) { + if (timestamp < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "IMU6 Event", "Called caerIMU6EventSetTimestamp() with negative value!"); + return; + } + + event->timestamp = htole32(timestamp); +} + +/** + * Check if this IMU 6-axes event is valid. + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * + * @return true if valid, false if not. + */ +static inline bool caerIMU6EventIsValid(caerIMU6Event event) { + return (GET_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK)); +} + +/** + * Validate the current event by setting its valid bit to true + * and increasing the event packet's event count and valid + * event count. Only works on events that are invalid. + * DO NOT CALL THIS AFTER HAVING PREVIOUSLY ALREADY + * INVALIDATED THIS EVENT, the total count will be incorrect. + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * @param packet the IMU6EventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerIMU6EventValidate(caerIMU6Event event, caerIMU6EventPacket packet) { + if (!caerIMU6EventIsValid(event)) { + SET_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK, 1); + + // Also increase number of events and valid events. + // Only call this on (still) invalid events! + caerEventPacketHeaderSetEventNumber(&packet->packetHeader, + caerEventPacketHeaderGetEventNumber(&packet->packetHeader) + 1); + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) + 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "IMU6 Event", "Called caerIMU6EventValidate() on already valid event."); + } +} + +/** + * Invalidate the current event by setting its valid bit + * to false and decreasing the number of valid events held + * in the packet. Only works with events that are already + * valid! + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * @param packet the IMU6EventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerIMU6EventInvalidate(caerIMU6Event event, caerIMU6EventPacket packet) { + if (caerIMU6EventIsValid(event)) { + CLEAR_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK); + + // Also decrease number of valid events. Number of total events doesn't change. + // Only call this on valid events! + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) - 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "IMU6 Event", "Called caerIMU6EventInvalidate() on already invalid event."); + } +} + +/** + * Get the X axis acceleration reading (from accelerometer). + * This is in g (1 g = 9.81 m/s²). + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * + * @return acceleration on the X axis. + */ +static inline float caerIMU6EventGetAccelX(caerIMU6Event event) { + return (le32toh(event->accel_x)); +} + +/** + * Set the X axis acceleration reading (from accelerometer). + * This is in g (1 g = 9.81 m/s²). + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * @param accelX acceleration on the X axis. + */ +static inline void caerIMU6EventSetAccelX(caerIMU6Event event, float accelX) { + event->accel_x = htole32(accelX); +} + +/** + * Get the Y axis acceleration reading (from accelerometer). + * This is in g (1 g = 9.81 m/s²). + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * + * @return acceleration on the Y axis. + */ +static inline float caerIMU6EventGetAccelY(caerIMU6Event event) { + return (le32toh(event->accel_y)); +} + +/** + * Set the Y axis acceleration reading (from accelerometer). + * This is in g (1 g = 9.81 m/s²). + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * @param accelY acceleration on the Y axis. + */ +static inline void caerIMU6EventSetAccelY(caerIMU6Event event, float accelY) { + event->accel_y = htole32(accelY); +} + +/** + * Get the Z axis acceleration reading (from accelerometer). + * This is in g (1 g = 9.81 m/s²). + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * + * @return acceleration on the Z axis. + */ +static inline float caerIMU6EventGetAccelZ(caerIMU6Event event) { + return (le32toh(event->accel_z)); +} + +/** + * Set the Z axis acceleration reading (from accelerometer). + * This is in g (1 g = 9.81 m/s²). + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * @param accelZ acceleration on the Z axis. + */ +static inline void caerIMU6EventSetAccelZ(caerIMU6Event event, float accelZ) { + event->accel_z = htole32(accelZ); +} + +/** + * Get the X axis (roll) angular velocity reading (from gyroscope). + * This is in °/s (deg/sec). + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * + * @return angular velocity on the X axis (roll). + */ +static inline float caerIMU6EventGetGyroX(caerIMU6Event event) { + return (le32toh(event->gyro_x)); +} + +/** + * Set the X axis (roll) angular velocity reading (from gyroscope). + * This is in °/s (deg/sec). + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * @param gyroX angular velocity on the X axis (roll). + */ +static inline void caerIMU6EventSetGyroX(caerIMU6Event event, float gyroX) { + event->gyro_x = htole32(gyroX); +} + +/** + * Get the Y axis (pitch) angular velocity reading (from gyroscope). + * This is in °/s (deg/sec). + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * + * @return angular velocity on the Y axis (pitch). + */ +static inline float caerIMU6EventGetGyroY(caerIMU6Event event) { + return (le32toh(event->gyro_y)); +} + +/** + * Set the Y axis (pitch) angular velocity reading (from gyroscope). + * This is in °/s (deg/sec). + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * @param gyroY angular velocity on the Y axis (pitch). + */ +static inline void caerIMU6EventSetGyroY(caerIMU6Event event, float gyroY) { + event->gyro_y = htole32(gyroY); +} + +/** + * Get the Z axis (yaw) angular velocity reading (from gyroscope). + * This is in °/s (deg/sec). + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * + * @return angular velocity on the Z axis (yaw). + */ +static inline float caerIMU6EventGetGyroZ(caerIMU6Event event) { + return (le32toh(event->gyro_z)); +} + +/** + * Set the Z axis (yaw) angular velocity reading (from gyroscope). + * This is in °/s (deg/sec). + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * @param gyroZ angular velocity on the Z axis (yaw). + */ +static inline void caerIMU6EventSetGyroZ(caerIMU6Event event, float gyroZ) { + event->gyro_z = htole32(gyroZ); +} + +/** + * Get the temperature reading. + * This is in °C. + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * + * @return temperature in °C. + */ +static inline float caerIMU6EventGetTemp(caerIMU6Event event) { + return (le32toh(event->temp)); +} + +/** + * Set the temperature reading. + * This is in °C. + * + * @param event a valid IMU6Event pointer. Cannot be NULL. + * @param temp temperature in °C. + */ +static inline void caerIMU6EventSetTemp(caerIMU6Event event, float temp) { + event->temp = htole32(temp); +} + +/** + * Iterator over all IMU6 events in a packet. + * Returns the current index in the 'caerIMU6IteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerIMU6IteratorElement' variable + * of type caerIMU6Event. + * + * IMU6_PACKET: a valid IMU6EventPacket pointer. Cannot be NULL. + */ +#define CAER_IMU6_ITERATOR_ALL_START(IMU6_PACKET) \ + for (int32_t caerIMU6IteratorCounter = 0; \ + caerIMU6IteratorCounter < caerEventPacketHeaderGetEventNumber(&(IMU6_PACKET)->packetHeader); \ + caerIMU6IteratorCounter++) { \ + caerIMU6Event caerIMU6IteratorElement = caerIMU6EventPacketGetEvent(IMU6_PACKET, caerIMU6IteratorCounter); + +/** + * Iterator close statement. + */ +#define CAER_IMU6_ITERATOR_ALL_END } + +/** + * Iterator over only the valid IMU6 events in a packet. + * Returns the current index in the 'caerIMU6IteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerIMU6IteratorElement' variable + * of type caerIMU6Event. + * + * IMU6_PACKET: a valid IMU6EventPacket pointer. Cannot be NULL. + */ +#define CAER_IMU6_ITERATOR_VALID_START(IMU6_PACKET) \ + for (int32_t caerIMU6IteratorCounter = 0; \ + caerIMU6IteratorCounter < caerEventPacketHeaderGetEventNumber(&(IMU6_PACKET)->packetHeader); \ + caerIMU6IteratorCounter++) { \ + caerIMU6Event caerIMU6IteratorElement = caerIMU6EventPacketGetEvent(IMU6_PACKET, caerIMU6IteratorCounter); \ + if (!caerIMU6EventIsValid(caerIMU6IteratorElement)) { continue; } // Skip invalid IMU6 events. + +/** + * Iterator close statement. + */ +#define CAER_IMU6_ITERATOR_VALID_END } + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_EVENTS_IMU6_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/imu9.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/imu9.h new file mode 100755 index 0000000000000000000000000000000000000000..77e8e38182350868a359fef581bb48f87dd5a264 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/imu9.h @@ -0,0 +1,497 @@ +/** + * @file imu9.h + * + * IMU9 (9 axes) Events format definition and handling functions. + * This contains data coming from the Inertial Measurement Unit + * chip, with the 3-axes accelerometer and 3-axes gyroscope. + * Temperature is also included. + * Further, 3-axes from the magnetometer are included, which + * can be used to get a compass-like heading. + */ + +#ifndef LIBCAER_EVENTS_IMU9_H_ +#define LIBCAER_EVENTS_IMU9_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "common.h" + +/** + * IMU 9-axes event data structure definition. + * This contains accelerometer and gyroscope headings, plus + * temperature, and magnetometer readings. + * The X, Y and Z axes are referred to the camera plane. + * X increases to the right, Y going up and Z towards where + * the lens is pointing. Rotation for the gyroscope is + * counter-clockwise along the increasing axis, for all three axes. + * Floats are in IEEE 754-2008 binary32 format. + * Signed integers are used for fields that are to be interpreted + * directly, for compatibility with languages that do not have + * unsigned integer types, such as Java. + */ +struct caer_imu9_event { + /// Event information. First because of valid mark. + uint32_t info; + /// Event timestamp. + int32_t timestamp; + /// Acceleration in the X axis, measured in g (9.81m/s²). + float accel_x; + /// Acceleration in the Y axis, measured in g (9.81m/s²). + float accel_y; + /// Acceleration in the Z axis, measured in g (9.81m/s²). + float accel_z; + /// Rotation in the X axis, measured in °/s. + float gyro_x; + /// Rotation in the Y axis, measured in °/s. + float gyro_y; + /// Rotation in the Z axis, measured in °/s. + float gyro_z; + /// Temperature, measured in °C. + float temp; + /// Magnetometer X axis, measured in µT (magnetic flux density). + float comp_x; + /// Magnetometer Y axis, measured in µT (magnetic flux density). + float comp_y; + /// Magnetometer Z axis, measured in µT (magnetic flux density). + float comp_z; +}__attribute__((__packed__)); + +/** + * Type for pointer to IMU 9-axes event data structure. + */ +typedef struct caer_imu9_event *caerIMU9Event; + +/** + * IMU 9-axes event packet data structure definition. + * EventPackets are always made up of the common packet header, + * followed by 'eventCapacity' events. Everything has to + * be in one contiguous memory block. + */ +struct caer_imu9_event_packet { + /// The common event packet header. + struct caer_event_packet_header packetHeader; + /// The events array. + struct caer_imu9_event events[]; +}__attribute__((__packed__)); + +/** + * Type for pointer to IMU 9-axes event packet data structure. + */ +typedef struct caer_imu9_event_packet *caerIMU9EventPacket; + +/** + * Allocate a new IMU 9-axes events packet. + * Use free() to reclaim this memory. + * + * @param eventCapacity the maximum number of events this packet will hold. + * @param eventSource the unique ID representing the source/generator of this packet. + * @param tsOverflow the current timestamp overflow counter value for this packet. + * + * @return a valid IMU9EventPacket handle or NULL on error. + */ +caerIMU9EventPacket caerIMU9EventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow); + +/** + * Get the IMU 9-axes event at the given index from the event packet. + * + * @param packet a valid IMU9EventPacket pointer. Cannot be NULL. + * @param n the index of the returned event. Must be within [0,eventCapacity[ bounds. + * + * @return the requested IMU 9-axes event. NULL on error. + */ +static inline caerIMU9Event caerIMU9EventPacketGetEvent(caerIMU9EventPacket packet, int32_t n) { + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketHeaderGetEventCapacity(&packet->packetHeader)) { + caerLog(CAER_LOG_CRITICAL, "IMU9 Event", + "Called caerIMU9EventPacketGetEvent() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ".", + n, caerEventPacketHeaderGetEventCapacity(&packet->packetHeader) - 1); + return (NULL); + } + + // Return a pointer to the specified event. + return (packet->events + n); +} + +/** + * Get the 32bit event timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond timestamp. + */ +static inline int32_t caerIMU9EventGetTimestamp(caerIMU9Event event) { + return (le32toh(event->timestamp)); +} + +/** + * Get the 64bit event timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * @param packet the IMU9EventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond timestamp. + */ +static inline int64_t caerIMU9EventGetTimestamp64(caerIMU9Event event, caerIMU9EventPacket packet) { + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerIMU9EventGetTimestamp(event)))); +} + +/** + * Set the 32bit event timestamp, the value has to be in microseconds. + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * @param timestamp a positive 32bit microsecond timestamp. + */ +static inline void caerIMU9EventSetTimestamp(caerIMU9Event event, int32_t timestamp) { + if (timestamp < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "IMU9 Event", "Called caerIMU9EventSetTimestamp() with negative value!"); + return; + } + + event->timestamp = htole32(timestamp); +} + +/** + * Check if this IMU 9-axes event is valid. + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * + * @return true if valid, false if not. + */ +static inline bool caerIMU9EventIsValid(caerIMU9Event event) { + return (GET_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK)); +} + +/** + * Validate the current event by setting its valid bit to true + * and increasing the event packet's event count and valid + * event count. Only works on events that are invalid. + * DO NOT CALL THIS AFTER HAVING PREVIOUSLY ALREADY + * INVALIDATED THIS EVENT, the total count will be incorrect. + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * @param packet the IMU9EventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerIMU9EventValidate(caerIMU9Event event, caerIMU9EventPacket packet) { + if (!caerIMU9EventIsValid(event)) { + SET_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK, 1); + + // Also increase number of events and valid events. + // Only call this on (still) invalid events! + caerEventPacketHeaderSetEventNumber(&packet->packetHeader, + caerEventPacketHeaderGetEventNumber(&packet->packetHeader) + 1); + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) + 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "IMU9 Event", "Called caerIMU9EventValidate() on already valid event."); + } +} + +/** + * Invalidate the current event by setting its valid bit + * to false and decreasing the number of valid events held + * in the packet. Only works with events that are already + * valid! + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * @param packet the IMU9EventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerIMU9EventInvalidate(caerIMU9Event event, caerIMU9EventPacket packet) { + if (caerIMU9EventIsValid(event)) { + CLEAR_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK); + + // Also decrease number of valid events. Number of total events doesn't change. + // Only call this on valid events! + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) - 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "IMU9 Event", "Called caerIMU9EventInvalidate() on already invalid event."); + } +} + +/** + * Get the X axis acceleration reading (from accelerometer). + * This is in g (1 g = 9.81 m/s²). + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * + * @return acceleration on the X axis. + */ +static inline float caerIMU9EventGetAccelX(caerIMU9Event event) { + return (le32toh(event->accel_x)); +} + +/** + * Set the X axis acceleration reading (from accelerometer). + * This is in g (1 g = 9.81 m/s²). + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * @param accelX acceleration on the X axis. + */ +static inline void caerIMU9EventSetAccelX(caerIMU9Event event, float accelX) { + event->accel_x = htole32(accelX); +} + +/** + * Get the Y axis acceleration reading (from accelerometer). + * This is in g (1 g = 9.81 m/s²). + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * + * @return acceleration on the Y axis. + */ +static inline float caerIMU9EventGetAccelY(caerIMU9Event event) { + return (le32toh(event->accel_y)); +} + +/** + * Set the Y axis acceleration reading (from accelerometer). + * This is in g (1 g = 9.81 m/s²). + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * @param accelY acceleration on the Y axis. + */ +static inline void caerIMU9EventSetAccelY(caerIMU9Event event, float accelY) { + event->accel_y = htole32(accelY); +} + +/** + * Get the Z axis acceleration reading (from accelerometer). + * This is in g (1 g = 9.81 m/s²). + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * + * @return acceleration on the Z axis. + */ +static inline float caerIMU9EventGetAccelZ(caerIMU9Event event) { + return (le32toh(event->accel_z)); +} + +/** + * Set the Z axis acceleration reading (from accelerometer). + * This is in g (1 g = 9.81 m/s²). + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * @param accelZ acceleration on the Z axis. + */ +static inline void caerIMU9EventSetAccelZ(caerIMU9Event event, float accelZ) { + event->accel_z = htole32(accelZ); +} + +/** + * Get the X axis (roll) angular velocity reading (from gyroscope). + * This is in °/s (deg/sec). + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * + * @return angular velocity on the X axis (roll). + */ +static inline float caerIMU9EventGetGyroX(caerIMU9Event event) { + return (le32toh(event->gyro_x)); +} + +/** + * Set the X axis (roll) angular velocity reading (from gyroscope). + * This is in °/s (deg/sec). + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * @param gyroX angular velocity on the X axis (roll). + */ +static inline void caerIMU9EventSetGyroX(caerIMU9Event event, float gyroX) { + event->gyro_x = htole32(gyroX); +} + +/** + * Get the Y axis (pitch) angular velocity reading (from gyroscope). + * This is in °/s (deg/sec). + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * + * @return angular velocity on the Y axis (pitch). + */ +static inline float caerIMU9EventGetGyroY(caerIMU9Event event) { + return (le32toh(event->gyro_y)); +} + +/** + * Set the Y axis (pitch) angular velocity reading (from gyroscope). + * This is in °/s (deg/sec). + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * @param gyroY angular velocity on the Y axis (pitch). + */ +static inline void caerIMU9EventSetGyroY(caerIMU9Event event, float gyroY) { + event->gyro_y = htole32(gyroY); +} + +/** + * Get the Z axis (yaw) angular velocity reading (from gyroscope). + * This is in °/s (deg/sec). + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * + * @return angular velocity on the Z axis (yaw). + */ +static inline float caerIMU9EventGetGyroZ(caerIMU9Event event) { + return (le32toh(event->gyro_z)); +} + +/** + * Set the Z axis (yaw) angular velocity reading (from gyroscope). + * This is in °/s (deg/sec). + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * @param gyroZ angular velocity on the Z axis (yaw). + */ +static inline void caerIMU9EventSetGyroZ(caerIMU9Event event, float gyroZ) { + event->gyro_z = htole32(gyroZ); +} + +/** + * Get the temperature reading. + * This is in °C. + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * + * @return temperature in °C. + */ +static inline float caerIMU9EventGetTemp(caerIMU9Event event) { + return (le32toh(event->temp)); +} + +/** + * Set the temperature reading. + * This is in °C. + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * @param temp temperature in °C. + */ +static inline void caerIMU9EventSetTemp(caerIMU9Event event, float temp) { + event->temp = htole32(temp); +} + +/** + * Get the X axis compass heading (from magnetometer). + * This is in µT. + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * + * @return X axis compass heading. + */ +static inline float caerIMU9EventGetCompX(caerIMU9Event event) { + return (le32toh(event->comp_x)); +} + +/** + * Set the X axis compass heading (from magnetometer). + * This is in µT. + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * @param compX X axis compass heading. + */ +static inline void caerIMU9EventSetCompX(caerIMU9Event event, float compX) { + event->comp_x = htole32(compX); +} + +/** + * Get the Y axis compass heading (from magnetometer). + * This is in µT. + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * + * @return Y axis compass heading. + */ +static inline float caerIMU9EventGetCompY(caerIMU9Event event) { + return (le32toh(event->comp_y)); +} + +/** + * Set the Y axis compass heading (from magnetometer). + * This is in µT. + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * @param compY Y axis compass heading. + */ +static inline void caerIMU9EventSetCompY(caerIMU9Event event, float compY) { + event->comp_y = htole32(compY); +} + +/** + * Get the Z axis compass heading (from magnetometer). + * This is in µT. + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * + * @return Z axis compass heading. + */ +static inline float caerIMU9EventGetCompZ(caerIMU9Event event) { + return (le32toh(event->comp_z)); +} + +/** + * Set the Z axis compass heading (from magnetometer). + * This is in µT. + * + * @param event a valid IMU9Event pointer. Cannot be NULL. + * @param compZ Z axis compass heading. + */ +static inline void caerIMU9EventSetCompZ(caerIMU9Event event, float compZ) { + event->comp_z = htole32(compZ); +} + +/** + * Iterator over all IMU9 events in a packet. + * Returns the current index in the 'caerIMU9IteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerIMU9IteratorElement' variable + * of type caerIMU9Event. + * + * IMU9_PACKET: a valid IMU9EventPacket pointer. Cannot be NULL. + */ +#define CAER_IMU9_ITERATOR_ALL_START(IMU9_PACKET) \ + for (int32_t caerIMU9IteratorCounter = 0; \ + caerIMU9IteratorCounter < caerEventPacketHeaderGetEventNumber(&(IMU9_PACKET)->packetHeader); \ + caerIMU9IteratorCounter++) { \ + caerIMU9Event caerIMU9IteratorElement = caerIMU9EventPacketGetEvent(IMU9_PACKET, caerIMU9IteratorCounter); + +/** + * Iterator close statement. + */ +#define CAER_IMU9_ITERATOR_ALL_END } + +/** + * Iterator over only the valid IMU9 events in a packet. + * Returns the current index in the 'caerIMU9IteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerIMU9IteratorElement' variable + * of type caerIMU9Event. + * + * IMU9_PACKET: a valid IMU9EventPacket pointer. Cannot be NULL. + */ +#define CAER_IMU9_ITERATOR_VALID_START(IMU9_PACKET) \ + for (int32_t caerIMU9IteratorCounter = 0; \ + caerIMU9IteratorCounter < caerEventPacketHeaderGetEventNumber(&(IMU9_PACKET)->packetHeader); \ + caerIMU9IteratorCounter++) { \ + caerIMU9Event caerIMU9IteratorElement = caerIMU9EventPacketGetEvent(IMU9_PACKET, caerIMU9IteratorCounter); \ + if (!caerIMU9EventIsValid(caerIMU9IteratorElement)) { continue; } // Skip invalid IMU9 events. + +/** + * Iterator close statement. + */ +#define CAER_IMU9_ITERATOR_VALID_END } + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_EVENTS_IMU9_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/packetContainer.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/packetContainer.h new file mode 100755 index 0000000000000000000000000000000000000000..2a458b6fb5a5c902d27d6d11249b5a58a6b6a78a --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/packetContainer.h @@ -0,0 +1,374 @@ +/** + * @file packetContainer.h + * + * EventPacketContainer format definition and handling functions. + * An EventPacketContainer is a logical construct that contains packets + * of events (EventPackets) of different event types, with the aim of + * keeping related events of differing types, such as DVS and IMU data, + * together. Such a relation is usually based on time intervals, trying + * to keep groups of event happening in a certain time-slice together. + * This time-order is based on the *main* time-stamp of an event, the one + * whose offset is referenced in the event packet header and that is + * used by the caerGenericEvent*() functions. It's guaranteed that all + * conforming input modules keep to this rule, generating containers + * that include all events from all types within the given time-slice. + * The smallest and largest timestamps are tracked at the packet container + * level as a convenience, to avoid having to examine all packets for + * this often useful piece of information. + * All integers are in their native host format, as this is a purely + * internal, in-memory data structure, never meant for exchange between + * different systems (and different endianness). + */ + +#ifndef LIBCAER_EVENTS_PACKETCONTAINER_H_ +#define LIBCAER_EVENTS_PACKETCONTAINER_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "common.h" + +/** + * EventPacketContainer data structure definition. + * Signed integers are used for compatibility with languages that + * do not have unsigned ones, such as Java. + */ +struct caer_event_packet_container { + /// Smallest event timestamp contained in this packet container. + int64_t lowestEventTimestamp; + /// Largest event timestamp contained in this packet container. + int64_t highestEventTimestamp; + /// Number of events contained within all the packets in this container. + int32_t eventsNumber; + /// Number of valid events contained within all the packets in this container. + int32_t eventsValidNumber; + /// Number of different event packets contained. + int32_t eventPacketsNumber; + /// Array of pointers to the actual event packets. + caerEventPacketHeader eventPackets[]; +}__attribute__((__packed__)); + +/** + * Type for pointer to EventPacketContainer data structure. + */ +typedef struct caer_event_packet_container *caerEventPacketContainer; + +/** + * Allocate a new EventPacketContainer with enough space to + * store up to the given number of EventPacket references. + * All packet references will be NULL initially. + * + * @param eventPacketsNumber the maximum number of EventPacket references + * that can be stored in this container. + * + * @return a valid EventPacketContainer handle or NULL on error. + */ +caerEventPacketContainer caerEventPacketContainerAllocate(int32_t eventPacketsNumber); + +/** + * Free the memory occupied by an EventPacketContainer, as well as + * freeing all of its contained EventPackets and their memory. + * If you don't want the contained EventPackets to be freed, make + * sure that you set their reference to NULL before calling this. + + * @param container the container to be freed. + */ +void caerEventPacketContainerFree(caerEventPacketContainer container); + +/** + * Get the maximum number of EventPacket references that can be stored + * in this particular EventPacketContainer. + * + * @param container a valid EventPacketContainer handle. If NULL, zero is returned. + * + * @return the number of EventPacket references that can be contained. + */ +static inline int32_t caerEventPacketContainerGetEventPacketsNumber(caerEventPacketContainer container) { + // Non-existing (empty) containers have no valid packets in them! + if (container == NULL) { + return (0); + } + + return (container->eventPacketsNumber); +} + +/** + * Set the maximum number of EventPacket references that can be stored + * in this particular EventPacketContainer. This should never be used + * directly, caerEventPacketContainerAllocate() sets this for you. + * + * @param container a valid EventPacketContainer handle. If NULL, nothing happens. + * @param eventPacketsNumber the number of EventPacket references that can be contained. + */ +static inline void caerEventPacketContainerSetEventPacketsNumber(caerEventPacketContainer container, + int32_t eventPacketsNumber) { + // Non-existing (empty) containers have no valid packets in them! + if (container == NULL) { + return; + } + + if (eventPacketsNumber < 0) { + // Negative numbers (bit 31 set) are not allowed! + caerLog(CAER_LOG_CRITICAL, "EventPacket Container", + "Called caerEventPacketContainerSetEventPacketsNumber() with negative value!"); + return; + } + + container->eventPacketsNumber = eventPacketsNumber; +} + +/** + * Get the reference for the EventPacket stored in this container + * at the given index. + * + * @param container a valid EventPacketContainer handle. If NULL, returns NULL too. + * @param n the index of the EventPacket to get. + * + * @return a reference to an EventPacket or NULL on error. + */ +static inline caerEventPacketHeader caerEventPacketContainerGetEventPacket(caerEventPacketContainer container, + int32_t n) { + // Non-existing (empty) containers have no valid packets in them! + if (container == NULL) { + return (NULL); + } + + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketContainerGetEventPacketsNumber(container)) { + caerLog(CAER_LOG_CRITICAL, "EventPacket Container", + "Called caerEventPacketContainerGetEventPacket() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ". Negative values are not allowed!", + n, caerEventPacketContainerGetEventPacketsNumber(container) - 1); + return (NULL); + } + + // Return a pointer to the specified event packet. + return (container->eventPackets[n]); +} + +/** + * Set the reference for the EventPacket stored in this container + * at the given index. + * + * @param container a valid EventPacketContainer handle. If NULL, nothing happens. + * @param n the index of the EventPacket to set. + * @param packetHeader a reference to an EventPacket's header. Can be NULL. + */ +static inline void caerEventPacketContainerSetEventPacket(caerEventPacketContainer container, int32_t n, + caerEventPacketHeader packetHeader) { + // Non-existing (empty) containers have no valid packets in them! + if (container == NULL) { + return; + } + + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketContainerGetEventPacketsNumber(container)) { + caerLog(CAER_LOG_CRITICAL, "EventPacket Container", + "Called caerEventPacketContainerSetEventPacket() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ". Negative values are not allowed!", + n, caerEventPacketContainerGetEventPacketsNumber(container) - 1); + return; + } + + // Store the given event packet. + container->eventPackets[n] = packetHeader; + + // Deleting packets (setting to NULL) will not result in timestamp-tracking updates. + if (packetHeader == NULL) { + return; + } + + // Get timestamps to update lowest/highest tracking. + void *firstEvent = caerGenericEventGetEvent(packetHeader, 0); + int64_t currLowestEventTimestamp = caerGenericEventGetTimestamp64(firstEvent, packetHeader); + + void *lastEvent = caerGenericEventGetEvent(packetHeader, caerEventPacketHeaderGetEventNumber(packetHeader) - 1); + int64_t currHighestEventTimestamp = caerGenericEventGetTimestamp64(lastEvent, packetHeader); + + // Update tracked timestamps (or initialize if needed). + if ((container->lowestEventTimestamp == -1) || (container->lowestEventTimestamp > currLowestEventTimestamp)) { + container->lowestEventTimestamp = currLowestEventTimestamp; + } + + if ((container->highestEventTimestamp == -1) || (container->highestEventTimestamp < currHighestEventTimestamp)) { + container->highestEventTimestamp = currHighestEventTimestamp; + } + + container->eventsNumber += caerEventPacketHeaderGetEventNumber(packetHeader); + container->eventsValidNumber += caerEventPacketHeaderGetEventValid(packetHeader); +} + +/** + * Get the lowest timestamp contained in this event packet container. + * + * @param container a valid EventPacketContainer handle. If NULL, -1 is returned. + * + * @return the lowest timestamp (in µs) or -1 if not initialized. + */ +static inline int64_t caerEventPacketContainerGetLowestEventTimestamp(caerEventPacketContainer container) { + // Non-existing (empty) containers have no valid packets in them! + if (container == NULL) { + return (-1); + } + + return (container->lowestEventTimestamp); +} + +/** + * Get the highest timestamp contained in this event packet container. + * + * @param container a valid EventPacketContainer handle. If NULL, -1 is returned. + * + * @return the highest timestamp (in µs) or -1 if not initialized. + */ +static inline int64_t caerEventPacketContainerGetHighestEventTimestamp(caerEventPacketContainer container) { + // Non-existing (empty) containers have no valid packets in them! + if (container == NULL) { + return (-1); + } + + return (container->highestEventTimestamp); +} + +/** + * Get the number of events contained in this event packet container. + * + * @param container a valid EventPacketContainer handle. If NULL, 0 is returned. + * + * @return the number of events in this container. + */ +static inline int32_t caerEventPacketContainerGetEventsNumber(caerEventPacketContainer container) { + // Non-existing (empty) containers have no valid packets in them! + if (container == NULL) { + return (0); + } + + return (container->eventsNumber); +} + +/** + * Get the number of valid events contained in this event packet container. + * + * @param container a valid EventPacketContainer handle. If NULL, 0 is returned. + * + * @return the number of valid events in this container. + */ +static inline int32_t caerEventPacketContainerGetEventsValidNumber(caerEventPacketContainer container) { + // Non-existing (empty) containers have no valid packets in them! + if (container == NULL) { + return (0); + } + + return (container->eventsValidNumber); +} + +/** + * Iterator over all event packets in an event packet container. + * Returns the current index in the 'caerEventPacketContainerIteratorCounter' variable + * of type 'int32_t' and the current event packet in the 'caerEventPacketContainerIteratorElement' + * variable of type caerEventPacketHeader. The current packet may be NULL, in which case it is + * skipped during iteration. + * + * PACKET_CONTAINER: a valid EventPacketContainer handle. If NULL, no iteration is performed. + */ +#define CAER_EVENT_PACKET_CONTAINER_ITERATOR_START(PACKET_CONTAINER) \ + if ((PACKET_CONTAINER) != NULL) { \ + for (int32_t caerEventPacketContainerIteratorCounter = 0; \ + caerEventPacketContainerIteratorCounter < caerEventPacketContainerGetEventPacketsNumber(PACKET_CONTAINER); \ + caerEventPacketContainerIteratorCounter++) { \ + caerEventPacketHeader caerEventPacketContainerIteratorElement = caerEventPacketContainerGetEventPacket(PACKET_CONTAINER, caerEventPacketContainerIteratorCounter); \ + if (caerEventPacketContainerIteratorElement == NULL) { continue; } + +/** + * Iterator close statement. + */ +#define CAER_EVENT_PACKET_CONTAINER_ITERATOR_END } } + +/** + * Get the reference for an EventPacket stored in this container + * with the given event type. This returns the first found event packet + * with that type ID, or NULL if we get to the end without finding any + * such event packet. + * + * @param container a valid EventPacketContainer handle. If NULL, returns NULL too. + * @param typeID the event type to search for. + * + * @return a reference to an EventPacket with a certain type or NULL if none found. + */ +static inline caerEventPacketHeader caerEventPacketContainerFindEventPacketByType(caerEventPacketContainer container, + int16_t typeID) { + // Non-existing (empty) containers have no valid packets in them! + if (container == NULL) { + return (NULL); + } + + CAER_EVENT_PACKET_CONTAINER_ITERATOR_START(container) + if (caerEventPacketHeaderGetEventType(caerEventPacketContainerIteratorElement) == typeID) { + // Found it, return it. + return (caerEventPacketContainerIteratorElement); + } + CAER_EVENT_PACKET_CONTAINER_ITERATOR_END + + // Found nothing, return nothing. + return (NULL); +} + +/** + * Make a deep copy of an event packet container and all of its + * event packets and their current events. + * + * @param container an event packet container to copy. + * + * @return a deep copy of an event packet container, containing all events. + */ +static inline caerEventPacketContainer caerEventPacketContainerCopyAllEvents(caerEventPacketContainer container) { + if (container == NULL) { + return (NULL); + } + + caerEventPacketContainer newContainer = caerEventPacketContainerAllocate( + caerEventPacketContainerGetEventsNumber(container)); + if (newContainer == NULL) { + return (NULL); + } + + CAER_EVENT_PACKET_CONTAINER_ITERATOR_START(container) + caerEventPacketContainerSetEventPacket(newContainer, caerEventPacketContainerIteratorCounter, + (caerEventPacketHeader) caerCopyEventPacketOnlyEvents((void *) caerEventPacketContainerIteratorElement)); + CAER_EVENT_PACKET_CONTAINER_ITERATOR_END + + return (newContainer); +} + +/** + * Make a deep copy of an event packet container, with its event packets + * sized down to only include the currently valid events (eventValid), + * and discarding everything else. + * + * @param container an event packet container to copy. + * + * @return a deep copy of an event packet container, containing only valid events. + */ +static inline caerEventPacketContainer caerEventPacketContainerCopyValidEvents(caerEventPacketContainer container) { + if (container == NULL) { + return (NULL); + } + + caerEventPacketContainer newContainer = caerEventPacketContainerAllocate( + caerEventPacketContainerGetEventsNumber(container)); + if (newContainer == NULL) { + return (NULL); + } + + CAER_EVENT_PACKET_CONTAINER_ITERATOR_START(container) + caerEventPacketContainerSetEventPacket(newContainer, caerEventPacketContainerIteratorCounter, + (caerEventPacketHeader) caerCopyEventPacketOnlyValidEvents((void *) caerEventPacketContainerIteratorElement)); + CAER_EVENT_PACKET_CONTAINER_ITERATOR_END + + return (newContainer); +} + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_EVENTS_PACKETCONTAINER_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/point1d.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/point1d.h new file mode 100755 index 0000000000000000000000000000000000000000..5c797d0af8e5e872a4eca3a787c5645d0c23b66a --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/point1d.h @@ -0,0 +1,335 @@ +/** + * @file point1d.h + * + * THIS EVENT DEFINITIONS IS STILL TO BE CONSIDERED EXPERIMENTAL + * AND IS SUBJECT TO FUTURE CHANGES AND REVISIONS! + * + * Point1D Events format definition and handling functions. + * This contains one dimensional data points as floats, + * together with support for distinguishing type and scale. + */ + +#ifndef LIBCAER_EVENTS_POINT1D_H_ +#define LIBCAER_EVENTS_POINT1D_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "common.h" + +/** + * Shift and mask values for type and scale information + * associated with a Point1D event. + * Up to 128 types are supported. The scale is given as orders + * of magnitude, from 10^-128 to 10^127. + * Bit 0 is the valid mark, see 'common.h' for more details. + */ +//@{ +#define POINT1D_TYPE_SHIFT 1 +#define POINT1D_TYPE_MASK 0x0000007F +#define POINT1D_SCALE_SHIFT 8 +#define POINT1D_SCALE_MASK 0x000000FF +//@} + +/** + * Point1D event data structure definition. + * This contains information about the measurement, such as a type + * and a scale field, together with the usual validity mark. + * The one measurement (x) is stored as a float. + * Floats are in IEEE 754-2008 binary32 format. + * Signed integers are used for fields that are to be interpreted + * directly, for compatibility with languages that do not have + * unsigned integer types, such as Java. + */ +struct caer_point1d_event { + /// Event information. First because of valid mark. + uint32_t info; + /// X axis measurement. + float x; + /// Event timestamp. + int32_t timestamp; +}__attribute__((__packed__)); + +/** + * Type for pointer to Point1D event data structure. + */ +typedef struct caer_point1d_event *caerPoint1DEvent; + +/** + * Point1D event packet data structure definition. + * EventPackets are always made up of the common packet header, + * followed by 'eventCapacity' events. Everything has to + * be in one contiguous memory block. + */ +struct caer_point1d_event_packet { + /// The common event packet header. + struct caer_event_packet_header packetHeader; + /// The events array. + struct caer_point1d_event events[]; +}__attribute__((__packed__)); + +/** + * Type for pointer to Point1D event packet data structure. + */ +typedef struct caer_point1d_event_packet *caerPoint1DEventPacket; + +/** + * Allocate a new Point1D events packet. + * Use free() to reclaim this memory. + * + * @param eventCapacity the maximum number of events this packet will hold. + * @param eventSource the unique ID representing the source/generator of this packet. + * @param tsOverflow the current timestamp overflow counter value for this packet. + * + * @return a valid Point1DEventPacket handle or NULL on error. + */ +caerPoint1DEventPacket caerPoint1DEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow); + +/** + * Get the Point1D event at the given index from the event packet. + * + * @param packet a valid Point1DEventPacket pointer. Cannot be NULL. + * @param n the index of the returned event. Must be within [0,eventCapacity[ bounds. + * + * @return the requested Point1D event. NULL on error. + */ +static inline caerPoint1DEvent caerPoint1DEventPacketGetEvent(caerPoint1DEventPacket packet, int32_t n) { + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketHeaderGetEventCapacity(&packet->packetHeader)) { + caerLog(CAER_LOG_CRITICAL, "Point1D Event", + "Called caerPoint1DEventPacketGetEvent() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ".", + n, caerEventPacketHeaderGetEventCapacity(&packet->packetHeader) - 1); + return (NULL); + } + + // Return a pointer to the specified event. + return (packet->events + n); +} + +/** + * Get the 32bit event timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid Point1DEvent pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond timestamp. + */ +static inline int32_t caerPoint1DEventGetTimestamp(caerPoint1DEvent event) { + return (le32toh(event->timestamp)); +} + +/** + * Get the 64bit event timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid Point1DEvent pointer. Cannot be NULL. + * @param packet the Point1DEventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond timestamp. + */ +static inline int64_t caerPoint1DEventGetTimestamp64(caerPoint1DEvent event, caerPoint1DEventPacket packet) { + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerPoint1DEventGetTimestamp(event)))); +} + +/** + * Set the 32bit event timestamp, the value has to be in microseconds. + * + * @param event a valid Point1DEvent pointer. Cannot be NULL. + * @param timestamp a positive 32bit microsecond timestamp. + */ +static inline void caerPoint1DEventSetTimestamp(caerPoint1DEvent event, int32_t timestamp) { + if (timestamp < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "Point1D Event", "Called caerPoint1DEventSetTimestamp() with negative value!"); + return; + } + + event->timestamp = htole32(timestamp); +} + +/** + * Check if this Point1D event is valid. + * + * @param event a valid Point1DEvent pointer. Cannot be NULL. + * + * @return true if valid, false if not. + */ +static inline bool caerPoint1DEventIsValid(caerPoint1DEvent event) { + return (GET_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK)); +} + +/** + * Validate the current event by setting its valid bit to true + * and increasing the event packet's event count and valid + * event count. Only works on events that are invalid. + * DO NOT CALL THIS AFTER HAVING PREVIOUSLY ALREADY + * INVALIDATED THIS EVENT, the total count will be incorrect. + * + * @param event a valid Point1DEvent pointer. Cannot be NULL. + * @param packet the Point1DEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerPoint1DEventValidate(caerPoint1DEvent event, caerPoint1DEventPacket packet) { + if (!caerPoint1DEventIsValid(event)) { + SET_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK, 1); + + // Also increase number of events and valid events. + // Only call this on (still) invalid events! + caerEventPacketHeaderSetEventNumber(&packet->packetHeader, + caerEventPacketHeaderGetEventNumber(&packet->packetHeader) + 1); + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) + 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Point1D Event", "Called caerPoint1DEventValidate() on already valid event."); + } +} + +/** + * Invalidate the current event by setting its valid bit + * to false and decreasing the number of valid events held + * in the packet. Only works with events that are already + * valid! + * + * @param event a valid Point1DEvent pointer. Cannot be NULL. + * @param packet the Point1DEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerPoint1DEventInvalidate(caerPoint1DEvent event, caerPoint1DEventPacket packet) { + if (caerPoint1DEventIsValid(event)) { + CLEAR_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK); + + // Also decrease number of valid events. Number of total events doesn't change. + // Only call this on valid events! + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) - 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Point1D Event", "Called caerPoint1DEventInvalidate() on already invalid event."); + } +} + +/** + * Get the measurement event type. This is useful to distinguish + * between different measurements, for example distance or weight. + * + * @param event a valid Point1DEvent pointer. Cannot be NULL. + * + * @return the Point1D measurement type. + */ +static inline uint8_t caerPoint1DEventGetType(caerPoint1DEvent event) { + return U8T(GET_NUMBITS32(event->info, POINT1D_TYPE_SHIFT, POINT1D_TYPE_MASK)); +} + +/** + * Set the measurement event type. This is useful to distinguish + * between different measurements, for example distance or weight. + * + * @param event a valid Point1DEvent pointer. Cannot be NULL. + * @param type the Point1D measurement type. + */ +static inline void caerPoint1DEventSetType(caerPoint1DEvent event, uint8_t type) { + CLEAR_NUMBITS32(event->info, POINT1D_TYPE_SHIFT, POINT1D_TYPE_MASK); + SET_NUMBITS32(event->info, POINT1D_TYPE_SHIFT, POINT1D_TYPE_MASK, type); +} + +/** + * Get the measurement scale. This allows order of magnitude shifts + * on the measured value to be applied automatically, such as having + * measurements of type Distance (meters) and storing the values as + * centimeters (10^-2) for higher precision, but keeping that information + * around to allow easy changes of unit. + * + * @param event a valid Point1DEvent pointer. Cannot be NULL. + * + * @return the Point1D measurement scale. + */ +static inline int8_t caerPoint1DEventGetScale(caerPoint1DEvent event) { + return I8T(GET_NUMBITS32(event->info, POINT1D_SCALE_SHIFT, POINT1D_SCALE_MASK)); +} + +/** + * Set the measurement scale. This allows order of magnitude shifts + * on the measured value to be applied automatically, such as having + * measurements of type Distance (meters) and storing the values as + * centimeters (10^-2) for higher precision, but keeping that information + * around to allow easy changes of unit. + * + * @param event a valid Point1DEvent pointer. Cannot be NULL. + * @param scale the Point1D measurement scale. + */ +static inline void caerPoint1DEventSetScale(caerPoint1DEvent event, int8_t scale) { + CLEAR_NUMBITS32(event->info, POINT1D_SCALE_SHIFT, POINT1D_SCALE_MASK); + SET_NUMBITS32(event->info, POINT1D_SCALE_SHIFT, POINT1D_SCALE_MASK, U8T(scale)); +} + +/** + * Get the X axis measurement. + * + * @param event a valid Point1DEvent pointer. Cannot be NULL. + * + * @return X axis measurement. + */ +static inline float caerPoint1DEventGetX(caerPoint1DEvent event) { + return (le32toh(event->x)); +} + +/** + * Set the X axis measurement. + * + * @param event a valid Point1DEvent pointer. Cannot be NULL. + * @param x X axis measurement. + */ +static inline void caerPoint1DEventSetX(caerPoint1DEvent event, float x) { + event->x = htole32(x); +} + +/** + * Iterator over all Point1D events in a packet. + * Returns the current index in the 'caerPoint1DIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerPoint1DIteratorElement' variable + * of type caerPoint1DEvent. + * + * POINT1D_PACKET: a valid Point1DEventPacket pointer. Cannot be NULL. + */ +#define CAER_POINT1D_ITERATOR_ALL_START(POINT1D_PACKET) \ + for (int32_t caerPoint1DIteratorCounter = 0; \ + caerPoint1DIteratorCounter < caerEventPacketHeaderGetEventNumber(&(POINT1D_PACKET)->packetHeader); \ + caerPoint1DIteratorCounter++) { \ + caerPoint1DEvent caerPoint1DIteratorElement = caerPoint1DEventPacketGetEvent(POINT1D_PACKET, caerPoint1DIteratorCounter); + +/** + * Iterator close statement. + */ +#define CAER_POINT1D_ITERATOR_ALL_END } + +/** + * Iterator over only the valid Point1D events in a packet. + * Returns the current index in the 'caerPoint1DIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerPoint1DIteratorElement' variable + * of type caerPoint1DEvent. + * + * POINT1D_PACKET: a valid Point1DEventPacket pointer. Cannot be NULL. + */ +#define CAER_POINT1D_ITERATOR_VALID_START(POINT1D_PACKET) \ + for (int32_t caerPoint1DIteratorCounter = 0; \ + caerPoint1DIteratorCounter < caerEventPacketHeaderGetEventNumber(&(POINT1D_PACKET)->packetHeader); \ + caerPoint1DIteratorCounter++) { \ + caerPoint1DEvent caerPoint1DIteratorElement = caerPoint1DEventPacketGetEvent(POINT1D_PACKET, caerPoint1DIteratorCounter); \ + if (!caerPoint1DEventIsValid(caerPoint1DIteratorElement)) { continue; } // Skip invalid Point1D events. + +/** + * Iterator close statement. + */ +#define CAER_POINT1D_ITERATOR_VALID_END } + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_EVENTS_POINT1D_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/point2d.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/point2d.h new file mode 100755 index 0000000000000000000000000000000000000000..eed4668914a84b2df27a55b62e7a69458d5cf8f3 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/point2d.h @@ -0,0 +1,358 @@ +/** + * @file point2d.h + * + * THIS EVENT DEFINITIONS IS STILL TO BE CONSIDERED EXPERIMENTAL + * AND IS SUBJECT TO FUTURE CHANGES AND REVISIONS! + * + * Point2D Events format definition and handling functions. + * This contains two dimensional data points as floats, + * together with support for distinguishing type and scale. + */ + +#ifndef LIBCAER_EVENTS_POINT2D_H_ +#define LIBCAER_EVENTS_POINT2D_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "common.h" + +/** + * Shift and mask values for type and scale information + * associated with a Point2D event. + * Up to 128 types are supported. The scale is given as orders + * of magnitude, from 10^-128 to 10^127. + * Bit 0 is the valid mark, see 'common.h' for more details. + */ +//@{ +#define POINT2D_TYPE_SHIFT 1 +#define POINT2D_TYPE_MASK 0x0000007F +#define POINT2D_SCALE_SHIFT 8 +#define POINT2D_SCALE_MASK 0x000000FF +//@} + +/** + * Point2D event data structure definition. + * This contains information about the measurement, such as a type + * and a scale field, together with the usual validity mark. + * The two measurements (x, y) are stored as floats. + * Floats are in IEEE 754-2008 binary32 format. + * Signed integers are used for fields that are to be interpreted + * directly, for compatibility with languages that do not have + * unsigned integer types, such as Java. + */ +struct caer_point2d_event { + /// Event information. First because of valid mark. + uint32_t info; + /// X axis measurement. + float x; + /// Y axis measurement. + float y; + /// Event timestamp. + int32_t timestamp; +}__attribute__((__packed__)); + +/** + * Type for pointer to Point2D event data structure. + */ +typedef struct caer_point2d_event *caerPoint2DEvent; + +/** + * Point2D event packet data structure definition. + * EventPackets are always made up of the common packet header, + * followed by 'eventCapacity' events. Everything has to + * be in one contiguous memory block. + */ +struct caer_point2d_event_packet { + /// The common event packet header. + struct caer_event_packet_header packetHeader; + /// The events array. + struct caer_point2d_event events[]; +}__attribute__((__packed__)); + +/** + * Type for pointer to Point2D event packet data structure. + */ +typedef struct caer_point2d_event_packet *caerPoint2DEventPacket; + +/** + * Allocate a new Point2D events packet. + * Use free() to reclaim this memory. + * + * @param eventCapacity the maximum number of events this packet will hold. + * @param eventSource the unique ID representing the source/generator of this packet. + * @param tsOverflow the current timestamp overflow counter value for this packet. + * + * @return a valid Point2DEventPacket handle or NULL on error. + */ +caerPoint2DEventPacket caerPoint2DEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow); + +/** + * Get the Point2D event at the given index from the event packet. + * + * @param packet a valid Point2DEventPacket pointer. Cannot be NULL. + * @param n the index of the returned event. Must be within [0,eventCapacity[ bounds. + * + * @return the requested Point2D event. NULL on error. + */ +static inline caerPoint2DEvent caerPoint2DEventPacketGetEvent(caerPoint2DEventPacket packet, int32_t n) { + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketHeaderGetEventCapacity(&packet->packetHeader)) { + caerLog(CAER_LOG_CRITICAL, "Point2D Event", + "Called caerPoint2DEventPacketGetEvent() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ".", + n, caerEventPacketHeaderGetEventCapacity(&packet->packetHeader) - 1); + return (NULL); + } + + // Return a pointer to the specified event. + return (packet->events + n); +} + +/** + * Get the 32bit event timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid Point2DEvent pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond timestamp. + */ +static inline int32_t caerPoint2DEventGetTimestamp(caerPoint2DEvent event) { + return (le32toh(event->timestamp)); +} + +/** + * Get the 64bit event timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid Point2DEvent pointer. Cannot be NULL. + * @param packet the Point2DEventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond timestamp. + */ +static inline int64_t caerPoint2DEventGetTimestamp64(caerPoint2DEvent event, caerPoint2DEventPacket packet) { + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerPoint2DEventGetTimestamp(event)))); +} + +/** + * Set the 32bit event timestamp, the value has to be in microseconds. + * + * @param event a valid Point2DEvent pointer. Cannot be NULL. + * @param timestamp a positive 32bit microsecond timestamp. + */ +static inline void caerPoint2DEventSetTimestamp(caerPoint2DEvent event, int32_t timestamp) { + if (timestamp < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "Point2D Event", "Called caerPoint2DEventSetTimestamp() with negative value!"); + return; + } + + event->timestamp = htole32(timestamp); +} + +/** + * Check if this Point2D event is valid. + * + * @param event a valid Point2DEvent pointer. Cannot be NULL. + * + * @return true if valid, false if not. + */ +static inline bool caerPoint2DEventIsValid(caerPoint2DEvent event) { + return (GET_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK)); +} + +/** + * Validate the current event by setting its valid bit to true + * and increasing the event packet's event count and valid + * event count. Only works on events that are invalid. + * DO NOT CALL THIS AFTER HAVING PREVIOUSLY ALREADY + * INVALIDATED THIS EVENT, the total count will be incorrect. + * + * @param event a valid Point2DEvent pointer. Cannot be NULL. + * @param packet the Point2DEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerPoint2DEventValidate(caerPoint2DEvent event, caerPoint2DEventPacket packet) { + if (!caerPoint2DEventIsValid(event)) { + SET_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK, 1); + + // Also increase number of events and valid events. + // Only call this on (still) invalid events! + caerEventPacketHeaderSetEventNumber(&packet->packetHeader, + caerEventPacketHeaderGetEventNumber(&packet->packetHeader) + 1); + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) + 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Point2D Event", "Called caerPoint2DEventValidate() on already valid event."); + } +} + +/** + * Invalidate the current event by setting its valid bit + * to false and decreasing the number of valid events held + * in the packet. Only works with events that are already + * valid! + * + * @param event a valid Point2DEvent pointer. Cannot be NULL. + * @param packet the Point2DEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerPoint2DEventInvalidate(caerPoint2DEvent event, caerPoint2DEventPacket packet) { + if (caerPoint2DEventIsValid(event)) { + CLEAR_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK); + + // Also decrease number of valid events. Number of total events doesn't change. + // Only call this on valid events! + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) - 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Point2D Event", "Called caerPoint2DEventInvalidate() on already invalid event."); + } +} + +/** + * Get the measurement event type. This is useful to distinguish + * between different measurements, for example distance or weight. + * + * @param event a valid Point2DEvent pointer. Cannot be NULL. + * + * @return the Point2D measurement type. + */ +static inline uint8_t caerPoint2DEventGetType(caerPoint2DEvent event) { + return U8T(GET_NUMBITS32(event->info, POINT2D_TYPE_SHIFT, POINT2D_TYPE_MASK)); +} + +/** + * Set the measurement event type. This is useful to distinguish + * between different measurements, for example distance or weight. + * + * @param event a valid Point2DEvent pointer. Cannot be NULL. + * @param type the Point2D measurement type. + */ +static inline void caerPoint2DEventSetType(caerPoint2DEvent event, uint8_t type) { + CLEAR_NUMBITS32(event->info, POINT2D_TYPE_SHIFT, POINT2D_TYPE_MASK); + SET_NUMBITS32(event->info, POINT2D_TYPE_SHIFT, POINT2D_TYPE_MASK, type); +} + +/** + * Get the measurement scale. This allows order of magnitude shifts + * on the measured value to be applied automatically, such as having + * measurements of type Distance (meters) and storing the values as + * centimeters (10^-2) for higher precision, but keeping that information + * around to allow easy changes of unit. + * + * @param event a valid Point2DEvent pointer. Cannot be NULL. + * + * @return the Point2D measurement scale. + */ +static inline int8_t caerPoint2DEventGetScale(caerPoint2DEvent event) { + return I8T(GET_NUMBITS32(event->info, POINT2D_SCALE_SHIFT, POINT2D_SCALE_MASK)); +} + +/** + * Set the measurement scale. This allows order of magnitude shifts + * on the measured value to be applied automatically, such as having + * measurements of type Distance (meters) and storing the values as + * centimeters (10^-2) for higher precision, but keeping that information + * around to allow easy changes of unit. + * + * @param event a valid Point2DEvent pointer. Cannot be NULL. + * @param scale the Point2D measurement scale. + */ +static inline void caerPoint2DEventSetScale(caerPoint2DEvent event, int8_t scale) { + CLEAR_NUMBITS32(event->info, POINT2D_SCALE_SHIFT, POINT2D_SCALE_MASK); + SET_NUMBITS32(event->info, POINT2D_SCALE_SHIFT, POINT2D_SCALE_MASK, U8T(scale)); +} + +/** + * Get the X axis measurement. + * + * @param event a valid Point2DEvent pointer. Cannot be NULL. + * + * @return X axis measurement. + */ +static inline float caerPoint2DEventGetX(caerPoint2DEvent event) { + return (le32toh(event->x)); +} + +/** + * Set the X axis measurement. + * + * @param event a valid Point2DEvent pointer. Cannot be NULL. + * @param x X axis measurement. + */ +static inline void caerPoint2DEventSetX(caerPoint2DEvent event, float x) { + event->x = htole32(x); +} + +/** + * Get the Y axis measurement. + * + * @param event a valid Point2DEvent pointer. Cannot be NULL. + * + * @return Y axis measurement. + */ +static inline float caerPoint2DEventGetY(caerPoint2DEvent event) { + return (le32toh(event->y)); +} + +/** + * Set the Y axis measurement. + * + * @param event a valid Point2DEvent pointer. Cannot be NULL. + * @param y Y axis measurement. + */ +static inline void caerPoint2DEventSetY(caerPoint2DEvent event, float y) { + event->y = htole32(y); +} + +/** + * Iterator over all Point2D events in a packet. + * Returns the current index in the 'caerPoint2DIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerPoint2DIteratorElement' variable + * of type caerPoint2DEvent. + * + * POINT2D_PACKET: a valid Point2DEventPacket pointer. Cannot be NULL. + */ +#define CAER_POINT2D_ITERATOR_ALL_START(POINT2D_PACKET) \ + for (int32_t caerPoint2DIteratorCounter = 0; \ + caerPoint2DIteratorCounter < caerEventPacketHeaderGetEventNumber(&(POINT2D_PACKET)->packetHeader); \ + caerPoint2DIteratorCounter++) { \ + caerPoint2DEvent caerPoint2DIteratorElement = caerPoint2DEventPacketGetEvent(POINT2D_PACKET, caerPoint2DIteratorCounter); + +/** + * Iterator close statement. + */ +#define CAER_POINT2D_ITERATOR_ALL_END } + +/** + * Iterator over only the valid Point2D events in a packet. + * Returns the current index in the 'caerPoint2DIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerPoint2DIteratorElement' variable + * of type caerPoint2DEvent. + * + * POINT2D_PACKET: a valid Point2DEventPacket pointer. Cannot be NULL. + */ +#define CAER_POINT2D_ITERATOR_VALID_START(POINT2D_PACKET) \ + for (int32_t caerPoint2DIteratorCounter = 0; \ + caerPoint2DIteratorCounter < caerEventPacketHeaderGetEventNumber(&(POINT2D_PACKET)->packetHeader); \ + caerPoint2DIteratorCounter++) { \ + caerPoint2DEvent caerPoint2DIteratorElement = caerPoint2DEventPacketGetEvent(POINT2D_PACKET, caerPoint2DIteratorCounter); \ + if (!caerPoint2DEventIsValid(caerPoint2DIteratorElement)) { continue; } // Skip invalid Point2D events. + +/** + * Iterator close statement. + */ +#define CAER_POINT2D_ITERATOR_VALID_END } + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_EVENTS_POINT2D_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/point3d.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/point3d.h new file mode 100755 index 0000000000000000000000000000000000000000..a86cc6a0e03d4f4313b9ac7836f9da8187f6063f --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/point3d.h @@ -0,0 +1,381 @@ +/** + * @file point3d.h + * + * THIS EVENT DEFINITIONS IS STILL TO BE CONSIDERED EXPERIMENTAL + * AND IS SUBJECT TO FUTURE CHANGES AND REVISIONS! + * + * Point3D Events format definition and handling functions. + * This contains three dimensional data points as floats, + * together with support for distinguishing type and scale. + */ + +#ifndef LIBCAER_EVENTS_POINT3D_H_ +#define LIBCAER_EVENTS_POINT3D_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "common.h" + +/** + * Shift and mask values for type and scale information + * associated with a Point3D event. + * Up to 128 types are supported. The scale is given as orders + * of magnitude, from 10^-128 to 10^127. + * Bit 0 is the valid mark, see 'common.h' for more details. + */ +//@{ +#define POINT3D_TYPE_SHIFT 1 +#define POINT3D_TYPE_MASK 0x0000007F +#define POINT3D_SCALE_SHIFT 8 +#define POINT3D_SCALE_MASK 0x000000FF +//@} + +/** + * Point3D event data structure definition. + * This contains information about the measurement, such as a type + * and a scale field, together with the usual validity mark. + * The three measurements (x, y, z) are stored as floats. + * Floats are in IEEE 754-2008 binary32 format. + * Signed integers are used for fields that are to be interpreted + * directly, for compatibility with languages that do not have + * unsigned integer types, such as Java. + */ +struct caer_point3d_event { + /// Event information. First because of valid mark. + uint32_t info; + /// X axis measurement. + float x; + /// Y axis measurement. + float y; + /// Z axis measurement. + float z; + /// Event timestamp. + int32_t timestamp; +}__attribute__((__packed__)); + +/** + * Type for pointer to Point3D event data structure. + */ +typedef struct caer_point3d_event *caerPoint3DEvent; + +/** + * Point3D event packet data structure definition. + * EventPackets are always made up of the common packet header, + * followed by 'eventCapacity' events. Everything has to + * be in one contiguous memory block. + */ +struct caer_point3d_event_packet { + /// The common event packet header. + struct caer_event_packet_header packetHeader; + /// The events array. + struct caer_point3d_event events[]; +}__attribute__((__packed__)); + +/** + * Type for pointer to Point3D event packet data structure. + */ +typedef struct caer_point3d_event_packet *caerPoint3DEventPacket; + +/** + * Allocate a new Point3D events packet. + * Use free() to reclaim this memory. + * + * @param eventCapacity the maximum number of events this packet will hold. + * @param eventSource the unique ID representing the source/generator of this packet. + * @param tsOverflow the current timestamp overflow counter value for this packet. + * + * @return a valid Point3DEventPacket handle or NULL on error. + */ +caerPoint3DEventPacket caerPoint3DEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow); + +/** + * Get the Point3D event at the given index from the event packet. + * + * @param packet a valid Point3DEventPacket pointer. Cannot be NULL. + * @param n the index of the returned event. Must be within [0,eventCapacity[ bounds. + * + * @return the requested Point3D event. NULL on error. + */ +static inline caerPoint3DEvent caerPoint3DEventPacketGetEvent(caerPoint3DEventPacket packet, int32_t n) { + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketHeaderGetEventCapacity(&packet->packetHeader)) { + caerLog(CAER_LOG_CRITICAL, "Point3D Event", + "Called caerPoint3DEventPacketGetEvent() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ".", + n, caerEventPacketHeaderGetEventCapacity(&packet->packetHeader) - 1); + return (NULL); + } + + // Return a pointer to the specified event. + return (packet->events + n); +} + +/** + * Get the 32bit event timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond timestamp. + */ +static inline int32_t caerPoint3DEventGetTimestamp(caerPoint3DEvent event) { + return (le32toh(event->timestamp)); +} + +/** + * Get the 64bit event timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * @param packet the Point3DEventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond timestamp. + */ +static inline int64_t caerPoint3DEventGetTimestamp64(caerPoint3DEvent event, caerPoint3DEventPacket packet) { + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerPoint3DEventGetTimestamp(event)))); +} + +/** + * Set the 32bit event timestamp, the value has to be in microseconds. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * @param timestamp a positive 32bit microsecond timestamp. + */ +static inline void caerPoint3DEventSetTimestamp(caerPoint3DEvent event, int32_t timestamp) { + if (timestamp < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "Point3D Event", "Called caerPoint3DEventSetTimestamp() with negative value!"); + return; + } + + event->timestamp = htole32(timestamp); +} + +/** + * Check if this Point3D event is valid. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * + * @return true if valid, false if not. + */ +static inline bool caerPoint3DEventIsValid(caerPoint3DEvent event) { + return (GET_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK)); +} + +/** + * Validate the current event by setting its valid bit to true + * and increasing the event packet's event count and valid + * event count. Only works on events that are invalid. + * DO NOT CALL THIS AFTER HAVING PREVIOUSLY ALREADY + * INVALIDATED THIS EVENT, the total count will be incorrect. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * @param packet the Point3DEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerPoint3DEventValidate(caerPoint3DEvent event, caerPoint3DEventPacket packet) { + if (!caerPoint3DEventIsValid(event)) { + SET_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK, 1); + + // Also increase number of events and valid events. + // Only call this on (still) invalid events! + caerEventPacketHeaderSetEventNumber(&packet->packetHeader, + caerEventPacketHeaderGetEventNumber(&packet->packetHeader) + 1); + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) + 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Point3D Event", "Called caerPoint3DEventValidate() on already valid event."); + } +} + +/** + * Invalidate the current event by setting its valid bit + * to false and decreasing the number of valid events held + * in the packet. Only works with events that are already + * valid! + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * @param packet the Point3DEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerPoint3DEventInvalidate(caerPoint3DEvent event, caerPoint3DEventPacket packet) { + if (caerPoint3DEventIsValid(event)) { + CLEAR_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK); + + // Also decrease number of valid events. Number of total events doesn't change. + // Only call this on valid events! + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) - 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Point3D Event", "Called caerPoint3DEventInvalidate() on already invalid event."); + } +} + +/** + * Get the measurement event type. This is useful to distinguish + * between different measurements, for example distance or weight. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * + * @return the Point3D measurement type. + */ +static inline uint8_t caerPoint3DEventGetType(caerPoint3DEvent event) { + return U8T(GET_NUMBITS32(event->info, POINT3D_TYPE_SHIFT, POINT3D_TYPE_MASK)); +} + +/** + * Set the measurement event type. This is useful to distinguish + * between different measurements, for example distance or weight. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * @param type the Point3D measurement type. + */ +static inline void caerPoint3DEventSetType(caerPoint3DEvent event, uint8_t type) { + CLEAR_NUMBITS32(event->info, POINT3D_TYPE_SHIFT, POINT3D_TYPE_MASK); + SET_NUMBITS32(event->info, POINT3D_TYPE_SHIFT, POINT3D_TYPE_MASK, type); +} + +/** + * Get the measurement scale. This allows order of magnitude shifts + * on the measured value to be applied automatically, such as having + * measurements of type Distance (meters) and storing the values as + * centimeters (10^-2) for higher precision, but keeping that information + * around to allow easy changes of unit. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * + * @return the Point3D measurement scale. + */ +static inline int8_t caerPoint3DEventGetScale(caerPoint3DEvent event) { + return I8T(GET_NUMBITS32(event->info, POINT3D_SCALE_SHIFT, POINT3D_SCALE_MASK)); +} + +/** + * Set the measurement scale. This allows order of magnitude shifts + * on the measured value to be applied automatically, such as having + * measurements of type Distance (meters) and storing the values as + * centimeters (10^-2) for higher precision, but keeping that information + * around to allow easy changes of unit. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * @param scale the Point3D measurement scale. + */ +static inline void caerPoint3DEventSetScale(caerPoint3DEvent event, int8_t scale) { + CLEAR_NUMBITS32(event->info, POINT3D_SCALE_SHIFT, POINT3D_SCALE_MASK); + SET_NUMBITS32(event->info, POINT3D_SCALE_SHIFT, POINT3D_SCALE_MASK, U8T(scale)); +} + +/** + * Get the X axis measurement. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * + * @return X axis measurement. + */ +static inline float caerPoint3DEventGetX(caerPoint3DEvent event) { + return (le32toh(event->x)); +} + +/** + * Set the X axis measurement. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * @param x X axis measurement. + */ +static inline void caerPoint3DEventSetX(caerPoint3DEvent event, float x) { + event->x = htole32(x); +} + +/** + * Get the Y axis measurement. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * + * @return Y axis measurement. + */ +static inline float caerPoint3DEventGetY(caerPoint3DEvent event) { + return (le32toh(event->y)); +} + +/** + * Set the Y axis measurement. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * @param y Y axis measurement. + */ +static inline void caerPoint3DEventSetY(caerPoint3DEvent event, float y) { + event->y = htole32(y); +} + +/** + * Get the Z axis measurement. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * + * @return Z axis measurement. + */ +static inline float caerPoint3DEventGetZ(caerPoint3DEvent event) { + return (le32toh(event->z)); +} + +/** + * Set the Z axis measurement. + * + * @param event a valid Point3DEvent pointer. Cannot be NULL. + * @param z Z axis measurement. + */ +static inline void caerPoint3DEventSetZ(caerPoint3DEvent event, float z) { + event->z = htole32(z); +} + +/** + * Iterator over all Point3D events in a packet. + * Returns the current index in the 'caerPoint3DIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerPoint3DIteratorElement' variable + * of type caerPoint3DEvent. + * + * POINT3D_PACKET: a valid Point3DEventPacket pointer. Cannot be NULL. + */ +#define CAER_POINT3D_ITERATOR_ALL_START(POINT3D_PACKET) \ + for (int32_t caerPoint3DIteratorCounter = 0; \ + caerPoint3DIteratorCounter < caerEventPacketHeaderGetEventNumber(&(POINT3D_PACKET)->packetHeader); \ + caerPoint3DIteratorCounter++) { \ + caerPoint3DEvent caerPoint3DIteratorElement = caerPoint3DEventPacketGetEvent(POINT3D_PACKET, caerPoint3DIteratorCounter); + +/** + * Iterator close statement. + */ +#define CAER_POINT3D_ITERATOR_ALL_END } + +/** + * Iterator over only the valid Point3D events in a packet. + * Returns the current index in the 'caerPoint3DIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerPoint3DIteratorElement' variable + * of type caerPoint3DEvent. + * + * POINT3D_PACKET: a valid Point3DEventPacket pointer. Cannot be NULL. + */ +#define CAER_POINT3D_ITERATOR_VALID_START(POINT3D_PACKET) \ + for (int32_t caerPoint3DIteratorCounter = 0; \ + caerPoint3DIteratorCounter < caerEventPacketHeaderGetEventNumber(&(POINT3D_PACKET)->packetHeader); \ + caerPoint3DIteratorCounter++) { \ + caerPoint3DEvent caerPoint3DIteratorElement = caerPoint3DEventPacketGetEvent(POINT3D_PACKET, caerPoint3DIteratorCounter); \ + if (!caerPoint3DEventIsValid(caerPoint3DIteratorElement)) { continue; } // Skip invalid Point3D events. + +/** + * Iterator close statement. + */ +#define CAER_POINT3D_ITERATOR_VALID_END } + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_EVENTS_POINT3D_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/point4d.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/point4d.h new file mode 100755 index 0000000000000000000000000000000000000000..2c115757dad392c8d75dd183e54090dbce963e5e --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/point4d.h @@ -0,0 +1,405 @@ +/** + * @file point4d.h + * + * THIS EVENT DEFINITION IS STILL TO BE CONSIDERED EXPERIMENTAL + * AND IS SUBJECT TO FUTURE CHANGES AND REVISIONS! + * + * Point4D Events format definition and handling functions. + * This contains four dimensional data points as floats, + * together with support for distinguishing type and scale. + * Useful for homogeneous coordinates for example. + */ + +#ifndef LIBCAER_EVENTS_POINT4D_H_ +#define LIBCAER_EVENTS_POINT4D_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "common.h" + +/** + * Shift and mask values for type and scale information + * associated with a Point4D event. + * Up to 128 types are supported. The scale is given as orders + * of magnitude, from 10^-128 to 10^127. + * Bit 0 is the valid mark, see 'common.h' for more details. + */ +//@{ +#define POINT4D_TYPE_SHIFT 1 +#define POINT4D_TYPE_MASK 0x0000007F +#define POINT4D_SCALE_SHIFT 8 +#define POINT4D_SCALE_MASK 0x000000FF +//@} + +/** + * Point4D event data structure definition. + * This contains information about the measurement, such as a type + * and a scale field, together with the usual validity mark. + * The four measurements (x, y, z, w) are stored as floats. + * Floats are in IEEE 754-2008 binary32 format. + * Signed integers are used for fields that are to be interpreted + * directly, for compatibility with languages that do not have + * unsigned integer types, such as Java. + */ +struct caer_point4d_event { + /// Event information. First because of valid mark. + uint32_t info; + /// X axis measurement. + float x; + /// Y axis measurement. + float y; + /// Z axis measurement. + float z; + /// W axis measurement. + float w; + /// Event timestamp. + int32_t timestamp; +}__attribute__((__packed__)); + +/** + * Type for pointer to Point4D event data structure. + */ +typedef struct caer_point4d_event *caerPoint4DEvent; + +/** + * Point4D event packet data structure definition. + * EventPackets are always made up of the common packet header, + * followed by 'eventCapacity' events. Everything has to + * be in one contiguous memory block. + */ +struct caer_point4d_event_packet { + /// The common event packet header. + struct caer_event_packet_header packetHeader; + /// The events array. + struct caer_point4d_event events[]; +}__attribute__((__packed__)); + +/** + * Type for pointer to Point4D event packet data structure. + */ +typedef struct caer_point4d_event_packet *caerPoint4DEventPacket; + +/** + * Allocate a new Point4D events packet. + * Use free() to reclaim this memory. + * + * @param eventCapacity the maximum number of events this packet will hold. + * @param eventSource the unique ID representing the source/generator of this packet. + * @param tsOverflow the current timestamp overflow counter value for this packet. + * + * @return a valid Point4DEventPacket handle or NULL on error. + */ +caerPoint4DEventPacket caerPoint4DEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow); + +/** + * Get the Point4D event at the given index from the event packet. + * + * @param packet a valid Point4DEventPacket pointer. Cannot be NULL. + * @param n the index of the returned event. Must be within [0,eventCapacity[ bounds. + * + * @return the requested Point4D event. NULL on error. + */ +static inline caerPoint4DEvent caerPoint4DEventPacketGetEvent(caerPoint4DEventPacket packet, int32_t n) { + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketHeaderGetEventCapacity(&packet->packetHeader)) { + caerLog(CAER_LOG_CRITICAL, "Point4D Event", + "Called caerPoint4DEventPacketGetEvent() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ".", + n, caerEventPacketHeaderGetEventCapacity(&packet->packetHeader) - 1); + return (NULL); + } + + // Return a pointer to the specified event. + return (packet->events + n); +} + +/** + * Get the 32bit event timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond timestamp. + */ +static inline int32_t caerPoint4DEventGetTimestamp(caerPoint4DEvent event) { + return (le32toh(event->timestamp)); +} + +/** + * Get the 64bit event timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * @param packet the Point4DEventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond timestamp. + */ +static inline int64_t caerPoint4DEventGetTimestamp64(caerPoint4DEvent event, caerPoint4DEventPacket packet) { + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerPoint4DEventGetTimestamp(event)))); +} + +/** + * Set the 32bit event timestamp, the value has to be in microseconds. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * @param timestamp a positive 32bit microsecond timestamp. + */ +static inline void caerPoint4DEventSetTimestamp(caerPoint4DEvent event, int32_t timestamp) { + if (timestamp < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "Point4D Event", "Called caerPoint4DEventSetTimestamp() with negative value!"); + return; + } + + event->timestamp = htole32(timestamp); +} + +/** + * Check if this Point4D event is valid. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * + * @return true if valid, false if not. + */ +static inline bool caerPoint4DEventIsValid(caerPoint4DEvent event) { + return (GET_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK)); +} + +/** + * Validate the current event by setting its valid bit to true + * and increasing the event packet's event count and valid + * event count. Only works on events that are invalid. + * DO NOT CALL THIS AFTER HAVING PREVIOUSLY ALREADY + * INVALIDATED THIS EVENT, the total count will be incorrect. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * @param packet the Point4DEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerPoint4DEventValidate(caerPoint4DEvent event, caerPoint4DEventPacket packet) { + if (!caerPoint4DEventIsValid(event)) { + SET_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK, 1); + + // Also increase number of events and valid events. + // Only call this on (still) invalid events! + caerEventPacketHeaderSetEventNumber(&packet->packetHeader, + caerEventPacketHeaderGetEventNumber(&packet->packetHeader) + 1); + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) + 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Point4D Event", "Called caerPoint4DEventValidate() on already valid event."); + } +} + +/** + * Invalidate the current event by setting its valid bit + * to false and decreasing the number of valid events held + * in the packet. Only works with events that are already + * valid! + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * @param packet the Point4DEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerPoint4DEventInvalidate(caerPoint4DEvent event, caerPoint4DEventPacket packet) { + if (caerPoint4DEventIsValid(event)) { + CLEAR_NUMBITS32(event->info, VALID_MARK_SHIFT, VALID_MARK_MASK); + + // Also decrease number of valid events. Number of total events doesn't change. + // Only call this on valid events! + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) - 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Point4D Event", "Called caerPoint4DEventInvalidate() on already invalid event."); + } +} + +/** + * Get the measurement event type. This is useful to distinguish + * between different measurements, for example distance or weight. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * + * @return the Point4D measurement type. + */ +static inline uint8_t caerPoint4DEventGetType(caerPoint4DEvent event) { + return U8T(GET_NUMBITS32(event->info, POINT4D_TYPE_SHIFT, POINT4D_TYPE_MASK)); +} + +/** + * Set the measurement event type. This is useful to distinguish + * between different measurements, for example distance or weight. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * @param type the Point4D measurement type. + */ +static inline void caerPoint4DEventSetType(caerPoint4DEvent event, uint8_t type) { + CLEAR_NUMBITS32(event->info, POINT4D_TYPE_SHIFT, POINT4D_TYPE_MASK); + SET_NUMBITS32(event->info, POINT4D_TYPE_SHIFT, POINT4D_TYPE_MASK, type); +} + +/** + * Get the measurement scale. This allows order of magnitude shifts + * on the measured value to be applied automatically, such as having + * measurements of type Distance (meters) and storing the values as + * centimeters (10^-2) for higher precision, but keeping that information + * around to allow easy changes of unit. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * + * @return the Point4D measurement scale. + */ +static inline int8_t caerPoint4DEventGetScale(caerPoint4DEvent event) { + return I8T(GET_NUMBITS32(event->info, POINT4D_SCALE_SHIFT, POINT4D_SCALE_MASK)); +} + +/** + * Set the measurement scale. This allows order of magnitude shifts + * on the measured value to be applied automatically, such as having + * measurements of type Distance (meters) and storing the values as + * centimeters (10^-2) for higher precision, but keeping that information + * around to allow easy changes of unit. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * @param scale the Point4D measurement scale. + */ +static inline void caerPoint4DEventSetScale(caerPoint4DEvent event, int8_t scale) { + CLEAR_NUMBITS32(event->info, POINT4D_SCALE_SHIFT, POINT4D_SCALE_MASK); + SET_NUMBITS32(event->info, POINT4D_SCALE_SHIFT, POINT4D_SCALE_MASK, U8T(scale)); +} + +/** + * Get the X axis measurement. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * + * @return X axis measurement. + */ +static inline float caerPoint4DEventGetX(caerPoint4DEvent event) { + return (le32toh(event->x)); +} + +/** + * Set the X axis measurement. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * @param x X axis measurement. + */ +static inline void caerPoint4DEventSetX(caerPoint4DEvent event, float x) { + event->x = htole32(x); +} + +/** + * Get the Y axis measurement. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * + * @return Y axis measurement. + */ +static inline float caerPoint4DEventGetY(caerPoint4DEvent event) { + return (le32toh(event->y)); +} + +/** + * Set the Y axis measurement. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * @param y Y axis measurement. + */ +static inline void caerPoint4DEventSetY(caerPoint4DEvent event, float y) { + event->y = htole32(y); +} + +/** + * Get the Z axis measurement. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * + * @return Z axis measurement. + */ +static inline float caerPoint4DEventGetZ(caerPoint4DEvent event) { + return (le32toh(event->z)); +} + +/** + * Set the Z axis measurement. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * @param z Z axis measurement. + */ +static inline void caerPoint4DEventSetZ(caerPoint4DEvent event, float z) { + event->z = htole32(z); +} + +/** + * Get the W axis measurement. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * + * @return W axis measurement. + */ +static inline float caerPoint4DEventGetW(caerPoint4DEvent event) { + return (le32toh(event->w)); +} + +/** + * Set the W axis measurement. + * + * @param event a valid Point4DEvent pointer. Cannot be NULL. + * @param w W axis measurement. + */ +static inline void caerPoint4DEventSetW(caerPoint4DEvent event, float w) { + event->w = htole32(w); +} + +/** + * Iterator over all Point4D events in a packet. + * Returns the current index in the 'caerPoint4DIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerPoint4DIteratorElement' variable + * of type caerPoint4DEvent. + * + * POINT4D_PACKET: a valid Point4DEventPacket pointer. Cannot be NULL. + */ +#define CAER_POINT4D_ITERATOR_ALL_START(POINT4D_PACKET) \ + for (int32_t caerPoint4DIteratorCounter = 0; \ + caerPoint4DIteratorCounter < caerEventPacketHeaderGetEventNumber(&(POINT4D_PACKET)->packetHeader); \ + caerPoint4DIteratorCounter++) { \ + caerPoint4DEvent caerPoint4DIteratorElement = caerPoint4DEventPacketGetEvent(POINT4D_PACKET, caerPoint4DIteratorCounter); + +/** + * Iterator close statement. + */ +#define CAER_POINT4D_ITERATOR_ALL_END } + +/** + * Iterator over only the valid Point4D events in a packet. + * Returns the current index in the 'caerPoint4DIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerPoint4DIteratorElement' variable + * of type caerPoint4DEvent. + * + * POINT4D_PACKET: a valid Point4DEventPacket pointer. Cannot be NULL. + */ +#define CAER_POINT4D_ITERATOR_VALID_START(POINT4D_PACKET) \ + for (int32_t caerPoint4DIteratorCounter = 0; \ + caerPoint4DIteratorCounter < caerEventPacketHeaderGetEventNumber(&(POINT4D_PACKET)->packetHeader); \ + caerPoint4DIteratorCounter++) { \ + caerPoint4DEvent caerPoint4DIteratorElement = caerPoint4DEventPacketGetEvent(POINT4D_PACKET, caerPoint4DIteratorCounter); \ + if (!caerPoint4DEventIsValid(caerPoint4DIteratorElement)) { continue; } // Skip invalid Point4D events. + +/** + * Iterator close statement. + */ +#define CAER_POINT4D_ITERATOR_VALID_END } + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_EVENTS_POINT4D_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/polarity.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/polarity.h new file mode 100755 index 0000000000000000000000000000000000000000..392554af7499c1d60b82185cf9640884cce9caa2 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/polarity.h @@ -0,0 +1,367 @@ +/** + * @file polarity.h + * + * Polarity Events format definition and handling functions. + * This event contains change information, with an X/Y address + * and an ON/OFF polarity. + * The (0, 0) address is in the upper left corner of the screen, + * like in OpenCV/computer graphics. + */ + +#ifndef LIBCAER_EVENTS_POLARITY_H_ +#define LIBCAER_EVENTS_POLARITY_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "common.h" + +/** + * Shift and mask values for the polarity, X and Y addresses + * of a polarity event. + * Addresses up to 14 bit are supported. Polarity is ON(=1) or OFF(=0). + * Bit 0 is the valid mark, see 'common.h' for more details. + */ +//@{ +#define POLARITY_SHIFT 1 +#define POLARITY_MASK 0x00000001 +#define Y_ADDR_SHIFT 2 +#define Y_ADDR_MASK 0x00007FFF +#define X_ADDR_SHIFT 17 +#define X_ADDR_MASK 0x00007FFF +//@} + +/** + * Polarity event data structure definition. + * This contains the actual X/Y addresses, the polarity, + * as well as the 32 bit event timestamp. + * The (0, 0) address is in the upper left corner of the screen, + * like in OpenCV/computer graphics. + * Signed integers are used for fields that are to be interpreted + * directly, for compatibility with languages that do not have + * unsigned integer types, such as Java. + */ +struct caer_polarity_event { + /// Event data. First because of valid mark. + uint32_t data; + /// Event timestamp. + int32_t timestamp; +}__attribute__((__packed__)); + +/** + * Type for pointer to polarity event data structure. + */ +typedef struct caer_polarity_event *caerPolarityEvent; + +/** + * Polarity event packet data structure definition. + * EventPackets are always made up of the common packet header, + * followed by 'eventCapacity' events. Everything has to + * be in one contiguous memory block. + */ +struct caer_polarity_event_packet { + /// The common event packet header. + struct caer_event_packet_header packetHeader; + /// The events array. + struct caer_polarity_event events[]; +}__attribute__((__packed__)); + +/** + * Type for pointer to polarity event packet data structure. + */ +typedef struct caer_polarity_event_packet *caerPolarityEventPacket; + +/** + * Allocate a new polarity events packet. + * Use free() to reclaim this memory. + * + * @param eventCapacity the maximum number of events this packet will hold. + * @param eventSource the unique ID representing the source/generator of this packet. + * @param tsOverflow the current timestamp overflow counter value for this packet. + * + * @return a valid PolarityEventPacket handle or NULL on error. + */ +caerPolarityEventPacket caerPolarityEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow); + +/** + * Get the polarity event at the given index from the event packet. + * + * @param packet a valid PolarityEventPacket pointer. Cannot be NULL. + * @param n the index of the returned event. Must be within [0,eventCapacity[ bounds. + * + * @return the requested polarity event. NULL on error. + */ +static inline caerPolarityEvent caerPolarityEventPacketGetEvent(caerPolarityEventPacket packet, int32_t n) { + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketHeaderGetEventCapacity(&packet->packetHeader)) { + caerLog(CAER_LOG_CRITICAL, "Polarity Event", + "Called caerPolarityEventPacketGetEvent() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ".", + n, caerEventPacketHeaderGetEventCapacity(&packet->packetHeader) - 1); + return (NULL); + } + + // Return a pointer to the specified event. + return (packet->events + n); +} + +/** + * Get the 32bit event timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid PolarityEvent pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond timestamp. + */ +static inline int32_t caerPolarityEventGetTimestamp(caerPolarityEvent event) { + return (le32toh(event->timestamp)); +} + +/** + * Get the 64bit event timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid PolarityEvent pointer. Cannot be NULL. + * @param packet the PolarityEventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond timestamp. + */ +static inline int64_t caerPolarityEventGetTimestamp64(caerPolarityEvent event, caerPolarityEventPacket packet) { + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerPolarityEventGetTimestamp(event)))); +} + +/** + * Set the 32bit event timestamp, the value has to be in microseconds. + * + * @param event a valid PolarityEvent pointer. Cannot be NULL. + * @param timestamp a positive 32bit microsecond timestamp. + */ +static inline void caerPolarityEventSetTimestamp(caerPolarityEvent event, int32_t timestamp) { + if (timestamp < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "Polarity Event", "Called caerPolarityEventSetTimestamp() with negative value!"); + return; + } + + event->timestamp = htole32(timestamp); +} + +/** + * Check if this polarity event is valid. + * + * @param event a valid PolarityEvent pointer. Cannot be NULL. + * + * @return true if valid, false if not. + */ +static inline bool caerPolarityEventIsValid(caerPolarityEvent event) { + return (GET_NUMBITS32(event->data, VALID_MARK_SHIFT, VALID_MARK_MASK)); +} + +/** + * Validate the current event by setting its valid bit to true + * and increasing the event packet's event count and valid + * event count. Only works on events that are invalid. + * DO NOT CALL THIS AFTER HAVING PREVIOUSLY ALREADY + * INVALIDATED THIS EVENT, the total count will be incorrect. + * + * @param event a valid PolarityEvent pointer. Cannot be NULL. + * @param packet the PolarityEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerPolarityEventValidate(caerPolarityEvent event, caerPolarityEventPacket packet) { + if (!caerPolarityEventIsValid(event)) { + SET_NUMBITS32(event->data, VALID_MARK_SHIFT, VALID_MARK_MASK, 1); + + // Also increase number of events and valid events. + // Only call this on (still) invalid events! + caerEventPacketHeaderSetEventNumber(&packet->packetHeader, + caerEventPacketHeaderGetEventNumber(&packet->packetHeader) + 1); + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) + 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Polarity Event", "Called caerPolarityEventValidate() on already valid event."); + } +} + +/** + * Invalidate the current event by setting its valid bit + * to false and decreasing the number of valid events held + * in the packet. Only works with events that are already + * valid! + * + * @param event a valid PolarityEvent pointer. Cannot be NULL. + * @param packet the PolarityEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerPolarityEventInvalidate(caerPolarityEvent event, caerPolarityEventPacket packet) { + if (caerPolarityEventIsValid(event)) { + CLEAR_NUMBITS32(event->data, VALID_MARK_SHIFT, VALID_MARK_MASK); + + // Also decrease number of valid events. Number of total events doesn't change. + // Only call this on valid events! + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) - 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Polarity Event", "Called caerPolarityEventInvalidate() on already invalid event."); + } +} + +/** + * Get the change event polarity. 1 is ON, 0 is OFF. + * + * @param event a valid PolarityEvent pointer. Cannot be NULL. + * + * @return event polarity value. + */ +static inline bool caerPolarityEventGetPolarity(caerPolarityEvent event) { + return (GET_NUMBITS32(event->data, POLARITY_SHIFT, POLARITY_MASK)); +} + +/** + * Set the change event polarity. 1 is ON, 0 is OFF. + * + * @param event a valid PolarityEvent pointer. Cannot be NULL. + * @param polarity event polarity value. + */ +static inline void caerPolarityEventSetPolarity(caerPolarityEvent event, bool polarity) { + CLEAR_NUMBITS32(event->data, POLARITY_SHIFT, POLARITY_MASK); + SET_NUMBITS32(event->data, POLARITY_SHIFT, POLARITY_MASK, polarity); +} + +/** + * Get the Y (row) address for a change event, in pixels. + * The (0, 0) address is in the upper left corner, like in OpenCV/computer graphics. + * + * @param event a valid PolarityEvent pointer. Cannot be NULL. + * + * @return the event Y address. + */ +static inline uint16_t caerPolarityEventGetY(caerPolarityEvent event) { + return U16T(GET_NUMBITS32(event->data, Y_ADDR_SHIFT, Y_ADDR_MASK)); +} + +/** + * Set the Y (row) address for a change event, in pixels. + * The (0, 0) address is in the upper left corner, like in OpenCV/computer graphics. + * + * @param event a valid PolarityEvent pointer. Cannot be NULL. + * @param yAddress the event Y address. + */ +static inline void caerPolarityEventSetY(caerPolarityEvent event, uint16_t yAddress) { + CLEAR_NUMBITS32(event->data, Y_ADDR_SHIFT, Y_ADDR_MASK); + SET_NUMBITS32(event->data, Y_ADDR_SHIFT, Y_ADDR_MASK, yAddress); +} + +/** + * Get the X (column) address for a change event, in pixels. + * The (0, 0) address is in the upper left corner, like in OpenCV/computer graphics. + * + * @param event a valid PolarityEvent pointer. Cannot be NULL. + * + * @return the event X address. + */ +static inline uint16_t caerPolarityEventGetX(caerPolarityEvent event) { + return U16T(GET_NUMBITS32(event->data, X_ADDR_SHIFT, X_ADDR_MASK)); +} + +/** + * Set the X (column) address for a change event, in pixels. + * The (0, 0) address is in the upper left corner, like in OpenCV/computer graphics. + * + * @param event a valid PolarityEvent pointer. Cannot be NULL. + * @param xAddress the event X address. + */ +static inline void caerPolarityEventSetX(caerPolarityEvent event, uint16_t xAddress) { + CLEAR_NUMBITS32(event->data, X_ADDR_SHIFT, X_ADDR_MASK); + SET_NUMBITS32(event->data, X_ADDR_SHIFT, X_ADDR_MASK, xAddress); +} + +/** + * Iterator over all polarity events in a packet. + * Returns the current index in the 'caerPolarityIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerPolarityIteratorElement' variable + * of type caerPolarityEvent. + * + * POLARITY_PACKET: a valid PolarityEventPacket pointer. Cannot be NULL. + */ +#define CAER_POLARITY_ITERATOR_ALL_START(POLARITY_PACKET) \ + for (int32_t caerPolarityIteratorCounter = 0; \ + caerPolarityIteratorCounter < caerEventPacketHeaderGetEventNumber(&(POLARITY_PACKET)->packetHeader); \ + caerPolarityIteratorCounter++) { \ + caerPolarityEvent caerPolarityIteratorElement = caerPolarityEventPacketGetEvent(POLARITY_PACKET, caerPolarityIteratorCounter); + +/** + * Iterator close statement. + */ +#define CAER_POLARITY_ITERATOR_ALL_END } + +/** + * Iterator over only the valid polarity events in a packet. + * Returns the current index in the 'caerPolarityIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerPolarityIteratorElement' variable + * of type caerPolarityEvent. + * + * POLARITY_PACKET: a valid PolarityEventPacket pointer. Cannot be NULL. + */ +#define CAER_POLARITY_ITERATOR_VALID_START(POLARITY_PACKET) \ + for (int32_t caerPolarityIteratorCounter = 0; \ + caerPolarityIteratorCounter < caerEventPacketHeaderGetEventNumber(&(POLARITY_PACKET)->packetHeader); \ + caerPolarityIteratorCounter++) { \ + caerPolarityEvent caerPolarityIteratorElement = caerPolarityEventPacketGetEvent(POLARITY_PACKET, caerPolarityIteratorCounter); \ + if (!caerPolarityEventIsValid(caerPolarityIteratorElement)) { continue; } // Skip invalid polarity events. + +/** + * Iterator close statement. + */ +#define CAER_POLARITY_ITERATOR_VALID_END } + +/** + * Reverse iterator over all polarity events in a packet. + * Returns the current index in the 'caerPolarityIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerPolarityIteratorElement' variable + * of type caerPolarityEvent. + * + * POLARITY_PACKET: a valid PolarityEventPacket pointer. Cannot be NULL. + */ +#define CAER_POLARITY_REVERSE_ITERATOR_ALL_START(POLARITY_PACKET) \ + for (int32_t caerPolarityIteratorCounter = caerEventPacketHeaderGetEventNumber(&(POLARITY_PACKET)->packetHeader) - 1; \ + caerPolarityIteratorCounter >= 0; \ + caerPolarityIteratorCounter--) { \ + caerPolarityEvent caerPolarityIteratorElement = caerPolarityEventPacketGetEvent(POLARITY_PACKET, caerPolarityIteratorCounter); + +/** + * Reverse iterator close statement. + */ +#define CAER_POLARITY_REVERSE_ITERATOR_ALL_END } + +/** + * Reverse iterator over only the valid polarity events in a packet. + * Returns the current index in the 'caerPolarityIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerPolarityIteratorElement' variable + * of type caerPolarityEvent. + * + * POLARITY_PACKET: a valid PolarityEventPacket pointer. Cannot be NULL. + */ +#define CAER_POLARITY_REVERSE_ITERATOR_VALID_START(POLARITY_PACKET) \ + for (int32_t caerPolarityIteratorCounter = caerEventPacketHeaderGetEventNumber(&(POLARITY_PACKET)->packetHeader) - 1; \ + caerPolarityIteratorCounter >= 0; \ + caerPolarityIteratorCounter--) { \ + caerPolarityEvent caerPolarityIteratorElement = caerPolarityEventPacketGetEvent(POLARITY_PACKET, caerPolarityIteratorCounter); \ + if (!caerPolarityEventIsValid(caerPolarityIteratorElement)) { continue; } // Skip invalid polarity events. + +/** + * Reverse iterator close statement. + */ +#define CAER_POLARITY_REVERSE_ITERATOR_VALID_END } + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_EVENTS_POLARITY_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/sample.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/sample.h new file mode 100755 index 0000000000000000000000000000000000000000..c0b9837ef0683ec96f3d9714c7364ff3532e4fa1 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/sample.h @@ -0,0 +1,304 @@ +/** + * @file sample.h + * + * Sample (ADC) Events format definition and handling functions. + * Represents different types of ADC readings, up to 24 bits + * of resolution. + */ + +#ifndef LIBCAER_EVENTS_SAMPLE_H_ +#define LIBCAER_EVENTS_SAMPLE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "common.h" + +/** + * Shift and mask values for the sample type and the actual + * sample value of an ADC sample. + * Up to 128 sample types are supported, with 24 bits of + * data per sample. Higher values mean a higher voltage, + * 0 is ground. + * Bit 0 is the valid mark, see 'common.h' for more details. + */ +//@{ +#define SAMPLE_TYPE_SHIFT 1 +#define SAMPLE_TYPE_MASK 0x0000007F +#define SAMPLE_SHIFT 8 +#define SAMPLE_MASK 0x00FFFFFF +//@} + +/** + * ADC sample event data structure definition. + * Contains a type indication to separate different ADC readouts, + * as well as a value for that readout, up to 24 bits resolution. + * Signed integers are used for fields that are to be interpreted + * directly, for compatibility with languages that do not have + * unsigned integer types, such as Java. + */ +struct caer_sample_event { + /// Event data. First because of valid mark. + uint32_t data; + /// Event timestamp. + int32_t timestamp; +}__attribute__((__packed__)); + +/** + * Type for pointer to ADC sample event data structure. + */ +typedef struct caer_sample_event *caerSampleEvent; + +/** + * ADC sample event packet data structure definition. + * EventPackets are always made up of the common packet header, + * followed by 'eventCapacity' events. Everything has to + * be in one contiguous memory block. + */ +struct caer_sample_event_packet { + /// The common event packet header. + struct caer_event_packet_header packetHeader; + /// The events array. + struct caer_sample_event events[]; +}__attribute__((__packed__)); + +/** + * Type for pointer to ADC sample event packet data structure. + */ +typedef struct caer_sample_event_packet *caerSampleEventPacket; + +/** + * Allocate a new ADC sample events packet. + * Use free() to reclaim this memory. + * + * @param eventCapacity the maximum number of events this packet will hold. + * @param eventSource the unique ID representing the source/generator of this packet. + * @param tsOverflow the current timestamp overflow counter value for this packet. + * + * @return a valid SampleEventPacket handle or NULL on error. + */ +caerSampleEventPacket caerSampleEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow); + +/** + * Get the ADC sample event at the given index from the event packet. + * + * @param packet a valid SampleEventPacket pointer. Cannot be NULL. + * @param n the index of the returned event. Must be within [0,eventCapacity[ bounds. + * + * @return the requested ADC sample event. NULL on error. + */ +static inline caerSampleEvent caerSampleEventPacketGetEvent(caerSampleEventPacket packet, int32_t n) { + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketHeaderGetEventCapacity(&packet->packetHeader)) { + caerLog(CAER_LOG_CRITICAL, "Sample Event", + "Called caerSampleEventPacketGetEvent() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ".", + n, caerEventPacketHeaderGetEventCapacity(&packet->packetHeader) - 1); + return (NULL); + } + + // Return a pointer to the specified event. + return (packet->events + n); +} + +/** + * Get the 32bit event timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid SampleEvent pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond timestamp. + */ +static inline int32_t caerSampleEventGetTimestamp(caerSampleEvent event) { + return (le32toh(event->timestamp)); +} + +/** + * Get the 64bit event timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid SampleEvent pointer. Cannot be NULL. + * @param packet the SampleEventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond timestamp. + */ +static inline int64_t caerSampleEventGetTimestamp64(caerSampleEvent event, caerSampleEventPacket packet) { + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerSampleEventGetTimestamp(event)))); +} + +/** + * Set the 32bit event timestamp, the value has to be in microseconds. + * + * @param event a valid SampleEvent pointer. Cannot be NULL. + * @param timestamp a positive 32bit microsecond timestamp. + */ +static inline void caerSampleEventSetTimestamp(caerSampleEvent event, int32_t timestamp) { + if (timestamp < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "Sample Event", "Called caerSampleEventSetTimestamp() with negative value!"); + return; + } + + event->timestamp = htole32(timestamp); +} + +/** + * Check if this ADC sample event is valid. + * + * @param event a valid SampleEvent pointer. Cannot be NULL. + * + * @return true if valid, false if not. + */ +static inline bool caerSampleEventIsValid(caerSampleEvent event) { + return (GET_NUMBITS32(event->data, VALID_MARK_SHIFT, VALID_MARK_MASK)); +} + +/** + * Validate the current event by setting its valid bit to true + * and increasing the event packet's event count and valid + * event count. Only works on events that are invalid. + * DO NOT CALL THIS AFTER HAVING PREVIOUSLY ALREADY + * INVALIDATED THIS EVENT, the total count will be incorrect. + * + * @param event a valid SampleEvent pointer. Cannot be NULL. + * @param packet the SampleEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerSampleEventValidate(caerSampleEvent event, caerSampleEventPacket packet) { + if (!caerSampleEventIsValid(event)) { + SET_NUMBITS32(event->data, VALID_MARK_SHIFT, VALID_MARK_MASK, 1); + + // Also increase number of events and valid events. + // Only call this on (still) invalid events! + caerEventPacketHeaderSetEventNumber(&packet->packetHeader, + caerEventPacketHeaderGetEventNumber(&packet->packetHeader) + 1); + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) + 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Sample Event", "Called caerSampleEventValidate() on already valid event."); + } +} + +/** + * Invalidate the current event by setting its valid bit + * to false and decreasing the number of valid events held + * in the packet. Only works with events that are already + * valid! + * + * @param event a valid SampleEvent pointer. Cannot be NULL. + * @param packet the SampleEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerSampleEventInvalidate(caerSampleEvent event, caerSampleEventPacket packet) { + if (caerSampleEventIsValid(event)) { + CLEAR_NUMBITS32(event->data, VALID_MARK_SHIFT, VALID_MARK_MASK); + + // Also decrease number of valid events. Number of total events doesn't change. + // Only call this on valid events! + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) - 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Sample Event", "Called caerSampleEventInvalidate() on already invalid event."); + } +} + +/** + * Get the ADC sample event type. This is useful to distinguish + * between different measurements, for example from two separate + * microphones on a device. + * + * @param event a valid SampleEvent pointer. Cannot be NULL. + * + * @return the ADC sample type. + */ +static inline uint8_t caerSampleEventGetType(caerSampleEvent event) { + return U8T(GET_NUMBITS32(event->data, SAMPLE_TYPE_SHIFT, SAMPLE_TYPE_MASK)); +} + +/** + * Set the ADC sample event type. This is useful to distinguish + * between different measurements, for example from two separate + * microphones on a device. + * + * @param event a valid SampleEvent pointer. Cannot be NULL. + * @param type the ADC sample type. + */ +static inline void caerSampleEventSetType(caerSampleEvent event, uint8_t type) { + CLEAR_NUMBITS32(event->data, SAMPLE_TYPE_SHIFT, SAMPLE_TYPE_MASK); + SET_NUMBITS32(event->data, SAMPLE_TYPE_SHIFT, SAMPLE_TYPE_MASK, type); +} + +/** + * Get the ADC sample value. Up to 24 bits of resolution are possible. + * Higher values mean a higher voltage, 0 is ground. + * + * @param event a valid SampleEvent pointer. Cannot be NULL. + * + * @return the ADC sample value. + */ +static inline uint32_t caerSampleEventGetSample(caerSampleEvent event) { + return U32T(GET_NUMBITS32(event->data, SAMPLE_SHIFT, SAMPLE_MASK)); +} + +/** + * Set the ADC sample value. Up to 24 bits of resolution are possible. + * Higher values mean a higher voltage, 0 is ground. + * + * @param event a valid SampleEvent pointer. Cannot be NULL. + * @param sample the ADC sample value. + */ +static inline void caerSampleEventSetSample(caerSampleEvent event, uint32_t sample) { + CLEAR_NUMBITS32(event->data, SAMPLE_SHIFT, SAMPLE_MASK); + SET_NUMBITS32(event->data, SAMPLE_SHIFT, SAMPLE_MASK, sample); +} + +/** + * Iterator over all sample events in a packet. + * Returns the current index in the 'caerSampleIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerSampleIteratorElement' variable + * of type caerSampleEvent. + * + * SAMPLE_PACKET: a valid SampleEventPacket pointer. Cannot be NULL. + */ +#define CAER_SAMPLE_ITERATOR_ALL_START(SAMPLE_PACKET) \ + for (int32_t caerSampleIteratorCounter = 0; \ + caerSampleIteratorCounter < caerEventPacketHeaderGetEventNumber(&(SAMPLE_PACKET)->packetHeader); \ + caerSampleIteratorCounter++) { \ + caerSampleEvent caerSampleIteratorElement = caerSampleEventPacketGetEvent(SAMPLE_PACKET, caerSampleIteratorCounter); + +/** + * Iterator close statement. + */ +#define CAER_SAMPLE_ITERATOR_ALL_END } + +/** + * Iterator over only the valid sample events in a packet. + * Returns the current index in the 'caerSampleIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerSampleIteratorElement' variable + * of type caerSampleEvent. + * + * SAMPLE_PACKET: a valid SampleEventPacket pointer. Cannot be NULL. + */ +#define CAER_SAMPLE_ITERATOR_VALID_START(SAMPLE_PACKET) \ + for (int32_t caerSampleIteratorCounter = 0; \ + caerSampleIteratorCounter < caerEventPacketHeaderGetEventNumber(&(SAMPLE_PACKET)->packetHeader); \ + caerSampleIteratorCounter++) { \ + caerSampleEvent caerSampleIteratorElement = caerSampleEventPacketGetEvent(SAMPLE_PACKET, caerSampleIteratorCounter); \ + if (!caerSampleEventIsValid(caerSampleIteratorElement)) { continue; } // Skip invalid sample events. + +/** + * Iterator close statement. + */ +#define CAER_SAMPLE_ITERATOR_VALID_END } + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_EVENTS_SAMPLE_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/special.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/special.h new file mode 100755 index 0000000000000000000000000000000000000000..eca28152d668a147188c735ffa5b100977aa4c98 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/events/special.h @@ -0,0 +1,368 @@ +/** + * @file special.h + * + * Special Events format definition and handling functions. + * This event type encodes special occurrences, such as + * timestamp related notifications or external input events. + */ + +#ifndef LIBCAER_EVENTS_SPECIAL_H_ +#define LIBCAER_EVENTS_SPECIAL_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "common.h" + +/** + * Shift and mask values for the type and data portions + * of a special event. + * Up to 128 types, with 24 bits of data each, are possible. + * Bit 0 is the valid mark, see 'common.h' for more details. + */ +//@{ +#define TYPE_SHIFT 1 +#define TYPE_MASK 0x0000007F +#define DATA_SHIFT 8 +#define DATA_MASK 0x00FFFFFF +//@} + +/** + * List of all special event type identifiers. + * Used to interpret the special event type field. + */ +enum caer_special_event_types { + TIMESTAMP_WRAP = 0, //!< A 32 bit timestamp wrap occurred. + TIMESTAMP_RESET = 1, //!< A timestamp reset occurred. + EXTERNAL_INPUT_RISING_EDGE = 2, //!< A rising edge was detected (External Input module on device). + EXTERNAL_INPUT_FALLING_EDGE = 3, //!< A falling edge was detected (External Input module on device). + EXTERNAL_INPUT_PULSE = 4, //!< A pulse was detected (External Input module on device). + DVS_ROW_ONLY = 5, //!< A DVS row-only event was detected (a row address without any following column addresses). + EXTERNAL_INPUT1_RISING_EDGE = 6, //!< A rising edge was detected (External Input 1 module on device). + EXTERNAL_INPUT1_FALLING_EDGE = 7, //!< A falling edge was detected (External Input 1 module on device). + EXTERNAL_INPUT1_PULSE = 8, //!< A pulse was detected (External Input 1 module on device). + EXTERNAL_INPUT2_RISING_EDGE = 9, //!< A rising edge was detected (External Input 2 module on device). + EXTERNAL_INPUT2_FALLING_EDGE = 10, //!< A falling edge was detected (External Input 2 module on device). + EXTERNAL_INPUT2_PULSE = 11, //!< A pulse was detected (External Input 2 module on device). + EXTERNAL_GENERATOR_RISING_EDGE = 12, //!< A rising edge was generated (External Input Generator module on device). + EXTERNAL_GENERATOR_FALLING_EDGE = 13, //!< A falling edge was generated (External Input Generator module on device). + APS_FRAME_START = 14, //!< An APS frame capture has started (Frame Event will follow). + APS_FRAME_END = 15, //!< An APS frame capture has completed (Frame Event is alongside). + APS_EXPOSURE_START = 16, //!< An APS frame exposure has started (Frame Event will follow). + APS_EXPOSURE_END = 17, //!< An APS frame exposure has completed (Frame Event will follow). +}; + +/** + * Special event data structure definition. + * This contains the actual data, as well as the 32 bit event timestamp. + * Signed integers are used for fields that are to be interpreted + * directly, for compatibility with languages that do not have + * unsigned integer types, such as Java. + */ +struct caer_special_event { + /// Event data. First because of valid mark. + uint32_t data; + /// Event timestamp. + int32_t timestamp; +}__attribute__((__packed__)); + +/** + * Type for pointer to special event data structure. + */ +typedef struct caer_special_event *caerSpecialEvent; + +/** + * Special event packet data structure definition. + * EventPackets are always made up of the common packet header, + * followed by 'eventCapacity' events. Everything has to + * be in one contiguous memory block. + */ +struct caer_special_event_packet { + /// The common event packet header. + struct caer_event_packet_header packetHeader; + /// The events array. + struct caer_special_event events[]; +}__attribute__((__packed__)); + +/** + * Type for pointer to special event packet data structure. + */ +typedef struct caer_special_event_packet *caerSpecialEventPacket; + +/** + * Allocate a new special events packet. + * Use free() to reclaim this memory. + * + * @param eventCapacity the maximum number of events this packet will hold. + * @param eventSource the unique ID representing the source/generator of this packet. + * @param tsOverflow the current timestamp overflow counter value for this packet. + * + * @return a valid SpecialEventPacket handle or NULL on error. + */ +caerSpecialEventPacket caerSpecialEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow); + +/** + * Get the special event at the given index from the event packet. + * + * @param packet a valid SpecialEventPacket pointer. Cannot be NULL. + * @param n the index of the returned event. Must be within [0,eventCapacity[ bounds. + * + * @return the requested special event. NULL on error. + */ +static inline caerSpecialEvent caerSpecialEventPacketGetEvent(caerSpecialEventPacket packet, int32_t n) { + // Check that we're not out of bounds. + if (n < 0 || n >= caerEventPacketHeaderGetEventCapacity(&packet->packetHeader)) { + caerLog(CAER_LOG_CRITICAL, "Special Event", + "Called caerSpecialEventPacketGetEvent() with invalid event offset %" PRIi32 ", while maximum allowed value is %" PRIi32 ".", + n, caerEventPacketHeaderGetEventCapacity(&packet->packetHeader) - 1); + return (NULL); + } + + // Return a pointer to the specified event. + return (packet->events + n); +} + +/** + * Get the 32bit event timestamp, in microseconds. + * Be aware that this wraps around! You can either ignore this fact, + * or handle the special 'TIMESTAMP_WRAP' event that is generated when + * this happens, or use the 64bit timestamp which never wraps around. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid SpecialEvent pointer. Cannot be NULL. + * + * @return this event's 32bit microsecond timestamp. + */ +static inline int32_t caerSpecialEventGetTimestamp(caerSpecialEvent event) { + return (le32toh(event->timestamp)); +} + +/** + * Get the 64bit event timestamp, in microseconds. + * See 'caerEventPacketHeaderGetEventTSOverflow()' documentation + * for more details on the 64bit timestamp. + * + * @param event a valid SpecialEvent pointer. Cannot be NULL. + * @param packet the SpecialEventPacket pointer for the packet containing this event. Cannot be NULL. + * + * @return this event's 64bit microsecond timestamp. + */ +static inline int64_t caerSpecialEventGetTimestamp64(caerSpecialEvent event, caerSpecialEventPacket packet) { + return (I64T( + (U64T(caerEventPacketHeaderGetEventTSOverflow(&packet->packetHeader)) << TS_OVERFLOW_SHIFT) | U64T(caerSpecialEventGetTimestamp(event)))); +} + +/** + * Set the 32bit event timestamp, the value has to be in microseconds. + * + * @param event a valid SpecialEvent pointer. Cannot be NULL. + * @param timestamp a positive 32bit microsecond timestamp. + */ +static inline void caerSpecialEventSetTimestamp(caerSpecialEvent event, int32_t timestamp) { + if (timestamp < 0) { + // Negative means using the 31st bit! + caerLog(CAER_LOG_CRITICAL, "Special Event", "Called caerSpecialEventSetTimestamp() with negative value!"); + return; + } + + event->timestamp = htole32(timestamp); +} + +/** + * Check if this special event is valid. + * + * @param event a valid SpecialEvent pointer. Cannot be NULL. + * + * @return true if valid, false if not. + */ +static inline bool caerSpecialEventIsValid(caerSpecialEvent event) { + return (GET_NUMBITS32(event->data, VALID_MARK_SHIFT, VALID_MARK_MASK)); +} + +/** + * Validate the current event by setting its valid bit to true + * and increasing the event packet's event count and valid + * event count. Only works on events that are invalid. + * DO NOT CALL THIS AFTER HAVING PREVIOUSLY ALREADY + * INVALIDATED THIS EVENT, the total count will be incorrect. + * + * @param event a valid SpecialEvent pointer. Cannot be NULL. + * @param packet the SpecialEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerSpecialEventValidate(caerSpecialEvent event, caerSpecialEventPacket packet) { + if (!caerSpecialEventIsValid(event)) { + SET_NUMBITS32(event->data, VALID_MARK_SHIFT, VALID_MARK_MASK, 1); + + // Also increase number of events and valid events. + // Only call this on (still) invalid events! + caerEventPacketHeaderSetEventNumber(&packet->packetHeader, + caerEventPacketHeaderGetEventNumber(&packet->packetHeader) + 1); + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) + 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Special Event", "Called caerSpecialEventValidate() on already valid event."); + } +} + +/** + * Invalidate the current event by setting its valid bit + * to false and decreasing the number of valid events held + * in the packet. Only works with events that are already + * valid! + * + * @param event a valid SpecialEvent pointer. Cannot be NULL. + * @param packet the SpecialEventPacket pointer for the packet containing this event. Cannot be NULL. + */ +static inline void caerSpecialEventInvalidate(caerSpecialEvent event, caerSpecialEventPacket packet) { + if (caerSpecialEventIsValid(event)) { + CLEAR_NUMBITS32(event->data, VALID_MARK_SHIFT, VALID_MARK_MASK); + + // Also decrease number of valid events. Number of total events doesn't change. + // Only call this on valid events! + caerEventPacketHeaderSetEventValid(&packet->packetHeader, + caerEventPacketHeaderGetEventValid(&packet->packetHeader) - 1); + } + else { + caerLog(CAER_LOG_CRITICAL, "Special Event", "Called caerSpecialEventInvalidate() on already invalid event."); + } +} + +/** + * Get the numerical special event type. + * + * @param event a valid SpecialEvent pointer. Cannot be NULL. + * + * @return the special event type (see 'enum caer_special_event_types'). + */ +static inline uint8_t caerSpecialEventGetType(caerSpecialEvent event) { + return U8T(GET_NUMBITS32(event->data, TYPE_SHIFT, TYPE_MASK)); +} + +/** + * Set the numerical special event type. + * + * @param event a valid SpecialEvent pointer. Cannot be NULL. + * @param type the special event type (see 'enum caer_special_event_types'). + */ +static inline void caerSpecialEventSetType(caerSpecialEvent event, uint8_t type) { + CLEAR_NUMBITS32(event->data, TYPE_SHIFT, TYPE_MASK); + SET_NUMBITS32(event->data, TYPE_SHIFT, TYPE_MASK, type); +} + +/** + * Get the special event data. Its meaning depends on the type. + * Current types that make use of it are (see 'enum caer_special_event_types'): + * - DVS_ROW_ONLY: encodes the address of the row from the row-only event. + * + * @param event a valid SpecialEvent pointer. Cannot be NULL. + * + * @return the special event data. + */ +static inline uint32_t caerSpecialEventGetData(caerSpecialEvent event) { + return U32T(GET_NUMBITS32(event->data, DATA_SHIFT, DATA_MASK)); +} + +/** + * Set the special event data. Its meaning depends on the type. + * Current types that make use of it are (see 'enum caer_special_event_types'): + * - DVS_ROW_ONLY: encodes the address of the row from the row-only event. + * + * @param event a valid SpecialEvent pointer. Cannot be NULL. + * @param data the special event data. + */ +static inline void caerSpecialEventSetData(caerSpecialEvent event, uint32_t data) { + CLEAR_NUMBITS32(event->data, DATA_SHIFT, DATA_MASK); + SET_NUMBITS32(event->data, DATA_SHIFT, DATA_MASK, data); +} + +/** + * Iterator over all special events in a packet. + * Returns the current index in the 'caerSpecialIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerSpecialIteratorElement' variable + * of type caerSpecialEvent. + * + * SPECIAL_PACKET: a valid SpecialEventPacket pointer. Cannot be NULL. + */ +#define CAER_SPECIAL_ITERATOR_ALL_START(SPECIAL_PACKET) \ + for (int32_t caerSpecialIteratorCounter = 0; \ + caerSpecialIteratorCounter < caerEventPacketHeaderGetEventNumber(&(SPECIAL_PACKET)->packetHeader); \ + caerSpecialIteratorCounter++) { \ + caerSpecialEvent caerSpecialIteratorElement = caerSpecialEventPacketGetEvent(SPECIAL_PACKET, caerSpecialIteratorCounter); + +/** + * Iterator close statement. + */ +#define CAER_SPECIAL_ITERATOR_ALL_END } + +/** + * Iterator over only the valid special events in a packet. + * Returns the current index in the 'caerSpecialIteratorCounter' variable of type + * 'int32_t' and the current event in the 'caerSpecialIteratorElement' variable + * of type caerSpecialEvent. + * + * SPECIAL_PACKET: a valid SpecialEventPacket pointer. Cannot be NULL. + */ +#define CAER_SPECIAL_ITERATOR_VALID_START(SPECIAL_PACKET) \ + for (int32_t caerSpecialIteratorCounter = 0; \ + caerSpecialIteratorCounter < caerEventPacketHeaderGetEventNumber(&(SPECIAL_PACKET)->packetHeader); \ + caerSpecialIteratorCounter++) { \ + caerSpecialEvent caerSpecialIteratorElement = caerSpecialEventPacketGetEvent(SPECIAL_PACKET, caerSpecialIteratorCounter); \ + if (!caerSpecialEventIsValid(caerSpecialIteratorElement)) { continue; } // Skip invalid special events. + +/** + * Iterator close statement. + */ +#define CAER_SPECIAL_ITERATOR_VALID_END } + +/** + * Get the first special event with the given event type in this + * event packet. This returns the first found event with that type ID, + * or NULL if we get to the end without finding any such event. + * + * @param packet a valid SpecialEventPacket pointer. Cannot be NULL. + * @param type the special event type to search for. + * + * @return the requested special event or NULL on error/not found. + */ +static inline caerSpecialEvent caerSpecialEventPacketFindEventByType(caerSpecialEventPacket packet, uint8_t type) { + CAER_SPECIAL_ITERATOR_ALL_START(packet) + if (caerSpecialEventGetType(caerSpecialIteratorElement) == type) { + // Found it, return it. + return (caerSpecialIteratorElement); + } + CAER_SPECIAL_ITERATOR_ALL_END + + // Found nothing, return nothing. + return (NULL); +} + +/** + * Get the first valid special event with the given event type in this + * event packet. This returns the first found valid event with that type ID, + * or NULL if we get to the end without finding any such event. + * + * @param packet a valid SpecialEventPacket pointer. Cannot be NULL. + * @param type the special event type to search for. + * + * @return the requested valid special event or NULL on error/not found. + */ +static inline caerSpecialEvent caerSpecialEventPacketFindValidEventByType(caerSpecialEventPacket packet, uint8_t type) { + CAER_SPECIAL_ITERATOR_VALID_START(packet) + if (caerSpecialEventGetType(caerSpecialIteratorElement) == type) { + // Found it, return it. + return (caerSpecialIteratorElement); + } + CAER_SPECIAL_ITERATOR_VALID_END + + // Found nothing, return nothing. + return (NULL); +} + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_EVENTS_SPECIAL_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/frame_utils.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/frame_utils.h new file mode 100755 index 0000000000000000000000000000000000000000..200d5705bbd9c3b039ad9d62f8e7dae9e8da89af --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/frame_utils.h @@ -0,0 +1,26 @@ +/** + * @file frame_utils.h + * + * Basic functions for frame enhancement and demosaicing, that don't + * require any external dependencies, such as OpenCV. + * Use of the OpenCV variants is recommended for quality and performance. + */ + +#ifndef LIBCAER_FRAME_UTILS_H_ +#define LIBCAER_FRAME_UTILS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "events/frame.h" + +caerFrameEventPacket caerFrameUtilsDemosaic(caerFrameEventPacket framePacket); +void caerFrameUtilsContrast(caerFrameEventPacket framePacket); +void caerFrameUtilsWhiteBalance(caerFrameEventPacket framePacket); + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_FRAME_UTILS_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/frame_utils_opencv.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/frame_utils_opencv.h new file mode 100755 index 0000000000000000000000000000000000000000..18fed55eecc34a76f0e277bd26242edc82142224 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/frame_utils_opencv.h @@ -0,0 +1,39 @@ +/** + * @file frame_utils_opencv.h + * + * Functions for frame enhancement and demosaicing, using + * the popular OpenCV image processing library. + */ + +#ifndef LIBCAER_FRAME_UTILS_OPENCV_H_ +#define LIBCAER_FRAME_UTILS_OPENCV_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "events/frame.h" + +enum caer_frame_utils_opencv_demosaic { + DEMOSAIC_NORMAL, DEMOSAIC_EDGE_AWARE, // DEMOSAIC_VARIABLE_NUMBER_OF_GRADIENTS not supported on 16bit images currently. +}; + +enum caer_frame_utils_opencv_contrast { + CONTRAST_NORMALIZATION, CONTRAST_HISTOGRAM_EQUALIZATION, CONTRAST_CLAHE, +}; + +enum caer_frame_utils_opencv_white_balance { + WHITEBALANCE_SIMPLE, WHITEBALANCE_GRAYWORLD, +}; + +caerFrameEventPacket caerFrameUtilsOpenCVDemosaic(caerFrameEventPacket framePacket, + enum caer_frame_utils_opencv_demosaic demosaicType); +void caerFrameUtilsOpenCVContrast(caerFrameEventPacket framePacket, enum caer_frame_utils_opencv_contrast contrastType); +void caerFrameUtilsOpenCVWhiteBalance(caerFrameEventPacket framePacket, + enum caer_frame_utils_opencv_white_balance whiteBalanceType); + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_FRAME_UTILS_OPENCV_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/libcaer_in.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/libcaer_in.h new file mode 100755 index 0000000000000000000000000000000000000000..b2dbce04ae1d6cc2cb8755080d344e06abc69667 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/libcaer_in.h @@ -0,0 +1,251 @@ +/** + * @file libcaer.h + * + * Main libcaer header; provides inclusions for common system functions + * and definitions for useful macros used often in the code. Also includes + * the logging functions and definitions and several useful static inline + * functions for string comparison and byte array manipulation. + * When including libcaer, please make sure to always use the full path, + * ie. #include <libcaer/libcaer.h> and not just #include <libcaer.h>. + */ + +#ifndef LIBCAER_H_ +#define LIBCAER_H_ + +#ifdef __cplusplus + +#include <cstdio> +#include <cstdint> +#include <cinttypes> +#include <cstring> +#include <cerrno> + +extern "C" { +#endif + +// Common includes, useful for everyone. +#include <stddef.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdbool.h> +#include <stdint.h> +#include <inttypes.h> +#include <string.h> +#include <errno.h> + +// Use portable endian conversion functions. +#include "portable_endian.h" + +// Include libcaer's log headers always. +#include "log.h" + +/** + * libcaer version (MAJOR * 10000 + MINOR * 100 + PATCH). + */ +#define LIBCAER_VERSION ((@PROJECT_VERSION_MAJOR@ * 10000) + (@PROJECT_VERSION_MINOR@ * 100) + @PROJECT_VERSION_PATCH@) +/** + * libcaer name string. + */ +#define LIBCAER_NAME_STRING "@PROJECT_NAME@" +/** + * libcaer version string. + */ +#define LIBCAER_VERSION_STRING "@PROJECT_VERSION_NOREV@" + +/** + * Cast argument to uint8_t (8bit unsigned integer). + */ +#define U8T(X) ((uint8_t) (X)) +/** + * Cast argument to uint16_t (16bit unsigned integer). + */ +#define U16T(X) ((uint16_t) (X)) +/** + * Cast argument to uint32_t (32bit unsigned integer). + */ +#define U32T(X) ((uint32_t) (X)) +/** + * Cast argument to uint64_t (64bit unsigned integer). + */ +#define U64T(X) ((uint64_t) (X)) +/** + * Cast argument to int8_t (8bit signed integer). + */ +#define I8T(X) ((int8_t) (X)) +/** + * Cast argument to int16_t (16bit signed integer). + */ +#define I16T(X) ((int16_t) (X)) +/** + * Cast argument to int32_t (32bit signed integer). + */ +#define I32T(X) ((int32_t) (X)) +/** + * Cast argument to int64_t (64bit signed integer). + */ +#define I64T(X) ((int64_t) (X)) +/** + * Mask and keep only the lower X bits of a 32bit (unsigned) integer. + */ +#define MASK_NUMBITS32(X) U32T(U32T(U32T(1) << X) - 1) +/** + * Mask and keep only the lower X bits of a 64bit (unsigned) integer. + */ +#define MASK_NUMBITS64(X) U64T(U64T(U64T(1) << X) - 1) +/** + * Swap the two values of the two variables X and Y, of a common type TYPE. + */ +#define SWAP_VAR(type, x, y) { type tmpv; tmpv = (x); (x) = (y); (y) = tmpv; } + +/** + * Clear bits given by mask (amount) and shift (position). + */ +//@{ +#define CLEAR_NUMBITS32(VAR, SHIFT, MASK) (VAR) &= htole32(~(U32T(U32T(MASK) << (SHIFT)))) +#define CLEAR_NUMBITS16(VAR, SHIFT, MASK) (VAR) &= htole16(~(U16T(U16T(MASK) << (SHIFT)))) +#define CLEAR_NUMBITS8(VAR, SHIFT, MASK) (VAR) &= U8T(~(U8T(U8T(MASK) << (SHIFT)))) +//@} +/** + * Set bits given by mask (amount) and shift (position) to a value. + */ +//@{ +#define SET_NUMBITS32(VAR, SHIFT, MASK, VALUE) (VAR) |= htole32(U32T((U32T(VALUE) & (MASK)) << (SHIFT))) +#define SET_NUMBITS16(VAR, SHIFT, MASK, VALUE) (VAR) |= htole16(U16T((U16T(VALUE) & (MASK)) << (SHIFT))) +#define SET_NUMBITS8(VAR, SHIFT, MASK, VALUE) (VAR) |= U8T((U8T(VALUE) & (MASK)) << (SHIFT)) +//@} +/** + * Get value of bits given by mask (amount) and shift (position). + */ +//@{ +#define GET_NUMBITS32(VAR, SHIFT, MASK) ((le32toh(VAR) >> (SHIFT)) & (MASK)) +#define GET_NUMBITS16(VAR, SHIFT, MASK) ((le16toh(VAR) >> (SHIFT)) & (MASK)) +#define GET_NUMBITS8(VAR, SHIFT, MASK) ((U8T(VAR) >> (SHIFT)) & (MASK)) +//@} + +/** + * Compare two strings for equality. + * + * @param s1 the first string, cannot be NULL. + * @param s2 the second string, cannot be NULL. + * + * @return true if equal, false otherwise. + */ +static inline bool caerStrEquals(const char *s1, const char *s2) { + if (s1 == NULL || s2 == NULL) { + return (false); + } + + if (strcmp(s1, s2) == 0) { + return (true); + } + + return (false); +} + +/** + * Compare two strings for equality, up to a specified maximum length. + * + * @param s1 the first string, cannot be NULL. + * @param s2 the second string, cannot be NULL. + * @param len maximum comparison length, cannot be zero. + * + * @return true if equal, false otherwise. + */ +static inline bool caerStrEqualsUpTo(const char *s1, const char *s2, size_t len) { + if (s1 == NULL || s2 == NULL || len == 0) { + return (false); + } + + if (strncmp(s1, s2, len) == 0) { + return (true); + } + + return (false); +} + +/** + * Convert a 32bit unsigned integer into an unsigned byte array of up to four bytes. + * The integer will be stored in big-endian order, and the length will specify how + * many bits to convert, starting from the lowest bit. + * + * @param integer the integer to convert. + * @param byteArray pointer to the byte array in which to store the converted values. + * @param byteArrayLength length of the byte array to convert to. + */ +static inline void caerIntegerToByteArray(uint32_t integer, uint8_t *byteArray, uint8_t byteArrayLength) { + switch (byteArrayLength) { + case 4: + byteArray[0] = U8T(integer >> 24); + byteArray[1] = U8T(integer >> 16); + byteArray[2] = U8T(integer >> 8); + byteArray[3] = U8T(integer); + break; + + case 3: + byteArray[0] = U8T(integer >> 16); + byteArray[1] = U8T(integer >> 8); + byteArray[2] = U8T(integer); + break; + + case 2: + byteArray[0] = U8T(integer >> 8); + byteArray[1] = U8T(integer); + break; + + case 1: + byteArray[0] = U8T(integer); + break; + + default: + break; + } +} + +/** + * Convert an unsigned byte array of up to four bytes into a 32bit unsigned integer. + * The byte array length decides how many resulting bits in the integer are set, + * and the single bytes are placed in the integer following big-endian ordering. + * + * @param byteArray pointer to the byte array with parts of the value stored. + * @param byteArrayLength length of the array from which to convert. + * + * @return integer representing the value stored in the byte array. + */ +static inline uint32_t caerByteArrayToInteger(uint8_t *byteArray, uint8_t byteArrayLength) { + uint32_t integer = 0; + + switch (byteArrayLength) { + case 4: + integer |= U32T(byteArray[0] << 24); + integer |= U32T(byteArray[1] << 16); + integer |= U32T(byteArray[2] << 8); + integer |= U32T(byteArray[3]); + break; + + case 3: + integer |= U32T(byteArray[0] << 16); + integer |= U32T(byteArray[1] << 8); + integer |= U32T(byteArray[2]); + break; + + case 2: + integer |= U32T(byteArray[0] << 8); + integer |= U32T(byteArray[1]); + break; + + case 1: + integer |= U32T(byteArray[0]); + break; + + default: + break; + } + + return (integer); +} + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/log.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/log.h new file mode 100755 index 0000000000000000000000000000000000000000..bddbd705621b5031a057deb149dbe670ecf9a1e5 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/log.h @@ -0,0 +1,89 @@ +/** + * @file log.h + * + * Logging functions to print useful messages for the user. + */ + +#ifndef LIBCAER_LOG_H_ +#define LIBCAER_LOG_H_ + +#ifdef __cplusplus + +#include <cstdint> + +extern "C" { +#endif + +#include <stdint.h> + +//@{ +/** + * Log levels for caerLog() logging function. + * Log messages only get printed if their log level is equal or + * above the global system log level, which can be set with + * caerLogLevelSet(). + * The default log level is CAER_LOG_ERROR. + * CAER_LOG_EMERGENCY is the most urgent log level and will always + * be printed, while CAER_LOG_DEBUG is the least urgent log + * level and will only be delivered if configured by the user. + */ +#define CAER_LOG_EMERGENCY (0) +#define CAER_LOG_ALERT (1) +#define CAER_LOG_CRITICAL (2) +#define CAER_LOG_ERROR (3) +#define CAER_LOG_WARNING (4) +#define CAER_LOG_NOTICE (5) +#define CAER_LOG_INFO (6) +#define CAER_LOG_DEBUG (7) +//@} + +/** + * Set the system-wide log level. + * Log messages will only be printed if their level is equal or above + * this level. + * + * @param logLevel the system-wide log level. + */ +void caerLogLevelSet(uint8_t logLevel); + +/** + * Get the current system-wide log level. + * Log messages are only printed if their level is equal or above + * this level. + * + * @return the current system-wide log level. + */ +uint8_t caerLogLevelGet(void); + +/** + * Set to which file descriptors log messages are sent. + * Up to two different file descriptors can be configured here. + * By default logging to STDERR only is enabled. + * If both file descriptors are identical, logging to it will + * only happen once, as if the second one was disabled. + * + * @param fd1 first file descriptor to log to. A negative value will disable it. + * @param fd2 second file descriptor to log to. A negative value will disable it. + */ +void caerLogFileDescriptorsSet(int fd1, int fd2); + +/** + * Main logging function. + * This function takes messages, formats them and sends them out to a file descriptor, + * respecting the system-wide log level setting and prepending the current time, the + * log level and a user-specified common string to the actual formatted output. + * The format is specified exactly as with the printf() family of functions. + * Please see their manual-page for more information. + * + * @param logLevel the message-specific log level. + * @param subSystem a common, user-specified string to prepend before the message. + * @param format the message format string (see printf()). + * @param ... the parameters to be formatted according to the format string (see printf()). + */ +void caerLog(uint8_t logLevel, const char *subSystem, const char *format, ...) __attribute__ ((format (printf, 3, 4))); + +#ifdef __cplusplus +} +#endif + +#endif /* LIBCAER_LOG_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/portable_endian.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/portable_endian.h new file mode 100755 index 0000000000000000000000000000000000000000..ab390002c1f4581984cacf612a4655e5b135353b --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/include/portable_endian.h @@ -0,0 +1,105 @@ +/** + * @file portable_endian.h + * + * Endianness conversion functions for a wide variety of systems, + * including Linux, FreeBSD, MacOS X and Windows. + */ + +// "License": Public Domain +// I, Mathias Panzenböck, place this file hereby into the public domain. Use it at your own risk for whatever you like. +// In case there are jurisdictions that don't support putting things in the public domain you can also consider it to +// be "dual licensed" under the BSD, MIT and Apache licenses, if you want to. This code is trivial anyway. Consider it +// an example on how to get the endian conversion functions on different platforms. + +#ifndef PORTABLE_ENDIAN_H__ +#define PORTABLE_ENDIAN_H__ + +#if (defined(_WIN16) || defined(_WIN32) || defined(_WIN64)) && !defined(__WINDOWS__) + #define __WINDOWS__ +#endif + +#if defined(__linux__) || defined(__CYGWIN__) + #include <endian.h> +#elif defined(__APPLE__) + #include <libkern/OSByteOrder.h> + + #define htobe16(x) OSSwapHostToBigInt16(x) + #define htole16(x) OSSwapHostToLittleInt16(x) + #define be16toh(x) OSSwapBigToHostInt16(x) + #define le16toh(x) OSSwapLittleToHostInt16(x) + + #define htobe32(x) OSSwapHostToBigInt32(x) + #define htole32(x) OSSwapHostToLittleInt32(x) + #define be32toh(x) OSSwapBigToHostInt32(x) + #define le32toh(x) OSSwapLittleToHostInt32(x) + + #define htobe64(x) OSSwapHostToBigInt64(x) + #define htole64(x) OSSwapHostToLittleInt64(x) + #define be64toh(x) OSSwapBigToHostInt64(x) + #define le64toh(x) OSSwapLittleToHostInt64(x) + + #define __BYTE_ORDER BYTE_ORDER + #define __BIG_ENDIAN BIG_ENDIAN + #define __LITTLE_ENDIAN LITTLE_ENDIAN + #define __PDP_ENDIAN PDP_ENDIAN +#elif defined(__OpenBSD__) + #include <sys/endian.h> +#elif defined(__NetBSD__) || defined(__FreeBSD__) || defined(__DragonFly__) + #include <sys/endian.h> + + #define be16toh(x) betoh16(x) + #define le16toh(x) letoh16(x) + + #define be32toh(x) betoh32(x) + #define le32toh(x) letoh32(x) + + #define be64toh(x) betoh64(x) + #define le64toh(x) letoh64(x) +#elif defined(__WINDOWS__) + #include <winsock2.h> + #include <sys/param.h> + + #if BYTE_ORDER == LITTLE_ENDIAN + #define htobe16(x) htons(x) + #define htole16(x) (x) + #define be16toh(x) ntohs(x) + #define le16toh(x) (x) + + #define htobe32(x) htonl(x) + #define htole32(x) (x) + #define be32toh(x) ntohl(x) + #define le32toh(x) (x) + + #define htobe64(x) htonll(x) + #define htole64(x) (x) + #define be64toh(x) ntohll(x) + #define le64toh(x) (x) + #elif BYTE_ORDER == BIG_ENDIAN + /* That would be Xbox 360. */ + #define htobe16(x) (x) + #define htole16(x) __builtin_bswap16(x) + #define be16toh(x) (x) + #define le16toh(x) __builtin_bswap16(x) + + #define htobe32(x) (x) + #define htole32(x) __builtin_bswap32(x) + #define be32toh(x) (x) + #define le32toh(x) __builtin_bswap32(x) + + #define htobe64(x) (x) + #define htole64(x) __builtin_bswap64(x) + #define be64toh(x) (x) + #define le64toh(x) __builtin_bswap64(x) + #else + #error byte order not supported + #endif + + #define __BYTE_ORDER BYTE_ORDER + #define __BIG_ENDIAN BIG_ENDIAN + #define __LITTLE_ENDIAN LITTLE_ENDIAN + #define __PDP_ENDIAN PDP_ENDIAN +#else + #error platform not supported +#endif + +#endif /* PORTABLE_ENDIAN_H__ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/libcaer.pc.in b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/libcaer.pc.in new file mode 100755 index 0000000000000000000000000000000000000000..6f8e52cc38e2ef8a08cb7eaa190efaabaa6975ea --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/libcaer.pc.in @@ -0,0 +1,12 @@ +prefix=@CMAKE_INSTALL_PREFIX@ +exec_prefix=${prefix} +libdir=${prefix}/@CMAKE_INSTALL_LIBDIR@ +includedir=${prefix}/@CMAKE_INSTALL_INCLUDEDIR@ + +Name: @CMAKE_PROJECT_NAME@ +Description: Minimal C API for low-level access to DVS128, DAVIS devices. +Version: @PROJECT_VERSION_NOREV@ +Requires.private: libusb-1.0 >= 1.0.17 +Libs: -L${libdir} -lcaer +Libs.private: @PRIVATE_LIBS@ +Cflags: -I${includedir} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/libcaer_opencv.pc.in b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/libcaer_opencv.pc.in new file mode 100755 index 0000000000000000000000000000000000000000..94f03d7e40412dc2b48b43277b428b3aa8db9a47 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/libcaer_opencv.pc.in @@ -0,0 +1,12 @@ +prefix=@CMAKE_INSTALL_PREFIX@ +exec_prefix=${prefix} +libdir=${prefix}/@CMAKE_INSTALL_LIBDIR@ +includedir=${prefix}/@CMAKE_INSTALL_INCLUDEDIR@ + +Name: @CMAKE_PROJECT_NAME@ +Description: Minimal C API for low-level access to DVS128, DAVIS devices. Uses OpenCV for frame enhancement. +Version: @PROJECT_VERSION_NOREV@ +Requires.private: libusb-1.0 >= 1.0.17, opencv >= 3.1 +Libs: -L${libdir} -lcaer +Libs.private: @PRIVATE_LIBS@ +Cflags: -I${includedir} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/.gitignore b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/.gitignore new file mode 100755 index 0000000000000000000000000000000000000000..7d79361ead52f82eb71651f8b2a85deb8795d476 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/.gitignore @@ -0,0 +1,2 @@ +/*.dylib +/libcaer.so* diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/CMakeLists.txt b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/CMakeLists.txt new file mode 100755 index 0000000000000000000000000000000000000000..bc313230a92bab7414530a071455b2cf35dff81f --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/CMakeLists.txt @@ -0,0 +1,38 @@ +SET(LIBCAER_SRC_FILES + ringbuffer/ringbuffer.c + log.c + events.c + frame_utils.c + device.c + dvs128.c + davis_common.c + davis_fx2.c + davis_fx3.c) + +IF (ENABLE_OPENCV) + # Add C++ OpenCV file and its C wrapper. + SET(LIBCAER_SRC_FILES ${LIBCAER_SRC_FILES} frame_utils_opencv.cpp) +ENDIF() + +SET(LIBCAER_SRC_FILES ${LIBCAER_SRC_FILES} PARENT_SCOPE) + +IF (UNIX AND NOT APPLE) + # Add --as-needed to linker flags for library. + SET(CMAKE_SHARED_LINKER_FLAGS "-Wl,--as-needed") +ENDIF() + +ADD_LIBRARY(caer SHARED ${LIBCAER_SRC_FILES}) + +SET_TARGET_PROPERTIES(caer + PROPERTIES + SOVERSION ${PROJECT_VERSION_MAJOR} + VERSION ${PROJECT_VERSION_NOREV} +) + +TARGET_LINK_LIBRARIES(caer ${LIBCAER_LIBS}) + +IF (OS_WINDOWS) + INSTALL(TARGETS caer RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) +ELSE() + INSTALL(TARGETS caer DESTINATION ${CMAKE_INSTALL_LIBDIR}) +ENDIF() diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/c11threads_posix.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/c11threads_posix.h new file mode 100755 index 0000000000000000000000000000000000000000..6efb2ca28a7e1edcea088fb7df845f0a436419f5 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/c11threads_posix.h @@ -0,0 +1,398 @@ +#ifndef C11THREADS_POSIX_H_ +#define C11THREADS_POSIX_H_ + +#include <stdint.h> +#include <pthread.h> +#include <time.h> +#include <sys/time.h> +#include <sched.h> +#include <errno.h> + +#if defined(OS_LINUX) + #include <sys/prctl.h> + #include <sys/resource.h> +#endif + +typedef pthread_t thrd_t; +typedef pthread_once_t once_flag; +typedef pthread_mutex_t mtx_t; +typedef pthread_rwlock_t mtx_shared_t; // NON STANDARD! +typedef int (*thrd_start_t)(void *); + +enum { + thrd_success = 0, thrd_error = 1, thrd_nomem = 2, thrd_timedout = 3, thrd_busy = 4, +}; + +enum { + mtx_plain = 0, mtx_timed = 1, mtx_recursive = 2, +}; + +#define ONCE_FLAG_INIT PTHREAD_ONCE_INIT + +static inline int thrd_create(thrd_t *thr, thrd_start_t func, void *arg) { + int ret = pthread_create(thr, NULL, (void *(*)(void *)) func, arg); + + switch (ret) { + case 0: + return (thrd_success); + + case EAGAIN: + return (thrd_nomem); + + default: + return (thrd_error); + } +} + +static inline _Noreturn void thrd_exit(int res) { + pthread_exit((void*) (intptr_t) res); +} + +static inline int thrd_detach(thrd_t thr) { + if (pthread_detach(thr) != 0) { + return (thrd_error); + } + + return (thrd_success); +} + +static inline int thrd_join(thrd_t thr, int *res) { + void *pthread_res; + + if (pthread_join(thr, &pthread_res) != 0) { + return (thrd_error); + } + + if (res != NULL) { + *res = (int) (intptr_t) pthread_res; + } + + return (thrd_success); +} + +static inline int thrd_sleep(const struct timespec *time_point, struct timespec *remaining) { + if (nanosleep(time_point, remaining) == 0) { + return (0); // Successful sleep. + } + + if (errno == EINTR) { + return (-1); // C11: a signal occurred. + } + + return (-2); // C11: other negative value if an error occurred. +} + +static inline void thrd_yield(void) { + sched_yield(); +} + +static inline thrd_t thrd_current(void) { + return (pthread_self()); +} + +static inline int thrd_equal(thrd_t lhs, thrd_t rhs) { + return (pthread_equal(lhs, rhs)); +} + +static inline void call_once(once_flag *flag, void (*func)(void)) { + pthread_once(flag, func); +} + +static inline int mtx_init(mtx_t *mutex, int type) { + pthread_mutexattr_t attr; + if (pthread_mutexattr_init(&attr) != 0) { + return (thrd_error); + } + + // TIMED and PLAIN + pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_NORMAL); + + if (type & 0x02) { + // RECURSIVE + pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE); + } + + if (pthread_mutex_init(mutex, &attr) != 0) { + pthread_mutexattr_destroy(&attr); + return (thrd_error); + } + + pthread_mutexattr_destroy(&attr); + return (thrd_success); +} + +static inline void mtx_destroy(mtx_t *mutex) { + pthread_mutex_destroy(mutex); +} + +static inline int mtx_lock(mtx_t *mutex) { + if (pthread_mutex_lock(mutex) != 0) { + return (thrd_error); + } + + return (thrd_success); +} + +static inline int mtx_trylock(mtx_t *mutex) { + int ret = pthread_mutex_trylock(mutex); + + switch (ret) { + case 0: + return (thrd_success); + + case EBUSY: + return (thrd_busy); + + default: + return (thrd_error); + } +} + +static inline int mtx_timedlock(mtx_t *restrict mutex, const struct timespec *restrict time_point) { +#if defined(__APPLE__) + // Emulate on MacOS X. + struct timespec sleepTime = { .tv_sec = 0, .tv_nsec = 1000000 /* 1ms */ }; + struct timeval currentTime; + int ret; + + while ((ret = mtx_trylock(mutex)) == thrd_busy) { + gettimeofday(¤tTime, NULL); + + if (currentTime.tv_sec >= time_point->tv_sec && (currentTime.tv_usec * 1000) >= time_point->tv_nsec) { + return (thrd_timedout); + } + + nanosleep(&sleepTime, NULL); + } + + return (ret); +#elif defined(__unix__) || defined(__UNIX__) + int ret = pthread_mutex_timedlock(mutex, time_point); + + switch (ret) { + case 0: + return (thrd_success); + + case ETIMEDOUT: + return (thrd_timedout); + + default: + return (thrd_error); + } +#endif +} + +static inline int mtx_unlock(mtx_t *mutex) { + if (pthread_mutex_unlock(mutex) != 0) { + return (thrd_error); + } + + return (thrd_success); +} + +// NON STANDARD! 'int type' argument doesn't make sense here, always timed and recursive. +static inline int mtx_shared_init(mtx_shared_t *mutex) { + if (pthread_rwlock_init(mutex, NULL) != 0) { + return (thrd_error); + } + + return (thrd_success); +} + +// NON STANDARD! +static inline void mtx_shared_destroy(mtx_shared_t *mutex) { + pthread_rwlock_destroy(mutex); +} + +// NON STANDARD! +static inline int mtx_shared_lock_exclusive(mtx_shared_t *mutex) { + if (pthread_rwlock_wrlock(mutex) != 0) { + return (thrd_error); + } + + return (thrd_success); +} + +// NON STANDARD! +static inline int mtx_shared_trylock_exclusive(mtx_shared_t *mutex) { + int ret = pthread_rwlock_trywrlock(mutex); + + switch (ret) { + case 0: + return (thrd_success); + + case EBUSY: + return (thrd_busy); + + default: + return (thrd_error); + } +} + +// NON STANDARD! +static inline int mtx_shared_timedlock_exclusive(mtx_shared_t *restrict mutex, + const struct timespec *restrict time_point) { +#if defined(__APPLE__) + // Emulate on MacOS X. + struct timespec sleepTime = { .tv_sec = 0, .tv_nsec = 1000000 /* 1ms */ }; + struct timeval currentTime; + int ret; + + while ((ret = mtx_shared_trylock_exclusive(mutex)) == thrd_busy) { + gettimeofday(¤tTime, NULL); + + if (currentTime.tv_sec >= time_point->tv_sec && (currentTime.tv_usec * 1000) >= time_point->tv_nsec) { + return (thrd_timedout); + } + + nanosleep(&sleepTime, NULL); + } + + return (ret); +#elif defined(__unix__) || defined(__UNIX__) + int ret = pthread_rwlock_timedwrlock(mutex, time_point); + + switch (ret) { + case 0: + return (thrd_success); + + case ETIMEDOUT: + return (thrd_timedout); + + default: + return (thrd_error); + } +#endif +} + +// NON STANDARD! +static inline int mtx_shared_unlock_exclusive(mtx_shared_t *mutex) { + if (pthread_rwlock_unlock(mutex) != 0) { + return (thrd_error); + } + + return (thrd_success); +} + +// NON STANDARD! +static inline int mtx_shared_lock_shared(mtx_shared_t *mutex) { + if (pthread_rwlock_rdlock(mutex) != 0) { + return (thrd_error); + } + + return (thrd_success); +} + +// NON STANDARD! +static inline int mtx_shared_trylock_shared(mtx_shared_t *mutex) { + int ret = pthread_rwlock_tryrdlock(mutex); + + switch (ret) { + case 0: + return (thrd_success); + + case EBUSY: + return (thrd_busy); + + default: + return (thrd_error); + } +} + +// NON STANDARD! +static inline int mtx_shared_timedlock_shared(mtx_shared_t *restrict mutex, const struct timespec *restrict time_point) { +#if defined(__APPLE__) + // Emulate on MacOS X. + struct timespec sleepTime = { .tv_sec = 0, .tv_nsec = 1000000 /* 1ms */ }; + struct timeval currentTime; + int ret; + + while ((ret = mtx_shared_trylock_shared(mutex)) == thrd_busy) { + gettimeofday(¤tTime, NULL); + + if (currentTime.tv_sec >= time_point->tv_sec && (currentTime.tv_usec * 1000) >= time_point->tv_nsec) { + return (thrd_timedout); + } + + nanosleep(&sleepTime, NULL); + } + + return (ret); +#elif defined(__unix__) || defined(__UNIX__) + int ret = pthread_rwlock_timedrdlock(mutex, time_point); + + switch (ret) { + case 0: + return (thrd_success); + + case ETIMEDOUT: + return (thrd_timedout); + + default: + return (thrd_error); + } +#endif +} + +// NON STANDARD! +static inline int mtx_shared_unlock_shared(mtx_shared_t *mutex) { + if (pthread_rwlock_unlock(mutex) != 0) { + return (thrd_error); + } + + return (thrd_success); +} + +// NON STANDARD! +static inline int thrd_set_name(const char *name) { +#if defined(OS_LINUX) + if (prctl(PR_SET_NAME, name) != 0) { + return (thrd_error); + } + + return (thrd_success); +#elif defined(OS_MACOSX) + if (pthread_setname_np(name) != 0) { + return (thrd_error); + } + + return (thrd_success); +#else + return (thrd_error); +#endif +} + +static inline int thrd_get_name(char *name, size_t maxNameLength) { +#if defined(OS_LINUX) + (void)(maxNameLength); // UNUSED FOR LINUX. + + if (prctl(PR_GET_NAME, name) != 0) { + return (thrd_error); + } + + return (thrd_success); +#elif defined(OS_MACOSX) + if (pthread_getname_np(pthread_self(), name, maxNameLength) != 0) { + return (thrd_error); + } + + return (thrd_success); +#else + return (thrd_error); +#endif +} + +// NON STANDARD! +static inline int thrd_set_priority(int priority) { +#if defined(OS_LINUX) + if (setpriority(PRIO_PROCESS, 0, priority) != 0) { + return (thrd_error); + } + + return (thrd_success); +#else + return (thrd_error); +#endif +} + +#endif /* C11THREADS_POSIX_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_common.c b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_common.c new file mode 100755 index 0000000000000000000000000000000000000000..c72c1f77882ef4dfe022bc514f058c5cbcb4107f --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_common.c @@ -0,0 +1,4021 @@ +#include "davis_common.h" + +static bool spiConfigSend(libusb_device_handle *devHandle, uint8_t moduleAddr, uint8_t paramAddr, uint32_t param); +static bool spiConfigReceive(libusb_device_handle *devHandle, uint8_t moduleAddr, uint8_t paramAddr, uint32_t *param); +static libusb_device_handle *davisDeviceOpen(libusb_context *devContext, uint16_t devVID, uint16_t devPID, + uint8_t devType, uint8_t busNumber, uint8_t devAddress, const char *serialNumber, uint16_t requiredLogicRevision, + uint16_t requiredFirmwareVersion); +static void davisDeviceClose(libusb_device_handle *devHandle); +static void davisAllocateTransfers(davisHandle handle, uint32_t bufferNum, uint32_t bufferSize); +static void davisDeallocateTransfers(davisHandle handle); +static void LIBUSB_CALL davisLibUsbCallback(struct libusb_transfer *transfer); +static void davisEventTranslator(davisHandle handle, uint8_t *buffer, size_t bytesSent); +static int davisDataAcquisitionThread(void *inPtr); +static void davisDataAcquisitionThreadConfig(davisHandle handle); + +static inline void checkStrictMonotonicTimestamp(davisHandle handle) { + if (handle->state.currentTimestamp <= handle->state.lastTimestamp) { + caerLog(CAER_LOG_ALERT, handle->info.deviceString, + "Timestamps: non strictly-monotonic timestamp detected: lastTimestamp=%" PRIi32 ", currentTimestamp=%" PRIi32 ", difference=%" PRIi32 ".", + handle->state.lastTimestamp, handle->state.currentTimestamp, + (handle->state.lastTimestamp - handle->state.currentTimestamp)); + } +} + +static inline void updateROISizes(davisState state) { + // Calculate APS ROI sizes for each region. + for (size_t i = 0; i < APS_ROI_REGIONS_MAX; i++) { + uint16_t startColumn = state->apsROIPositionX[i]; + uint16_t startRow = state->apsROIPositionY[i]; + uint16_t endColumn = state->apsROISizeX[i]; + uint16_t endRow = state->apsROISizeY[i]; + + // Position is already set to startCol/Row, so we don't have to reset + // it here. We only have to calculate size from start and end. + if (startColumn < state->apsSizeX) { + state->apsROISizeX[i] = U16T(endColumn + 1 - startColumn); + state->apsROISizeY[i] = U16T(endRow + 1 - startRow); + } + else { + // Turn off this ROI region. + state->apsROISizeX[i] = state->apsROISizeY[i] = 0; + state->apsROIPositionX[i] = state->apsROIPositionY[i] = 0; + } + } +} + +static inline void initFrame(davisHandle handle) { + davisState state = &handle->state; + + state->apsCurrentReadoutType = APS_READOUT_RESET; + for (size_t i = 0; i < APS_READOUT_TYPES_NUM; i++) { + state->apsCountX[i] = 0; + state->apsCountY[i] = 0; + } + + // for (size_t i = 0; i < APS_ROI_REGIONS_MAX; i++) { + memset(state->currentFrameEvent[0], 0, sizeof(struct caer_frame_event)); + // } + + if (state->apsROIUpdate != 0) { + updateROISizes(state); + } + + // Write out start of frame timestamp. + caerFrameEventSetTSStartOfFrame(state->currentFrameEvent[0], state->currentTimestamp); + + // Send APS info event out (as special event). + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent(state->currentSpecialPacket, + state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, APS_FRAME_START); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + + // Setup frame. Only ROI region 0 is supported currently. + caerFrameEventSetLengthXLengthYChannelNumber(state->currentFrameEvent[0], state->apsROISizeX[0], + state->apsROISizeY[0], APS_ADC_CHANNELS, state->currentFramePacket); + caerFrameEventSetROIIdentifier(state->currentFrameEvent[0], 0); + caerFrameEventSetColorFilter(state->currentFrameEvent[0], handle->info.apsColorFilter); + caerFrameEventSetPositionX(state->currentFrameEvent[0], state->apsROIPositionX[0]); + caerFrameEventSetPositionY(state->currentFrameEvent[0], state->apsROIPositionY[0]); +} + +static inline float calculateIMUAccelScale(uint8_t imuAccelScale) { + // Accelerometer scale is: + // 0 - +-2 g - 16384 LSB/g + // 1 - +-4 g - 8192 LSB/g + // 2 - +-8 g - 4096 LSB/g + // 3 - +-16 g - 2048 LSB/g + float accelScale = 65536.0f / (float) U32T(4 * (1 << imuAccelScale)); + + return (accelScale); +} + +static inline float calculateIMUGyroScale(uint8_t imuGyroScale) { + // Gyroscope scale is: + // 0 - +-250 °/s - 131 LSB/°/s + // 1 - +-500 °/s - 65.5 LSB/°/s + // 2 - +-1000 °/s - 32.8 LSB/°/s + // 3 - +-2000 °/s - 16.4 LSB/°/s + float gyroScale = 65536.0f / (float) U32T(500 * (1 << imuGyroScale)); + + return (gyroScale); +} + +static inline void freeAllDataMemory(davisState state) { + if (state->dataExchangeBuffer != NULL) { + ringBufferFree(state->dataExchangeBuffer); + state->dataExchangeBuffer = NULL; + } + + // Since the current event packets aren't necessarily + // already assigned to the current packet container, we + // free them separately from it. + if (state->currentPolarityPacket != NULL) { + free(&state->currentPolarityPacket->packetHeader); + state->currentPolarityPacket = NULL; + + if (state->currentPacketContainer != NULL) { + caerEventPacketContainerSetEventPacket(state->currentPacketContainer, POLARITY_EVENT, NULL); + } + } + + if (state->currentSpecialPacket != NULL) { + free(&state->currentSpecialPacket->packetHeader); + state->currentSpecialPacket = NULL; + + if (state->currentPacketContainer != NULL) { + caerEventPacketContainerSetEventPacket(state->currentPacketContainer, SPECIAL_EVENT, NULL); + } + } + + if (state->currentFramePacket != NULL) { + free(&state->currentFramePacket->packetHeader); + state->currentFramePacket = NULL; + + if (state->currentPacketContainer != NULL) { + caerEventPacketContainerSetEventPacket(state->currentPacketContainer, FRAME_EVENT, NULL); + } + } + + if (state->currentIMU6Packet != NULL) { + free(&state->currentIMU6Packet->packetHeader); + state->currentIMU6Packet = NULL; + + if (state->currentPacketContainer != NULL) { + caerEventPacketContainerSetEventPacket(state->currentPacketContainer, IMU6_EVENT, NULL); + } + } + + if (state->currentPacketContainer != NULL) { + caerEventPacketContainerFree(state->currentPacketContainer); + state->currentPacketContainer = NULL; + } + + if (state->apsCurrentResetFrame != NULL) { + free(state->apsCurrentResetFrame); + state->apsCurrentResetFrame = NULL; + } + + // Also free current ROI frame events. + free(state->currentFrameEvent[0]); // Other regions are contained within contiguous memory block. + + for (size_t i = 0; i < APS_ROI_REGIONS_MAX; i++) { + // Reset pointers to NULL. + state->currentFrameEvent[i] = NULL; + } +} + +bool davisCommonOpen(davisHandle handle, uint16_t VID, uint16_t PID, uint8_t DID_TYPE, const char *deviceName, + uint16_t deviceID, uint8_t busNumberRestrict, uint8_t devAddressRestrict, const char *serialNumberRestrict, + uint16_t requiredLogicRevision, uint16_t requiredFirmwareVersion) { + davisState state = &handle->state; + + // Initialize state variables to default values (if not zero, taken care of by calloc above). + atomic_store_explicit(&state->dataExchangeBufferSize, 64, memory_order_relaxed); + atomic_store_explicit(&state->dataExchangeBlocking, false, memory_order_relaxed); + atomic_store_explicit(&state->dataExchangeStartProducers, true, memory_order_relaxed); + atomic_store_explicit(&state->dataExchangeStopProducers, true, memory_order_relaxed); + atomic_store_explicit(&state->usbBufferNumber, 8, memory_order_relaxed); + atomic_store_explicit(&state->usbBufferSize, 8192, memory_order_relaxed); + + // Packet settings (size (in events) and time interval (in µs)). + atomic_store_explicit(&state->maxPacketContainerPacketSize, 8192, memory_order_relaxed); + atomic_store_explicit(&state->maxPacketContainerInterval, 10000, memory_order_relaxed); + + atomic_thread_fence(memory_order_release); + + // Set device thread name. Maximum length of 15 chars due to Linux limitations. + snprintf(state->deviceThreadName, 15 + 1, "%s ID-%" PRIu16, deviceName, deviceID); + state->deviceThreadName[15] = '\0'; + + // Search for device and open it. + // Initialize libusb using a separate context for each device. + // This is to correctly support one thread per device. + // libusb may create its own threads at this stage, so we temporarly set + // a different thread name. + char originalThreadName[15 + 1]; // +1 for terminating NUL character. + thrd_get_name(originalThreadName, 15); + originalThreadName[15] = '\0'; + + thrd_set_name(state->deviceThreadName); + int res = libusb_init(&state->deviceContext); + + thrd_set_name(originalThreadName); + + if (res != LIBUSB_SUCCESS) { + caerLog(CAER_LOG_CRITICAL, __func__, "Failed to initialize libusb context. Error: %d.", res); + return (false); + } + + // Try to open a DAVIS device on a specific USB port. + state->deviceHandle = davisDeviceOpen(state->deviceContext, VID, PID, DID_TYPE, busNumberRestrict, + devAddressRestrict, serialNumberRestrict, requiredLogicRevision, requiredFirmwareVersion); + if (state->deviceHandle == NULL) { + libusb_exit(state->deviceContext); + + caerLog(CAER_LOG_CRITICAL, __func__, "Failed to open %s device.", deviceName); + return (false); + } + + // At this point we can get some more precise data on the device and update + // the logging string to reflect that and be more informative. + uint8_t busNumber = libusb_get_bus_number(libusb_get_device(state->deviceHandle)); + uint8_t devAddress = libusb_get_device_address(libusb_get_device(state->deviceHandle)); + + char serialNumber[8 + 1] = { 0 }; + int getStringDescResult = libusb_get_string_descriptor_ascii(state->deviceHandle, 3, (unsigned char *) serialNumber, + 8 + 1); + + // Check serial number success and length. + if (getStringDescResult < 0 || getStringDescResult > 8) { + davisDeviceClose(state->deviceHandle); + libusb_exit(state->deviceContext); + + caerLog(CAER_LOG_CRITICAL, __func__, "Unable to get serial number for %s device.", deviceName); + return (false); + } + + size_t fullLogStringLength = (size_t) snprintf(NULL, 0, "%s ID-%" PRIu16 " SN-%s [%" PRIu8 ":%" PRIu8 "]", + deviceName, deviceID, serialNumber, busNumber, devAddress); + + char *fullLogString = malloc(fullLogStringLength + 1); + if (fullLogString == NULL) { + davisDeviceClose(state->deviceHandle); + libusb_exit(state->deviceContext); + + caerLog(CAER_LOG_CRITICAL, __func__, "Unable to allocate memory for %s device info string.", deviceName); + return (false); + } + + snprintf(fullLogString, fullLogStringLength + 1, "%s ID-%" PRIu16 " SN-%s [%" PRIu8 ":%" PRIu8 "]", deviceName, + deviceID, serialNumber, busNumber, devAddress); + + // Populate info variables based on data from device. + uint32_t param32 = 0; + + handle->info.deviceID = I16T(deviceID); + strncpy(handle->info.deviceSerialNumber, serialNumber, 8 + 1); + handle->info.deviceUSBBusNumber = busNumber; + handle->info.deviceUSBDeviceAddress = devAddress; + handle->info.deviceString = fullLogString; + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_SYSINFO, DAVIS_CONFIG_SYSINFO_LOGIC_VERSION, ¶m32); + handle->info.logicVersion = I16T(param32); + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_SYSINFO, DAVIS_CONFIG_SYSINFO_DEVICE_IS_MASTER, ¶m32); + handle->info.deviceIsMaster = param32; + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_SYSINFO, DAVIS_CONFIG_SYSINFO_LOGIC_CLOCK, ¶m32); + handle->info.logicClock = I16T(param32); + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_SYSINFO, DAVIS_CONFIG_SYSINFO_ADC_CLOCK, ¶m32); + handle->info.adcClock = I16T(param32); + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_SYSINFO, DAVIS_CONFIG_SYSINFO_CHIP_IDENTIFIER, ¶m32); + handle->info.chipID = I16T(param32); + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_HAS_PIXEL_FILTER, ¶m32); + handle->info.dvsHasPixelFilter = param32; + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_HAS_BACKGROUND_ACTIVITY_FILTER, ¶m32); + handle->info.dvsHasBackgroundActivityFilter = param32; + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_HAS_TEST_EVENT_GENERATOR, ¶m32); + handle->info.dvsHasTestEventGenerator = param32; + + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_COLOR_FILTER, ¶m32); + handle->info.apsColorFilter = U8T(param32); + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_HAS_GLOBAL_SHUTTER, ¶m32); + handle->info.apsHasGlobalShutter = param32; + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_HAS_QUAD_ROI, ¶m32); + handle->info.apsHasQuadROI = param32; + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_HAS_EXTERNAL_ADC, ¶m32); + handle->info.apsHasExternalADC = param32; + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_HAS_INTERNAL_ADC, ¶m32); + handle->info.apsHasInternalADC = param32; + + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_HAS_GENERATOR, ¶m32); + handle->info.extInputHasGenerator = param32; + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_HAS_EXTRA_DETECTORS, ¶m32); + handle->info.extInputHasExtraDetectors = param32; + + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_SIZE_COLUMNS, ¶m32); + state->dvsSizeX = I16T(param32); + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_SIZE_ROWS, ¶m32); + state->dvsSizeY = I16T(param32); + + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_ORIENTATION_INFO, ¶m32); + state->dvsInvertXY = U16T(param32) & 0x04; + + if (state->dvsInvertXY) { + handle->info.dvsSizeX = state->dvsSizeY; + handle->info.dvsSizeY = state->dvsSizeX; + } + else { + handle->info.dvsSizeX = state->dvsSizeX; + handle->info.dvsSizeY = state->dvsSizeY; + } + + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_SIZE_COLUMNS, ¶m32); + state->apsSizeX = I16T(param32); + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_SIZE_ROWS, ¶m32); + state->apsSizeY = I16T(param32); + + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_ORIENTATION_INFO, ¶m32); + uint16_t apsOrientationInfo = U16T(param32); + state->apsInvertXY = apsOrientationInfo & 0x04; + state->apsFlipX = apsOrientationInfo & 0x02; + state->apsFlipY = apsOrientationInfo & 0x01; + + if (state->apsInvertXY) { + handle->info.apsSizeX = state->apsSizeY; + handle->info.apsSizeY = state->apsSizeX; + } + else { + handle->info.apsSizeX = state->apsSizeX; + handle->info.apsSizeY = state->apsSizeY; + } + + caerLog(CAER_LOG_DEBUG, fullLogString, "Initialized device successfully with USB Bus=%" PRIu8 ":Addr=%" PRIu8 ".", + busNumber, devAddress); + + return (true); +} + +bool davisCommonClose(davisHandle handle) { + davisState state = &handle->state; + + // Finally, close the device fully. + davisDeviceClose(state->deviceHandle); + + // Destroy libusb context. + libusb_exit(state->deviceContext); + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "Shutdown successful."); + + // Free memory. + free(handle->info.deviceString); + free(handle); + + return (true); +} + +struct caer_davis_info caerDavisInfoGet(caerDeviceHandle cdh) { + davisHandle handle = (davisHandle) cdh; + + // Check if the pointer is valid. + if (handle == NULL) { + struct caer_davis_info emptyInfo = { 0, .deviceString = NULL }; + return (emptyInfo); + } + + // Check if device type is supported. + if (handle->deviceType != CAER_DEVICE_DAVIS_FX2 && handle->deviceType != CAER_DEVICE_DAVIS_FX3) { + struct caer_davis_info emptyInfo = { 0, .deviceString = NULL }; + return (emptyInfo); + } + + // Return a copy of the device information. + return (handle->info); +} + +bool davisCommonSendDefaultFPGAConfig(caerDeviceHandle cdh, +bool (*configSet)(caerDeviceHandle cdh, int8_t modAddr, uint8_t paramAddr, uint32_t param)) { + davisHandle handle = (davisHandle) cdh; + + (*configSet)(cdh, DAVIS_CONFIG_MUX, DAVIS_CONFIG_MUX_TIMESTAMP_RESET, false); + (*configSet)(cdh, DAVIS_CONFIG_MUX, DAVIS_CONFIG_MUX_FORCE_CHIP_BIAS_ENABLE, false); + (*configSet)(cdh, DAVIS_CONFIG_MUX, DAVIS_CONFIG_MUX_DROP_DVS_ON_TRANSFER_STALL, true); + (*configSet)(cdh, DAVIS_CONFIG_MUX, DAVIS_CONFIG_MUX_DROP_APS_ON_TRANSFER_STALL, false); + (*configSet)(cdh, DAVIS_CONFIG_MUX, DAVIS_CONFIG_MUX_DROP_IMU_ON_TRANSFER_STALL, false); + (*configSet)(cdh, DAVIS_CONFIG_MUX, DAVIS_CONFIG_MUX_DROP_EXTINPUT_ON_TRANSFER_STALL, true); + + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_ACK_DELAY_ROW, 4); // in cycles @ LogicClock + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_ACK_DELAY_COLUMN, 0); // in cycles @ LogicClock + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_ACK_EXTENSION_ROW, 1); // in cycles @ LogicClock + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_ACK_EXTENSION_COLUMN, 0); // in cycles @ LogicClock + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_WAIT_ON_TRANSFER_STALL, false); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_ROW_ONLY_EVENTS, true); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_EXTERNAL_AER_CONTROL, false); + if (handle->info.dvsHasPixelFilter) { + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_0_ROW, U32T(handle->info.dvsSizeY)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_0_COLUMN, U32T(handle->info.dvsSizeX)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_1_ROW, U32T(handle->info.dvsSizeY)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_1_COLUMN, U32T(handle->info.dvsSizeX)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_2_ROW, U32T(handle->info.dvsSizeY)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_2_COLUMN, U32T(handle->info.dvsSizeX)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_3_ROW, U32T(handle->info.dvsSizeY)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_3_COLUMN, U32T(handle->info.dvsSizeX)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_4_ROW, U32T(handle->info.dvsSizeY)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_4_COLUMN, U32T(handle->info.dvsSizeX)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_5_ROW, U32T(handle->info.dvsSizeY)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_5_COLUMN, U32T(handle->info.dvsSizeX)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_6_ROW, U32T(handle->info.dvsSizeY)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_6_COLUMN, U32T(handle->info.dvsSizeX)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_7_ROW, U32T(handle->info.dvsSizeY)); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_PIXEL_7_COLUMN, U32T(handle->info.dvsSizeX)); + } + if (handle->info.dvsHasBackgroundActivityFilter) { + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_BACKGROUND_ACTIVITY, true); + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_FILTER_BACKGROUND_ACTIVITY_DELTAT, 20000); // in µs + } + if (handle->info.dvsHasTestEventGenerator) { + (*configSet)(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_TEST_EVENT_GENERATOR_ENABLE, false); + } + + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_RESET_READ, true); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_WAIT_ON_TRANSFER_STALL, true); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_GLOBAL_SHUTTER, handle->info.apsHasGlobalShutter); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_START_COLUMN_0, 0); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_START_ROW_0, 0); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_END_COLUMN_0, U16T(handle->info.apsSizeX - 1)); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_END_ROW_0, U16T(handle->info.apsSizeY - 1)); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_EXPOSURE, 4000); // in µs, converted to cycles @ ADCClock later + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_FRAME_DELAY, 1000); // in µs, converted to cycles @ ADCClock later + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_RESET_SETTLE, U32T(handle->info.adcClock / 3)); // in cycles @ ADCClock + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_COLUMN_SETTLE, U32T(handle->info.adcClock)); // in cycles @ ADCClock + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_ROW_SETTLE, U32T(handle->info.adcClock / 3)); // in cycles @ ADCClock + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_NULL_SETTLE, U32T(handle->info.adcClock / 10)); // in cycles @ ADCClock + if (handle->info.apsHasQuadROI) { + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_START_COLUMN_1, U32T(handle->info.apsSizeX)); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_START_ROW_1, U32T(handle->info.apsSizeY)); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_END_COLUMN_1, U32T(handle->info.apsSizeX)); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_END_ROW_1, U32T(handle->info.apsSizeY)); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_START_COLUMN_2, U32T(handle->info.apsSizeX)); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_START_ROW_2, U32T(handle->info.apsSizeY)); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_END_COLUMN_2, U32T(handle->info.apsSizeX)); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_END_ROW_2, U32T(handle->info.apsSizeY)); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_START_COLUMN_3, U32T(handle->info.apsSizeX)); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_START_ROW_3, U32T(handle->info.apsSizeY)); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_END_COLUMN_3, U32T(handle->info.apsSizeX)); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_END_ROW_3, U32T(handle->info.apsSizeY)); + } + if (handle->info.apsHasInternalADC) { + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_USE_INTERNAL_ADC, true); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_SAMPLE_ENABLE, true); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_SAMPLE_SETTLE, U32T(handle->info.adcClock)); // in cycles @ ADCClock + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_RAMP_RESET, U32T(handle->info.adcClock / 3)); // in cycles @ ADCClock + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_RAMP_SHORT_RESET, false); + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_ADC_TEST_MODE, false); + } + if (IS_DAVISRGB(handle->info.chipID)) { + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVISRGB_CONFIG_APS_TRANSFER, U32T(handle->info.adcClock * 25)); // in cycles @ ADCClock + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVISRGB_CONFIG_APS_RSFDSETTLE, U32T(handle->info.adcClock * 15)); // in cycles @ ADCClock + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVISRGB_CONFIG_APS_GSPDRESET, U32T(handle->info.adcClock * 15)); // in cycles @ ADCClock + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVISRGB_CONFIG_APS_GSRESETFALL, U32T(handle->info.adcClock * 15)); // in cycles @ ADCClock + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVISRGB_CONFIG_APS_GSTXFALL, U32T(handle->info.adcClock * 15)); // in cycles @ ADCClock + (*configSet)(cdh, DAVIS_CONFIG_APS, DAVISRGB_CONFIG_APS_GSFDRESET, U32T(handle->info.adcClock * 15)); // in cycles @ ADCClock + } + + (*configSet)(cdh, DAVIS_CONFIG_IMU, DAVIS_CONFIG_IMU_TEMP_STANDBY, false); + (*configSet)(cdh, DAVIS_CONFIG_IMU, DAVIS_CONFIG_IMU_ACCEL_STANDBY, false); + (*configSet)(cdh, DAVIS_CONFIG_IMU, DAVIS_CONFIG_IMU_GYRO_STANDBY, false); + (*configSet)(cdh, DAVIS_CONFIG_IMU, DAVIS_CONFIG_IMU_LP_CYCLE, false); + (*configSet)(cdh, DAVIS_CONFIG_IMU, DAVIS_CONFIG_IMU_LP_WAKEUP, 1); + (*configSet)(cdh, DAVIS_CONFIG_IMU, DAVIS_CONFIG_IMU_SAMPLE_RATE_DIVIDER, 0); + (*configSet)(cdh, DAVIS_CONFIG_IMU, DAVIS_CONFIG_IMU_DIGITAL_LOW_PASS_FILTER, 1); + (*configSet)(cdh, DAVIS_CONFIG_IMU, DAVIS_CONFIG_IMU_ACCEL_FULL_SCALE, 1); + (*configSet)(cdh, DAVIS_CONFIG_IMU, DAVIS_CONFIG_IMU_GYRO_FULL_SCALE, 1); + + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_RISING_EDGES, false); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_FALLING_EDGES, false); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_PULSES, true); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY, true); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH, U32T(handle->info.logicClock)); // in cycles @ LogicClock + + if (handle->info.extInputHasGenerator) { + // Disable generator by default. Has to be enabled manually after sendDefaultConfig() by user! + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_RUN_GENERATOR, false); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_GENERATE_USE_CUSTOM_SIGNAL, false); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_POLARITY, true); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_INTERVAL, + U32T(handle->info.logicClock)); // in cycles @ LogicClock + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_LENGTH, + U32T(handle->info.logicClock / 2)); // in cycles @ LogicClock + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_GENERATE_INJECT_ON_RISING_EDGE, false); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_GENERATE_INJECT_ON_FALLING_EDGE, false); + } + + if (handle->info.extInputHasExtraDetectors) { + // Disable extra detectors by default. Have to be enabled manually after sendDefaultConfig() by user! + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_RUN_DETECTOR1, false); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_RISING_EDGES1, false); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_FALLING_EDGES1, false); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_PULSES1, true); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY1, true); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH1, + U32T(handle->info.logicClock)); // in cycles @ LogicClock + + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_RUN_DETECTOR2, false); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_RISING_EDGES2, false); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_FALLING_EDGES2, false); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_PULSES2, true); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY2, true); + (*configSet)(cdh, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH2, + U32T(handle->info.logicClock)); // in cycles @ LogicClock + } + + (*configSet)(cdh, DAVIS_CONFIG_USB, DAVIS_CONFIG_USB_EARLY_PACKET_DELAY, 8); // in 125µs time-slices (defaults to 1ms) + + return (true); +} + +#define CF_N_TYPE(COARSE, FINE) (struct caer_bias_coarsefine) \ + { .coarseValue = COARSE, .fineValue = FINE, .enabled = true, .sexN = true, \ + .typeNormal = true, .currentLevelNormal = true } + +#define CF_P_TYPE(COARSE, FINE) (struct caer_bias_coarsefine) \ + { .coarseValue = COARSE, .fineValue = FINE, .enabled = true, .sexN = false, \ + .typeNormal = true, .currentLevelNormal = true } + +#define CF_N_TYPE_CAS(COARSE, FINE) (struct caer_bias_coarsefine) \ + { .coarseValue = COARSE, .fineValue = FINE, .enabled = true, .sexN = true, \ + .typeNormal = false, .currentLevelNormal = true } + +/* + * #define CF_P_TYPE_CAS(COARSE, FINE) (struct caer_bias_coarsefine) \ + * { .coarseValue = COARSE, .fineValue = FINE, .enabled = true, .sexN = false, \ + * .typeNormal = false, .currentLevelNormal = true } + */ + +#define CF_N_TYPE_OFF(COARSE, FINE) (struct caer_bias_coarsefine) \ + { .coarseValue = COARSE, .fineValue = FINE, .enabled = false, .sexN = true, \ + .typeNormal = true, .currentLevelNormal = true } + +#define CF_P_TYPE_OFF(COARSE, FINE) (struct caer_bias_coarsefine) \ + { .coarseValue = COARSE, .fineValue = FINE, .enabled = false, .sexN = false, \ + .typeNormal = true, .currentLevelNormal = true } + +#define SHIFTSOURCE(REF, REG, OPMODE) (struct caer_bias_shiftedsource) \ + { .refValue = REF, .regValue = REG, .operatingMode = OPMODE, .voltageLevel = SPLIT_GATE } + +#define VDAC(VOLT, CURR) (struct caer_bias_vdac) \ + { .voltageValue = VOLT, .currentValue = CURR } + +bool davisCommonSendDefaultChipConfig(caerDeviceHandle cdh, +bool (*configSet)(caerDeviceHandle cdh, int8_t modAddr, uint8_t paramAddr, uint32_t param)) { + davisHandle handle = (davisHandle) cdh; + + // Default bias configuration. + if (IS_DAVIS240(handle->info.chipID)) { + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_DIFFBN, caerBiasCoarseFineGenerate(CF_N_TYPE(4, 39))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_ONBN, caerBiasCoarseFineGenerate(CF_N_TYPE(5, 255))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_OFFBN, caerBiasCoarseFineGenerate(CF_N_TYPE(4, 0))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_APSCASEPC, + caerBiasCoarseFineGenerate(CF_N_TYPE_CAS(5, 185))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_DIFFCASBNC, + caerBiasCoarseFineGenerate(CF_N_TYPE_CAS(5, 115))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_APSROSFBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(6, 219))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_LOCALBUFBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 164))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_PIXINVBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 129))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_PRBP, caerBiasCoarseFineGenerate(CF_P_TYPE(2, 58))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_PRSFBP, caerBiasCoarseFineGenerate(CF_P_TYPE(1, 16))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_REFRBP, caerBiasCoarseFineGenerate(CF_P_TYPE(4, 25))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_AEPDBN, caerBiasCoarseFineGenerate(CF_N_TYPE(6, 91))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_LCOLTIMEOUTBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 49))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_AEPUXBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(4, 80))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_AEPUYBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(7, 152))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_IFTHRBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 255))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_IFREFRBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 255))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_PADFOLLBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(7, 215))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_APSOVERFLOWLEVELBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(6, 253))); + + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_BIASBUFFER, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 254))); + + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_SSP, + caerBiasShiftedSourceGenerate(SHIFTSOURCE(1, 33, SHIFTED_SOURCE))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS240_CONFIG_BIAS_SSN, + caerBiasShiftedSourceGenerate(SHIFTSOURCE(1, 33, SHIFTED_SOURCE))); + + } + + if (IS_DAVIS128(handle->info.chipID) || IS_DAVIS208(handle->info.chipID) + || IS_DAVIS346(handle->info.chipID) || IS_DAVIS640(handle->info.chipID)) { + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_APSOVERFLOWLEVEL, caerBiasVDACGenerate(VDAC(27, 6))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_APSCAS, caerBiasVDACGenerate(VDAC(21, 6))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_ADCREFHIGH, caerBiasVDACGenerate(VDAC(30, 7))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_ADCREFLOW, caerBiasVDACGenerate(VDAC(1, 7))); + + if (IS_DAVIS346(handle->info.chipID) || IS_DAVIS640(handle->info.chipID)) { + // Only DAVIS346 and 640 have ADC testing. + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS346_CONFIG_BIAS_ADCTESTVOLTAGE, + caerBiasVDACGenerate(VDAC(21, 7))); + } + + if (IS_DAVIS208(handle->info.chipID)) { + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS208_CONFIG_BIAS_RESETHIGHPASS, caerBiasVDACGenerate(VDAC(63, 7))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS208_CONFIG_BIAS_REFSS, caerBiasVDACGenerate(VDAC(11, 5))); + + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS208_CONFIG_BIAS_REGBIASBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(5, 20))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS208_CONFIG_BIAS_REFSSBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 20))); + } + + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_LOCALBUFBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 164))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_PADFOLLBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(7, 215))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_DIFFBN, caerBiasCoarseFineGenerate(CF_N_TYPE(4, 39))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_ONBN, caerBiasCoarseFineGenerate(CF_N_TYPE(5, 255))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_OFFBN, caerBiasCoarseFineGenerate(CF_N_TYPE(4, 1))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_PIXINVBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 129))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_PRBP, caerBiasCoarseFineGenerate(CF_P_TYPE(2, 58))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_PRSFBP, caerBiasCoarseFineGenerate(CF_P_TYPE(1, 16))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_REFRBP, caerBiasCoarseFineGenerate(CF_P_TYPE(4, 25))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_READOUTBUFBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(6, 20))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_APSROSFBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(6, 219))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_ADCCOMPBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(5, 20))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_COLSELLOWBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(0, 1))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_DACBUFBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(6, 60))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_LCOLTIMEOUTBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 49))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_AEPDBN, caerBiasCoarseFineGenerate(CF_N_TYPE(6, 91))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_AEPUXBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(4, 80))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_AEPUYBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(7, 152))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_IFREFRBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 255))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_IFTHRBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 255))); + + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_BIASBUFFER, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 254))); + + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_SSP, + caerBiasShiftedSourceGenerate(SHIFTSOURCE(1, 33, SHIFTED_SOURCE))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS128_CONFIG_BIAS_SSN, + caerBiasShiftedSourceGenerate(SHIFTSOURCE(1, 33, SHIFTED_SOURCE))); + + if (IS_DAVIS640(handle->info.chipID)) { + // Slow down pixels for big 640x480 array, to avoid overwhelming the AER bus. + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS640_CONFIG_BIAS_PRBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(2, 3))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVIS640_CONFIG_BIAS_PRSFBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(1, 1))); + } + } + + if (IS_DAVISRGB(handle->info.chipID)) { + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_APSCAS, caerBiasVDACGenerate(VDAC(21, 4))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_OVG1LO, caerBiasVDACGenerate(VDAC(21, 4))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_OVG2LO, caerBiasVDACGenerate(VDAC(0, 0))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_TX2OVG2HI, caerBiasVDACGenerate(VDAC(63, 0))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_GND07, caerBiasVDACGenerate(VDAC(13, 4))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_ADCTESTVOLTAGE, caerBiasVDACGenerate(VDAC(21, 0))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_ADCREFHIGH, caerBiasVDACGenerate(VDAC(63, 7))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_ADCREFLOW, caerBiasVDACGenerate(VDAC(0, 7))); + + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_IFREFRBN, + caerBiasCoarseFineGenerate(CF_N_TYPE_OFF(5, 255))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_IFTHRBN, + caerBiasCoarseFineGenerate(CF_N_TYPE_OFF(5, 255))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_LOCALBUFBN, + caerBiasCoarseFineGenerate(CF_N_TYPE_OFF(5, 164))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_PADFOLLBN, + caerBiasCoarseFineGenerate(CF_N_TYPE_OFF(7, 209))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_PIXINVBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(4, 164))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_DIFFBN, caerBiasCoarseFineGenerate(CF_N_TYPE(4, 54))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_ONBN, caerBiasCoarseFineGenerate(CF_N_TYPE(6, 63))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_OFFBN, caerBiasCoarseFineGenerate(CF_N_TYPE(2, 138))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_PRBP, caerBiasCoarseFineGenerate(CF_P_TYPE(1, 108))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_PRSFBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(1, 108))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_REFRBP, caerBiasCoarseFineGenerate(CF_P_TYPE(4, 28))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_ARRAYBIASBUFFERBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(6, 128))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_ARRAYLOGICBUFFERBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 255))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_FALLTIMEBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(7, 41))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_RISETIMEBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(6, 162))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_READOUTBUFBP, + caerBiasCoarseFineGenerate(CF_P_TYPE_OFF(6, 20))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_APSROSFBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(6, 255))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_ADCCOMPBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(4, 159))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_DACBUFBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(6, 194))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_LCOLTIMEOUTBN, + caerBiasCoarseFineGenerate(CF_N_TYPE(5, 49))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_AEPDBN, caerBiasCoarseFineGenerate(CF_N_TYPE(6, 91))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_AEPUXBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(4, 80))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_AEPUYBP, + caerBiasCoarseFineGenerate(CF_P_TYPE(7, 152))); + + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_BIASBUFFER, + caerBiasCoarseFineGenerate(CF_N_TYPE(6, 251))); + + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_SSP, + caerBiasShiftedSourceGenerate(SHIFTSOURCE(1, 33, TIED_TO_RAIL))); + (*configSet)(cdh, DAVIS_CONFIG_BIAS, DAVISRGB_CONFIG_BIAS_SSN, + caerBiasShiftedSourceGenerate(SHIFTSOURCE(2, 33, SHIFTED_SOURCE))); + } + + // Default chip configuration. + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_DIGITALMUX0, 0); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_DIGITALMUX1, 0); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_DIGITALMUX2, 0); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_DIGITALMUX3, 0); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_ANALOGMUX0, 0); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_ANALOGMUX1, 0); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_ANALOGMUX2, 0); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_BIASMUX0, 0); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_RESETCALIBNEURON, true); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_TYPENCALIBNEURON, false); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_RESETTESTPIXEL, true); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_AERNAROW, false); // Use nArow in the AER state machine. + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_USEAOUT, false); // Enable analog pads for aMUX output (testing). + + // No GlobalShutter flag set here, we already set it above for the APS GS flag, + // and that is automatically propagated to the chip config shift-register in + // configSet() and kept in sync. + + // Special extra pixels control for DAVIS240 A/B. + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS240_CONFIG_CHIP_SPECIALPIXELCONTROL, false); + + // Select which gray counter to use with the internal ADC: '0' means the external gray counter is used, which + // has to be supplied off-chip. '1' means the on-chip gray counter is used instead. + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_SELECTGRAYCOUNTER, 1); + + // Test ADC functionality: if true, the ADC takes its input voltage not from the pixel, but from the + // VDAC 'AdcTestVoltage'. If false, the voltage comes from the pixels. + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS346_CONFIG_CHIP_TESTADC, false); + + if (IS_DAVIS208(handle->info.chipID)) { + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS208_CONFIG_CHIP_SELECTPREAMPAVG, false); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS208_CONFIG_CHIP_SELECTBIASREFSS, false); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS208_CONFIG_CHIP_SELECTSENSE, true); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS208_CONFIG_CHIP_SELECTPOSFB, false); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVIS208_CONFIG_CHIP_SELECTHIGHPASS, false); + } + + if (IS_DAVISRGB(handle->info.chipID)) { + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVISRGB_CONFIG_CHIP_ADJUSTOVG1LO, true); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVISRGB_CONFIG_CHIP_ADJUSTOVG2LO, false); + (*configSet)(cdh, DAVIS_CONFIG_CHIP, DAVISRGB_CONFIG_CHIP_ADJUSTTX2OVG2HI, false); + } + + return (true); +} + +bool davisCommonConfigSet(davisHandle handle, int8_t modAddr, uint8_t paramAddr, uint32_t param) { + davisState state = &handle->state; + + switch (modAddr) { + case CAER_HOST_CONFIG_USB: + switch (paramAddr) { + case CAER_HOST_CONFIG_USB_BUFFER_NUMBER: + atomic_store(&state->usbBufferNumber, param); + + // Notify data acquisition thread to change buffers. + atomic_fetch_or(&state->dataAcquisitionThreadConfigUpdate, 1 << 0); + break; + + case CAER_HOST_CONFIG_USB_BUFFER_SIZE: + atomic_store(&state->usbBufferSize, param); + + // Notify data acquisition thread to change buffers. + atomic_fetch_or(&state->dataAcquisitionThreadConfigUpdate, 1 << 0); + break; + + default: + return (false); + break; + } + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE: + switch (paramAddr) { + case CAER_HOST_CONFIG_DATAEXCHANGE_BUFFER_SIZE: + atomic_store(&state->dataExchangeBufferSize, param); + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE_BLOCKING: + atomic_store(&state->dataExchangeBlocking, param); + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE_START_PRODUCERS: + atomic_store(&state->dataExchangeStartProducers, param); + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE_STOP_PRODUCERS: + atomic_store(&state->dataExchangeStopProducers, param); + break; + + default: + return (false); + break; + } + break; + + case CAER_HOST_CONFIG_PACKETS: + switch (paramAddr) { + case CAER_HOST_CONFIG_PACKETS_MAX_CONTAINER_PACKET_SIZE: + atomic_store(&state->maxPacketContainerPacketSize, param); + break; + + case CAER_HOST_CONFIG_PACKETS_MAX_CONTAINER_INTERVAL: + atomic_store(&state->maxPacketContainerInterval, param); + break; + + default: + return (false); + break; + } + break; + + case DAVIS_CONFIG_MUX: + switch (paramAddr) { + case DAVIS_CONFIG_MUX_RUN: + case DAVIS_CONFIG_MUX_TIMESTAMP_RUN: + case DAVIS_CONFIG_MUX_FORCE_CHIP_BIAS_ENABLE: + case DAVIS_CONFIG_MUX_DROP_DVS_ON_TRANSFER_STALL: + case DAVIS_CONFIG_MUX_DROP_APS_ON_TRANSFER_STALL: + case DAVIS_CONFIG_MUX_DROP_IMU_ON_TRANSFER_STALL: + case DAVIS_CONFIG_MUX_DROP_EXTINPUT_ON_TRANSFER_STALL: + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_MUX, paramAddr, param)); + break; + + case DAVIS_CONFIG_MUX_TIMESTAMP_RESET: { + // Use multi-command VR for more efficient implementation of reset, + // that also guarantees returning to the default state. + if (param) { + uint8_t spiMultiConfig[6 + 6] = { 0 }; + + spiMultiConfig[0] = DAVIS_CONFIG_MUX; + spiMultiConfig[1] = DAVIS_CONFIG_MUX_TIMESTAMP_RESET; + spiMultiConfig[2] = 0x00; + spiMultiConfig[3] = 0x00; + spiMultiConfig[4] = 0x00; + spiMultiConfig[5] = 0x01; + + spiMultiConfig[6] = DAVIS_CONFIG_MUX; + spiMultiConfig[7] = DAVIS_CONFIG_MUX_TIMESTAMP_RESET; + spiMultiConfig[8] = 0x00; + spiMultiConfig[9] = 0x00; + spiMultiConfig[10] = 0x00; + spiMultiConfig[11] = 0x00; + + return (libusb_control_transfer(state->deviceHandle, + LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_FPGA_CONFIG_MULTIPLE, 2, 0, spiMultiConfig, sizeof(spiMultiConfig), 0) + == sizeof(spiMultiConfig)); + } + break; + } + + default: + return (false); + break; + } + break; + + case DAVIS_CONFIG_DVS: + switch (paramAddr) { + case DAVIS_CONFIG_DVS_RUN: + case DAVIS_CONFIG_DVS_ACK_DELAY_ROW: + case DAVIS_CONFIG_DVS_ACK_DELAY_COLUMN: + case DAVIS_CONFIG_DVS_ACK_EXTENSION_ROW: + case DAVIS_CONFIG_DVS_ACK_EXTENSION_COLUMN: + case DAVIS_CONFIG_DVS_WAIT_ON_TRANSFER_STALL: + case DAVIS_CONFIG_DVS_FILTER_ROW_ONLY_EVENTS: + case DAVIS_CONFIG_DVS_EXTERNAL_AER_CONTROL: + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_DVS, paramAddr, param)); + break; + + case DAVIS_CONFIG_DVS_FILTER_PIXEL_0_ROW: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_1_ROW: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_2_ROW: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_3_ROW: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_4_ROW: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_5_ROW: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_6_ROW: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_7_ROW: + if (handle->info.dvsHasPixelFilter) { + if (handle->state.dvsInvertXY) { + // Convert to column if X/Y inverted. + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_DVS, U8T(paramAddr + 1), param)); + } + else { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_DVS, paramAddr, param)); + } + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_DVS_FILTER_PIXEL_0_COLUMN: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_1_COLUMN: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_2_COLUMN: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_3_COLUMN: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_4_COLUMN: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_5_COLUMN: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_6_COLUMN: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_7_COLUMN: + if (handle->info.dvsHasPixelFilter) { + if (handle->state.dvsInvertXY) { + // Convert to row if X/Y inverted. + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_DVS, U8T(paramAddr - 1), param)); + } + else { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_DVS, paramAddr, param)); + } + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_DVS_FILTER_BACKGROUND_ACTIVITY: + case DAVIS_CONFIG_DVS_FILTER_BACKGROUND_ACTIVITY_DELTAT: + if (handle->info.dvsHasBackgroundActivityFilter) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_DVS, paramAddr, param)); + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_DVS_TEST_EVENT_GENERATOR_ENABLE: + if (handle->info.dvsHasTestEventGenerator) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_DVS, paramAddr, param)); + } + else { + return (false); + } + break; + + default: + return (false); + break; + } + break; + + case DAVIS_CONFIG_APS: + switch (paramAddr) { + case DAVIS_CONFIG_APS_RUN: + case DAVIS_CONFIG_APS_RESET_READ: + case DAVIS_CONFIG_APS_WAIT_ON_TRANSFER_STALL: + case DAVIS_CONFIG_APS_ROW_SETTLE: + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + break; + + case DAVIS_CONFIG_APS_RESET_SETTLE: + case DAVIS_CONFIG_APS_COLUMN_SETTLE: + case DAVIS_CONFIG_APS_NULL_SETTLE: + // Not supported on DAVIS RGB APS state machine. + if (!IS_DAVISRGB(handle->info.chipID)) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_APS_START_COLUMN_0: + case DAVIS_CONFIG_APS_END_COLUMN_0: + if (state->apsInvertXY) { + // Convert to row if X/Y inverted. + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, U8T(paramAddr + 1), param)); + } + else { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + break; + + case DAVIS_CONFIG_APS_START_ROW_0: + case DAVIS_CONFIG_APS_END_ROW_0: + if (state->apsInvertXY) { + // Convert to column if X/Y inverted. + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, U8T(paramAddr - 1), param)); + } + else { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + break; + + case DAVIS_CONFIG_APS_EXPOSURE: + case DAVIS_CONFIG_APS_FRAME_DELAY: + // Exposure and Frame Delay are in µs, must be converted to native FPGA cycles + // by multiplying with ADC clock value. + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, + param * U16T(handle->info.adcClock))); + break; + + case DAVIS_CONFIG_APS_GLOBAL_SHUTTER: + if (handle->info.apsHasGlobalShutter) { + // Keep in sync with chip config module GlobalShutter parameter. + if (!spiConfigSend(state->deviceHandle, DAVIS_CONFIG_CHIP, DAVIS128_CONFIG_CHIP_GLOBAL_SHUTTER, + param)) { + return (false); + } + + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + else { + return (false); + } + break; + + // TODO: no support on host-side for QuadROI and multi-frame decoding. + case DAVIS_CONFIG_APS_START_COLUMN_1: + case DAVIS_CONFIG_APS_END_COLUMN_1: + case DAVIS_CONFIG_APS_START_COLUMN_2: + case DAVIS_CONFIG_APS_END_COLUMN_2: + case DAVIS_CONFIG_APS_START_COLUMN_3: + case DAVIS_CONFIG_APS_END_COLUMN_3: + if (handle->info.apsHasQuadROI) { + if (state->apsInvertXY) { + // Convert to row if X/Y inverted. + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, U8T(paramAddr + 1), param)); + } + else { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_APS_START_ROW_1: + case DAVIS_CONFIG_APS_END_ROW_1: + case DAVIS_CONFIG_APS_START_ROW_2: + case DAVIS_CONFIG_APS_END_ROW_2: + case DAVIS_CONFIG_APS_START_ROW_3: + case DAVIS_CONFIG_APS_END_ROW_3: + if (handle->info.apsHasQuadROI) { + if (state->apsInvertXY) { + // Convert to column if X/Y inverted. + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, U8T(paramAddr - 1), param)); + } + else { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_APS_USE_INTERNAL_ADC: + case DAVIS_CONFIG_APS_SAMPLE_ENABLE: + case DAVIS_CONFIG_APS_SAMPLE_SETTLE: + case DAVIS_CONFIG_APS_RAMP_RESET: + case DAVIS_CONFIG_APS_RAMP_SHORT_RESET: + case DAVIS_CONFIG_APS_ADC_TEST_MODE: + if (handle->info.apsHasInternalADC) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + else { + return (false); + } + break; + + case DAVISRGB_CONFIG_APS_TRANSFER: + case DAVISRGB_CONFIG_APS_RSFDSETTLE: + case DAVISRGB_CONFIG_APS_GSPDRESET: + case DAVISRGB_CONFIG_APS_GSRESETFALL: + case DAVISRGB_CONFIG_APS_GSTXFALL: + case DAVISRGB_CONFIG_APS_GSFDRESET: + // Support for DAVISRGB extra timing parameters. + if (IS_DAVISRGB(handle->info.chipID)) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_APS_SNAPSHOT: { + // Use multi-command VR for more efficient implementation of snapshot, + // that also guarantees returning to the default state (not running). + if (param) { + uint8_t spiMultiConfig[6 + 6] = { 0 }; + + spiMultiConfig[0] = DAVIS_CONFIG_APS; + spiMultiConfig[1] = DAVIS_CONFIG_APS_RUN; + spiMultiConfig[2] = 0x00; + spiMultiConfig[3] = 0x00; + spiMultiConfig[4] = 0x00; + spiMultiConfig[5] = 0x01; + + spiMultiConfig[6] = DAVIS_CONFIG_APS; + spiMultiConfig[7] = DAVIS_CONFIG_APS_RUN; + spiMultiConfig[8] = 0x00; + spiMultiConfig[9] = 0x00; + spiMultiConfig[10] = 0x00; + spiMultiConfig[11] = 0x00; + + return (libusb_control_transfer(state->deviceHandle, + LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_FPGA_CONFIG_MULTIPLE, 2, 0, spiMultiConfig, sizeof(spiMultiConfig), 0) + == sizeof(spiMultiConfig)); + } + break; + } + + default: + return (false); + break; + } + break; + + case DAVIS_CONFIG_IMU: + switch (paramAddr) { + case DAVIS_CONFIG_IMU_RUN: + case DAVIS_CONFIG_IMU_TEMP_STANDBY: + case DAVIS_CONFIG_IMU_ACCEL_STANDBY: + case DAVIS_CONFIG_IMU_GYRO_STANDBY: + case DAVIS_CONFIG_IMU_LP_CYCLE: + case DAVIS_CONFIG_IMU_LP_WAKEUP: + case DAVIS_CONFIG_IMU_SAMPLE_RATE_DIVIDER: + case DAVIS_CONFIG_IMU_DIGITAL_LOW_PASS_FILTER: + case DAVIS_CONFIG_IMU_ACCEL_FULL_SCALE: + case DAVIS_CONFIG_IMU_GYRO_FULL_SCALE: + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_IMU, paramAddr, param)); + break; + + default: + return (false); + break; + } + break; + + case DAVIS_CONFIG_EXTINPUT: + switch (paramAddr) { + case DAVIS_CONFIG_EXTINPUT_RUN_DETECTOR: + case DAVIS_CONFIG_EXTINPUT_DETECT_RISING_EDGES: + case DAVIS_CONFIG_EXTINPUT_DETECT_FALLING_EDGES: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSES: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH: + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_EXTINPUT, paramAddr, param)); + break; + + case DAVIS_CONFIG_EXTINPUT_RUN_GENERATOR: + case DAVIS_CONFIG_EXTINPUT_GENERATE_USE_CUSTOM_SIGNAL: + case DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_POLARITY: + case DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_INTERVAL: + case DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_LENGTH: + case DAVIS_CONFIG_EXTINPUT_GENERATE_INJECT_ON_RISING_EDGE: + case DAVIS_CONFIG_EXTINPUT_GENERATE_INJECT_ON_FALLING_EDGE: + if (handle->info.extInputHasGenerator) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_EXTINPUT, paramAddr, param)); + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_EXTINPUT_RUN_DETECTOR1: + case DAVIS_CONFIG_EXTINPUT_DETECT_RISING_EDGES1: + case DAVIS_CONFIG_EXTINPUT_DETECT_FALLING_EDGES1: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSES1: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY1: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH1: + case DAVIS_CONFIG_EXTINPUT_RUN_DETECTOR2: + case DAVIS_CONFIG_EXTINPUT_DETECT_RISING_EDGES2: + case DAVIS_CONFIG_EXTINPUT_DETECT_FALLING_EDGES2: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSES2: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY2: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH2: + if (handle->info.extInputHasExtraDetectors) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_EXTINPUT, paramAddr, param)); + } + else { + return (false); + } + break; + + default: + return (false); + break; + } + break; + + case DAVIS_CONFIG_BIAS: // Also DAVIS_CONFIG_CHIP (starts at address 128). + if (paramAddr < 128) { + // BIASING (DAVIS_CONFIG_BIAS). + if (IS_DAVIS240(handle->info.chipID)) { + // DAVIS240 uses the old bias generator with 22 branches, and uses all of them. + if (paramAddr < 22) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_BIAS, paramAddr, param)); + } + } + else if (IS_DAVIS128(handle->info.chipID) || IS_DAVIS208(handle->info.chipID) + || IS_DAVIS346(handle->info.chipID) || IS_DAVIS640(handle->info.chipID)) { + // All new DAVISes use the new bias generator with 37 branches. + switch (paramAddr) { + // Same and shared between all of the above chips. + case DAVIS128_CONFIG_BIAS_APSOVERFLOWLEVEL: + case DAVIS128_CONFIG_BIAS_APSCAS: + case DAVIS128_CONFIG_BIAS_ADCREFHIGH: + case DAVIS128_CONFIG_BIAS_ADCREFLOW: + case DAVIS128_CONFIG_BIAS_LOCALBUFBN: + case DAVIS128_CONFIG_BIAS_PADFOLLBN: + case DAVIS128_CONFIG_BIAS_DIFFBN: + case DAVIS128_CONFIG_BIAS_ONBN: + case DAVIS128_CONFIG_BIAS_OFFBN: + case DAVIS128_CONFIG_BIAS_PIXINVBN: + case DAVIS128_CONFIG_BIAS_PRBP: + case DAVIS128_CONFIG_BIAS_PRSFBP: + case DAVIS128_CONFIG_BIAS_REFRBP: + case DAVIS128_CONFIG_BIAS_READOUTBUFBP: + case DAVIS128_CONFIG_BIAS_APSROSFBN: + case DAVIS128_CONFIG_BIAS_ADCCOMPBP: + case DAVIS128_CONFIG_BIAS_COLSELLOWBN: + case DAVIS128_CONFIG_BIAS_DACBUFBP: + case DAVIS128_CONFIG_BIAS_LCOLTIMEOUTBN: + case DAVIS128_CONFIG_BIAS_AEPDBN: + case DAVIS128_CONFIG_BIAS_AEPUXBP: + case DAVIS128_CONFIG_BIAS_AEPUYBP: + case DAVIS128_CONFIG_BIAS_IFREFRBN: + case DAVIS128_CONFIG_BIAS_IFTHRBN: + case DAVIS128_CONFIG_BIAS_BIASBUFFER: + case DAVIS128_CONFIG_BIAS_SSP: + case DAVIS128_CONFIG_BIAS_SSN: + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_BIAS, paramAddr, param)); + break; + + case DAVIS346_CONFIG_BIAS_ADCTESTVOLTAGE: + // Only supported by DAVIS346 and DAVIS640 chips. + if (IS_DAVIS346(handle->info.chipID) || IS_DAVIS640(handle->info.chipID)) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_BIAS, paramAddr, param)); + } + break; + + case DAVIS208_CONFIG_BIAS_RESETHIGHPASS: + case DAVIS208_CONFIG_BIAS_REFSS: + case DAVIS208_CONFIG_BIAS_REGBIASBP: + case DAVIS208_CONFIG_BIAS_REFSSBN: + // Only supported by DAVIS208 chips. + if (IS_DAVIS208(handle->info.chipID)) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_BIAS, paramAddr, param)); + } + break; + + default: + return (false); + break; + } + } + else if (IS_DAVISRGB(handle->info.chipID)) { + // DAVISRGB also uses the 37 branches bias generator, with different values. + switch (paramAddr) { + case DAVISRGB_CONFIG_BIAS_APSCAS: + case DAVISRGB_CONFIG_BIAS_OVG1LO: + case DAVISRGB_CONFIG_BIAS_OVG2LO: + case DAVISRGB_CONFIG_BIAS_TX2OVG2HI: + case DAVISRGB_CONFIG_BIAS_GND07: + case DAVISRGB_CONFIG_BIAS_ADCTESTVOLTAGE: + case DAVISRGB_CONFIG_BIAS_ADCREFHIGH: + case DAVISRGB_CONFIG_BIAS_ADCREFLOW: + case DAVISRGB_CONFIG_BIAS_IFREFRBN: + case DAVISRGB_CONFIG_BIAS_IFTHRBN: + case DAVISRGB_CONFIG_BIAS_LOCALBUFBN: + case DAVISRGB_CONFIG_BIAS_PADFOLLBN: + case DAVISRGB_CONFIG_BIAS_PIXINVBN: + case DAVISRGB_CONFIG_BIAS_DIFFBN: + case DAVISRGB_CONFIG_BIAS_ONBN: + case DAVISRGB_CONFIG_BIAS_OFFBN: + case DAVISRGB_CONFIG_BIAS_PRBP: + case DAVISRGB_CONFIG_BIAS_PRSFBP: + case DAVISRGB_CONFIG_BIAS_REFRBP: + case DAVISRGB_CONFIG_BIAS_ARRAYBIASBUFFERBN: + case DAVISRGB_CONFIG_BIAS_ARRAYLOGICBUFFERBN: + case DAVISRGB_CONFIG_BIAS_FALLTIMEBN: + case DAVISRGB_CONFIG_BIAS_RISETIMEBP: + case DAVISRGB_CONFIG_BIAS_READOUTBUFBP: + case DAVISRGB_CONFIG_BIAS_APSROSFBN: + case DAVISRGB_CONFIG_BIAS_ADCCOMPBP: + case DAVISRGB_CONFIG_BIAS_DACBUFBP: + case DAVISRGB_CONFIG_BIAS_LCOLTIMEOUTBN: + case DAVISRGB_CONFIG_BIAS_AEPDBN: + case DAVISRGB_CONFIG_BIAS_AEPUXBP: + case DAVISRGB_CONFIG_BIAS_AEPUYBP: + case DAVISRGB_CONFIG_BIAS_BIASBUFFER: + case DAVISRGB_CONFIG_BIAS_SSP: + case DAVISRGB_CONFIG_BIAS_SSN: + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_BIAS, paramAddr, param)); + break; + + default: + return (false); + break; + } + } + } + else { + // CHIP CONFIGURATION (DAVIS_CONFIG_CHIP). + switch (paramAddr) { + // Chip configuration common to all chips. + case DAVIS128_CONFIG_CHIP_DIGITALMUX0: + case DAVIS128_CONFIG_CHIP_DIGITALMUX1: + case DAVIS128_CONFIG_CHIP_DIGITALMUX2: + case DAVIS128_CONFIG_CHIP_DIGITALMUX3: + case DAVIS128_CONFIG_CHIP_ANALOGMUX0: + case DAVIS128_CONFIG_CHIP_ANALOGMUX1: + case DAVIS128_CONFIG_CHIP_ANALOGMUX2: + case DAVIS128_CONFIG_CHIP_BIASMUX0: + case DAVIS128_CONFIG_CHIP_RESETCALIBNEURON: + case DAVIS128_CONFIG_CHIP_TYPENCALIBNEURON: + case DAVIS128_CONFIG_CHIP_RESETTESTPIXEL: + case DAVIS128_CONFIG_CHIP_AERNAROW: + case DAVIS128_CONFIG_CHIP_USEAOUT: + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_CHIP, paramAddr, param)); + break; + + case DAVIS240_CONFIG_CHIP_SPECIALPIXELCONTROL: + // Only supported by DAVIS240 A/B chips. + if (IS_DAVIS240A(handle->info.chipID) || IS_DAVIS240B(handle->info.chipID)) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_CHIP, paramAddr, param)); + } + break; + + case DAVIS128_CONFIG_CHIP_GLOBAL_SHUTTER: + // Only supported by some chips. + if (handle->info.apsHasGlobalShutter) { + // Keep in sync with APS module GlobalShutter parameter. + if (!spiConfigSend(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_GLOBAL_SHUTTER, + param)) { + return (false); + } + + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_CHIP, paramAddr, param)); + } + break; + + case DAVIS128_CONFIG_CHIP_SELECTGRAYCOUNTER: + // Only supported by the new DAVIS chips. + if (IS_DAVIS128( + handle->info.chipID) || IS_DAVIS208(handle->info.chipID) || IS_DAVIS346(handle->info.chipID) + || IS_DAVIS640(handle->info.chipID) || IS_DAVISRGB(handle->info.chipID)) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_CHIP, paramAddr, param)); + } + break; + + case DAVIS346_CONFIG_CHIP_TESTADC: + // Only supported by some of the new DAVIS chips. + if (IS_DAVIS346( + handle->info.chipID) || IS_DAVIS640(handle->info.chipID) || IS_DAVISRGB(handle->info.chipID)) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_CHIP, paramAddr, param)); + } + break; + + case DAVISRGB_CONFIG_CHIP_ADJUSTOVG1LO: // Also DAVIS208_CONFIG_CHIP_SELECTPREAMPAVG. + case DAVISRGB_CONFIG_CHIP_ADJUSTOVG2LO: // Also DAVIS208_CONFIG_CHIP_SELECTBIASREFSS. + case DAVISRGB_CONFIG_CHIP_ADJUSTTX2OVG2HI: // Also DAVIS208_CONFIG_CHIP_SELECTSENSE. + // Only supported by DAVIS208 and DAVISRGB. + if (IS_DAVIS208(handle->info.chipID) || IS_DAVISRGB(handle->info.chipID)) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_CHIP, paramAddr, param)); + } + break; + + case DAVIS208_CONFIG_CHIP_SELECTPOSFB: + case DAVIS208_CONFIG_CHIP_SELECTHIGHPASS: + // Only supported by DAVIS208. + if (IS_DAVIS208(handle->info.chipID)) { + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_CHIP, paramAddr, param)); + } + break; + + default: + return (false); + break; + } + } + + return (false); + break; + + case DAVIS_CONFIG_SYSINFO: + // No SystemInfo parameters can ever be set! + return (false); + break; + + case DAVIS_CONFIG_USB: + switch (paramAddr) { + case DAVIS_CONFIG_USB_RUN: + case DAVIS_CONFIG_USB_EARLY_PACKET_DELAY: + return (spiConfigSend(state->deviceHandle, DAVIS_CONFIG_USB, paramAddr, param)); + break; + + default: + return (false); + break; + } + break; + + default: + return (false); + break; + } + + return (true); +} + +bool davisCommonConfigGet(davisHandle handle, int8_t modAddr, uint8_t paramAddr, uint32_t *param) { + davisState state = &handle->state; + + switch (modAddr) { + case CAER_HOST_CONFIG_USB: + switch (paramAddr) { + case CAER_HOST_CONFIG_USB_BUFFER_NUMBER: + *param = U32T(atomic_load(&state->usbBufferNumber)); + break; + + case CAER_HOST_CONFIG_USB_BUFFER_SIZE: + *param = U32T(atomic_load(&state->usbBufferSize)); + break; + + default: + return (false); + break; + } + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE: + switch (paramAddr) { + case CAER_HOST_CONFIG_DATAEXCHANGE_BUFFER_SIZE: + *param = U32T(atomic_load(&state->dataExchangeBufferSize)); + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE_BLOCKING: + *param = atomic_load(&state->dataExchangeBlocking); + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE_START_PRODUCERS: + *param = atomic_load(&state->dataExchangeStartProducers); + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE_STOP_PRODUCERS: + *param = atomic_load(&state->dataExchangeStopProducers); + break; + + default: + return (false); + break; + } + break; + + case CAER_HOST_CONFIG_PACKETS: + switch (paramAddr) { + case CAER_HOST_CONFIG_PACKETS_MAX_CONTAINER_PACKET_SIZE: + *param = U32T(atomic_load(&state->maxPacketContainerPacketSize)); + break; + + case CAER_HOST_CONFIG_PACKETS_MAX_CONTAINER_INTERVAL: + *param = U32T(atomic_load(&state->maxPacketContainerInterval)); + break; + + default: + return (false); + break; + } + break; + + case DAVIS_CONFIG_MUX: + switch (paramAddr) { + case DAVIS_CONFIG_MUX_RUN: + case DAVIS_CONFIG_MUX_TIMESTAMP_RUN: + case DAVIS_CONFIG_MUX_FORCE_CHIP_BIAS_ENABLE: + case DAVIS_CONFIG_MUX_DROP_DVS_ON_TRANSFER_STALL: + case DAVIS_CONFIG_MUX_DROP_APS_ON_TRANSFER_STALL: + case DAVIS_CONFIG_MUX_DROP_IMU_ON_TRANSFER_STALL: + case DAVIS_CONFIG_MUX_DROP_EXTINPUT_ON_TRANSFER_STALL: + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_MUX, paramAddr, param)); + break; + + case DAVIS_CONFIG_MUX_TIMESTAMP_RESET: + // Always false because it's an impulse, it resets itself automatically. + *param = false; + break; + + default: + return (false); + break; + } + break; + + case DAVIS_CONFIG_DVS: + switch (paramAddr) { + case DAVIS_CONFIG_DVS_SIZE_COLUMNS: + case DAVIS_CONFIG_DVS_SIZE_ROWS: + case DAVIS_CONFIG_DVS_ORIENTATION_INFO: + case DAVIS_CONFIG_DVS_RUN: + case DAVIS_CONFIG_DVS_ACK_DELAY_ROW: + case DAVIS_CONFIG_DVS_ACK_DELAY_COLUMN: + case DAVIS_CONFIG_DVS_ACK_EXTENSION_ROW: + case DAVIS_CONFIG_DVS_ACK_EXTENSION_COLUMN: + case DAVIS_CONFIG_DVS_WAIT_ON_TRANSFER_STALL: + case DAVIS_CONFIG_DVS_FILTER_ROW_ONLY_EVENTS: + case DAVIS_CONFIG_DVS_EXTERNAL_AER_CONTROL: + case DAVIS_CONFIG_DVS_HAS_PIXEL_FILTER: + case DAVIS_CONFIG_DVS_HAS_BACKGROUND_ACTIVITY_FILTER: + case DAVIS_CONFIG_DVS_HAS_TEST_EVENT_GENERATOR: + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_DVS, paramAddr, param)); + break; + + case DAVIS_CONFIG_DVS_FILTER_PIXEL_0_ROW: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_1_ROW: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_2_ROW: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_3_ROW: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_4_ROW: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_5_ROW: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_6_ROW: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_7_ROW: + if (handle->info.dvsHasPixelFilter) { + if (handle->state.dvsInvertXY) { + // Convert to column if X/Y inverted. + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_DVS, U8T(paramAddr + 1), param)); + } + else { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_DVS, paramAddr, param)); + } + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_DVS_FILTER_PIXEL_0_COLUMN: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_1_COLUMN: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_2_COLUMN: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_3_COLUMN: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_4_COLUMN: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_5_COLUMN: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_6_COLUMN: + case DAVIS_CONFIG_DVS_FILTER_PIXEL_7_COLUMN: + if (handle->info.dvsHasPixelFilter) { + if (handle->state.dvsInvertXY) { + // Convert to row if X/Y inverted. + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_DVS, U8T(paramAddr - 1), param)); + } + else { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_DVS, paramAddr, param)); + } + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_DVS_FILTER_BACKGROUND_ACTIVITY: + case DAVIS_CONFIG_DVS_FILTER_BACKGROUND_ACTIVITY_DELTAT: + if (handle->info.dvsHasBackgroundActivityFilter) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_DVS, paramAddr, param)); + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_DVS_TEST_EVENT_GENERATOR_ENABLE: + if (handle->info.dvsHasTestEventGenerator) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_DVS, paramAddr, param)); + } + else { + return (false); + } + break; + + default: + return (false); + break; + } + break; + + case DAVIS_CONFIG_APS: + switch (paramAddr) { + case DAVIS_CONFIG_APS_SIZE_COLUMNS: + case DAVIS_CONFIG_APS_SIZE_ROWS: + case DAVIS_CONFIG_APS_ORIENTATION_INFO: + case DAVIS_CONFIG_APS_COLOR_FILTER: + case DAVIS_CONFIG_APS_RUN: + case DAVIS_CONFIG_APS_RESET_READ: + case DAVIS_CONFIG_APS_WAIT_ON_TRANSFER_STALL: + case DAVIS_CONFIG_APS_ROW_SETTLE: + case DAVIS_CONFIG_APS_HAS_GLOBAL_SHUTTER: + case DAVIS_CONFIG_APS_HAS_QUAD_ROI: + case DAVIS_CONFIG_APS_HAS_EXTERNAL_ADC: + case DAVIS_CONFIG_APS_HAS_INTERNAL_ADC: + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + break; + + case DAVIS_CONFIG_APS_START_COLUMN_0: + case DAVIS_CONFIG_APS_END_COLUMN_0: + if (state->apsInvertXY) { + // Convert to row if X/Y inverted. + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, U8T(paramAddr + 1), param)); + } + else { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + break; + + case DAVIS_CONFIG_APS_START_ROW_0: + case DAVIS_CONFIG_APS_END_ROW_0: + if (state->apsInvertXY) { + // Convert to column if X/Y inverted. + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, U8T(paramAddr - 1), param)); + } + else { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + break; + + case DAVIS_CONFIG_APS_RESET_SETTLE: + case DAVIS_CONFIG_APS_COLUMN_SETTLE: + case DAVIS_CONFIG_APS_NULL_SETTLE: + // Not supported on DAVIS RGB APS state machine. + if (!IS_DAVISRGB(handle->info.chipID)) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_APS_EXPOSURE: + case DAVIS_CONFIG_APS_FRAME_DELAY: { + // Exposure and Frame Delay are in µs, must be converted from native FPGA cycles + // by dividing with ADC clock value. + uint32_t cyclesValue = 0; + if (!spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, &cyclesValue)) { + return (false); + } + + *param = cyclesValue / U16T(handle->info.adcClock); + + return (true); + break; + } + + case DAVIS_CONFIG_APS_GLOBAL_SHUTTER: + if (handle->info.apsHasGlobalShutter) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_APS_START_COLUMN_1: + case DAVIS_CONFIG_APS_END_COLUMN_1: + case DAVIS_CONFIG_APS_START_COLUMN_2: + case DAVIS_CONFIG_APS_END_COLUMN_2: + case DAVIS_CONFIG_APS_START_COLUMN_3: + case DAVIS_CONFIG_APS_END_COLUMN_3: + if (handle->info.apsHasQuadROI) { + if (state->apsInvertXY) { + // Convert to row if X/Y inverted. + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, U8T(paramAddr + 1), param)); + } + else { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_APS_START_ROW_1: + case DAVIS_CONFIG_APS_END_ROW_1: + case DAVIS_CONFIG_APS_START_ROW_2: + case DAVIS_CONFIG_APS_END_ROW_2: + case DAVIS_CONFIG_APS_START_ROW_3: + case DAVIS_CONFIG_APS_END_ROW_3: + if (handle->info.apsHasQuadROI) { + if (state->apsInvertXY) { + // Convert to column if X/Y inverted. + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, U8T(paramAddr - 1), param)); + } + else { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_APS_USE_INTERNAL_ADC: + case DAVIS_CONFIG_APS_SAMPLE_ENABLE: + case DAVIS_CONFIG_APS_SAMPLE_SETTLE: + case DAVIS_CONFIG_APS_RAMP_RESET: + case DAVIS_CONFIG_APS_RAMP_SHORT_RESET: + case DAVIS_CONFIG_APS_ADC_TEST_MODE: + if (handle->info.apsHasInternalADC) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + else { + return (false); + } + break; + + case DAVISRGB_CONFIG_APS_TRANSFER: + case DAVISRGB_CONFIG_APS_RSFDSETTLE: + case DAVISRGB_CONFIG_APS_GSPDRESET: + case DAVISRGB_CONFIG_APS_GSRESETFALL: + case DAVISRGB_CONFIG_APS_GSTXFALL: + case DAVISRGB_CONFIG_APS_GSFDRESET: + // Support for DAVISRGB extra timing parameters. + if (IS_DAVISRGB(handle->info.chipID)) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, paramAddr, param)); + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_APS_SNAPSHOT: + // Always false because it's an impulse, it resets itself automatically. + *param = false; + break; + + default: + return (false); + break; + } + break; + + case DAVIS_CONFIG_IMU: + switch (paramAddr) { + case DAVIS_CONFIG_IMU_RUN: + case DAVIS_CONFIG_IMU_TEMP_STANDBY: + case DAVIS_CONFIG_IMU_ACCEL_STANDBY: + case DAVIS_CONFIG_IMU_GYRO_STANDBY: + case DAVIS_CONFIG_IMU_LP_CYCLE: + case DAVIS_CONFIG_IMU_LP_WAKEUP: + case DAVIS_CONFIG_IMU_SAMPLE_RATE_DIVIDER: + case DAVIS_CONFIG_IMU_DIGITAL_LOW_PASS_FILTER: + case DAVIS_CONFIG_IMU_ACCEL_FULL_SCALE: + case DAVIS_CONFIG_IMU_GYRO_FULL_SCALE: + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_IMU, paramAddr, param)); + break; + + default: + return (false); + break; + } + break; + + case DAVIS_CONFIG_EXTINPUT: + switch (paramAddr) { + case DAVIS_CONFIG_EXTINPUT_RUN_DETECTOR: + case DAVIS_CONFIG_EXTINPUT_DETECT_RISING_EDGES: + case DAVIS_CONFIG_EXTINPUT_DETECT_FALLING_EDGES: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSES: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH: + case DAVIS_CONFIG_EXTINPUT_HAS_GENERATOR: + case DAVIS_CONFIG_EXTINPUT_HAS_EXTRA_DETECTORS: + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_EXTINPUT, paramAddr, param)); + break; + + case DAVIS_CONFIG_EXTINPUT_RUN_GENERATOR: + case DAVIS_CONFIG_EXTINPUT_GENERATE_USE_CUSTOM_SIGNAL: + case DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_POLARITY: + case DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_INTERVAL: + case DAVIS_CONFIG_EXTINPUT_GENERATE_PULSE_LENGTH: + case DAVIS_CONFIG_EXTINPUT_GENERATE_INJECT_ON_RISING_EDGE: + case DAVIS_CONFIG_EXTINPUT_GENERATE_INJECT_ON_FALLING_EDGE: + if (handle->info.extInputHasGenerator) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_EXTINPUT, paramAddr, param)); + } + else { + return (false); + } + break; + + case DAVIS_CONFIG_EXTINPUT_RUN_DETECTOR1: + case DAVIS_CONFIG_EXTINPUT_DETECT_RISING_EDGES1: + case DAVIS_CONFIG_EXTINPUT_DETECT_FALLING_EDGES1: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSES1: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY1: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH1: + case DAVIS_CONFIG_EXTINPUT_RUN_DETECTOR2: + case DAVIS_CONFIG_EXTINPUT_DETECT_RISING_EDGES2: + case DAVIS_CONFIG_EXTINPUT_DETECT_FALLING_EDGES2: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSES2: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_POLARITY2: + case DAVIS_CONFIG_EXTINPUT_DETECT_PULSE_LENGTH2: + if (handle->info.extInputHasExtraDetectors) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_EXTINPUT, paramAddr, param)); + } + else { + return (false); + } + break; + + default: + return (false); + break; + } + break; + + case DAVIS_CONFIG_BIAS: // Also DAVIS_CONFIG_CHIP (starts at address 128). + if (paramAddr < 128) { + // BIASING (DAVIS_CONFIG_BIAS). + if (IS_DAVIS240(handle->info.chipID)) { + // DAVIS240 uses the old bias generator with 22 branches, and uses all of them. + if (paramAddr < 22) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_BIAS, paramAddr, param)); + } + } + else if (IS_DAVIS128(handle->info.chipID) || IS_DAVIS208(handle->info.chipID) + || IS_DAVIS346(handle->info.chipID) || IS_DAVIS640(handle->info.chipID)) { + // All new DAVISes use the new bias generator with 37 branches. + switch (paramAddr) { + // Same and shared between all of the above chips. + case DAVIS128_CONFIG_BIAS_APSOVERFLOWLEVEL: + case DAVIS128_CONFIG_BIAS_APSCAS: + case DAVIS128_CONFIG_BIAS_ADCREFHIGH: + case DAVIS128_CONFIG_BIAS_ADCREFLOW: + case DAVIS128_CONFIG_BIAS_LOCALBUFBN: + case DAVIS128_CONFIG_BIAS_PADFOLLBN: + case DAVIS128_CONFIG_BIAS_DIFFBN: + case DAVIS128_CONFIG_BIAS_ONBN: + case DAVIS128_CONFIG_BIAS_OFFBN: + case DAVIS128_CONFIG_BIAS_PIXINVBN: + case DAVIS128_CONFIG_BIAS_PRBP: + case DAVIS128_CONFIG_BIAS_PRSFBP: + case DAVIS128_CONFIG_BIAS_REFRBP: + case DAVIS128_CONFIG_BIAS_READOUTBUFBP: + case DAVIS128_CONFIG_BIAS_APSROSFBN: + case DAVIS128_CONFIG_BIAS_ADCCOMPBP: + case DAVIS128_CONFIG_BIAS_COLSELLOWBN: + case DAVIS128_CONFIG_BIAS_DACBUFBP: + case DAVIS128_CONFIG_BIAS_LCOLTIMEOUTBN: + case DAVIS128_CONFIG_BIAS_AEPDBN: + case DAVIS128_CONFIG_BIAS_AEPUXBP: + case DAVIS128_CONFIG_BIAS_AEPUYBP: + case DAVIS128_CONFIG_BIAS_IFREFRBN: + case DAVIS128_CONFIG_BIAS_IFTHRBN: + case DAVIS128_CONFIG_BIAS_BIASBUFFER: + case DAVIS128_CONFIG_BIAS_SSP: + case DAVIS128_CONFIG_BIAS_SSN: + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_BIAS, paramAddr, param)); + break; + + case DAVIS346_CONFIG_BIAS_ADCTESTVOLTAGE: + // Only supported by DAVIS346 and DAVIS640 chips. + if (IS_DAVIS346(handle->info.chipID) || IS_DAVIS640(handle->info.chipID)) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_BIAS, paramAddr, param)); + } + break; + + case DAVIS208_CONFIG_BIAS_RESETHIGHPASS: + case DAVIS208_CONFIG_BIAS_REFSS: + case DAVIS208_CONFIG_BIAS_REGBIASBP: + case DAVIS208_CONFIG_BIAS_REFSSBN: + // Only supported by DAVIS208 chips. + if (IS_DAVIS208(handle->info.chipID)) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_BIAS, paramAddr, param)); + } + break; + + default: + return (false); + break; + } + } + else if (IS_DAVISRGB(handle->info.chipID)) { + // DAVISRGB also uses the 37 branches bias generator, with different values. + switch (paramAddr) { + case DAVISRGB_CONFIG_BIAS_APSCAS: + case DAVISRGB_CONFIG_BIAS_OVG1LO: + case DAVISRGB_CONFIG_BIAS_OVG2LO: + case DAVISRGB_CONFIG_BIAS_TX2OVG2HI: + case DAVISRGB_CONFIG_BIAS_GND07: + case DAVISRGB_CONFIG_BIAS_ADCTESTVOLTAGE: + case DAVISRGB_CONFIG_BIAS_ADCREFHIGH: + case DAVISRGB_CONFIG_BIAS_ADCREFLOW: + case DAVISRGB_CONFIG_BIAS_IFREFRBN: + case DAVISRGB_CONFIG_BIAS_IFTHRBN: + case DAVISRGB_CONFIG_BIAS_LOCALBUFBN: + case DAVISRGB_CONFIG_BIAS_PADFOLLBN: + case DAVISRGB_CONFIG_BIAS_PIXINVBN: + case DAVISRGB_CONFIG_BIAS_DIFFBN: + case DAVISRGB_CONFIG_BIAS_ONBN: + case DAVISRGB_CONFIG_BIAS_OFFBN: + case DAVISRGB_CONFIG_BIAS_PRBP: + case DAVISRGB_CONFIG_BIAS_PRSFBP: + case DAVISRGB_CONFIG_BIAS_REFRBP: + case DAVISRGB_CONFIG_BIAS_ARRAYBIASBUFFERBN: + case DAVISRGB_CONFIG_BIAS_ARRAYLOGICBUFFERBN: + case DAVISRGB_CONFIG_BIAS_FALLTIMEBN: + case DAVISRGB_CONFIG_BIAS_RISETIMEBP: + case DAVISRGB_CONFIG_BIAS_READOUTBUFBP: + case DAVISRGB_CONFIG_BIAS_APSROSFBN: + case DAVISRGB_CONFIG_BIAS_ADCCOMPBP: + case DAVISRGB_CONFIG_BIAS_DACBUFBP: + case DAVISRGB_CONFIG_BIAS_LCOLTIMEOUTBN: + case DAVISRGB_CONFIG_BIAS_AEPDBN: + case DAVISRGB_CONFIG_BIAS_AEPUXBP: + case DAVISRGB_CONFIG_BIAS_AEPUYBP: + case DAVISRGB_CONFIG_BIAS_BIASBUFFER: + case DAVISRGB_CONFIG_BIAS_SSP: + case DAVISRGB_CONFIG_BIAS_SSN: + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_BIAS, paramAddr, param)); + break; + + default: + return (false); + break; + } + } + } + else { + // CHIP CONFIGURATION (DAVIS_CONFIG_CHIP). + switch (paramAddr) { + // Chip configuration common to all chips. + case DAVIS128_CONFIG_CHIP_DIGITALMUX0: + case DAVIS128_CONFIG_CHIP_DIGITALMUX1: + case DAVIS128_CONFIG_CHIP_DIGITALMUX2: + case DAVIS128_CONFIG_CHIP_DIGITALMUX3: + case DAVIS128_CONFIG_CHIP_ANALOGMUX0: + case DAVIS128_CONFIG_CHIP_ANALOGMUX1: + case DAVIS128_CONFIG_CHIP_ANALOGMUX2: + case DAVIS128_CONFIG_CHIP_BIASMUX0: + case DAVIS128_CONFIG_CHIP_RESETCALIBNEURON: + case DAVIS128_CONFIG_CHIP_TYPENCALIBNEURON: + case DAVIS128_CONFIG_CHIP_RESETTESTPIXEL: + case DAVIS128_CONFIG_CHIP_AERNAROW: + case DAVIS128_CONFIG_CHIP_USEAOUT: + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_CHIP, paramAddr, param)); + break; + + case DAVIS240_CONFIG_CHIP_SPECIALPIXELCONTROL: + // Only supported by DAVIS240 A/B chips. + if (IS_DAVIS240A(handle->info.chipID) || IS_DAVIS240B(handle->info.chipID)) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_CHIP, paramAddr, param)); + } + break; + + case DAVIS128_CONFIG_CHIP_GLOBAL_SHUTTER: + // Only supported by some chips. + if (handle->info.apsHasGlobalShutter) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_CHIP, paramAddr, param)); + } + break; + + case DAVIS128_CONFIG_CHIP_SELECTGRAYCOUNTER: + // Only supported by the new DAVIS chips. + if (IS_DAVIS128( + handle->info.chipID) || IS_DAVIS208(handle->info.chipID) || IS_DAVIS346(handle->info.chipID) + || IS_DAVIS640(handle->info.chipID) || IS_DAVISRGB(handle->info.chipID)) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_CHIP, paramAddr, param)); + } + break; + + case DAVIS346_CONFIG_CHIP_TESTADC: + // Only supported by some of the new DAVIS chips. + if (IS_DAVIS346( + handle->info.chipID) || IS_DAVIS640(handle->info.chipID) || IS_DAVISRGB(handle->info.chipID)) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_CHIP, paramAddr, param)); + } + break; + + case DAVISRGB_CONFIG_CHIP_ADJUSTOVG1LO: // Also DAVIS208_CONFIG_CHIP_SELECTPREAMPAVG. + case DAVISRGB_CONFIG_CHIP_ADJUSTOVG2LO: // Also DAVIS208_CONFIG_CHIP_SELECTBIASREFSS. + case DAVISRGB_CONFIG_CHIP_ADJUSTTX2OVG2HI: // Also DAVIS208_CONFIG_CHIP_SELECTSENSE. + // Only supported by DAVIS208 and DAVISRGB. + if (IS_DAVIS208(handle->info.chipID) || IS_DAVISRGB(handle->info.chipID)) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_CHIP, paramAddr, param)); + } + break; + + case DAVIS208_CONFIG_CHIP_SELECTPOSFB: + case DAVIS208_CONFIG_CHIP_SELECTHIGHPASS: + // Only supported by DAVIS208. + if (IS_DAVIS208(handle->info.chipID)) { + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_CHIP, paramAddr, param)); + } + break; + + default: + return (false); + break; + } + } + + return (false); + break; + + case DAVIS_CONFIG_SYSINFO: + switch (paramAddr) { + case DAVIS_CONFIG_SYSINFO_LOGIC_VERSION: + case DAVIS_CONFIG_SYSINFO_CHIP_IDENTIFIER: + case DAVIS_CONFIG_SYSINFO_DEVICE_IS_MASTER: + case DAVIS_CONFIG_SYSINFO_LOGIC_CLOCK: + case DAVIS_CONFIG_SYSINFO_ADC_CLOCK: + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_SYSINFO, paramAddr, param)); + break; + + default: + return (false); + break; + } + break; + + case DAVIS_CONFIG_USB: + switch (paramAddr) { + case DAVIS_CONFIG_USB_RUN: + case DAVIS_CONFIG_USB_EARLY_PACKET_DELAY: + return (spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_USB, paramAddr, param)); + break; + + default: + return (false); + break; + } + break; + + default: + return (false); + break; + } + + return (true); +} + +bool davisCommonDataStart(caerDeviceHandle cdh, void (*dataNotifyIncrease)(void *ptr), + void (*dataNotifyDecrease)(void *ptr), void *dataNotifyUserPtr, void (*dataShutdownNotify)(void *ptr), + void *dataShutdownUserPtr) { + davisHandle handle = (davisHandle) cdh; + davisState state = &handle->state; + + // Store new data available/not available anymore call-backs. + state->dataNotifyIncrease = dataNotifyIncrease; + state->dataNotifyDecrease = dataNotifyDecrease; + state->dataNotifyUserPtr = dataNotifyUserPtr; + state->dataShutdownNotify = dataShutdownNotify; + state->dataShutdownUserPtr = dataShutdownUserPtr; + + // Set wanted time interval to uninitialized. Getting the first TS or TS_RESET + // will then set this correctly. + state->currentPacketContainerCommitTimestamp = -1; + + // Initialize RingBuffer. + state->dataExchangeBuffer = ringBufferInit(atomic_load(&state->dataExchangeBufferSize)); + if (state->dataExchangeBuffer == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to initialize data exchange buffer."); + return (false); + } + + // Allocate packets. + state->currentPacketContainer = caerEventPacketContainerAllocate(DAVIS_EVENT_TYPES); + if (state->currentPacketContainer == NULL) { + freeAllDataMemory(state); + + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate event packet container."); + return (false); + } + + state->currentPolarityPacket = caerPolarityEventPacketAllocate(DAVIS_POLARITY_DEFAULT_SIZE, + I16T(handle->info.deviceID), 0); + if (state->currentPolarityPacket == NULL) { + freeAllDataMemory(state); + + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate polarity event packet."); + return (false); + } + + state->currentSpecialPacket = caerSpecialEventPacketAllocate(DAVIS_SPECIAL_DEFAULT_SIZE, + I16T(handle->info.deviceID), 0); + if (state->currentSpecialPacket == NULL) { + freeAllDataMemory(state); + + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate special event packet."); + return (false); + } + + state->currentFramePacket = caerFrameEventPacketAllocate(DAVIS_FRAME_DEFAULT_SIZE, I16T(handle->info.deviceID), 0, + state->apsSizeX, state->apsSizeY, 1); + if (state->currentFramePacket == NULL) { + freeAllDataMemory(state); + + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate frame event packet."); + return (false); + } + + // Allocate memory for the current FrameEvents. Use contiguous memory for all ROI FrameEvents. + size_t eventSize = sizeof(struct caer_frame_event) + + ((size_t) state->apsSizeX * (size_t) state->apsSizeY * APS_ADC_CHANNELS * sizeof(uint16_t)); + + state->currentFrameEvent[0] = calloc(APS_ROI_REGIONS_MAX, eventSize); + if (state->currentFrameEvent[0] == NULL) { + freeAllDataMemory(state); + + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate ROI frame events."); + return (false); + } + + for (size_t i = 1; i < APS_ROI_REGIONS_MAX; i++) { + // Assign the right memory offset to the pointers into the block that + // contains all the ROI FrameEvents. + state->currentFrameEvent[i] = (caerFrameEvent) (((uint8_t*) state->currentFrameEvent[0]) + (i * eventSize)); + } + + state->currentIMU6Packet = caerIMU6EventPacketAllocate(DAVIS_IMU_DEFAULT_SIZE, I16T(handle->info.deviceID), 0); + if (state->currentIMU6Packet == NULL) { + freeAllDataMemory(state); + + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate IMU6 event packet."); + return (false); + } + + state->apsCurrentResetFrame = calloc((size_t) (state->apsSizeX * state->apsSizeY * APS_ADC_CHANNELS), + sizeof(uint16_t)); + if (state->apsCurrentResetFrame == NULL) { + freeAllDataMemory(state); + + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate APS reset frame memory."); + return (false); + } + + // Default IMU settings (for event parsing). + uint32_t param32 = 0; + + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_IMU, DAVIS_CONFIG_IMU_ACCEL_FULL_SCALE, ¶m32); + state->imuAccelScale = calculateIMUAccelScale(U8T(param32)); + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_IMU, DAVIS_CONFIG_IMU_GYRO_FULL_SCALE, ¶m32); + state->imuGyroScale = calculateIMUGyroScale(U8T(param32)); + + // Default APS settings (for event parsing). + uint32_t param32start = 0; + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_START_COLUMN_0, ¶m32start); + + // If StartColumn0 is bigger or equal to APS size X, disable ROI region 0. + if (param32start < U32T(state->apsSizeX)) { + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_END_COLUMN_0, ¶m32); + + state->apsROISizeX[0] = U16T(param32 + 1 - param32start); + state->apsROIPositionX[0] = U16T(param32start); + + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_START_ROW_0, ¶m32start); + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_END_ROW_0, ¶m32); + + state->apsROISizeY[0] = U16T(param32 + 1 - param32start); + state->apsROIPositionY[0] = U16T(param32start); + } + else { + // Disable ROI region 0 by setting all parameters to zero. + state->apsROISizeX[0] = state->apsROIPositionX[0] = 0; + state->apsROISizeY[0] = state->apsROIPositionY[0] = 0; + } + + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_GLOBAL_SHUTTER, ¶m32); + state->apsGlobalShutter = param32; + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_RESET_READ, ¶m32); + state->apsResetRead = param32; + + if ((errno = thrd_create(&state->dataAcquisitionThread, &davisDataAcquisitionThread, handle)) != thrd_success) { + freeAllDataMemory(state); + + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to start data acquisition thread. Error: %d.", + errno); + return (false); + } + + // Wait for the data acquisition thread to be ready. + while (!atomic_load_explicit(&state->dataAcquisitionThreadRun, memory_order_relaxed)) { + ; + } + + return (true); +} + +bool davisCommonDataStop(caerDeviceHandle cdh) { + davisHandle handle = (davisHandle) cdh; + davisState state = &handle->state; + + // Stop data acquisition thread. + if (atomic_load(&state->dataExchangeStopProducers)) { + // Disable data transfer on USB end-point 2. Reverse order of enabling. + davisCommonConfigSet(handle, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_RUN_DETECTOR, false); + davisCommonConfigSet(handle, DAVIS_CONFIG_IMU, DAVIS_CONFIG_IMU_RUN, false); + davisCommonConfigSet(handle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_RUN, false); + davisCommonConfigSet(handle, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_RUN, false); + davisCommonConfigSet(handle, DAVIS_CONFIG_MUX, DAVIS_CONFIG_MUX_FORCE_CHIP_BIAS_ENABLE, false); // Ensure chip turns off. + davisCommonConfigSet(handle, DAVIS_CONFIG_MUX, DAVIS_CONFIG_MUX_TIMESTAMP_RUN, false); // Turn off timestamping too. + davisCommonConfigSet(handle, DAVIS_CONFIG_MUX, DAVIS_CONFIG_MUX_RUN, false); + davisCommonConfigSet(handle, DAVIS_CONFIG_USB, DAVIS_CONFIG_USB_RUN, false); + } + + atomic_store(&state->dataAcquisitionThreadRun, false); + + // Wait for data acquisition thread to terminate... + if ((errno = thrd_join(state->dataAcquisitionThread, NULL)) != thrd_success) { + // This should never happen! + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to join data acquisition thread. Error: %d.", + errno); + return (false); + } + + // Empty ringbuffer. + caerEventPacketContainer container; + while ((container = ringBufferGet(state->dataExchangeBuffer)) != NULL) { + // Notify data-not-available call-back. + if (state->dataNotifyDecrease != NULL) { + state->dataNotifyDecrease(state->dataNotifyUserPtr); + } + + // Free container, which will free its subordinate packets too. + caerEventPacketContainerFree(container); + } + + // Free current, uncommitted packets and ringbuffer. + freeAllDataMemory(state); + + // Reset packet positions. + state->currentPolarityPacketPosition = 0; + state->currentSpecialPacketPosition = 0; + state->currentFramePacketPosition = 0; + state->currentIMU6PacketPosition = 0; + + // Reset private composite events. 'currentFrameEvent' is taken care of in freeAllDataMemory(). + memset(&state->currentIMU6Event, 0, sizeof(struct caer_imu6_event)); + + return (true); +} + +caerEventPacketContainer davisCommonDataGet(caerDeviceHandle cdh) { + davisHandle handle = (davisHandle) cdh; + davisState state = &handle->state; + caerEventPacketContainer container = NULL; + + retry: container = ringBufferGet(state->dataExchangeBuffer); + + if (container != NULL) { + // Found an event container, return it and signal this piece of data + // is no longer available for later acquisition. + if (state->dataNotifyDecrease != NULL) { + state->dataNotifyDecrease(state->dataNotifyUserPtr); + } + + return (container); + } + + // Didn't find any event container, either report this or retry, depending + // on blocking setting. + if (atomic_load_explicit(&state->dataExchangeBlocking, memory_order_relaxed)) { + // Don't retry right away in a tight loop, back off and wait a little. + // If no data is available, sleep for a millisecond to avoid wasting resources. + struct timespec noDataSleep = { .tv_sec = 0, .tv_nsec = 1000000 }; + if (thrd_sleep(&noDataSleep, NULL) == 0) { + goto retry; + } + } + + // Nothing. + return (NULL); +} + +static bool spiConfigSend(libusb_device_handle *devHandle, uint8_t moduleAddr, uint8_t paramAddr, uint32_t param) { + uint8_t spiConfig[4] = { 0 }; + + spiConfig[0] = U8T(param >> 24); + spiConfig[1] = U8T(param >> 16); + spiConfig[2] = U8T(param >> 8); + spiConfig[3] = U8T(param >> 0); + + return (libusb_control_transfer(devHandle, + LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_FPGA_CONFIG, moduleAddr, paramAddr, spiConfig, sizeof(spiConfig), 0) == sizeof(spiConfig)); +} + +static bool spiConfigReceive(libusb_device_handle *devHandle, uint8_t moduleAddr, uint8_t paramAddr, uint32_t *param) { + uint8_t spiConfig[4] = { 0 }; + + if (libusb_control_transfer(devHandle, LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_FPGA_CONFIG, moduleAddr, paramAddr, spiConfig, sizeof(spiConfig), 0) != sizeof(spiConfig)) { + return (false); + } + + *param = 0; + *param |= U32T(spiConfig[0] << 24); + *param |= U32T(spiConfig[1] << 16); + *param |= U32T(spiConfig[2] << 8); + *param |= U32T(spiConfig[3] << 0); + + return (true); +} + +static libusb_device_handle *davisDeviceOpen(libusb_context *devContext, uint16_t devVID, uint16_t devPID, + uint8_t devType, uint8_t busNumber, uint8_t devAddress, const char *serialNumber, uint16_t requiredLogicRevision, + uint16_t requiredFirmwareVersion) { + libusb_device_handle *devHandle = NULL; + libusb_device **devicesList; + + ssize_t result = libusb_get_device_list(devContext, &devicesList); + + if (result >= 0) { + // Cycle thorough all discovered devices and find a match. + for (size_t i = 0; i < (size_t) result; i++) { + struct libusb_device_descriptor devDesc; + + if (libusb_get_device_descriptor(devicesList[i], &devDesc) != LIBUSB_SUCCESS) { + continue; + } + + // Check if this is the device we want (VID/PID). + if (devDesc.idVendor == devVID && devDesc.idProduct == devPID + && U8T((devDesc.bcdDevice & 0xFF00) >> 8) == devType) { + // Verify device firmware version. + if (U8T(devDesc.bcdDevice & 0x00FF) < requiredFirmwareVersion) { + caerLog(CAER_LOG_CRITICAL, __func__, + "Device firmware version too old. You have version %" PRIu8 "; but at least version %" PRIu16 " is required. Please updated by following the Flashy upgrade documentation at 'http://inilabs.com/support/reflashing/'.", + U8T(devDesc.bcdDevice & 0x00FF), requiredFirmwareVersion); + + continue; + } + + // If a USB port restriction is given, honor it. + if (busNumber > 0 && libusb_get_bus_number(devicesList[i]) != busNumber) { + caerLog(CAER_LOG_INFO, __func__, + "USB bus number restriction is present (%" PRIu8 "), this device didn't match it (%" PRIu8 ").", + busNumber, libusb_get_bus_number(devicesList[i])); + + continue; + } + + if (devAddress > 0 && libusb_get_device_address(devicesList[i]) != devAddress) { + caerLog(CAER_LOG_INFO, __func__, + "USB device address restriction is present (%" PRIu8 "), this device didn't match it (%" PRIu8 ").", + devAddress, libusb_get_device_address(devicesList[i])); + + continue; + } + + if (libusb_open(devicesList[i], &devHandle) != LIBUSB_SUCCESS) { + devHandle = NULL; + + continue; + } + + // Check the serial number restriction, if any is present. + if (serialNumber != NULL && !caerStrEquals(serialNumber, "")) { + char deviceSerialNumber[8 + 1] = { 0 }; + int getStringDescResult = libusb_get_string_descriptor_ascii(devHandle, devDesc.iSerialNumber, + (unsigned char *) deviceSerialNumber, 8 + 1); + + // Check serial number success and length. + if (getStringDescResult < 0 || getStringDescResult > 8) { + libusb_close(devHandle); + devHandle = NULL; + + continue; + } + + // Now check if the Serial Number matches. + if (!caerStrEquals(serialNumber, deviceSerialNumber)) { + libusb_close(devHandle); + devHandle = NULL; + + caerLog(CAER_LOG_INFO, __func__, + "USB serial number restriction is present (%s), this device didn't match it (%s).", + serialNumber, deviceSerialNumber); + + continue; + } + } + + // Check that the active configuration is set to number 1. If not, do so. + int activeConfiguration; + if (libusb_get_configuration(devHandle, &activeConfiguration) != LIBUSB_SUCCESS) { + libusb_close(devHandle); + devHandle = NULL; + + continue; + } + + if (activeConfiguration != 1) { + if (libusb_set_configuration(devHandle, 1) != LIBUSB_SUCCESS) { + libusb_close(devHandle); + devHandle = NULL; + + continue; + } + } + + // Claim interface 0 (default). + if (libusb_claim_interface(devHandle, 0) != LIBUSB_SUCCESS) { + libusb_close(devHandle); + devHandle = NULL; + + continue; + } + + // Communication with device open, get logic version information. + uint32_t param32 = 0; + + spiConfigReceive(devHandle, DAVIS_CONFIG_SYSINFO, DAVIS_CONFIG_SYSINFO_LOGIC_VERSION, ¶m32); + uint16_t logicVersion = U16T(param32); + + // Verify device logic version. + if (logicVersion < requiredLogicRevision) { + libusb_release_interface(devHandle, 0); + libusb_close(devHandle); + devHandle = NULL; + + caerLog(CAER_LOG_CRITICAL, __func__, + "Device logic revision too old. You have revision %" PRIu16 "; but at least revision %" PRIu16 " is required. Please updated by following the Flashy upgrade documentation at 'http://inilabs.com/support/reflashing/'.", + logicVersion, requiredLogicRevision); + + continue; + } + + // Found and configured it! + break; + } + } + + libusb_free_device_list(devicesList, true); + } + + return (devHandle); +} + +static void davisDeviceClose(libusb_device_handle *devHandle) { + // Release interface 0 (default). + libusb_release_interface(devHandle, 0); + + libusb_close(devHandle); +} + +static void davisAllocateTransfers(davisHandle handle, uint32_t bufferNum, uint32_t bufferSize) { + davisState state = &handle->state; + + // Set number of transfers and allocate memory for the main transfer array. + state->dataTransfers = calloc(bufferNum, sizeof(struct libusb_transfer *)); + if (state->dataTransfers == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, + "Failed to allocate memory for %" PRIu32 " libusb transfers. Error: %d.", bufferNum, errno); + return; + } + state->dataTransfersLength = bufferNum; + + // Allocate transfers and set them up. + for (size_t i = 0; i < bufferNum; i++) { + state->dataTransfers[i] = libusb_alloc_transfer(0); + if (state->dataTransfers[i] == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, + "Unable to allocate further libusb transfers (%zu of %" PRIu32 ").", i, bufferNum); + continue; + } + + // Create data buffer. + state->dataTransfers[i]->length = (int) bufferSize; + state->dataTransfers[i]->buffer = malloc(bufferSize); + if (state->dataTransfers[i]->buffer == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, + "Unable to allocate buffer for libusb transfer %zu. Error: %d.", i, errno); + + libusb_free_transfer(state->dataTransfers[i]); + state->dataTransfers[i] = NULL; + + continue; + } + + // Initialize Transfer. + state->dataTransfers[i]->dev_handle = state->deviceHandle; + state->dataTransfers[i]->endpoint = DAVIS_DATA_ENDPOINT; + state->dataTransfers[i]->type = LIBUSB_TRANSFER_TYPE_BULK; + state->dataTransfers[i]->callback = &davisLibUsbCallback; + state->dataTransfers[i]->user_data = handle; + state->dataTransfers[i]->timeout = 0; + state->dataTransfers[i]->flags = LIBUSB_TRANSFER_FREE_BUFFER; + + if ((errno = libusb_submit_transfer(state->dataTransfers[i])) == LIBUSB_SUCCESS) { + state->activeDataTransfers++; + } + else { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, + "Unable to submit libusb transfer %zu. Error: %s (%d).", i, libusb_strerror(errno), errno); + + // The transfer buffer is freed automatically here thanks to + // the LIBUSB_TRANSFER_FREE_BUFFER flag set above. + libusb_free_transfer(state->dataTransfers[i]); + state->dataTransfers[i] = NULL; + + continue; + } + } + + if (state->activeDataTransfers == 0) { + // Didn't manage to allocate any USB transfers, free array memory and log failure. + free(state->dataTransfers); + state->dataTransfers = NULL; + state->dataTransfersLength = 0; + + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Unable to allocate any libusb transfers."); + } +} + +static void davisDeallocateTransfers(davisHandle handle) { + davisState state = &handle->state; + + // Cancel all current transfers first. + for (size_t i = 0; i < state->dataTransfersLength; i++) { + if (state->dataTransfers[i] != NULL) { + errno = libusb_cancel_transfer(state->dataTransfers[i]); + if (errno != LIBUSB_SUCCESS && errno != LIBUSB_ERROR_NOT_FOUND) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, + "Unable to cancel libusb transfer %zu. Error: %s (%d).", i, libusb_strerror(errno), errno); + // Proceed with trying to cancel all transfers regardless of errors. + } + } + } + + // Wait for all transfers to go away (0.1 seconds timeout). + struct timeval te = { .tv_sec = 0, .tv_usec = 100000 }; + + while (state->activeDataTransfers > 0) { + libusb_handle_events_timeout(state->deviceContext, &te); + } + + // The buffers and transfers have been deallocated in the callback. + // Only the transfers array remains, which we free here. + free(state->dataTransfers); + state->dataTransfers = NULL; + state->dataTransfersLength = 0; +} + +static void LIBUSB_CALL davisLibUsbCallback(struct libusb_transfer *transfer) { + davisHandle handle = transfer->user_data; + davisState state = &handle->state; + + if (transfer->status == LIBUSB_TRANSFER_COMPLETED) { + // Handle data. + davisEventTranslator(handle, transfer->buffer, (size_t) transfer->actual_length); + } + + if (transfer->status != LIBUSB_TRANSFER_CANCELLED && transfer->status != LIBUSB_TRANSFER_NO_DEVICE) { + // Submit transfer again. + if (libusb_submit_transfer(transfer) == LIBUSB_SUCCESS) { + return; + } + } + + // Cannot recover (cancelled, no device, or other critical error). + // Signal this by adjusting the counter, free and exit. + state->activeDataTransfers--; + for (size_t i = 0; i < state->dataTransfersLength; i++) { + // Remove from list, so we don't try to cancel it later on. + if (state->dataTransfers[i] == transfer) { + state->dataTransfers[i] = NULL; + } + } + libusb_free_transfer(transfer); +} + +#define TS_WRAP_ADD 0x8000 + +static inline int64_t generateFullTimestamp(int32_t tsOverflow, int32_t timestamp) { + return (I64T((U64T(tsOverflow) << TS_OVERFLOW_SHIFT) | U64T(timestamp))); +} + +static inline void initContainerCommitTimestamp(davisState state) { + if (state->currentPacketContainerCommitTimestamp == -1) { + state->currentPacketContainerCommitTimestamp = state->currentTimestamp + + atomic_load_explicit(&state->maxPacketContainerInterval, memory_order_relaxed) - 1; + } +} + +static void davisEventTranslator(davisHandle handle, uint8_t *buffer, size_t bytesSent) { + davisState state = &handle->state; + + // Return right away if not running anymore. This prevents useless work if many + // buffers are still waiting when shut down, as well as incorrect event sequences + // if a TS_RESET is stuck on ring-buffer commit further down, and detects shut-down; + // then any subsequent buffers should also detect shut-down and not be handled. + if (!atomic_load_explicit(&state->dataAcquisitionThreadRun, memory_order_relaxed)) { + return; + } + + // Truncate off any extra partial event. + if ((bytesSent & 0x01) != 0) { + caerLog(CAER_LOG_ALERT, handle->info.deviceString, + "%zu bytes received via USB, which is not a multiple of two.", bytesSent); + bytesSent &= (size_t) ~0x01; + } + + for (size_t i = 0; i < bytesSent; i += 2) { + // Allocate new packets for next iteration as needed. + if (state->currentPacketContainer == NULL) { + state->currentPacketContainer = caerEventPacketContainerAllocate(DAVIS_EVENT_TYPES); + if (state->currentPacketContainer == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate event packet container."); + return; + } + } + + if (state->currentPolarityPacket == NULL) { + state->currentPolarityPacket = caerPolarityEventPacketAllocate( + DAVIS_POLARITY_DEFAULT_SIZE, I16T(handle->info.deviceID), state->wrapOverflow); + if (state->currentPolarityPacket == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate polarity event packet."); + return; + } + } + else if (state->currentPolarityPacketPosition + >= caerEventPacketHeaderGetEventCapacity((caerEventPacketHeader) state->currentPolarityPacket)) { + // If not committed, let's check if any of the packets has reached its maximum + // capacity limit. If yes, we grow them to accomodate new events. + caerPolarityEventPacket grownPacket = (caerPolarityEventPacket) caerGenericEventPacketGrow( + (caerEventPacketHeader) state->currentPolarityPacket, state->currentPolarityPacketPosition * 2); + if (grownPacket == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to grow polarity event packet."); + return; + } + + state->currentPolarityPacket = grownPacket; + } + + if (state->currentSpecialPacket == NULL) { + state->currentSpecialPacket = caerSpecialEventPacketAllocate( + DAVIS_SPECIAL_DEFAULT_SIZE, I16T(handle->info.deviceID), state->wrapOverflow); + if (state->currentSpecialPacket == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate special event packet."); + return; + } + } + else if (state->currentSpecialPacketPosition + >= caerEventPacketHeaderGetEventCapacity((caerEventPacketHeader) state->currentSpecialPacket)) { + // If not committed, let's check if any of the packets has reached its maximum + // capacity limit. If yes, we grow them to accomodate new events. + caerSpecialEventPacket grownPacket = (caerSpecialEventPacket) caerGenericEventPacketGrow( + (caerEventPacketHeader) state->currentSpecialPacket, state->currentSpecialPacketPosition * 2); + if (grownPacket == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to grow special event packet."); + return; + } + + state->currentSpecialPacket = grownPacket; + } + + if (state->currentFramePacket == NULL) { + state->currentFramePacket = caerFrameEventPacketAllocate( + DAVIS_FRAME_DEFAULT_SIZE, I16T(handle->info.deviceID), state->wrapOverflow, state->apsSizeX, + state->apsSizeY, 1); + if (state->currentFramePacket == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate frame event packet."); + return; + } + } + else if (state->currentFramePacketPosition + >= caerEventPacketHeaderGetEventCapacity((caerEventPacketHeader) state->currentFramePacket)) { + // If not committed, let's check if any of the packets has reached its maximum + // capacity limit. If yes, we grow them to accomodate new events. + caerFrameEventPacket grownPacket = (caerFrameEventPacket) caerGenericEventPacketGrow( + (caerEventPacketHeader) state->currentFramePacket, state->currentFramePacketPosition * 2); + if (grownPacket == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to grow frame event packet."); + return; + } + + state->currentFramePacket = grownPacket; + } + + if (state->currentIMU6Packet == NULL) { + state->currentIMU6Packet = caerIMU6EventPacketAllocate( + DAVIS_IMU_DEFAULT_SIZE, I16T(handle->info.deviceID), state->wrapOverflow); + if (state->currentIMU6Packet == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate IMU6 event packet."); + return; + } + } + else if (state->currentIMU6PacketPosition + >= caerEventPacketHeaderGetEventCapacity((caerEventPacketHeader) state->currentIMU6Packet)) { + // If not committed, let's check if any of the packets has reached its maximum + // capacity limit. If yes, we grow them to accomodate new events. + caerIMU6EventPacket grownPacket = (caerIMU6EventPacket) caerGenericEventPacketGrow( + (caerEventPacketHeader) state->currentIMU6Packet, state->currentIMU6PacketPosition * 2); + if (grownPacket == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to grow IMU6 event packet."); + return; + } + + state->currentIMU6Packet = grownPacket; + } + + bool tsReset = false; + bool tsBigWrap = false; + + uint16_t event = le16toh(*((uint16_t * ) (&buffer[i]))); + + // Check if timestamp. + if ((event & 0x8000) != 0) { + // Is a timestamp! Expand to 32 bits. (Tick is 1µs already.) + state->lastTimestamp = state->currentTimestamp; + state->currentTimestamp = state->wrapAdd + (event & 0x7FFF); + initContainerCommitTimestamp(state); + + // Check monotonicity of timestamps. + checkStrictMonotonicTimestamp(handle); + } + else { + // Look at the code, to determine event and data type. + uint8_t code = U8T((event & 0x7000) >> 12); + uint16_t data = (event & 0x0FFF); + + switch (code) { + case 0: // Special event + switch (data) { + case 0: // Ignore this, but log it. + caerLog(CAER_LOG_ERROR, handle->info.deviceString, "Caught special reserved event!"); + break; + + case 1: { // Timetamp reset + state->wrapOverflow = 0; + state->wrapAdd = 0; + state->lastTimestamp = 0; + state->currentTimestamp = 0; + state->currentPacketContainerCommitTimestamp = -1; + initContainerCommitTimestamp(state); + + caerLog(CAER_LOG_INFO, handle->info.deviceString, "Timestamp reset event received."); + + // Defer timestamp reset event to later, so we commit it + // alone, in its own packet. + // Commit packets when doing a reset to clearly separate them. + tsReset = true; + + // Update Master/Slave status on incoming TS resets. Done in main thread + // to avoid deadlock inside callback. + atomic_fetch_or(&state->dataAcquisitionThreadConfigUpdate, 1 << 1); + + break; + } + + case 2: { // External input (falling edge) + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "External input (falling edge) event received."); + + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, EXTERNAL_INPUT_FALLING_EDGE); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + break; + } + + case 3: { // External input (rising edge) + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "External input (rising edge) event received."); + + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, EXTERNAL_INPUT_RISING_EDGE); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + break; + } + + case 4: { // External input (pulse) + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "External input (pulse) event received."); + + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, EXTERNAL_INPUT_PULSE); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + break; + } + + case 5: { // IMU Start (6 axes) + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "IMU6 Start event received."); + + state->imuIgnoreEvents = false; + state->imuCount = 0; + + memset(&state->currentIMU6Event, 0, sizeof(struct caer_imu6_event)); + + break; + } + + case 7: { // IMU End + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "IMU End event received."); + if (state->imuIgnoreEvents) { + break; + } + + if (state->imuCount == IMU6_COUNT) { + // Timestamp at event-stream insertion point. + caerIMU6EventSetTimestamp(&state->currentIMU6Event, state->currentTimestamp); + + caerIMU6EventValidate(&state->currentIMU6Event, state->currentIMU6Packet); + + // IMU6 and APS operate on an internal event and copy that to the actual output + // packet here, in the END state, for a reason: if a packetContainer, with all its + // packets, is committed due to hitting any of the triggers that are not TS reset + // or TS wrap-around related, like number of polarity events, the event in the packet + // would be left incomplete, and the event in the new packet would be corrupted. + // We could avoid this like for the TS reset/TS wrap-around case (see forceCommit) by + // just deleting that event, but these kinds of commits happen much more often and the + // possible data loss would be too significant. So instead we keep a private event, + // fill it, and then only copy it into the packet here in the END state, at which point + // the whole event is ready and cannot be broken/corrupted in any way anymore. + caerIMU6Event currentIMU6Event = caerIMU6EventPacketGetEvent(state->currentIMU6Packet, + state->currentIMU6PacketPosition); + memcpy(currentIMU6Event, &state->currentIMU6Event, sizeof(struct caer_imu6_event)); + state->currentIMU6PacketPosition++; + } + else { + caerLog(CAER_LOG_INFO, handle->info.deviceString, + "IMU End: failed to validate IMU sample count (%" PRIu8 "), discarding samples.", + state->imuCount); + } + break; + } + + case 8: { // APS Global Shutter Frame Start + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "APS GS Frame Start event received."); + state->apsIgnoreEvents = false; + state->apsGlobalShutter = true; + state->apsResetRead = true; + + initFrame(handle); + + break; + } + + case 9: { // APS Rolling Shutter Frame Start + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "APS RS Frame Start event received."); + state->apsIgnoreEvents = false; + state->apsGlobalShutter = false; + state->apsResetRead = true; + + initFrame(handle); + + break; + } + + case 10: { // APS Frame End + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "APS Frame End event received."); + if (state->apsIgnoreEvents) { + break; + } + + bool validFrame = true; + + for (size_t j = 0; j < APS_READOUT_TYPES_NUM; j++) { + int32_t checkValue = caerFrameEventGetLengthX(state->currentFrameEvent[0]); + + // Check main reset read against zero if disabled. + if (j == APS_READOUT_RESET && !state->apsResetRead) { + checkValue = 0; + } + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "APS Frame End: CountX[%zu] is %d.", + j, state->apsCountX[j]); + + if (state->apsCountX[j] != checkValue) { + caerLog(CAER_LOG_ERROR, handle->info.deviceString, + "APS Frame End - %zu: wrong column count %d detected, expected %d.", j, + state->apsCountX[j], checkValue); + validFrame = false; + } + } + + // Write out end of frame timestamp. + caerFrameEventSetTSEndOfFrame(state->currentFrameEvent[0], state->currentTimestamp); + + // Send APS info event out (as special event). + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, APS_FRAME_END); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + + // Validate event and advance frame packet position. + if (validFrame) { + caerFrameEventValidate(state->currentFrameEvent[0], state->currentFramePacket); + + // Invert X and Y axes if image from chip is inverted. + if (state->apsInvertXY) { + SWAP_VAR(int32_t, state->currentFrameEvent[0]->lengthX, + state->currentFrameEvent[0]->lengthY); + SWAP_VAR(int32_t, state->currentFrameEvent[0]->positionX, + state->currentFrameEvent[0]->positionY); + } + + // IMU6 and APS operate on an internal event and copy that to the actual output + // packet here, in the END state, for a reason: if a packetContainer, with all its + // packets, is committed due to hitting any of the triggers that are not TS reset + // or TS wrap-around related, like number of polarity events, the event in the packet + // would be left incomplete, and the event in the new packet would be corrupted. + // We could avoid this like for the TS reset/TS wrap-around case (see forceCommit) by + // just deleting that event, but these kinds of commits happen much more often and the + // possible data loss would be too significant. So instead we keep a private event, + // fill it, and then only copy it into the packet here in the END state, at which point + // the whole event is ready and cannot be broken/corrupted in any way anymore. + caerFrameEvent currentFrameEvent = caerFrameEventPacketGetEvent( + state->currentFramePacket, state->currentFramePacketPosition); + memcpy(currentFrameEvent, state->currentFrameEvent[0], + sizeof(struct caer_frame_event) + + caerFrameEventGetPixelsSize(state->currentFrameEvent[0])); + state->currentFramePacketPosition++; + } + + break; + } + + case 11: { // APS Reset Column Start + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "APS Reset Column Start event received."); + if (state->apsIgnoreEvents) { + break; + } + + state->apsCurrentReadoutType = APS_READOUT_RESET; + state->apsCountY[state->apsCurrentReadoutType] = 0; + + state->apsRGBPixelOffsetDirection = 0; + state->apsRGBPixelOffset = 1; // RGB support, first pixel of row always even. + + // The first Reset Column Read Start is also the start + // of the exposure for the RS. + if (!state->apsGlobalShutter && state->apsCountX[APS_READOUT_RESET] == 0) { + caerFrameEventSetTSStartOfExposure(state->currentFrameEvent[0], + state->currentTimestamp); + + // Send APS info event out (as special event). + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, APS_EXPOSURE_START); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + } + + break; + } + + case 12: { // APS Signal Column Start + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "APS Signal Column Start event received."); + if (state->apsIgnoreEvents) { + break; + } + + state->apsCurrentReadoutType = APS_READOUT_SIGNAL; + state->apsCountY[state->apsCurrentReadoutType] = 0; + + state->apsRGBPixelOffsetDirection = 0; + state->apsRGBPixelOffset = 1; // RGB support, first pixel of row always even. + + // The first Signal Column Read Start is also always the end + // of the exposure time, for both RS and GS. + if (state->apsCountX[APS_READOUT_SIGNAL] == 0) { + caerFrameEventSetTSEndOfExposure(state->currentFrameEvent[0], state->currentTimestamp); + + // Send APS info event out (as special event). + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, APS_EXPOSURE_END); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + } + + break; + } + + case 13: { // APS Column End + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "APS Column End event received."); + if (state->apsIgnoreEvents) { + break; + } + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "APS Column End: CountX[%d] is %d.", + state->apsCurrentReadoutType, state->apsCountX[state->apsCurrentReadoutType]); + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "APS Column End: CountY[%d] is %d.", + state->apsCurrentReadoutType, state->apsCountY[state->apsCurrentReadoutType]); + + if (state->apsCountY[state->apsCurrentReadoutType] + != caerFrameEventGetLengthY(state->currentFrameEvent[0])) { + caerLog(CAER_LOG_ERROR, handle->info.deviceString, + "APS Column End - %d: wrong row count %d detected, expected %d.", + state->apsCurrentReadoutType, state->apsCountY[state->apsCurrentReadoutType], + caerFrameEventGetLengthY(state->currentFrameEvent[0])); + } + + state->apsCountX[state->apsCurrentReadoutType]++; + + // The last Reset Column Read End is also the start + // of the exposure for the GS. + if (state->apsGlobalShutter && state->apsCurrentReadoutType == APS_READOUT_RESET + && state->apsCountX[APS_READOUT_RESET] + == caerFrameEventGetLengthX(state->currentFrameEvent[0])) { + caerFrameEventSetTSStartOfExposure(state->currentFrameEvent[0], + state->currentTimestamp); + + // Send APS info event out (as special event). + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, APS_EXPOSURE_START); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + } + + break; + } + + case 14: { // APS Global Shutter Frame Start with no Reset Read + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "APS GS NORST Frame Start event received."); + state->apsIgnoreEvents = false; + state->apsGlobalShutter = true; + state->apsResetRead = false; + + initFrame(handle); + + // If reset reads are disabled, the start of exposure is closest to + // the start of frame. + caerFrameEventSetTSStartOfExposure(state->currentFrameEvent[0], state->currentTimestamp); + + // No APS info event is sent out (as special event). Only one event + // per type can be sent out per cycle, and initFrame() already does + // that and sets APS_FRAME_START. + + break; + } + + case 15: { // APS Rolling Shutter Frame Start with no Reset Read + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "APS RS NORST Frame Start event received."); + state->apsIgnoreEvents = false; + state->apsGlobalShutter = false; + state->apsResetRead = false; + + initFrame(handle); + + // If reset reads are disabled, the start of exposure is closest to + // the start of frame. + caerFrameEventSetTSStartOfExposure(state->currentFrameEvent[0], state->currentTimestamp); + + // No APS info event is sent out (as special event). Only one event + // per type can be sent out per cycle, and initFrame() already does + // that and sets APS_FRAME_START. + + break; + } + + case 16: + case 17: + case 18: + case 19: + case 20: + case 21: + case 22: + case 23: + case 24: + case 25: + case 26: + case 27: + case 28: + case 29: + case 30: + case 31: { + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "IMU Scale Config event (%" PRIu16 ") received.", data); + if (state->imuIgnoreEvents) { + break; + } + + // Set correct IMU accel and gyro scales, used to interpret subsequent + // IMU samples from the device. + state->imuAccelScale = calculateIMUAccelScale((data >> 2) & 0x03); + state->imuGyroScale = calculateIMUGyroScale(data & 0x03); + + // At this point the IMU event count should be zero (reset by start). + if (state->imuCount != 0) { + caerLog(CAER_LOG_INFO, handle->info.deviceString, + "IMU Scale Config: previous IMU start event missed, attempting recovery."); + } + + // Increase IMU count by one, to a total of one (0+1=1). + // This way we can recover from the above error of missing start, and we can + // later discover if the IMU Scale Config event actually arrived itself. + state->imuCount = 1; + + break; + } + + case 32: { + // Next Misc8 APS ROI Size events will refer to ROI region 0. + // 0/1 used to distinguish between X and Y sizes. + state->apsROIUpdate = (0 << 2); + state->apsROISizeX[0] = state->apsROISizeY[0] = 0; + state->apsROIPositionX[0] = state->apsROIPositionY[0] = 0; + break; + } + + case 33: { + // Next Misc8 APS ROI Size events will refer to ROI region 1. + // 2/3 used to distinguish between X and Y sizes. + state->apsROIUpdate = (1 << 2); + state->apsROISizeX[1] = state->apsROISizeY[1] = 0; + state->apsROIPositionX[1] = state->apsROIPositionY[1] = 0; + break; + } + + case 34: { + // Next Misc8 APS ROI Size events will refer to ROI region 2. + // 4/5 used to distinguish between X and Y sizes. + state->apsROIUpdate = (2 << 2); + state->apsROISizeX[2] = state->apsROISizeY[2] = 0; + state->apsROIPositionX[2] = state->apsROIPositionY[2] = 0; + break; + } + + case 35: { + // Next Misc8 APS ROI Size events will refer to ROI region 3. + // 6/7 used to distinguish between X and Y sizes. + state->apsROIUpdate = (3 << 2); + state->apsROISizeX[3] = state->apsROISizeY[3] = 0; + state->apsROIPositionX[3] = state->apsROIPositionY[3] = 0; + break; + } + + case 36: { // External input 1 (falling edge) + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "External input 1 (falling edge) event received."); + + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, EXTERNAL_INPUT1_FALLING_EDGE); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + break; + } + + case 37: { // External input 1 (rising edge) + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "External input 1 (rising edge) event received."); + + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, EXTERNAL_INPUT1_RISING_EDGE); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + break; + } + + case 38: { // External input 1 (pulse) + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "External input 1 (pulse) event received."); + + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, EXTERNAL_INPUT1_PULSE); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + break; + } + + case 39: { // External input 2 (falling edge) + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "External input 2 (falling edge) event received."); + + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, EXTERNAL_INPUT2_FALLING_EDGE); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + break; + } + + case 40: { // External input 2 (rising edge) + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "External input 2 (rising edge) event received."); + + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, EXTERNAL_INPUT2_RISING_EDGE); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + break; + } + + case 41: { // External input 2 (pulse) + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "External input 2 (pulse) event received."); + + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, EXTERNAL_INPUT2_PULSE); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + break; + } + + case 42: { // External generator (falling edge) + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "External generator (falling edge) event received."); + + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, EXTERNAL_GENERATOR_FALLING_EDGE); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + break; + } + + case 43: { // External generator (rising edge) + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "External generator (rising edge) event received."); + + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, EXTERNAL_GENERATOR_RISING_EDGE); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + break; + } + + default: + caerLog(CAER_LOG_ERROR, handle->info.deviceString, + "Caught special event that can't be handled: %d.", data); + break; + } + break; + + case 1: // Y address + // Check range conformity. + if (data >= state->dvsSizeY) { + caerLog(CAER_LOG_ALERT, handle->info.deviceString, + "DVS: Y address out of range (0-%d): %" PRIu16 ".", state->dvsSizeY - 1, data); + break; // Skip invalid Y address (don't update lastY). + } + + if (state->dvsGotY) { + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + + // Timestamp at event-stream insertion point. + caerSpecialEventSetTimestamp(currentSpecialEvent, state->currentTimestamp); + caerSpecialEventSetType(currentSpecialEvent, DVS_ROW_ONLY); + caerSpecialEventSetData(currentSpecialEvent, state->dvsLastY); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "DVS: row-only event received for address Y=%" PRIu16 ".", state->dvsLastY); + } + + state->dvsLastY = data; + state->dvsGotY = true; + + break; + + case 2: // X address, Polarity OFF + case 3: { // X address, Polarity ON + // Check range conformity. + if (data >= state->dvsSizeX) { + caerLog(CAER_LOG_ALERT, handle->info.deviceString, + "DVS: X address out of range (0-%d): %" PRIu16 ".", state->dvsSizeX - 1, data); + break; // Skip invalid event. + } + + // Invert polarity for PixelParade high gain pixels (DavisSense), because of + // negative gain from pre-amplifier. + uint8_t polarity = ((IS_DAVIS208(handle->info.chipID)) && (data < 192)) ? U8T(~code) : (code); + + caerPolarityEvent currentPolarityEvent = caerPolarityEventPacketGetEvent( + state->currentPolarityPacket, state->currentPolarityPacketPosition); + + // Timestamp at event-stream insertion point. + caerPolarityEventSetTimestamp(currentPolarityEvent, state->currentTimestamp); + caerPolarityEventSetPolarity(currentPolarityEvent, (polarity & 0x01)); + if (state->dvsInvertXY) { + // Flip Y address to conform to CG format. + caerPolarityEventSetY(currentPolarityEvent, U16T((state->dvsSizeX - 1) - data)); + caerPolarityEventSetX(currentPolarityEvent, state->dvsLastY); + } + else { + // Flip Y address to conform to CG format. + caerPolarityEventSetY(currentPolarityEvent, U16T((state->dvsSizeY - 1) - state->dvsLastY)); + caerPolarityEventSetX(currentPolarityEvent, data); + } + caerPolarityEventValidate(currentPolarityEvent, state->currentPolarityPacket); + state->currentPolarityPacketPosition++; + + state->dvsGotY = false; + + break; + } + + case 4: { + if (state->apsIgnoreEvents) { + break; + } + + // Let's check that apsCountX is not above the maximum. This could happen + // if the maximum is a smaller number that comes from ROI, while we're still + // reading out a frame with a bigger, old size. + if (state->apsCountX[state->apsCurrentReadoutType] + >= caerFrameEventGetLengthX(state->currentFrameEvent[0])) { + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "APS ADC sample: column count is at maximum, discarding further samples."); + break; + } + + // Let's check that apsCountY is not above the maximum. This could happen + // if start/end of column events are discarded (no wait on transfer stall). + if (state->apsCountY[state->apsCurrentReadoutType] + >= caerFrameEventGetLengthY(state->currentFrameEvent[0])) { + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "APS ADC sample: row count is at maximum, discarding further samples."); + break; + } + + // If reset read, we store the values in a local array. If signal read, we + // store the final pixel value directly in the output frame event. We already + // do the subtraction between reset and signal here, to avoid carrying that + // around all the time and consuming memory. This way we can also only take + // infrequent reset reads and re-use them for multiple frames, which can heavily + // reduce traffic, and should not impact image quality heavily, at least in GS. + uint16_t xPos = + (state->apsFlipX) ? + (U16T( + caerFrameEventGetLengthX(state->currentFrameEvent[0]) - 1 + - state->apsCountX[state->apsCurrentReadoutType])) : + (U16T(state->apsCountX[state->apsCurrentReadoutType])); + uint16_t yPos = + (state->apsFlipY) ? + (U16T( + caerFrameEventGetLengthY(state->currentFrameEvent[0]) - 1 + - state->apsCountY[state->apsCurrentReadoutType])) : + (U16T(state->apsCountY[state->apsCurrentReadoutType])); + + if (IS_DAVISRGB(handle->info.chipID)) { + yPos = U16T(yPos + state->apsRGBPixelOffset); + } + + int32_t stride = 0; + + if (state->apsInvertXY) { + SWAP_VAR(uint16_t, xPos, yPos); + + stride = caerFrameEventGetLengthY(state->currentFrameEvent[0]); + + // Flip Y address to conform to CG format. + yPos = U16T((state->apsSizeX - 1) - yPos); + } + else { + stride = caerFrameEventGetLengthX(state->currentFrameEvent[0]); + + // Flip Y address to conform to CG format. + yPos = U16T((state->apsSizeY - 1) - yPos); + } + + size_t pixelPosition = (size_t) (yPos * stride) + xPos; + + if ((state->apsCurrentReadoutType == APS_READOUT_RESET + && !(IS_DAVISRGB(handle->info.chipID) && state->apsGlobalShutter)) + || (state->apsCurrentReadoutType == APS_READOUT_SIGNAL + && (IS_DAVISRGB(handle->info.chipID) && state->apsGlobalShutter))) { + state->apsCurrentResetFrame[pixelPosition] = data; + } + else { + int32_t pixelValue = 0; + + if (IS_DAVISRGB(handle->info.chipID) && state->apsGlobalShutter) { + // DAVIS RGB GS has inverted samples, signal read comes first + // and was stored above inside state->apsCurrentResetFrame. +#if APS_DEBUG_FRAME == 1 + // Reset read only. + pixelValue = (data); +#elif APS_DEBUG_FRAME == 2 + // Signal read only. + pixelValue = (state->apsCurrentResetFrame[pixelPosition]); +#else + // Both/CDS done. + pixelValue = (data - state->apsCurrentResetFrame[pixelPosition]); +#endif + } + else { +#if APS_DEBUG_FRAME == 1 + // Reset read only. + pixelValue = (state->apsCurrentResetFrame[pixelPosition]); +#elif APS_DEBUG_FRAME == 2 + // Signal read only. + pixelValue = (data); +#else + // Both/CDS done. + pixelValue = (state->apsCurrentResetFrame[pixelPosition] - data); +#endif + } + + // Normalize the ADC value to 16bit generic depth and check for underflow. + pixelValue = (pixelValue < 0) ? (0) : (pixelValue); + pixelValue = pixelValue << (16 - APS_ADC_DEPTH); + + caerFrameEventGetPixelArrayUnsafe(state->currentFrameEvent[0])[pixelPosition] = htole16( + U16T(pixelValue)); + } + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "APS ADC Sample: column=%" PRIu16 ", row=%" PRIu16 ", xPos=%" PRIu16 ", yPos=%" PRIu16 ", data=%" PRIu16 ".", + state->apsCountX[state->apsCurrentReadoutType], state->apsCountY[state->apsCurrentReadoutType], + xPos, yPos, data); + + state->apsCountY[state->apsCurrentReadoutType]++; + + // RGB support: first 320 pixels are even, then odd. + if (IS_DAVISRGB(handle->info.chipID)) { + if (state->apsRGBPixelOffsetDirection == 0) { // Increasing + state->apsRGBPixelOffset++; + + if (state->apsRGBPixelOffset == 321) { + // Switch to decreasing after last even pixel. + state->apsRGBPixelOffsetDirection = 1; + state->apsRGBPixelOffset = 318; + } + } + else { // Decreasing + state->apsRGBPixelOffset = I16T(state->apsRGBPixelOffset - 3); + } + } + + break; + } + + case 5: { + // Misc 8bit data, used currently only + // for IMU events in DAVIS FX3 boards. + uint8_t misc8Code = U8T((data & 0x0F00) >> 8); + uint8_t misc8Data = U8T(data & 0x00FF); + + switch (misc8Code) { + case 0: + if (state->imuIgnoreEvents) { + break; + } + + // Detect missing IMU end events. + if (state->imuCount >= IMU6_COUNT) { + caerLog(CAER_LOG_INFO, handle->info.deviceString, + "IMU data: IMU samples count is at maximum, discarding further samples."); + break; + } + + // IMU data event. + switch (state->imuCount) { + case 0: + caerLog(CAER_LOG_ERROR, handle->info.deviceString, + "IMU data: missing IMU Scale Config event. Parsing of IMU events will still be attempted, but be aware that Accel/Gyro scale conversions may be inaccurate."); + state->imuCount = 1; + // Fall through to next case, as if imuCount was equal to 1. + + case 1: + case 3: + case 5: + case 7: + case 9: + case 11: + case 13: + state->imuTmpData = misc8Data; + break; + + case 2: { + int16_t accelX = I16T((state->imuTmpData << 8) | misc8Data); + caerIMU6EventSetAccelX(&state->currentIMU6Event, accelX / state->imuAccelScale); + break; + } + + case 4: { + int16_t accelY = I16T((state->imuTmpData << 8) | misc8Data); + caerIMU6EventSetAccelY(&state->currentIMU6Event, accelY / state->imuAccelScale); + break; + } + + case 6: { + int16_t accelZ = I16T((state->imuTmpData << 8) | misc8Data); + caerIMU6EventSetAccelZ(&state->currentIMU6Event, accelZ / state->imuAccelScale); + break; + } + + // Temperature is signed. Formula for converting to °C: + // (SIGNED_VAL / 340) + 36.53 + case 8: { + int16_t temp = I16T((state->imuTmpData << 8) | misc8Data); + caerIMU6EventSetTemp(&state->currentIMU6Event, (temp / 340.0f) + 36.53f); + break; + } + + case 10: { + int16_t gyroX = I16T((state->imuTmpData << 8) | misc8Data); + caerIMU6EventSetGyroX(&state->currentIMU6Event, gyroX / state->imuGyroScale); + break; + } + + case 12: { + int16_t gyroY = I16T((state->imuTmpData << 8) | misc8Data); + caerIMU6EventSetGyroY(&state->currentIMU6Event, gyroY / state->imuGyroScale); + break; + } + + case 14: { + int16_t gyroZ = I16T((state->imuTmpData << 8) | misc8Data); + caerIMU6EventSetGyroZ(&state->currentIMU6Event, gyroZ / state->imuGyroScale); + break; + } + } + + state->imuCount++; + + break; + + case 1: + // APS ROI Size Part 1 (bits 15-8). + // Here we just store the temporary value, and use it again + // in the next case statement. + state->apsROITmpData = U16T(misc8Data << 8); + + break; + + case 2: { + // APS ROI Size Part 2 (bits 7-0). + // Here we just store the values and re-use the four fields + // sizeX/Y and positionX/Y to store endCol/Row and startCol/Row. + // We then recalculate all the right values and set everything + // up in START_FRAME. + size_t apsROIRegion = state->apsROIUpdate >> 2; + + switch (state->apsROIUpdate & 0x03) { + case 0: + // START COLUMN + state->apsROIPositionX[apsROIRegion] = U16T(state->apsROITmpData | misc8Data); + break; + + case 1: + // START ROW + state->apsROIPositionY[apsROIRegion] = U16T(state->apsROITmpData | misc8Data); + break; + + case 2: + // END COLUMN + state->apsROISizeX[apsROIRegion] = U16T(state->apsROITmpData | misc8Data); + break; + + case 3: + // END ROW + state->apsROISizeY[apsROIRegion] = U16T(state->apsROITmpData | misc8Data); + break; + + default: + break; + } + + // Jump to next type of APS info (col->row, start->end). + state->apsROIUpdate++; + + break; + } + + default: + caerLog(CAER_LOG_ERROR, handle->info.deviceString, + "Caught Misc8 event that can't be handled."); + break; + } + + break; + } + + case 7: { // Timestamp wrap + // Detect big timestamp wrap-around. + int64_t wrapJump = (TS_WRAP_ADD * data); + int64_t wrapSum = I64T(state->wrapAdd) + wrapJump; + + if (wrapSum > I64T(INT32_MAX)) { + // Reset wrapAdd at this point, so we can again + // start detecting overruns of the 32bit value. + // We reset not to zero, but to the remaining value after + // multiple wrap-jumps are taken into account. + int64_t wrapRemainder = wrapSum - I64T(INT32_MAX) - 1LL; + state->wrapAdd = I32T(wrapRemainder); + + state->lastTimestamp = 0; + state->currentTimestamp = state->wrapAdd; + + // Increment TSOverflow counter. + state->wrapOverflow++; + + caerSpecialEvent currentSpecialEvent = caerSpecialEventPacketGetEvent( + state->currentSpecialPacket, state->currentSpecialPacketPosition); + caerSpecialEventSetTimestamp(currentSpecialEvent, INT32_MAX); + caerSpecialEventSetType(currentSpecialEvent, TIMESTAMP_WRAP); + caerSpecialEventValidate(currentSpecialEvent, state->currentSpecialPacket); + state->currentSpecialPacketPosition++; + + // Commit packets to separate before wrap from after cleanly. + tsBigWrap = true; + } + else { + // Each wrap is 2^15 µs (~32ms), and we have + // to multiply it with the wrap counter, + // which is located in the data part of this + // event. + state->wrapAdd = I32T(wrapSum); + + state->lastTimestamp = state->currentTimestamp; + state->currentTimestamp = state->wrapAdd; + initContainerCommitTimestamp(state); + + // Check monotonicity of timestamps. + checkStrictMonotonicTimestamp(handle); + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, + "Timestamp wrap event received with multiplier of %" PRIu16 ".", data); + } + + break; + } + + default: + caerLog(CAER_LOG_ERROR, handle->info.deviceString, "Caught event that can't be handled."); + break; + } + } + + // Thresholds on which to trigger packet container commit. + // forceCommit is already defined above. + // Trigger if any of the global container-wide thresholds are met. + int32_t currentPacketContainerCommitSize = I32T( + atomic_load_explicit(&state->maxPacketContainerPacketSize, memory_order_relaxed)); + bool containerSizeCommit = (currentPacketContainerCommitSize > 0) + && ((state->currentPolarityPacketPosition >= currentPacketContainerCommitSize) + || (state->currentSpecialPacketPosition >= currentPacketContainerCommitSize) + || (state->currentFramePacketPosition >= currentPacketContainerCommitSize) + || (state->currentIMU6PacketPosition >= currentPacketContainerCommitSize)); + + bool containerTimeCommit = generateFullTimestamp(state->wrapOverflow, state->currentTimestamp) + > state->currentPacketContainerCommitTimestamp; + + // Commit packet containers to the ring-buffer, so they can be processed by the + // main-loop, when any of the required conditions are met. + if (tsReset || tsBigWrap || containerSizeCommit || containerTimeCommit) { + // One or more of the commit triggers are hit. Set the packet container up to contain + // any non-empty packets. Empty packets are not forwarded to save memory. + bool emptyContainerCommit = true; + + if (state->currentPolarityPacketPosition > 0) { + caerEventPacketContainerSetEventPacket(state->currentPacketContainer, POLARITY_EVENT, + (caerEventPacketHeader) state->currentPolarityPacket); + + state->currentPolarityPacket = NULL; + state->currentPolarityPacketPosition = 0; + emptyContainerCommit = false; + } + + if (state->currentSpecialPacketPosition > 0) { + caerEventPacketContainerSetEventPacket(state->currentPacketContainer, SPECIAL_EVENT, + (caerEventPacketHeader) state->currentSpecialPacket); + + state->currentSpecialPacket = NULL; + state->currentSpecialPacketPosition = 0; + emptyContainerCommit = false; + } + + if (state->currentFramePacketPosition > 0) { + caerEventPacketContainerSetEventPacket(state->currentPacketContainer, FRAME_EVENT, + (caerEventPacketHeader) state->currentFramePacket); + + state->currentFramePacket = NULL; + state->currentFramePacketPosition = 0; + emptyContainerCommit = false; + } + + if (state->currentIMU6PacketPosition > 0) { + caerEventPacketContainerSetEventPacket(state->currentPacketContainer, IMU6_EVENT, + (caerEventPacketHeader) state->currentIMU6Packet); + + state->currentIMU6Packet = NULL; + state->currentIMU6PacketPosition = 0; + emptyContainerCommit = false; + } + + if (tsReset || tsBigWrap) { + // Ignore all APS and IMU6 (composite) events, until a new APS or IMU6 + // Start event comes in, for the next packet. + // This is to correctly support the forced packet commits that a TS reset, + // or a TS big wrap, impose. Continuing to parse events would result + // in a corrupted state of the first event in the new packet, as it would + // be incomplete, incorrect and miss vital initialization data. + // See APS and IMU6 END states for more details on a related issue. + state->apsIgnoreEvents = true; + state->imuIgnoreEvents = true; + } + + // If the commit was triggered by a packet container limit being reached, we always + // update the time related limit. The size related one is updated implicitly by size + // being reset to zero after commit (new packets are empty). + if (containerTimeCommit) { + while (generateFullTimestamp(state->wrapOverflow, state->currentTimestamp) + > state->currentPacketContainerCommitTimestamp) { + state->currentPacketContainerCommitTimestamp += atomic_load_explicit( + &state->maxPacketContainerInterval, memory_order_relaxed); + } + } + + // Filter out completely empty commits. This can happen when data is turned off, + // but the timestamps are still going forward. + if (emptyContainerCommit) { + caerEventPacketContainerFree(state->currentPacketContainer); + state->currentPacketContainer = NULL; + } + else { + if (!ringBufferPut(state->dataExchangeBuffer, state->currentPacketContainer)) { + // Failed to forward packet container, just drop it, it doesn't contain + // any critical information anyway. + caerLog(CAER_LOG_INFO, handle->info.deviceString, + "Dropped EventPacket Container because ring-buffer full!"); + + caerEventPacketContainerFree(state->currentPacketContainer); + state->currentPacketContainer = NULL; + } + else { + if (state->dataNotifyIncrease != NULL) { + state->dataNotifyIncrease(state->dataNotifyUserPtr); + } + + state->currentPacketContainer = NULL; + } + } + + // The only critical timestamp information to forward is the timestamp reset event. + // The timestamp big-wrap can also (and should!) be detected by observing a packet's + // tsOverflow value, not the special packet TIMESTAMP_WRAP event, which is only informative. + // For the timestamp reset event (TIMESTAMP_RESET), we thus ensure that it is always + // committed, and we send it alone, in its own packet container, to ensure it will always + // be ordered after any other event packets in any processing or output stream. + if (tsReset) { + // Allocate packet container just for this event. + caerEventPacketContainer tsResetContainer = caerEventPacketContainerAllocate(DAVIS_EVENT_TYPES); + if (tsResetContainer == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, + "Failed to allocate tsReset event packet container."); + return; + } + + // Allocate special packet just for this event. + caerSpecialEventPacket tsResetPacket = caerSpecialEventPacketAllocate(1, I16T(handle->info.deviceID), + state->wrapOverflow); + if (tsResetPacket == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, + "Failed to allocate tsReset special event packet."); + return; + } + + // Create timestamp reset event. + caerSpecialEvent tsResetEvent = caerSpecialEventPacketGetEvent(tsResetPacket, 0); + caerSpecialEventSetTimestamp(tsResetEvent, INT32_MAX); + caerSpecialEventSetType(tsResetEvent, TIMESTAMP_RESET); + caerSpecialEventValidate(tsResetEvent, tsResetPacket); + + // Assign special packet to packet container. + caerEventPacketContainerSetEventPacket(tsResetContainer, SPECIAL_EVENT, + (caerEventPacketHeader) tsResetPacket); + + // Reset MUST be committed, always, else downstream data processing and + // outputs get confused if they have no notification of timestamps + // jumping back go zero. + while (!ringBufferPut(state->dataExchangeBuffer, tsResetContainer)) { + // Prevent dead-lock if shutdown is requested and nothing is consuming + // data anymore, but the ring-buffer is full (and would thus never empty), + // thus blocking the USB handling thread in this loop. + if (!atomic_load_explicit(&state->dataAcquisitionThreadRun, memory_order_relaxed)) { + return; + } + } + + // Signal new container as usual. + if (state->dataNotifyIncrease != NULL) { + state->dataNotifyIncrease(state->dataNotifyUserPtr); + } + } + } + } +} + +static int davisDataAcquisitionThread(void *inPtr) { + // inPtr is a pointer to device handle. + davisHandle handle = inPtr; + davisState state = &handle->state; + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "Initializing data acquisition thread ..."); + + // Set thread name. + thrd_set_name(state->deviceThreadName); + + // Reset configuration update, so as to not re-do work afterwards. + atomic_store(&state->dataAcquisitionThreadConfigUpdate, 0); + + if (atomic_load(&state->dataExchangeStartProducers)) { + // Enable data transfer on USB end-point 2. + davisCommonConfigSet(handle, DAVIS_CONFIG_USB, DAVIS_CONFIG_USB_RUN, true); + davisCommonConfigSet(handle, DAVIS_CONFIG_MUX, DAVIS_CONFIG_MUX_RUN, true); + davisCommonConfigSet(handle, DAVIS_CONFIG_MUX, DAVIS_CONFIG_MUX_TIMESTAMP_RUN, true); + davisCommonConfigSet(handle, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_RUN, true); + davisCommonConfigSet(handle, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_RUN, true); + davisCommonConfigSet(handle, DAVIS_CONFIG_IMU, DAVIS_CONFIG_IMU_RUN, true); + davisCommonConfigSet(handle, DAVIS_CONFIG_EXTINPUT, DAVIS_CONFIG_EXTINPUT_RUN_DETECTOR, true); + } + + // Create buffers as specified in config file. + davisAllocateTransfers(handle, U32T(atomic_load(&state->usbBufferNumber)), + U32T(atomic_load(&state->usbBufferSize))); + + // Signal data thread ready back to start function. + atomic_store(&state->dataAcquisitionThreadRun, true); + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "data acquisition thread ready to process events."); + + // Handle USB events (1 second timeout). + struct timeval te = { .tv_sec = 1, .tv_usec = 0 }; + + while (atomic_load_explicit(&state->dataAcquisitionThreadRun, memory_order_relaxed) + && state->activeDataTransfers > 0) { + // Check config refresh, in this case to adjust buffer sizes. + if (atomic_load_explicit(&state->dataAcquisitionThreadConfigUpdate, memory_order_relaxed) != 0) { + davisDataAcquisitionThreadConfig(handle); + } + + libusb_handle_events_timeout(state->deviceContext, &te); + } + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "shutting down data acquisition thread ..."); + + // Cancel all transfers and handle them. + davisDeallocateTransfers(handle); + + // Ensure shutdown is stored and notified, could be because of all data transfers going away! + atomic_store(&state->dataAcquisitionThreadRun, false); + + if (state->dataShutdownNotify != NULL) { + state->dataShutdownNotify(state->dataShutdownUserPtr); + } + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "data acquisition thread shut down."); + + return (EXIT_SUCCESS); +} + +static void davisDataAcquisitionThreadConfig(davisHandle handle) { + davisState state = &handle->state; + + // Get the current value to examine by atomic exchange, since we don't + // want there to be any possible store between a load/store pair. + uint32_t configUpdate = U32T(atomic_exchange(&state->dataAcquisitionThreadConfigUpdate, 0)); + + if ((configUpdate >> 0) & 0x01) { + // Do buffer size change: cancel all and recreate them. + davisDeallocateTransfers(handle); + davisAllocateTransfers(handle, U32T(atomic_load(&state->usbBufferNumber)), + U32T(atomic_load(&state->usbBufferSize))); + } + + if ((configUpdate >> 1) & 0x01) { + // Get new Master/Slave information from device. Done here to prevent deadlock + // inside asynchronous callback. + uint32_t param32 = 0; + + spiConfigReceive(state->deviceHandle, DAVIS_CONFIG_SYSINFO, DAVIS_CONFIG_SYSINFO_DEVICE_IS_MASTER, ¶m32); + + atomic_thread_fence(memory_order_seq_cst); + handle->info.deviceIsMaster = param32; + atomic_thread_fence(memory_order_seq_cst); + } +} + +uint16_t caerBiasVDACGenerate(struct caer_bias_vdac vdacBias) { + // Build up bias value from all its components. + uint16_t biasValue = U16T((vdacBias.voltageValue & 0x3F) << 0); + biasValue |= U16T((vdacBias.currentValue & 0x07) << 6); + + return (biasValue); +} + +struct caer_bias_vdac caerBiasVDACParse(uint16_t vdacBias) { + struct caer_bias_vdac biasValue; + + // Decompose bias integer into its parts. + biasValue.voltageValue = vdacBias & 0x3F; + biasValue.currentValue = (vdacBias >> 6) & 0x07; + + return (biasValue); +} + +uint16_t caerBiasCoarseFineGenerate(struct caer_bias_coarsefine coarseFineBias) { + uint16_t biasValue = 0; + + // Build up bias value from all its components. + if (coarseFineBias.enabled) { + biasValue |= 0x01; + } + if (coarseFineBias.sexN) { + biasValue |= 0x02; + } + if (coarseFineBias.typeNormal) { + biasValue |= 0x04; + } + if (coarseFineBias.currentLevelNormal) { + biasValue |= 0x08; + } + + biasValue |= U16T((coarseFineBias.fineValue & 0xFF) << 4); + biasValue |= U16T((coarseFineBias.coarseValue & 0x07) << 12); + + return (biasValue); +} + +struct caer_bias_coarsefine caerBiasCoarseFineParse(uint16_t coarseFineBias) { + struct caer_bias_coarsefine biasValue; + + // Decompose bias integer into its parts. + biasValue.enabled = (coarseFineBias & 0x01); + biasValue.sexN = (coarseFineBias & 0x02); + biasValue.typeNormal = (coarseFineBias & 0x04); + biasValue.currentLevelNormal = (coarseFineBias & 0x08); + biasValue.fineValue = U8T(coarseFineBias >> 4) & 0xFF; + biasValue.coarseValue = U8T(coarseFineBias >> 12) & 0x07; + + return (biasValue); +} + +uint16_t caerBiasShiftedSourceGenerate(struct caer_bias_shiftedsource shiftedSourceBias) { + uint16_t biasValue = 0; + + if (shiftedSourceBias.operatingMode == HI_Z) { + biasValue |= 0x01; + } + else if (shiftedSourceBias.operatingMode == TIED_TO_RAIL) { + biasValue |= 0x02; + } + + if (shiftedSourceBias.voltageLevel == SINGLE_DIODE) { + biasValue |= (0x01 << 2); + } + else if (shiftedSourceBias.voltageLevel == DOUBLE_DIODE) { + biasValue |= (0x02 << 2); + } + + biasValue |= U16T((shiftedSourceBias.refValue & 0x3F) << 4); + biasValue |= U16T((shiftedSourceBias.regValue & 0x3F) << 10); + + return (biasValue); +} + +struct caer_bias_shiftedsource caerBiasShiftedSourceParse(uint16_t shiftedSourceBias) { + struct caer_bias_shiftedsource biasValue; + + // Decompose bias integer into its parts. + biasValue.operatingMode = + (shiftedSourceBias & 0x01) ? (HI_Z) : ((shiftedSourceBias & 0x02) ? (TIED_TO_RAIL) : (SHIFTED_SOURCE)); + biasValue.voltageLevel = + ((shiftedSourceBias >> 2) & 0x01) ? + (SINGLE_DIODE) : (((shiftedSourceBias >> 2) & 0x02) ? (DOUBLE_DIODE) : (SPLIT_GATE)); + biasValue.refValue = (shiftedSourceBias >> 4) & 0x3F; + biasValue.regValue = (shiftedSourceBias >> 10) & 0x3F; + + return (biasValue); +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_common.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_common.h new file mode 100755 index 0000000000000000000000000000000000000000..cf07615b765147f4291a6accc9bd59b08a8613c5 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_common.h @@ -0,0 +1,165 @@ +#ifndef LIBCAER_SRC_DAVIS_COMMON_H_ +#define LIBCAER_SRC_DAVIS_COMMON_H_ + +#include "devices/davis.h" +#include "ringbuffer/ringbuffer.h" +#include <stdatomic.h> +#include <libusb.h> + +#ifdef HAVE_PTHREADS + #include "c11threads_posix.h" +#endif + +#define APS_READOUT_TYPES_NUM 2 +#define APS_READOUT_RESET 0 +#define APS_READOUT_SIGNAL 1 + +/** + * Enable APS frame debugging by only looking at the reset or signal + * frames, and not at the resulting correlated frame. + * Supported values: + * 0 - both/CDS (default) + * 1 - reset read only + * 2 - signal read only + */ +#define APS_DEBUG_FRAME 0 + +#define APS_ADC_DEPTH 10 + +#define APS_ADC_CHANNELS 1 + +#define APS_ROI_REGIONS_MAX 4 + +#define IMU6_COUNT 15 +#define IMU9_COUNT 21 + +#define DAVIS_EVENT_TYPES 4 + +#define DAVIS_POLARITY_DEFAULT_SIZE 4096 +#define DAVIS_SPECIAL_DEFAULT_SIZE 128 +#define DAVIS_FRAME_DEFAULT_SIZE 4 +#define DAVIS_IMU_DEFAULT_SIZE 64 + +#define DAVIS_DATA_ENDPOINT 0x82 + +#define VENDOR_REQUEST_FPGA_CONFIG 0xBF +#define VENDOR_REQUEST_FPGA_CONFIG_MULTIPLE 0xC2 + +struct davis_state { + // Data Acquisition Thread -> Mainloop Exchange + RingBuffer dataExchangeBuffer; + atomic_uint_fast32_t dataExchangeBufferSize; // Only takes effect on DataStart() calls! + atomic_bool dataExchangeBlocking; + atomic_bool dataExchangeStartProducers; + atomic_bool dataExchangeStopProducers; + void (*dataNotifyIncrease)(void *ptr); + void (*dataNotifyDecrease)(void *ptr); + void *dataNotifyUserPtr; + void (*dataShutdownNotify)(void *ptr); + void *dataShutdownUserPtr; + // USB Device State + char deviceThreadName[15 + 1]; // +1 for terminating NUL character. + libusb_context *deviceContext; + libusb_device_handle *deviceHandle; + // USB Transfer Settings + atomic_uint_fast32_t usbBufferNumber; + atomic_uint_fast32_t usbBufferSize; + // Data Acquisition Thread + thrd_t dataAcquisitionThread; + atomic_bool dataAcquisitionThreadRun; + atomic_uint_fast32_t dataAcquisitionThreadConfigUpdate; + struct libusb_transfer **dataTransfers; + size_t dataTransfersLength; + size_t activeDataTransfers; + // Timestamp fields + int32_t wrapOverflow; + int32_t wrapAdd; + int32_t lastTimestamp; + int32_t currentTimestamp; + // DVS specific fields + uint16_t dvsLastY; + bool dvsGotY; + int16_t dvsSizeX; + int16_t dvsSizeY; + bool dvsInvertXY; + // APS specific fields + int16_t apsSizeX; + int16_t apsSizeY; + bool apsInvertXY; + bool apsFlipX; + bool apsFlipY; + bool apsIgnoreEvents; + bool apsGlobalShutter; + bool apsResetRead; + bool apsRGBPixelOffsetDirection; // 0 is increasing, 1 is decreasing. + int16_t apsRGBPixelOffset; + uint16_t apsCurrentReadoutType; + uint16_t apsCountX[APS_READOUT_TYPES_NUM]; + uint16_t apsCountY[APS_READOUT_TYPES_NUM]; + uint16_t *apsCurrentResetFrame; + uint16_t apsROIUpdate; + uint16_t apsROITmpData; + uint16_t apsROISizeX[APS_ROI_REGIONS_MAX]; + uint16_t apsROISizeY[APS_ROI_REGIONS_MAX]; + uint16_t apsROIPositionX[APS_ROI_REGIONS_MAX]; + uint16_t apsROIPositionY[APS_ROI_REGIONS_MAX]; + // IMU specific fields + bool imuIgnoreEvents; + uint8_t imuCount; + uint8_t imuTmpData; + float imuAccelScale; + float imuGyroScale; + // Packet Container state + caerEventPacketContainer currentPacketContainer; + atomic_int_fast32_t maxPacketContainerPacketSize; + atomic_int_fast32_t maxPacketContainerInterval; + int64_t currentPacketContainerCommitTimestamp; + // Polarity Packet state + caerPolarityEventPacket currentPolarityPacket; + int32_t currentPolarityPacketPosition; + // Frame Packet state + caerFrameEventPacket currentFramePacket; + int32_t currentFramePacketPosition; + // IMU6 Packet state + caerIMU6EventPacket currentIMU6Packet; + int32_t currentIMU6PacketPosition; + // Special Packet state + caerSpecialEventPacket currentSpecialPacket; + int32_t currentSpecialPacketPosition; + // Current composite events, for later copy, to not loose them on commits. + caerFrameEvent currentFrameEvent[APS_ROI_REGIONS_MAX]; + struct caer_imu6_event currentIMU6Event; +}; + +typedef struct davis_state *davisState; + +struct davis_handle { + uint16_t deviceType; + // Information fields + struct caer_davis_info info; + // State for data management, common to all DAVIS. + struct davis_state state; +}; + +typedef struct davis_handle *davisHandle; + +bool davisCommonOpen(davisHandle handle, uint16_t VID, uint16_t PID, uint8_t DID_TYPE, const char *deviceName, + uint16_t deviceID, uint8_t busNumberRestrict, uint8_t devAddressRestrict, const char *serialNumberRestrict, + uint16_t requiredLogicRevision, uint16_t requiredFirmwareVersion); +bool davisCommonClose(davisHandle handle); + +bool davisCommonSendDefaultFPGAConfig(caerDeviceHandle cdh, + bool (*configSet)(caerDeviceHandle cdh, int8_t modAddr, uint8_t paramAddr, uint32_t param)); +bool davisCommonSendDefaultChipConfig(caerDeviceHandle cdh, + bool (*configSet)(caerDeviceHandle cdh, int8_t modAddr, uint8_t paramAddr, uint32_t param)); + +bool davisCommonConfigSet(davisHandle handle, int8_t modAddr, uint8_t paramAddr, uint32_t param); +bool davisCommonConfigGet(davisHandle handle, int8_t modAddr, uint8_t paramAddr, uint32_t *param); + +bool davisCommonDataStart(caerDeviceHandle handle, void (*dataNotifyIncrease)(void *ptr), + void (*dataNotifyDecrease)(void *ptr), void *dataNotifyUserPtr, void (*dataShutdownNotify)(void *ptr), + void *dataShutdownUserPtr); +bool davisCommonDataStop(caerDeviceHandle handle); +caerEventPacketContainer davisCommonDataGet(caerDeviceHandle handle); + +#endif /* LIBCAER_SRC_DAVIS_COMMON_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_fx2.c b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_fx2.c new file mode 100755 index 0000000000000000000000000000000000000000..c6b7646e36874531d8e977595c858d6de196d2d3 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_fx2.c @@ -0,0 +1,70 @@ +#include "davis_fx2.h" + +caerDeviceHandle davisFX2Open(uint16_t deviceID, uint8_t busNumberRestrict, uint8_t devAddressRestrict, + const char *serialNumberRestrict) { + caerLog(CAER_LOG_DEBUG, __func__, "Initializing %s.", DAVIS_FX2_DEVICE_NAME); + + davisFX2Handle handle = calloc(1, sizeof(*handle)); + if (handle == NULL) { + // Failed to allocate memory for device handle! + caerLog(CAER_LOG_CRITICAL, __func__, "Failed to allocate memory for device handle."); + return (NULL); + } + + // Set main deviceType correctly right away. + handle->h.deviceType = CAER_DEVICE_DAVIS_FX2; + + bool openRetVal = davisCommonOpen((davisHandle) handle, DAVIS_FX2_DEVICE_VID, DAVIS_FX2_DEVICE_PID, + DAVIS_FX2_DEVICE_DID_TYPE, DAVIS_FX2_DEVICE_NAME, deviceID, busNumberRestrict, devAddressRestrict, + serialNumberRestrict, DAVIS_FX2_REQUIRED_LOGIC_REVISION, + DAVIS_FX2_REQUIRED_FIRMWARE_VERSION); + if (!openRetVal) { + free(handle); + + // Failed to open device and grab basic information! + return (NULL); + } + + return ((caerDeviceHandle) handle); +} + +bool davisFX2Close(caerDeviceHandle cdh) { + caerLog(CAER_LOG_DEBUG, ((davisHandle) cdh)->info.deviceString, "Shutting down ..."); + + return (davisCommonClose((davisHandle) cdh)); +} + +bool davisFX2SendDefaultConfig(caerDeviceHandle cdh) { + // First send default chip/bias config. + if (!davisCommonSendDefaultChipConfig(cdh, &davisFX2ConfigSet)) { + return (false); + } + + // Send default FPGA config. + if (!davisCommonSendDefaultFPGAConfig(cdh, &davisFX2ConfigSet)) { + return (false); + } + + // FX2 specific FPGA configuration. + if (!davisFX2ConfigSet(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_ACK_DELAY_ROW, 14)) { + return (false); + } + + if (!davisFX2ConfigSet(cdh, DAVIS_CONFIG_DVS, DAVIS_CONFIG_DVS_ACK_EXTENSION_ROW, 4)) { + return (false); + } + + return (true); +} + +bool davisFX2ConfigSet(caerDeviceHandle cdh, int8_t modAddr, uint8_t paramAddr, uint32_t param) { + davisHandle handle = (davisHandle) cdh; + + return (davisCommonConfigSet(handle, modAddr, paramAddr, param)); +} + +bool davisFX2ConfigGet(caerDeviceHandle cdh, int8_t modAddr, uint8_t paramAddr, uint32_t *param) { + davisHandle handle = (davisHandle) cdh; + + return (davisCommonConfigGet(handle, modAddr, paramAddr, param)); +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_fx2.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_fx2.h new file mode 100755 index 0000000000000000000000000000000000000000..4d106239b200ef54145fafe4a900441089744d66 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_fx2.h @@ -0,0 +1,35 @@ +#ifndef LIBCAER_SRC_DAVIS_FX2_H_ +#define LIBCAER_SRC_DAVIS_FX2_H_ + +#include "davis_common.h" + +#define DAVIS_FX2_DEVICE_NAME "DAVIS FX2" + +#define DAVIS_FX2_DEVICE_VID 0x152A +#define DAVIS_FX2_DEVICE_PID 0x841B +#define DAVIS_FX2_DEVICE_DID_TYPE 0x00 + +#define DAVIS_FX2_REQUIRED_LOGIC_REVISION 7449 +#define DAVIS_FX2_REQUIRED_FIRMWARE_VERSION 3 + +#define VENDOR_REQUEST_CHIP_BIAS 0xC0 +#define VENDOR_REQUEST_CHIP_DIAG 0xC1 + +struct davis_fx2_handle { + // Common info and state structure (handle). + struct davis_handle h; +}; + +typedef struct davis_fx2_handle *davisFX2Handle; + +caerDeviceHandle davisFX2Open(uint16_t deviceID, uint8_t busNumberRestrict, uint8_t devAddressRestrict, + const char *serialNumberRestrict); +bool davisFX2Close(caerDeviceHandle handle); + +bool davisFX2SendDefaultConfig(caerDeviceHandle handle); +// Negative addresses are used for host-side configuration. +// Positive addresses (including zero) are used for device-side configuration. +bool davisFX2ConfigSet(caerDeviceHandle handle, int8_t modAddr, uint8_t paramAddr, uint32_t param); +bool davisFX2ConfigGet(caerDeviceHandle handle, int8_t modAddr, uint8_t paramAddr, uint32_t *param); + +#endif /* LIBCAER_SRC_DAVIS_FX2_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_fx3.c b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_fx3.c new file mode 100755 index 0000000000000000000000000000000000000000..f5994595422292267fd5a31b5f0803121ce6328f --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_fx3.c @@ -0,0 +1,190 @@ +#include "davis_fx3.h" + +static void allocateDebugTransfers(davisFX3Handle handle); +static void deallocateDebugTransfers(davisFX3Handle handle); +static void LIBUSB_CALL libUsbDebugCallback(struct libusb_transfer *transfer); +static void debugTranslator(davisFX3Handle handle, uint8_t *buffer, size_t bytesSent); + +caerDeviceHandle davisFX3Open(uint16_t deviceID, uint8_t busNumberRestrict, uint8_t devAddressRestrict, + const char *serialNumberRestrict) { + caerLog(CAER_LOG_DEBUG, __func__, "Initializing %s.", DAVIS_FX3_DEVICE_NAME); + + davisFX3Handle handle = calloc(1, sizeof(*handle)); + if (handle == NULL) { + // Failed to allocate memory for device handle! + caerLog(CAER_LOG_CRITICAL, __func__, "Failed to allocate memory for device handle."); + return (NULL); + } + + // Set main deviceType correctly right away. + handle->h.deviceType = CAER_DEVICE_DAVIS_FX3; + + bool openRetVal = davisCommonOpen((davisHandle) handle, DAVIS_FX3_DEVICE_VID, DAVIS_FX3_DEVICE_PID, + DAVIS_FX3_DEVICE_DID_TYPE, DAVIS_FX3_DEVICE_NAME, deviceID, busNumberRestrict, devAddressRestrict, + serialNumberRestrict, DAVIS_FX3_REQUIRED_LOGIC_REVISION, + DAVIS_FX3_REQUIRED_FIRMWARE_VERSION); + if (!openRetVal) { + free(handle); + + // Failed to open device and grab basic information! + return (NULL); + } + + allocateDebugTransfers(handle); + + return ((caerDeviceHandle) handle); +} + +bool davisFX3Close(caerDeviceHandle cdh) { + caerLog(CAER_LOG_DEBUG, ((davisHandle) cdh)->info.deviceString, "Shutting down ..."); + + deallocateDebugTransfers((davisFX3Handle) cdh); + + return (davisCommonClose((davisHandle) cdh)); +} + +bool davisFX3SendDefaultConfig(caerDeviceHandle cdh) { + // First send default chip/bias config. + if (!davisCommonSendDefaultChipConfig(cdh, &davisFX3ConfigSet)) { + return (false); + } + + // Send default FPGA config. + if (!davisCommonSendDefaultFPGAConfig(cdh, &davisFX3ConfigSet)) { + return (false); + } + + return (true); +} + +bool davisFX3ConfigSet(caerDeviceHandle cdh, int8_t modAddr, uint8_t paramAddr, uint32_t param) { + davisHandle handle = (davisHandle) cdh; + + return (davisCommonConfigSet(handle, modAddr, paramAddr, param)); +} + +bool davisFX3ConfigGet(caerDeviceHandle cdh, int8_t modAddr, uint8_t paramAddr, uint32_t *param) { + davisHandle handle = (davisHandle) cdh; + + return (davisCommonConfigGet(handle, modAddr, paramAddr, param)); +} + +static void allocateDebugTransfers(davisFX3Handle handle) { + // Set number of transfers and allocate memory for the main transfer array. + + // Allocate transfers and set them up. + for (size_t i = 0; i < DEBUG_TRANSFER_NUM; i++) { + handle->debugTransfers[i] = libusb_alloc_transfer(0); + if (handle->debugTransfers[i] == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->h.info.deviceString, + "Unable to allocate further libusb transfers (debug channel, %zu of %" PRIu32 ").", i, + DEBUG_TRANSFER_NUM); + continue; + } + + // Create data buffer. + handle->debugTransfers[i]->length = DEBUG_TRANSFER_SIZE; + handle->debugTransfers[i]->buffer = malloc(DEBUG_TRANSFER_SIZE); + if (handle->debugTransfers[i]->buffer == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->h.info.deviceString, + "Unable to allocate buffer for libusb transfer %zu (debug channel). Error: %d.", i, errno); + + libusb_free_transfer(handle->debugTransfers[i]); + handle->debugTransfers[i] = NULL; + + continue; + } + + // Initialize Transfer. + handle->debugTransfers[i]->dev_handle = handle->h.state.deviceHandle; + handle->debugTransfers[i]->endpoint = DEBUG_ENDPOINT; + handle->debugTransfers[i]->type = LIBUSB_TRANSFER_TYPE_INTERRUPT; + handle->debugTransfers[i]->callback = &libUsbDebugCallback; + handle->debugTransfers[i]->user_data = handle; + handle->debugTransfers[i]->timeout = 0; + handle->debugTransfers[i]->flags = LIBUSB_TRANSFER_FREE_BUFFER; + + if ((errno = libusb_submit_transfer(handle->debugTransfers[i])) == LIBUSB_SUCCESS) { + handle->activeDebugTransfers++; + } + else { + caerLog(CAER_LOG_CRITICAL, handle->h.info.deviceString, + "Unable to submit libusb transfer %zu (debug channel). Error: %s (%d).", i, libusb_strerror(errno), + errno); + + // The transfer buffer is freed automatically here thanks to + // the LIBUSB_TRANSFER_FREE_BUFFER flag set above. + libusb_free_transfer(handle->debugTransfers[i]); + handle->debugTransfers[i] = NULL; + + continue; + } + } + + if (handle->activeDebugTransfers == 0) { + // Didn't manage to allocate any USB transfers, log failure. + caerLog(CAER_LOG_CRITICAL, handle->h.info.deviceString, "Unable to allocate any libusb transfers."); + } +} + +static void deallocateDebugTransfers(davisFX3Handle handle) { + // Cancel all current transfers first. + for (size_t i = 0; i < DEBUG_TRANSFER_NUM; i++) { + if (handle->debugTransfers[i] != NULL) { + errno = libusb_cancel_transfer(handle->debugTransfers[i]); + if (errno != LIBUSB_SUCCESS && errno != LIBUSB_ERROR_NOT_FOUND) { + caerLog(CAER_LOG_CRITICAL, handle->h.info.deviceString, + "Unable to cancel libusb transfer %zu (debug channel). Error: %s (%d).", i, libusb_strerror(errno), + errno); + // Proceed with trying to cancel all transfers regardless of errors. + } + } + } + + // Wait for all transfers to go away (0.1 seconds timeout). + struct timeval te = { .tv_sec = 0, .tv_usec = 100000 }; + + while (handle->activeDebugTransfers > 0) { + libusb_handle_events_timeout(handle->h.state.deviceContext, &te); + } +} + +static void LIBUSB_CALL libUsbDebugCallback(struct libusb_transfer *transfer) { + davisFX3Handle handle = transfer->user_data; + + if (transfer->status == LIBUSB_TRANSFER_COMPLETED) { + // Handle debug data. + debugTranslator(handle, transfer->buffer, (size_t) transfer->actual_length); + } + + if (transfer->status != LIBUSB_TRANSFER_CANCELLED && transfer->status != LIBUSB_TRANSFER_NO_DEVICE) { + // Submit transfer again. + if (libusb_submit_transfer(transfer) == LIBUSB_SUCCESS) { + return; + } + } + + // Cannot recover (cancelled, no device, or other critical error). + // Signal this by adjusting the counter, free and exit. + handle->activeDebugTransfers--; + for (size_t i = 0; i < DEBUG_TRANSFER_NUM; i++) { + // Remove from list, so we don't try to cancel it later on. + if (handle->debugTransfers[i] == transfer) { + handle->debugTransfers[i] = NULL; + } + } + libusb_free_transfer(transfer); +} + +static void debugTranslator(davisFX3Handle handle, uint8_t *buffer, size_t bytesSent) { + // Check if this is a debug message (length 7-64 bytes). + if (bytesSent >= 7 && buffer[0] == 0x00) { + // Debug message, log this. + caerLog(CAER_LOG_ERROR, handle->h.info.deviceString, "Error message: '%s' (code %u at time %u).", &buffer[6], + buffer[1], *((uint32_t *) &buffer[2])); + } + else { + // Unknown/invalid debug message, log this. + caerLog(CAER_LOG_WARNING, handle->h.info.deviceString, "Unknown/invalid debug message."); + } +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_fx3.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_fx3.h new file mode 100755 index 0000000000000000000000000000000000000000..78fca031da628d06aed2dd8d46bf9195290e81ed --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/davis_fx3.h @@ -0,0 +1,39 @@ +#ifndef LIBCAER_SRC_DAVIS_FX3_H_ +#define LIBCAER_SRC_DAVIS_FX3_H_ + +#include "davis_common.h" + +#define DAVIS_FX3_DEVICE_NAME "DAVIS FX3" + +#define DAVIS_FX3_DEVICE_VID 0x152A +#define DAVIS_FX3_DEVICE_PID 0x841A +#define DAVIS_FX3_DEVICE_DID_TYPE 0x01 + +#define DAVIS_FX3_REQUIRED_LOGIC_REVISION 7449 +#define DAVIS_FX3_REQUIRED_FIRMWARE_VERSION 2 + +#define DEBUG_ENDPOINT 0x81 +#define DEBUG_TRANSFER_NUM 4 +#define DEBUG_TRANSFER_SIZE 64 + +struct davis_fx3_handle { + // Common info and state structure (handle). + struct davis_handle h; + // Debug transfer support (FX3 only). + struct libusb_transfer *debugTransfers[DEBUG_TRANSFER_NUM]; + size_t activeDebugTransfers; +}; + +typedef struct davis_fx3_handle *davisFX3Handle; + +caerDeviceHandle davisFX3Open(uint16_t deviceID, uint8_t busNumberRestrict, uint8_t devAddressRestrict, + const char *serialNumberRestrict); +bool davisFX3Close(caerDeviceHandle handle); + +bool davisFX3SendDefaultConfig(caerDeviceHandle handle); +// Negative addresses are used for host-side configuration. +// Positive addresses (including zero) are used for device-side configuration. +bool davisFX3ConfigSet(caerDeviceHandle handle, int8_t modAddr, uint8_t paramAddr, uint32_t param); +bool davisFX3ConfigGet(caerDeviceHandle handle, int8_t modAddr, uint8_t paramAddr, uint32_t *param); + +#endif /* LIBCAER_SRC_DAVIS_FX3_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/device.c b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/device.c new file mode 100755 index 0000000000000000000000000000000000000000..bd4c0848e4468de005de05bd9a92def3980bf20f --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/device.c @@ -0,0 +1,206 @@ +#include "devices/usb.h" + +#include "dvs128.h" +#include "davis_common.h" +#include "davis_fx2.h" +#include "davis_fx3.h" + +/** + * Number of devices supported by this library. + */ +#define SUPPORTED_DEVICES_NUMBER 3 + +// Supported devices and their functions. +static caerDeviceHandle (*constructors[SUPPORTED_DEVICES_NUMBER])(uint16_t deviceID, uint8_t busNumberRestrict, + uint8_t devAddressRestrict, const char *serialNumberRestrict) = { + [CAER_DEVICE_DVS128] = &dvs128Open, + [CAER_DEVICE_DAVIS_FX2] = &davisFX2Open, + [CAER_DEVICE_DAVIS_FX3] = &davisFX3Open +}; + +static bool (*destructors[SUPPORTED_DEVICES_NUMBER])(caerDeviceHandle handle) = { + [CAER_DEVICE_DVS128] = &dvs128Close, + [CAER_DEVICE_DAVIS_FX2] = &davisFX2Close, + [CAER_DEVICE_DAVIS_FX3] = &davisFX3Close +}; + +static bool (*defaultConfigSenders[SUPPORTED_DEVICES_NUMBER])(caerDeviceHandle handle) = { + [CAER_DEVICE_DVS128] = &dvs128SendDefaultConfig, + [CAER_DEVICE_DAVIS_FX2] = &davisFX2SendDefaultConfig, + [CAER_DEVICE_DAVIS_FX3] = &davisFX3SendDefaultConfig +}; + +static bool (*configSetters[SUPPORTED_DEVICES_NUMBER])(caerDeviceHandle handle, int8_t modAddr, uint8_t paramAddr, + uint32_t param) = { + [CAER_DEVICE_DVS128] = &dvs128ConfigSet, + [CAER_DEVICE_DAVIS_FX2] = &davisFX2ConfigSet, + [CAER_DEVICE_DAVIS_FX3] = &davisFX3ConfigSet +}; + +static bool (*configGetters[SUPPORTED_DEVICES_NUMBER])(caerDeviceHandle handle, int8_t modAddr, uint8_t paramAddr, + uint32_t *param) = { + [CAER_DEVICE_DVS128] = &dvs128ConfigGet, + [CAER_DEVICE_DAVIS_FX2] = &davisFX2ConfigGet, + [CAER_DEVICE_DAVIS_FX3] = &davisFX3ConfigGet +}; + +static bool (*dataStarters[SUPPORTED_DEVICES_NUMBER])(caerDeviceHandle handle, void (*dataNotifyIncrease)(void *ptr), + void (*dataNotifyDecrease)(void *ptr), void *dataNotifyUserPtr, void (*dataShutdownNotify)(void *ptr), + void *dataShutdownUserPtr) = { + [CAER_DEVICE_DVS128] = &dvs128DataStart, + [CAER_DEVICE_DAVIS_FX2] = &davisCommonDataStart, + [CAER_DEVICE_DAVIS_FX3] = &davisCommonDataStart +}; + +static bool (*dataStoppers[SUPPORTED_DEVICES_NUMBER])(caerDeviceHandle handle) = { + [CAER_DEVICE_DVS128] = &dvs128DataStop, + [CAER_DEVICE_DAVIS_FX2] = &davisCommonDataStop, + [CAER_DEVICE_DAVIS_FX3] = &davisCommonDataStop +}; + +static caerEventPacketContainer (*dataGetters[SUPPORTED_DEVICES_NUMBER])(caerDeviceHandle handle) = { + [CAER_DEVICE_DVS128] = &dvs128DataGet, + [CAER_DEVICE_DAVIS_FX2] = &davisCommonDataGet, + [CAER_DEVICE_DAVIS_FX3] = &davisCommonDataGet +}; + +struct caer_device_handle { + uint16_t deviceType; +// This is compatible with all device handle structures. +// The first member is always 'uint16_t deviceType'. +}; + +caerDeviceHandle caerDeviceOpen(uint16_t deviceID, uint16_t deviceType, uint8_t busNumberRestrict, + uint8_t devAddressRestrict, const char *serialNumberRestrict) { + // Check if device type is supported. + if (deviceType >= SUPPORTED_DEVICES_NUMBER) { + return (NULL); + } + + // Execute main constructor function. + return (constructors[deviceType](deviceID, busNumberRestrict, devAddressRestrict, serialNumberRestrict)); +} + +bool caerDeviceClose(caerDeviceHandle *handlePtr) { + // We want a pointer here so we can ensure the reference is set to NULL. + // Check if either it, or the memory pointed to, are NULL and abort + // if that's the case. + if (handlePtr == NULL) { + return (false); + } + + if (*handlePtr == NULL) { + return (false); + } + + // Check if device type is supported. + if ((*handlePtr)->deviceType >= SUPPORTED_DEVICES_NUMBER) { + return (false); + } + + // Call appropriate destructor function. + bool retVal = destructors[(*handlePtr)->deviceType](*handlePtr); + + // Done. Set reference to NULL if successful. + if (retVal) { + *handlePtr = NULL; + } + + return (retVal); +} + +bool caerDeviceSendDefaultConfig(caerDeviceHandle handle) { + // Check if the pointer is valid. + if (handle == NULL) { + return (false); + } + + // Check if device type is supported. + if (handle->deviceType >= SUPPORTED_DEVICES_NUMBER) { + return (false); + } + + // Call appropriate function. + return (defaultConfigSenders[handle->deviceType](handle)); +} + +bool caerDeviceConfigSet(caerDeviceHandle handle, int8_t modAddr, uint8_t paramAddr, uint32_t param) { + // Check if the pointer is valid. + if (handle == NULL) { + return (false); + } + + // Check if device type is supported. + if (handle->deviceType >= SUPPORTED_DEVICES_NUMBER) { + return (false); + } + + // Call appropriate function. + return (configSetters[handle->deviceType](handle, modAddr, paramAddr, param)); +} + +bool caerDeviceConfigGet(caerDeviceHandle handle, int8_t modAddr, uint8_t paramAddr, uint32_t *param) { + // Check if the pointer is valid. + if (handle == NULL) { + return (false); + } + + // Check if device type is supported. + if (handle->deviceType >= SUPPORTED_DEVICES_NUMBER) { + return (false); + } + + // Ensure content of param is zeroed out. + *param = 0; + + // Call appropriate function. + return (configGetters[handle->deviceType](handle, modAddr, paramAddr, param)); +} + +bool caerDeviceDataStart(caerDeviceHandle handle, void (*dataNotifyIncrease)(void *ptr), + void (*dataNotifyDecrease)(void *ptr), void *dataNotifyUserPtr, void (*dataShutdownNotify)(void *ptr), + void *dataShutdownUserPtr) { + // Check if the pointer is valid. + if (handle == NULL) { + return (false); + } + + // Check if device type is supported. + if (handle->deviceType >= SUPPORTED_DEVICES_NUMBER) { + return (false); + } + + // Call appropriate function. + return (dataStarters[handle->deviceType](handle, dataNotifyIncrease, dataNotifyDecrease, dataNotifyUserPtr, + dataShutdownNotify, dataShutdownUserPtr)); +} + +bool caerDeviceDataStop(caerDeviceHandle handle) { + // Check if the pointer is valid. + if (handle == NULL) { + return (false); + } + + // Check if device type is supported. + if (handle->deviceType >= SUPPORTED_DEVICES_NUMBER) { + return (false); + } + + // Call appropriate function. + return (dataStoppers[handle->deviceType](handle)); +} + +caerEventPacketContainer caerDeviceDataGet(caerDeviceHandle handle) { + // Check if the pointer is valid. + if (handle == NULL) { + return (false); + } + + // Check if device type is supported. + if (handle->deviceType >= SUPPORTED_DEVICES_NUMBER) { + return (false); + } + + // Call appropriate function. + return (dataGetters[handle->deviceType](handle)); +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/dvs128.c b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/dvs128.c new file mode 100755 index 0000000000000000000000000000000000000000..a07857add5b89d22a3c92483f65a56c5bcfc3f07 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/dvs128.c @@ -0,0 +1,1319 @@ +#include "dvs128.h" + +static libusb_device_handle *dvs128DeviceOpen(libusb_context *devContext, uint16_t devVID, uint16_t devPID, + uint8_t devType, uint8_t busNumber, uint8_t devAddress, const char *serialNumber, uint16_t requiredFirmwareVersion); +static void dvs128DeviceClose(libusb_device_handle *devHandle); +static void dvs128AllocateTransfers(dvs128Handle handle, uint32_t bufferNum, uint32_t bufferSize); +static void dvs128DeallocateTransfers(dvs128Handle handle); +static void LIBUSB_CALL dvs128LibUsbCallback(struct libusb_transfer *transfer); +static void dvs128EventTranslator(dvs128Handle handle, uint8_t *buffer, size_t bytesSent); +static bool dvs128SendBiases(dvs128State state); +static int dvs128DataAcquisitionThread(void *inPtr); +static void dvs128DataAcquisitionThreadConfig(dvs128Handle handle); + +static inline void checkMonotonicTimestamp(dvs128Handle handle) { + if (handle->state.currentTimestamp < handle->state.lastTimestamp) { + caerLog(CAER_LOG_ALERT, handle->info.deviceString, + "Timestamps: non monotonic timestamp detected: lastTimestamp=%" PRIi32 ", currentTimestamp=%" PRIi32 ", difference=%" PRIi32 ".", + handle->state.lastTimestamp, handle->state.currentTimestamp, + (handle->state.lastTimestamp - handle->state.currentTimestamp)); + } +} + +static inline void freeAllDataMemory(dvs128State state) { + if (state->dataExchangeBuffer != NULL) { + ringBufferFree(state->dataExchangeBuffer); + state->dataExchangeBuffer = NULL; + } + + // Since the current event packets aren't necessarily + // already assigned to the current packet container, we + // free them separately from it. + if (state->currentPolarityPacket != NULL) { + free(&state->currentPolarityPacket->packetHeader); + state->currentPolarityPacket = NULL; + + if (state->currentPacketContainer != NULL) { + caerEventPacketContainerSetEventPacket(state->currentPacketContainer, POLARITY_EVENT, NULL); + } + } + + if (state->currentSpecialPacket != NULL) { + free(&state->currentSpecialPacket->packetHeader); + state->currentSpecialPacket = NULL; + + if (state->currentPacketContainer != NULL) { + caerEventPacketContainerSetEventPacket(state->currentPacketContainer, SPECIAL_EVENT, NULL); + } + } + + if (state->currentPacketContainer != NULL) { + caerEventPacketContainerFree(state->currentPacketContainer); + state->currentPacketContainer = NULL; + } +} + +caerDeviceHandle dvs128Open(uint16_t deviceID, uint8_t busNumberRestrict, uint8_t devAddressRestrict, + const char *serialNumberRestrict) { + caerLog(CAER_LOG_DEBUG, __func__, "Initializing %s.", DVS_DEVICE_NAME); + + dvs128Handle handle = calloc(1, sizeof(*handle)); + if (handle == NULL) { + // Failed to allocate memory for device handle! + caerLog(CAER_LOG_CRITICAL, __func__, "Failed to allocate memory for device handle."); + return (NULL); + } + + // Set main deviceType correctly right away. + handle->deviceType = CAER_DEVICE_DVS128; + + dvs128State state = &handle->state; + + // Initialize state variables to default values (if not zero, taken care of by calloc above). + atomic_store_explicit(&state->dataExchangeBufferSize, 64, memory_order_relaxed); + atomic_store_explicit(&state->dataExchangeBlocking, false, memory_order_relaxed); + atomic_store_explicit(&state->dataExchangeStartProducers, true, memory_order_relaxed); + atomic_store_explicit(&state->dataExchangeStopProducers, true, memory_order_relaxed); + atomic_store_explicit(&state->usbBufferNumber, 8, memory_order_relaxed); + atomic_store_explicit(&state->usbBufferSize, 4096, memory_order_relaxed); + + // Packet settings (size (in events) and time interval (in µs)). + atomic_store_explicit(&state->maxPacketContainerPacketSize, 4096, memory_order_relaxed); + atomic_store_explicit(&state->maxPacketContainerInterval, 10000, memory_order_relaxed); + + atomic_store_explicit(&state->dvsIsMaster, true, memory_order_relaxed); // Always master by default. + + atomic_thread_fence(memory_order_release); + + // Set device thread name. Maximum length of 15 chars due to Linux limitations. + snprintf(state->deviceThreadName, 15 + 1, "%s ID-%" PRIu16, DVS_DEVICE_NAME, deviceID); + state->deviceThreadName[15] = '\0'; + + // Search for device and open it. + // Initialize libusb using a separate context for each device. + // This is to correctly support one thread per device. + // libusb may create its own threads at this stage, so we temporarly set + // a different thread name. + char originalThreadName[15 + 1]; // +1 for terminating NUL character. + thrd_get_name(originalThreadName, 15); + originalThreadName[15] = '\0'; + + thrd_set_name(state->deviceThreadName); + int res = libusb_init(&state->deviceContext); + + thrd_set_name(originalThreadName); + + if (res != LIBUSB_SUCCESS) { + free(handle); + caerLog(CAER_LOG_CRITICAL, __func__, "Failed to initialize libusb context. Error: %d.", res); + return (NULL); + } + + // Try to open a DVS128 device on a specific USB port. + state->deviceHandle = dvs128DeviceOpen(state->deviceContext, DVS_DEVICE_VID, DVS_DEVICE_PID, DVS_DEVICE_DID_TYPE, + busNumberRestrict, devAddressRestrict, serialNumberRestrict, + DVS_REQUIRED_FIRMWARE_VERSION); + if (state->deviceHandle == NULL) { + libusb_exit(state->deviceContext); + free(handle); + + caerLog(CAER_LOG_CRITICAL, __func__, "Failed to open %s device.", DVS_DEVICE_NAME); + return (NULL); + } + + // At this point we can get some more precise data on the device and update + // the logging string to reflect that and be more informative. + uint8_t busNumber = libusb_get_bus_number(libusb_get_device(state->deviceHandle)); + uint8_t devAddress = libusb_get_device_address(libusb_get_device(state->deviceHandle)); + + char serialNumber[8 + 1] = { 0 }; + int getStringDescResult = libusb_get_string_descriptor_ascii(state->deviceHandle, 3, (unsigned char *) serialNumber, + 8 + 1); + + // Check serial number success and length. + if (getStringDescResult < 0 || getStringDescResult > 8) { + dvs128DeviceClose(state->deviceHandle); + libusb_exit(state->deviceContext); + free(handle); + + caerLog(CAER_LOG_CRITICAL, __func__, "Unable to get serial number for %s device.", DVS_DEVICE_NAME); + return (NULL); + } + + size_t fullLogStringLength = (size_t) snprintf(NULL, 0, "%s ID-%" PRIu16 " SN-%s [%" PRIu8 ":%" PRIu8 "]", + DVS_DEVICE_NAME, deviceID, serialNumber, busNumber, devAddress); + + char *fullLogString = malloc(fullLogStringLength + 1); + if (fullLogString == NULL) { + dvs128DeviceClose(state->deviceHandle); + libusb_exit(state->deviceContext); + free(handle); + + caerLog(CAER_LOG_CRITICAL, __func__, "Unable to allocate memory for %s device info string.", DVS_DEVICE_NAME); + return (NULL); + } + + snprintf(fullLogString, fullLogStringLength + 1, "%s ID-%" PRIu16 " SN-%s [%" PRIu8 ":%" PRIu8 "]", DVS_DEVICE_NAME, + deviceID, serialNumber, busNumber, devAddress); + + // Populate info variables based on data from device. + handle->info.deviceID = I16T(deviceID); + strncpy(handle->info.deviceSerialNumber, serialNumber, 8 + 1); + handle->info.deviceUSBBusNumber = busNumber; + handle->info.deviceUSBDeviceAddress = devAddress; + handle->info.deviceString = fullLogString; + handle->info.logicVersion = 1; // TODO: real logic revision, once that information is exposed by new logic. + handle->info.deviceIsMaster = true; // TODO: master/slave support. + handle->info.dvsSizeX = DVS_ARRAY_SIZE_X; + handle->info.dvsSizeY = DVS_ARRAY_SIZE_Y; + + caerLog(CAER_LOG_DEBUG, fullLogString, "Initialized device successfully with USB Bus=%" PRIu8 ":Addr=%" PRIu8 ".", + busNumber, devAddress); + + return ((caerDeviceHandle) handle); +} + +bool dvs128Close(caerDeviceHandle cdh) { + dvs128Handle handle = (dvs128Handle) cdh; + dvs128State state = &handle->state; + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "Shutting down ..."); + + // Finally, close the device fully. + dvs128DeviceClose(state->deviceHandle); + + // Destroy libusb context. + libusb_exit(state->deviceContext); + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "Shutdown successful."); + + // Free memory. + free(handle->info.deviceString); + free(handle); + + return (true); +} + +struct caer_dvs128_info caerDVS128InfoGet(caerDeviceHandle cdh) { + dvs128Handle handle = (dvs128Handle) cdh; + + // Check if the pointer is valid. + if (handle == NULL) { + struct caer_dvs128_info emptyInfo = { 0, .deviceString = NULL }; + return (emptyInfo); + } + + // Check if device type is supported. + if (handle->deviceType != CAER_DEVICE_DVS128) { + struct caer_dvs128_info emptyInfo = { 0, .deviceString = NULL }; + return (emptyInfo); + } + + // Return a copy of the device information. + return (handle->info); +} + +bool dvs128SendDefaultConfig(caerDeviceHandle cdh) { + dvs128Handle handle = (dvs128Handle) cdh; + dvs128State state = &handle->state; + + // Set all biases to default value. Based on DVS128 Fast biases. + caerIntegerToByteArray(1992, state->biases[DVS128_CONFIG_BIAS_CAS], BIAS_LENGTH); + caerIntegerToByteArray(1108364, state->biases[DVS128_CONFIG_BIAS_INJGND], BIAS_LENGTH); + caerIntegerToByteArray(16777215, state->biases[DVS128_CONFIG_BIAS_REQPD], BIAS_LENGTH); + caerIntegerToByteArray(8159221, state->biases[DVS128_CONFIG_BIAS_PUX], BIAS_LENGTH); + caerIntegerToByteArray(132, state->biases[DVS128_CONFIG_BIAS_DIFFOFF], BIAS_LENGTH); + caerIntegerToByteArray(309590, state->biases[DVS128_CONFIG_BIAS_REQ], BIAS_LENGTH); + caerIntegerToByteArray(969, state->biases[DVS128_CONFIG_BIAS_REFR], BIAS_LENGTH); + caerIntegerToByteArray(16777215, state->biases[DVS128_CONFIG_BIAS_PUY], BIAS_LENGTH); + caerIntegerToByteArray(209996, state->biases[DVS128_CONFIG_BIAS_DIFFON], BIAS_LENGTH); + caerIntegerToByteArray(13125, state->biases[DVS128_CONFIG_BIAS_DIFF], BIAS_LENGTH); + caerIntegerToByteArray(271, state->biases[DVS128_CONFIG_BIAS_FOLL], BIAS_LENGTH); + caerIntegerToByteArray(217, state->biases[DVS128_CONFIG_BIAS_PR], BIAS_LENGTH); + + // Send biases to device. + return (dvs128SendBiases(state)); +} + +bool dvs128ConfigSet(caerDeviceHandle cdh, int8_t modAddr, uint8_t paramAddr, uint32_t param) { + dvs128Handle handle = (dvs128Handle) cdh; + dvs128State state = &handle->state; + + switch (modAddr) { + case CAER_HOST_CONFIG_USB: + switch (paramAddr) { + case CAER_HOST_CONFIG_USB_BUFFER_NUMBER: + atomic_store(&state->usbBufferNumber, param); + + // Notify data acquisition thread to change buffers. + atomic_fetch_or(&state->dataAcquisitionThreadConfigUpdate, 1 << 0); + break; + + case CAER_HOST_CONFIG_USB_BUFFER_SIZE: + atomic_store(&state->usbBufferSize, param); + + // Notify data acquisition thread to change buffers. + atomic_fetch_or(&state->dataAcquisitionThreadConfigUpdate, 1 << 0); + break; + + default: + return (false); + break; + } + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE: + switch (paramAddr) { + case CAER_HOST_CONFIG_DATAEXCHANGE_BUFFER_SIZE: + atomic_store(&state->dataExchangeBufferSize, param); + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE_BLOCKING: + atomic_store(&state->dataExchangeBlocking, param); + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE_START_PRODUCERS: + atomic_store(&state->dataExchangeStartProducers, param); + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE_STOP_PRODUCERS: + atomic_store(&state->dataExchangeStopProducers, param); + break; + + default: + return (false); + break; + } + break; + + case CAER_HOST_CONFIG_PACKETS: + switch (paramAddr) { + case CAER_HOST_CONFIG_PACKETS_MAX_CONTAINER_PACKET_SIZE: + atomic_store(&state->maxPacketContainerPacketSize, param); + break; + + case CAER_HOST_CONFIG_PACKETS_MAX_CONTAINER_INTERVAL: + atomic_store(&state->maxPacketContainerInterval, param); + break; + + default: + return (false); + break; + } + break; + + case DVS128_CONFIG_DVS: + switch (paramAddr) { + case DVS128_CONFIG_DVS_RUN: + if (param && !atomic_load(&state->dvsRunning)) { + if (libusb_control_transfer(state->deviceHandle, + LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_START_TRANSFER, 0, 0, NULL, 0, 0) != 0) { + return (false); + } + + atomic_store(&state->dvsRunning, true); + } + else if (!param && atomic_load(&state->dvsRunning)) { + if (libusb_control_transfer(state->deviceHandle, + LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_STOP_TRANSFER, 0, 0, NULL, 0, 0) != 0) { + return (false); + } + + atomic_store(&state->dvsRunning, false); + } + break; + + case DVS128_CONFIG_DVS_TIMESTAMP_RESET: + if (param) { + if (libusb_control_transfer(state->deviceHandle, + LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_RESET_TS, 0, 0, NULL, 0, 0) != 0) { + return (false); + } + } + break; + + case DVS128_CONFIG_DVS_ARRAY_RESET: + if (param) { + if (libusb_control_transfer(state->deviceHandle, + LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_RESET_ARRAY, 0, 0, NULL, 0, 0) != 0) { + return (false); + } + } + break; + + case DVS128_CONFIG_DVS_TS_MASTER: + if (libusb_control_transfer(state->deviceHandle, + LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_TS_MASTER, (param & 0x01), 0, NULL, 0, 0) != 0) { + return (false); + } + atomic_store(&state->dvsIsMaster, (param & 0x01)); + + // Ensure info struct also gets this update. + atomic_thread_fence(memory_order_seq_cst); + handle->info.deviceIsMaster = atomic_load(&state->dvsIsMaster); + atomic_thread_fence(memory_order_seq_cst); + break; + + default: + return (false); + break; + } + break; + + case DVS128_CONFIG_BIAS: + switch (paramAddr) { + case DVS128_CONFIG_BIAS_CAS: + case DVS128_CONFIG_BIAS_INJGND: + case DVS128_CONFIG_BIAS_PUX: + case DVS128_CONFIG_BIAS_PUY: + case DVS128_CONFIG_BIAS_REQPD: + case DVS128_CONFIG_BIAS_REQ: + case DVS128_CONFIG_BIAS_FOLL: + case DVS128_CONFIG_BIAS_PR: + case DVS128_CONFIG_BIAS_REFR: + case DVS128_CONFIG_BIAS_DIFF: + case DVS128_CONFIG_BIAS_DIFFON: + case DVS128_CONFIG_BIAS_DIFFOFF: + caerIntegerToByteArray(param, state->biases[paramAddr], BIAS_LENGTH); + return (dvs128SendBiases(state)); + break; + + default: + return (false); + break; + } + break; + + default: + return (false); + break; + } + + return (true); +} + +bool dvs128ConfigGet(caerDeviceHandle cdh, int8_t modAddr, uint8_t paramAddr, uint32_t *param) { + dvs128Handle handle = (dvs128Handle) cdh; + dvs128State state = &handle->state; + + switch (modAddr) { + case CAER_HOST_CONFIG_USB: + switch (paramAddr) { + case CAER_HOST_CONFIG_USB_BUFFER_NUMBER: + *param = U32T(atomic_load(&state->usbBufferNumber)); + break; + + case CAER_HOST_CONFIG_USB_BUFFER_SIZE: + *param = U32T(atomic_load(&state->usbBufferSize)); + break; + + default: + return (false); + break; + } + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE: + switch (paramAddr) { + case CAER_HOST_CONFIG_DATAEXCHANGE_BUFFER_SIZE: + *param = U32T(atomic_load(&state->dataExchangeBufferSize)); + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE_BLOCKING: + *param = atomic_load(&state->dataExchangeBlocking); + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE_START_PRODUCERS: + *param = atomic_load(&state->dataExchangeStartProducers); + break; + + case CAER_HOST_CONFIG_DATAEXCHANGE_STOP_PRODUCERS: + *param = atomic_load(&state->dataExchangeStopProducers); + break; + + default: + return (false); + break; + } + break; + + case CAER_HOST_CONFIG_PACKETS: + switch (paramAddr) { + case CAER_HOST_CONFIG_PACKETS_MAX_CONTAINER_PACKET_SIZE: + *param = U32T(atomic_load(&state->maxPacketContainerPacketSize)); + break; + + case CAER_HOST_CONFIG_PACKETS_MAX_CONTAINER_INTERVAL: + *param = U32T(atomic_load(&state->maxPacketContainerInterval)); + break; + + default: + return (false); + break; + } + break; + + case DVS128_CONFIG_DVS: + switch (paramAddr) { + case DVS128_CONFIG_DVS_RUN: + *param = atomic_load(&state->dvsRunning); + break; + + case DVS128_CONFIG_DVS_TIMESTAMP_RESET: + case DVS128_CONFIG_DVS_ARRAY_RESET: + // Always false because it's an impulse, it resets itself automatically. + *param = false; + break; + + case DVS128_CONFIG_DVS_TS_MASTER: + *param = atomic_load(&state->dvsIsMaster); + break; + + default: + return (false); + break; + } + break; + + case DVS128_CONFIG_BIAS: + switch (paramAddr) { + case DVS128_CONFIG_BIAS_CAS: + case DVS128_CONFIG_BIAS_INJGND: + case DVS128_CONFIG_BIAS_PUX: + case DVS128_CONFIG_BIAS_PUY: + case DVS128_CONFIG_BIAS_REQPD: + case DVS128_CONFIG_BIAS_REQ: + case DVS128_CONFIG_BIAS_FOLL: + case DVS128_CONFIG_BIAS_PR: + case DVS128_CONFIG_BIAS_REFR: + case DVS128_CONFIG_BIAS_DIFF: + case DVS128_CONFIG_BIAS_DIFFON: + case DVS128_CONFIG_BIAS_DIFFOFF: + *param = caerByteArrayToInteger(state->biases[paramAddr], BIAS_LENGTH); + break; + + default: + return (false); + break; + } + break; + + default: + return (false); + break; + } + + return (true); +} + +bool dvs128DataStart(caerDeviceHandle cdh, void (*dataNotifyIncrease)(void *ptr), void (*dataNotifyDecrease)(void *ptr), + void *dataNotifyUserPtr, void (*dataShutdownNotify)(void *ptr), void *dataShutdownUserPtr) { + dvs128Handle handle = (dvs128Handle) cdh; + dvs128State state = &handle->state; + + // Store new data available/not available anymore call-backs. + state->dataNotifyIncrease = dataNotifyIncrease; + state->dataNotifyDecrease = dataNotifyDecrease; + state->dataNotifyUserPtr = dataNotifyUserPtr; + state->dataShutdownNotify = dataShutdownNotify; + state->dataShutdownUserPtr = dataShutdownUserPtr; + + // Set wanted time interval to uninitialized. Getting the first TS or TS_RESET + // will then set this correctly. + state->currentPacketContainerCommitTimestamp = -1; + + // Initialize RingBuffer. + state->dataExchangeBuffer = ringBufferInit(atomic_load(&state->dataExchangeBufferSize)); + if (state->dataExchangeBuffer == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to initialize data exchange buffer."); + return (false); + } + + // Allocate packets. + state->currentPacketContainer = caerEventPacketContainerAllocate(DVS_EVENT_TYPES); + if (state->currentPacketContainer == NULL) { + freeAllDataMemory(state); + + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate event packet container."); + return (false); + } + + state->currentPolarityPacket = caerPolarityEventPacketAllocate(DVS_POLARITY_DEFAULT_SIZE, + I16T(handle->info.deviceID), 0); + if (state->currentPolarityPacket == NULL) { + freeAllDataMemory(state); + + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate polarity event packet."); + return (false); + } + + state->currentSpecialPacket = caerSpecialEventPacketAllocate(DVS_SPECIAL_DEFAULT_SIZE, I16T(handle->info.deviceID), + 0); + if (state->currentSpecialPacket == NULL) { + freeAllDataMemory(state); + + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate special event packet."); + return (false); + } + + if ((errno = thrd_create(&state->dataAcquisitionThread, &dvs128DataAcquisitionThread, handle)) != thrd_success) { + freeAllDataMemory(state); + + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to start data acquisition thread. Error: %d.", + errno); + return (false); + } + + // Wait for the data acquisition thread to be ready. + while (!atomic_load_explicit(&state->dataAcquisitionThreadRun, memory_order_relaxed)) { + ; + } + + return (true); +} + +bool dvs128DataStop(caerDeviceHandle cdh) { + dvs128Handle handle = (dvs128Handle) cdh; + dvs128State state = &handle->state; + + // Stop data acquisition thread. + if (atomic_load(&state->dataExchangeStopProducers)) { + // Disable data transfer on USB end-point 6. + dvs128ConfigSet((caerDeviceHandle) handle, DVS128_CONFIG_DVS, DVS128_CONFIG_DVS_RUN, false); + } + + atomic_store(&state->dataAcquisitionThreadRun, false); + + // Wait for data acquisition thread to terminate... + if ((errno = thrd_join(state->dataAcquisitionThread, NULL)) != thrd_success) { + // This should never happen! + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to join data acquisition thread. Error: %d.", + errno); + return (false); + } + + // Empty ringbuffer. + caerEventPacketContainer container; + while ((container = ringBufferGet(state->dataExchangeBuffer)) != NULL) { + // Notify data-not-available call-back. + if (state->dataNotifyDecrease != NULL) { + state->dataNotifyDecrease(state->dataNotifyUserPtr); + } + + // Free container, which will free its subordinate packets too. + caerEventPacketContainerFree(container); + } + + // Free current, uncommitted packets and ringbuffer. + freeAllDataMemory(state); + + // Reset packet positions. + state->currentPolarityPacketPosition = 0; + state->currentSpecialPacketPosition = 0; + + return (true); +} + +// Remember to properly free the returned memory after usage! +caerEventPacketContainer dvs128DataGet(caerDeviceHandle cdh) { + dvs128Handle handle = (dvs128Handle) cdh; + dvs128State state = &handle->state; + caerEventPacketContainer container = NULL; + + retry: container = ringBufferGet(state->dataExchangeBuffer); + + if (container != NULL) { + // Found an event container, return it and signal this piece of data + // is no longer available for later acquisition. + if (state->dataNotifyDecrease != NULL) { + state->dataNotifyDecrease(state->dataNotifyUserPtr); + } + + return (container); + } + + // Didn't find any event container, either report this or retry, depending + // on blocking setting. + if (atomic_load_explicit(&state->dataExchangeBlocking, memory_order_relaxed)) { + // Don't retry right away in a tight loop, back off and wait a little. + // If no data is available, sleep for a millisecond to avoid wasting resources. + struct timespec noDataSleep = { .tv_sec = 0, .tv_nsec = 1000000 }; + if (thrd_sleep(&noDataSleep, NULL) == 0) { + goto retry; + } + } + + // Nothing. + return (NULL); +} + +static libusb_device_handle *dvs128DeviceOpen(libusb_context *devContext, uint16_t devVID, uint16_t devPID, + uint8_t devType, uint8_t busNumber, uint8_t devAddress, const char *serialNumber, uint16_t requiredFirmwareVersion) { + libusb_device_handle *devHandle = NULL; + libusb_device **devicesList; + + ssize_t result = libusb_get_device_list(devContext, &devicesList); + + if (result >= 0) { + // Cycle thorough all discovered devices and find a match. + for (size_t i = 0; i < (size_t) result; i++) { + struct libusb_device_descriptor devDesc; + + if (libusb_get_device_descriptor(devicesList[i], &devDesc) != LIBUSB_SUCCESS) { + continue; + } + + // Check if this is the device we want (VID/PID). + if (devDesc.idVendor == devVID && devDesc.idProduct == devPID + && U8T((devDesc.bcdDevice & 0xFF00) >> 8) == devType) { + // Verify device firmware version. + if (U8T(devDesc.bcdDevice & 0x00FF) < requiredFirmwareVersion) { + caerLog(CAER_LOG_CRITICAL, __func__, + "Device firmware version too old. You have version %" PRIu8 "; but at least version %" PRIu16 " is required. Please updated by following the Flashy upgrade documentation at 'http://inilabs.com/support/reflashing/'.", + U8T(devDesc.bcdDevice & 0x00FF), requiredFirmwareVersion); + + continue; + } + + // If a USB port restriction is given, honor it. + if (busNumber > 0 && libusb_get_bus_number(devicesList[i]) != busNumber) { + caerLog(CAER_LOG_INFO, __func__, + "USB bus number restriction is present (%" PRIu8 "), this device didn't match it (%" PRIu8 ").", + busNumber, libusb_get_bus_number(devicesList[i])); + + continue; + } + + if (devAddress > 0 && libusb_get_device_address(devicesList[i]) != devAddress) { + caerLog(CAER_LOG_INFO, __func__, + "USB device address restriction is present (%" PRIu8 "), this device didn't match it (%" PRIu8 ").", + devAddress, libusb_get_device_address(devicesList[i])); + + continue; + } + + if (libusb_open(devicesList[i], &devHandle) != LIBUSB_SUCCESS) { + devHandle = NULL; + + continue; + } + + // Check the serial number restriction, if any is present. + if (serialNumber != NULL && !caerStrEquals(serialNumber, "")) { + char deviceSerialNumber[8 + 1] = { 0 }; + int getStringDescResult = libusb_get_string_descriptor_ascii(devHandle, devDesc.iSerialNumber, + (unsigned char *) deviceSerialNumber, 8 + 1); + + // Check serial number success and length. + if (getStringDescResult < 0 || getStringDescResult > 8) { + libusb_close(devHandle); + devHandle = NULL; + + continue; + } + + // Now check if the Serial Number matches. + if (!caerStrEquals(serialNumber, deviceSerialNumber)) { + libusb_close(devHandle); + devHandle = NULL; + + caerLog(CAER_LOG_INFO, __func__, + "USB serial number restriction is present (%s), this device didn't match it (%s).", + serialNumber, deviceSerialNumber); + + continue; + } + } + + // Check that the active configuration is set to number 1. If not, do so. + int activeConfiguration; + if (libusb_get_configuration(devHandle, &activeConfiguration) != LIBUSB_SUCCESS) { + libusb_close(devHandle); + devHandle = NULL; + + continue; + } + + if (activeConfiguration != 1) { + if (libusb_set_configuration(devHandle, 1) != LIBUSB_SUCCESS) { + libusb_close(devHandle); + devHandle = NULL; + + continue; + } + } + + // Claim interface 0 (default). + if (libusb_claim_interface(devHandle, 0) != LIBUSB_SUCCESS) { + libusb_close(devHandle); + devHandle = NULL; + + continue; + } + + // TODO: check logic revision, once that information is exposed by new logic. + + // Found and configured it! + break; + } + } + + libusb_free_device_list(devicesList, true); + } + + return (devHandle); +} + +static void dvs128DeviceClose(libusb_device_handle *devHandle) { + // Release interface 0 (default). + libusb_release_interface(devHandle, 0); + + libusb_close(devHandle); +} + +static void dvs128AllocateTransfers(dvs128Handle handle, uint32_t bufferNum, uint32_t bufferSize) { + dvs128State state = &handle->state; + + // Set number of transfers and allocate memory for the main transfer array. + state->dataTransfers = calloc(bufferNum, sizeof(struct libusb_transfer *)); + if (state->dataTransfers == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, + "Failed to allocate memory for %" PRIu32 " libusb transfers. Error: %d.", bufferNum, errno); + return; + } + state->dataTransfersLength = bufferNum; + + // Allocate transfers and set them up. + for (size_t i = 0; i < bufferNum; i++) { + state->dataTransfers[i] = libusb_alloc_transfer(0); + if (state->dataTransfers[i] == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, + "Unable to allocate further libusb transfers (%zu of %" PRIu32 ").", i, bufferNum); + continue; + } + + // Create data buffer. + state->dataTransfers[i]->length = (int) bufferSize; + state->dataTransfers[i]->buffer = malloc(bufferSize); + if (state->dataTransfers[i]->buffer == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, + "Unable to allocate buffer for libusb transfer %zu. Error: %d.", i, errno); + + libusb_free_transfer(state->dataTransfers[i]); + state->dataTransfers[i] = NULL; + + continue; + } + + // Initialize Transfer. + state->dataTransfers[i]->dev_handle = state->deviceHandle; + state->dataTransfers[i]->endpoint = DVS_DATA_ENDPOINT; + state->dataTransfers[i]->type = LIBUSB_TRANSFER_TYPE_BULK; + state->dataTransfers[i]->callback = &dvs128LibUsbCallback; + state->dataTransfers[i]->user_data = handle; + state->dataTransfers[i]->timeout = 0; + state->dataTransfers[i]->flags = LIBUSB_TRANSFER_FREE_BUFFER; + + if ((errno = libusb_submit_transfer(state->dataTransfers[i])) == LIBUSB_SUCCESS) { + state->activeDataTransfers++; + } + else { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, + "Unable to submit libusb transfer %zu. Error: %s (%d).", i, libusb_strerror(errno), errno); + + // The transfer buffer is freed automatically here thanks to + // the LIBUSB_TRANSFER_FREE_BUFFER flag set above. + libusb_free_transfer(state->dataTransfers[i]); + state->dataTransfers[i] = NULL; + + continue; + } + } + + if (state->activeDataTransfers == 0) { + // Didn't manage to allocate any USB transfers, free array memory and log failure. + free(state->dataTransfers); + state->dataTransfers = NULL; + state->dataTransfersLength = 0; + + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Unable to allocate any libusb transfers."); + } +} + +static void dvs128DeallocateTransfers(dvs128Handle handle) { + dvs128State state = &handle->state; + + // Cancel all current transfers first. + for (size_t i = 0; i < state->dataTransfersLength; i++) { + if (state->dataTransfers[i] != NULL) { + errno = libusb_cancel_transfer(state->dataTransfers[i]); + if (errno != LIBUSB_SUCCESS && errno != LIBUSB_ERROR_NOT_FOUND) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, + "Unable to cancel libusb transfer %zu. Error: %s (%d).", i, libusb_strerror(errno), errno); + // Proceed with trying to cancel all transfers regardless of errors. + } + } + } + + // Wait for all transfers to go away (0.1 seconds timeout). + struct timeval te = { .tv_sec = 0, .tv_usec = 100000 }; + + while (state->activeDataTransfers > 0) { + libusb_handle_events_timeout(state->deviceContext, &te); + } + + // The buffers and transfers have been deallocated in the callback. + // Only the transfers array remains, which we free here. + free(state->dataTransfers); + state->dataTransfers = NULL; + state->dataTransfersLength = 0; +} + +static void LIBUSB_CALL dvs128LibUsbCallback(struct libusb_transfer *transfer) { + dvs128Handle handle = transfer->user_data; + dvs128State state = &handle->state; + + if (transfer->status == LIBUSB_TRANSFER_COMPLETED) { + // Handle data. + dvs128EventTranslator(handle, transfer->buffer, (size_t) transfer->actual_length); + } + + if (transfer->status != LIBUSB_TRANSFER_CANCELLED && transfer->status != LIBUSB_TRANSFER_NO_DEVICE) { + // Submit transfer again. + if (libusb_submit_transfer(transfer) == LIBUSB_SUCCESS) { + return; + } + } + + // Cannot recover (cancelled, no device, or other critical error). + // Signal this by adjusting the counter, free and exit. + state->activeDataTransfers--; + for (size_t i = 0; i < state->dataTransfersLength; i++) { + // Remove from list, so we don't try to cancel it later on. + if (state->dataTransfers[i] == transfer) { + state->dataTransfers[i] = NULL; + } + } + libusb_free_transfer(transfer); +} + +#define DVS128_TIMESTAMP_WRAP_MASK 0x80 +#define DVS128_TIMESTAMP_RESET_MASK 0x40 +#define DVS128_POLARITY_SHIFT 0 +#define DVS128_POLARITY_MASK 0x0001 +#define DVS128_Y_ADDR_SHIFT 8 +#define DVS128_Y_ADDR_MASK 0x007F +#define DVS128_X_ADDR_SHIFT 1 +#define DVS128_X_ADDR_MASK 0x007F +#define DVS128_SYNC_EVENT_MASK 0x8000 +#define TS_WRAP_ADD 0x4000 + +static inline int64_t generateFullTimestamp(int32_t tsOverflow, int32_t timestamp) { + return (I64T((U64T(tsOverflow) << TS_OVERFLOW_SHIFT) | U64T(timestamp))); +} + +static inline void initContainerCommitTimestamp(dvs128State state) { + if (state->currentPacketContainerCommitTimestamp == -1) { + state->currentPacketContainerCommitTimestamp = state->currentTimestamp + + atomic_load_explicit(&state->maxPacketContainerInterval, memory_order_relaxed) - 1; + } +} + +static void dvs128EventTranslator(dvs128Handle handle, uint8_t *buffer, size_t bytesSent) { + dvs128State state = &handle->state; + + // Return right away if not running anymore. This prevents useless work if many + // buffers are still waiting when shut down, as well as incorrect event sequences + // if a TS_RESET is stuck on ring-buffer commit further down, and detects shut-down; + // then any subsequent buffers should also detect shut-down and not be handled. + if (!atomic_load_explicit(&state->dataAcquisitionThreadRun, memory_order_relaxed)) { + return; + } + + // Truncate off any extra partial event. + if ((bytesSent & 0x03) != 0) { + caerLog(CAER_LOG_ALERT, handle->info.deviceString, + "%zu bytes received via USB, which is not a multiple of four.", bytesSent); + bytesSent &= (size_t) ~0x03; + } + + for (size_t i = 0; i < bytesSent; i += 4) { + // Allocate new packets for next iteration as needed. + if (state->currentPacketContainer == NULL) { + state->currentPacketContainer = caerEventPacketContainerAllocate(DVS_EVENT_TYPES); + if (state->currentPacketContainer == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate event packet container."); + return; + } + } + + if (state->currentPolarityPacket == NULL) { + state->currentPolarityPacket = caerPolarityEventPacketAllocate(DVS_POLARITY_DEFAULT_SIZE, + I16T(handle->info.deviceID), state->wrapOverflow); + if (state->currentPolarityPacket == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate polarity event packet."); + return; + } + } + else if (state->currentPolarityPacketPosition + >= caerEventPacketHeaderGetEventCapacity((caerEventPacketHeader) state->currentPolarityPacket)) { + // If not committed, let's check if any of the packets has reached its maximum + // capacity limit. If yes, we grow them to accomodate new events. + caerPolarityEventPacket grownPacket = (caerPolarityEventPacket) caerGenericEventPacketGrow( + (caerEventPacketHeader) state->currentPolarityPacket, state->currentPolarityPacketPosition * 2); + if (grownPacket == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to grow polarity event packet."); + return; + } + + state->currentPolarityPacket = grownPacket; + } + + if (state->currentSpecialPacket == NULL) { + state->currentSpecialPacket = caerSpecialEventPacketAllocate(DVS_SPECIAL_DEFAULT_SIZE, + I16T(handle->info.deviceID), state->wrapOverflow); + if (state->currentSpecialPacket == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to allocate special event packet."); + return; + } + } + else if (state->currentSpecialPacketPosition + >= caerEventPacketHeaderGetEventCapacity((caerEventPacketHeader) state->currentSpecialPacket)) { + // If not committed, let's check if any of the packets has reached its maximum + // capacity limit. If yes, we grow them to accomodate new events. + caerSpecialEventPacket grownPacket = (caerSpecialEventPacket) caerGenericEventPacketGrow( + (caerEventPacketHeader) state->currentSpecialPacket, state->currentSpecialPacketPosition * 2); + if (grownPacket == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, "Failed to grow special event packet."); + return; + } + + state->currentSpecialPacket = grownPacket; + } + + bool tsReset = false; + bool tsBigWrap = false; + + if ((buffer[i + 3] & DVS128_TIMESTAMP_WRAP_MASK) == DVS128_TIMESTAMP_WRAP_MASK) { + // Detect big timestamp wrap-around. + if (state->wrapAdd == (INT32_MAX - (TS_WRAP_ADD - 1))) { + // Reset wrapAdd to zero at this point, so we can again + // start detecting overruns of the 32bit value. + state->wrapAdd = 0; + + state->lastTimestamp = 0; + state->currentTimestamp = 0; + + // Increment TSOverflow counter. + state->wrapOverflow++; + + caerSpecialEvent currentEvent = caerSpecialEventPacketGetEvent(state->currentSpecialPacket, + state->currentSpecialPacketPosition++); + caerSpecialEventSetTimestamp(currentEvent, INT32_MAX); + caerSpecialEventSetType(currentEvent, TIMESTAMP_WRAP); + caerSpecialEventValidate(currentEvent, state->currentSpecialPacket); + + // Commit packets to separate before wrap from after cleanly. + tsBigWrap = true; + } + else { + // timestamp bit 15 is one -> wrap: now we need to increment + // the wrapAdd, uses only 14 bit timestamps. Each wrap is 2^14 µs (~16ms). + state->wrapAdd += TS_WRAP_ADD; + + state->lastTimestamp = state->currentTimestamp; + state->currentTimestamp = state->wrapAdd; + initContainerCommitTimestamp(state); + + // Check monotonicity of timestamps. + checkMonotonicTimestamp(handle); + } + } + else if ((buffer[i + 3] & DVS128_TIMESTAMP_RESET_MASK) == DVS128_TIMESTAMP_RESET_MASK) { + // timestamp bit 14 is one -> wrapAdd reset: this firmware + // version uses reset events to reset timestamps + state->wrapOverflow = 0; + state->wrapAdd = 0; + state->lastTimestamp = 0; + state->currentTimestamp = 0; + state->currentPacketContainerCommitTimestamp = -1; + initContainerCommitTimestamp(state); + + // Defer timestamp reset event to later, so we commit it + // alone, in its own packet. + // Commit packets when doing a reset to clearly separate them. + tsReset = true; + } + else { + // address is LSB MSB (USB is LE) + uint16_t addressUSB = le16toh(*((uint16_t * ) (&buffer[i]))); + + // same for timestamp, LSB MSB (USB is LE) + // 15 bit value of timestamp in 1 us tick + uint16_t timestampUSB = le16toh(*((uint16_t * ) (&buffer[i + 2]))); + + // Expand to 32 bits. (Tick is 1µs already.) + state->lastTimestamp = state->currentTimestamp; + state->currentTimestamp = state->wrapAdd + timestampUSB; + initContainerCommitTimestamp(state); + + // Check monotonicity of timestamps. + checkMonotonicTimestamp(handle); + + if ((addressUSB & DVS128_SYNC_EVENT_MASK) != 0) { + // Special Trigger Event (MSB is set) + caerSpecialEvent currentEvent = caerSpecialEventPacketGetEvent(state->currentSpecialPacket, + state->currentSpecialPacketPosition++); + caerSpecialEventSetTimestamp(currentEvent, state->currentTimestamp); + caerSpecialEventSetType(currentEvent, EXTERNAL_INPUT_RISING_EDGE); + caerSpecialEventValidate(currentEvent, state->currentSpecialPacket); + } + else { + // Invert X values (flip along X axis). To correct for flipped camera. + uint16_t x = U16T( + (DVS_ARRAY_SIZE_X - 1) - U16T((addressUSB >> DVS128_X_ADDR_SHIFT) & DVS128_X_ADDR_MASK)); + // Invert Y values (flip along Y axis). To convert to CG format. + uint16_t y = U16T( + (DVS_ARRAY_SIZE_Y - 1) - U16T((addressUSB >> DVS128_Y_ADDR_SHIFT) & DVS128_Y_ADDR_MASK)); + // Invert polarity bit. Hardware is like this. + bool polarity = (((addressUSB >> DVS128_POLARITY_SHIFT) & DVS128_POLARITY_MASK) == 0) ? (1) : (0); + + // Check range conformity. + if (x >= DVS_ARRAY_SIZE_X) { + caerLog(CAER_LOG_ALERT, handle->info.deviceString, "X address out of range (0-%d): %" PRIu16 ".", + DVS_ARRAY_SIZE_X - 1, x); + continue; // Skip invalid event. + } + if (y >= DVS_ARRAY_SIZE_Y) { + caerLog(CAER_LOG_ALERT, handle->info.deviceString, "Y address out of range (0-%d): %" PRIu16 ".", + DVS_ARRAY_SIZE_Y - 1, y); + continue; // Skip invalid event. + } + + caerPolarityEvent currentEvent = caerPolarityEventPacketGetEvent(state->currentPolarityPacket, + state->currentPolarityPacketPosition++); + caerPolarityEventSetTimestamp(currentEvent, state->currentTimestamp); + caerPolarityEventSetPolarity(currentEvent, polarity); + caerPolarityEventSetY(currentEvent, y); + caerPolarityEventSetX(currentEvent, x); + caerPolarityEventValidate(currentEvent, state->currentPolarityPacket); + } + } + + // Thresholds on which to trigger packet container commit. + // forceCommit is already defined above. + // Trigger if any of the global container-wide thresholds are met. + int32_t currentPacketContainerCommitSize = I32T( + atomic_load_explicit(&state->maxPacketContainerPacketSize, memory_order_relaxed)); + bool containerSizeCommit = (currentPacketContainerCommitSize > 0) + && ((state->currentPolarityPacketPosition >= currentPacketContainerCommitSize) + || (state->currentSpecialPacketPosition >= currentPacketContainerCommitSize)); + + bool containerTimeCommit = generateFullTimestamp(state->wrapOverflow, state->currentTimestamp) + > state->currentPacketContainerCommitTimestamp; + + // FIXME: with the current DVS128 architecture, currentTimestamp always comes together + // with an event, so the very first event that matches this threshold will be + // also part of the committed packet container. This doesn't break any of the invariants. + + // Commit packet containers to the ring-buffer, so they can be processed by the + // main-loop, when any of the required conditions are met. + if (tsReset || tsBigWrap || containerSizeCommit || containerTimeCommit) { + // One or more of the commit triggers are hit. Set the packet container up to contain + // any non-empty packets. Empty packets are not forwarded to save memory. + bool emptyContainerCommit = true; + + if (state->currentPolarityPacketPosition > 0) { + caerEventPacketContainerSetEventPacket(state->currentPacketContainer, POLARITY_EVENT, + (caerEventPacketHeader) state->currentPolarityPacket); + + state->currentPolarityPacket = NULL; + state->currentPolarityPacketPosition = 0; + emptyContainerCommit = false; + } + + if (state->currentSpecialPacketPosition > 0) { + caerEventPacketContainerSetEventPacket(state->currentPacketContainer, SPECIAL_EVENT, + (caerEventPacketHeader) state->currentSpecialPacket); + + state->currentSpecialPacket = NULL; + state->currentSpecialPacketPosition = 0; + emptyContainerCommit = false; + } + + // If the commit was triggered by a packet container limit being reached, we always + // update the time related limit. The size related one is updated implicitly by size + // being reset to zero after commit (new packets are empty). + if (containerTimeCommit) { + while (generateFullTimestamp(state->wrapOverflow, state->currentTimestamp) + > state->currentPacketContainerCommitTimestamp) { + state->currentPacketContainerCommitTimestamp += atomic_load_explicit( + &state->maxPacketContainerInterval, memory_order_relaxed); + } + } + + // Filter out completely empty commits. This can happen when data is turned off, + // but the timestamps are still going forward. + if (emptyContainerCommit) { + caerEventPacketContainerFree(state->currentPacketContainer); + state->currentPacketContainer = NULL; + } + else { + if (!ringBufferPut(state->dataExchangeBuffer, state->currentPacketContainer)) { + // Failed to forward packet container, just drop it, it doesn't contain + // any critical information anyway. + caerLog(CAER_LOG_INFO, handle->info.deviceString, + "Dropped EventPacket Container because ring-buffer full!"); + + caerEventPacketContainerFree(state->currentPacketContainer); + state->currentPacketContainer = NULL; + } + else { + if (state->dataNotifyIncrease != NULL) { + state->dataNotifyIncrease(state->dataNotifyUserPtr); + } + + state->currentPacketContainer = NULL; + } + } + + // The only critical timestamp information to forward is the timestamp reset event. + // The timestamp big-wrap can also (and should!) be detected by observing a packet's + // tsOverflow value, not the special packet TIMESTAMP_WRAP event, which is only informative. + // For the timestamp reset event (TIMESTAMP_RESET), we thus ensure that it is always + // committed, and we send it alone, in its own packet container, to ensure it will always + // be ordered after any other event packets in any processing or output stream. + if (tsReset) { + // Allocate packet container just for this event. + caerEventPacketContainer tsResetContainer = caerEventPacketContainerAllocate(DVS_EVENT_TYPES); + if (tsResetContainer == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, + "Failed to allocate tsReset event packet container."); + return; + } + + // Allocate special packet just for this event. + caerSpecialEventPacket tsResetPacket = caerSpecialEventPacketAllocate(1, I16T(handle->info.deviceID), + state->wrapOverflow); + if (tsResetPacket == NULL) { + caerLog(CAER_LOG_CRITICAL, handle->info.deviceString, + "Failed to allocate tsReset special event packet."); + return; + } + + // Create timestamp reset event. + caerSpecialEvent tsResetEvent = caerSpecialEventPacketGetEvent(tsResetPacket, 0); + caerSpecialEventSetTimestamp(tsResetEvent, INT32_MAX); + caerSpecialEventSetType(tsResetEvent, TIMESTAMP_RESET); + caerSpecialEventValidate(tsResetEvent, tsResetPacket); + + // Assign special packet to packet container. + caerEventPacketContainerSetEventPacket(tsResetContainer, SPECIAL_EVENT, + (caerEventPacketHeader) tsResetPacket); + + // Reset MUST be committed, always, else downstream data processing and + // outputs get confused if they have no notification of timestamps + // jumping back go zero. + while (!ringBufferPut(state->dataExchangeBuffer, tsResetContainer)) { + // Prevent dead-lock if shutdown is requested and nothing is consuming + // data anymore, but the ring-buffer is full (and would thus never empty), + // thus blocking the USB handling thread in this loop. + if (!atomic_load_explicit(&state->dataAcquisitionThreadRun, memory_order_relaxed)) { + return; + } + } + + // Signal new container as usual. + if (state->dataNotifyIncrease != NULL) { + state->dataNotifyIncrease(state->dataNotifyUserPtr); + } + } + } + } +} + +static bool dvs128SendBiases(dvs128State state) { + // Biases are already stored in an array with the same format as expected by + // the device, we can thus send it directly. + return (libusb_control_transfer(state->deviceHandle, + LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE, + VENDOR_REQUEST_SEND_BIASES, 0, 0, (uint8_t *) state->biases, (BIAS_NUMBER * BIAS_LENGTH), 0) + == (BIAS_NUMBER * BIAS_LENGTH)); +} + +static int dvs128DataAcquisitionThread(void *inPtr) { + // inPtr is a pointer to device handle. + dvs128Handle handle = inPtr; + dvs128State state = &handle->state; + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "Initializing data acquisition thread ..."); + + // Set thread name. + thrd_set_name(state->deviceThreadName); + + // Reset configuration update, so as to not re-do work afterwards. + atomic_store(&state->dataAcquisitionThreadConfigUpdate, 0); + + if (atomic_load(&state->dataExchangeStartProducers)) { + // Enable data transfer on USB end-point 6. + dvs128ConfigSet((caerDeviceHandle) handle, DVS128_CONFIG_DVS, DVS128_CONFIG_DVS_RUN, true); + } + + // Create buffers as specified in config file. + dvs128AllocateTransfers(handle, U32T(atomic_load(&state->usbBufferNumber)), + U32T(atomic_load(&state->usbBufferSize))); + + // Signal data thread ready back to start function. + atomic_store(&state->dataAcquisitionThreadRun, true); + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "data acquisition thread ready to process events."); + + // Handle USB events (1 second timeout). + struct timeval te = { .tv_sec = 1, .tv_usec = 0 }; + + while (atomic_load_explicit(&state->dataAcquisitionThreadRun, memory_order_relaxed) + && state->activeDataTransfers > 0) { + // Check config refresh, in this case to adjust buffer sizes. + if (atomic_load_explicit(&state->dataAcquisitionThreadConfigUpdate, memory_order_relaxed) != 0) { + dvs128DataAcquisitionThreadConfig(handle); + } + + libusb_handle_events_timeout(state->deviceContext, &te); + } + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "shutting down data acquisition thread ..."); + + // Cancel all transfers and handle them. + dvs128DeallocateTransfers(handle); + + // Ensure shutdown is stored and notified, could be because of all data transfers going away! + atomic_store(&state->dataAcquisitionThreadRun, false); + + if (state->dataShutdownNotify != NULL) { + state->dataShutdownNotify(state->dataShutdownUserPtr); + } + + caerLog(CAER_LOG_DEBUG, handle->info.deviceString, "data acquisition thread shut down."); + + return (EXIT_SUCCESS); +} + +static void dvs128DataAcquisitionThreadConfig(dvs128Handle handle) { + dvs128State state = &handle->state; + + // Get the current value to examine by atomic exchange, since we don't + // want there to be any possible store between a load/store pair. + uint32_t configUpdate = U32T(atomic_exchange(&state->dataAcquisitionThreadConfigUpdate, 0)); + + if ((configUpdate >> 0) & 0x01) { + // Do buffer size change: cancel all and recreate them. + dvs128DeallocateTransfers(handle); + dvs128AllocateTransfers(handle, U32T(atomic_load(&state->usbBufferNumber)), + U32T(atomic_load(&state->usbBufferSize))); + } +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/dvs128.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/dvs128.h new file mode 100755 index 0000000000000000000000000000000000000000..ef645923eeb8f1250e17d4a1bb56de78828d83ec --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/dvs128.h @@ -0,0 +1,119 @@ +#ifndef LIBCAER_SRC_DVS128_H_ +#define LIBCAER_SRC_DVS128_H_ + +#include "devices/dvs128.h" +#include "ringbuffer/ringbuffer.h" +#include <stdatomic.h> +#include <libusb.h> + +#ifdef HAVE_PTHREADS + #include "c11threads_posix.h" +#endif + +#define DVS_DEVICE_NAME "DVS128" + +#define DVS_DEVICE_VID 0x152A +#define DVS_DEVICE_PID 0x8400 +#define DVS_DEVICE_DID_TYPE 0x00 + +// #define DVS_REQUIRED_LOGIC_REVISION 1 +#define DVS_REQUIRED_FIRMWARE_VERSION 14 + +#define DVS_ARRAY_SIZE_X 128 +#define DVS_ARRAY_SIZE_Y 128 + +#define DVS_EVENT_TYPES 2 + +#define DVS_POLARITY_DEFAULT_SIZE 4096 +#define DVS_SPECIAL_DEFAULT_SIZE 128 + +#define DVS_DATA_ENDPOINT 0x86 + +#define VENDOR_REQUEST_START_TRANSFER 0xB3 +#define VENDOR_REQUEST_STOP_TRANSFER 0xB4 +#define VENDOR_REQUEST_SEND_BIASES 0xB8 +#define VENDOR_REQUEST_RESET_TS 0xBB +#define VENDOR_REQUEST_RESET_ARRAY 0xBD +#define VENDOR_REQUEST_TS_MASTER 0xBE + +#define BIAS_NUMBER 12 +#define BIAS_LENGTH 3 + +struct dvs128_state { + // Data Acquisition Thread -> Mainloop Exchange + RingBuffer dataExchangeBuffer; + atomic_uint_fast32_t dataExchangeBufferSize; // Only takes effect on DataStart() calls! + atomic_bool dataExchangeBlocking; + atomic_bool dataExchangeStartProducers; + atomic_bool dataExchangeStopProducers; + void (*dataNotifyIncrease)(void *ptr); + void (*dataNotifyDecrease)(void *ptr); + void *dataNotifyUserPtr; + void (*dataShutdownNotify)(void *ptr); + void *dataShutdownUserPtr; + // USB Device State + char deviceThreadName[15 + 1]; // +1 for terminating NUL character. + libusb_context *deviceContext; + libusb_device_handle *deviceHandle; + // USB Transfer Settings + atomic_uint_fast32_t usbBufferNumber; + atomic_uint_fast32_t usbBufferSize; + // Data Acquisition Thread + thrd_t dataAcquisitionThread; + atomic_bool dataAcquisitionThreadRun; + atomic_uint_fast32_t dataAcquisitionThreadConfigUpdate; + struct libusb_transfer **dataTransfers; + size_t dataTransfersLength; + size_t activeDataTransfers; + // Timestamp fields + int32_t wrapOverflow; + int32_t wrapAdd; + int32_t lastTimestamp; + int32_t currentTimestamp; + // Packet Container state + caerEventPacketContainer currentPacketContainer; + atomic_int_fast32_t maxPacketContainerPacketSize; + atomic_int_fast32_t maxPacketContainerInterval; + int64_t currentPacketContainerCommitTimestamp; + // Polarity Packet State + caerPolarityEventPacket currentPolarityPacket; + int32_t currentPolarityPacketPosition; + // Special Packet State + caerSpecialEventPacket currentSpecialPacket; + int32_t currentSpecialPacketPosition; + // Camera bias and settings memory (for getter operations) + // TODO: replace with real device calls once DVS128 logic rewritten. + uint8_t biases[BIAS_NUMBER][BIAS_LENGTH]; + atomic_bool dvsRunning; + atomic_bool dvsIsMaster; +}; + +typedef struct dvs128_state *dvs128State; + +struct dvs128_handle { + uint16_t deviceType; + // Information fields + struct caer_dvs128_info info; + // State for data management. + struct dvs128_state state; +}; + +typedef struct dvs128_handle *dvs128Handle; + +caerDeviceHandle dvs128Open(uint16_t deviceID, uint8_t busNumberRestrict, uint8_t devAddressRestrict, + const char *serialNumberRestrict); +bool dvs128Close(caerDeviceHandle handle); + +bool dvs128SendDefaultConfig(caerDeviceHandle handle); +// Negative addresses are used for host-side configuration. +// Positive addresses (including zero) are used for device-side configuration. +bool dvs128ConfigSet(caerDeviceHandle handle, int8_t modAddr, uint8_t paramAddr, uint32_t param); +bool dvs128ConfigGet(caerDeviceHandle handle, int8_t modAddr, uint8_t paramAddr, uint32_t *param); + +bool dvs128DataStart(caerDeviceHandle handle, void (*dataNotifyIncrease)(void *ptr), + void (*dataNotifyDecrease)(void *ptr), void *dataNotifyUserPtr, void (*dataShutdownNotify)(void *ptr), + void *dataShutdownUserPtr); +bool dvs128DataStop(caerDeviceHandle handle); +caerEventPacketContainer dvs128DataGet(caerDeviceHandle handle); + +#endif /* LIBCAER_SRC_DVS128_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/events.c b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/events.c new file mode 100755 index 0000000000000000000000000000000000000000..d55dfdbb91301656b0dd79a677171b8bb2a4e362 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/events.c @@ -0,0 +1,406 @@ +#include "events/packetContainer.h" +#include "events/special.h" +#include "events/polarity.h" +#include "events/frame.h" +#include "events/imu6.h" +#include "events/imu9.h" +#include "events/sample.h" +#include "events/ear.h" +#include "events/config.h" +#include "events/point1d.h" +#include "events/point2d.h" +#include "events/point3d.h" +#include "events/point4d.h" + +caerEventPacketContainer caerEventPacketContainerAllocate(int32_t eventPacketsNumber) { + if (eventPacketsNumber == 0) { + return (NULL); + } + + size_t eventPacketContainerSize = sizeof(struct caer_event_packet_container) + + ((size_t) eventPacketsNumber * sizeof(caerEventPacketHeader)); + + caerEventPacketContainer packetContainer = calloc(1, eventPacketContainerSize); + if (packetContainer == NULL) { + caerLog(CAER_LOG_CRITICAL, "EventPacket Container", + "Failed to allocate %zu bytes of memory for Event Packet Container, containing %" + PRIi32 " packets. Error: %d.", eventPacketContainerSize, eventPacketsNumber, errno); + return (NULL); + } + + // Fill in header fields. Don't care about endianness here, purely internal + // memory construct, never meant for inter-system exchange. + packetContainer->eventPacketsNumber = eventPacketsNumber; + packetContainer->lowestEventTimestamp = -1; + packetContainer->highestEventTimestamp = -1; + + return (packetContainer); +} + +void caerEventPacketContainerFree(caerEventPacketContainer container) { + if (container == NULL) { + return; + } + + // Free packet container and ensure all subordinate memory is also freed. + for (int32_t i = 0; i < caerEventPacketContainerGetEventPacketsNumber(container); i++) { + caerEventPacketHeader packetHeader = caerEventPacketContainerGetEventPacket(container, i); + + if (packetHeader != NULL) { + free(packetHeader); + } + } + + free(container); +} + +caerSpecialEventPacket caerSpecialEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow) { + if (eventCapacity == 0) { + return (NULL); + } + + size_t eventSize = sizeof(struct caer_special_event); + size_t eventPacketSize = sizeof(struct caer_special_event_packet) + ((size_t) eventCapacity * eventSize); + + // Zero out event memory (all events invalid). + caerSpecialEventPacket packet = calloc(1, eventPacketSize); + if (packet == NULL) { + caerLog(CAER_LOG_CRITICAL, "Special Event", + "Failed to allocate %zu bytes of memory for Special Event Packet of capacity %" + PRIi32 " from source %" PRIi16 ". Error: %d.", eventPacketSize, eventCapacity, eventSource, + errno); + return (NULL); + } + + // Fill in header fields. + caerEventPacketHeaderSetEventType(&packet->packetHeader, SPECIAL_EVENT); + caerEventPacketHeaderSetEventSource(&packet->packetHeader, eventSource); + caerEventPacketHeaderSetEventSize(&packet->packetHeader, I32T(eventSize)); + caerEventPacketHeaderSetEventTSOffset(&packet->packetHeader, offsetof(struct caer_special_event, timestamp)); + caerEventPacketHeaderSetEventTSOverflow(&packet->packetHeader, tsOverflow); + caerEventPacketHeaderSetEventCapacity(&packet->packetHeader, eventCapacity); + + return (packet); +} + +caerPolarityEventPacket caerPolarityEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow) { + if (eventCapacity == 0) { + return (NULL); + } + + size_t eventSize = sizeof(struct caer_polarity_event); + size_t eventPacketSize = sizeof(struct caer_polarity_event_packet) + ((size_t) eventCapacity * eventSize); + + // Zero out event memory (all events invalid). + caerPolarityEventPacket packet = calloc(1, eventPacketSize); + if (packet == NULL) { + caerLog(CAER_LOG_CRITICAL, "Polarity Event", + "Failed to allocate %zu bytes of memory for Polarity Event Packet of capacity %" + PRIi32 " from source %" PRIi16 ". Error: %d.", eventPacketSize, eventCapacity, eventSource, + errno); + return (NULL); + } + + // Fill in header fields. + caerEventPacketHeaderSetEventType(&packet->packetHeader, POLARITY_EVENT); + caerEventPacketHeaderSetEventSource(&packet->packetHeader, eventSource); + caerEventPacketHeaderSetEventSize(&packet->packetHeader, I32T(eventSize)); + caerEventPacketHeaderSetEventTSOffset(&packet->packetHeader, offsetof(struct caer_polarity_event, timestamp)); + caerEventPacketHeaderSetEventTSOverflow(&packet->packetHeader, tsOverflow); + caerEventPacketHeaderSetEventCapacity(&packet->packetHeader, eventCapacity); + + return (packet); +} + +caerFrameEventPacket caerFrameEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow, + int32_t maxLengthX, int32_t maxLengthY, int16_t maxChannelNumber) { + if (eventCapacity == 0) { + return (NULL); + } + + size_t pixelSize = sizeof(uint16_t) * (size_t) maxLengthX * (size_t) maxLengthY * (size_t) maxChannelNumber; + size_t eventSize = sizeof(struct caer_frame_event) + pixelSize; + size_t eventPacketSize = sizeof(struct caer_frame_event_packet) + ((size_t) eventCapacity * eventSize); + + // Zero out event memory (all events invalid). + caerFrameEventPacket packet = calloc(1, eventPacketSize); + if (packet == NULL) { + caerLog(CAER_LOG_CRITICAL, "Frame Event", + "Failed to allocate %zu bytes of memory for Frame Event Packet of capacity %" + PRIi32 " from source %" PRIi16 ". Error: %d.", eventPacketSize, eventCapacity, eventSource, + errno); + return (NULL); + } + + // Fill in header fields. + caerEventPacketHeaderSetEventType(&packet->packetHeader, FRAME_EVENT); + caerEventPacketHeaderSetEventSource(&packet->packetHeader, eventSource); + caerEventPacketHeaderSetEventSize(&packet->packetHeader, I32T(eventSize)); + caerEventPacketHeaderSetEventTSOffset(&packet->packetHeader, offsetof(struct caer_frame_event, ts_endframe)); + caerEventPacketHeaderSetEventTSOverflow(&packet->packetHeader, tsOverflow); + caerEventPacketHeaderSetEventCapacity(&packet->packetHeader, eventCapacity); + + return (packet); +} + +caerIMU6EventPacket caerIMU6EventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow) { + if (eventCapacity == 0) { + return (NULL); + } + + size_t eventSize = sizeof(struct caer_imu6_event); + size_t eventPacketSize = sizeof(struct caer_imu6_event_packet) + ((size_t) eventCapacity * eventSize); + + // Zero out event memory (all events invalid). + caerIMU6EventPacket packet = calloc(1, eventPacketSize); + if (packet == NULL) { + caerLog(CAER_LOG_CRITICAL, "IMU6 Event", + "Failed to allocate %zu bytes of memory for IMU6 Event Packet of capacity %" + PRIi32 " from source %" PRIi16 ". Error: %d.", eventPacketSize, eventCapacity, eventSource, + errno); + return (NULL); + } + + // Fill in header fields. + caerEventPacketHeaderSetEventType(&packet->packetHeader, IMU6_EVENT); + caerEventPacketHeaderSetEventSource(&packet->packetHeader, eventSource); + caerEventPacketHeaderSetEventSize(&packet->packetHeader, I32T(eventSize)); + caerEventPacketHeaderSetEventTSOffset(&packet->packetHeader, offsetof(struct caer_imu6_event, timestamp)); + caerEventPacketHeaderSetEventTSOverflow(&packet->packetHeader, tsOverflow); + caerEventPacketHeaderSetEventCapacity(&packet->packetHeader, eventCapacity); + + return (packet); +} + +caerIMU9EventPacket caerIMU9EventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow) { + if (eventCapacity == 0) { + return (NULL); + } + + size_t eventSize = sizeof(struct caer_imu9_event); + size_t eventPacketSize = sizeof(struct caer_imu9_event_packet) + ((size_t) eventCapacity * eventSize); + + // Zero out event memory (all events invalid). + caerIMU9EventPacket packet = calloc(1, eventPacketSize); + if (packet == NULL) { + caerLog(CAER_LOG_CRITICAL, "IMU9 Event", + "Failed to allocate %zu bytes of memory for IMU9 Event Packet of capacity %" + PRIi32 " from source %" PRIi16 ". Error: %d.", eventPacketSize, eventCapacity, eventSource, + errno); + return (NULL); + } + + // Fill in header fields. + caerEventPacketHeaderSetEventType(&packet->packetHeader, IMU9_EVENT); + caerEventPacketHeaderSetEventSource(&packet->packetHeader, eventSource); + caerEventPacketHeaderSetEventSize(&packet->packetHeader, I32T(eventSize)); + caerEventPacketHeaderSetEventTSOffset(&packet->packetHeader, offsetof(struct caer_imu9_event, timestamp)); + caerEventPacketHeaderSetEventTSOverflow(&packet->packetHeader, tsOverflow); + caerEventPacketHeaderSetEventCapacity(&packet->packetHeader, eventCapacity); + + return (packet); +} + +caerSampleEventPacket caerSampleEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow) { + if (eventCapacity == 0) { + return (NULL); + } + + size_t eventSize = sizeof(struct caer_sample_event); + size_t eventPacketSize = sizeof(struct caer_sample_event_packet) + ((size_t) eventCapacity * eventSize); + + // Zero out event memory (all events invalid). + caerSampleEventPacket packet = calloc(1, eventPacketSize); + if (packet == NULL) { + caerLog(CAER_LOG_CRITICAL, "Sample Event", + "Failed to allocate %zu bytes of memory for Sample Event Packet of capacity %" + PRIi32 " from source %" PRIi16 ". Error: %d.", eventPacketSize, eventCapacity, eventSource, + errno); + return (NULL); + } + + // Fill in header fields. + caerEventPacketHeaderSetEventType(&packet->packetHeader, SAMPLE_EVENT); + caerEventPacketHeaderSetEventSource(&packet->packetHeader, eventSource); + caerEventPacketHeaderSetEventSize(&packet->packetHeader, I32T(eventSize)); + caerEventPacketHeaderSetEventTSOffset(&packet->packetHeader, offsetof(struct caer_sample_event, timestamp)); + caerEventPacketHeaderSetEventTSOverflow(&packet->packetHeader, tsOverflow); + caerEventPacketHeaderSetEventCapacity(&packet->packetHeader, eventCapacity); + + return (packet); +} + +caerEarEventPacket caerEarEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow) { + if (eventCapacity == 0) { + return (NULL); + } + + size_t eventSize = sizeof(struct caer_ear_event); + size_t eventPacketSize = sizeof(struct caer_ear_event_packet) + ((size_t) eventCapacity * eventSize); + + // Zero out event memory (all events invalid). + caerEarEventPacket packet = calloc(1, eventPacketSize); + if (packet == NULL) { + caerLog(CAER_LOG_CRITICAL, "Ear Event", + "Failed to allocate %zu bytes of memory for Ear Event Packet of capacity %" + PRIi32 " from source %" PRIi16 ". Error: %d.", eventPacketSize, eventCapacity, eventSource, + errno); + return (NULL); + } + + // Fill in header fields. + caerEventPacketHeaderSetEventType(&packet->packetHeader, EAR_EVENT); + caerEventPacketHeaderSetEventSource(&packet->packetHeader, eventSource); + caerEventPacketHeaderSetEventSize(&packet->packetHeader, I32T(eventSize)); + caerEventPacketHeaderSetEventTSOffset(&packet->packetHeader, offsetof(struct caer_ear_event, timestamp)); + caerEventPacketHeaderSetEventTSOverflow(&packet->packetHeader, tsOverflow); + caerEventPacketHeaderSetEventCapacity(&packet->packetHeader, eventCapacity); + + return (packet); +} + +caerConfigurationEventPacket caerConfigurationEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, + int32_t tsOverflow) { + if (eventCapacity == 0) { + return (NULL); + } + + size_t eventSize = sizeof(struct caer_configuration_event); + size_t eventPacketSize = sizeof(struct caer_configuration_event_packet) + ((size_t) eventCapacity * eventSize); + + // Zero out event memory (all events invalid). + caerConfigurationEventPacket packet = calloc(1, eventPacketSize); + if (packet == NULL) { + caerLog(CAER_LOG_CRITICAL, "Configuration Event", + "Failed to allocate %zu bytes of memory for Configuration Event Packet of capacity %" + PRIi32 " from source %" PRIi16 ". Error: %d.", eventPacketSize, eventCapacity, eventSource, + errno); + return (NULL); + } + + // Fill in header fields. + caerEventPacketHeaderSetEventType(&packet->packetHeader, CONFIG_EVENT); + caerEventPacketHeaderSetEventSource(&packet->packetHeader, eventSource); + caerEventPacketHeaderSetEventSize(&packet->packetHeader, I32T(eventSize)); + caerEventPacketHeaderSetEventTSOffset(&packet->packetHeader, offsetof(struct caer_configuration_event, timestamp)); + caerEventPacketHeaderSetEventTSOverflow(&packet->packetHeader, tsOverflow); + caerEventPacketHeaderSetEventCapacity(&packet->packetHeader, eventCapacity); + + return (packet); +} + +caerPoint1DEventPacket caerPoint1DEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow) { + if (eventCapacity == 0) { + return (NULL); + } + + size_t eventSize = sizeof(struct caer_point1d_event); + size_t eventPacketSize = sizeof(struct caer_point1d_event_packet) + ((size_t) eventCapacity * eventSize); + + // Zero out event memory (all events invalid). + caerPoint1DEventPacket packet = calloc(1, eventPacketSize); + if (packet == NULL) { + caerLog(CAER_LOG_CRITICAL, "Point1D Event", + "Failed to allocate %zu bytes of memory for Point1D Event Packet of capacity %" + PRIi32 " from source %" PRIi16 ". Error: %d.", eventPacketSize, eventCapacity, eventSource, + errno); + return (NULL); + } + + // Fill in header fields. + caerEventPacketHeaderSetEventType(&packet->packetHeader, POINT1D_EVENT); + caerEventPacketHeaderSetEventSource(&packet->packetHeader, eventSource); + caerEventPacketHeaderSetEventSize(&packet->packetHeader, I32T(eventSize)); + caerEventPacketHeaderSetEventTSOffset(&packet->packetHeader, offsetof(struct caer_point1d_event, timestamp)); + caerEventPacketHeaderSetEventTSOverflow(&packet->packetHeader, tsOverflow); + caerEventPacketHeaderSetEventCapacity(&packet->packetHeader, eventCapacity); + + return (packet); +} + +caerPoint2DEventPacket caerPoint2DEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow) { + if (eventCapacity == 0) { + return (NULL); + } + + size_t eventSize = sizeof(struct caer_point2d_event); + size_t eventPacketSize = sizeof(struct caer_point2d_event_packet) + ((size_t) eventCapacity * eventSize); + + // Zero out event memory (all events invalid). + caerPoint2DEventPacket packet = calloc(1, eventPacketSize); + if (packet == NULL) { + caerLog(CAER_LOG_CRITICAL, "Point2D Event", + "Failed to allocate %zu bytes of memory for Point2D Event Packet of capacity %" + PRIi32 " from source %" PRIi16 ". Error: %d.", eventPacketSize, eventCapacity, eventSource, + errno); + return (NULL); + } + + // Fill in header fields. + caerEventPacketHeaderSetEventType(&packet->packetHeader, POINT2D_EVENT); + caerEventPacketHeaderSetEventSource(&packet->packetHeader, eventSource); + caerEventPacketHeaderSetEventSize(&packet->packetHeader, I32T(eventSize)); + caerEventPacketHeaderSetEventTSOffset(&packet->packetHeader, offsetof(struct caer_point2d_event, timestamp)); + caerEventPacketHeaderSetEventTSOverflow(&packet->packetHeader, tsOverflow); + caerEventPacketHeaderSetEventCapacity(&packet->packetHeader, eventCapacity); + + return (packet); +} + +caerPoint3DEventPacket caerPoint3DEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow) { + if (eventCapacity == 0) { + return (NULL); + } + + size_t eventSize = sizeof(struct caer_point3d_event); + size_t eventPacketSize = sizeof(struct caer_point3d_event_packet) + ((size_t) eventCapacity * eventSize); + + // Zero out event memory (all events invalid). + caerPoint3DEventPacket packet = calloc(1, eventPacketSize); + if (packet == NULL) { + caerLog(CAER_LOG_CRITICAL, "Point3D Event", + "Failed to allocate %zu bytes of memory for Point3D Event Packet of capacity %" + PRIi32 " from source %" PRIi16 ". Error: %d.", eventPacketSize, eventCapacity, eventSource, + errno); + return (NULL); + } + + // Fill in header fields. + caerEventPacketHeaderSetEventType(&packet->packetHeader, POINT3D_EVENT); + caerEventPacketHeaderSetEventSource(&packet->packetHeader, eventSource); + caerEventPacketHeaderSetEventSize(&packet->packetHeader, I32T(eventSize)); + caerEventPacketHeaderSetEventTSOffset(&packet->packetHeader, offsetof(struct caer_point3d_event, timestamp)); + caerEventPacketHeaderSetEventTSOverflow(&packet->packetHeader, tsOverflow); + caerEventPacketHeaderSetEventCapacity(&packet->packetHeader, eventCapacity); + + return (packet); +} + +caerPoint4DEventPacket caerPoint4DEventPacketAllocate(int32_t eventCapacity, int16_t eventSource, int32_t tsOverflow) { + if (eventCapacity == 0) { + return (NULL); + } + + size_t eventSize = sizeof(struct caer_point4d_event); + size_t eventPacketSize = sizeof(struct caer_point4d_event_packet) + ((size_t) eventCapacity * eventSize); + + // Zero out event memory (all events invalid). + caerPoint4DEventPacket packet = calloc(1, eventPacketSize); + if (packet == NULL) { + caerLog(CAER_LOG_CRITICAL, "Point4D Event", + "Failed to allocate %zu bytes of memory for Point4D Event Packet of capacity %" + PRIi32 " from source %" PRIi16 ". Error: %d.", eventPacketSize, eventCapacity, eventSource, + errno); + return (NULL); + } + + // Fill in header fields. + caerEventPacketHeaderSetEventType(&packet->packetHeader, POINT4D_EVENT); + caerEventPacketHeaderSetEventSource(&packet->packetHeader, eventSource); + caerEventPacketHeaderSetEventSize(&packet->packetHeader, I32T(eventSize)); + caerEventPacketHeaderSetEventTSOffset(&packet->packetHeader, offsetof(struct caer_point4d_event, timestamp)); + caerEventPacketHeaderSetEventTSOverflow(&packet->packetHeader, tsOverflow); + caerEventPacketHeaderSetEventCapacity(&packet->packetHeader, eventCapacity); + + return (packet); +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/frame_utils.c b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/frame_utils.c new file mode 100755 index 0000000000000000000000000000000000000000..4ca263c5635a86d768803169d926672967897f88 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/frame_utils.c @@ -0,0 +1,651 @@ +#include "frame_utils.h" + +enum pixelColorEnum { + PXR, PXB, PXG1, PXG2, PXW +}; + +static void frameUtilsDemosaicFrame(caerFrameEvent colorFrame, caerFrameEvent monoFrame); + +static inline enum pixelColorEnum determinePixelColor(enum caer_frame_event_color_filter colorFilter, int32_t x, + int32_t y) { + switch (colorFilter) { + case RGBG: + if (x & 0x01) { + if (y & 0x01) { + return (PXB); + } + else { + return (PXG1); + } + } + else { + if (y & 0x01) { + return (PXG2); + } + else { + return (PXR); + } + } + break; + + case GRGB: + if (x & 0x01) { + if (y & 0x01) { + return (PXG2); + } + else { + return (PXR); + } + } + else { + if (y & 0x01) { + return (PXB); + } + else { + return (PXG1); + } + } + break; + + case GBGR: + if (x & 0x01) { + if (y & 0x01) { + return (PXG1); + } + else { + return (PXB); + } + } + else { + if (y & 0x01) { + return (PXR); + } + else { + return (PXG2); + } + } + break; + + case BGRG: + if (x & 0x01) { + if (y & 0x01) { + return (PXR); + } + else { + return (PXG2); + } + } + else { + if (y & 0x01) { + return (PXG1); + } + else { + return (PXB); + } + } + break; + + case RGBW: + if (x & 0x01) { + if (y & 0x01) { + return (PXB); + } + else { + return (PXG1); + } + } + else { + if (y & 0x01) { + return (PXW); + } + else { + return (PXR); + } + } + break; + + case GRWB: + if (x & 0x01) { + if (y & 0x01) { + return (PXW); + } + else { + return (PXR); + } + } + else { + if (y & 0x01) { + return (PXB); + } + else { + return (PXG1); + } + } + break; + + case WBGR: + if (x & 0x01) { + if (y & 0x01) { + return (PXG1); + } + else { + return (PXB); + } + } + else { + if (y & 0x01) { + return (PXR); + } + else { + return (PXW); + } + } + break; + + case BWRG: + if (x & 0x01) { + if (y & 0x01) { + return (PXR); + } + else { + return (PXW); + } + } + else { + if (y & 0x01) { + return (PXG1); + } + else { + return (PXB); + } + } + break; + + case MONO: + default: + // Just fall through. + break; + } + + // This is impossible (MONO), so just use red pixel value, as good as any. + return (PXR); +} + +static void frameUtilsDemosaicFrame(caerFrameEvent colorFrame, caerFrameEvent monoFrame) { + uint16_t *colorPixels = caerFrameEventGetPixelArrayUnsafe(colorFrame); + uint16_t *monoPixels = caerFrameEventGetPixelArrayUnsafe(monoFrame); + + enum caer_frame_event_color_filter colorFilter = caerFrameEventGetColorFilter(monoFrame); + int32_t lengthY = caerFrameEventGetLengthY(monoFrame); + int32_t lengthX = caerFrameEventGetLengthX(monoFrame); + int32_t idxCENTER = 0; + int32_t idxCOLOR = 0; + + for (int32_t y = 0; y < lengthY; y++) { + for (int32_t x = 0; x < lengthX; x++) { + // Calculate all neighbor indexes. + int32_t idxLEFT = idxCENTER - 1; + int32_t idxRIGHT = idxCENTER + 1; + + int32_t idxCENTERUP = idxCENTER - lengthX; + int32_t idxLEFTUP = idxCENTERUP - 1; + int32_t idxRIGHTUP = idxCENTERUP + 1; + + int32_t idxCENTERDOWN = idxCENTER + lengthX; + int32_t idxLEFTDOWN = idxCENTERDOWN - 1; + int32_t idxRIGHTDOWN = idxCENTERDOWN + 1; + + enum pixelColorEnum pixelColor = determinePixelColor(colorFilter, x, y); + int32_t RComp; + int32_t GComp; + int32_t BComp; + + switch (pixelColor) { + case PXR: { + // This is a R pixel. It is always surrounded by G and B only. + RComp = monoPixels[idxCENTER]; + + if (y == 0) { + // First row. + if (x == 0) { + // First column. + GComp = (monoPixels[idxCENTERDOWN] + monoPixels[idxRIGHT]) / 2; + BComp = monoPixels[idxRIGHTDOWN]; + } + else if (x == (lengthX - 1)) { + // Last column. + GComp = (monoPixels[idxCENTERDOWN] + monoPixels[idxLEFT]) / 2; + BComp = monoPixels[idxLEFTDOWN]; + } + else { + // In-between columns. + GComp = (monoPixels[idxCENTERDOWN] + monoPixels[idxLEFT] + monoPixels[idxRIGHT]) / 3; + BComp = (monoPixels[idxRIGHTDOWN] + monoPixels[idxLEFTDOWN]) / 2; + } + } + else if (y == (lengthY - 1)) { + // Last row. + if (x == 0) { + // First column. + GComp = (monoPixels[idxCENTERUP] + monoPixels[idxRIGHT]) / 2; + BComp = monoPixels[idxRIGHTUP]; + } + else if (x == (lengthX - 1)) { + // Last column. + GComp = (monoPixels[idxCENTERUP] + monoPixels[idxLEFT]) / 2; + BComp = monoPixels[idxLEFTUP]; + } + else { + // In-between columns. + GComp = (monoPixels[idxCENTERUP] + monoPixels[idxLEFT] + monoPixels[idxRIGHT]) / 3; + BComp = (monoPixels[idxRIGHTUP] + monoPixels[idxLEFTUP]) / 2; + } + } + else { + // In-between rows. + if (x == 0) { + // First column. + GComp = (monoPixels[idxCENTERUP] + monoPixels[idxCENTERDOWN] + monoPixels[idxRIGHT]) / 3; + BComp = (monoPixels[idxRIGHTUP] + monoPixels[idxRIGHTDOWN]) / 2; + } + else if (x == (lengthX - 1)) { + // Last column. + GComp = (monoPixels[idxCENTERUP] + monoPixels[idxCENTERDOWN] + monoPixels[idxLEFT]) / 3; + BComp = (monoPixels[idxLEFTUP] + monoPixels[idxLEFTDOWN]) / 2; + } + else { + // In-between columns. + GComp = (monoPixels[idxCENTERUP] + monoPixels[idxCENTERDOWN] + monoPixels[idxLEFT] + + monoPixels[idxRIGHT]) / 4; + BComp = (monoPixels[idxRIGHTUP] + monoPixels[idxLEFTUP] + monoPixels[idxRIGHTDOWN] + + monoPixels[idxLEFTDOWN]) / 4; + } + } + + break; + } + + case PXB: { + // This is a B pixel. It is always surrounded by G and R only. + BComp = monoPixels[idxCENTER]; + + if (y == 0) { + // First row. + if (x == 0) { + // First column. + RComp = monoPixels[idxRIGHTDOWN]; + GComp = (monoPixels[idxCENTERDOWN] + monoPixels[idxRIGHT]) / 2; + } + else if (x == (lengthX - 1)) { + // Last column. + RComp = monoPixels[idxLEFTDOWN]; + GComp = (monoPixels[idxCENTERDOWN] + monoPixels[idxLEFT]) / 2; + } + else { + // In-between columns. + RComp = (monoPixels[idxRIGHTDOWN] + monoPixels[idxLEFTDOWN]) / 2; + GComp = (monoPixels[idxCENTERDOWN] + monoPixels[idxLEFT] + monoPixels[idxRIGHT]) / 3; + } + } + else if (y == (lengthY - 1)) { + // Last row. + if (x == 0) { + // First column. + RComp = monoPixels[idxRIGHTUP]; + GComp = (monoPixels[idxCENTERUP] + monoPixels[idxRIGHT]) / 2; + } + else if (x == (lengthX - 1)) { + // Last column. + RComp = monoPixels[idxLEFTUP]; + GComp = (monoPixels[idxCENTERUP] + monoPixels[idxLEFT]) / 2; + } + else { + // In-between columns. + RComp = (monoPixels[idxRIGHTUP] + monoPixels[idxLEFTUP]) / 2; + GComp = (monoPixels[idxCENTERUP] + monoPixels[idxLEFT] + monoPixels[idxRIGHT]) / 3; + } + } + else { + // In-between rows. + if (x == 0) { + // First column. + RComp = (monoPixels[idxRIGHTUP] + monoPixels[idxRIGHTDOWN]) / 2; + GComp = (monoPixels[idxCENTERUP] + monoPixels[idxCENTERDOWN] + monoPixels[idxRIGHT]) / 3; + } + else if (x == (lengthX - 1)) { + // Last column. + RComp = (monoPixels[idxLEFTUP] + monoPixels[idxLEFTDOWN]) / 2; + GComp = (monoPixels[idxCENTERUP] + monoPixels[idxCENTERDOWN] + monoPixels[idxLEFT]) / 3; + } + else { + // In-between columns. + RComp = (monoPixels[idxRIGHTUP] + monoPixels[idxLEFTUP] + monoPixels[idxRIGHTDOWN] + + monoPixels[idxLEFTDOWN]) / 4; + GComp = (monoPixels[idxCENTERUP] + monoPixels[idxCENTERDOWN] + monoPixels[idxLEFT] + + monoPixels[idxRIGHT]) / 4; + } + } + + break; + } + + case PXG1: { + // This is a G1 (first green) pixel. It is always surrounded by all of R, G, B. + GComp = monoPixels[idxCENTER]; + + if (y == 0) { + // First row. + BComp = monoPixels[idxCENTERDOWN]; + + if (x == 0) { + // First column. + RComp = monoPixels[idxRIGHT]; + } + else if (x == (lengthX - 1)) { + // Last column. + RComp = monoPixels[idxLEFT]; + } + else { + // In-between columns. + RComp = (monoPixels[idxLEFT] + monoPixels[idxRIGHT]) / 2; + } + } + else if (y == (lengthY - 1)) { + // Last row. + BComp = monoPixels[idxCENTERUP]; + + if (x == 0) { + // First column. + RComp = monoPixels[idxRIGHT]; + } + else if (x == (lengthX - 1)) { + // Last column. + RComp = monoPixels[idxLEFT]; + } + else { + // In-between columns. + RComp = (monoPixels[idxLEFT] + monoPixels[idxRIGHT]) / 2; + } + } + else { + // In-between rows. + BComp = (monoPixels[idxCENTERUP] + monoPixels[idxCENTERDOWN]) / 2; + + if (x == 0) { + // First column. + RComp = monoPixels[idxRIGHT]; + } + else if (x == (lengthX - 1)) { + // Last column. + RComp = monoPixels[idxLEFT]; + } + else { + // In-between columns. + RComp = (monoPixels[idxLEFT] + monoPixels[idxRIGHT]) / 2; + } + } + + break; + } + + case PXG2: { + // This is a G2 (second green) pixel. It is always surrounded by all of R, G, B. + GComp = monoPixels[idxCENTER]; + + if (y == 0) { + // First row. + RComp = monoPixels[idxCENTERDOWN]; + + if (x == 0) { + // First column. + BComp = monoPixels[idxRIGHT]; + } + else if (x == (lengthX - 1)) { + // Last column. + BComp = monoPixels[idxLEFT]; + } + else { + // In-between columns. + BComp = (monoPixels[idxLEFT] + monoPixels[idxRIGHT]) / 2; + } + } + else if (y == (lengthY - 1)) { + // Last row. + RComp = monoPixels[idxCENTERUP]; + + if (x == 0) { + // First column. + BComp = monoPixels[idxRIGHT]; + } + else if (x == (lengthX - 1)) { + // Last column. + BComp = monoPixels[idxLEFT]; + } + else { + // In-between columns. + BComp = (monoPixels[idxLEFT] + monoPixels[idxRIGHT]) / 2; + } + } + else { + // In-between rows. + RComp = (monoPixels[idxCENTERUP] + monoPixels[idxCENTERDOWN]) / 2; + + if (x == 0) { + // First column. + BComp = monoPixels[idxRIGHT]; + } + else if (x == (lengthX - 1)) { + // Last column. + BComp = monoPixels[idxLEFT]; + } + else { + // In-between columns. + BComp = (monoPixels[idxLEFT] + monoPixels[idxRIGHT]) / 2; + } + } + + break; + } + + case PXW: { + // This is a W pixel, modified Bayer pattern instead of G2. It is always surrounded by all of R, G, B. + // TODO: how can W itself contribute to the three colors? + if (y == 0) { + // First row. + RComp = monoPixels[idxCENTERDOWN]; + + if (x == 0) { + // First column. + GComp = monoPixels[idxRIGHTDOWN]; + BComp = monoPixels[idxRIGHT]; + } + else if (x == (lengthX - 1)) { + // Last column. + GComp = monoPixels[idxLEFTDOWN]; + BComp = monoPixels[idxLEFT]; + } + else { + // In-between columns. + GComp = (monoPixels[idxRIGHTDOWN] + monoPixels[idxLEFTDOWN]) / 2; + BComp = (monoPixels[idxLEFT] + monoPixels[idxRIGHT]) / 2; + } + } + else if (y == (lengthY - 1)) { + // Last row. + RComp = monoPixels[idxCENTERUP]; + + if (x == 0) { + // First column. + GComp = monoPixels[idxRIGHTUP]; + BComp = monoPixels[idxRIGHT]; + } + else if (x == (lengthX - 1)) { + // Last column. + GComp = monoPixels[idxLEFTUP]; + BComp = monoPixels[idxRIGHT]; + } + else { + // In-between columns. + GComp = (monoPixels[idxRIGHTUP] + monoPixels[idxLEFTUP]) / 2; + BComp = (monoPixels[idxLEFT] + monoPixels[idxRIGHT]) / 2; + } + } + else { + // In-between rows. + RComp = (monoPixels[idxCENTERUP] + monoPixels[idxCENTERDOWN]) / 2; + + if (x == 0) { + // First column. + GComp = (monoPixels[idxRIGHTUP] + monoPixels[idxRIGHTDOWN]) / 2; + BComp = monoPixels[idxRIGHT]; + } + else if (x == (lengthX - 1)) { + // Last column. + GComp = (monoPixels[idxLEFTUP] + monoPixels[idxLEFTDOWN]) / 2; + BComp = monoPixels[idxLEFT]; + } + else { + // In-between columns. + GComp = (monoPixels[idxRIGHTUP] + monoPixels[idxLEFTUP] + monoPixels[idxRIGHTDOWN] + + monoPixels[idxLEFTDOWN]) / 4; + BComp = (monoPixels[idxLEFT] + monoPixels[idxRIGHT]) / 2; + } + } + + break; + } + } + + // Set color frame pixel values for all color channels. + colorPixels[idxCOLOR] = U16T(RComp); + colorPixels[idxCOLOR + 1] = U16T(GComp); + colorPixels[idxCOLOR + 2] = U16T(BComp); + + // Go to next pixel. + idxCENTER++; + idxCOLOR += RGB; + } + } +} + +caerFrameEventPacket caerFrameUtilsDemosaic(caerFrameEventPacket framePacket) { + if (framePacket == NULL) { + return (NULL); + } + + int32_t countValid = 0; + int32_t maxLengthX = 0; + int32_t maxLengthY = 0; + + // This only works on valid frames coming from a camera: only one color channel, + // but with color filter information defined. + CAER_FRAME_ITERATOR_VALID_START(framePacket) + if (caerFrameEventGetChannelNumber(caerFrameIteratorElement) == GRAYSCALE + && caerFrameEventGetColorFilter(caerFrameIteratorElement) != MONO) { + countValid++; + + if (caerFrameEventGetLengthX(caerFrameIteratorElement) > maxLengthX) { + maxLengthX = caerFrameEventGetLengthX(caerFrameIteratorElement); + } + + if (caerFrameEventGetLengthY(caerFrameIteratorElement) > maxLengthY) { + maxLengthY = caerFrameEventGetLengthY(caerFrameIteratorElement); + } + } + CAER_FRAME_ITERATOR_VALID_END + + // Allocate new frame with RGB channels to hold resulting color image. + caerFrameEventPacket colorFramePacket = caerFrameEventPacketAllocate(countValid, + caerEventPacketHeaderGetEventSource(&framePacket->packetHeader), + caerEventPacketHeaderGetEventTSOverflow(&framePacket->packetHeader), maxLengthX, maxLengthY, RGB); + if (colorFramePacket == NULL) { + return (NULL); + } + + int32_t colorIndex = 0; + + // Now that we have a valid new color frame packet, we can convert the frames one by one. + CAER_FRAME_ITERATOR_VALID_START(framePacket) + if (caerFrameEventGetChannelNumber(caerFrameIteratorElement) == GRAYSCALE + && caerFrameEventGetColorFilter(caerFrameIteratorElement) != MONO) { + // If all conditions are met, copy from framePacket's mono frame to colorFramePacket's RGB frame. + caerFrameEvent colorFrame = caerFrameEventPacketGetEvent(colorFramePacket, colorIndex++); + + // First copy all the metadata. + caerFrameEventSetColorFilter(colorFrame, caerFrameEventGetColorFilter(caerFrameIteratorElement)); + caerFrameEventSetLengthXLengthYChannelNumber(colorFrame, caerFrameEventGetLengthX(caerFrameIteratorElement), + caerFrameEventGetLengthY(caerFrameIteratorElement), RGB, colorFramePacket); + caerFrameEventSetPositionX(colorFrame, caerFrameEventGetPositionX(caerFrameIteratorElement)); + caerFrameEventSetPositionY(colorFrame, caerFrameEventGetPositionY(caerFrameIteratorElement)); + caerFrameEventSetROIIdentifier(colorFrame, caerFrameEventGetROIIdentifier(caerFrameIteratorElement)); + caerFrameEventSetTSStartOfFrame(colorFrame, caerFrameEventGetTSStartOfFrame(caerFrameIteratorElement)); + caerFrameEventSetTSEndOfFrame(colorFrame, caerFrameEventGetTSEndOfFrame(caerFrameIteratorElement)); + caerFrameEventSetTSStartOfExposure(colorFrame, + caerFrameEventGetTSStartOfExposure(caerFrameIteratorElement)); + caerFrameEventSetTSEndOfExposure(colorFrame, caerFrameEventGetTSEndOfExposure(caerFrameIteratorElement)); + + // Then the actual pixels. + frameUtilsDemosaicFrame(colorFrame, caerFrameIteratorElement); + + // Finally validate the new frame. + caerFrameEventValidate(colorFrame, colorFramePacket); + } + CAER_FRAME_ITERATOR_VALID_END + + return (colorFramePacket); +} + +void caerFrameUtilsContrast(caerFrameEventPacket framePacket) { + if (framePacket == NULL) { + return; + } + + // O(x, y) = alpha * I(x, y) + beta, where alpha maximizes the range + // (contrast) and beta shifts it so lowest is zero (brightness). + // Only works with grayscale images currently. Doing so for color (RGB/RGBA) images would require + // conversion into another color space that has an intensity channel separate from the color + // channels, such as Lab or YCrCb. The same algorithm would then be applied on the intensity only. + CAER_FRAME_ITERATOR_VALID_START(framePacket) + if (caerFrameEventGetChannelNumber(caerFrameIteratorElement) == GRAYSCALE) { + uint16_t *pixels = caerFrameEventGetPixelArrayUnsafe(caerFrameIteratorElement); + int32_t lengthY = caerFrameEventGetLengthY(caerFrameIteratorElement); + int32_t lengthX = caerFrameEventGetLengthX(caerFrameIteratorElement); + + // On first pass, determine minimum and maximum values. + int32_t minValue = INT32_MAX; + int32_t maxValue = INT32_MIN; + + for (int32_t idx = 0; idx < (lengthY * lengthX); idx++) { + if (pixels[idx] < minValue) { + minValue = pixels[idx]; + } + + if (pixels[idx] > maxValue) { + maxValue = pixels[idx]; + } + } + + // Use min/max to calculate input range. + int32_t range = maxValue - minValue; + + // Calculate alpha (contrast). + float alpha = ((float) UINT16_MAX) / ((float) range); + + // Calculate beta (brightness). + float beta = ((float) -minValue) * alpha; + + // Apply alpha and beta to pixels array. + for (int32_t idx = 0; idx < (lengthY * lengthX); idx++) { + pixels[idx] = U16T(alpha * ((float ) pixels[idx]) + beta); + } + } + else { + caerLog(CAER_LOG_WARNING, "caerFrameUtilsContrast()", + "Standard contrast enhancement only works with grayscale images. For color image support, please use caerFrameUtilsOpenCVContrast()."); + } + CAER_FRAME_ITERATOR_VALID_END +} + +void caerFrameUtilsWhiteBalance(caerFrameEventPacket framePacket) { + +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/frame_utils_opencv.cpp b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/frame_utils_opencv.cpp new file mode 100755 index 0000000000000000000000000000000000000000..53d2bbdea52e43d08363d58f23d2dd36a90202f3 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/frame_utils_opencv.cpp @@ -0,0 +1,412 @@ +#include "frame_utils_opencv.h" + +#include <opencv2/core.hpp> +#include <opencv2/core/utility.hpp> +#include <opencv2/imgproc.hpp> + +using namespace cv; + +static void frameUtilsOpenCVDemosaicFrame(caerFrameEvent colorFrame, caerFrameEvent monoFrame, + enum caer_frame_utils_opencv_demosaic demosaicType); +static void frameUtilsOpenCVContrastNormalize(Mat &intensity, float clipHistPercent); +static void frameUtilsOpenCVContrastEqualize(Mat &intensity); +static void frameUtilsOpenCVContrastCLAHE(Mat &intensity, float clipLimit, int tilesGridSize); + +static void frameUtilsOpenCVDemosaicFrame(caerFrameEvent colorFrame, caerFrameEvent monoFrame, + enum caer_frame_utils_opencv_demosaic demosaicType) { + // Initialize OpenCV Mat based on caerFrameEvent data directly (no image copy). + Size frameSize(caerFrameEventGetLengthX(monoFrame), caerFrameEventGetLengthY(monoFrame)); + Mat monoMat(frameSize, CV_16UC(caerFrameEventGetChannelNumber(monoFrame)), + caerFrameEventGetPixelArrayUnsafe(monoFrame)); + Mat colorMat(frameSize, CV_16UC(caerFrameEventGetChannelNumber(colorFrame)), + caerFrameEventGetPixelArrayUnsafe(colorFrame)); + + CV_Assert(monoMat.type() == CV_16UC1 && colorMat.type() == CV_16UC3); + + // Select correct type code for OpenCV demosaic algorithm. + int code = 0; + + switch (demosaicType) { + case DEMOSAIC_NORMAL: + switch (caerFrameEventGetColorFilter(monoFrame)) { + case RGBG: + code = COLOR_BayerBG2RGB; + break; + + case GRGB: + code = COLOR_BayerGB2RGB; + break; + + case GBGR: + code = COLOR_BayerGR2RGB; + break; + + case BGRG: + code = COLOR_BayerRG2RGB; + break; + + default: + // Impossible, other color filters get filtered out above. + break; + } + break; + + /*case DEMOSAIC_VARIABLE_NUMBER_OF_GRADIENTS: + switch (caerFrameEventGetColorFilter(monoFrame)) { + case RGBG: + code = COLOR_BayerBG2RGB_VNG; + break; + + case GRGB: + code = COLOR_BayerGB2RGB_VNG; + break; + + case GBGR: + code = COLOR_BayerGR2RGB_VNG; + break; + + case BGRG: + code = COLOR_BayerRG2RGB_VNG; + break; + + default: + // Impossible, other color filters get filtered out above. + break; + } + break;*/ + + case DEMOSAIC_EDGE_AWARE: + switch (caerFrameEventGetColorFilter(monoFrame)) { + case RGBG: + code = COLOR_BayerBG2RGB_EA; + break; + + case GRGB: + code = COLOR_BayerGB2RGB_EA; + break; + + case GBGR: + code = COLOR_BayerGR2RGB_EA; + break; + + case BGRG: + code = COLOR_BayerRG2RGB_EA; + break; + + default: + // Impossible, other color filters get filtered out above. + break; + } + break; + } + + // Convert Bayer pattern to RGB image. + cvtColor(monoMat, colorMat, code); +} + +caerFrameEventPacket caerFrameUtilsOpenCVDemosaic(caerFrameEventPacket framePacket, + enum caer_frame_utils_opencv_demosaic demosaicType) { + if (framePacket == NULL) { + return (NULL); + } + + int32_t countValid = 0; + int32_t maxLengthX = 0; + int32_t maxLengthY = 0; + + // This only works on valid frames coming from a camera: only one color channel, + // but with color filter information defined. + CAER_FRAME_ITERATOR_VALID_START(framePacket) + if (caerFrameEventGetChannelNumber(caerFrameIteratorElement) == GRAYSCALE + && caerFrameEventGetColorFilter(caerFrameIteratorElement) != MONO) { + if (caerFrameEventGetColorFilter(caerFrameIteratorElement) == RGBG + || caerFrameEventGetColorFilter(caerFrameIteratorElement) == GRGB + || caerFrameEventGetColorFilter(caerFrameIteratorElement) == GBGR + || caerFrameEventGetColorFilter(caerFrameIteratorElement) == BGRG) { + countValid++; + + if (caerFrameEventGetLengthX(caerFrameIteratorElement) > maxLengthX) { + maxLengthX = caerFrameEventGetLengthX(caerFrameIteratorElement); + } + + if (caerFrameEventGetLengthY(caerFrameIteratorElement) > maxLengthY) { + maxLengthY = caerFrameEventGetLengthY(caerFrameIteratorElement); + } + } + else { + caerLog(CAER_LOG_WARNING, "caerFrameUtilsOpenCVDemosaic()", + "OpenCV demosaicing doesn't support the RGBW color filter, only RGBG. Please use caerFrameUtilsDemosaic() instead."); + } + } + CAER_FRAME_ITERATOR_VALID_END + + // Allocate new frame with RGB channels to hold resulting color image. + caerFrameEventPacket colorFramePacket = caerFrameEventPacketAllocate(countValid, + caerEventPacketHeaderGetEventSource(&framePacket->packetHeader), + caerEventPacketHeaderGetEventTSOverflow(&framePacket->packetHeader), maxLengthX, maxLengthY, RGB); + if (colorFramePacket == NULL) { + return (NULL); + } + + int32_t colorIndex = 0; + + // Now that we have a valid new color frame packet, we can convert the frames one by one. + CAER_FRAME_ITERATOR_VALID_START(framePacket) + if (caerFrameEventGetChannelNumber(caerFrameIteratorElement) == GRAYSCALE + && caerFrameEventGetColorFilter(caerFrameIteratorElement) != MONO) { + if (caerFrameEventGetColorFilter(caerFrameIteratorElement) == RGBG + || caerFrameEventGetColorFilter(caerFrameIteratorElement) == GRGB + || caerFrameEventGetColorFilter(caerFrameIteratorElement) == GBGR + || caerFrameEventGetColorFilter(caerFrameIteratorElement) == BGRG) { + // If all conditions are met, copy from framePacket's mono frame to colorFramePacket's RGB frame. + caerFrameEvent colorFrame = caerFrameEventPacketGetEvent(colorFramePacket, colorIndex++); + + // First copy all the metadata. + caerFrameEventSetColorFilter(colorFrame, caerFrameEventGetColorFilter(caerFrameIteratorElement)); + caerFrameEventSetLengthXLengthYChannelNumber(colorFrame, + caerFrameEventGetLengthX(caerFrameIteratorElement), + caerFrameEventGetLengthY(caerFrameIteratorElement), RGB, colorFramePacket); + caerFrameEventSetPositionX(colorFrame, caerFrameEventGetPositionX(caerFrameIteratorElement)); + caerFrameEventSetPositionY(colorFrame, caerFrameEventGetPositionY(caerFrameIteratorElement)); + caerFrameEventSetROIIdentifier(colorFrame, caerFrameEventGetROIIdentifier(caerFrameIteratorElement)); + caerFrameEventSetTSStartOfFrame(colorFrame, caerFrameEventGetTSStartOfFrame(caerFrameIteratorElement)); + caerFrameEventSetTSEndOfFrame(colorFrame, caerFrameEventGetTSEndOfFrame(caerFrameIteratorElement)); + caerFrameEventSetTSStartOfExposure(colorFrame, + caerFrameEventGetTSStartOfExposure(caerFrameIteratorElement)); + caerFrameEventSetTSEndOfExposure(colorFrame, + caerFrameEventGetTSEndOfExposure(caerFrameIteratorElement)); + + // Then the actual pixels. Only supports RGBG! + frameUtilsOpenCVDemosaicFrame(colorFrame, caerFrameIteratorElement, demosaicType); + + // Finally validate the new frame. + caerFrameEventValidate(colorFrame, colorFramePacket); + } + } + CAER_FRAME_ITERATOR_VALID_END + + return (colorFramePacket); +} + +void caerFrameUtilsOpenCVContrast(caerFrameEventPacket framePacket, + enum caer_frame_utils_opencv_contrast contrastType) { + if (framePacket == NULL) { + return; + } + + CAER_FRAME_ITERATOR_VALID_START(framePacket) + Size frameSize(caerFrameEventGetLengthX(caerFrameIteratorElement), + caerFrameEventGetLengthY(caerFrameIteratorElement)); + Mat orig(frameSize, CV_16UC(caerFrameEventGetChannelNumber(caerFrameIteratorElement)), + caerFrameEventGetPixelArrayUnsafe(caerFrameIteratorElement)); + + CV_Assert(orig.type() == CV_16UC1 || orig.type() == CV_16UC3 || orig.type() == CV_16UC4); + + // This generally only works well on grayscale intensity images. + // So, if this is a grayscale image, good, else if its a color + // image we convert it to YCrCb and operate on the Y channel only. + Mat intensity; + std::vector<Mat> yCrCbPlanes(3); + Mat rgbaAlpha; + + // Grayscale, no intensity extraction needed. + if (orig.channels() == GRAYSCALE) { + intensity = orig; + } + else { + // Color image, extract RGB and intensity/luminance. + Mat rgb; + + if (orig.channels() == RGBA) { + // We separate Alpha from RGB first. + // We will restore alpha at the end. + rgb = Mat(orig.rows, orig.cols, CV_16UC3); + rgbaAlpha = Mat(orig.rows, orig.cols, CV_16UC1); + + Mat out[] = { rgb, rgbaAlpha }; + // rgba[0] -> rgb[0], rgba[1] -> rgb[1], + // rgba[2] -> rgb[2], rgba[3] -> rgbaAlpha[0] + int channelTransform[] = { 0, 0, 1, 1, 2, 2, 3, 3 }; + mixChannels(&orig, 1, out, 2, channelTransform, 4); + } + else { + // Already an RGB image. + rgb = orig; + CV_Assert(rgb.type() == CV_16UC3); + } + + // First we convert from RGB to a color space with + // separate luminance channel. + Mat rgbYCrCb; + cvtColor(rgb, rgbYCrCb, COLOR_RGB2YCrCb); + + CV_Assert(rgbYCrCb.type() == CV_16UC3); + + // Then we split it up so that we can access the luminance + // channel on its own separately. + split(rgbYCrCb, yCrCbPlanes); + + // Now we have the luminance image in yCrCbPlanes[0]. + intensity = yCrCbPlanes[0]; + } + + CV_Assert(intensity.type() == CV_16UC1); + + // Apply contrast enhancement algorithm. + switch (contrastType) { + case CONTRAST_NORMALIZATION: + frameUtilsOpenCVContrastNormalize(intensity, 1.0); + break; + + case CONTRAST_HISTOGRAM_EQUALIZATION: + frameUtilsOpenCVContrastEqualize(intensity); + break; + + case CONTRAST_CLAHE: + frameUtilsOpenCVContrastCLAHE(intensity, 4.0, 8); + break; + } + + // If original was a color frame, we have to mix the various + // components back together into an RGB(A) image. + if (orig.channels() != GRAYSCALE) { + Mat YCrCbrgb; + merge(yCrCbPlanes, YCrCbrgb); + + CV_Assert(YCrCbrgb.type() == CV_16UC3); + + if (orig.channels() == RGBA) { + Mat rgb; + cvtColor(YCrCbrgb, rgb, COLOR_YCrCb2RGB); + + CV_Assert(rgb.type() == CV_16UC3); + + // Restore original alpha. + Mat in[] = { rgb, rgbaAlpha }; + // rgb[0] -> rgba[0], rgb[1] -> rgba[1], + // rgb[2] -> rgba[2], rgbaAlpha[0] -> rgba[3] + int channelTransform[] = { 0, 0, 1, 1, 2, 2, 3, 3 }; + mixChannels(in, 2, &orig, 1, channelTransform, 4); + } + else { + cvtColor(YCrCbrgb, orig, COLOR_YCrCb2RGB); + } + } + CAER_FRAME_ITERATOR_VALID_END +} + +void caerFrameUtilsOpenCVWhiteBalance(caerFrameEventPacket framePacket, + enum caer_frame_utils_opencv_white_balance whiteBalanceType) { + +} + +static void frameUtilsOpenCVContrastNormalize(Mat &intensity, float clipHistPercent) { + CV_Assert(intensity.type() == CV_16UC1); + CV_Assert(clipHistPercent >= 0 && clipHistPercent < 100); + + // O(x, y) = alpha * I(x, y) + beta, where alpha maximizes the range + // (contrast) and beta shifts it so lowest is zero (brightness). + double minValue, maxValue; + + if (clipHistPercent == 0) { + // Determine minimum and maximum values. + minMaxLoc(intensity, &minValue, &maxValue); + } + else { + // Calculate histogram. + int histSize = UINT16_MAX + 1; + float hRange[] = { 0, (float) histSize }; + const float *histRange = { hRange }; + bool uniform = true; + bool accumulate = false; + + Mat hist; + calcHist(&intensity, 1, 0, Mat(), hist, 1, &histSize, &histRange, uniform, accumulate); + + // Calculate cumulative distribution from the histogram. + for (size_t i = 1; i < (size_t) histSize; i++) { + hist.at<float>(i) += hist.at<float>(i - 1); + } + + // Locate points that cut at required value. + float max = hist.at<float>(histSize - 1); + clipHistPercent *= (max / 100.0); // Calculate absolute value from percent. + clipHistPercent /= 2.0; // Left and right wings, so divide by two. + + // Locate left cut. + minValue = 0; + while (hist.at<float>(minValue) < clipHistPercent) { + minValue++; + } + + // Locate right cut. + maxValue = UINT16_MAX; + while (hist.at<float>(maxValue) >= (max - clipHistPercent)) { + maxValue--; + } + } + + // Use min/max to calculate input range. + double range = maxValue - minValue; + + // Calculate alpha (contrast). + double alpha = ((double) UINT16_MAX) / range; + + // Calculate beta (brightness). + double beta = -minValue * alpha; + + // Apply alpha and beta to pixels array. + intensity.convertTo(intensity, -1, alpha, beta); +} + +static void frameUtilsOpenCVContrastEqualize(Mat &intensity) { + CV_Assert(intensity.type() == CV_16UC1); + + // Calculate histogram. + int histSize = UINT16_MAX + 1; + float hRange[] = { 0, (float) histSize }; + const float *histRange = { hRange }; + bool uniform = true; + bool accumulate = false; + + Mat hist; + calcHist(&intensity, 1, 0, Mat(), hist, 1, &histSize, &histRange, uniform, accumulate); + + // Calculate CDF from the histogram. + for (size_t i = 1; i < (size_t) histSize; i++) { + hist.at<float>(i) += hist.at<float>(i - 1); + } + + // Total number of pixels. Must be the last value! + float total = hist.at<float>(histSize - 1); + + // Smallest non-zero CDF value. Must be the first non-zero value! + float min = 0; + for (size_t i = 0; i < (size_t) histSize; i++) { + if (hist.at<float>(i) > 0) { + min = hist.at<float>(i); + break; + } + } + + // Calculate lookup table for histogram equalization. + hist -= min; + hist /= (total - min); + hist *= (float) UINT16_MAX; + + // Apply lookup table to input image. + std::for_each(intensity.begin<uint16_t>(), intensity.end<uint16_t>(), + [&hist](uint16_t &elem) {elem = hist.at<float>(elem);}); +} + +static void frameUtilsOpenCVContrastCLAHE(Mat &intensity, float clipLimit, int tilesGridSize) { + CV_Assert(intensity.type() == CV_16UC1); + CV_Assert(clipLimit >= 0 && clipLimit < 100); + CV_Assert(tilesGridSize >= 1 && tilesGridSize <= 64); + + // Apply the CLAHE algorithm to the intensity channel (luminance). + Ptr<CLAHE> clahe = createCLAHE(); + clahe->setClipLimit(clipLimit); + clahe->setTilesGridSize(Size(tilesGridSize, tilesGridSize)); + clahe->apply(intensity, intensity); +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/log.c b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/log.c new file mode 100755 index 0000000000000000000000000000000000000000..c8008102047a361bfd4a048ceba89097518ec833 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/log.c @@ -0,0 +1,143 @@ +#include "libcaer.h" +#include <stdatomic.h> +#include <stdarg.h> +#include <time.h> +#include <unistd.h> + +static atomic_uint_fast8_t caerLogLevel = ATOMIC_VAR_INIT(CAER_LOG_ERROR); +static atomic_int caerLogFileDescriptor1 = ATOMIC_VAR_INIT(STDERR_FILENO); +static atomic_int caerLogFileDescriptor2 = ATOMIC_VAR_INIT(-1); + +void caerLogLevelSet(uint8_t logLevel) { + atomic_store_explicit(&caerLogLevel, logLevel, memory_order_relaxed); +} + +uint8_t caerLogLevelGet(void) { + return (atomic_load_explicit(&caerLogLevel, memory_order_relaxed)); +} + +void caerLogFileDescriptorsSet(int fd1, int fd2) { + if (fd1 == fd2) { + // If the same fd is passed twice, just disable it the second time. + fd2 = -1; + } + + atomic_store_explicit(&caerLogFileDescriptor1, fd1, memory_order_relaxed); + atomic_store_explicit(&caerLogFileDescriptor2, fd2, memory_order_relaxed); +} + +void caerLog(uint8_t logLevel, const char *subSystem, const char *format, ...) { + // Check that subSystem and format are defined correctly. + if (subSystem == NULL || format == NULL) { + caerLog(CAER_LOG_ERROR, "Logger", "Missing subSystem or format strings. Neither can be NULL."); + return; + } + + // Only log messages if there is a destination (file-descriptor) to write them to. + int logFileDescriptor1 = atomic_load_explicit(&caerLogFileDescriptor1, memory_order_relaxed); + int logFileDescriptor2 = atomic_load_explicit(&caerLogFileDescriptor2, memory_order_relaxed); + + if (logFileDescriptor1 < 0 && logFileDescriptor2 < 0) { + // Logging is disabled. + return; + } + + // Only log messages above the specified severity level. + if (logLevel > atomic_load_explicit(&caerLogLevel, memory_order_relaxed)) { + return; + } + + // First prepend the time. + time_t currentTimeEpoch = time(NULL); + + // From localtime_r() man-page: "According to POSIX.1-2004, localtime() + // is required to behave as though tzset(3) was called, while + // localtime_r() does not have this requirement." + // So we make sure to call it here, to be portable. + tzset(); + +#if defined(OS_WINDOWS) + // localtime() is thread-safe on Windows (and there is no localtime_r() at all). + struct tm *currentTime = localtime(¤tTimeEpoch); +#else + struct tm currentTimeStruct; + struct tm *currentTime = ¤tTimeStruct; + localtime_r(¤tTimeEpoch, currentTime); +#endif + + // Following time format uses exactly 29 characters (8 separators/punctuation, + // 4 year, 2 month, 2 day, 2 hours, 2 minutes, 2 seconds, 2 'TZ', 5 timezone). + size_t currentTimeStringLength = 29; + char currentTimeString[currentTimeStringLength + 1]; // + 1 for terminating NUL byte. + strftime(currentTimeString, currentTimeStringLength + 1, "%Y-%m-%d %H:%M:%S (TZ%z)", currentTime); + + // Prepend debug level as a string to format. + const char *logLevelString; + switch (logLevel) { + case CAER_LOG_EMERGENCY: + logLevelString = "EMERGENCY"; + break; + + case CAER_LOG_ALERT: + logLevelString = "ALERT"; + break; + + case CAER_LOG_CRITICAL: + logLevelString = "CRITICAL"; + break; + + case CAER_LOG_ERROR: + logLevelString = "ERROR"; + break; + + case CAER_LOG_WARNING: + logLevelString = "WARNING"; + break; + + case CAER_LOG_NOTICE: + logLevelString = "NOTICE"; + break; + + case CAER_LOG_INFO: + logLevelString = "INFO"; + break; + + case CAER_LOG_DEBUG: + logLevelString = "DEBUG"; + break; + + default: + logLevelString = "UNKNOWN"; + break; + } + + // Copy all strings into one and ensure NUL termination. + size_t logLength = (size_t) snprintf(NULL, 0, "%s: %s: %s: %s\n", currentTimeString, logLevelString, subSystem, + format); + char logString[logLength + 1]; + snprintf(logString, logLength + 1, "%s: %s: %s: %s\n", currentTimeString, logLevelString, subSystem, format); + + va_list argptr; + + if (logFileDescriptor1 >= 0) { + va_start(argptr, format); +#if defined(OS_WINDOWS) + FILE *logFilePointer1 = fdopen(logFileDescriptor1, "a"); + vfprintf(logFilePointer1, logString, argptr); +#else + vdprintf(logFileDescriptor1, logString, argptr); +#endif + va_end(argptr); + } + + if (logFileDescriptor2 >= 0) { + va_start(argptr, format); +#if defined(OS_WINDOWS) + FILE *logFilePointer2 = fdopen(logFileDescriptor2, "a"); + vfprintf(logFilePointer2, logString, argptr); +#else + vdprintf(logFileDescriptor2, logString, argptr); +#endif + va_end(argptr); + } +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/ringbuffer/portable_aligned_alloc.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/ringbuffer/portable_aligned_alloc.h new file mode 100755 index 0000000000000000000000000000000000000000..79c578dab1dd3ba5babaf280bbedbc62e08c76ce --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/ringbuffer/portable_aligned_alloc.h @@ -0,0 +1,34 @@ +#ifndef PORTABLE_ALIGNED_ALLOC_H_ +#define PORTABLE_ALIGNED_ALLOC_H_ + +#if (!defined(__APPLE__) && !defined(_WIN32) && (__STDC_VERSION__ >= 201112L || defined(_ISOC11_SOURCE))) + #include <stdlib.h> + + static inline void *portable_aligned_alloc(size_t alignment, size_t size) { + return (aligned_alloc(alignment, size)); + } +#elif (_POSIX_C_SOURCE >= 200112L || _XOPEN_SOURCE >= 600) + #include <stdlib.h> + + static inline void *portable_aligned_alloc(size_t alignment, size_t size) { + void* mem; + + if (posix_memalign(&mem, alignment, size) == 0) { + // Success! + return (mem); + } + + // Failure. + return (NULL); + } +#elif defined(_WIN32) || defined(__CYGWIN__) + #include <malloc.h> + + static inline void *portable_aligned_alloc(size_t alignment, size_t size) { + return (_aligned_malloc(size, alignment)); + } +#else + #error "No portable way of getting aligned memory found." +#endif + +#endif /* PORTABLE_ALIGNED_ALLOC_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/ringbuffer/ringbuffer.c b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/ringbuffer/ringbuffer.c new file mode 100755 index 0000000000000000000000000000000000000000..3f388934d5be94eb09ea41543012d62a4a146068 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/ringbuffer/ringbuffer.c @@ -0,0 +1,117 @@ +/* + * ringbuffer.c + * + * Created on: Dec 10, 2013 + * Author: llongi + */ + +#include "ringbuffer.h" +#include "portable_aligned_alloc.h" +#include <stdatomic.h> +#include <stdalign.h> // To get alignas() macro. + +// Alignment specification support (with defines for cache line alignment). +#undef CACHELINE_ALIGNED +#undef CACHELINE_ALONE + +#if !defined(CACHELINE_SIZE) + #define CACHELINE_SIZE 64 // Default (big enough for almost all processors). + // Must be power of two! +#endif + +#define CACHELINE_ALIGNED alignas(CACHELINE_SIZE) +#define CACHELINE_ALONE(t, v) CACHELINE_ALIGNED t v; uint8_t PAD_##v[CACHELINE_SIZE - (sizeof(t) & (CACHELINE_SIZE - 1))] + +struct ring_buffer { + CACHELINE_ALONE(size_t, putPos); + CACHELINE_ALONE(size_t, getPos); + CACHELINE_ALONE(size_t, size); + atomic_uintptr_t elements[]; +}; + +RingBuffer ringBufferInit(size_t size) { + // Force multiple of two size for performance. + if (size == 0 || (size & (size - 1)) != 0) { + return (NULL); + } + + RingBuffer rBuf = portable_aligned_alloc(CACHELINE_SIZE, + sizeof(struct ring_buffer) + (size * sizeof(atomic_uintptr_t))); + if (rBuf == NULL) { + return (NULL); + } + + // Initialize counter variables. + rBuf->putPos = 0; + rBuf->getPos = 0; + rBuf->size = size; + + // Initialize pointers. + for (size_t i = 0; i < size; i++) { + atomic_store_explicit(&rBuf->elements[i], (uintptr_t) NULL, memory_order_relaxed); + } + + atomic_thread_fence(memory_order_release); + + return (rBuf); +} + +void ringBufferFree(RingBuffer rBuf) { + free(rBuf); +} + +bool ringBufferPut(RingBuffer rBuf, void *elem) { + if (elem == NULL) { + // NULL elements are disallowed (used as place-holders). + // Critical error, should never happen -> exit! + exit(EXIT_FAILURE); + } + + void *curr = (void *) atomic_load_explicit(&rBuf->elements[rBuf->putPos], memory_order_acquire); + + // If the place where we want to put the new element is NULL, it's still + // free and we can use it. + if (curr == NULL) { + atomic_store_explicit(&rBuf->elements[rBuf->putPos], (uintptr_t ) elem, memory_order_release); + + // Increase local put pointer. + rBuf->putPos = ((rBuf->putPos + 1) & (rBuf->size - 1)); + + return (true); + } + + // Else, buffer is full. + return (false); +} + +void *ringBufferGet(RingBuffer rBuf) { + void *curr = (void *) atomic_load_explicit(&rBuf->elements[rBuf->getPos], memory_order_acquire); + + // If the place where we want to get an element from is not NULL, there + // is valid content there, which we return, and reset the place to NULL. + if (curr != NULL) { + atomic_store_explicit(&rBuf->elements[rBuf->getPos], (uintptr_t) NULL, memory_order_release); + + // Increase local get pointer. + rBuf->getPos = ((rBuf->getPos + 1) & (rBuf->size - 1)); + + return (curr); + } + + // Else, buffer is empty. + return (NULL); +} + +void *ringBufferLook(RingBuffer rBuf) { + void *curr = (void *) atomic_load_explicit(&rBuf->elements[rBuf->getPos], memory_order_acquire); + + // If the place where we want to get an element from is not NULL, there + // is valid content there, which we return, without removing it from the + // ring buffer. + if (curr != NULL) { + return (curr); + } + + // Else, buffer is empty. + return (NULL); +} diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/ringbuffer/ringbuffer.h b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/ringbuffer/ringbuffer.h new file mode 100755 index 0000000000000000000000000000000000000000..947775dde5dac8761b650f5e7b252dcd59781ece --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer/src/ringbuffer/ringbuffer.h @@ -0,0 +1,24 @@ +/* + * ringbuffer.h + * + * Created on: Dec 10, 2013 + * Author: llongi + */ + +#ifndef RINGBUFFER_H_ +#define RINGBUFFER_H_ + +// Common includes, useful for everyone. +#include <stdlib.h> +#include <stdbool.h> +#include <stdint.h> + +typedef struct ring_buffer *RingBuffer; + +RingBuffer ringBufferInit(size_t size); +void ringBufferFree(RingBuffer rBuf); +bool ringBufferPut(RingBuffer rBuf, void *elem); +void *ringBufferGet(RingBuffer rBuf); +void *ringBufferLook(RingBuffer rBuf); + +#endif /* RINGBUFFER_H_ */ diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/65-inilabs.rules b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/65-inilabs.rules new file mode 100755 index 0000000000000000000000000000000000000000..511feed8fec8b9e90f7d7685b1c4a0339c5d0269 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/65-inilabs.rules @@ -0,0 +1,3 @@ +# All DVS/DAVIS systems + +SUBSYSTEM=="usb", ATTR{idVendor}=="152a", ATTR{idProduct}=="84[0-1]?", MODE="0666" diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/CMakeLists.txt b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/CMakeLists.txt new file mode 100755 index 0000000000000000000000000000000000000000..1449acbcf8ee754b53eb343f7c5a675b4fa774be --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 2.8.3) +project(libcaer_catkin) + +find_package(catkin_simple REQUIRED) + +catkin_simple() + +include(ExternalProject) + +file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include/libcaer) + +ExternalProject_Add(libcaer_src + GIT_REPOSITORY https://github.com/inilabs/libcaer.git + GIT_TAG 57466910da84c25231aab57e99f246239722a628 + UPDATE_COMMAND "" + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH=${CATKIN_DEVEL_PREFIX} -DCMAKE_BUILD_TYPE:STRING=Release -DCMAKE_INSTALL_LIBDIR=${CATKIN_DEVEL_PREFIX}/lib + BUILD_COMMAND make + INSTALL_COMMAND make install +) + +cs_install() +install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/include/libcaer + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} + FILES_MATCHING + PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +cs_export(INCLUDE_DIRS ${CATKIN_DEVEL_PREFIX}/include/libcaer/ + CFG_EXTRAS libcaer-extras.cmake) diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/cmake/libcaer-extras.cmake b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/cmake/libcaer-extras.cmake new file mode 100755 index 0000000000000000000000000000000000000000..298368a985fd245de7c10b9067249775a75669a4 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/cmake/libcaer-extras.cmake @@ -0,0 +1,4 @@ +# This overrides the dependency tracker with the caer library file. +set(@PROJECT_NAME@_LIBRARIES + @CATKIN_DEVEL_PREFIX@/lib/libcaer${CMAKE_SHARED_LIBRARY_SUFFIX}) +set(@PROJECT_NAME@_INCLUDE_DIR @CATKIN_DEVEL_PREFIX@/include) diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/install.sh b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/install.sh new file mode 100755 index 0000000000000000000000000000000000000000..20619f34cc56b761372b2e465f45ad29bc1cb1c9 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/install.sh @@ -0,0 +1,9 @@ +#!/bin/bash +echo "Copying udev rule (needs root privileges)." +sudo cp 65-inilabs.rules /etc/udev/rules.d/ + +echo "Reloading udev rules." +sudo udevadm control --reload-rules +sudo udevadm trigger + +echo "Done!" diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/package.xml b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/package.xml new file mode 100755 index 0000000000000000000000000000000000000000..8dd9c9847903fc3a5a92c640699f7218e3bd2a39 --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/libcaer_catkin/package.xml @@ -0,0 +1,11 @@ +<?xml version="1.0"?> +<package format="2"> + <name>libcaer_catkin</name> + <version>0.0.0</version> + <description>The libcaer_catkin package</description> + <maintainer email="mueggler@ifi.uzh.ch">Elias Mueggler</maintainer> + <license>GNU GPL</license> + + <buildtool_depend>catkin</buildtool_depend> + <buildtool_depend>catkin_simple</buildtool_depend> +</package> diff --git a/RWR/src/read_event_pub/src/dvs_clustering_tracking/rpg_dvs_ros b/RWR/src/read_event_pub/src/dvs_clustering_tracking/rpg_dvs_ros new file mode 160000 index 0000000000000000000000000000000000000000..95f08d565c3115b181a4763e1e350f4df0375c4b --- /dev/null +++ b/RWR/src/read_event_pub/src/dvs_clustering_tracking/rpg_dvs_ros @@ -0,0 +1 @@ +Subproject commit 95f08d565c3115b181a4763e1e350f4df0375c4b