diff --git a/.gitmodules b/.gitmodules index 8950bcc7506a..1ebafcf5a4e5 100644 --- a/.gitmodules +++ b/.gitmodules @@ -2,10 +2,6 @@ path = src/modules/mavlink/mavlink url = https://github.com/mavlink/mavlink.git branch = master -[submodule "src/drivers/uavcan/libuavcan"] - path = src/drivers/uavcan/libuavcan - url = https://github.com/dronecan/libuavcan.git - branch = main [submodule "Tools/simulation/jmavsim/jMAVSim"] path = Tools/simulation/jmavsim/jMAVSim url = https://github.com/PX4/jMAVSim.git @@ -87,3 +83,9 @@ path = src/drivers/actuators/vertiq_io/iq-module-communication-cpp url = https://github.com/PX4/iq-module-communication-cpp.git branch = master +[submodule "src/drivers/uavcan/libdronecan/dsdl"] + path = src/drivers/uavcan/libdronecan/dsdl + url = https://github.com/PX4/DSDL.git +[submodule "src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan"] + path = src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan + url = https://github.com/dronecan/pydronecan diff --git a/Documentation/Doxyfile.in b/Documentation/Doxyfile.in index 0874d152cfd5..979366b83816 100644 --- a/Documentation/Doxyfile.in +++ b/Documentation/Doxyfile.in @@ -828,7 +828,7 @@ RECURSIVE = YES # Note that relative paths are relative to the directory from which doxygen is # run. -EXCLUDE = @CMAKE_SOURCE_DIR@/src/modules/uavcan/libuavcan \ +EXCLUDE = @CMAKE_SOURCE_DIR@/src/modules/uavcan/libdronecan \ @CMAKE_SOURCE_DIR@/src/examples \ @CMAKE_SOURCE_DIR@/src/templates diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index c94132867ee1..45ceff41092f 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -11,7 +11,7 @@ exec find boards msg src platforms test \ -path platforms/nuttx/NuttX -prune -o \ -path platforms/qurt/dspal -prune -o \ -path src/drivers/ins/vectornav/libvnc -prune -o \ - -path src/drivers/uavcan/libuavcan -prune -o \ + -path src/drivers/uavcan/libdronecan -prune -o \ -path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \ -path src/drivers/cyphal/libcanard -prune -o \ -path src/lib/crypto/monocypher -prune -o \ diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index f8bc1eea8a6a..560c8b4d569c 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -31,12 +31,14 @@ # ############################################################################ -set(LIBUAVCAN_DIR ${PX4_SOURCE_DIR}/src/drivers/uavcan/libuavcan) -set(LIBUAVCAN_DIR_DRIVERS ${PX4_SOURCE_DIR}/src/drivers/uavcan/uavcan_drivers) +set(LIBDRONECAN_DIR ${PX4_SOURCE_DIR}/src/drivers/uavcan/libdronecan) +set(LIBDRONECAN_DIR_DRIVERS ${PX4_SOURCE_DIR}/src/drivers/uavcan/uavcan_drivers) -px4_add_git_submodule(TARGET git_uavcan PATH ${LIBUAVCAN_DIR}) +set(DSDLC_DIR "${PX4_SOURCE_DIR}/src/drivers/uavcan/libdronecan/dsdl") + +px4_add_git_submodule(TARGET git_uavcan_dsdl PATH ${DSDLC_DIR}) +px4_add_git_submodule(TARGET git_uavcan_pydronecan PATH ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/pydronecan) -set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03") set(UAVCAN_PLATFORM "generic") if(CONFIG_ARCH_CHIP) @@ -90,25 +92,24 @@ add_compile_options( -Wno-address-of-packed-member ) set(CMAKE_WARN_DEPRECATED OFF CACHE BOOL "" FORCE) # silence libuavcan deprecation warning for now (TODO: fix and remove) -add_subdirectory(${LIBUAVCAN_DIR} libuavcan EXCLUDE_FROM_ALL) +add_subdirectory(${LIBDRONECAN_DIR} libdronecan EXCLUDE_FROM_ALL) add_dependencies(uavcan prebuild_targets) # driver -add_subdirectory(${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver libuavcan_drivers EXCLUDE_FROM_ALL) +add_subdirectory(${LIBDRONECAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver libuavcan_drivers EXCLUDE_FROM_ALL) target_include_directories(uavcan_${UAVCAN_DRIVER}_driver PUBLIC - ${LIBUAVCAN_DIR}/libuavcan/include - ${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated + ${LIBDRONECAN_DIR}/libuavcan/include + ${LIBDRONECAN_DIR}/libuavcan/include/dsdlc_generated ) # generated DSDL -set(DSDLC_DIR "${PX4_SOURCE_DIR}/src/drivers/uavcan/dsdl") set(DSDLC_INPUTS - "${LIBUAVCAN_DIR}/dsdl/ardupilot" - "${LIBUAVCAN_DIR}/dsdl/com" - "${LIBUAVCAN_DIR}/dsdl/cuav" - "${LIBUAVCAN_DIR}/dsdl/dronecan" - "${LIBUAVCAN_DIR}/dsdl/uavcan" + "${DSDLC_DIR}/ardupilot" + "${DSDLC_DIR}/com" + "${DSDLC_DIR}/cuav" + "${DSDLC_DIR}/dronecan" + "${DSDLC_DIR}/uavcan" ) set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated") @@ -119,7 +120,7 @@ foreach(DSDLC_INPUT ${DSDLC_INPUTS}) endforeach(DSDLC_INPUT) add_custom_command(OUTPUT px4_uavcan_dsdlc_run.stamp COMMAND - ${PYTHON_EXECUTABLE} ${LIBUAVCAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc + ${PYTHON_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc --outdir ${DSDLC_OUTPUT} ${DSDLC_INPUTS} #--verbose COMMAND ${CMAKE_COMMAND} -E touch px4_uavcan_dsdlc_run.stamp @@ -138,10 +139,10 @@ px4_add_module( #-DDEBUG_BUILD INCLUDES ${DSDLC_OUTPUT} - ${LIBUAVCAN_DIR}/libuavcan/include - ${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated - ${LIBUAVCAN_DIR}/libuavcan_drivers/posix/include - ${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver/include + ${LIBDRONECAN_DIR}/libuavcan/include + ${LIBDRONECAN_DIR}/libuavcan/include/dsdlc_generated + ${LIBDRONECAN_DIR_DRIVERS}/posix/include + ${LIBDRONECAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver/include SRCS arming_status.cpp arming_status.hpp @@ -191,7 +192,8 @@ px4_add_module( mixer_module version - git_uavcan + git_uavcan_dsdl + git_uavcan_pydronecan uavcan_${UAVCAN_DRIVER}_driver drivers_rangefinder # Fix undefined reference when no distance sensors are selected diff --git a/src/drivers/uavcan/libdronecan/.gitignore b/src/drivers/uavcan/libdronecan/.gitignore new file mode 100644 index 000000000000..a901d640751b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/.gitignore @@ -0,0 +1,30 @@ +# Build outputs +*.o +*.d +lib*.so +lib*.so.* +*.a +build*/ +.dep +__pycache__ +*.pyc + +# Eclipse +.metadata +.settings +.project +.cproject +.pydevproject +.gdbinit + +# vsstudio code +.vscode + +# vagrant +.vagrant + +# libuavcan DSDL compiler default output directory +dsdlc_generated + +# Log files +*.log diff --git a/src/drivers/uavcan/libdronecan/CMakeLists.txt b/src/drivers/uavcan/libdronecan/CMakeLists.txt new file mode 100644 index 000000000000..5cb858dc14c3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/CMakeLists.txt @@ -0,0 +1,113 @@ +# +# Copyright (C) 2014 Pavel Kirienko +# + +cmake_minimum_required(VERSION 3.9) + +project(uavcan C CXX) + +# +# Build options +# +if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") + set(DEFAULT_UAVCAN_PLATFORM "linux") +endif() + +# options are listed in a table format below +set(opts + # name: type: default value: string options list : description + "CMAKE_BUILD_TYPE:STRING:RelWithDebInfo:Debug Release RelWithDebInfo MinSizeRel:Build type." + "CMAKE_CXX_FLAGS:STRING:::C++ flags." + "CMAKE_C_FLAGS:STRING:::C flags." + "UAVCAN_PLATFORM:STRING:generic:generic kinetis linux stm32:Platform." + "CONTINUOUS_INTEGRATION_BUILD:BOOL:OFF::Disable error redirection and timing tests" + "UAVCAN_CMAKE_VERBOSE:BOOL:OFF::Verbose CMake configure output" + ) +foreach(_opt ${opts}) + # arguments are : delimited + string(REPLACE ":" ";" _opt ${_opt}) + list(GET _opt 0 _name) + list(GET _opt 1 _type) + list(GET _opt 2 _default) + list(GET _opt 3 _options) + list(GET _opt 4 _descr) + # options are space delimited + string(REPLACE " " ";" _options "${_options}") + # if a default has not already been defined, use default from table + if(NOT DEFINED DEFAULT_${_name}) + set(DEFAULT_${_name} ${_default}) + endif() + # option has not been set already or it is empty, set it with the default + if(NOT DEFINED ${_name} OR ${_name} STREQUAL "") + set(${_name} ${DEFAULT_${_name}}) + endif() + # create a cache from the variable and force it to set +if(UAVCAN_CMAKE_VERBOSE) + message(STATUS "${_name}\t: ${${_name}} : ${_descr}") +endif() + set("${_name}" "${${_name}}" CACHE "${_type}" "${_descr}" FORCE) + # if an options list is provided for the cache, set it + if("${_type}" STREQUAL "STRING" AND NOT "${_options}" STREQUAL "") + set_property(CACHE ${_name} PROPERTY STRINGS ${_options}) + endif() +endforeach() + +# +# Set flags +# +include_directories( + ./libuavcan/include/ + ./libuavcan/include/dsdlc_generated + ) + +# +# Install +# +# DSDL definitions +install(DIRECTORY dsdl DESTINATION share/uavcan) + +# +# Googletest +# +if( CMAKE_BUILD_TYPE STREQUAL "Debug" ) + # (Taken from googletest/README.md documentation) + # GTest executables + # Download and unpack googletest at configure time + configure_file(CMakeLists.txt.in googletest-download/CMakeLists.txt) + execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . + RESULT_VARIABLE result + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/googletest-download ) + if(result) + message(WARNING "CMake step for googletest failed: ${result}") + else() + execute_process(COMMAND ${CMAKE_COMMAND} --build . + RESULT_VARIABLE result + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/googletest-download ) + if(result) + message(WARNING "Build step for googletest failed: ${result}") + else() + + # Prevent overriding the parent project's compiler/linker + # settings on Windows + set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) + + # Add googletest directly to our build. This defines + # the gtest and gtest_main targets. + add_subdirectory(${CMAKE_BINARY_DIR}/googletest-src + ${CMAKE_BINARY_DIR}/googletest-build + EXCLUDE_FROM_ALL) + + set(GTEST_FOUND ON) + set(BUILD_TESTING ON) + enable_testing() + endif() + endif() +endif() + +# +# Subdirectories +# +# library +add_subdirectory(libuavcan) + +# vim: set et ft=cmake fenc=utf-8 ff=unix sts=4 sw=4 ts=4 : diff --git a/src/drivers/uavcan/libdronecan/CMakeLists.txt.in b/src/drivers/uavcan/libdronecan/CMakeLists.txt.in new file mode 100644 index 000000000000..6eeed756a48a --- /dev/null +++ b/src/drivers/uavcan/libdronecan/CMakeLists.txt.in @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.9) + +project(googletest-download NONE) + +include(ExternalProject) +ExternalProject_Add(googletest + GIT_REPOSITORY https://github.com/google/googletest.git + GIT_TAG ba96d0b1161f540656efdaed035b3c062b60e006 + SOURCE_DIR "${CMAKE_BINARY_DIR}/googletest-src" + BINARY_DIR "${CMAKE_BINARY_DIR}/googletest-build" + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + TEST_COMMAND "" +) diff --git a/src/drivers/uavcan/libdronecan/LICENSE b/src/drivers/uavcan/libdronecan/LICENSE new file mode 100644 index 000000000000..41d1234c01f0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/LICENSE @@ -0,0 +1,20 @@ +The MIT License (MIT) + +Copyright (c) 2014 Pavel Kirienko + +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of +the Software, and to permit persons to whom the Software is furnished to do so, +subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS +FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR +COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER +IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN +CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/src/drivers/uavcan/libdronecan/README.md b/src/drivers/uavcan/libdronecan/README.md new file mode 100644 index 000000000000..60f803ca9ee2 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/README.md @@ -0,0 +1,120 @@ +DroneCAN stack in C++ +===================== + +Portable reference implementation of the [DroneCAN protocol stack](http://dronecan.org) in C++ for embedded systems +and Linux. + +DroneCAN is a lightweight protocol designed for reliable communication in aerospace and robotic applications via CAN bus. + +## Documentation + +* [DroneCAN website](http://dronecan.org) +* [DroneCAN forum](https://dronecan.org/discord) + +## Library usage + +### Cloning the repository + +```bash +git clone https://github.com/DroneCAN/libuavcan +cd libuavcan +git submodule update --init +``` + +If this repository is used as a git submodule in your project, make sure to use `--recursive` when updating it. + +### Using in a Linux application + +Libuavcan can be built as a static library and installed on the system globally as shown below. + +```bash +mkdir build +cd build +cmake .. # Default build type is RelWithDebInfo, which can be overriden if needed. +make -j8 +sudo make install +``` + +The following components will be installed: + +* Libuavcan headers and the static library +* Generated DSDL headers +* Libuavcan DSDL compiler (a Python script named `libuavcan_dsdlc`) +* Libuavcan DSDL compiler's support library (a Python package named `libuavcan_dsdl_compiler`) + +Note that Pyuavcan (an implementation of DroneCAN in Python) will not be installed. +You will need to install it separately if you intend to use the Libuavcan's DSDL compiler in your applications. + +It is also possible to use the library as a submodule rather than installing it system-wide. +Please refer to the example applications supplied with the Linux platform driver for more information. + +### Using with an embedded system + +For ARM targets, it is recommended to use [GCC ARM Embedded](https://launchpad.net/gcc-arm-embedded); +however, any other standard-compliant C++ compiler should also work. + +## Library development + +Despite the fact that the library itself can be used on virtually any platform that has a standard-compliant +C++11 compiler, the library development process assumes that the host OS is Linux. + +Prerequisites: + +* Google test library for C++ - gtest (dowloaded as part of the build from [github](https://github.com/google/googletest)) +* C++11 capable compiler with GCC-like interface (e.g. GCC, Clang) +* CMake 2.8+ +* Optional: static analysis tool for C++ - cppcheck (on Debian/Ubuntu use package `cppcheck`) + +Building the debug version and running the unit tests: +```bash +mkdir build +cd build +cmake .. -DCMAKE_BUILD_TYPE=Debug +make -j8 +make ARGS=-VV test +``` + +Test outputs can be found in the build directory under `libuavcan`. + +> Note that unit tests suffixed with "_RealTime" must be executed in real time, otherwise they may produce false warnings; +this implies that they will likely fail if ran on a virtual machine or on a highly loaded system. + +### Vagrant +Vagrant can be used to setup a compatible Ubuntu virtual image. Follow the instructions on [Vagrantup](https://www.vagrantup.com/) to install virtualbox and vagrant then do: + +```bash +vagrant up +vagrant ssh +mkdir build +cd build +mkdir build && cd build && cmake .. -DCMAKE_BUILD_TYPE=Debug -DCONTINUOUS_INTEGRATION_BUILD=1 +``` + +> Note that -DCONTINUOUS_INTEGRATION_BUILD=1 is required for this build as the realtime unit tests will not work on a virt. + +You can build using commands like: + +```bash +vagrant ssh -c "cd /vagrant/build && make -j4 && make test" +``` + +or to run a single test: + +```bash +vagrant ssh -c "cd /vagrant/build && make libuavcan_test && ./libuavcan/libuavcan_test --gtest_filter=Node.Basic" +``` + +### Developing with Eclipse + +An Eclipse project can be generated like that: + +```bash +cmake ../../libuavcan -G"Eclipse CDT4 - Unix Makefiles" \ + -DCMAKE_ECLIPSE_VERSION=4.3 \ + -DCMAKE_BUILD_TYPE=Debug \ + -DCMAKE_CXX_COMPILER_ARG1=-std=c++11 +``` + +Path `../../libuavcan` in the command above points at the directory where the top-level `CMakeLists.txt` is located; +you may need to adjust this per your environment. +Note that the directory where Eclipse project is generated must not be a descendant of the source directory. diff --git a/src/drivers/uavcan/libdronecan/dsdl b/src/drivers/uavcan/libdronecan/dsdl new file mode 160000 index 000000000000..993be80a62ec --- /dev/null +++ b/src/drivers/uavcan/libdronecan/dsdl @@ -0,0 +1 @@ +Subproject commit 993be80a62ec957c01fb41115b83663959a49f46 diff --git a/src/drivers/uavcan/libdronecan/libuavcan/CMakeLists.txt b/src/drivers/uavcan/libdronecan/libuavcan/CMakeLists.txt new file mode 100644 index 000000000000..8e095162e28c --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/CMakeLists.txt @@ -0,0 +1,146 @@ +# +# Copyright (C) 2014 Pavel Kirienko +# + +cmake_minimum_required(VERSION 3.9) + +if(DEFINED CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} CACHE STRING "Debug Release RelWithDebInfo MinSizeRel") +else() + set(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING "Debug Release RelWithDebInfo MinSizeRel") +endif() + +# Detecting whether we need to add debug targets +string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_lower) +if (build_type_lower STREQUAL "debug") + set(DEBUG_BUILD 1) + message(STATUS "Debug build") +else () + set(DEBUG_BUILD 0) +endif () + +project(libuavcan) + +find_package(PythonInterp) + +if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU" OR "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") + set(COMPILER_IS_GCC_COMPATIBLE 1) +else () + set(COMPILER_IS_GCC_COMPATIBLE 0) +endif () + +# +# DSDL compiler invocation +# Probably output files should be saved into CMake output dir? +# +set(DSDLC_INPUTS "test/dsdl_test/root_ns_a" "test/dsdl_test/root_ns_b" "${CMAKE_CURRENT_SOURCE_DIR}/../dsdl/uavcan") +set(DSDLC_OUTPUT "include/dsdlc_generated") + +set(DSDLC_INPUT_FILES "") +foreach(DSDLC_INPUT ${DSDLC_INPUTS}) + file(GLOB_RECURSE DSDLC_NEW_INPUT_FILES ${CMAKE_CURRENT_SOURCE_DIR} "${DSDLC_INPUT}/*.uavcan") + set(DSDLC_INPUT_FILES ${DSDLC_INPUT_FILES} ${DSDLC_NEW_INPUT_FILES}) +endforeach(DSDLC_INPUT) +add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp + COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/dsdl_compiler/libuavcan_dsdlc ${DSDLC_INPUTS} -O${DSDLC_OUTPUT} + COMMAND ${CMAKE_COMMAND} -E touch ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp + DEPENDS ${DSDLC_INPUT_FILES} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + COMMENT "Running dsdl compiler") +add_custom_target(libuavcan_dsdlc DEPENDS ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp) +include_directories(${DSDLC_OUTPUT}) + +# +# Compiler flags +# +if (COMPILER_IS_GCC_COMPATIBLE) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wundef") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +endif () + +if (DEBUG_BUILD) + add_definitions(-DUAVCAN_DEBUG=1) +endif () + +include_directories(include) + +# +# libuavcan +# +file(GLOB_RECURSE LIBUAVCAN_CXX_FILES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} "src/*.cpp") +add_library(uavcan STATIC ${LIBUAVCAN_CXX_FILES}) +add_dependencies(uavcan libuavcan_dsdlc) + +install(TARGETS uavcan DESTINATION lib) +install(DIRECTORY include/uavcan DESTINATION include) +install(DIRECTORY include/dsdlc_generated/uavcan DESTINATION include) # Generated and lib's .hpp + +# +# Tests and static analysis - only for debug builds +# +function(add_libuavcan_test name library flags) # Adds GTest executable and creates target to execute it every build + find_package(Threads REQUIRED) + + file(GLOB_RECURSE TEST_CXX_FILES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} "test/*.cpp") + add_executable(${name} ${TEST_CXX_FILES}) + add_dependencies(${name} ${library}) + + if (flags) + set_target_properties(${name} PROPERTIES COMPILE_FLAGS ${flags}) + endif () + + target_link_libraries(${name} gmock_main) + target_link_libraries(${name} ${library}) + if (${UAVCAN_PLATFORM} STREQUAL "linux") + target_link_libraries(${name} rt) + endif() + + # Tests run automatically upon successful build + # If failing tests need to be investigated with debugger, use 'make --ignore-errors' + if (CONTINUOUS_INTEGRATION_BUILD) + # Don't redirect test output, and don't run tests suffixed with "RealTime" + add_test(NAME ${name} + COMMAND ${name} --gtest_filter=-*RealTime + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) + else () + add_test(NAME ${name} + COMMAND ${name} 1>"${name}.log" 2>&1 + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) + endif() +endfunction() + +if (DEBUG_BUILD) + message(STATUS "Debug build (note: requires gtest)") + + if (COMPILER_IS_GCC_COMPATIBLE) + # No such thing as too many warnings + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Werror -pedantic -Wfloat-equal -Wconversion") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wsign-conversion -Wcast-align -Wmissing-declarations -Wlogical-op") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wdouble-promotion -Wswitch-enum -Wtype-limits -Wno-error=array-bounds") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wzero-as-null-pointer-constant -Wnon-virtual-dtor") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Woverloaded-virtual -Wsign-promo -Wold-style-cast") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-error=deprecated-declarations") + set(optim_flags "-O3 -DNDEBUG -g0") + else () + message(STATUS "Compiler ID: ${CMAKE_CXX_COMPILER_ID}") + message(FATAL_ERROR "This compiler cannot be used to build tests; use release build instead.") + endif () + + # Additional flavours of the library + + add_library(uavcan_optim STATIC ${LIBUAVCAN_CXX_FILES}) + set_target_properties(uavcan_optim PROPERTIES COMPILE_FLAGS ${optim_flags}) + add_dependencies(uavcan_optim libuavcan_dsdlc) + + if (GTEST_FOUND) + message(STATUS "GTest found, tests will be built and run.") + add_libuavcan_test(libuavcan_test uavcan "") # Default + add_libuavcan_test(libuavcan_test_optim uavcan_optim "${optim_flags}") # Max optimization + else (GTEST_FOUND) + message(STATUS "GTest was not found, tests will not be built") + endif (GTEST_FOUND) +else () + message(STATUS "Release build type: " ${CMAKE_BUILD_TYPE}) +endif () + +# vim: set et ft=cmake fenc=utf-8 ff=unix sts=4 sw=4 ts=4 : diff --git a/src/drivers/uavcan/libdronecan/libuavcan/cppcheck.sh b/src/drivers/uavcan/libdronecan/libuavcan/cppcheck.sh new file mode 100755 index 000000000000..416cc506c5df --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/cppcheck.sh @@ -0,0 +1,18 @@ +#!/bin/sh +# +# cppcheck static analysis +# For Debian based: apt-get install cppcheck +# + +num_cores=$(grep -c ^processor /proc/cpuinfo) +if [ -z "$num_cores" ]; then + echo "Hey, it looks like we're not on Linux. Please fix this script to add support for this OS." + num_cores=4 +fi +echo "Number of threads for cppcheck: $num_cores" + +# TODO: with future versions of cppcheck, add --library=glibc +cppcheck . --error-exitcode=1 --quiet --enable=all --platform=unix64 --std=c99 --std=c++11 \ + --inline-suppr --force --template=gcc -j$num_cores \ + -U__BIGGEST_ALIGNMENT__ -UUAVCAN_MEM_POOL_BLOCK_SIZE -UBIG_ENDIAN -UBYTE_ORDER \ + -Iinclude $@ diff --git a/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/__init__.py b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/__init__.py new file mode 100644 index 000000000000..cb7071146246 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/__init__.py @@ -0,0 +1,313 @@ +#!/usr/bin/env python +# +# UAVCAN DSDL compiler for libuavcan +# +# Copyright (C) 2014 Pavel Kirienko +# + +''' +This module implements the core functionality of the UAVCAN DSDL compiler for libuavcan. +Supported Python versions: 3.2+, 2.7. +It accepts a list of root namespaces and produces the set of C++ header files for libuavcan. +It is based on the DSDL parsing package from pyuavcan. +''' + +from __future__ import division, absolute_import, print_function, unicode_literals +import sys, os, logging, errno, re +from .pyratemp import Template +from dronecan import dsdl + +# Python 2.7 compatibility +try: + str = unicode +except NameError: + pass + +OUTPUT_FILE_EXTENSION = 'hpp' +OUTPUT_FILE_PERMISSIONS = 0o444 # Read only for all +TEMPLATE_FILENAME = os.path.join(os.path.dirname(__file__), 'data_type_template.tmpl') + +__all__ = ['run', 'logger', 'DsdlCompilerException'] + +class DsdlCompilerException(Exception): + pass + +logger = logging.getLogger(__name__) + +def run(source_dirs, include_dirs, output_dir): + ''' + This function takes a list of root namespace directories (containing DSDL definition files to parse), a + possibly empty list of search directories (containing DSDL definition files that can be referenced from the types + that are going to be parsed), and the output directory path (possibly nonexistent) where the generated C++ + header files will be stored. + + Note that this module features lazy write, i.e. if an output file does already exist and its content is not going + to change, it will not be overwritten. This feature allows to avoid unnecessary recompilation of dependent object + files. + + Args: + source_dirs List of root namespace directories to parse. + include_dirs List of root namespace directories with referenced types (possibly empty). This list is + automaitcally extended with source_dirs. + output_dir Output directory path. Will be created if doesn't exist. + ''' + assert isinstance(source_dirs, list) + assert isinstance(include_dirs, list) + output_dir = str(output_dir) + + types = run_parser(source_dirs, include_dirs + source_dirs) + if not types: + die('No type definitions were found') + + logger.info('%d types total', len(types)) + run_generator(types, output_dir) + +# ----------------- + +def pretty_filename(filename): + try: + a = os.path.abspath(filename) + r = os.path.relpath(filename) + return a if '..' in r else r + except ValueError: + return filename + +def type_output_filename(t): + assert t.category == t.CATEGORY_COMPOUND + return t.full_name.replace('.', os.path.sep) + '.' + OUTPUT_FILE_EXTENSION + +def makedirs(path): + try: + try: + os.makedirs(path, exist_ok=True) # May throw "File exists" when executed as root, which is wrong + except TypeError: + os.makedirs(path) # Python 2.7 compatibility + except OSError as ex: + if ex.errno != errno.EEXIST: # http://stackoverflow.com/questions/12468022 + raise + +def die(text): + raise DsdlCompilerException(str(text)) + +def run_parser(source_dirs, search_dirs): + try: + types = dsdl.parse_namespaces(source_dirs, search_dirs) + except dsdl.DsdlException as ex: + logger.info('Parser failure', exc_info=True) + die(ex) + return types + +def run_generator(types, dest_dir): + try: + template_expander = make_template_expander(TEMPLATE_FILENAME) + dest_dir = os.path.abspath(dest_dir) # Removing '..' + makedirs(dest_dir) + for t in types: + logger.info('Generating type %s', t.full_name) + filename = os.path.join(dest_dir, type_output_filename(t)) + text = generate_one_type(template_expander, t) + write_generated_data(filename, text) + except Exception as ex: + logger.info('Generator failure', exc_info=True) + die(ex) + +def write_generated_data(filename, data): + dirname = os.path.dirname(filename) + makedirs(dirname) + + # Lazy update - file will not be rewritten if its content is not going to change + if os.path.exists(filename): + with open(filename) as f: + existing_data = f.read() + if data == existing_data: + logger.info('Up to date [%s]', pretty_filename(filename)) + return + logger.info('Rewriting [%s]', pretty_filename(filename)) + os.remove(filename) + else: + logger.info('Creating [%s]', pretty_filename(filename)) + + # Full rewrite + with open(filename, 'w') as f: + f.write(data) + try: + os.chmod(filename, OUTPUT_FILE_PERMISSIONS) + except (OSError, IOError) as ex: + logger.warning('Failed to set permissions for %s: %s', pretty_filename(filename), ex) + +def type_to_cpp_type(t): + if t.category == t.CATEGORY_PRIMITIVE: + cast_mode = { + t.CAST_MODE_SATURATED: '::uavcan::CastModeSaturate', + t.CAST_MODE_TRUNCATED: '::uavcan::CastModeTruncate', + }[t.cast_mode] + if t.kind == t.KIND_FLOAT: + return '::uavcan::FloatSpec< %d, %s >' % (t.bitlen, cast_mode) + else: + signedness = { + t.KIND_BOOLEAN: '::uavcan::SignednessUnsigned', + t.KIND_UNSIGNED_INT: '::uavcan::SignednessUnsigned', + t.KIND_SIGNED_INT: '::uavcan::SignednessSigned', + }[t.kind] + return '::uavcan::IntegerSpec< %d, %s, %s >' % (t.bitlen, signedness, cast_mode) + elif t.category == t.CATEGORY_ARRAY: + value_type = type_to_cpp_type(t.value_type) + mode = { + t.MODE_STATIC: '::uavcan::ArrayModeStatic', + t.MODE_DYNAMIC: '::uavcan::ArrayModeDynamic', + }[t.mode] + return '::uavcan::Array< %s, %s, %d >' % (value_type, mode, t.max_size) + elif t.category == t.CATEGORY_COMPOUND: + return '::' + t.full_name.replace('.', '::') + elif t.category == t.CATEGORY_VOID: + return '::uavcan::IntegerSpec< %d, ::uavcan::SignednessUnsigned, ::uavcan::CastModeSaturate >' % t.bitlen + else: + raise DsdlCompilerException('Unknown type category: %s' % t.category) + +def generate_one_type(template_expander, t): + t.short_name = t.full_name.split('.')[-1] + t.cpp_type_name = t.short_name + '_' + t.cpp_full_type_name = '::' + t.full_name.replace('.', '::') + t.include_guard = t.full_name.replace('.', '_').upper() + '_HPP_INCLUDED' + + # Dependencies (no duplicates) + def fields_includes(fields): + def detect_include(t): + if t.category == t.CATEGORY_COMPOUND: + return type_output_filename(t) + if t.category == t.CATEGORY_ARRAY: + return detect_include(t.value_type) + return list(sorted(set(filter(None, [detect_include(x.type) for x in fields])))) + + if t.kind == t.KIND_MESSAGE: + t.cpp_includes = fields_includes(t.fields) + else: + t.cpp_includes = fields_includes(t.request_fields + t.response_fields) + + t.cpp_namespace_components = t.full_name.split('.')[:-1] + t.has_default_dtid = t.default_dtid is not None + + # Attribute types + def inject_cpp_types(attributes): + void_index = 0 + for a in attributes: + a.cpp_type = type_to_cpp_type(a.type) + a.void = a.type.category == a.type.CATEGORY_VOID + if a.void: + assert not a.name + a.name = '_void_%d' % void_index + void_index += 1 + + if t.kind == t.KIND_MESSAGE: + inject_cpp_types(t.fields) + inject_cpp_types(t.constants) + t.all_attributes = t.fields + t.constants + t.union = t.union and len(t.fields) + else: + inject_cpp_types(t.request_fields) + inject_cpp_types(t.request_constants) + inject_cpp_types(t.response_fields) + inject_cpp_types(t.response_constants) + t.all_attributes = t.request_fields + t.request_constants + t.response_fields + t.response_constants + t.request_union = t.request_union and len(t.request_fields) + t.response_union = t.response_union and len(t.response_fields) + + # Constant properties + def inject_constant_info(constants): + for c in constants: + if c.type.kind == c.type.KIND_FLOAT: + float(c.string_value) # Making sure that this is a valid float literal + c.cpp_value = c.string_value + else: + int(c.string_value) # Making sure that this is a valid integer literal + c.cpp_value = c.string_value + if c.type.kind == c.type.KIND_UNSIGNED_INT: + c.cpp_value += 'U' + + if t.kind == t.KIND_MESSAGE: + inject_constant_info(t.constants) + else: + inject_constant_info(t.request_constants) + inject_constant_info(t.response_constants) + + # Data type kind + t.cpp_kind = { + t.KIND_MESSAGE: '::uavcan::DataTypeKindMessage', + t.KIND_SERVICE: '::uavcan::DataTypeKindService', + }[t.kind] + + # Generation + text = template_expander(t=t) # t for Type + text = '\n'.join(x.rstrip() for x in text.splitlines()) + text = text.replace('\n\n\n\n\n', '\n\n').replace('\n\n\n\n', '\n\n').replace('\n\n\n', '\n\n') + text = text.replace('{\n\n ', '{\n ') + return text + +def make_template_expander(filename): + ''' + Templating is based on pyratemp (http://www.simple-is-better.org/template/pyratemp.html). + The pyratemp's syntax is rather verbose and not so human friendly, so we define some + custom extensions to make it easier to read and write. + The resulting syntax somewhat resembles Mako (which was used earlier instead of pyratemp): + Substitution: + ${expression} + Line joining through backslash (replaced with a single space): + ${foo(bar(very_long_arument=42, \ + second_line=72))} + Blocks: + % for a in range(10): + % if a == 5: + ${foo()} + % endif + % endfor + The extended syntax is converted into pyratemp's through regexp substitution. + ''' + with open(filename) as f: + template_text = f.read() + + # Backslash-newline elimination + template_text = re.sub(r'\\\r{0,1}\n\ *', r' ', template_text) + + # Substitution syntax transformation: ${foo} ==> $!foo!$ + template_text = re.sub(r'([^\$]{0,1})\$\{([^\}]+)\}', r'\1$!\2!$', template_text) + + # Flow control expression transformation: % foo: ==> + template_text = re.sub(r'(?m)^(\ *)\%\ *(.+?):{0,1}$', r'\1', template_text) + + # Block termination transformation: ==> + template_text = re.sub(r'\<\!--\(end[a-z]+\)--\>', r'', template_text) + + # Pyratemp workaround. + # The problem is that if there's no empty line after a macro declaration, first line will be doubly indented. + # Workaround: + # 1. Remove trailing comments + # 2. Add a newline after each macro declaration + template_text = re.sub(r'\ *\#\!.*', '', template_text) + template_text = re.sub(r'(\<\!--\(macro\ [a-zA-Z0-9_]+\)--\>.*?)', r'\1\n', template_text) + + # Preprocessed text output for debugging +# with open(filename + '.d', 'w') as f: +# f.write(template_text) + + template = Template(template_text) + + def expand(**args): + # This function adds one indentation level (4 spaces); it will be used from the template + args['indent'] = lambda text, idnt = ' ': idnt + text.replace('\n', '\n' + idnt) + # This function works like enumerate(), telling you whether the current item is the last one + def enum_last_value(iterable, start=0): + it = iter(iterable) + count = start + try: + last = next(it) + except StopIteration: + return + for val in it: + yield count, False, last + last = val + count += 1 + yield count, True, last + args['enum_last_value'] = enum_last_value + return template(**args) + + return expand diff --git a/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl new file mode 100644 index 000000000000..3107426bf080 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl @@ -0,0 +1,558 @@ +/* + * UAVCAN data structure definition for libuavcan. + * + * Autogenerated, do not edit. + * + * Source file: ${t.source_file} + */ + +#ifndef ${t.include_guard} +#define ${t.include_guard} + +#include +#include +#include + +% for inc in t.cpp_includes: +#include <${inc}> +% endfor + +/******************************* Source text ********************************** +% for line in t.source_text.strip().splitlines(): +${line} +% endfor +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +% for line in t.get_dsdl_signature_source_definition().splitlines(): +${line} +% endfor +******************************************************************************/ + +% for a in t.all_attributes: +#undef ${a.name} +% endfor + +% for nsc in t.cpp_namespace_components: +namespace ${nsc} +{ +% endfor + +% if t.kind != t.KIND_SERVICE: +template +% endif +struct UAVCAN_EXPORT ${t.cpp_type_name} +{ + #! type_name, max_bitlen, fields, constants, union + typedef const ${type_name}<_tmpl>& ParameterType; + typedef ${type_name}<_tmpl>& ReferenceType; + + #! group_name, attrs + struct ${group_name} + { + % for a in attrs: + typedef ${a.cpp_type} ${a.name}; + % endfor + }; + + ${expand_attr_types(group_name='ConstantTypes', attrs=constants)} + ${expand_attr_types(group_name='FieldTypes', attrs=fields)} + + % if union: + + struct Tag + { + enum Type + { + % for idx,last,a in enum_last_value(fields): + ${a.name}${',' if not last else ''} + % endfor + }; + }; + + typedef ::uavcan::IntegerSpec< ::uavcan::IntegerBitLen< ${len(fields)}U - 1U >::Result, + ::uavcan::SignednessUnsigned, ::uavcan::CastModeTruncate > TagType; + + #! enum_name, enum_comparator + enum + { + ${enum_name} = TagType::BitLen + + % for idx,last,a in enum_last_value(fields): + % if not last: + ::uavcan::${enum_comparator}::Result' * (len(fields) - 1)} + % endif + % endfor + }; + + + ${expand_enum_per_field(enum_name='MinBitLen', enum_comparator='EnumMin')} + ${expand_enum_per_field(enum_name='MaxBitLen', enum_comparator='EnumMax')} + + % else: + + #! enum_name + enum + { + ${enum_name} + % for idx,a in enumerate(fields): + ${'=' if idx == 0 else '+'} FieldTypes::${a.name}::${enum_name} + % endfor + }; + + + ${expand_enum_per_field(enum_name='MinBitLen')} + ${expand_enum_per_field(enum_name='MaxBitLen')} + + % endif + + // Constants + % for a in constants: + static const typename ::uavcan::StorageType< typename ConstantTypes::${a.name} >::Type ${a.name}; // ${a.init_expression} + % endfor + + // Fields + % for a in [x for x in fields if not x.void]: + typename ::uavcan::StorageType< typename FieldTypes::${a.name} >::Type ${a.name}; + % endfor + + % if union: +private: + typename ::uavcan::StorageType< TagType >::Type _tag_; // The name is mangled to avoid clashing with fields + + template + struct TagToType; + +public: + % endif + + ${type_name}() + % for idx,a in enumerate([x for x in fields if not x.void]): + ${':' if idx == 0 else ','} ${a.name}() + % endfor + % if union: + , _tag_() + % endif + { + ::uavcan::StaticAssert<_tmpl == 0>::check(); // Usage check + +#if UAVCAN_DEBUG + /* + * Cross-checking MaxBitLen provided by the DSDL compiler. + * This check shall never be performed in user code because MaxBitLen value + * actually depends on the nested types, thus it is not invariant. + */ + ::uavcan::StaticAssert<${max_bitlen} == MaxBitLen>::check(); +#endif + } + + bool operator==(ParameterType rhs) const; + bool operator!=(ParameterType rhs) const { return !operator==(rhs); } + + /** + * This comparison is based on @ref uavcan::areClose(), which ensures proper comparison of + * floating point fields at any depth. + */ + bool isClose(ParameterType rhs) const; + + static int encode(ParameterType self, ::uavcan::ScalarCodec& codec, + ::uavcan::TailArrayOptimizationMode tao_mode = ::uavcan::TailArrayOptEnabled); + + static int decode(ReferenceType self, ::uavcan::ScalarCodec& codec, + ::uavcan::TailArrayOptimizationMode tao_mode = ::uavcan::TailArrayOptEnabled); + + % if union: + /** + * Explicit access to the tag. + * It is safer to use is()/as()/to() instead. + */ + typename Tag::Type getTag() const { return typename Tag::Type(_tag_); } + void setTag(typename Tag::Type x) { _tag_ = typename ::uavcan::StorageType< TagType >::Type(x); } + + /** + * Whether the union is set to the given type. + * Access by tag; this will work even if there are non-unique types within the union. + */ + bool is(typename Tag::Type x) const { return typename Tag::Type(_tag_) == x; } + + /** + * If the union is currently set to the type T, returns pointer to the appropriate field. + * If the union is set to another type, returns null pointer. + */ + template + inline const typename TagToType::StorageType* as() const; + + /** + * Switches the union to the given type and returns a mutable reference to the appropriate field. + * If the previous type was different, a default constructor will be called first. + */ + template + inline typename TagToType::StorageType& to(); + % endif + + +% if t.kind == t.KIND_SERVICE: + template + struct Request_ + { + ${indent(generate_primary_body(type_name='Request_', max_bitlen=t.get_max_bitlen_request(), \ + fields=t.request_fields, constants=t.request_constants, \ + union=t.request_union))} + }; + + template + struct Response_ + { + ${indent(generate_primary_body(type_name='Response_', max_bitlen=t.get_max_bitlen_response(), \ + fields=t.response_fields, constants=t.response_constants, \ + union=t.response_union))} + }; + + typedef Request_<0> Request; + typedef Response_<0> Response; +% else: + ${generate_primary_body(type_name=t.cpp_type_name, max_bitlen=t.get_max_bitlen(), \ + fields=t.fields, constants=t.constants, union=t.union)} +% endif + + /* + * Static type info + */ + enum { DataTypeKind = ${t.cpp_kind} }; +% if t.has_default_dtid: + enum { DefaultDataTypeID = ${t.default_dtid} }; +% else: + // This type has no default data type ID +% endif + + static const char* getDataTypeFullName() + { + return "${t.full_name}"; + } + + static void extendDataTypeSignature(::uavcan::DataTypeSignature& signature) + { + signature.extend(getDataTypeSignature()); + } + + static ::uavcan::DataTypeSignature getDataTypeSignature(); + +% if t.kind == t.KIND_SERVICE: +private: + ${t.cpp_type_name}(); // Don't create objects of this type. Use Request/Response instead. +% endif +}; + +/* + * Out of line struct method definitions + */ + #! scope_prefix, fields, union + +template +bool ${scope_prefix}<_tmpl>::operator==(ParameterType rhs) const +{ + % if union: + if (_tag_ != rhs._tag_) + { + return false; + } + % for idx,a in enumerate(fields): + if (_tag_ == ${idx}) + { + return ${a.name} == rhs.${a.name}; + } + % endfor + UAVCAN_ASSERT(0); // Invalid tag + return false; + % else: + % if fields: + return + % for idx,last,a in enum_last_value([x for x in fields if not x.void]): + ${a.name} == rhs.${a.name}${' &&' if not last else ';'} + % endfor + % else: + (void)rhs; + return true; + % endif + % endif +} + +template +bool ${scope_prefix}<_tmpl>::isClose(ParameterType rhs) const +{ + % if union: + if (_tag_ != rhs._tag_) + { + return false; + } + % for idx,a in enumerate(fields): + if (_tag_ == ${idx}) + { + return ::uavcan::areClose(${a.name}, rhs.${a.name}); + } + % endfor + UAVCAN_ASSERT(0); // Invalid tag + return false; + % else: + % if fields: + return + % for idx,last,a in enum_last_value([x for x in fields if not x.void]): + ::uavcan::areClose(${a.name}, rhs.${a.name})${' &&' if not last else ';'} + % endfor + % else: + (void)rhs; + return true; + % endif + % endif +} + + #! call_name, self_parameter_type +template +int ${scope_prefix}<_tmpl>::${call_name}(${self_parameter_type} self, ::uavcan::ScalarCodec& codec, + ::uavcan::TailArrayOptimizationMode tao_mode) +{ + (void)self; + (void)codec; + (void)tao_mode; + % if union: + const int res = TagType::${call_name}(self._tag_, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + % for idx,a in enumerate(fields): + if (self._tag_ == ${idx}) + { + return FieldTypes::${a.name}::${call_name}(self.${a.name}, codec, tao_mode); + } + % endfor + return -1; // Invalid tag value + % else: + % for a in [x for x in fields if x.void]: + typename ::uavcan::StorageType< typename FieldTypes::${a.name} >::Type ${a.name} = 0; + % endfor + int res = 1; + % for idx,last,a in enum_last_value(fields): + res = FieldTypes::${a.name}::${call_name}(${'self.' * (not a.void)}${a.name}, codec, \ +${'::uavcan::TailArrayOptDisabled' if not last else 'tao_mode'}); + % if not last: + if (res <= 0) + { + return res; + } + % endif + % endfor + return res; + % endif +} + +${generate_codec_calls_per_field(call_name='encode', self_parameter_type='ParameterType')} +${generate_codec_calls_per_field(call_name='decode', self_parameter_type='ReferenceType')} + + % if union: + % for idx,a in enumerate(fields): +template <> +template <> +struct ${scope_prefix}<0>::TagToType<${scope_prefix}<0>::Tag::${a.name}> +{ + typedef typename ${scope_prefix}<0>::FieldTypes::${a.name} Type; + typedef typename ::uavcan::StorageType::Type StorageType; +}; + +template <> +template <> +inline const typename ${scope_prefix}<0>::TagToType< ${scope_prefix}<0>::Tag::${a.name} >::StorageType* +${scope_prefix}<0>::as< ${scope_prefix}<0>::Tag::${a.name} >() const +{ + return is(${scope_prefix}<0>::Tag::${a.name}) ? &${a.name} : UAVCAN_NULLPTR; +} + +template <> +template <> +inline typename ${scope_prefix}<0>::TagToType< ${scope_prefix}<0>::Tag::${a.name} >::StorageType& +${scope_prefix}<0>::to< ${scope_prefix}<0>::Tag::${a.name} >() +{ + if (_tag_ != ${idx}) + { + _tag_ = ${idx}; + ${a.name} = typename TagToType< ${scope_prefix}<0>::Tag::${a.name} >::StorageType(); + } + return ${a.name}; +} + + % endfor + % endif + + +% if t.kind == t.KIND_SERVICE: +${define_out_of_line_struct_methods(scope_prefix=t.cpp_type_name + '::Request_', fields=t.request_fields, \ + union=t.request_union)} +${define_out_of_line_struct_methods(scope_prefix=t.cpp_type_name + '::Response_', fields=t.response_fields, \ + union=t.response_union)} +% else: +${define_out_of_line_struct_methods(scope_prefix=t.cpp_type_name, fields=t.fields, union=t.union)} +% endif + +/* + * Out of line type method definitions + */ +% if t.kind == t.KIND_SERVICE: +inline ::uavcan::DataTypeSignature ${t.cpp_type_name}::getDataTypeSignature() +% else: +template +::uavcan::DataTypeSignature ${t.cpp_type_name}<_tmpl>::getDataTypeSignature() +% endif +{ + ::uavcan::DataTypeSignature signature(${'0x%08X' % t.get_dsdl_signature()}ULL); + #! scope_prefix, fields + % for a in fields: + ${scope_prefix}FieldTypes::${a.name}::extendDataTypeSignature(signature); + % endfor + +% if t.kind == t.KIND_SERVICE: +${extend_signature_per_field(scope_prefix='Request::', fields=t.request_fields)} +${extend_signature_per_field(scope_prefix='Response::', fields=t.response_fields)} +% else: +${extend_signature_per_field(scope_prefix='', fields=t.fields)} +% endif + return signature; +} + +/* + * Out of line constant definitions + */ + #! scope_prefix, constants + % for a in constants: +template +const typename ::uavcan::StorageType< typename ${scope_prefix}<_tmpl>::ConstantTypes::${a.name} >::Type + ${scope_prefix}<_tmpl>::${a.name} = ${a.cpp_value}; // ${a.init_expression} + + % endfor + +% if t.kind == t.KIND_SERVICE: +${define_out_of_line_constants(scope_prefix=t.cpp_type_name + '::Request_', constants=t.request_constants)} +${define_out_of_line_constants(scope_prefix=t.cpp_type_name + '::Response_', constants=t.response_constants)} +% else: +${define_out_of_line_constants(scope_prefix=t.cpp_type_name, constants=t.constants)} +% endif + +/* + * Final typedef + */ +% if t.kind == t.KIND_SERVICE: +typedef ${t.cpp_type_name} ${t.short_name}; +% else: +typedef ${t.cpp_type_name}<0> ${t.short_name}; +% endif + +% if t.has_default_dtid: +namespace +{ + +const ::uavcan::DefaultDataTypeRegistrator< ${t.cpp_full_type_name} > _uavcan_gdtr_registrator_${t.short_name}; + +} +% else: +// No default registration +% endif + +% for nsc in t.cpp_namespace_components[::-1]: +} // Namespace ${nsc} +% endfor + +/* + * YAML streamer specialization + */ +namespace uavcan +{ + + #! type_name, fields, union +template <> +class UAVCAN_EXPORT YamlStreamer< ${type_name} > +{ +public: + template + static void stream(Stream& s, ${type_name}::ParameterType obj, const int level); +}; + +template +void YamlStreamer< ${type_name} >::stream(Stream& s, ${type_name}::ParameterType obj, const int level) +{ + (void)s; + (void)obj; + (void)level; + % if union: + if (level > 0) + { + s << '\n'; + for (int pos = 0; pos < level; pos++) + { + s << " "; + } + } + % for idx,a in enumerate(fields): + if (static_cast(obj.getTag()) == ${idx}) + { + s << "${a.name}: "; + YamlStreamer< ${type_name}::FieldTypes::${a.name} >::stream(s, obj.${a.name}, level + 1); + } + % endfor + % else: + % for idx,a in enumerate([x for x in fields if not x.void]): + % if idx == 0: + if (level > 0) + { + s << '\n'; + for (int pos = 0; pos < level; pos++) + { + s << " "; + } + } + % else: + s << '\n'; + for (int pos = 0; pos < level; pos++) + { + s << " "; + } + % endif + s << "${a.name}: "; + YamlStreamer< ${type_name}::FieldTypes::${a.name} >::stream(s, obj.${a.name}, level + 1); + % endfor + % endif +} + +% if t.kind == t.KIND_SERVICE: +${define_yaml_streamer(type_name=t.cpp_full_type_name + '::Request', fields=t.request_fields, union=t.request_union)} +${define_yaml_streamer(type_name=t.cpp_full_type_name + '::Response', fields=t.response_fields, union=t.response_union)} +% else: +${define_yaml_streamer(type_name=t.cpp_full_type_name, fields=t.fields, union=t.union)} +% endif + +} + +% for nsc in t.cpp_namespace_components: +namespace ${nsc} +{ +% endfor + + #! type_name +template +inline Stream& operator<<(Stream& s, ${type_name}::ParameterType obj) +{ + ::uavcan::YamlStreamer< ${type_name} >::stream(s, obj, 0); + return s; +} + +% if t.kind == t.KIND_SERVICE: +${define_streaming_operator(type_name=t.cpp_full_type_name + '::Request')} +${define_streaming_operator(type_name=t.cpp_full_type_name + '::Response')} +% else: +${define_streaming_operator(type_name=t.cpp_full_type_name)} +% endif + +% for nsc in t.cpp_namespace_components[::-1]: +} // Namespace ${nsc} +% endfor + +#endif // ${t.include_guard} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/pyratemp.py b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/pyratemp.py new file mode 100755 index 000000000000..ed117b7c01ab --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/pyratemp.py @@ -0,0 +1,1225 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +Small, simple and powerful template-engine for Python. + +A template-engine for Python, which is very simple, easy to use, small, +fast, powerful, modular, extensible, well documented and pythonic. + +See documentation for a list of features, template-syntax etc. + +:Version: 0.3.0 +:Requires: Python >=2.6 / 3.x + +:Usage: + see class ``Template`` and examples below. + +:Example: + + Note that the examples are in Python 2; they also work in + Python 3 if you replace u"..." by "...", unicode() by str() + and partly "..." by b"...". + + quickstart:: + >>> t = Template("hello @!name!@") + >>> print(t(name="marvin")) + hello marvin + + quickstart with a template-file:: + # >>> t = Template(filename="mytemplate.tmpl") + # >>> print(t(name="marvin")) + # hello marvin + + generic usage:: + >>> t = Template(u"output is in Unicode \\xe4\\xf6\\xfc\\u20ac") + >>> t #doctest: +ELLIPSIS + <...Template instance at 0x...> + >>> t() + u'output is in Unicode \\xe4\\xf6\\xfc\\u20ac' + >>> unicode(t) + u'output is in Unicode \\xe4\\xf6\\xfc\\u20ac' + + with data:: + >>> t = Template("hello @!name!@", data={"name":"world"}) + >>> t() + u'hello world' + >>> t(name="worlds") + u'hello worlds' + + # >>> t(note="data must be Unicode or ASCII", name=u"\\xe4") + # u'hello \\xe4' + + escaping:: + >>> t = Template("hello escaped: @!name!@, unescaped: $!name!$") + >>> t(name='''<>&'"''') + u'hello escaped: <>&'", unescaped: <>&\\'"' + + result-encoding:: + # encode the unicode-object to your encoding with encode() + >>> t = Template(u"hello \\xe4\\xf6\\xfc\\u20ac") + >>> result = t() + >>> result + u'hello \\xe4\\xf6\\xfc\\u20ac' + >>> result.encode("utf-8") + 'hello \\xc3\\xa4\\xc3\\xb6\\xc3\\xbc\\xe2\\x82\\xac' + >>> result.encode("ascii") + Traceback (most recent call last): + ... + UnicodeEncodeError: 'ascii' codec can't encode characters in position 6-9: ordinal not in range(128) + >>> result.encode("ascii", 'xmlcharrefreplace') + 'hello äöü€' + + Python-expressions:: + >>> Template('formatted: @! "%8.5f" % value !@')(value=3.141592653) + u'formatted: 3.14159' + >>> Template("hello --@!name.upper().center(20)!@--")(name="world") + u'hello -- WORLD --' + >>> Template("calculate @!var*5+7!@")(var=7) + u'calculate 42' + + blocks (if/for/macros/...):: + >>> t = Template("barbazunknown(@!foo!@)") + >>> t(foo=2) + u'baz' + >>> t(foo=5) + u'unknown(5)' + + >>> t = Template("@!i!@ (empty)") + >>> t(mylist=[]) + u'(empty)' + >>> t(mylist=[1,2,3]) + u'1 2 3 ' + + >>> t = Template(" - @!i!@: @!elem!@") + >>> t(mylist=["a","b","c"]) + u' - 0: a - 1: b - 2: c' + + >>> t = Template('hello @!name!@ @!greetings(name=user)!@') + >>> t(user="monty") + u' hello monty' + + exists:: + >>> t = Template('YESNO') + >>> t() + u'NO' + >>> t(foo=1) + u'YES' + >>> t(foo=None) # note this difference to 'default()' + u'YES' + + default-values:: + # non-existing variables raise an error + >>> Template('hi @!optional!@')() + Traceback (most recent call last): + ... + TemplateRenderError: Cannot eval expression 'optional'. (NameError: name 'optional' is not defined) + + >>> t = Template('hi @!default("optional","anyone")!@') + >>> t() + u'hi anyone' + >>> t(optional=None) + u'hi anyone' + >>> t(optional="there") + u'hi there' + + # the 1st parameter can be any eval-expression + >>> t = Template('@!default("5*var1+var2","missing variable")!@') + >>> t(var1=10) + u'missing variable' + >>> t(var1=10, var2=2) + u'52' + + # also in blocks + >>> t = Template('yesno') + >>> t() + u'no' + >>> t(opt1=23, opt2=42) + u'yes' + + >>> t = Template('@!i!@') + >>> t() + u'' + >>> t(optional_list=[1,2,3]) + u'123' + + + # but make sure to put the expression in quotation marks, otherwise: + >>> Template('@!default(optional,"fallback")!@')() + Traceback (most recent call last): + ... + TemplateRenderError: Cannot eval expression 'default(optional,"fallback")'. (NameError: name 'optional' is not defined) + + setvar:: + >>> t = Template('$!setvar("i", "i+1")!$@!i!@') + >>> t(i=6) + u'7' + + >>> t = Template('''$!setvar("s", '"\\\\\\\\n".join(s)')!$@!s!@''') + >>> t(isinstance=isinstance, s="123") + u'123' + >>> t(isinstance=isinstance, s=["123", "456"]) + u'123\\n456' + +:Author: Roland Koebler (rk at simple-is-better dot org) +:Copyright: Roland Koebler +:License: MIT/X11-like, see __license__ + +:RCS: $Id: pyratemp.py,v 1.12 2013/04/02 20:26:06 rk Exp $ +""" +from __future__ import unicode_literals + +__version__ = "0.3.0" +__author__ = "Roland Koebler " +__license__ = """Copyright (c) Roland Koebler, 2007-2013 + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +IN THE SOFTWARE.""" + +#========================================= + +import os, re, sys +if sys.version_info[0] >= 3: + import builtins + unicode = str + long = int +else: + import __builtin__ as builtins + from codecs import open + +#========================================= +# some useful functions + +#---------------------- +# string-position: i <-> row,col + +def srow(string, i): + """Get line numer of ``string[i]`` in `string`. + + :Returns: row, starting at 1 + :Note: This works for text-strings with ``\\n`` or ``\\r\\n``. + """ + return string.count('\n', 0, max(0, i)) + 1 + +def scol(string, i): + """Get column number of ``string[i]`` in `string`. + + :Returns: column, starting at 1 (but may be <1 if i<0) + :Note: This works for text-strings with ``\\n`` or ``\\r\\n``. + """ + return i - string.rfind('\n', 0, max(0, i)) + +def sindex(string, row, col): + """Get index of the character at `row`/`col` in `string`. + + :Parameters: + - `row`: row number, starting at 1. + - `col`: column number, starting at 1. + :Returns: ``i``, starting at 0 (but may be <1 if row/col<0) + :Note: This works for text-strings with '\\n' or '\\r\\n'. + """ + n = 0 + for _ in range(row-1): + n = string.find('\n', n) + 1 + return n+col-1 + +#---------------------- + +def dictkeyclean(d): + """Convert all keys of the dict `d` to strings. + """ + new_d = {} + for k, v in d.items(): + new_d[str(k)] = v + return new_d + +#---------------------- + +def dummy(*_, **__): + """Dummy function, doing nothing. + """ + pass + +def dummy_raise(exception, value): + """Create an exception-raising dummy function. + + :Returns: dummy function, raising ``exception(value)`` + """ + def mydummy(*_, **__): + raise exception(value) + return mydummy + +#========================================= +# escaping + +(NONE, HTML, LATEX, MAIL_HEADER) = range(0, 4) +ESCAPE_SUPPORTED = {"NONE":None, "HTML":HTML, "LATEX":LATEX, "MAIL_HEADER":MAIL_HEADER} + +def escape(s, format=HTML): + """Replace special characters by their escape sequence. + + :Parameters: + - `s`: unicode-string to escape + - `format`: + + - `NONE`: nothing is replaced + - `HTML`: replace &<>'" by &...; + - `LATEX`: replace \#$%&_{}~^ + - `MAIL_HEADER`: escape non-ASCII mail-header-contents + :Returns: + the escaped string in unicode + :Exceptions: + - `ValueError`: if `format` is invalid. + + :Uses: + MAIL_HEADER uses module email + """ + #Note: If you have to make sure that every character gets replaced + # only once (and if you cannot achieve this with the following code), + # use something like "".join([replacedict.get(c,c) for c in s]) + # which is about 2-3 times slower (but maybe needs less memory). + #Note: This is one of the most time-consuming parts of the template. + if format is None or format == NONE: + pass + elif format == HTML: + s = s.replace("&", "&") # must be done first! + s = s.replace("<", "<") + s = s.replace(">", ">") + s = s.replace('"', """) + s = s.replace("'", "'") + elif format == LATEX: + s = s.replace("\\", "\\x") #must be done first! + s = s.replace("#", "\\#") + s = s.replace("$", "\\$") + s = s.replace("%", "\\%") + s = s.replace("&", "\\&") + s = s.replace("_", "\\_") + s = s.replace("{", "\\{") + s = s.replace("}", "\\}") + s = s.replace("\\x","\\textbackslash{}") + s = s.replace("~", "\\textasciitilde{}") + s = s.replace("^", "\\textasciicircum{}") + elif format == MAIL_HEADER: + import email.header + try: + s.encode("ascii") + return s + except UnicodeEncodeError: + return email.header.make_header([(s, "utf-8")]).encode() + else: + raise ValueError('Invalid format (only None, HTML, LATEX and MAIL_HEADER are supported).') + return s + +#========================================= + +#----------------------------------------- +# Exceptions + +class TemplateException(Exception): + """Base class for template-exceptions.""" + pass + +class TemplateParseError(TemplateException): + """Template parsing failed.""" + def __init__(self, err, errpos): + """ + :Parameters: + - `err`: error-message or exception to wrap + - `errpos`: ``(filename,row,col)`` where the error occured. + """ + self.err = err + self.filename, self.row, self.col = errpos + TemplateException.__init__(self) + def __str__(self): + if not self.filename: + return "line %d, col %d: %s" % (self.row, self.col, str(self.err)) + else: + return "file %s, line %d, col %d: %s" % (self.filename, self.row, self.col, str(self.err)) + +class TemplateSyntaxError(TemplateParseError, SyntaxError): + """Template syntax-error.""" + pass + +class TemplateIncludeError(TemplateParseError): + """Template 'include' failed.""" + pass + +class TemplateRenderError(TemplateException): + """Template rendering failed.""" + pass + +#----------------------------------------- +# Loader + +class LoaderString: + """Load template from a string/unicode. + + Note that 'include' is not possible in such templates. + """ + def __init__(self, encoding='utf-8'): + self.encoding = encoding + + def load(self, s): + """Return template-string as unicode. + """ + if isinstance(s, unicode): + u = s + else: + u = s.decode(self.encoding) + return u + +class LoaderFile: + """Load template from a file. + + When loading a template from a file, it's possible to including other + templates (by using 'include' in the template). But for simplicity + and security, all included templates have to be in the same directory! + (see ``allowed_path``) + """ + def __init__(self, allowed_path=None, encoding='utf-8'): + """Init the loader. + + :Parameters: + - `allowed_path`: path of the template-files + - `encoding`: encoding of the template-files + :Exceptions: + - `ValueError`: if `allowed_path` is not a directory + """ + if allowed_path and not os.path.isdir(allowed_path): + raise ValueError("'allowed_path' has to be a directory.") + self.path = allowed_path + self.encoding = encoding + + def load(self, filename): + """Load a template from a file. + + Check if filename is allowed and return its contens in unicode. + + :Parameters: + - `filename`: filename of the template without path + :Returns: + the contents of the template-file in unicode + :Exceptions: + - `ValueError`: if `filename` contains a path + """ + if filename != os.path.basename(filename): + raise ValueError("No path allowed in filename. (%s)" %(filename)) + filename = os.path.join(self.path, filename) + + f = open(filename, 'r', encoding=self.encoding) + u = f.read() + f.close() + + return u + +#----------------------------------------- +# Parser + +class Parser(object): + """Parse a template into a parse-tree. + + Includes a syntax-check, an optional expression-check and verbose + error-messages. + + See documentation for a description of the parse-tree. + """ + # template-syntax + _comment_start = "#!" + _comment_end = "!#" + _sub_start = "$!" + _sub_end = "!$" + _subesc_start = "@!" + _subesc_end = "!@" + _block_start = "" + + # build regexps + # comment + # single-line, until end-tag or end-of-line. + _strComment = r"""%s(?P.*?)(?P%s|\n|$)""" \ + % (re.escape(_comment_start), re.escape(_comment_end)) + _reComment = re.compile(_strComment, re.M) + + # escaped or unescaped substitution + # single-line ("|$" is needed to be able to generate good error-messges) + _strSubstitution = r""" + ( + %s\s*(?P.*?)\s*(?P%s|$) #substitution + | + %s\s*(?P.*?)\s*(?P%s|$) #escaped substitution + ) + """ % (re.escape(_sub_start), re.escape(_sub_end), + re.escape(_subesc_start), re.escape(_subesc_end)) + _reSubstitution = re.compile(_strSubstitution, re.X|re.M) + + # block + # - single-line, no nesting. + # or + # - multi-line, nested by whitespace indentation: + # * start- and end-tag of a block must have exactly the same indentation. + # * start- and end-tags of *nested* blocks should have a greater indentation. + # NOTE: A single-line block must not start at beginning of the line with + # the same indentation as the enclosing multi-line blocks! + # Note that " " and "\t" are different, although they may + # look the same in an editor! + _s = re.escape(_block_start) + _e = re.escape(_block_end) + _strBlock = r""" + ^(?P[ \t]*)%send%s(?P.*)\r?\n? # multi-line end (^ IGNORED_TEXT\n) + | + (?P)%send%s # single-line end () + | + (?P[ \t]*) # single-line tag (no nesting) + %s(?P\w+)[ \t]*(?P.*?)%s + (?P.*?) + (?=(?:%s.*?%s.*?)??%send%s) # (match until end or i.e. ) + | + # multi-line tag, nested by whitespace indentation + ^(?P[ \t]*) # save indentation of start tag + %s(?P\w+)\s*(?P.*?)%s(?P.*)\r?\n + (?P(?:.*\n)*?) + (?=(?P=indent)%s(?:.|\s)*?%s) # match indentation + """ % (_s, _e, + _s, _e, + _s, _e, _s, _e, _s, _e, + _s, _e, _s, _e) + _reBlock = re.compile(_strBlock, re.X|re.M) + + # "for"-block parameters: "var(,var)* in ..." + _strForParam = r"""^(?P\w+(?:\s*,\s*\w+)*)\s+in\s+(?P.+)$""" + _reForParam = re.compile(_strForParam) + + # allowed macro-names + _reMacroParam = re.compile(r"""^\w+$""") + + + def __init__(self, loadfunc=None, testexpr=None, escape=HTML): + """Init the parser. + + :Parameters: + - `loadfunc`: function to load included templates + (i.e. ``LoaderFile(...).load``) + - `testexpr`: function to test if a template-expressions is valid + (i.e. ``EvalPseudoSandbox().compile``) + - `escape`: default-escaping (may be modified by the template) + :Exceptions: + - `ValueError`: if `testexpr` or `escape` is invalid. + """ + if loadfunc is None: + self._load = dummy_raise(NotImplementedError, "'include' not supported, since no 'loadfunc' was given.") + else: + self._load = loadfunc + + if testexpr is None: + self._testexprfunc = dummy + else: + try: # test if testexpr() works + testexpr("i==1") + except Exception as err: + raise ValueError("Invalid 'testexpr'. (%s)" %(err)) + self._testexprfunc = testexpr + + if escape not in ESCAPE_SUPPORTED.values(): + raise ValueError("Unsupported 'escape'. (%s)" %(escape)) + self.escape = escape + self._includestack = [] + + def parse(self, template): + """Parse a template. + + :Parameters: + - `template`: template-unicode-string + :Returns: the resulting parse-tree + :Exceptions: + - `TemplateSyntaxError`: for template-syntax-errors + - `TemplateIncludeError`: if template-inclusion failed + - `TemplateException` + """ + self._includestack = [(None, template)] # for error-messages (_errpos) + return self._parse(template) + + def _errpos(self, fpos): + """Convert `fpos` to ``(filename,row,column)`` for error-messages.""" + filename, string = self._includestack[-1] + return filename, srow(string, fpos), scol(string, fpos) + + def _testexpr(self, expr, fpos=0): + """Test a template-expression to detect errors.""" + try: + self._testexprfunc(expr) + except SyntaxError as err: + raise TemplateSyntaxError(err, self._errpos(fpos)) + + def _parse_sub(self, parsetree, text, fpos=0): + """Parse substitutions, and append them to the parse-tree. + + Additionally, remove comments. + """ + curr = 0 + for match in self._reSubstitution.finditer(text): + start = match.start() + if start > curr: + parsetree.append(("str", self._reComment.sub('', text[curr:start]))) + + if match.group("sub") is not None: + if not match.group("end"): + raise TemplateSyntaxError("Missing closing tag '%s' for '%s'." + % (self._sub_end, match.group()), self._errpos(fpos+start)) + if len(match.group("sub")) > 0: + self._testexpr(match.group("sub"), fpos+start) + parsetree.append(("sub", match.group("sub"))) + else: + assert(match.group("escsub") is not None) + if not match.group("escend"): + raise TemplateSyntaxError("Missing closing tag '%s' for '%s'." + % (self._subesc_end, match.group()), self._errpos(fpos+start)) + if len(match.group("escsub")) > 0: + self._testexpr(match.group("escsub"), fpos+start) + parsetree.append(("esc", self.escape, match.group("escsub"))) + + curr = match.end() + + if len(text) > curr: + parsetree.append(("str", self._reComment.sub('', text[curr:]))) + + def _parse(self, template, fpos=0): + """Recursive part of `parse()`. + + :Parameters: + - template + - fpos: position of ``template`` in the complete template (for error-messages) + """ + # blank out comments + # (So that its content does not collide with other syntax, and + # because removing them completely would falsify the character- + # position ("match.start()") of error-messages) + template = self._reComment.sub(lambda match: self._comment_start+" "*len(match.group(1))+match.group(2), template) + + # init parser + parsetree = [] + curr = 0 # current position (= end of previous block) + block_type = None # block type: if,for,macro,raw,... + block_indent = None # None: single-line, >=0: multi-line + + # find blocks + for match in self._reBlock.finditer(template): + start = match.start() + # process template-part before this block + if start > curr: + self._parse_sub(parsetree, template[curr:start], fpos) + + # analyze block syntax (incl. error-checking and -messages) + keyword = None + block = match.groupdict() + pos__ = fpos + start # shortcut + if block["sKeyw"] is not None: # single-line block tag + block_indent = None + keyword = block["sKeyw"] + param = block["sParam"] + content = block["sContent"] + if block["sSpace"]: # restore spaces before start-tag + if len(parsetree) > 0 and parsetree[-1][0] == "str": + parsetree[-1] = ("str", parsetree[-1][1] + block["sSpace"]) + else: + parsetree.append(("str", block["sSpace"])) + pos_p = fpos + match.start("sParam") # shortcuts + pos_c = fpos + match.start("sContent") + elif block["mKeyw"] is not None: # multi-line block tag + block_indent = len(block["indent"]) + keyword = block["mKeyw"] + param = block["mParam"] + content = block["mContent"] + pos_p = fpos + match.start("mParam") + pos_c = fpos + match.start("mContent") + ignored = block["mIgnored"].strip() + if ignored and ignored != self._comment_start: + raise TemplateSyntaxError("No code allowed after block-tag.", self._errpos(fpos+match.start("mIgnored"))) + elif block["mEnd"] is not None: # multi-line block end + if block_type is None: + raise TemplateSyntaxError("No block to end here/invalid indent.", self._errpos(pos__) ) + if block_indent != len(block["mEnd"]): + raise TemplateSyntaxError("Invalid indent for end-tag.", self._errpos(pos__) ) + ignored = block["meIgnored"].strip() + if ignored and ignored != self._comment_start: + raise TemplateSyntaxError("No code allowed after end-tag.", self._errpos(fpos+match.start("meIgnored"))) + block_type = None + elif block["sEnd"] is not None: # single-line block end + if block_type is None: + raise TemplateSyntaxError("No block to end here/invalid indent.", self._errpos(pos__)) + if block_indent is not None: + raise TemplateSyntaxError("Invalid indent for end-tag.", self._errpos(pos__)) + block_type = None + else: + raise TemplateException("FATAL: Block regexp error. Please contact the author. (%s)" % match.group()) + + # analyze block content (mainly error-checking and -messages) + if keyword: + keyword = keyword.lower() + if 'for' == keyword: + if block_type is not None: + raise TemplateSyntaxError("Missing block-end-tag before new block at '%s'." %(match.group()), self._errpos(pos__)) + block_type = 'for' + cond = self._reForParam.match(param) + if cond is None: + raise TemplateSyntaxError("Invalid 'for ...' at '%s'." %(param), self._errpos(pos_p)) + names = tuple(n.strip() for n in cond.group("names").split(",")) + self._testexpr(cond.group("iter"), pos_p+cond.start("iter")) + parsetree.append(("for", names, cond.group("iter"), self._parse(content, pos_c))) + elif 'if' == keyword: + if block_type is not None: + raise TemplateSyntaxError("Missing block-end-tag before new block at '%s'." %(match.group()), self._errpos(pos__)) + if not param: + raise TemplateSyntaxError("Missing condition for 'if' at '%s'." %(match.group()), self._errpos(pos__)) + block_type = 'if' + self._testexpr(param, pos_p) + parsetree.append(("if", param, self._parse(content, pos_c))) + elif 'elif' == keyword: + if block_type != 'if': + raise TemplateSyntaxError("'elif' may only appear after 'if' at '%s'." %(match.group()), self._errpos(pos__)) + if not param: + raise TemplateSyntaxError("Missing condition for 'elif' at '%s'." %(match.group()), self._errpos(pos__)) + self._testexpr(param, pos_p) + parsetree.append(("elif", param, self._parse(content, pos_c))) + elif 'else' == keyword: + if block_type not in ('if', 'for'): + raise TemplateSyntaxError("'else' may only appear after 'if' or 'for' at '%s'." %(match.group()), self._errpos(pos__)) + if param: + raise TemplateSyntaxError("'else' may not have parameters at '%s'." %(match.group()), self._errpos(pos__)) + parsetree.append(("else", self._parse(content, pos_c))) + elif 'macro' == keyword: + if block_type is not None: + raise TemplateSyntaxError("Missing block-end-tag before new block '%s'." %(match.group()), self._errpos(pos__)) + block_type = 'macro' + # make sure param is "\w+" (instead of ".+") + if not param: + raise TemplateSyntaxError("Missing name for 'macro' at '%s'." %(match.group()), self._errpos(pos__)) + if not self._reMacroParam.match(param): + raise TemplateSyntaxError("Invalid name for 'macro' at '%s'." %(match.group()), self._errpos(pos__)) + #remove last newline + if len(content) > 0 and content[-1] == '\n': + content = content[:-1] + if len(content) > 0 and content[-1] == '\r': + content = content[:-1] + parsetree.append(("macro", param, self._parse(content, pos_c))) + + # parser-commands + elif 'raw' == keyword: + if block_type is not None: + raise TemplateSyntaxError("Missing block-end-tag before new block '%s'." %(match.group()), self._errpos(pos__)) + if param: + raise TemplateSyntaxError("'raw' may not have parameters at '%s'." %(match.group()), self._errpos(pos__)) + block_type = 'raw' + parsetree.append(("str", content)) + elif 'include' == keyword: + if block_type is not None: + raise TemplateSyntaxError("Missing block-end-tag before new block '%s'." %(match.group()), self._errpos(pos__)) + if param: + raise TemplateSyntaxError("'include' may not have parameters at '%s'." %(match.group()), self._errpos(pos__)) + block_type = 'include' + try: + u = self._load(content.strip()) + except Exception as err: + raise TemplateIncludeError(err, self._errpos(pos__)) + self._includestack.append((content.strip(), u)) # current filename/template for error-msg. + p = self._parse(u) + self._includestack.pop() + parsetree.extend(p) + elif 'set_escape' == keyword: + if block_type is not None: + raise TemplateSyntaxError("Missing block-end-tag before new block '%s'." %(match.group()), self._errpos(pos__)) + if param: + raise TemplateSyntaxError("'set_escape' may not have parameters at '%s'." %(match.group()), self._errpos(pos__)) + block_type = 'set_escape' + esc = content.strip().upper() + if esc not in ESCAPE_SUPPORTED: + raise TemplateSyntaxError("Unsupported escape '%s'." %(esc), self._errpos(pos__)) + self.escape = ESCAPE_SUPPORTED[esc] + else: + raise TemplateSyntaxError("Invalid keyword '%s'." %(keyword), self._errpos(pos__)) + curr = match.end() + + if block_type is not None: + raise TemplateSyntaxError("Missing end-tag.", self._errpos(pos__)) + + if len(template) > curr: # process template-part after last block + self._parse_sub(parsetree, template[curr:], fpos+curr) + + return parsetree + +#----------------------------------------- +# Evaluation + +# some checks +assert len(eval("dir()", {'__builtins__':{'dir':dir}})) == 1, \ + "FATAL: 'eval' does not work as expected (%s)." +assert compile("0 .__class__", "", "eval").co_names == ('__class__',), \ + "FATAL: 'compile' does not work as expected." + +class EvalPseudoSandbox: + """An eval-pseudo-sandbox. + + The pseudo-sandbox restricts the available functions/objects, so the + code can only access: + + - some of the builtin Python-functions, which are considered "safe" + (see safe_builtins) + - some additional functions (exists(), default(), setvar(), escape()) + - the passed objects incl. their methods. + + Additionally, names beginning with "_" are forbidden. + This is to prevent things like '0 .__class__', with which you could + easily break out of a "sandbox". + + Be careful to only pass "safe" objects/functions to the template, + because any unsafe function/method could break the sandbox! + For maximum security, restrict the access to as few objects/functions + as possible! + + :Warning: + Note that this is no real sandbox! (And although I don't know any + way to break out of the sandbox without passing-in an unsafe object, + I cannot guarantee that there is no such way. So use with care.) + + Take care if you want to use it for untrusted code!! + """ + + safe_builtins = { + "True" : True, + "False" : False, + "None" : None, + + "abs" : builtins.abs, + "chr" : builtins.chr, + "divmod" : builtins.divmod, + "hash" : builtins.hash, + "hex" : builtins.hex, + "len" : builtins.len, + "max" : builtins.max, + "min" : builtins.min, + "oct" : builtins.oct, + "ord" : builtins.ord, + "pow" : builtins.pow, + "range" : builtins.range, + "round" : builtins.round, + "sorted" : builtins.sorted, + "sum" : builtins.sum, + "unichr" : builtins.chr, + "zip" : builtins.zip, + + "bool" : builtins.bool, + "bytes" : builtins.bytes, + "complex" : builtins.complex, + "dict" : builtins.dict, + "enumerate" : builtins.enumerate, + "float" : builtins.float, + "int" : builtins.int, + "list" : builtins.list, + "long" : long, + "reversed" : builtins.reversed, + "str" : builtins.str, + "tuple" : builtins.tuple, + "unicode" : unicode, + } + if sys.version_info[0] < 3: + safe_builtins["unichr"] = builtins.unichr + + def __init__(self): + self._compile_cache = {} + self.locals_ptr = None + self.eval_allowed_globals = self.safe_builtins.copy() + self.register("__import__", self.f_import) + self.register("exists", self.f_exists) + self.register("default", self.f_default) + self.register("setvar", self.f_setvar) + self.register("escape", self.f_escape) + + def register(self, name, obj): + """Add an object to the "allowed eval-globals". + + Mainly useful to add user-defined functions to the pseudo-sandbox. + """ + self.eval_allowed_globals[name] = obj + + def compile(self, expr): + """Compile a Python-eval-expression. + + - Use a compile-cache. + - Raise a `NameError` if `expr` contains a name beginning with ``_``. + + :Returns: the compiled `expr` + :Exceptions: + - `SyntaxError`: for compile-errors + - `NameError`: if expr contains a name beginning with ``_`` + """ + if expr not in self._compile_cache: + c = compile(expr, "", "eval") + for i in c.co_names: #prevent breakout via new-style-classes + if i[0] == '_': + raise NameError("Name '%s' is not allowed." % i) + self._compile_cache[expr] = c + return self._compile_cache[expr] + + def eval(self, expr, locals): + """Eval a Python-eval-expression. + + Sets ``self.locals_ptr`` to ``locales`` and compiles the code + before evaluating. + """ + sav = self.locals_ptr + self.locals_ptr = locals + x = eval(self.compile(expr), {"__builtins__":self.eval_allowed_globals}, locals) + self.locals_ptr = sav + return x + + def f_import(self, name, *_, **__): + """``import``/``__import__()`` for the sandboxed code. + + Since "import" is insecure, the PseudoSandbox does not allow to + import other modules. But since some functions need to import + other modules (e.g. "datetime.datetime.strftime" imports "time"), + this function replaces the builtin "import" and allows to use + modules which are already accessible by the sandboxed code. + + :Note: + - This probably only works for rather simple imports. + - For security, it may be better to avoid such (complex) modules + which import other modules. (e.g. use time.localtime and + time.strftime instead of datetime.datetime.strftime, + or write a small wrapper.) + + :Example: + + >>> from datetime import datetime + >>> import pyratemp + >>> t = pyratemp.Template('@!mytime.strftime("%H:%M:%S")!@') + + # >>> print(t(mytime=datetime.now())) + # Traceback (most recent call last): + # ... + # ImportError: import not allowed in pseudo-sandbox; try to import 'time' yourself and pass it to the sandbox/template + + >>> import time + >>> print(t(mytime=datetime.strptime("13:40:54", "%H:%M:%S"), time=time)) + 13:40:54 + + # >>> print(t(mytime=datetime.now(), time=time)) + # 13:40:54 + """ + import types + if self.locals_ptr is not None and name in self.locals_ptr and isinstance(self.locals_ptr[name], types.ModuleType): + return self.locals_ptr[name] + else: + raise ImportError("import not allowed in pseudo-sandbox; try to import '%s' yourself (and maybe pass it to the sandbox/template)" % name) + + def f_exists(self, varname): + """``exists()`` for the sandboxed code. + + Test if the variable `varname` exists in the current locals-namespace. + + This only works for single variable names. If you want to test + complicated expressions, use i.e. `default`. + (i.e. `default("expr",False)`) + + :Note: the variable-name has to be quoted! (like in eval) + :Example: see module-docstring + """ + return (varname in self.locals_ptr) + + def f_default(self, expr, default=None): + """``default()`` for the sandboxed code. + + Try to evaluate an expression and return the result or a + fallback-/default-value; the `default`-value is used + if `expr` does not exist/is invalid/results in None. + + This is very useful for optional data. + + :Parameter: + - expr: eval-expression + - default: fallback-falue if eval(expr) fails or is None. + :Returns: + the eval-result or the "fallback"-value. + + :Note: the eval-expression has to be quoted! (like in eval) + :Example: see module-docstring + """ + try: + r = self.eval(expr, self.locals_ptr) + if r is None: + return default + return r + #TODO: which exceptions should be catched here? + except (NameError, LookupError, TypeError): + return default + + def f_setvar(self, name, expr): + """``setvar()`` for the sandboxed code. + + Set a variable. + + :Example: see module-docstring + """ + self.locals_ptr[name] = self.eval(expr, self.locals_ptr) + return "" + + def f_escape(self, s, format="HTML"): + """``escape()`` for the sandboxed code. + """ + if isinstance(format, (str, unicode)): + format = ESCAPE_SUPPORTED[format.upper()] + return escape(unicode(s), format) + +#----------------------------------------- +# basic template / subtemplate + +class TemplateBase: + """Basic template-class. + + Used both for the template itself and for 'macro's ("subtemplates") in + the template. + """ + + def __init__(self, parsetree, renderfunc, data=None): + """Create the Template/Subtemplate/Macro. + + :Parameters: + - `parsetree`: parse-tree of the template/subtemplate/macro + - `renderfunc`: render-function + - `data`: data to fill into the template by default (dictionary). + This data may later be overridden when rendering the template. + :Exceptions: + - `TypeError`: if `data` is not a dictionary + """ + #TODO: parameter-checking? + self.parsetree = parsetree + if isinstance(data, dict): + self.data = data + elif data is None: + self.data = {} + else: + raise TypeError('"data" must be a dict (or None).') + self.current_data = data + self._render = renderfunc + + def __call__(self, **override): + """Fill out/render the template. + + :Parameters: + - `override`: objects to add to the data-namespace, overriding + the "default"-data. + :Returns: the filled template (in unicode) + :Note: This is also called when invoking macros + (i.e. ``$!mymacro()!$``). + """ + self.current_data = self.data.copy() + self.current_data.update(override) + u = "".join(self._render(self.parsetree, self.current_data)) + self.current_data = self.data # restore current_data + return _dontescape(u) # (see class _dontescape) + + def __unicode__(self): + """Alias for __call__().""" + return self.__call__() + def __str__(self): + """Alias for __call__().""" + return self.__call__() + +#----------------------------------------- +# Renderer + +class _dontescape(unicode): + """Unicode-string which should not be escaped. + + If ``isinstance(object,_dontescape)``, then don't escape the object in + ``@!...!@``. It's useful for not double-escaping macros, and it's + automatically used for macros/subtemplates. + + :Note: This only works if the object is used on its own in ``@!...!@``. + It i.e. does not work in ``@!object*2!@`` or ``@!object + "hi"!@``. + """ + __slots__ = [] + + +class Renderer(object): + """Render a template-parse-tree. + + :Uses: `TemplateBase` for macros + """ + + def __init__(self, evalfunc, escapefunc): + """Init the renderer. + + :Parameters: + - `evalfunc`: function for template-expression-evaluation + (i.e. ``EvalPseudoSandbox().eval``) + - `escapefunc`: function for escaping special characters + (i.e. `escape`) + """ + #TODO: test evalfunc + self.evalfunc = evalfunc + self.escapefunc = escapefunc + + def _eval(self, expr, data): + """evalfunc with error-messages""" + try: + return self.evalfunc(expr, data) + #TODO: any other errors to catch here? + except (TypeError,NameError,LookupError,AttributeError, SyntaxError) as err: + raise TemplateRenderError("Cannot eval expression '%s'. (%s: %s)" %(expr, err.__class__.__name__, err)) + + def render(self, parsetree, data): + """Render a parse-tree of a template. + + :Parameters: + - `parsetree`: the parse-tree + - `data`: the data to fill into the template (dictionary) + :Returns: the rendered output-unicode-string + :Exceptions: + - `TemplateRenderError` + """ + _eval = self._eval # shortcut + output = [] + do_else = False # use else/elif-branch? + + if parsetree is None: + return "" + for elem in parsetree: + if "str" == elem[0]: + output.append(elem[1]) + elif "sub" == elem[0]: + output.append(unicode(_eval(elem[1], data))) + elif "esc" == elem[0]: + obj = _eval(elem[2], data) + #prevent double-escape + if isinstance(obj, _dontescape) or isinstance(obj, TemplateBase): + output.append(unicode(obj)) + else: + output.append(self.escapefunc(unicode(obj), elem[1])) + elif "for" == elem[0]: + do_else = True + (names, iterable) = elem[1:3] + try: + loop_iter = iter(_eval(iterable, data)) + except TypeError: + raise TemplateRenderError("Cannot loop over '%s'." % iterable) + for i in loop_iter: + do_else = False + if len(names) == 1: + data[names[0]] = i + else: + data.update(zip(names, i)) #"for a,b,.. in list" + output.extend(self.render(elem[3], data)) + elif "if" == elem[0]: + do_else = True + if _eval(elem[1], data): + do_else = False + output.extend(self.render(elem[2], data)) + elif "elif" == elem[0]: + if do_else and _eval(elem[1], data): + do_else = False + output.extend(self.render(elem[2], data)) + elif "else" == elem[0]: + if do_else: + do_else = False + output.extend(self.render(elem[1], data)) + elif "macro" == elem[0]: + data[elem[1]] = TemplateBase(elem[2], self.render, data) + else: + raise TemplateRenderError("Invalid parse-tree (%s)." %(elem)) + + return output + +#----------------------------------------- +# template user-interface (putting it all together) + +class Template(TemplateBase): + """Template-User-Interface. + + :Usage: + :: + t = Template(...) (<- see __init__) + output = t(...) (<- see TemplateBase.__call__) + + :Example: + see module-docstring + """ + + def __init__(self, string=None,filename=None,parsetree=None, encoding='utf-8', data=None, escape=HTML, + loader_class=LoaderFile, + parser_class=Parser, + renderer_class=Renderer, + eval_class=EvalPseudoSandbox, + escape_func=escape): + """Load (+parse) a template. + + :Parameters: + - `string,filename,parsetree`: a template-string, + filename of a template to load, + or a template-parsetree. + (only one of these 3 is allowed) + - `encoding`: encoding of the template-files (only used for "filename") + - `data`: data to fill into the template by default (dictionary). + This data may later be overridden when rendering the template. + - `escape`: default-escaping for the template, may be overwritten by the template! + - `loader_class` + - `parser_class` + - `renderer_class` + - `eval_class` + - `escapefunc` + """ + if [string, filename, parsetree].count(None) != 2: + raise ValueError('Exactly 1 of string,filename,parsetree is necessary.') + + tmpl = None + # load template + if filename is not None: + incl_load = loader_class(os.path.dirname(filename), encoding).load + tmpl = incl_load(os.path.basename(filename)) + if string is not None: + incl_load = dummy_raise(NotImplementedError, "'include' not supported for template-strings.") + tmpl = LoaderString(encoding).load(string) + + # eval (incl. compile-cache) + templateeval = eval_class() + + # parse + if tmpl is not None: + p = parser_class(loadfunc=incl_load, testexpr=templateeval.compile, escape=escape) + parsetree = p.parse(tmpl) + del p + + # renderer + renderfunc = renderer_class(templateeval.eval, escape_func).render + + #create template + TemplateBase.__init__(self, parsetree, renderfunc, data) + + +#========================================= +#doctest + +def _doctest(): + """doctest this module.""" + import doctest + doctest.testmod() + +#---------------------- +if __name__ == '__main__': + if sys.version_info[0] <= 2: + _doctest() + +#========================================= diff --git a/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdlc b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdlc new file mode 100755 index 000000000000..d1718173c02c --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdlc @@ -0,0 +1,64 @@ +#!/usr/bin/env python +# +# UAVCAN DSDL compiler for libuavcan +# Supported Python versions: 3.2+, 2.7. +# +# Copyright (C) 2014 Pavel Kirienko +# + +from __future__ import division, absolute_import, print_function, unicode_literals +import os, sys, logging, argparse + +# This trickery allows to use the compiler even if pyuavcan is not installed in the system. +# This is extremely important, as it makes the compiler (and therefore libuavcan in general) +# totally dependency-free, except for the Python interpreter itself. +SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__)) +LOCAL_PYUAVCAN_DIR = os.path.join(SCRIPT_DIR, 'pydronecan') +RUNNING_FROM_SRC_DIR = os.path.isdir(LOCAL_PYUAVCAN_DIR) +if RUNNING_FROM_SRC_DIR: + #print('Running from the source directory') + sys.path.insert(0, SCRIPT_DIR) + sys.path.insert(0, LOCAL_PYUAVCAN_DIR) + +def configure_logging(verbosity): + fmt = '%(message)s' + level = { 0: logging.WARNING, 1: logging.INFO, 2: logging.DEBUG }.get(verbosity or 0, logging.DEBUG) + logging.basicConfig(stream=sys.stderr, level=level, format=fmt) + +def die(text): + print(text, file=sys.stderr) + exit(1) + +DEFAULT_OUTDIR = 'dsdlc_generated' + +DESCRIPTION = '''UAVCAN DSDL compiler for libuavcan. +Takes an input directory that contains an hierarchy of DSDL +definitions and converts it into compatible hierarchy of C++ types for libuavcan. +This script can be used directly from the source directory, no installation required! +Supported Python versions: 3.2+, 2.7. +''' + +argparser = argparse.ArgumentParser(description=DESCRIPTION) +argparser.add_argument('source_dir', nargs='+', help='source directory with DSDL definitions') +argparser.add_argument('--verbose', '-v', action='count', help='verbosity level (-v, -vv)') +argparser.add_argument('--outdir', '-O', default=DEFAULT_OUTDIR, help='output directory, default %s' % DEFAULT_OUTDIR) +argparser.add_argument('--incdir', '-I', default=[], action='append', help= +'''nested type namespaces, one path per argument. Can be also specified through the environment variable +UAVCAN_DSDL_INCLUDE_PATH, where the path entries are separated by colons ":"''') +args = argparser.parse_args() + +configure_logging(args.verbose) + +try: + extra_incdir = os.environ['UAVCAN_DSDL_INCLUDE_PATH'].split(':') + logging.info('Additional include directories: %s', extra_incdir) + args.incdir += extra_incdir +except KeyError: + pass + +from libuavcan_dsdl_compiler import run as dsdlc_run +try: + dsdlc_run(args.source_dir, args.incdir, args.outdir) +except Exception as ex: + logging.error('Compiler failure', exc_info=True) + die(str(ex)) diff --git a/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan new file mode 160000 index 000000000000..19fdf2e5b383 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan @@ -0,0 +1 @@ +Subproject commit 19fdf2e5b383243ccdb1094edae0603cf11469e8 diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/build_config.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/build_config.hpp new file mode 100644 index 000000000000..8a886c2975fc --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/build_config.hpp @@ -0,0 +1,276 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_BUILD_CONFIG_HPP_INCLUDED +#define UAVCAN_BUILD_CONFIG_HPP_INCLUDED + +/** + * UAVCAN version definition + */ +#define UAVCAN_VERSION_MAJOR 1 +#define UAVCAN_VERSION_MINOR 0 + +/** + * UAVCAN_CPP_VERSION - version of the C++ standard used during compilation. + * This definition contains the integer year number after which the standard was named: + * - 2003 for C++03 + * - 2011 for C++11 + * + * This config automatically sets according to the actual C++ standard used by the compiler. + * + * In C++03 mode the library has almost zero dependency on the C++ standard library, which allows + * to use it on platforms with a very limited C++ support. On the other hand, C++11 mode requires + * many parts of the standard library (e.g. ), thus the user might want to force older + * standard than used by the compiler, in which case this symbol can be overridden manually via + * compiler flags. + */ +#define UAVCAN_CPP11 2011 +#define UAVCAN_CPP03 2003 + +#ifndef UAVCAN_CPP_VERSION +# if __cplusplus > 201200 +# error Unsupported C++ standard. You can explicitly set UAVCAN_CPP_VERSION=UAVCAN_CPP11 to silence this error. +# elif (__cplusplus > 201100) || defined(__GXX_EXPERIMENTAL_CXX0X__) +# define UAVCAN_CPP_VERSION UAVCAN_CPP11 +# else +# define UAVCAN_CPP_VERSION UAVCAN_CPP03 +# endif +#endif + +/** + * The library uses UAVCAN_NULLPTR instead of UAVCAN_NULLPTR and nullptr in order to allow the use of + * -Wzero-as-null-pointer-constant. + */ +#ifndef UAVCAN_NULLPTR +# if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# define UAVCAN_NULLPTR nullptr +# else +# define UAVCAN_NULLPTR NULL +# endif +#endif + +/** + * By default, libuavcan enables all features if it detects that it is being built for a general-purpose + * target like Linux. Value of this macro influences other configuration options located below in this file. + * This macro can be overriden if needed. + */ +#ifndef UAVCAN_GENERAL_PURPOSE_PLATFORM +# if (defined(__linux__) || defined(__linux) || defined(__APPLE__) ||\ + defined(_WIN64) || defined(_WIN32) || defined(__ANDROID__) ||\ + defined(_SYSTYPE_BSD) || defined(__FreeBSD__)) +# define UAVCAN_GENERAL_PURPOSE_PLATFORM 1 +# else +# define UAVCAN_GENERAL_PURPOSE_PLATFORM 0 +# endif +#endif + +/** + * This macro enables built-in runtime checks and debug output via printf(). + * Should be used only for library development. + */ +#ifndef UAVCAN_DEBUG +# define UAVCAN_DEBUG 0 +#endif + +/** + * This option allows to select whether libuavcan should throw exceptions on fatal errors, or try to handle + * errors differently. By default, exceptions will be enabled only if the library is built for a general-purpose + * OS like Linux. Set UAVCAN_EXCEPTIONS explicitly to override. + */ +#ifndef UAVCAN_EXCEPTIONS +# define UAVCAN_EXCEPTIONS UAVCAN_GENERAL_PURPOSE_PLATFORM +#endif + +/** + * This specification is used by some error reporting functions like in the Logger class. + * The default can be overriden by defining the macro UAVCAN_NOEXCEPT explicitly, e.g. via compiler options. + */ +#ifndef UAVCAN_NOEXCEPT +# if UAVCAN_EXCEPTIONS +# if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# define UAVCAN_NOEXCEPT noexcept +# else +# define UAVCAN_NOEXCEPT throw() +# endif +# else +# define UAVCAN_NOEXCEPT +# endif +#endif + +/** + * Declaration visibility + * http://gcc.gnu.org/wiki/Visibility + */ +#ifndef UAVCAN_EXPORT +# define UAVCAN_EXPORT +#endif + +/** + * Trade-off between ROM/RAM usage and functionality/determinism. + * Note that this feature is not well tested and should be avoided. + * Use code search for UAVCAN_TINY to find what functionality will be disabled. + * This is particularly useful for embedded systems with less than 40kB of ROM. + */ +#ifndef UAVCAN_TINY +# define UAVCAN_TINY 0 +#endif + +/** + * Disable the global data type registry, which can save some space on embedded systems. + */ +#ifndef UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY +# define UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY 0 +#endif + +/** + * toString() methods will be disabled by default, unless the library is built for a general-purpose target like Linux. + * It is not recommended to enable toString() on embedded targets as code size will explode. + */ +#ifndef UAVCAN_TOSTRING +# if UAVCAN_EXCEPTIONS +# define UAVCAN_TOSTRING UAVCAN_GENERAL_PURPOSE_PLATFORM +# else +# define UAVCAN_TOSTRING 0 +# endif +#endif + +#if UAVCAN_TOSTRING +# if !UAVCAN_EXCEPTIONS +# error UAVCAN_TOSTRING requires UAVCAN_EXCEPTIONS +# endif +# include +#endif + +/** + * Some C++ implementations are half-broken because they don't implement the placement new operator. + * If UAVCAN_IMPLEMENT_PLACEMENT_NEW is defined, libuavcan will implement its own operator new (std::size_t, void*) + * and its delete() counterpart, instead of relying on the standard header . + */ +#ifndef UAVCAN_IMPLEMENT_PLACEMENT_NEW +# define UAVCAN_IMPLEMENT_PLACEMENT_NEW 0 +#endif + +/** + * Allows the user's application to provide custom implementation of uavcan::snprintf(), + * which is often useful on deeply embedded systems. + */ +#ifndef UAVCAN_USE_EXTERNAL_SNPRINTF +# define UAVCAN_USE_EXTERNAL_SNPRINTF 0 +#endif + +/** + * Allows the user's application to provide a custom implementation of IEEE754Converter::nativeIeeeToHalf and + * IEEE754Converter::halfToNativeIeee. + */ +#ifndef UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION +# define UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION 0 +#endif + +/** + * Run time checks. + * Resolves to the standard assert() by default. + * Disabled completely if UAVCAN_NO_ASSERTIONS is defined. + */ +#ifndef UAVCAN_ASSERT +# ifndef UAVCAN_NO_ASSERTIONS +# define UAVCAN_NO_ASSERTIONS 0 +# endif +# if UAVCAN_NO_ASSERTIONS +# define UAVCAN_ASSERT(x) +# else +# define UAVCAN_ASSERT(x) assert(x) +# endif +#endif + +#ifndef UAVCAN_LIKELY +# if __GNUC__ +# define UAVCAN_LIKELY(x) __builtin_expect(!!(x), true) +# else +# define UAVCAN_LIKELY(x) (x) +# endif +#endif + +#ifndef UAVCAN_UNLIKELY +# if __GNUC__ +# define UAVCAN_UNLIKELY(x) __builtin_expect(!!(x), false) +# else +# define UAVCAN_UNLIKELY(x) (x) +# endif +#endif + +namespace uavcan +{ +/** + * Memory pool block size. + * + * The default of 64 bytes should be OK for any target arch up to AMD64 and any compiler. + * + * The library leverages compile-time checks to ensure that all types that are subject to dynamic allocation + * fit this size, otherwise compilation fails. + * + * For platforms featuring small pointer width (16..32 bits), UAVCAN_MEM_POOL_BLOCK_SIZE can often be safely + * reduced to 56 or even 48 bytes, which leads to lower memory footprint. + * + * Note that the pool block size shall be aligned at biggest alignment of the target platform (detected and + * checked automatically at compile time). + */ +#ifdef UAVCAN_MEM_POOL_BLOCK_SIZE +/// Explicitly specified by the user. +static const unsigned MemPoolBlockSize = UAVCAN_MEM_POOL_BLOCK_SIZE; +#elif defined(__BIGGEST_ALIGNMENT__) && (__BIGGEST_ALIGNMENT__ <= 8) +/// Convenient default for GCC-like compilers - if alignment allows, pool block size can be safely reduced. +static const unsigned MemPoolBlockSize = 56; +#else +/// Safe default that should be OK for any platform. +static const unsigned MemPoolBlockSize = 64; +#endif + +#ifdef __BIGGEST_ALIGNMENT__ +static const unsigned MemPoolAlignment = __BIGGEST_ALIGNMENT__; +#else +static const unsigned MemPoolAlignment = 16; +#endif + +typedef char _alignment_check_for_MEM_POOL_BLOCK_SIZE[((MemPoolBlockSize & (MemPoolAlignment - 1)) == 0) ? 1 : -1]; + +/** + * This class that allows to check at compile time whether type T can be allocated using the memory pool. + * If the check fails, compilation fails. + */ +template +struct UAVCAN_EXPORT IsDynamicallyAllocatable +{ + static void check() + { + char dummy[(sizeof(T) <= MemPoolBlockSize) ? 1 : -1] = { '0' }; + (void)dummy; + } +}; + +/** + * Float comparison precision. + * For details refer to: + * http://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ + * https://code.google.com/p/googletest/source/browse/trunk/include/gtest/internal/gtest-internal.h + */ +#ifdef UAVCAN_FLOAT_COMPARISON_EPSILON_MULT +static const unsigned FloatComparisonEpsilonMult = UAVCAN_FLOAT_COMPARISON_EPSILON_MULT; +#else +static const unsigned FloatComparisonEpsilonMult = 10; +#endif + +/** + * Maximum number of CAN acceptance filters available on the platform + */ +#ifdef UAVCAN_MAX_CAN_ACCEPTANCE_FILTERS +/// Explicitly specified by the user. +static const unsigned MaxCanAcceptanceFilters = UAVCAN_MAX_CAN_ACCEPTANCE_FILTERS; +#else +/// Default that should be OK for any platform. +static const unsigned MaxCanAcceptanceFilters = 32; +#endif + +} + +#endif // UAVCAN_BUILD_CONFIG_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/data_type.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/data_type.hpp new file mode 100644 index 000000000000..96a8a7f71bc7 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/data_type.hpp @@ -0,0 +1,176 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_DATA_TYPE_HPP_INCLUDED +#define UAVCAN_DATA_TYPE_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class UAVCAN_EXPORT TransferCRC; + +enum DataTypeKind +{ + DataTypeKindService, + DataTypeKindMessage +}; + +static const uint8_t NumDataTypeKinds = 2; + + +static inline DataTypeKind getDataTypeKindForTransferType(const TransferType tt) +{ + if (tt == TransferTypeServiceResponse || + tt == TransferTypeServiceRequest) + { + return DataTypeKindService; + } + else if (tt == TransferTypeMessageBroadcast) + { + return DataTypeKindMessage; + } + else + { + UAVCAN_ASSERT(0); + return DataTypeKind(0); + } +} + + +class UAVCAN_EXPORT DataTypeID +{ + uint32_t value_; + +public: + static const uint16_t MaxServiceDataTypeIDValue = 255; + static const uint16_t MaxMessageDataTypeIDValue = 65535; + static const uint16_t MaxPossibleDataTypeIDValue = MaxMessageDataTypeIDValue; + + DataTypeID() : value_(0xFFFFFFFFUL) { } + + DataTypeID(uint16_t id) // Implicit + : value_(id) + { } + + static DataTypeID getMaxValueForDataTypeKind(const DataTypeKind dtkind); + + bool isValidForDataTypeKind(DataTypeKind dtkind) const + { + return value_ <= getMaxValueForDataTypeKind(dtkind).get(); + } + + uint16_t get() const { return static_cast(value_); } + + bool operator==(DataTypeID rhs) const { return value_ == rhs.value_; } + bool operator!=(DataTypeID rhs) const { return value_ != rhs.value_; } + + bool operator<(DataTypeID rhs) const { return value_ < rhs.value_; } + bool operator>(DataTypeID rhs) const { return value_ > rhs.value_; } + bool operator<=(DataTypeID rhs) const { return value_ <= rhs.value_; } + bool operator>=(DataTypeID rhs) const { return value_ >= rhs.value_; } +}; + + +/** + * CRC-64-WE + * Description: http://reveng.sourceforge.net/crc-catalogue/17plus.htm#crc.cat-bits.64 + * Initial value: 0xFFFFFFFFFFFFFFFF + * Poly: 0x42F0E1EBA9EA3693 + * Reverse: no + * Output xor: 0xFFFFFFFFFFFFFFFF + * Check: 0x62EC59E3F1A4F00A + */ +class UAVCAN_EXPORT DataTypeSignatureCRC +{ + uint64_t crc_; + +public: + static DataTypeSignatureCRC extend(uint64_t crc); + + DataTypeSignatureCRC() : crc_(0xFFFFFFFFFFFFFFFFULL) { } + + void add(uint8_t byte); + + void add(const uint8_t* bytes, unsigned len); + + uint64_t get() const { return crc_ ^ 0xFFFFFFFFFFFFFFFFULL; } +}; + + +class UAVCAN_EXPORT DataTypeSignature +{ + uint64_t value_; + + void mixin64(uint64_t x); + +public: + DataTypeSignature() : value_(0) { } + explicit DataTypeSignature(uint64_t value) : value_(value) { } + + void extend(DataTypeSignature dts); + + TransferCRC toTransferCRC() const; + + uint64_t get() const { return value_; } + + bool operator==(DataTypeSignature rhs) const { return value_ == rhs.value_; } + bool operator!=(DataTypeSignature rhs) const { return !operator==(rhs); } +}; + +/** + * This class contains complete description of a data type. + */ +class UAVCAN_EXPORT DataTypeDescriptor +{ + DataTypeSignature signature_; + const char* full_name_; + DataTypeKind kind_; + DataTypeID id_; + +public: + static const unsigned MaxFullNameLen = 80; + + DataTypeDescriptor() : + full_name_(""), + kind_(DataTypeKind(0)) + { } + + DataTypeDescriptor(DataTypeKind kind, DataTypeID id, const DataTypeSignature& signature, const char* name) : + signature_(signature), + full_name_(name), + kind_(kind), + id_(id) + { + UAVCAN_ASSERT((kind == DataTypeKindMessage) || (kind == DataTypeKindService)); + UAVCAN_ASSERT(name); + UAVCAN_ASSERT(std::strlen(name) <= MaxFullNameLen); + } + + bool isValid() const; + + DataTypeKind getKind() const { return kind_; } + DataTypeID getID() const { return id_; } + const DataTypeSignature& getSignature() const { return signature_; } + const char* getFullName() const { return full_name_; } + + bool match(DataTypeKind kind, const char* name) const; + bool match(DataTypeKind kind, DataTypeID id) const; + + bool operator!=(const DataTypeDescriptor& rhs) const { return !operator==(rhs); } + bool operator==(const DataTypeDescriptor& rhs) const; + +#if UAVCAN_TOSTRING + std::string toString() const; +#endif +}; + +} + +#endif // UAVCAN_DATA_TYPE_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/debug.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/debug.hpp new file mode 100644 index 000000000000..8b2ca172476f --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/debug.hpp @@ -0,0 +1,35 @@ +/* + * Debug stuff, should only be used for library development. + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_DEBUG_HPP_INCLUDED +#define UAVCAN_DEBUG_HPP_INCLUDED + +#include + +#if UAVCAN_DEBUG + +# include +# include + +# if __GNUC__ +__attribute__ ((format(printf, 2, 3))) +# endif +static void UAVCAN_TRACE(const char* src, const char* fmt, ...) +{ + va_list args; + (void)std::printf("UAVCAN: %s: ", src); + va_start(args, fmt); + (void)std::vprintf(fmt, args); + va_end(args); + (void)std::puts(""); +} + +#else + +# define UAVCAN_TRACE(...) ((void)0) + +#endif + +#endif // UAVCAN_DEBUG_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/driver/can.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/driver/can.hpp new file mode 100644 index 000000000000..d20feca1547e --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/driver/can.hpp @@ -0,0 +1,249 @@ +/* + * CAN bus driver interface. + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_DRIVER_CAN_HPP_INCLUDED +#define UAVCAN_DRIVER_CAN_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This limit is defined by the specification. + */ +enum { MaxCanIfaces = 3 }; + +/** + * Raw CAN frame, as passed to/from the CAN driver. + */ +struct UAVCAN_EXPORT CanFrame +{ + static const uint32_t MaskStdID = 0x000007FFU; + static const uint32_t MaskExtID = 0x1FFFFFFFU; + static const uint32_t FlagEFF = 1U << 31; ///< Extended frame format + static const uint32_t FlagRTR = 1U << 30; ///< Remote transmission request + static const uint32_t FlagERR = 1U << 29; ///< Error frame + + static const uint8_t MaxDataLen = 8; + + uint32_t id; ///< CAN ID with flags (above) + uint8_t data[MaxDataLen]; + uint8_t dlc; ///< Data Length Code + + CanFrame() : + id(0), + dlc(0) + { + fill(data, data + MaxDataLen, uint8_t(0)); + } + + CanFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len) : + id(can_id), + dlc((data_len > MaxDataLen) ? MaxDataLen : data_len) + { + UAVCAN_ASSERT(can_data != UAVCAN_NULLPTR); + UAVCAN_ASSERT(data_len == dlc); + (void)copy(can_data, can_data + dlc, this->data); + } + + bool operator!=(const CanFrame& rhs) const { return !operator==(rhs); } + bool operator==(const CanFrame& rhs) const + { + return (id == rhs.id) && (dlc == rhs.dlc) && equal(data, data + dlc, rhs.data); + } + + bool isExtended() const { return id & FlagEFF; } + bool isRemoteTransmissionRequest() const { return id & FlagRTR; } + bool isErrorFrame() const { return id & FlagERR; } + +#if UAVCAN_TOSTRING + enum StringRepresentation + { + StrTight, ///< Minimum string length (default) + StrAligned ///< Fixed formatting for any frame + }; + + std::string toString(StringRepresentation mode = StrTight) const; + +#endif + + /** + * CAN frame arbitration rules, particularly STD vs EXT: + * Marco Di Natale - "Understanding and using the Controller Area Network" + * http://www6.in.tum.de/pub/Main/TeachingWs2013MSE/CANbus.pdf + */ + bool priorityHigherThan(const CanFrame& rhs) const; + bool priorityLowerThan(const CanFrame& rhs) const { return rhs.priorityHigherThan(*this); } +}; + +/** + * CAN hardware filter config struct. + * Flags from @ref CanFrame can be applied to define frame type (EFF, EXT, etc.). + * @ref ICanIface::configureFilters(). + */ +struct UAVCAN_EXPORT CanFilterConfig +{ + uint32_t id; + uint32_t mask; + + bool operator==(const CanFilterConfig& rhs) const + { + return rhs.id == id && rhs.mask == mask; + } + + CanFilterConfig() : + id(0), + mask(0) + { } +}; + +/** + * Events to look for during @ref ICanDriver::select() call. + * Bit position defines iface index, e.g. read = 1 << 2 to read from the third iface. + */ +struct UAVCAN_EXPORT CanSelectMasks +{ + uint8_t read; + uint8_t write; + + CanSelectMasks() : + read(0), + write(0) + { } +}; + +/** + * Special IO flags. + * + * @ref CanIOFlagLoopback - Send the frame back to RX with true TX timestamps. + * + * @ref CanIOFlagAbortOnError - Abort transmission on first bus error instead of retransmitting. This does not + * affect the case of arbitration loss, in which case the retransmission will work + * as usual. This flag is used together with anonymous messages which allows to + * implement CSMA bus access. Read the spec for details. + */ +typedef uint16_t CanIOFlags; +static const CanIOFlags CanIOFlagLoopback = 1; +static const CanIOFlags CanIOFlagAbortOnError = 2; + +/** + * Single non-blocking CAN interface. + */ +class UAVCAN_EXPORT ICanIface +{ +public: + virtual ~ICanIface() { } + + /** + * Non-blocking transmission. + * + * If the frame wasn't transmitted upon TX deadline, the driver should discard it. + * + * Note that it is LIKELY that the library will want to send the frames that were passed into the select() + * method as the next ones to transmit, but it is NOT guaranteed. The library can replace those with new + * frames between the calls. + * + * @return 1 = one frame transmitted, 0 = TX buffer full, negative for error. + */ + virtual int16_t send(const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags) = 0; + + /** + * Non-blocking reception. + * + * Timestamps should be provided by the CAN driver, ideally by the hardware CAN controller. + * + * Monotonic timestamp is required and can be not precise since it is needed only for + * protocol timing validation (transfer timeouts and inter-transfer intervals). + * + * UTC timestamp is optional, if available it will be used for precise time synchronization; + * must be set to zero if not available. + * + * Refer to @ref ISystemClock to learn more about timestamps. + * + * @param [out] out_ts_monotonic Monotonic timestamp, mandatory. + * @param [out] out_ts_utc UTC timestamp, optional, zero if unknown. + * @return 1 = one frame received, 0 = RX buffer empty, negative for error. + */ + virtual int16_t receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic, UtcTime& out_ts_utc, + CanIOFlags& out_flags) = 0; + + /** + * Configure the hardware CAN filters. @ref CanFilterConfig. + * + * @return 0 = success, negative for error. + */ + virtual int16_t configureFilters(const CanFilterConfig* filter_configs, uint16_t num_configs) = 0; + + /** + * Number of available hardware filters. + */ + virtual uint16_t getNumFilters() const = 0; + + /** + * Continuously incrementing counter of hardware errors. + * Arbitration lost should not be treated as a hardware error. + */ + virtual uint64_t getErrorCount() const = 0; +}; + +/** + * Generic CAN driver. + */ +class UAVCAN_EXPORT ICanDriver +{ +public: + virtual ~ICanDriver() { } + + /** + * Returns an interface by index, or null pointer if the index is out of range. + */ + virtual ICanIface* getIface(uint8_t iface_index) = 0; + + /** + * Default implementation of this method calls the non-const overload of getIface(). + * Can be overriden by the application if necessary. + */ + virtual const ICanIface* getIface(uint8_t iface_index) const + { + return const_cast(this)->getIface(iface_index); + } + + /** + * Total number of available CAN interfaces. + * This value shall not change after initialization. + */ + virtual uint8_t getNumIfaces() const = 0; + + /** + * Block until the deadline, or one of the specified interfaces becomes available for read or write. + * + * Iface masks will be modified by the driver to indicate which exactly interfaces are available for IO. + * + * Bit position in the masks defines interface index. + * + * Note that it is allowed to return from this method even if no requested events actually happened, or if + * there are events that were not requested by the library. + * + * The pending TX argument contains an array of pointers to CAN frames that the library wants to transmit + * next, per interface. This is intended to allow the driver to properly prioritize transmissions; many + * drivers will not need to use it. If a write flag for the given interface is set to one in the select mask + * structure, then the corresponding pointer is guaranteed to be valid (not UAVCAN_NULLPTR). + * + * @param [in,out] inout_masks Masks indicating which interfaces are needed/available for IO. + * @param [in] pending_tx Array of frames, per interface, that are likely to be transmitted next. + * @param [in] blocking_deadline Zero means non-blocking operation. + * @return Positive number of ready interfaces or negative error code. + */ + virtual int16_t select(CanSelectMasks& inout_masks, + const CanFrame* (& pending_tx)[MaxCanIfaces], + MonotonicTime blocking_deadline) = 0; +}; + +} + +#endif // UAVCAN_DRIVER_CAN_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/driver/system_clock.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/driver/system_clock.hpp new file mode 100644 index 000000000000..6926e9af6186 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/driver/system_clock.hpp @@ -0,0 +1,60 @@ +/* + * System clock driver interface. + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_DRIVER_SYSTEM_CLOCK_HPP_INCLUDED +#define UAVCAN_DRIVER_SYSTEM_CLOCK_HPP_INCLUDED + +#include +#include +#include + +namespace uavcan +{ + +/** + * System clock interface - monotonic and UTC. + */ +class UAVCAN_EXPORT ISystemClock +{ +public: + virtual ~ISystemClock() { } + + /** + * Monototic system clock. + * + * This clock shall never jump or change rate; the base time is irrelevant. + * This clock is mandatory and must remain functional at all times. + * + * On POSIX systems use clock_gettime() with CLOCK_MONOTONIC. + */ + virtual MonotonicTime getMonotonic() const = 0; + + /** + * Global network clock. + * It doesn't have to be UTC, the name is a bit misleading - actual time base doesn't matter. + * + * This clock can be synchronized with other nodes on the bus, hence it can jump and/or change + * rate occasionally. + * This clock is optional; if it is not supported, return zero. Also return zero if the UTC time + * is not available yet (e.g. the device has just started up with no battery clock). + * + * For POSIX refer to clock_gettime(), gettimeofday(). + */ + virtual UtcTime getUtc() const = 0; + + /** + * Adjust the network-synchronized clock. + * Refer to @ref getUtc() for details. + * + * For POSIX refer to adjtime(), settimeofday(). + * + * @param [in] adjustment Amount of time to add to the clock value. + */ + virtual void adjustUtc(UtcDuration adjustment) = 0; +}; + +} + +#endif // UAVCAN_DRIVER_SYSTEM_CLOCK_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/dynamic_memory.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/dynamic_memory.hpp new file mode 100644 index 000000000000..da977d8c6845 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/dynamic_memory.hpp @@ -0,0 +1,210 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_DYNAMIC_MEMORY_HPP_INCLUDED +#define UAVCAN_DYNAMIC_MEMORY_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This interface is used by other library components that need dynamic memory. + */ +class UAVCAN_EXPORT IPoolAllocator +{ +public: + virtual ~IPoolAllocator() { } + + virtual void* allocate(std::size_t size) = 0; + virtual void deallocate(const void* ptr) = 0; + + /** + * Returns the maximum number of blocks this allocator can allocate. + */ + virtual uint16_t getBlockCapacity() const = 0; +}; + +/** + * Classic implementation of a pool allocator (Meyers). + * + * The allocator can be made thread-safe (optional) by means of providing a RAII-lock type via the second template + * argument. The allocator uses the lock only to access the shared state, therefore critical sections are only a few + * cycles long, which implies that it should be acceptable to use hardware IRQ disabling instead of a mutex for + * performance reasons. For example, an IRQ-based RAII-lock type can be implemented as follows: + * struct RaiiSynchronizer + * { + * RaiiSynchronizer() { __disable_irq(); } + * ~RaiiSynchronizer() { __enable_irq(); } + * }; + */ +template +class UAVCAN_EXPORT PoolAllocator : public IPoolAllocator, + Noncopyable +{ + union Node + { + uint8_t data[BlockSize]; + Node* next; + }; + + Node* free_list_; + union + { + uint8_t bytes[PoolSize]; + long double _aligner1; + long long _aligner2; + Node _aligner3; + } pool_; + + uint16_t used_; + uint16_t max_used_; + +public: + static const uint16_t NumBlocks = PoolSize / BlockSize; + + PoolAllocator(); + + virtual void* allocate(std::size_t size) override; + virtual void deallocate(const void* ptr) override; + + virtual uint16_t getBlockCapacity() const override { return NumBlocks; } + + /** + * Return the number of blocks that are currently allocated/unallocated. + */ + uint16_t getNumUsedBlocks() const + { + RaiiSynchronizer lock; + (void)lock; + return used_; + } + uint16_t getNumFreeBlocks() const + { + RaiiSynchronizer lock; + (void)lock; + return static_cast(NumBlocks - used_); + } + + /** + * Returns the maximum number of blocks that were ever allocated at the same time. + */ + uint16_t getPeakNumUsedBlocks() const + { + RaiiSynchronizer lock; + (void)lock; + return max_used_; + } +}; + +/** + * Limits the maximum number of blocks that can be allocated in a given allocator. + */ +class LimitedPoolAllocator : public IPoolAllocator +{ + IPoolAllocator& allocator_; + const uint16_t max_blocks_; + uint16_t used_blocks_; + +public: + LimitedPoolAllocator(IPoolAllocator& allocator, std::size_t max_blocks) + : allocator_(allocator) + , max_blocks_(static_cast(min(max_blocks, 0xFFFFU))) + , used_blocks_(0) + { + UAVCAN_ASSERT(max_blocks_ > 0); + } + + virtual void* allocate(std::size_t size) override; + virtual void deallocate(const void* ptr) override; + + virtual uint16_t getBlockCapacity() const override; +}; + +// ---------------------------------------------------------------------------- + +/* + * PoolAllocator<> + */ +template +const uint16_t PoolAllocator::NumBlocks; + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-align" + +template +PoolAllocator::PoolAllocator() : + free_list_(reinterpret_cast(pool_.bytes)), + used_(0), + max_used_(0) +{ + // The limit is imposed by the width of the pool usage tracking variables. + StaticAssert<((PoolSize / BlockSize) <= 0xFFFFU)>::check(); + + (void)std::memset(pool_.bytes, 0, PoolSize); + for (unsigned i = 0; (i + 1) < (NumBlocks - 1 + 1); i++) // -Werror=type-limits + { + // coverity[dead_error_line : FALSE] + free_list_[i].next = free_list_ + i + 1; + } + free_list_[NumBlocks - 1].next = UAVCAN_NULLPTR; +} +#pragma GCC diagnostic pop + +template +void* PoolAllocator::allocate(std::size_t size) +{ + if (free_list_ == UAVCAN_NULLPTR || size > BlockSize) + { + return UAVCAN_NULLPTR; + } + + RaiiSynchronizer lock; + (void)lock; + + void* pmem = free_list_; + free_list_ = free_list_->next; + + // Statistics + UAVCAN_ASSERT(used_ < NumBlocks); + used_++; + if (used_ > max_used_) + { + max_used_ = used_; + } + + return pmem; +} + +template +void PoolAllocator::deallocate(const void* ptr) +{ + if (ptr == UAVCAN_NULLPTR) + { + return; + } + + RaiiSynchronizer lock; + (void)lock; + + Node* p = static_cast(const_cast(ptr)); + p->next = free_list_; + free_list_ = p; + + // Statistics + UAVCAN_ASSERT(used_ > 0); + used_--; +} + +} + +#endif // UAVCAN_DYNAMIC_MEMORY_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/error.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/error.hpp new file mode 100644 index 000000000000..4f67e9a6ec8d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/error.hpp @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_ERROR_HPP_INCLUDED +#define UAVCAN_ERROR_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace +{ +/** + * Common error codes. + * + * Functions that return signed integers may also return inverted error codes, + * i.e. returned value should be inverted back to get the actual error code. + * + * Return code 0 (zero) means no error. + * + * @{ + */ +const int16_t ErrFailure = 1; ///< General failure +const int16_t ErrInvalidParam = 2; +const int16_t ErrMemory = 3; +const int16_t ErrDriver = 4; ///< Platform driver error +const int16_t ErrUnknownDataType = 5; +const int16_t ErrInvalidMarshalData = 6; +const int16_t ErrInvalidTransferListener = 7; +const int16_t ErrNotInited = 8; +const int16_t ErrRecursiveCall = 9; +const int16_t ErrLogic = 10; +const int16_t ErrPassiveMode = 11; ///< Operation not permitted in passive mode +const int16_t ErrTransferTooLong = 12; ///< Transfer of this length cannot be sent with given transfer type +const int16_t ErrInvalidConfiguration = 13; +/** + * @} + */ + +} + +/** + * Fatal error handler. + * Behavior: + * - If exceptions are enabled, throws std::runtime_error() with the supplied message text; + * - If assertions are enabled (see UAVCAN_ASSERT()), aborts execution using zero assertion. + * - Otherwise aborts execution via std::abort(). + */ +#if __GNUC__ +__attribute__ ((noreturn)) +#endif +UAVCAN_EXPORT +// coverity[+kill] +void handleFatalError(const char* msg); + +} + +#endif // UAVCAN_ERROR_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/helpers/heap_based_pool_allocator.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/helpers/heap_based_pool_allocator.hpp new file mode 100644 index 000000000000..42d4a35950c6 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/helpers/heap_based_pool_allocator.hpp @@ -0,0 +1,220 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_HELPERS_HEAP_BASED_POOL_ALLOCATOR_HPP_INCLUDED +#define UAVCAN_HELPERS_HEAP_BASED_POOL_ALLOCATOR_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ +/** + * A special-purpose implementation of a pool allocator that keeps the pool in the heap using malloc()/free(). + * The pool grows dynamically, ad-hoc, thus using as little memory as possible. + * + * Allocated blocks will not be freed back automatically, but there are two ways to force their deallocation: + * - Call @ref shrink() - this method frees all blocks that are unused at the moment. + * - Destroy the object - the desctructor calls @ref shrink(). + * + * The pool can be limited in growth with hard and soft limits. + * The soft limit defines the value that will be reported via @ref IPoolAllocator::getBlockCapacity(). + * The hard limit defines the maximum number of blocks that can be allocated from heap. + * Typically, the hard limit should be equal or greater than the soft limit. + * + * The allocator can be made thread-safe (optional) by means of providing a RAII-lock type via the second template + * argument. The allocator uses the lock only to access the shared state, therefore critical sections are only a few + * cycles long, which implies that it should be acceptable to use hardware IRQ disabling instead of a mutex for + * performance reasons. For example, an IRQ-based RAII-lock type can be implemented as follows: + * struct RaiiSynchronizer + * { + * RaiiSynchronizer() { __disable_irq(); } + * ~RaiiSynchronizer() { __enable_irq(); } + * }; + */ +template +class UAVCAN_EXPORT HeapBasedPoolAllocator : public IPoolAllocator, + Noncopyable +{ + union Node + { + Node* next; + private: + uint8_t data[BlockSize]; + long double _aligner1; + long long _aligner2; + }; + + const uint16_t capacity_soft_limit_; + const uint16_t capacity_hard_limit_; + + uint16_t num_reserved_blocks_; + uint16_t num_allocated_blocks_; + + Node* reserve_; + +public: + /** + * The allocator initializes with empty reserve, so first allocations will be served from heap. + * + * @param block_capacity_soft_limit Block capacity that will be reported via @ref getBlockCapacity(). + * + * @param block_capacity_hard_limit Real block capacity limit; the number of allocated blocks will never + * exceed this value. Hard limit should be higher than soft limit. + * Default value is two times the soft limit. + */ + HeapBasedPoolAllocator(uint16_t block_capacity_soft_limit, + uint16_t block_capacity_hard_limit = 0) : + capacity_soft_limit_(block_capacity_soft_limit), + capacity_hard_limit_((block_capacity_hard_limit > 0) ? block_capacity_hard_limit : + static_cast(min(static_cast(block_capacity_soft_limit) * 2U, + static_cast(NumericTraits::max())))), + num_reserved_blocks_(0), + num_allocated_blocks_(0), + reserve_(UAVCAN_NULLPTR) + { } + + /** + * The destructor de-allocates all blocks that are currently in the reserve. + * BLOCKS THAT ARE CURRENTLY HELD BY THE APPLICATION WILL LEAK. + */ + ~HeapBasedPoolAllocator() + { + shrink(); +#if UAVCAN_DEBUG + if (num_allocated_blocks_ > 0) + { + UAVCAN_TRACE("HeapBasedPoolAllocator", "%u BLOCKS LEAKED", num_allocated_blocks_); + } +#endif + } + + /** + * Takes a block from the reserve, unless it's empty. + * In the latter case, allocates a new block in the heap. + */ + virtual void* allocate(std::size_t size) override + { + if (size > BlockSize) + { + return UAVCAN_NULLPTR; + } + + { + RaiiSynchronizer lock; + (void)lock; + + Node* const p = reserve_; + + if (UAVCAN_LIKELY(p != UAVCAN_NULLPTR)) + { + reserve_ = reserve_->next; + num_allocated_blocks_++; + return p; + } + + if (num_reserved_blocks_ >= capacity_hard_limit_) // Hard limit reached, no further allocations + { + return UAVCAN_NULLPTR; + } + } + + // Unlikely branch + void* const m = std::malloc(sizeof(Node)); + if (m != UAVCAN_NULLPTR) + { + RaiiSynchronizer lock; + (void)lock; + + num_reserved_blocks_++; + num_allocated_blocks_++; + } + return m; + } + + /** + * Puts the block back to reserve. + * The block will not be free()d automatically; see @ref shrink(). + */ + virtual void deallocate(const void* ptr) override + { + if (ptr != UAVCAN_NULLPTR) + { + RaiiSynchronizer lock; + (void)lock; + + Node* const node = static_cast(const_cast(ptr)); + node->next = reserve_; + reserve_ = node; + + num_allocated_blocks_--; + } + } + + /** + * The soft limit. + */ + virtual uint16_t getBlockCapacity() const override { return capacity_soft_limit_; } + + /** + * The hard limit. + */ + uint16_t getBlockCapacityHardLimit() const { return capacity_hard_limit_; } + + /** + * Frees all blocks that are not in use at the moment. + * Post-condition is getNumAllocatedBlocks() == getNumReservedBlocks(). + */ + void shrink() + { + Node* p = UAVCAN_NULLPTR; + for (;;) + { + { + RaiiSynchronizer lock; + (void)lock; + // Removing from reserve and updating the counter. + p = reserve_; + if (p != UAVCAN_NULLPTR) + { + reserve_ = reserve_->next; + num_reserved_blocks_--; + } + else + { + break; + } + } + // Then freeing, having left the critical section. + std::free(p); + } + } + + /** + * Number of blocks that are currently in use by the application. + */ + uint16_t getNumAllocatedBlocks() const + { + RaiiSynchronizer lock; + (void)lock; + return num_allocated_blocks_; + } + + /** + * Number of blocks that are acquired from the heap. + */ + uint16_t getNumReservedBlocks() const + { + RaiiSynchronizer lock; + (void)lock; + return num_reserved_blocks_; + } +}; + +} + +#endif diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/helpers/ostream.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/helpers/ostream.hpp new file mode 100644 index 000000000000..56fd9753a914 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/helpers/ostream.hpp @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_HELPERS_OSTREAM_HPP_INCLUDED +#define UAVCAN_HELPERS_OSTREAM_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +/** + * Compact replacement for std::ostream for use on embedded systems. + * Can be used for printing UAVCAN messages to stdout. + * + * Relevant discussion: https://groups.google.com/forum/#!topic/px4users/6c1CLNutN90 + * + * Usage: + * OStream::instance() << "Hello world!" << OStream::endl; + */ +class UAVCAN_EXPORT OStream : uavcan::Noncopyable +{ + OStream() { } + +public: + static OStream& instance() + { + static OStream s; + return s; + } + + static OStream& endl(OStream& stream) + { + std::printf("\n"); + return stream; + } +}; + +inline OStream& operator<<(OStream& s, long long x) { std::printf("%lld", x); return s; } +inline OStream& operator<<(OStream& s, unsigned long long x) { std::printf("%llu", x); return s; } + +inline OStream& operator<<(OStream& s, long x) { std::printf("%ld", x); return s; } +inline OStream& operator<<(OStream& s, unsigned long x) { std::printf("%lu", x); return s; } + +inline OStream& operator<<(OStream& s, int x) { std::printf("%d", x); return s; } +inline OStream& operator<<(OStream& s, unsigned int x) { std::printf("%u", x); return s; } + +inline OStream& operator<<(OStream& s, short x) { return operator<<(s, static_cast(x)); } +inline OStream& operator<<(OStream& s, unsigned short x) { return operator<<(s, static_cast(x)); } + +inline OStream& operator<<(OStream& s, long double x) { std::printf("%Lg", x); return s; } +inline OStream& operator<<(OStream& s, double x) { std::printf("%g", x); return s; } +inline OStream& operator<<(OStream& s, float x) { return operator<<(s, static_cast(x)); } + +inline OStream& operator<<(OStream& s, char x) { std::printf("%c", x); return s; } +inline OStream& operator<<(OStream& s, const char* x) { std::printf("%s", x); return s; } + +inline OStream& operator<<(OStream& s, OStream&(*manip)(OStream&)) { return manip(s); } + +} + +#endif // UAVCAN_HELPERS_OSTREAM_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/array.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/array.hpp new file mode 100644 index 000000000000..b409d5d0c182 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/array.hpp @@ -0,0 +1,1270 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_ARRAY_HPP_INCLUDED +#define UAVCAN_MARSHAL_ARRAY_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif + +#ifndef UAVCAN_EXCEPTIONS +# error UAVCAN_EXCEPTIONS +#endif + +#if UAVCAN_EXCEPTIONS +# include +#endif + +namespace uavcan +{ + +enum ArrayMode { ArrayModeStatic, ArrayModeDynamic }; + +/** + * Properties of a square matrix; assuming row-major representation. + */ +template +struct SquareMatrixTraits +{ + enum { NumElements = NumElements_ }; + + enum { NumRowsCols = CompileTimeIntSqrt::Result }; + + enum { NumElementsInTriangle = ((1 + NumRowsCols) * NumRowsCols) / 2 }; + + static inline bool isIndexOnDiagonal(unsigned index) { return (index / NumRowsCols) == (index % NumRowsCols); } + + static inline int computeElementIndexAtRowCol(int row, int col) { return row * NumRowsCols + col; } +}; + +/** + * This class can be used to detect properties of square matrices. + * Element iterator is a random access forward constant iterator. + */ +template +class SquareMatrixAnalyzer : public SquareMatrixTraits +{ + typedef SquareMatrixTraits Traits; + + const ElementIterator first_; + +public: + enum PackingMode + { + PackingModeEmpty, + PackingModeScalar, + PackingModeDiagonal, + PackingModeSymmetric, + PackingModeFull + }; + + SquareMatrixAnalyzer(ElementIterator first_element_iterator) + : first_(first_element_iterator) + { + StaticAssert<(NumElements > 0)>::check(); + } + + ElementIterator accessElementAtRowCol(int row, int col) const + { + return first_ + Traits::computeElementIndexAtRowCol(row, col); + } + + bool areAllElementsNan() const + { + unsigned index = 0; + for (ElementIterator it = first_; index < NumElements; ++it, ++index) + { + if (!isNaN(*it)) + { + return false; + } + } + return true; + } + + bool isScalar() const + { + unsigned index = 0; + for (ElementIterator it = first_; index < NumElements; ++it, ++index) + { + if (!Traits::isIndexOnDiagonal(index) && !isCloseToZero(*it)) + { + return false; + } + if (Traits::isIndexOnDiagonal(index) && !areClose(*it, *first_)) + { + return false; + } + } + return true; + } + + bool isDiagonal() const + { + unsigned index = 0; + for (ElementIterator it = first_; index < NumElements; ++it, ++index) + { + if (!Traits::isIndexOnDiagonal(index) && !isCloseToZero(*it)) + { + return false; + } + } + return true; + } + + bool isSymmetric() const + { + for (int i = 0; i < Traits::NumRowsCols; ++i) + { + for (int k = 0; k < Traits::NumRowsCols; ++k) + { + // On diagonal comparison is pointless + if ((i != k) && + !areClose(*accessElementAtRowCol(i, k), + *accessElementAtRowCol(k, i))) + { + return false; + } + } + } + return true; + } + + PackingMode detectOptimalPackingMode() const + { + if (areAllElementsNan()) + { + return PackingModeEmpty; + } + if (isScalar()) + { + return PackingModeScalar; + } + if (isDiagonal()) + { + return PackingModeDiagonal; + } + if (isSymmetric()) + { + return PackingModeSymmetric; + } + return PackingModeFull; + } +}; + + +template +class UAVCAN_EXPORT StaticArrayBase +{ +protected: + typedef IntegerSpec::Result, SignednessUnsigned, CastModeSaturate> RawEncodedSizeType; + +public: + enum { SizeBitLen = 0 }; + + typedef typename StorageType::Result>::Result, + SignednessUnsigned, CastModeSaturate> >::Type SizeType; + + SizeType size() const { return SizeType(Size); } + SizeType capacity() const { return SizeType(Size); } + +protected: + StaticArrayBase() { } + ~StaticArrayBase() { } + + SizeType validateRange(SizeType pos) const + { + if (pos < SizeType(Size)) + { + return pos; + } +#if UAVCAN_EXCEPTIONS + throw std::out_of_range("uavcan::Array"); +#else + UAVCAN_ASSERT(0); + return SizeType(Size - 1U); // Ha ha +#endif + } +}; + + +template +class UAVCAN_EXPORT DynamicArrayBase +{ +protected: + typedef IntegerSpec::Result, SignednessUnsigned, CastModeSaturate> RawEncodedSizeType; +public: + typedef typename StorageType::Result>::Result, + SignednessUnsigned, CastModeSaturate> >::Type SizeType; + +private: + SizeType size_; + +protected: + DynamicArrayBase() : size_(0) { } + ~DynamicArrayBase() { } + + SizeType validateRange(SizeType pos) const + { + if (pos < size_) + { + return pos; + } +#if UAVCAN_EXCEPTIONS + throw std::out_of_range("uavcan::Array"); +#else + UAVCAN_ASSERT(0); + return SizeType((size_ == 0U) ? 0U : (size_ - 1U)); +#endif + } + + void grow() + { + if (size_ >= MaxSize) + { + (void)validateRange(MaxSize); // Will throw, UAVCAN_ASSERT() or do nothing + } + else + { + size_++; + } + } + + void shrink() + { + if (size_ > 0) + { + size_--; + } + } + +public: + enum { SizeBitLen = RawEncodedSizeType::BitLen }; + + SizeType size() const + { + UAVCAN_ASSERT(size_ ? ((size_ - 1u) <= (MaxSize - 1u)) : 1); // -Werror=type-limits + return size_; + } + + SizeType capacity() const { return MaxSize; } + + void clear() { size_ = 0; } +}; + +/** + * Common functionality for both static and dynamic arrays. + * Static arrays are of fixed size; methods that can alter the size (e.g. push_back() and such) will fail to compile. + * Dynamic arrays contain a fixed-size buffer (it's size is enough to fit maximum number of elements) plus the + * currently allocated number of elements. + */ +template +class UAVCAN_EXPORT ArrayImpl : public Select, StaticArrayBase >::Result +{ + typedef ArrayImpl SelfType; + typedef typename Select, StaticArrayBase >::Result Base; + +public: + enum + { + /// True if the array contents can be interpreted as a 8-bit string (ASCII or UTF8). + IsStringLike = IsIntegerSpec::Result && (T::MaxBitLen == 8 || T::MaxBitLen == 7) && + (ArrayMode == ArrayModeDynamic) + }; + +private: + typedef typename StorageType::Type BufferType[MaxSize + (IsStringLike ? 1 : 0)]; + BufferType data_; + + template + typename EnableIf= U())>::Type initialize(int) + { + if (ArrayMode != ArrayModeDynamic) + { + ::uavcan::fill(data_, data_ + MaxSize, U()); + } + } + template void initialize(...) { } + +protected: + ~ArrayImpl() { } + +public: + typedef typename StorageType::Type ValueType; + typedef typename Base::SizeType SizeType; + + using Base::size; + using Base::capacity; + + ArrayImpl() { initialize(0); } + + /** + * Returns zero-terminated string, same as std::string::c_str(). + * This method will compile only if the array can be interpreted as 8-bit string (ASCII of UTF8). + */ + const char* c_str() const + { + StaticAssert::check(); + UAVCAN_ASSERT(size() < (MaxSize + 1)); + const_cast(data_)[size()] = 0; // Ad-hoc string termination + return reinterpret_cast(data_); + } + + /** + * Range-checking subscript. + * If the index is out of range: + * - if exceptions are enabled, std::out_of_range will be thrown. + * - if exceptions are disabled and UAVCAN_ASSERT() is enabled, execution will be aborted. + * - if exceptions are disabled and UAVCAN_ASSERT() is disabled, index will be constrained to + * the closest valid value. + */ + ValueType& at(SizeType pos) { return data_[Base::validateRange(pos)]; } + const ValueType& at(SizeType pos) const { return data_[Base::validateRange(pos)]; } + + /** + * Range-checking subscript. @ref at() + */ + ValueType& operator[](SizeType pos) { return at(pos); } + const ValueType& operator[](SizeType pos) const { return at(pos); } + + /** + * Standard container methods. Applicable to both dynamic and static arrays. + */ + ValueType* begin() { return data_; } + const ValueType* begin() const { return data_; } + ValueType* end() { return data_ + Base::size(); } + const ValueType* end() const { return data_ + Base::size(); } + ValueType& front() { return at(0U); } + const ValueType& front() const { return at(0U); } + ValueType& back() { return at((Base::size() == 0U) ? 0U : SizeType(Base::size() - 1U)); } + const ValueType& back() const { return at((Base::size() == 0U) ? 0U : SizeType(Base::size() - 1U)); } + + /** + * Performs standard lexicographical compare of the elements. + */ + template + bool operator<(const R& rhs) const + { + return ::uavcan::lexicographical_compare(begin(), end(), rhs.begin(), rhs.end()); + } + + /** + * Aliases for compatibility with standard containers. + */ + typedef ValueType* iterator; + typedef const ValueType* const_iterator; +}; + +/** + * Memory-efficient specialization for bit arrays (each element maps to a single bit rather than single byte). + * This should be compatible with std::bitset. + */ +template +class UAVCAN_EXPORT ArrayImpl, ArrayMode, MaxSize> + : public BitSet + , public Select, StaticArrayBase >::Result +{ + typedef typename Select, StaticArrayBase >::Result ArrayBase; + +public: + enum { IsStringLike = 0 }; + + typedef typename BitSet::Reference Reference; + typedef typename ArrayBase::SizeType SizeType; + + using ArrayBase::size; + using ArrayBase::capacity; + + /** + * Range-checking subscript. Throws if enabled; UAVCAN_ASSERT() if enabled; else constraints the position. + */ + Reference at(SizeType pos) { return BitSet::operator[](ArrayBase::validateRange(pos)); } + bool at(SizeType pos) const { return BitSet::operator[](ArrayBase::validateRange(pos)); } + + /** + * @ref at() + */ + Reference operator[](SizeType pos) { return at(pos); } + bool operator[](SizeType pos) const { return at(pos); } +}; + +/** + * Zero length arrays are not allowed + */ +template class ArrayImpl; + +/** + * Generic array implementation. + * This class is compatible with most standard library functions operating on containers (e.g. std::sort(), + * std::lexicographical_compare(), etc.). + * No dynamic memory is used. + * All functions that can modify the array or access elements are range checking. If the range error occurs: + * - if exceptions are enabled, std::out_of_range will be thrown; + * - if UAVCAN_ASSERT() is enabled, program will be terminated on UAVCAN_ASSERT(0); + * - otherwise the index value will be constrained to the closest valid value. + */ +template +class UAVCAN_EXPORT Array : public ArrayImpl +{ + typedef ArrayImpl Base; + typedef Array SelfType; + + static bool isOptimizedTailArray(TailArrayOptimizationMode tao_mode) + { + return (T::MinBitLen >= 8) && (tao_mode == TailArrayOptEnabled); + } + + int encodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, FalseType) const /// Static + { + UAVCAN_ASSERT(size() > 0); + for (SizeType i = 0; i < size(); i++) + { + const bool last_item = i == (size() - 1); + const int res = RawValueType::encode(Base::at(i), codec, last_item ? tao_mode : TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + } + return 1; + } + + int encodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, TrueType) const /// Dynamic + { + StaticAssert::check(); + const bool self_tao_enabled = isOptimizedTailArray(tao_mode); + if (!self_tao_enabled) + { + const int res_sz = + Base::RawEncodedSizeType::encode(typename StorageType::Type(size()), + codec, TailArrayOptDisabled); + if (res_sz <= 0) + { + return res_sz; + } + } + if (size() == 0) + { + return 1; + } + return encodeImpl(codec, self_tao_enabled ? TailArrayOptDisabled : tao_mode, FalseType()); + } + + int decodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, FalseType) /// Static + { + UAVCAN_ASSERT(size() > 0); + for (SizeType i = 0; i < size(); i++) + { + const bool last_item = i == (size() - 1); + ValueType value = ValueType(); // TODO: avoid extra copy + const int res = RawValueType::decode(value, codec, last_item ? tao_mode : TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + Base::at(i) = value; + } + return 1; + } + +#if __GNUC__ +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wtype-limits" +#endif + int decodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, TrueType) /// Dynamic + { + StaticAssert::check(); + Base::clear(); + if (isOptimizedTailArray(tao_mode)) + { + while (true) + { + ValueType value = ValueType(); + const int res = RawValueType::decode(value, codec, TailArrayOptDisabled); + if (res < 0) + { + return res; + } + if (res == 0) // Success: End of stream reached (even if zero items were read) + { + return 1; + } + if (size() == MaxSize_) // Error: Max array length reached, but the end of stream is not + { + return -ErrInvalidMarshalData; + } + push_back(value); + } + } + else + { + typename StorageType::Type sz = 0; + const int res_sz = Base::RawEncodedSizeType::decode(sz, codec, TailArrayOptDisabled); + if (res_sz <= 0) + { + return res_sz; + } + // coverity[result_independent_of_operands] + if (static_cast(sz) > MaxSize_) // False 'type-limits' warning occurs here + { + return -ErrInvalidMarshalData; + } + resize(sz); + if (sz == 0) + { + return 1; + } + return decodeImpl(codec, tao_mode, FalseType()); + } + UAVCAN_ASSERT(0); // Unreachable + return -ErrLogic; + } +#if __GNUC__ +# pragma GCC diagnostic pop +#endif + + template + void packSquareMatrixImpl(const InputIter src_row_major) + { + StaticAssert::check(); + + this->clear(); + + typedef SquareMatrixAnalyzer Analyzer; + const Analyzer analyzer(src_row_major); + + switch (analyzer.detectOptimalPackingMode()) + { + case Analyzer::PackingModeEmpty: + { + break; // Nothing to insert + } + case Analyzer::PackingModeScalar: + { + this->push_back(ValueType(*src_row_major)); + break; + } + case Analyzer::PackingModeDiagonal: + { + for (int i = 0; i < Analyzer::NumRowsCols; i++) + { + this->push_back(ValueType(*analyzer.accessElementAtRowCol(i, i))); + } + break; + } + case Analyzer::PackingModeSymmetric: + { + for (int row = 0; row < Analyzer::NumRowsCols; row++) + { + for (int col = row; col < Analyzer::NumRowsCols; col++) + { + this->push_back(ValueType(*analyzer.accessElementAtRowCol(row, col))); + } + } + UAVCAN_ASSERT(this->size() == Analyzer::NumElementsInTriangle); + break; + } + case Analyzer::PackingModeFull: + { + InputIter it = src_row_major; + for (unsigned index = 0; index < MaxSize; index++, it++) + { + this->push_back(ValueType(*it)); + } + break; + } + default: + { + UAVCAN_ASSERT(0); + break; + } + } + } + + template + void unpackSquareMatrixImpl(const OutputIter dst_row_major) const + { + StaticAssert::check(); + typedef SquareMatrixTraits Traits; + + if (this->size() == Traits::NumRowsCols || this->size() == 1) // Scalar or diagonal + { + OutputIter it = dst_row_major; + for (unsigned index = 0; index < MaxSize; index++) + { + if (Traits::isIndexOnDiagonal(index)) + { + const SizeType source_index = SizeType((this->size() == 1) ? 0 : (index / Traits::NumRowsCols)); + *it++ = ScalarType(this->at(source_index)); + } + else + { + *it++ = ScalarType(0); + } + } + } + else if (this->size() == Traits::NumElementsInTriangle) // Symmetric + { + OutputIter it = dst_row_major; + SizeType source_index = 0; + for (int row = 0; row < Traits::NumRowsCols; row++) + { + for (int col = 0; col < Traits::NumRowsCols; col++) + { + if (col >= row) // Diagonal or upper-right triangle + { + *it++ = ScalarType(this->at(source_index)); + source_index++; + } + else // Lower-left triangle + { + // Transposing one element, argument swapping is intentional + // coverity[swapped_arguments] + *it++ = *(dst_row_major + Traits::computeElementIndexAtRowCol(col, row)); + } + } + } + UAVCAN_ASSERT(source_index == Traits::NumElementsInTriangle); + } + else if (this->size() == MaxSize) // Full - no packing whatsoever + { + OutputIter it = dst_row_major; + for (SizeType index = 0; index < MaxSize; index++) + { + *it++ = ScalarType(this->at(index)); + } + } + else // Everything else + { + // coverity[suspicious_sizeof : FALSE] + ::uavcan::fill_n(dst_row_major, MaxSize, ScalarType(0)); + } + } + +public: + typedef T RawValueType; ///< This may be not the same as the element type. + typedef typename StorageType::Type ValueType; ///< This is the actual stored element type. + typedef typename Base::SizeType SizeType; ///< Minimal width size type. + + using Base::size; + using Base::capacity; + + enum { IsDynamic = ArrayMode == ArrayModeDynamic }; + enum { MaxSize = MaxSize_ }; + enum + { + MinBitLen = (IsDynamic == 0) + ? (static_cast(RawValueType::MinBitLen) * static_cast(MaxSize)) + : 0 + }; + enum + { + MaxBitLen = static_cast(Base::SizeBitLen) + + static_cast(RawValueType::MaxBitLen) * static_cast(MaxSize) + }; + + /** + * Default constructor zero-initializes the storage even if it consists of primitive types. + */ + Array() { } + + /** + * String constructor - only for string-like arrays. + * Refer to @ref operator+=(const char*) for details. + */ + Array(const char* str) // Implicit + { + operator+=(str); + } + + static int encode(const SelfType& array, ScalarCodec& codec, const TailArrayOptimizationMode tao_mode) + { + return array.encodeImpl(codec, tao_mode, BooleanType()); + } + + static int decode(SelfType& array, ScalarCodec& codec, const TailArrayOptimizationMode tao_mode) + { + return array.decodeImpl(codec, tao_mode, BooleanType()); + } + + static void extendDataTypeSignature(DataTypeSignature& signature) + { + RawValueType::extendDataTypeSignature(signature); + } + + bool empty() const { return size() == 0; } + + /** + * Only for dynamic arrays. Range checking. + */ + void pop_back() { Base::shrink(); } + void push_back(const ValueType& value) + { + Base::grow(); + Base::at(SizeType(size() - 1)) = value; + } + + /** + * Only for dynamic arrays. Range checking. + */ + void resize(SizeType new_size, const ValueType& filler) + { + if (new_size > size()) + { + SizeType cnt = SizeType(new_size - size()); + while (cnt-- > 0) + { + push_back(filler); + } + } + else if (new_size < size()) + { + SizeType cnt = SizeType(size() - new_size); + while (cnt-- > 0) + { + pop_back(); + } + } + else + { + ; // Exact size + } + } + + /** + * Only for dynamic arrays. Range checking. + */ + void resize(SizeType new_size) + { + resize(new_size, ValueType()); + } + + /** + * This operator accepts any container with size() and []. + * Members must be comparable via operator ==. + */ + template + typename EnableIf(0))->size()) && + sizeof((*(reinterpret_cast(0)))[0]), bool>::Type + operator==(const R& rhs) const + { + if (size() != rhs.size()) + { + return false; + } + for (SizeType i = 0; i < size(); i++) // Bitset does not have iterators + { + if (!(Base::at(i) == rhs[i])) + { + return false; + } + } + return true; + } + + /** + * This method compares two arrays using @ref areClose(), which ensures proper comparison of + * floating point values, or DSDL data structures which contain floating point fields at any depth. + * Please refer to the documentation of @ref areClose() to learn more about how it works and how to + * define custom fuzzy comparison behavior. + * Any container with size() and [] is acceptable. + */ + template + typename EnableIf(0))->size()) && + sizeof((*(reinterpret_cast(0)))[0]), bool>::Type + isClose(const R& rhs) const + { + if (size() != rhs.size()) + { + return false; + } + for (SizeType i = 0; i < size(); i++) // Bitset does not have iterators + { + if (!areClose(Base::at(i), rhs[i])) + { + return false; + } + } + return true; + } + + /** + * This operator can only be used with string-like arrays; otherwise it will fail to compile. + * @ref c_str() + */ + bool operator==(const char* chr) const + { + if (chr == UAVCAN_NULLPTR) + { + return false; + } + return std::strncmp(Base::c_str(), chr, MaxSize) == 0; + } + + /** + * @ref operator==() + */ + template bool operator!=(const R& rhs) const { return !operator==(rhs); } + + /** + * This operator can only be used with string-like arrays; otherwise it will fail to compile. + * @ref c_str() + */ + SelfType& operator=(const char* chr) + { + StaticAssert::check(); + StaticAssert::check(); + Base::clear(); + if (chr == UAVCAN_NULLPTR) + { + handleFatalError("Array::operator=(const char*)"); + } + while (*chr) + { + push_back(ValueType(*chr++)); // Value type is likely to be unsigned char, so conversion may be required. + } + return *this; + } + + /** + * This operator can only be used with string-like arrays; otherwise it will fail to compile. + * @ref c_str() + */ + SelfType& operator+=(const char* chr) + { + StaticAssert::check(); + StaticAssert::check(); + if (chr == UAVCAN_NULLPTR) + { + handleFatalError("Array::operator+=(const char*)"); + } + while (*chr) + { + push_back(ValueType(*chr++)); + } + return *this; + } + + /** + * Appends another Array<> with the same element type. Mode and max size can be different. + */ + template + SelfType& operator+=(const Array& rhs) + { + typedef Array Rhs; + StaticAssert::check(); + for (typename Rhs::SizeType i = 0; i < rhs.size(); i++) + { + push_back(rhs[i]); + } + return *this; + } + + /** + * Formatting appender. + * This method doesn't raise an overflow error; instead it silently truncates the data to fit the array capacity. + * Works only with string-like arrays, otherwise fails to compile. + * @param format Format string for std::snprintf(), e.g. "%08x", "%f" + * @param value Arbitrary value of a primitive type (should fail to compile if there's a non-primitive type) + */ + template + void appendFormatted(const char* const format, const A value) + { + StaticAssert::check(); + StaticAssert::check(); + + StaticAssert= A(0))>::check(); // This check allows to weed out most compound types + StaticAssert<(sizeof(A) <= sizeof(long double)) || + (sizeof(A) <= sizeof(long long))>::check(); // Another stupid check to catch non-primitive types + + if (!format) + { + UAVCAN_ASSERT(0); + return; + } + // Add some hardcore runtime checks for the format string correctness? + + ValueType* const ptr = Base::end(); + UAVCAN_ASSERT(capacity() >= size()); + const SizeType max_size = SizeType(capacity() - size()); + + // We have one extra byte for the null terminator, hence +1 + const int ret = snprintf(reinterpret_cast(ptr), SizeType(max_size + 1U), format, value); + + for (int i = 0; i < min(ret, int(max_size)); i++) + { + Base::grow(); + } + if (ret < 0) + { + UAVCAN_ASSERT(0); // Likely an invalid format string + (*this) += format; // So we print it as is in release builds + } + } + + /** + * Converts the string to upper/lower case in place, assuming that encoding is ASCII. + * These methods can only be used with string-like arrays; otherwise compilation will fail. + */ + void convertToUpperCaseASCII() + { + StaticAssert::check(); + + for (SizeType i = 0; i < size(); i++) + { + const int x = Base::at(i); + if ((x <= 'z') && (x >= 'a')) + { + Base::at(i) = static_cast(x + ('Z' - 'z')); + } + } + } + + void convertToLowerCaseASCII() + { + StaticAssert::check(); + + for (SizeType i = 0; i < size(); i++) + { + const int x = Base::at(i); + if ((x <= 'Z') && (x >= 'A')) + { + Base::at(i) = static_cast(x - ('Z' - 'z')); + } + } + } + + /** + * Fills this array as a packed square matrix from a static array. + * Please refer to the specification to learn more about matrix packing. + * Note that matrix packing code uses @ref areClose() for comparison. + */ + template + void packSquareMatrix(const ScalarType (&src_row_major)[MaxSize]) + { + packSquareMatrixImpl(src_row_major); + } + + /** + * Fills this array as a packed square matrix in place. + * Please refer to the specification to learn more about matrix packing. + * Note that matrix packing code uses @ref areClose() for comparison. + */ + void packSquareMatrix() + { + if (this->size() == MaxSize) + { + ValueType matrix[MaxSize]; + for (SizeType i = 0; i < MaxSize; i++) + { + matrix[i] = this->at(i); + } + packSquareMatrix(matrix); + } + else if (this->size() == 0) + { + ; // Nothing to do - leave the matrix empty + } + else + { +#if UAVCAN_EXCEPTIONS + throw std::out_of_range("uavcan::Array::packSquareMatrix()"); +#else + UAVCAN_ASSERT(0); + this->clear(); +#endif + } + + } + + /** + * Fills this array as a packed square matrix from any container that has the following public entities: + * - method begin() + * - method size() + * - only for C++03: type value_type + * Please refer to the specification to learn more about matrix packing. + * Note that matrix packing code uses @ref areClose() for comparison. + */ + template + typename EnableIf(0))->begin()) && + sizeof((reinterpret_cast(0))->size())>::Type + packSquareMatrix(const R& src_row_major) + { + if (src_row_major.size() == MaxSize) + { + packSquareMatrixImpl(src_row_major.begin()); + } + else if (src_row_major.size() == 0) + { + this->clear(); + } + else + { +#if UAVCAN_EXCEPTIONS + throw std::out_of_range("uavcan::Array::packSquareMatrix()"); +#else + UAVCAN_ASSERT(0); + this->clear(); +#endif + } + } + + /** + * Reconstructs full matrix, result will be saved into a static array. + * Please refer to the specification to learn more about matrix packing. + */ + template + void unpackSquareMatrix(ScalarType (&dst_row_major)[MaxSize]) const + { + unpackSquareMatrixImpl(dst_row_major); + } + + /** + * Reconstructs full matrix in place. + * Please refer to the specification to learn more about matrix packing. + */ + void unpackSquareMatrix() + { + ValueType matrix[MaxSize]; + unpackSquareMatrix(matrix); + + this->clear(); + for (unsigned i = 0; i < MaxSize; i++) + { + this->push_back(matrix[i]); + } + } + + /** + * Reconstructs full matrix, result will be saved into container that has the following public entities: + * - method begin() + * - method size() + * - only for C++03: type value_type + * Please refer to the specification to learn more about matrix packing. + */ + template + typename EnableIf(0))->begin()) && + sizeof((reinterpret_cast(0))->size())>::Type + unpackSquareMatrix(R& dst_row_major) const + { + if (dst_row_major.size() == MaxSize) + { +#if UAVCAN_CPP_VERSION > UAVCAN_CPP03 + typedef typename RemoveReference::Type RhsValueType; + unpackSquareMatrixImpl(dst_row_major.begin()); +#else + unpackSquareMatrixImpl(dst_row_major.begin()); +#endif + } + else + { +#if UAVCAN_EXCEPTIONS + throw std::out_of_range("uavcan::Array::unpackSquareMatrix()"); +#else + UAVCAN_ASSERT(0); +#endif + } + } + + /** + * Aliases for compatibility with standard containers. + */ + typedef ValueType value_type; + typedef SizeType size_type; +}; + +/** + * These operators will only be enabled if rhs and lhs are different types. This precondition allows to work-around + * the ambiguity arising from the scope containing two definitions: one here and the others in Array<>. + * Refer to https://github.com/UAVCAN/libuavcan/issues/55 for more info. + */ +template +UAVCAN_EXPORT +inline typename EnableIf >::Result, bool>::Type +operator==(const R& rhs, const Array& lhs) +{ + return lhs.operator==(rhs); +} + +template +UAVCAN_EXPORT +inline typename EnableIf >::Result, bool>::Type +operator!=(const R& rhs, const Array& lhs) +{ + return lhs.operator!=(rhs); +} + +/** + * Shortcut for string-like array type instantiation. + * The proper way of doing this is actually "template<> using ... = ...", but this feature is not available in + * older C++ revisions which the library has to support. + */ +template +class MakeString +{ + MakeString(); // This class is not instantiatable. +public: + typedef Array, ArrayModeDynamic, MaxSize> Type; +}; + +/** + * YAML streamer specification for any Array<> + */ +template +class UAVCAN_EXPORT YamlStreamer > +{ + typedef Array ArrayType; + + static bool isNiceCharacter(int c) + { + if (c >= 32 && c <= 126) + { + return true; + } + static const char Good[] = {'\n', '\r', '\t'}; + for (unsigned i = 0; i < sizeof(Good) / sizeof(Good[0]); i++) + { + if (Good[i] == c) + { + return true; + } + } + return false; + } + + template + static void streamPrimitives(Stream& s, const ArrayType& array) + { + s << '['; + for (typename ArrayType::SizeType i = 0; i < array.size(); i++) + { + YamlStreamer::stream(s, array.at(i), 0); + if ((i + 1) < array.size()) + { + s << ", "; + } + } + s << ']'; + } + + template + static void streamCharacters(Stream& s, const ArrayType& array) + { + s << '"'; + for (typename ArrayType::SizeType i = 0; i < array.size(); i++) + { + const int c = array.at(i); + if (c < 32 || c > 126) + { + char nibbles[2] = {char((c >> 4) & 0xF), char(c & 0xF)}; + for (int k = 0; k < 2; k++) + { + nibbles[k] = char(nibbles[k] + '0'); + if (nibbles[k] > '9') + { + nibbles[k] = char(nibbles[k] + 'A' - '9' - 1); + } + } + s << "\\x" << nibbles[0] << nibbles[1]; + } + else + { + if (c == '"' || c == '\\') // YAML requires to escape these two + { + s << '\\'; + } + s << char(c); + } + } + s << '"'; + } + + struct SelectorStringLike { }; + struct SelectorPrimitives { }; + struct SelectorObjects { }; + + template + static void genericStreamImpl(Stream& s, const ArrayType& array, int, SelectorStringLike) + { + bool printable_only = true; + for (typename ArrayType::SizeType i = 0; i < array.size(); i++) + { + if (!isNiceCharacter(array[i])) + { + printable_only = false; + break; + } + } + if (printable_only) + { + streamCharacters(s, array); + } + else + { + streamPrimitives(s, array); + s << " # "; + streamCharacters(s, array); + } + } + + template + static void genericStreamImpl(Stream& s, const ArrayType& array, int, SelectorPrimitives) + { + streamPrimitives(s, array); + } + + template + static void genericStreamImpl(Stream& s, const ArrayType& array, int level, SelectorObjects) + { + if (array.empty()) + { + s << "[]"; + return; + } + for (typename ArrayType::SizeType i = 0; i < array.size(); i++) + { + s << '\n'; + for (int pos = 0; pos < level; pos++) + { + s << " "; + } + s << "- "; + YamlStreamer::stream(s, array.at(i), level + 1); + } + } + +public: + /** + * Prints Array<> into the stream in YAML format. + */ + template + static void stream(Stream& s, const ArrayType& array, int level) + { + typedef typename Select::Result, + SelectorPrimitives, + SelectorObjects>::Result >::Result Type; + genericStreamImpl(s, array, level, Type()); + } +}; + +} + +#endif // UAVCAN_MARSHAL_ARRAY_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/bit_stream.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/bit_stream.hpp new file mode 100644 index 000000000000..edbe1a522d37 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/bit_stream.hpp @@ -0,0 +1,93 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_BIT_STREAM_HPP_INCLUDED +#define UAVCAN_MARSHAL_BIT_STREAM_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This function implements fast copy of unaligned bit arrays. It isn't part of the library API, so it is not exported. + * @param src_org Source array + * @param src_offset Bit offset of the first source byte + * @param src_len Number of bits to copy + * @param dst_org Destination array + * @param dst_offset Bit offset of the first destination byte + */ +void bitarrayCopy(const unsigned char* src_org, std::size_t src_offset, std::size_t src_len, + unsigned char* dst_org, std::size_t dst_offset); + +/** + * This class treats a chunk of memory as an array of bits. + * It is used by the bit codec for serialization/deserialization. + */ +class UAVCAN_EXPORT BitStream +{ + static const unsigned MaxBytesPerRW = 16; + + ITransferBuffer& buf_; + unsigned bit_offset_; + uint8_t byte_cache_; + + static inline unsigned bitlenToBytelen(unsigned bits) { return (bits + 7) / 8; } + + static inline void copyBitArrayAlignedToUnaligned(const uint8_t* src_org, unsigned src_len, + uint8_t* dst_org, unsigned dst_offset) + { + bitarrayCopy(reinterpret_cast(src_org), 0, src_len, + reinterpret_cast(dst_org), dst_offset); + } + + static inline void copyBitArrayUnalignedToAligned(const uint8_t* src_org, unsigned src_offset, unsigned src_len, + uint8_t* dst_org) + { + bitarrayCopy(reinterpret_cast(src_org), src_offset, src_len, + reinterpret_cast(dst_org), 0); + } + +public: + static const unsigned MaxBitsPerRW = MaxBytesPerRW * 8; + + enum + { + ResultOutOfBuffer = 0, + ResultOk = 1 + }; + + explicit BitStream(ITransferBuffer& buf) + : buf_(buf) + , bit_offset_(0) + , byte_cache_(0) + { + StaticAssert::check(); + } + + /** + * Write/read calls interpret bytes as bit arrays, 8 bits per byte, where the most + * significant bits have lower index, i.e.: + * Hex: 55 2d + * Bits: 01010101 00101101 + * Indices: 0 .. 7 8 .. 15 + * Return values: + * Negative - Error + * Zero - Out of buffer space + * Positive - OK + */ + int write(const uint8_t* bytes, const unsigned bitlen); + int read(uint8_t* bytes, const unsigned bitlen); + +#if UAVCAN_TOSTRING + std::string toString() const; +#endif +}; + +} + +#endif // UAVCAN_MARSHAL_BIT_STREAM_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/char_array_formatter.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/char_array_formatter.hpp new file mode 100644 index 000000000000..244c3b1204d7 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/char_array_formatter.hpp @@ -0,0 +1,142 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED +#define UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED + +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +template +class UAVCAN_EXPORT CharArrayFormatter +{ + ArrayType_& array_; + + template + typename std::enable_if::value>::type + writeValue(T value) + { + array_.template appendFormatted("%g", double(value)); + } + + template + typename std::enable_if::value>::type + writeValue(T value) + { + if (std::is_same()) + { + if (array_.size() != array_.capacity()) + { + array_.push_back(typename ArrayType_::ValueType(value)); + } + } + else if (std::is_signed()) + { + array_.template appendFormatted("%lli", static_cast(value)); + } + else + { + array_.template appendFormatted("%llu", static_cast(value)); + } + } + + template + typename std::enable_if::value && !std::is_same::value>::type + writeValue(T value) + { + array_.template appendFormatted("%p", static_cast(value)); + } + + void writeValue(const char* value) + { + array_.template appendFormatted("%s", value); + } + +public: + typedef ArrayType_ ArrayType; + + explicit CharArrayFormatter(ArrayType& array) + : array_(array) + { } + + ArrayType& getArray() { return array_; } + const ArrayType& getArray() const { return array_; } + + void write(const char* text) + { + writeValue(text); + } + + template + void write(const char* s, T value, Args... args) + { + while (s && *s) + { + if (*s == '%') + { + s += 1; + if (*s != '%') + { + writeValue(value); + write(++s, args...); + break; + } + } + writeValue(*s++); + } + } +}; + +#else + +template +class UAVCAN_EXPORT CharArrayFormatter +{ + ArrayType_& array_; + +public: + typedef ArrayType_ ArrayType; + + CharArrayFormatter(ArrayType& array) + : array_(array) + { } + + ArrayType& getArray() { return array_; } + const ArrayType& getArray() const { return array_; } + + void write(const char* const text) + { + array_.template appendFormatted("%s", text); + } + + /** + * This version does not support more than one formatted argument, though it can be improved. + * And it is unsafe. + * There is typesafe version for C++11 above. + */ + template + void write(const char* const format, const A value) + { + array_.template appendFormatted(format, value); + } +}; + +#endif + +} + +#endif // UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/float_spec.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/float_spec.hpp new file mode 100644 index 000000000000..ef3e93b88444 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/float_spec.hpp @@ -0,0 +1,238 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_FLOAT_SPEC_HPP_INCLUDED +#define UAVCAN_MARSHAL_FLOAT_SPEC_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include // Assuming that in C++11 mode all standard headers are available +#endif + +namespace uavcan +{ + +template +struct NativeFloatSelector +{ + struct ErrorNoSuchFloat; + typedef typename Select<(sizeof(float) * 8 >= BitLen), float, + typename Select<(sizeof(double) * 8 >= BitLen), double, + typename Select<(sizeof(long double) * 8 >= BitLen), long double, + ErrorNoSuchFloat>::Result>::Result>::Result Type; +}; + + +class UAVCAN_EXPORT IEEE754Converter +{ + // TODO: Non-IEEE float support + + static uint16_t nativeIeeeToHalf(float value); + static float halfToNativeIeee(uint16_t value); + + IEEE754Converter(); + + template + static void enforceIeee() + { + /* + * Some compilers may have is_iec559 to be defined false despite the fact that IEEE754 is supported. + * An acceptable workaround would be to put an #ifdef here. + */ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + StaticAssert::Type>::is_iec559>::check(); +#endif + } + +public: +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + /// UAVCAN requires rounding to nearest for all float conversions + static std::float_round_style roundstyle() { return std::round_to_nearest; } +#endif + + template + static typename IntegerSpec::StorageType + toIeee(typename NativeFloatSelector::Type value) + { + enforceIeee(); + union + { + typename IntegerSpec::StorageType i; + typename NativeFloatSelector::Type f; + } u; + StaticAssert::check(); + u.f = value; + return u.i; + } + + template + static typename NativeFloatSelector::Type + toNative(typename IntegerSpec::StorageType value) + { + enforceIeee(); + union + { + typename IntegerSpec::StorageType i; + typename NativeFloatSelector::Type f; + } u; + StaticAssert::check(); + u.i = value; + return u.f; + } +}; +template <> +inline typename IntegerSpec<16, SignednessUnsigned, CastModeTruncate>::StorageType +IEEE754Converter::toIeee<16>(typename NativeFloatSelector<16>::Type value) +{ + return nativeIeeeToHalf(value); +} +template <> +inline typename NativeFloatSelector<16>::Type +IEEE754Converter::toNative<16>(typename IntegerSpec<16, SignednessUnsigned, CastModeTruncate>::StorageType value) +{ + return halfToNativeIeee(value); +} + + +template struct IEEE754Limits; +template <> struct IEEE754Limits<16> +{ + typedef typename NativeFloatSelector<16>::Type NativeType; + static NativeType max() { return static_cast(65504.0); } + static NativeType epsilon() { return static_cast(9.77e-04); } +}; +template <> struct IEEE754Limits<32> +{ + typedef typename NativeFloatSelector<32>::Type NativeType; + static NativeType max() { return static_cast(3.40282346638528859812e+38); } + static NativeType epsilon() { return static_cast(1.19209289550781250000e-7); } +}; +template <> struct IEEE754Limits<64> +{ + typedef typename NativeFloatSelector<64>::Type NativeType; + static NativeType max() { return static_cast(1.79769313486231570815e+308L); } + static NativeType epsilon() { return static_cast(2.22044604925031308085e-16L); } +}; + + +template +class UAVCAN_EXPORT FloatSpec : public IEEE754Limits +{ + FloatSpec(); + +public: + enum { BitLen = BitLen_ }; + enum { MinBitLen = BitLen }; + enum { MaxBitLen = BitLen }; + enum { IsPrimitive = 1 }; + + typedef typename NativeFloatSelector::Type StorageType; + +#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 + enum { IsExactRepresentation = (sizeof(StorageType) * 8 == BitLen) }; +#else + enum { IsExactRepresentation = (sizeof(StorageType) * 8 == BitLen) && std::numeric_limits::is_iec559 }; +#endif + + using IEEE754Limits::max; + using IEEE754Limits::epsilon; +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + static std::float_round_style roundstyle() { return IEEE754Converter::roundstyle(); } +#endif + + static int encode(StorageType value, ScalarCodec& codec, TailArrayOptimizationMode) + { + // cppcheck-suppress duplicateExpression + if (CastMode == CastModeSaturate) + { + saturate(value); + } + else + { + truncate(value); + } + return codec.encode(IEEE754Converter::toIeee(value)); + } + + static int decode(StorageType& out_value, ScalarCodec& codec, TailArrayOptimizationMode) + { + typename IntegerSpec::StorageType ieee = 0; + const int res = codec.decode(ieee); + if (res <= 0) + { + return res; + } + out_value = IEEE754Converter::toNative(ieee); + return res; + } + + static void extendDataTypeSignature(DataTypeSignature&) { } + +private: + static inline void saturate(StorageType& value) + { + if ((IsExactRepresentation == 0) && isFinite(value)) + { + if (value > max()) + { + value = max(); + } + else if (value < -max()) + { + value = -max(); + } + else + { + ; // Valid range + } + } + } + + static inline void truncate(StorageType& value) + { + if ((IsExactRepresentation == 0) && isFinite(value)) + { + if (value > max()) + { + value = NumericTraits::infinity(); + } + else if (value < -max()) + { + value = -NumericTraits::infinity(); + } + else + { + ; // Valid range + } + } + } +}; + + +template +class UAVCAN_EXPORT YamlStreamer > +{ + typedef typename FloatSpec::StorageType StorageType; + +public: + template // cppcheck-suppress passedByValue + static void stream(Stream& s, const StorageType value, int) + { + s << value; + } +}; + +} + +#endif // UAVCAN_MARSHAL_FLOAT_SPEC_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/integer_spec.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/integer_spec.hpp new file mode 100644 index 000000000000..e5a87d7cb6e3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/integer_spec.hpp @@ -0,0 +1,210 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_INTEGER_SPEC_HPP_INCLUDED +#define UAVCAN_MARSHAL_INTEGER_SPEC_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ + +enum Signedness { SignednessUnsigned, SignednessSigned }; + +/** + * This template will be used for signed and unsigned integers more than 1 bit long. + * There are explicit specializations for booleans below. + */ +template +class UAVCAN_EXPORT IntegerSpec +{ + struct ErrorNoSuchInteger; + +public: + enum { IsSigned = Signedness == SignednessSigned }; + enum { BitLen = BitLen_ }; + enum { MinBitLen = BitLen }; + enum { MaxBitLen = BitLen }; + enum { IsPrimitive = 1 }; + + typedef typename Select<(BitLen <= 8), typename Select::Result, + typename Select<(BitLen <= 16), typename Select::Result, + typename Select<(BitLen <= 32), typename Select::Result, + typename Select<(BitLen <= 64), typename Select::Result, + ErrorNoSuchInteger>::Result>::Result>::Result>::Result StorageType; + + typedef typename IntegerSpec::StorageType UnsignedStorageType; + +private: + IntegerSpec(); + + struct LimitsImplGeneric + { + static StorageType max() + { + StaticAssert<(sizeof(uintmax_t) >= 8)>::check(); + if (IsSigned == 0) + { + return StorageType((uintmax_t(1) << static_cast(BitLen)) - 1U); + } + else + { + return StorageType((uintmax_t(1) << (static_cast(BitLen) - 1U)) - 1); + } + } + static UnsignedStorageType mask() + { + StaticAssert<(sizeof(uintmax_t) >= 8U)>::check(); + return UnsignedStorageType((uintmax_t(1) << static_cast(BitLen)) - 1U); + } + }; + + struct LimitsImpl64 + { + static StorageType max() + { + return StorageType((IsSigned == 0) ? 0xFFFFFFFFFFFFFFFFULL : 0x7FFFFFFFFFFFFFFFLL); + } + static UnsignedStorageType mask() { return 0xFFFFFFFFFFFFFFFFULL; } + }; + + typedef typename Select<(BitLen == 64), LimitsImpl64, LimitsImplGeneric>::Result Limits; + + static void saturate(StorageType& value) + { + if (value > max()) + { + value = max(); + } + else if (value <= min()) // 'Less or Equal' allows to suppress compiler warning on unsigned types + { + value = min(); + } + else + { + ; // Valid range + } + } + + static void truncate(StorageType& value) { value = value & StorageType(mask()); } + + static void validate() + { + StaticAssert<(BitLen <= (sizeof(StorageType) * 8))>::check(); + // coverity[result_independent_of_operands : FALSE] + UAVCAN_ASSERT(max() <= NumericTraits::max()); + // coverity[result_independent_of_operands : FALSE] + UAVCAN_ASSERT(min() >= NumericTraits::min()); + } + +public: + static StorageType max() { return Limits::max(); } + static StorageType min() { return IsSigned ? StorageType(-max() - 1) : 0; } + static UnsignedStorageType mask() { return Limits::mask(); } + + static int encode(StorageType value, ScalarCodec& codec, TailArrayOptimizationMode) + { + validate(); + // cppcheck-suppress duplicateExpression + if (CastMode == CastModeSaturate) + { + saturate(value); + } + else + { + truncate(value); + } + return codec.encode(value); + } + + static int decode(StorageType& out_value, ScalarCodec& codec, TailArrayOptimizationMode) + { + validate(); + return codec.decode(out_value); + } + + static void extendDataTypeSignature(DataTypeSignature&) { } +}; + +/** + * Boolean specialization + */ +template +class UAVCAN_EXPORT IntegerSpec<1, SignednessUnsigned, CastMode> +{ +public: + enum { IsSigned = 0 }; + enum { BitLen = 1 }; + enum { MinBitLen = 1 }; + enum { MaxBitLen = 1 }; + enum { IsPrimitive = 1 }; + + typedef bool StorageType; + typedef bool UnsignedStorageType; + +private: + IntegerSpec(); + +public: + static StorageType max() { return true; } + static StorageType min() { return false; } + static UnsignedStorageType mask() { return true; } + + static int encode(StorageType value, ScalarCodec& codec, TailArrayOptimizationMode) + { + return codec.encode(value); + } + + static int decode(StorageType& out_value, ScalarCodec& codec, TailArrayOptimizationMode) + { + return codec.decode(out_value); + } + + static void extendDataTypeSignature(DataTypeSignature&) { } +}; + +template +class IntegerSpec<1, SignednessSigned, CastMode>; // Invalid instantiation + +template +class IntegerSpec<0, Signedness, CastMode>; // Invalid instantiation + + +template +struct IsIntegerSpec +{ + enum { Result = 0 }; +}; + +template +struct IsIntegerSpec > +{ + enum { Result = 1 }; +}; + + +template +class UAVCAN_EXPORT YamlStreamer > +{ + typedef IntegerSpec RawType; + typedef typename RawType::StorageType StorageType; + +public: + template // cppcheck-suppress passedByValue + static void stream(Stream& s, const StorageType value, int) + { + // Get rid of character types - we want its integer representation, not ASCII code + typedef typename Select<(sizeof(StorageType) >= sizeof(int)), StorageType, + typename Select::Result >::Result TempType; + s << TempType(value); + } +}; + +} + +#endif // UAVCAN_MARSHAL_INTEGER_SPEC_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/scalar_codec.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/scalar_codec.hpp new file mode 100644 index 000000000000..2690f6e1d665 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/scalar_codec.hpp @@ -0,0 +1,140 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_SCALAR_CODEC_HPP_INCLUDED +#define UAVCAN_MARSHAL_SCALAR_CODEC_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class implements fast encoding/decoding of primitive type scalars into/from bit arrays. + * It uses the compile-time type information to eliminate run-time operations where possible. + */ +class UAVCAN_EXPORT ScalarCodec +{ + BitStream& stream_; + + static void swapByteOrder(uint8_t* bytes, unsigned len); + + template + static typename EnableIf<(BitLen > 8)>::Type + convertByteOrder(uint8_t (&bytes)[Size]) + { +#if defined(BYTE_ORDER) && defined(BIG_ENDIAN) + static const bool big_endian = BYTE_ORDER == BIG_ENDIAN; +#else + union { long int l; char c[sizeof(long int)]; } u; + u.l = 1; + const bool big_endian = u.c[sizeof(long int) - 1] == 1; +#endif + /* + * I didn't have any big endian machine nearby, so big endian support wasn't tested yet. + * It is likely to be OK anyway, so feel free to remove this UAVCAN_ASSERT() as needed. + */ + UAVCAN_ASSERT(big_endian == false); + if (big_endian) + { + swapByteOrder(bytes, Size); + } + } + + template + static typename EnableIf<(BitLen <= 8)>::Type + convertByteOrder(uint8_t (&)[Size]) { } + + template + static typename EnableIf(NumericTraits::IsSigned) && ((sizeof(T) * 8) > BitLen)>::Type + fixTwosComplement(T& value) + { + StaticAssert::IsInteger>::check(); // Not applicable to floating point types + if (value & (T(1) << (BitLen - 1))) // The most significant bit is set --> negative + { + value |= T(T(0xFFFFFFFFFFFFFFFFULL) & ~((T(1) << BitLen) - 1)); + } + } + + template + static typename EnableIf(NumericTraits::IsSigned) || ((sizeof(T) * 8) == BitLen)>::Type + fixTwosComplement(T&) { } + + template + static typename EnableIf<((sizeof(T) * 8) > BitLen)>::Type + clearExtraBits(T& value) + { + value &= (T(1) << BitLen) - 1; // Signedness doesn't matter + } + + template + static typename EnableIf<((sizeof(T) * 8) == BitLen)>::Type + clearExtraBits(T&) { } + + template + void validate() + { + StaticAssert<((sizeof(T) * 8) >= BitLen)>::check(); + StaticAssert<(BitLen <= BitStream::MaxBitsPerRW)>::check(); + StaticAssert(NumericTraits::IsSigned) ? (BitLen > 1) : true>::check(); + } + + int encodeBytesImpl(uint8_t* bytes, unsigned bitlen); + int decodeBytesImpl(uint8_t* bytes, unsigned bitlen); + +public: + explicit ScalarCodec(BitStream& stream) + : stream_(stream) + { } + + template + int encode(const T value); + + template + int decode(T& value); +}; + +// ---------------------------------------------------------------------------- + +template +int ScalarCodec::encode(const T value) +{ + validate(); + union ByteUnion + { + T value; + uint8_t bytes[sizeof(T)]; + } byte_union; + byte_union.value = value; + clearExtraBits(byte_union.value); + convertByteOrder(byte_union.bytes); + return encodeBytesImpl(byte_union.bytes, BitLen); +} + +template +int ScalarCodec::decode(T& value) +{ + validate(); + union ByteUnion + { + T value; + uint8_t bytes[sizeof(T)]; + } byte_union; + byte_union.value = T(); + const int read_res = decodeBytesImpl(byte_union.bytes, BitLen); + if (read_res > 0) + { + convertByteOrder(byte_union.bytes); + fixTwosComplement(byte_union.value); + value = byte_union.value; + } + return read_res; +} + +} + +#endif // UAVCAN_MARSHAL_SCALAR_CODEC_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/type_util.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/type_util.hpp new file mode 100644 index 000000000000..5be02921e85b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/type_util.hpp @@ -0,0 +1,88 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED +#define UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED + +#include +#include +#include + +namespace uavcan +{ +/** + * Read the specs to learn more about cast modes. + */ +enum CastMode { CastModeSaturate, CastModeTruncate }; + +/** + * Read the specs to learn more about tail array optimizations. + */ +enum TailArrayOptimizationMode { TailArrayOptDisabled, TailArrayOptEnabled }; + +/** + * Compile-time: Returns the number of bits needed to represent an integer value. + */ +template +struct IntegerBitLen +{ + enum { Result = 1 + IntegerBitLen<(Num >> 1)>::Result }; +}; +template <> +struct IntegerBitLen<0> +{ + enum { Result = 0 }; +}; + +/** + * Compile-time: Returns the number of bytes needed to contain the given number of bits. Assumes 1 byte == 8 bit. + */ +template +struct BitLenToByteLen +{ + enum { Result = (BitLen + 7) / 8 }; +}; + +/** + * Compile-time: Helper for integer and float specialization classes. Returns the platform-specific storage type. + */ +template +struct StorageType +{ + typedef T Type; +}; +template +struct StorageType::Type> +{ + typedef typename T::StorageType Type; +}; + +/** + * Compile-time: Whether T is a primitive type on this platform. + */ +template +class IsPrimitiveType +{ + typedef char Yes; + struct No { Yes dummy[8]; }; + + template + static typename EnableIf::Type test(int); + + template + static No test(...); + +public: + enum { Result = sizeof(test(0)) == sizeof(Yes) }; +}; + +/** + * Streams a given value into YAML string. Please see the specializations. + */ +template +class UAVCAN_EXPORT YamlStreamer; + +} + +#endif // UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/types.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/types.hpp new file mode 100644 index 000000000000..4fbbe9837195 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/types.hpp @@ -0,0 +1,13 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_TYPES_HPP_INCLUDED +#define UAVCAN_MARSHAL_TYPES_HPP_INCLUDED + +#include +#include +#include +#include + +#endif // UAVCAN_MARSHAL_TYPES_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/abstract_node.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/abstract_node.hpp new file mode 100644 index 000000000000..2976c32e758e --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/abstract_node.hpp @@ -0,0 +1,130 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_ABSTRACT_NODE_HPP_INCLUDED +#define UAVCAN_NODE_ABSTRACT_NODE_HPP_INCLUDED + +#include +#include +#include + +namespace uavcan +{ +/** + * Abstract node class. If you're going to implement your own node class for your application, + * please inherit this class so it can be used with default publisher, subscriber, server, etc. classes. + * Normally you don't need to use it directly though - please refer to the class Node<> instead. + */ +class UAVCAN_EXPORT INode +{ +public: + virtual ~INode() { } + virtual IPoolAllocator& getAllocator() = 0; + virtual Scheduler& getScheduler() = 0; + virtual const Scheduler& getScheduler() const = 0; + virtual void registerInternalFailure(const char* msg) = 0; + + Dispatcher& getDispatcher() { return getScheduler().getDispatcher(); } + const Dispatcher& getDispatcher() const { return getScheduler().getDispatcher(); } + + ISystemClock& getSystemClock() { return getScheduler().getSystemClock(); } + MonotonicTime getMonotonicTime() const { return getScheduler().getMonotonicTime(); } + UtcTime getUtcTime() const { return getScheduler().getUtcTime(); } + + /** + * Returns the Node ID of this node. + * If Node ID was not set yet, an invalid value will be returned. + */ + NodeID getNodeID() const { return getScheduler().getDispatcher().getNodeID(); } + + /** + * Sets the Node ID of this node. + * Node ID can be assigned only once. This method returns true if the Node ID was successfully assigned, otherwise + * it returns false. + * As long as a valid Node ID is not set, the node will remain in passive mode. + * Using a non-unicast Node ID puts the node into passive mode (as default). + */ + bool setNodeID(NodeID nid) + { + return getScheduler().getDispatcher().setNodeID(nid); + } + + /** + * Whether the node is in passive mode, i.e. can't transmit anything to the bus. + * Please read the specs to learn more. + */ + bool isPassiveMode() const { return getScheduler().getDispatcher().isPassiveMode(); } + + /** + * Same as @ref spin(MonotonicDuration), but the deadline is specified as an absolute time value + * rather than duration. + */ + int spin(MonotonicTime deadline) + { + return getScheduler().spin(deadline); + } + + /** + * Runs the node. + * Normally your application should not block anywhere else. + * Block inside this method forever or call it periodically. + * This method returns 0 if no errors occurred, or a negative error code if something failed (see error.hpp). + */ + int spin(MonotonicDuration duration) + { + return getScheduler().spin(getMonotonicTime() + duration); + } + + /** + * This method is designed for non-blocking applications. + * Instead of blocking, it returns immediately once all available CAN frames and timer events are processed. + * Note that this is unlike plain @ref spin(), which will strictly return when the deadline is reached, + * even if there still are unprocessed events. + * This method returns 0 if no errors occurred, or a negative error code if something failed (see error.hpp). + */ + int spinOnce() + { + return getScheduler().spinOnce(); + } + + /** + * This method allows to directly transmit a raw CAN frame circumventing the whole UAVCAN stack. + * Mandatory parameters: + * + * @param frame CAN frame to be transmitted. + * + * @param tx_deadline The frame will be discarded if it could not be transmitted by this time. + * + * @param iface_mask This bitmask allows to select what CAN interfaces this frame should go into. + * Example: + * - 1 - the frame will be sent only to iface 0. + * - 4 - the frame will be sent only to iface 2. + * - 3 - the frame will be sent to ifaces 0 and 1. + * + * Optional parameters: + * + * @param qos Quality of service. Please refer to the CAN IO manager for details. + * + * @param flags CAN IO flags. Please refer to the CAN driver API for details. + */ + int injectTxFrame(const CanFrame& frame, MonotonicTime tx_deadline, uint8_t iface_mask, + CanTxQueue::Qos qos = CanTxQueue::Volatile, + CanIOFlags flags = 0) + { + return getDispatcher().getCanIOManager().send(frame, tx_deadline, MonotonicTime(), iface_mask, qos, flags); + } + +#if !UAVCAN_TINY + /** + * The @ref IRxFrameListener interface allows one to monitor all incoming CAN frames. + * This feature can be used to implement multithreaded nodes, or to add secondary protocol support. + */ + void removeRxFrameListener() { getDispatcher().removeRxFrameListener(); } + void installRxFrameListener(IRxFrameListener* lst) { getDispatcher().installRxFrameListener(lst); } +#endif +}; + +} + +#endif // UAVCAN_NODE_ABSTRACT_NODE_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/generic_publisher.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/generic_publisher.hpp new file mode 100644 index 000000000000..a8197b6cf9a9 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/generic_publisher.hpp @@ -0,0 +1,201 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_GENERIC_PUBLISHER_HPP_INCLUDED +#define UAVCAN_NODE_GENERIC_PUBLISHER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class GenericPublisherBase : Noncopyable +{ + TransferSender sender_; + MonotonicDuration tx_timeout_; + INode& node_; + +protected: + GenericPublisherBase(INode& node, MonotonicDuration tx_timeout, + MonotonicDuration max_transfer_interval) + : sender_(node.getDispatcher(), max_transfer_interval) + , tx_timeout_(tx_timeout) + , node_(node) + { + setTxTimeout(tx_timeout); +#if UAVCAN_DEBUG + UAVCAN_ASSERT(getTxTimeout() == tx_timeout); // Making sure default values are OK +#endif + } + + ~GenericPublisherBase() { } + + bool isInited() const; + + int doInit(DataTypeKind dtkind, const char* dtname, CanTxQueue::Qos qos); + + MonotonicTime getTxDeadline() const; + + int genericPublish(const StaticTransferBufferImpl& buffer, TransferType transfer_type, + NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline); + + TransferSender& getTransferSender() { return sender_; } + const TransferSender& getTransferSender() const { return sender_; } + +public: + static MonotonicDuration getMinTxTimeout() { return MonotonicDuration::fromUSec(200); } + static MonotonicDuration getMaxTxTimeout() { return MonotonicDuration::fromMSec(60000); } + + MonotonicDuration getTxTimeout() const { return tx_timeout_; } + void setTxTimeout(MonotonicDuration tx_timeout); + + /** + * By default, attempt to send a transfer from passive mode will result in an error @ref ErrPassive. + * This option allows to enable sending anonymous transfers from passive mode. + */ + void allowAnonymousTransfers() + { + sender_.allowAnonymousTransfers(); + } + + /** + * Priority of outgoing transfers. + */ + TransferPriority getPriority() const { return sender_.getPriority(); } + void setPriority(const TransferPriority prio) { sender_.setPriority(prio); } + + INode& getNode() const { return node_; } +}; + +/** + * Generic publisher, suitable for messages and services. + * DataSpec - data type specification class + * DataStruct - instantiable class + */ +template +class UAVCAN_EXPORT GenericPublisher : public GenericPublisherBase +{ + struct ZeroTransferBuffer : public StaticTransferBufferImpl + { + ZeroTransferBuffer() : StaticTransferBufferImpl(UAVCAN_NULLPTR, 0) { } + }; + + typedef typename Select::Result> >::Result Buffer; + + enum + { + Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ? + CanTxQueue::Volatile : CanTxQueue::Persistent + }; + + int checkInit(); + + int doEncode(const DataStruct& message, ITransferBuffer& buffer) const; + + int genericPublish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id, + TransferID* tid, MonotonicTime blocking_deadline); + +public: + /** + * @param max_transfer_interval Maximum expected time interval between subsequent publications. Leave default. + */ + GenericPublisher(INode& node, MonotonicDuration tx_timeout, + MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval()) + : GenericPublisherBase(node, tx_timeout, max_transfer_interval) + { } + + ~GenericPublisher() { } + + /** + * Init method can be called prior first publication, but it's not necessary + * because the publisher can be automatically initialized ad-hoc. + */ + int init() + { + return checkInit(); + } + + /** + * This overload allows to set the priority; otherwise it's the same. + */ + int init(TransferPriority priority) + { + setPriority(priority); + return checkInit(); + } + + int publish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id, + MonotonicTime blocking_deadline = MonotonicTime()) + { + return genericPublish(message, transfer_type, dst_node_id, UAVCAN_NULLPTR, blocking_deadline); + } + + int publish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id, TransferID tid, + MonotonicTime blocking_deadline = MonotonicTime()) + { + return genericPublish(message, transfer_type, dst_node_id, &tid, blocking_deadline); + } +}; + +// ---------------------------------------------------------------------------- + +template +int GenericPublisher::checkInit() +{ + if (isInited()) + { + return 0; + } + return doInit(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName(), CanTxQueue::Qos(Qos)); +} + +template +int GenericPublisher::doEncode(const DataStruct& message, ITransferBuffer& buffer) const +{ + BitStream bitstream(buffer); + ScalarCodec codec(bitstream); + const int encode_res = DataStruct::encode(message, codec); + if (encode_res <= 0) + { + UAVCAN_ASSERT(0); // Impossible, internal error + return -ErrInvalidMarshalData; + } + return encode_res; +} + +template +int GenericPublisher::genericPublish(const DataStruct& message, TransferType transfer_type, + NodeID dst_node_id, TransferID* tid, + MonotonicTime blocking_deadline) +{ + const int res = checkInit(); + if (res < 0) + { + return res; + } + + Buffer buffer; + + const int encode_res = doEncode(message, buffer); + if (encode_res < 0) + { + return encode_res; + } + + return GenericPublisherBase::genericPublish(buffer, transfer_type, dst_node_id, tid, blocking_deadline); +} + +} + +#endif // UAVCAN_NODE_GENERIC_PUBLISHER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/generic_subscriber.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/generic_subscriber.hpp new file mode 100644 index 000000000000..ef0cb877bfc9 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -0,0 +1,299 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_GENERIC_SUBSCRIBER_HPP_INCLUDED +#define UAVCAN_NODE_GENERIC_SUBSCRIBER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class extends the data structure with extra information obtained from the transport layer, + * such as Source Node ID, timestamps, Transfer ID, index of the interface this transfer was picked up from, etc. + * + * PLEASE NOTE that since this class inherits the data structure type, subscription callbacks can accept either + * object of this class or the data structure type directly if the extra information is not needed. + * + * For example, both of these callbacks can be used with the same data structure 'Foo': + * void first(const ReceivedDataStructure& msg); + * void second(const Foo& msg); + * In the latter case, an implicit cast will happen before the callback is invoked. + * + * This class is not copyable because it holds a reference to a stack-allocated transfer descriptor object. + * You can slice cast it to the underlying data type though, which would be copyable: + * DataType dt = rds; // where rds is of type ReceivedDataStructure + * // dt is now copyable + */ +template +class UAVCAN_EXPORT ReceivedDataStructure : public DataType_, Noncopyable +{ + const IncomingTransfer* const _transfer_; ///< Such weird name is necessary to avoid clashing with DataType fields + + template + Ret safeget() const + { + if (_transfer_ == UAVCAN_NULLPTR) + { + return Ret(); + } + return (_transfer_->*Fun)(); + } + +protected: + ReceivedDataStructure() + : _transfer_(UAVCAN_NULLPTR) + { } + + ReceivedDataStructure(const IncomingTransfer* arg_transfer) + : _transfer_(arg_transfer) + { + UAVCAN_ASSERT(arg_transfer != UAVCAN_NULLPTR); + } + +public: + typedef DataType_ DataType; + + MonotonicTime getMonotonicTimestamp() const + { + return safeget(); + } + UtcTime getUtcTimestamp() const { return safeget(); } + TransferPriority getPriority() const { return safeget(); } + TransferType getTransferType() const { return safeget(); } + TransferID getTransferID() const { return safeget(); } + NodeID getSrcNodeID() const { return safeget(); } + uint8_t getIfaceIndex() const { return safeget(); } + bool isAnonymousTransfer() const { return safeget(); } +}; + +/** + * This operator neatly prints the data structure prepended with extra data from the transport layer. + * The extra data will be represented as YAML comment. + */ +template +static Stream& operator<<(Stream& s, const ReceivedDataStructure& rds) +{ + s << "# Received struct ts_m=" << rds.getMonotonicTimestamp() + << " ts_utc=" << rds.getUtcTimestamp() + << " snid=" << int(rds.getSrcNodeID().get()) << "\n"; + s << static_cast(rds); + return s; +} + + +class GenericSubscriberBase : Noncopyable +{ +protected: + INode& node_; + uint32_t failure_count_; + + explicit GenericSubscriberBase(INode& node) + : node_(node) + , failure_count_(0) + { } + + ~GenericSubscriberBase() { } + + int genericStart(TransferListener* listener, bool (Dispatcher::*registration_method)(TransferListener*)); + + void stop(TransferListener* listener); + +public: + /** + * Returns the number of failed attempts to decode received message. Generally, a failed attempt means either: + * - Transient failure in the transport layer. + * - Incompatible data types. + */ + uint32_t getFailureCount() const { return failure_count_; } + + INode& getNode() const { return node_; } +}; + +/** + * Please note that the reference passed to the RX callback points to a stack-allocated object, which means + * that it gets invalidated shortly after the callback returns. + */ +template +class UAVCAN_EXPORT GenericSubscriber : public GenericSubscriberBase +{ + typedef GenericSubscriber SelfType; + + // We need to break the inheritance chain here to implement lazy initialization + class TransferForwarder : public TransferListenerType + { + SelfType& obj_; + + void handleIncomingTransfer(IncomingTransfer& transfer) override + { + obj_.handleIncomingTransfer(transfer); + } + + public: + TransferForwarder(SelfType& obj, + const DataTypeDescriptor& data_type, + uint16_t max_buffer_size, + IPoolAllocator& allocator) : + TransferListenerType(obj.node_.getDispatcher().getTransferPerfCounter(), + data_type, + max_buffer_size, + allocator), + obj_(obj) + { } + }; + + LazyConstructor forwarder_; + + int checkInit(); + + void handleIncomingTransfer(IncomingTransfer& transfer); + + int genericStart(bool (Dispatcher::*registration_method)(TransferListener*)); + +protected: + struct ReceivedDataStructureSpec : public ReceivedDataStructure + { + ReceivedDataStructureSpec() { } + + ReceivedDataStructureSpec(const IncomingTransfer* arg_transfer) : + ReceivedDataStructure(arg_transfer) + { } + }; + + explicit GenericSubscriber(INode& node) : GenericSubscriberBase(node) + { } + + virtual ~GenericSubscriber() { stop(); } + + virtual void handleReceivedDataStruct(ReceivedDataStructure&) = 0; + + int startAsMessageListener() + { + UAVCAN_TRACE("GenericSubscriber", "Start as message listener; dtname=%s", DataSpec::getDataTypeFullName()); + return genericStart(&Dispatcher::registerMessageListener); + } + + int startAsServiceRequestListener() + { + UAVCAN_TRACE("GenericSubscriber", "Start as service request listener; dtname=%s", + DataSpec::getDataTypeFullName()); + return genericStart(&Dispatcher::registerServiceRequestListener); + } + + int startAsServiceResponseListener() + { + UAVCAN_TRACE("GenericSubscriber", "Start as service response listener; dtname=%s", + DataSpec::getDataTypeFullName()); + return genericStart(&Dispatcher::registerServiceResponseListener); + } + + /** + * By default, anonymous transfers will be ignored. + * This option allows to enable reception of anonymous transfers. + */ + void allowAnonymousTransfers() + { + forwarder_->allowAnonymousTransfers(); + } + + /** + * Terminate the subscription. + * Dispatcher core will remove this instance from the subscribers list. + */ + void stop() + { + UAVCAN_TRACE("GenericSubscriber", "Stop; dtname=%s", DataSpec::getDataTypeFullName()); + GenericSubscriberBase::stop(forwarder_); + } + + TransferListenerType* getTransferListener() { return forwarder_; } +}; + +// ---------------------------------------------------------------------------- + +/* + * GenericSubscriber + */ +template +int GenericSubscriber::checkInit() +{ + if (forwarder_) + { + return 0; + } + + GlobalDataTypeRegistry::instance().freeze(); + const DataTypeDescriptor* const descr = + GlobalDataTypeRegistry::instance().find(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName()); + if (descr == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("GenericSubscriber", "Type [%s] is not registered", DataSpec::getDataTypeFullName()); + return -ErrUnknownDataType; + } + + static const uint16_t MaxBufferSize = BitLenToByteLen::Result; + + forwarder_.template construct + (*this, *descr, MaxBufferSize, node_.getAllocator()); + + return 0; +} + +template +void GenericSubscriber::handleIncomingTransfer(IncomingTransfer& transfer) +{ + ReceivedDataStructureSpec rx_struct(&transfer); + + /* + * Decoding into the temporary storage + */ + BitStream bitstream(transfer); + ScalarCodec codec(bitstream); + + const int decode_res = DataStruct::decode(rx_struct, codec); + + // We don't need the data anymore, the memory can be reused from the callback: + transfer.release(); + + if (decode_res <= 0) + { + UAVCAN_TRACE("GenericSubscriber", "Unable to decode the message [%i] [%s]", + decode_res, DataSpec::getDataTypeFullName()); + failure_count_++; + node_.getDispatcher().getTransferPerfCounter().addError(); + return; + } + + /* + * Invoking the callback + */ + handleReceivedDataStruct(rx_struct); +} + +template +int GenericSubscriber:: +genericStart(bool (Dispatcher::*registration_method)(TransferListener*)) +{ + const int res = checkInit(); + if (res < 0) + { + UAVCAN_TRACE("GenericSubscriber", "Initialization failure [%s]", DataSpec::getDataTypeFullName()); + return res; + } + return GenericSubscriberBase::genericStart(forwarder_, registration_method); +} + + +} + +#endif // UAVCAN_NODE_GENERIC_SUBSCRIBER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/global_data_type_registry.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/global_data_type_registry.hpp new file mode 100644 index 000000000000..0148ce0f5094 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/global_data_type_registry.hpp @@ -0,0 +1,241 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_GLOBAL_DATA_TYPE_REGISTRY_HPP_INCLUDED +#define UAVCAN_NODE_GLOBAL_DATA_TYPE_REGISTRY_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#if UAVCAN_DEBUG +# include +#endif + +namespace uavcan +{ +/** + * This singleton is shared among all existing node instances. It is instantiated automatically + * when the C++ runtime executes contstructors before main(). + * + * Its purpose is to keep the list of all UAVCAN data types known and used by this application. + * + * Also, the mapping between Data Type name and its Data Type ID is also stored in this singleton. + * UAVCAN data types with default Data Type ID that are autogenerated by the libuavcan DSDL compiler + * are registered automatically before main() (refer to the generated headers to see how exactly). + * Data types that don't have a default Data Type ID must be registered manually using the methods + * of this class (read the method documentation). + * + * Attempt to use a data type that was not registered with this singleton (e.g. publish, subscribe, + * perform a service call etc.) will fail with an error code @ref ErrUnknownDataType. + */ +class UAVCAN_EXPORT GlobalDataTypeRegistry : Noncopyable +{ + struct Entry : public LinkedListNode + { + DataTypeDescriptor descriptor; + + Entry() { } + + Entry(DataTypeKind kind, DataTypeID id, const DataTypeSignature& signature, const char* name) + : descriptor(kind, id, signature, name) + { } + }; + + struct EntryInsertionComparator + { + const DataTypeID id; + explicit EntryInsertionComparator(const Entry* dtd) + : id((dtd == UAVCAN_NULLPTR) ? DataTypeID() : dtd->descriptor.getID()) + { + UAVCAN_ASSERT(dtd != UAVCAN_NULLPTR); + } + bool operator()(const Entry* entry) const + { + UAVCAN_ASSERT(entry != UAVCAN_NULLPTR); + return entry->descriptor.getID() > id; + } + }; + +public: + /** + * Result of data type registration + */ + enum RegistrationResult + { + RegistrationResultOk, ///< Success, data type is now registered and can be used. + RegistrationResultCollision, ///< Data type name or ID is not unique. + RegistrationResultInvalidParams, ///< Invalid input parameters. + RegistrationResultFrozen ///< Data Type Registery has been frozen and can't be modified anymore. + }; + +private: + typedef LinkedListRoot List; + mutable List msgs_; + mutable List srvs_; + bool frozen_; + + GlobalDataTypeRegistry() : frozen_(false) { } + + List* selectList(DataTypeKind kind) const; + + RegistrationResult remove(Entry* dtd); + RegistrationResult registImpl(Entry* dtd); + +public: + /** + * Returns the reference to the singleton. + */ + static GlobalDataTypeRegistry& instance(); + + /** + * Register a data type 'Type' with ID 'id'. + * If this data type was registered earlier, its old registration will be overridden. + * This method will fail if the data type registry is frozen. + * + * @tparam Type Autogenerated UAVCAN data type to register. Data types are generated by the + * libuavcan DSDL compiler from DSDL definitions. + * + * @param id Data Type ID for this data type. + */ + template + RegistrationResult registerDataType(DataTypeID id); + + /** + * Data Type registry needs to be frozen before a node instance can use it in + * order to prevent accidental change in data type configuration on a running + * node. + * + * This method will be called automatically by the node during start up, so + * the user does not need to call it from the application manually. Subsequent + * calls will not have any effect. + * + * Once frozen, data type registry can't be unfrozen. + */ + void freeze(); + bool isFrozen() const { return frozen_; } + + /** + * Finds data type descriptor by full data type name, e.g. "uavcan.protocol.NodeStatus". + * Messages are searched first, then services. + * Returns null pointer if the data type with this name is not registered. + * @param name Full data type name + * @return Descriptor for this data type or null pointer if not found + */ + const DataTypeDescriptor* find(const char* name) const; + + /** + * Finds data type descriptor by full data type name, e.g. "uavcan.protocol.NodeStatus", and data type kind. + * Returns null pointer if the data type with this name is not registered. + * @param kind Data Type Kind - message or service + * @param name Full data type name + * @return Descriptor for this data type or null pointer if not found + */ + const DataTypeDescriptor* find(DataTypeKind kind, const char* name) const; + + /** + * Finds data type descriptor by data type ID. + * Returns null pointer if the data type with this ID is not registered. + * @param kind Data Type Kind - message or service + * @param dtid Data Type ID + * @return Descriptor for this data type or null pointer if not found + */ + const DataTypeDescriptor* find(DataTypeKind kind, DataTypeID dtid) const; + + /** + * Returns the number of registered message types. + */ + unsigned getNumMessageTypes() const { return msgs_.getLength(); } + + /** + * Returns the number of registered service types. + */ + unsigned getNumServiceTypes() const { return srvs_.getLength(); } + +#if UAVCAN_DEBUG + /// Required for unit testing + void reset() + { + UAVCAN_TRACE("GlobalDataTypeRegistry", "Reset; was frozen: %i, num msgs: %u, num srvs: %u", + int(frozen_), getNumMessageTypes(), getNumServiceTypes()); + frozen_ = false; + while (msgs_.get()) + { + msgs_.remove(msgs_.get()); + } + while (srvs_.get()) + { + srvs_.remove(srvs_.get()); + } + } +#endif +}; + +/** + * This class is used by autogenerated data types to register with the + * data type registry automatically before main() is called. Note that + * if a generated data type header file is not included by any translation + * unit of the application, the data type will not be registered. + * + * Data type needs to have a default ID to be registrable by this class. + */ +template +struct UAVCAN_EXPORT DefaultDataTypeRegistrator +{ + DefaultDataTypeRegistrator() + { +#if !UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY + const GlobalDataTypeRegistry::RegistrationResult res = + GlobalDataTypeRegistry::instance().registerDataType(Type::DefaultDataTypeID); + + if (res != GlobalDataTypeRegistry::RegistrationResultOk) + { + handleFatalError("Type reg failed"); + } +#endif + } +}; + +// ---------------------------------------------------------------------------- + +/* + * GlobalDataTypeRegistry + */ +template +GlobalDataTypeRegistry::RegistrationResult GlobalDataTypeRegistry::registerDataType(DataTypeID id) +{ + if (isFrozen()) + { + return RegistrationResultFrozen; + } + + static Entry entry; + + { + const RegistrationResult remove_res = remove(&entry); + if (remove_res != RegistrationResultOk) + { + return remove_res; + } + } + + // We can't just overwrite the entry itself because it's noncopyable + entry.descriptor = DataTypeDescriptor(DataTypeKind(Type::DataTypeKind), id, + Type::getDataTypeSignature(), Type::getDataTypeFullName()); + + { + const RegistrationResult remove_res = remove(&entry); + if (remove_res != RegistrationResultOk) + { + return remove_res; + } + } + return registImpl(&entry); +} + +} + +#endif // UAVCAN_NODE_GLOBAL_DATA_TYPE_REGISTRY_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/node.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/node.hpp new file mode 100644 index 000000000000..b19b246824a6 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/node.hpp @@ -0,0 +1,319 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_NODE_HPP_INCLUDED +#define UAVCAN_NODE_NODE_HPP_INCLUDED + +#include +#include +#include +#include + +// High-level functionality available by default +#include + +#if !UAVCAN_TINY +# include +# include +# include +# include +#endif + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +namespace uavcan +{ +/** + * This is the top-level node API. + * A custom node class can be implemented if needed, in which case it shall inherit INode. + * + * @tparam MemPoolSize Size of memory pool for this node, in bytes. + * Please refer to the documentation for details. + * If this value is zero, the constructor will accept a reference to user-provided allocator. + */ +template +class UAVCAN_EXPORT Node : public INode +{ + typedef typename + Select<(MemPoolSize > 0), + PoolAllocator, // If pool size is specified, use default allocator + IPoolAllocator& // Otherwise use reference to user-provided allocator + >::Result Allocator; + + Allocator pool_allocator_; + Scheduler scheduler_; + + NodeStatusProvider proto_nsp_; +#if !UAVCAN_TINY + DataTypeInfoProvider proto_dtp_; + Logger proto_logger_; + RestartRequestServer proto_rrs_; + TransportStatsProvider proto_tsp_; +#endif + + uint64_t internal_failure_cnt_; + bool started_; + + void commonInit() + { + internal_failure_cnt_ = 0; + started_ = false; + } + +protected: + virtual void registerInternalFailure(const char* msg) override + { + internal_failure_cnt_++; + UAVCAN_TRACE("Node", "Internal failure: %s", msg); +#if UAVCAN_TINY + (void)msg; +#else + (void)getLogger().log(protocol::debug::LogLevel::ERROR, "UAVCAN", msg); +#endif + } + +public: + /** + * This overload is only valid if MemPoolSize > 0. + */ + Node(ICanDriver& can_driver, + ISystemClock& system_clock) : + scheduler_(can_driver, pool_allocator_, system_clock), + proto_nsp_(*this) +#if !UAVCAN_TINY + , proto_dtp_(*this) + , proto_logger_(*this) + , proto_rrs_(*this) + , proto_tsp_(*this) +#endif + { + commonInit(); + } + + /** + * This overload is only valid if MemPoolSize == 0. + */ + Node(ICanDriver& can_driver, + ISystemClock& system_clock, + IPoolAllocator& allocator) : + pool_allocator_(allocator), + scheduler_(can_driver, pool_allocator_, system_clock), + proto_nsp_(*this) +#if !UAVCAN_TINY + , proto_dtp_(*this) + , proto_logger_(*this) + , proto_rrs_(*this) + , proto_tsp_(*this) +#endif + { + commonInit(); + } + + virtual typename RemoveReference::Type& getAllocator() override { return pool_allocator_; } + + virtual Scheduler& getScheduler() override { return scheduler_; } + virtual const Scheduler& getScheduler() const override { return scheduler_; } + + int spin(MonotonicTime deadline) + { + if (started_) + { + return INode::spin(deadline); + } + return -ErrNotInited; + } + + int spin(MonotonicDuration duration) + { + if (started_) + { + return INode::spin(duration); + } + return -ErrNotInited; + } + + int spinOnce() + { + if (started_) + { + return INode::spinOnce(); + } + return -ErrNotInited; + } + + bool isStarted() const { return started_; } + + uint64_t getInternalFailureCount() const { return internal_failure_cnt_; } + + /** + * Starts the node and publishes uavcan.protocol.NodeStatus immediately. + * Does not so anything if the node is already started. + * Once started, the node can't stop. + * If the node failed to start up, it's recommended to destroy the current node instance and start over. + * Returns negative error code. + * @param node_status_transfer_priority Transfer priority that will be used for outgoing NodeStatus messages. + * Normal priority is used by default. + */ + int start(const TransferPriority node_status_transfer_priority = TransferPriority::Default); + + /** + * Gets/sets the node name, e.g. "com.example.product_name". The node name can be set only once. + * The name must be set before the node is started, otherwise the node will refuse to start up. + */ + const NodeStatusProvider::NodeName& getName() const { return proto_nsp_.getName(); } + void setName(const NodeStatusProvider::NodeName& name) { proto_nsp_.setName(name); } + + /** + * Node health code helpers. + */ + void setHealthOk() { proto_nsp_.setHealthOk(); } + void setHealthWarning() { proto_nsp_.setHealthWarning(); } + void setHealthError() { proto_nsp_.setHealthError(); } + void setHealthCritical() { proto_nsp_.setHealthCritical(); } + + /** + * Node mode code helpers. + * Note that INITIALIZATION is the default mode; the application has to manually set it to OPERATIONAL. + */ + void setModeOperational() { proto_nsp_.setModeOperational(); } + void setModeInitialization() { proto_nsp_.setModeInitialization(); } + void setModeMaintenance() { proto_nsp_.setModeMaintenance(); } + void setModeSoftwareUpdate() { proto_nsp_.setModeSoftwareUpdate(); } + + void setModeOfflineAndPublish() + { + proto_nsp_.setModeOffline(); + (void)proto_nsp_.forcePublish(); + } + + /** + * Updates the vendor-specific status code. + */ + void setVendorSpecificStatusCode(NodeStatusProvider::VendorSpecificStatusCode code) + { + proto_nsp_.setVendorSpecificStatusCode(code); + } + + /** + * Gets/sets the node version information. + */ + void setSoftwareVersion(const protocol::SoftwareVersion& version) { proto_nsp_.setSoftwareVersion(version); } + void setHardwareVersion(const protocol::HardwareVersion& version) { proto_nsp_.setHardwareVersion(version); } + + const protocol::SoftwareVersion& getSoftwareVersion() const { return proto_nsp_.getSoftwareVersion(); } + const protocol::HardwareVersion& getHardwareVersion() const { return proto_nsp_.getHardwareVersion(); } + + NodeStatusProvider& getNodeStatusProvider() { return proto_nsp_; } + +#if !UAVCAN_TINY + /** + * Restart handler can be installed to handle external node restart requests (highly recommended). + */ + void setRestartRequestHandler(IRestartRequestHandler* handler) { proto_rrs_.setHandler(handler); } + + RestartRequestServer& getRestartRequestServer() { return proto_rrs_; } + + /** + * Node logging. + * Logging calls are passed directly into the @ref Logger instance. + * Type safe log formatting is supported only in C++11 mode. + * @{ + */ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + + template + inline void logDebug(const char* source, const char* format, Args... args) + { + (void)proto_logger_.logDebug(source, format, args...); + } + + template + inline void logInfo(const char* source, const char* format, Args... args) + { + (void)proto_logger_.logInfo(source, format, args...); + } + + template + inline void logWarning(const char* source, const char* format, Args... args) + { + (void)proto_logger_.logWarning(source, format, args...); + } + + template + inline void logError(const char* source, const char* format, Args... args) + { + (void)proto_logger_.logError(source, format, args...); + } + +#else + + void logDebug(const char* source, const char* text) { (void)proto_logger_.logDebug(source, text); } + void logInfo(const char* source, const char* text) { (void)proto_logger_.logInfo(source, text); } + void logWarning(const char* source, const char* text) { (void)proto_logger_.logWarning(source, text); } + void logError(const char* source, const char* text) { (void)proto_logger_.logError(source, text); } + +#endif + /** + * @} + */ + + /** + * Use this method to configure logging. + */ + Logger& getLogger() { return proto_logger_; } + +#endif // UAVCAN_TINY +}; + +// ---------------------------------------------------------------------------- + +template +int Node::start(const TransferPriority priority) +{ + if (started_) + { + return 0; + } + GlobalDataTypeRegistry::instance().freeze(); + + int res = 0; + res = proto_nsp_.startAndPublish(priority); + if (res < 0) + { + goto fail; + } +#if !UAVCAN_TINY + res = proto_dtp_.start(); + if (res < 0) + { + goto fail; + } + res = proto_logger_.init(); + if (res < 0) + { + goto fail; + } + res = proto_rrs_.start(); + if (res < 0) + { + goto fail; + } + res = proto_tsp_.start(); + if (res < 0) + { + goto fail; + } +#endif + started_ = res >= 0; + return res; +fail: + UAVCAN_ASSERT(res < 0); + return res; +} + +} + +#endif // UAVCAN_NODE_NODE_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/publisher.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/publisher.hpp new file mode 100644 index 000000000000..16ca3537a199 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/publisher.hpp @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_PUBLISHER_HPP_INCLUDED +#define UAVCAN_NODE_PUBLISHER_HPP_INCLUDED + +#include + +namespace uavcan +{ +/** + * Use this class to publish messages to the bus (broadcast, unicast, or both). + * + * @tparam DataType_ Message data type + */ +template +class UAVCAN_EXPORT Publisher : protected GenericPublisher +{ + typedef GenericPublisher BaseType; + +public: + typedef DataType_ DataType; ///< Message data type + + /** + * @param node Node instance this publisher will be registered with. + * + * @param tx_timeout If CAN frames of this message are not delivered to the bus + * in this amount of time, they will be discarded. Default value + * is good enough for rather high-frequency, high-priority messages. + * + * @param max_transfer_interval Maximum expected transfer interval. It's absolutely safe + * to leave default value here. It just defines how soon the + * Transfer ID tracking objects associated with this message type + * will be garbage collected by the library if it's no longer + * being published. + */ + explicit Publisher(INode& node, MonotonicDuration tx_timeout = getDefaultTxTimeout(), + MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval()) + : BaseType(node, tx_timeout, max_transfer_interval) + { +#if UAVCAN_DEBUG + UAVCAN_ASSERT(getTxTimeout() == tx_timeout); // Making sure default values are OK +#endif + StaticAssert::check(); + } + + /** + * Broadcast the message. + * Returns negative error code. + */ + int broadcast(const DataType& message) + { + return BaseType::publish(message, TransferTypeMessageBroadcast, NodeID::Broadcast); + } + + /** + * Warning: You probably don't want to use this method; it's for advanced use cases like + * e.g. network time synchronization. Use the overloaded method with fewer arguments instead. + * This overload allows to explicitly specify Transfer ID. + * Returns negative error code. + */ + int broadcast(const DataType& message, TransferID tid) + { + return BaseType::publish(message, TransferTypeMessageBroadcast, NodeID::Broadcast, tid); + } + + static MonotonicDuration getDefaultTxTimeout() { return MonotonicDuration::fromMSec(100); } + + /** + * Init method can be called prior first publication, but it's not necessary + * because the publisher can be automatically initialized ad-hoc. + */ + using BaseType::init; + + using BaseType::allowAnonymousTransfers; + using BaseType::getTransferSender; + using BaseType::getMinTxTimeout; + using BaseType::getMaxTxTimeout; + using BaseType::getTxTimeout; + using BaseType::setTxTimeout; + using BaseType::getPriority; + using BaseType::setPriority; + using BaseType::getNode; +}; + +} + +#endif // UAVCAN_NODE_PUBLISHER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/scheduler.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/scheduler.hpp new file mode 100644 index 000000000000..7973337983d0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/scheduler.hpp @@ -0,0 +1,155 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_SCHEDULER_HPP_INCLUDED +#define UAVCAN_NODE_SCHEDULER_HPP_INCLUDED + +#include +#include +#include + +namespace uavcan +{ + +class UAVCAN_EXPORT Scheduler; + +class UAVCAN_EXPORT DeadlineHandler : public LinkedListNode +{ + MonotonicTime deadline_; + +protected: + Scheduler& scheduler_; + + explicit DeadlineHandler(Scheduler& scheduler) + : scheduler_(scheduler) + { } + + virtual ~DeadlineHandler() { stop(); } + +public: + virtual void handleDeadline(MonotonicTime current) = 0; + + void startWithDeadline(MonotonicTime deadline); + void startWithDelay(MonotonicDuration delay); + void generateDeadlineImmediately() { startWithDeadline(MonotonicTime::fromUSec(1)); } + + void stop(); + + bool isRunning() const; + + MonotonicTime getDeadline() const { return deadline_; } + Scheduler& getScheduler() const { return scheduler_; } +}; + + +class UAVCAN_EXPORT DeadlineScheduler : Noncopyable +{ + LinkedListRoot handlers_; // Ordered by deadline, lowest first + +public: + void add(DeadlineHandler* mdh); + void remove(DeadlineHandler* mdh); + bool doesExist(const DeadlineHandler* mdh) const; + unsigned getNumHandlers() const { return handlers_.getLength(); } + + MonotonicTime pollAndGetMonotonicTime(ISystemClock& sysclock); + MonotonicTime getEarliestDeadline() const; +}; + +/** + * This class distributes processing time between library components (IO handling, deadline callbacks, ...). + */ +class UAVCAN_EXPORT Scheduler : Noncopyable +{ + enum { DefaultDeadlineResolutionMs = 5 }; + enum { MinDeadlineResolutionMs = 1 }; + enum { MaxDeadlineResolutionMs = 100 }; + + enum { DefaultCleanupPeriodMs = 1000 }; + enum { MinCleanupPeriodMs = 10 }; + enum { MaxCleanupPeriodMs = 10000 }; + + DeadlineScheduler deadline_scheduler_; + Dispatcher dispatcher_; + MonotonicTime prev_cleanup_ts_; + MonotonicDuration deadline_resolution_; + MonotonicDuration cleanup_period_; + bool inside_spin_; + + struct InsideSpinSetter + { + Scheduler& owner; + InsideSpinSetter(Scheduler& o) + : owner(o) + { + owner.inside_spin_ = true; + } + ~InsideSpinSetter() { owner.inside_spin_ = false; } + }; + + MonotonicTime computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const; + void pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin); + +public: + Scheduler(ICanDriver& can_driver, IPoolAllocator& allocator, ISystemClock& sysclock) + : dispatcher_(can_driver, allocator, sysclock) + , prev_cleanup_ts_(sysclock.getMonotonic()) + , deadline_resolution_(MonotonicDuration::fromMSec(DefaultDeadlineResolutionMs)) + , cleanup_period_(MonotonicDuration::fromMSec(DefaultCleanupPeriodMs)) + , inside_spin_(false) + { } + + /** + * Spin until the deadline, or until some error occurs. + * This function will return strictly when the deadline is reached, even if there are unprocessed frames. + * Returns negative error code. + */ + int spin(MonotonicTime deadline); + + /** + * Non-blocking version of @ref spin() - spins until all pending frames and events are processed, + * or until some error occurs. If there's nothing to do, returns immediately. + * Returns negative error code. + */ + int spinOnce(); + + DeadlineScheduler& getDeadlineScheduler() { return deadline_scheduler_; } + + Dispatcher& getDispatcher() { return dispatcher_; } + const Dispatcher& getDispatcher() const { return dispatcher_; } + + ISystemClock& getSystemClock() { return dispatcher_.getSystemClock(); } + MonotonicTime getMonotonicTime() const { return dispatcher_.getSystemClock().getMonotonic(); } + UtcTime getUtcTime() const { return dispatcher_.getSystemClock().getUtc(); } + + /** + * Worst case deadline callback resolution. + * Higher resolution increases CPU usage. + */ + MonotonicDuration getDeadlineResolution() const { return deadline_resolution_; } + void setDeadlineResolution(MonotonicDuration res) + { + res = min(res, MonotonicDuration::fromMSec(MaxDeadlineResolutionMs)); + res = max(res, MonotonicDuration::fromMSec(MinDeadlineResolutionMs)); + deadline_resolution_ = res; + } + + /** + * How often the scheduler will run cleanup (listeners, outgoing transfer registry, ...). + * Cleanup execution time grows linearly with number of listeners and number of items + * in the Outgoing Transfer ID registry. + * Lower period increases CPU usage. + */ + MonotonicDuration getCleanupPeriod() const { return cleanup_period_; } + void setCleanupPeriod(MonotonicDuration period) + { + period = min(period, MonotonicDuration::fromMSec(MaxCleanupPeriodMs)); + period = max(period, MonotonicDuration::fromMSec(MinCleanupPeriodMs)); + cleanup_period_ = period; + } +}; + +} + +#endif // UAVCAN_NODE_SCHEDULER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/service_client.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/service_client.hpp new file mode 100644 index 000000000000..d5e3c47d49d6 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/service_client.hpp @@ -0,0 +1,585 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED +#define UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED + +#include +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ +/** + * This struct describes a pending service call. + * Refer to @ref ServiceClient to learn more about service calls. + */ +struct ServiceCallID +{ + NodeID server_node_id; + TransferID transfer_id; + + ServiceCallID() { } + + ServiceCallID(NodeID arg_server_node_id, TransferID arg_transfer_id) + : server_node_id(arg_server_node_id) + , transfer_id(arg_transfer_id) + { } + + bool operator==(const ServiceCallID rhs) const + { + return (rhs.server_node_id == server_node_id) && + (rhs.transfer_id == transfer_id); + } + + bool isValid() const { return server_node_id.isUnicast(); } +}; + +/** + * Object of this type will be returned to the application as a result of service call. + * Note that application ALWAYS gets this result, even when it times out or fails because of some other reason. + * The class is made noncopyable because it keeps a reference to a stack-allocated object. + */ +template +class UAVCAN_EXPORT ServiceCallResult : Noncopyable +{ +public: + typedef ReceivedDataStructure ResponseFieldType; + + enum Status { Success, ErrorTimeout }; + +private: + const Status status_; ///< Whether successful or not. Failure to decode the response causes timeout. + ServiceCallID call_id_; ///< Identifies the call + ResponseFieldType& response_; ///< Returned data structure. Value undefined if the service call has failed. + +public: + ServiceCallResult(Status arg_status, ServiceCallID arg_call_id, ResponseFieldType& arg_response) + : status_(arg_status) + , call_id_(arg_call_id) + , response_(arg_response) + { + UAVCAN_ASSERT(call_id_.isValid()); + UAVCAN_ASSERT((status_ == Success) || (status_ == ErrorTimeout)); + } + + /** + * Shortcut to quickly check whether the call was successful. + */ + bool isSuccessful() const { return status_ == Success; } + + Status getStatus() const { return status_; } + + ServiceCallID getCallID() const { return call_id_; } + + /** + * Returned reference points to a stack-allocated object. + */ + const ResponseFieldType& getResponse() const { return response_; } + ResponseFieldType& getResponse() { return response_; } +}; + +/** + * This operator neatly prints the service call result prepended with extra data like Server Node ID. + * The extra data will be represented as YAML comment. + */ +template +static Stream& operator<<(Stream& s, const ServiceCallResult& scr) +{ + s << "# Service call result [" << DataType::getDataTypeFullName() << "] " + << (scr.isSuccessful() ? "OK" : "FAILURE") + << " server_node_id=" << int(scr.getCallID().server_node_id.get()) + << " tid=" << int(scr.getCallID().transfer_id.get()) << "\n"; + if (scr.isSuccessful()) + { + s << scr.getResponse(); + } + else + { + s << "# (no data)"; + } + return s; +} + +/** + * Do not use directly. + */ +class ServiceClientBase : protected ITransferAcceptanceFilter + , protected DeadlineHandler +{ + const DataTypeDescriptor* data_type_descriptor_; ///< This will be initialized at the time of first call + +protected: + class CallState : DeadlineHandler + { + ServiceClientBase& owner_; + const ServiceCallID id_; + bool timed_out_; + + virtual void handleDeadline(MonotonicTime) override; + + public: + CallState(INode& node, ServiceClientBase& owner, ServiceCallID call_id) + : DeadlineHandler(node.getScheduler()) + , owner_(owner) + , id_(call_id) + , timed_out_(false) + { + UAVCAN_ASSERT(id_.isValid()); + DeadlineHandler::startWithDelay(owner_.request_timeout_); + } + + ServiceCallID getCallID() const { return id_; } + + bool hasTimedOut() const { return timed_out_; } + + static bool hasTimedOutPredicate(const CallState& cs) { return cs.hasTimedOut(); } + + bool operator==(const CallState& rhs) const + { + return (&owner_ == &rhs.owner_) && (id_ == rhs.id_); + } + }; + + struct CallStateMatchingPredicate + { + const ServiceCallID id; + CallStateMatchingPredicate(ServiceCallID reference) : id(reference) { } + bool operator()(const CallState& state) const { return (state.getCallID() == id) && !state.hasTimedOut(); } + }; + + struct ServerSearchPredicate + { + const NodeID server_node_id; + ServerSearchPredicate(NodeID nid) : server_node_id(nid) { } + bool operator()(const CallState& state) const { return state.getCallID().server_node_id == server_node_id; } + }; + + MonotonicDuration request_timeout_; + + ServiceClientBase(INode& node) + : DeadlineHandler(node.getScheduler()) + , data_type_descriptor_(UAVCAN_NULLPTR) + , request_timeout_(getDefaultRequestTimeout()) + { } + + virtual ~ServiceClientBase() { } + + int prepareToCall(INode& node, const char* dtname, NodeID server_node_id, ServiceCallID& out_call_id); + +public: + /** + * It's not recommended to override default timeouts. + * Change of this value will not affect pending calls. + */ + static MonotonicDuration getDefaultRequestTimeout() { return MonotonicDuration::fromMSec(1000); } + static MonotonicDuration getMinRequestTimeout() { return MonotonicDuration::fromMSec(10); } + static MonotonicDuration getMaxRequestTimeout() { return MonotonicDuration::fromMSec(60000); } +}; + +/** + * Use this class to invoke services on remote nodes. + * + * This class can manage multiple concurrent calls to the same or different remote servers. Number of concurrent + * calls is limited only by amount of available pool memory. + * + * Note that the reference passed to the callback points to a stack-allocated object, which means that the + * reference invalidates once the callback returns. If you want to use this object after the callback execution, + * you need to copy it somewhere. + * + * Note that by default, service client objects use lower priority than message publishers. Use @ref setPriority() + * to override the default if necessary. + * + * @tparam DataType_ Service data type. + * + * @tparam Callback_ Service response will be delivered through the callback of this type. + * In C++11 mode this type defaults to std::function<>. + * In C++03 mode this type defaults to a plain function pointer; use binder to + * call member functions as callbacks. + */ +template = UAVCAN_CPP11 + typename Callback_ = std::function&)> +#else + typename Callback_ = void (*)(const ServiceCallResult&) +#endif + > +class UAVCAN_EXPORT ServiceClient + : public GenericSubscriber + , public ServiceClientBase +{ +public: + typedef DataType_ DataType; + typedef typename DataType::Request RequestType; + typedef typename DataType::Response ResponseType; + typedef ServiceCallResult ServiceCallResultType; + typedef Callback_ Callback; + +private: + typedef ServiceClient SelfType; + typedef GenericPublisher PublisherType; + typedef GenericSubscriber SubscriberType; + + typedef Multiset CallRegistry; + + struct TimeoutCallbackCaller + { + ServiceClient& owner; + + TimeoutCallbackCaller(ServiceClient& arg_owner) : owner(arg_owner) { } + + void operator()(const CallState& state) + { + if (state.hasTimedOut()) + { + UAVCAN_TRACE("ServiceClient::TimeoutCallbackCaller", "Timeout from nid=%d, tid=%d, dtname=%s", + int(state.getCallID().server_node_id.get()), int(state.getCallID().transfer_id.get()), + DataType::getDataTypeFullName()); + + typename SubscriberType::ReceivedDataStructureSpec rx_struct; // Default-initialized + + ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, state.getCallID(), + rx_struct); // Mutable! + + owner.invokeCallback(result); + } + } + }; + + CallRegistry call_registry_; + + PublisherType publisher_; + Callback callback_; + + virtual bool shouldAcceptFrame(const RxFrame& frame) const override; // Called from the transfer listener + + void invokeCallback(ServiceCallResultType& result); + + virtual void handleReceivedDataStruct(ReceivedDataStructure& response) override; + + virtual void handleDeadline(MonotonicTime) override; + + int addCallState(ServiceCallID call_id); + +public: + /** + * @param node Node instance this client will be registered with. + * @param callback Callback instance. Optional, can be assigned later. + */ + explicit ServiceClient(INode& node, const Callback& callback = Callback()) + : SubscriberType(node) + , ServiceClientBase(node) + , call_registry_(node.getAllocator()) + , publisher_(node, getDefaultRequestTimeout()) + , callback_(callback) + { + setPriority(TransferPriority::MiddleLower); + setRequestTimeout(getDefaultRequestTimeout()); +#if UAVCAN_DEBUG + UAVCAN_ASSERT(getRequestTimeout() == getDefaultRequestTimeout()); // Making sure default values are OK +#endif + } + + virtual ~ServiceClient() { cancelAllCalls(); } + + /** + * Shall be called before first use. + * Returns negative error code. + */ + int init() + { + return publisher_.init(); + } + + /** + * Shall be called before first use. + * This overload allows to set the priority, otherwise it's the same. + * Returns negative error code. + */ + int init(TransferPriority priority) + { + return publisher_.init(priority); + } + + /** + * Performs non-blocking service call. + * This method transmits the service request and returns immediately. + * + * Service response will be delivered into the application via the callback. + * Note that the callback will ALWAYS be called even if the service call times out; the + * actual result of the call (success/failure) will be passed to the callback as well. + * + * Returns negative error code. + */ + int call(NodeID server_node_id, const RequestType& request); + + /** + * Same as plain @ref call() above, but this overload also returns the call ID of the new call. + * The call ID structure can be used to cancel this request later if needed. + */ + int call(NodeID server_node_id, const RequestType& request, ServiceCallID& out_call_id); + + /** + * Cancels certain call referred via call ID structure. + */ + void cancelCall(ServiceCallID call_id); + + /** + * Cancels all pending calls. + */ + void cancelAllCalls(); + + /** + * Checks whether there's currently a pending call addressed to the specified node ID. + */ + bool hasPendingCallToServer(NodeID server_node_id) const; + + /** + * This method allows to traverse pending calls. If the index is out of range, an invalid call ID will be returned. + * Warning: average complexity is O(index); worst case complexity is O(size). + */ + ServiceCallID getCallIDByIndex(unsigned index) const; + + /** + * Service response callback must be set prior service call. + */ + const Callback& getCallback() const { return callback_; } + void setCallback(const Callback& cb) { callback_ = cb; } + + /** + * Complexity is O(N) of number of pending calls. + * Note that the number of pending calls will not be updated until the callback is executed. + */ + unsigned getNumPendingCalls() const { return call_registry_.getSize(); } + + /** + * Complexity is O(1). + * Note that the number of pending calls will not be updated until the callback is executed. + */ + bool hasPendingCalls() const { return !call_registry_.isEmpty(); } + + /** + * Returns the number of failed attempts to decode received response. Generally, a failed attempt means either: + * - Transient failure in the transport layer. + * - Incompatible data types. + */ + uint32_t getResponseFailureCount() const { return SubscriberType::getFailureCount(); } + + /** + * Request timeouts. Note that changing the request timeout will not affect calls that are already pending. + * There is no such config as TX timeout - TX timeouts are configured automagically according to request timeouts. + * Not recommended to change. + */ + MonotonicDuration getRequestTimeout() const { return request_timeout_; } + void setRequestTimeout(MonotonicDuration timeout) + { + timeout = max(timeout, getMinRequestTimeout()); + timeout = min(timeout, getMaxRequestTimeout()); + + publisher_.setTxTimeout(timeout); + request_timeout_ = max(timeout, publisher_.getTxTimeout()); // No less than TX timeout + } + + /** + * Priority of outgoing request transfers. + * The remote server is supposed to use the same priority for the response, but it's not guaranteed by + * the specification. + */ + TransferPriority getPriority() const { return publisher_.getPriority(); } + void setPriority(const TransferPriority prio) { publisher_.setPriority(prio); } +}; + +// ---------------------------------------------------------------------------- + +template +void ServiceClient::invokeCallback(ServiceCallResultType& result) +{ + if (coerceOrFallback(callback_, true)) + { + callback_(result); + } + else + { + handleFatalError("Srv client clbk"); + } +} + +template +bool ServiceClient::shouldAcceptFrame(const RxFrame& frame) const +{ + UAVCAN_ASSERT(frame.getTransferType() == TransferTypeServiceResponse); // Other types filtered out by dispatcher + + return UAVCAN_NULLPTR != call_registry_.find(CallStateMatchingPredicate(ServiceCallID(frame.getSrcNodeID(), + frame.getTransferID()))); + +} + +template +void ServiceClient::handleReceivedDataStruct(ReceivedDataStructure& response) +{ + UAVCAN_ASSERT(response.getTransferType() == TransferTypeServiceResponse); + + ServiceCallID call_id(response.getSrcNodeID(), response.getTransferID()); + cancelCall(call_id); + ServiceCallResultType result(ServiceCallResultType::Success, call_id, response); // Mutable! + invokeCallback(result); +} + + +template +void ServiceClient::handleDeadline(MonotonicTime) +{ + UAVCAN_TRACE("ServiceClient", "Shared deadline event received"); + /* + * Invoking callbacks for timed out call state objects. + */ + TimeoutCallbackCaller callback_caller(*this); + call_registry_.template forEach(callback_caller); + /* + * Removing timed out objects. + * This operation cannot be merged with the previous one because that will not work with recursive calls. + */ + call_registry_.removeAllWhere(&CallState::hasTimedOutPredicate); + /* + * Subscriber does not need to be registered if we don't have any pending calls. + * Removing it makes processing of incoming frames a bit faster. + */ + if (call_registry_.isEmpty()) + { + SubscriberType::stop(); + } +} + +template +int ServiceClient::addCallState(ServiceCallID call_id) +{ + if (call_registry_.isEmpty()) + { + const int subscriber_res = SubscriberType::startAsServiceResponseListener(); + if (subscriber_res < 0) + { + UAVCAN_TRACE("ServiceClient", "Failed to start the subscriber, error: %i", subscriber_res); + return subscriber_res; + } + } + + if (UAVCAN_NULLPTR == call_registry_.template emplace(SubscriberType::getNode(), *this, call_id)) + { + SubscriberType::stop(); + return -ErrMemory; + } + + return 0; +} + +template +int ServiceClient::call(NodeID server_node_id, const RequestType& request) +{ + ServiceCallID dummy; + return call(server_node_id, request, dummy); +} + +template +int ServiceClient::call(NodeID server_node_id, const RequestType& request, + ServiceCallID& out_call_id) +{ + if (!coerceOrFallback(callback_, true)) + { + UAVCAN_TRACE("ServiceClient", "Invalid callback"); + return -ErrInvalidConfiguration; + } + + /* + * Common procedures that don't depend on the struct data type + */ + const int prep_res = + prepareToCall(SubscriberType::getNode(), DataType::getDataTypeFullName(), server_node_id, out_call_id); + if (prep_res < 0) + { + UAVCAN_TRACE("ServiceClient", "Failed to prepare the call, error: %i", prep_res); + return prep_res; + } + + /* + * Initializing the call state - this will start the subscriber ad-hoc + */ + const int call_state_res = addCallState(out_call_id); + if (call_state_res < 0) + { + UAVCAN_TRACE("ServiceClient", "Failed to add call state, error: %i", call_state_res); + return call_state_res; + } + + /* + * Configuring the listener so it will accept only the matching responses + * TODO move to init(), but this requires to somewhat refactor GenericSubscriber<> (remove TransferForwarder) + */ + TransferListenerWithFilter* const tl = SubscriberType::getTransferListener(); + if (tl == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); // Must have been created + cancelCall(out_call_id); + return -ErrLogic; + } + tl->installAcceptanceFilter(this); + + /* + * Publishing the request + */ + const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id, + out_call_id.transfer_id); + if (publisher_res < 0) + { + cancelCall(out_call_id); + return publisher_res; + } + + UAVCAN_ASSERT(server_node_id == out_call_id.server_node_id); + return publisher_res; +} + +template +void ServiceClient::cancelCall(ServiceCallID call_id) +{ + call_registry_.removeFirstWhere(CallStateMatchingPredicate(call_id)); + if (call_registry_.isEmpty()) + { + SubscriberType::stop(); + } +} + +template +void ServiceClient::cancelAllCalls() +{ + call_registry_.clear(); + SubscriberType::stop(); +} + +template +bool ServiceClient::hasPendingCallToServer(NodeID server_node_id) const +{ + return UAVCAN_NULLPTR != call_registry_.find(ServerSearchPredicate(server_node_id)); +} + +template +ServiceCallID ServiceClient::getCallIDByIndex(unsigned index) const +{ + const CallState* const id = call_registry_.getByIndex(index); + return (id == UAVCAN_NULLPTR) ? ServiceCallID() : id->getCallID(); +} + +} + +#endif // UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/service_server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/service_server.hpp new file mode 100644 index 000000000000..574aaabf1915 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/service_server.hpp @@ -0,0 +1,201 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_SERVICE_SERVER_HPP_INCLUDED +#define UAVCAN_NODE_SERVICE_SERVER_HPP_INCLUDED + +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ +/** + * This type can be used in place of the response type in a service server callback to get more advanced control + * of service request processing. + * + * PLEASE NOTE that since this class inherits the response type, service server callbacks can accept either + * object of this class or the response type directly if the extra options are not needed. + * + * For example, both of these callbacks can be used with the same service type 'Foo': + * + * void first(const ReceivedDataStructure& request, + * ServiceResponseDataStructure& response); + * + * void second(const Foo::Request& request, + * Foo::Response& response); + * + * In the latter case, an implicit cast will happen before the callback is invoked. + */ +template +class ServiceResponseDataStructure : public ResponseDataType_ +{ + // Fields are weirdly named to avoid name clashing with the inherited data type + bool _enabled_; + +public: + typedef ResponseDataType_ ResponseDataType; + + ServiceResponseDataStructure() + : _enabled_(true) + { } + + /** + * When disabled, the server will not transmit the response transfer. + * By default it is enabled, i.e. response will be sent. + */ + void setResponseEnabled(bool x) { _enabled_ = x; } + + /** + * Whether the response will be sent. By default it will. + */ + bool isResponseEnabled() const { return _enabled_; } +}; + +/** + * Use this class to implement UAVCAN service servers. + * + * Note that the references passed to the callback may point to stack-allocated objects, which means that the + * references get invalidated once the callback returns. + * + * @tparam DataType_ Service data type. + * + * @tparam Callback_ Service calls will be delivered through the callback of this type, and service + * response will be returned via the output parameter of the callback. Note that + * the reference to service response data struct passed to the callback always points + * to a default initialized response object. + * Please also refer to @ref ReceivedDataStructure<> and @ref ServiceResponseDataStructure<>. + * In C++11 mode this type defaults to std::function<>. + * In C++03 mode this type defaults to a plain function pointer; use binder to + * call member functions as callbacks. + */ +template = UAVCAN_CPP11 + typename Callback_ = std::function&, + ServiceResponseDataStructure&)> +#else + typename Callback_ = void (*)(const ReceivedDataStructure&, + ServiceResponseDataStructure&) +#endif + > +class UAVCAN_EXPORT ServiceServer + : public GenericSubscriber +{ +public: + typedef DataType_ DataType; + typedef typename DataType::Request RequestType; + typedef typename DataType::Response ResponseType; + typedef Callback_ Callback; + +private: + typedef GenericSubscriber SubscriberType; + typedef GenericPublisher PublisherType; + + PublisherType publisher_; + Callback callback_; + uint32_t response_failure_count_; + + virtual void handleReceivedDataStruct(ReceivedDataStructure& request) override + { + UAVCAN_ASSERT(request.getTransferType() == TransferTypeServiceRequest); + + ServiceResponseDataStructure response; + + if (coerceOrFallback(callback_, true)) + { + UAVCAN_ASSERT(response.isResponseEnabled()); // Enabled by default + callback_(request, response); + } + else + { + handleFatalError("Srv serv clbk"); + } + + if (response.isResponseEnabled()) + { + publisher_.setPriority(request.getPriority()); // Responding at the same priority. + + const int res = publisher_.publish(response, TransferTypeServiceResponse, request.getSrcNodeID(), + request.getTransferID()); + if (res < 0) + { + UAVCAN_TRACE("ServiceServer", "Response publication failure: %i", res); + publisher_.getNode().getDispatcher().getTransferPerfCounter().addError(); + response_failure_count_++; + } + } + else + { + UAVCAN_TRACE("ServiceServer", "Response was suppressed by the application"); + } + } + +public: + explicit ServiceServer(INode& node) + : SubscriberType(node) + , publisher_(node, getDefaultTxTimeout()) + , callback_() + , response_failure_count_(0) + { + UAVCAN_ASSERT(getTxTimeout() == getDefaultTxTimeout()); // Making sure it is valid + + StaticAssert::check(); + } + + /** + * Starts the server. + * Incoming service requests will be passed to the application via the callback. + */ + int start(const Callback& callback) + { + stop(); + + if (!coerceOrFallback(callback, true)) + { + UAVCAN_TRACE("ServiceServer", "Invalid callback"); + return -ErrInvalidParam; + } + callback_ = callback; + + const int publisher_res = publisher_.init(); + if (publisher_res < 0) + { + UAVCAN_TRACE("ServiceServer", "Publisher initialization failure: %i", publisher_res); + return publisher_res; + } + return SubscriberType::startAsServiceRequestListener(); + } + + /** + * Stops the server. + */ + using SubscriberType::stop; + + static MonotonicDuration getDefaultTxTimeout() { return MonotonicDuration::fromMSec(1000); } + static MonotonicDuration getMinTxTimeout() { return PublisherType::getMinTxTimeout(); } + static MonotonicDuration getMaxTxTimeout() { return PublisherType::getMaxTxTimeout(); } + + MonotonicDuration getTxTimeout() const { return publisher_.getTxTimeout(); } + void setTxTimeout(MonotonicDuration tx_timeout) { publisher_.setTxTimeout(tx_timeout); } + + /** + * Returns the number of failed attempts to decode data structs. Generally, a failed attempt means either: + * - Transient failure in the transport layer. + * - Incompatible data types. + */ + uint32_t getRequestFailureCount() const { return SubscriberType::getFailureCount(); } + uint32_t getResponseFailureCount() const { return response_failure_count_; } +}; + +} + +#endif // UAVCAN_NODE_SERVICE_SERVER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/sub_node.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/sub_node.hpp new file mode 100644 index 000000000000..35b80492e51f --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/sub_node.hpp @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_SUB_NODE_NODE_HPP_INCLUDED +#define UAVCAN_SUB_NODE_NODE_HPP_INCLUDED + +#include +#include +#include + +#if UAVCAN_TINY +# error "This functionality is not available in tiny mode" +#endif + +namespace uavcan +{ +/** + * This node object can be used in multiprocess UAVCAN nodes. + * Please refer to the @ref Node<> for documentation concerning the template arguments; refer to the tutorials + * to lean how to use libuavcan in multiprocess applications. + */ +template +class UAVCAN_EXPORT SubNode : public INode +{ + typedef typename + Select<(MemPoolSize > 0), + PoolAllocator, // If pool size is specified, use default allocator + IPoolAllocator& // Otherwise use reference to user-provided allocator + >::Result Allocator; + + Allocator pool_allocator_; + Scheduler scheduler_; + + uint64_t internal_failure_cnt_; + +protected: + virtual void registerInternalFailure(const char* msg) + { + internal_failure_cnt_++; + UAVCAN_TRACE("Node", "Internal failure: %s", msg); + (void)msg; + } + +public: + /** + * This overload is only valid if MemPoolSize > 0. + */ + SubNode(ICanDriver& can_driver, + ISystemClock& system_clock) : + scheduler_(can_driver, pool_allocator_, system_clock), + internal_failure_cnt_(0) + { } + + /** + * This overload is only valid if MemPoolSize == 0. + */ + SubNode(ICanDriver& can_driver, + ISystemClock& system_clock, + IPoolAllocator& allocator) : + pool_allocator_(allocator), + scheduler_(can_driver, pool_allocator_, system_clock), + internal_failure_cnt_(0) + { } + + virtual typename RemoveReference::Type& getAllocator() { return pool_allocator_; } + + virtual Scheduler& getScheduler() { return scheduler_; } + virtual const Scheduler& getScheduler() const { return scheduler_; } + + uint64_t getInternalFailureCount() const { return internal_failure_cnt_; } +}; + +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/subscriber.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/subscriber.hpp new file mode 100644 index 000000000000..a28a9f037e5d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/subscriber.hpp @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_SUBSCRIBER_HPP_INCLUDED +#define UAVCAN_NODE_SUBSCRIBER_HPP_INCLUDED + +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ +/** + * Use this class to subscribe to a message. + * + * @tparam DataType_ Message data type. + * + * @tparam Callback_ Type of the callback that will be used to deliver received messages + * into the application. Type of the argument of the callback can be either: + * - DataType_& + * - const DataType_& + * - ReceivedDataStructure& + * - const ReceivedDataStructure& + * For the first two options, @ref ReceivedDataStructure<> will be casted implicitly. + * In C++11 mode this type defaults to std::function<>. + * In C++03 mode this type defaults to a plain function pointer; use binder to + * call member functions as callbacks. + */ +template = UAVCAN_CPP11 + typename Callback_ = std::function&)> +#else + typename Callback_ = void (*)(const ReceivedDataStructure&) +#endif + > +class UAVCAN_EXPORT Subscriber + : public GenericSubscriber +{ +public: + typedef Callback_ Callback; + +private: + typedef GenericSubscriber BaseType; + + Callback callback_; + + virtual void handleReceivedDataStruct(ReceivedDataStructure& msg) override + { + if (coerceOrFallback(callback_, true)) + { + callback_(msg); + } + else + { + handleFatalError("Sub clbk"); + } + } + +public: + typedef DataType_ DataType; + + explicit Subscriber(INode& node) + : BaseType(node) + , callback_() + { + StaticAssert::check(); + } + + /** + * Begin receiving messages. + * Each message will be passed to the application via the callback. + * Returns negative error code. + */ + int start(const Callback& callback) + { + stop(); + + if (!coerceOrFallback(callback, true)) + { + UAVCAN_TRACE("Subscriber", "Invalid callback"); + return -ErrInvalidParam; + } + callback_ = callback; + + return BaseType::startAsMessageListener(); + } + + using BaseType::allowAnonymousTransfers; + using BaseType::stop; + using BaseType::getFailureCount; +}; + +} + +#endif // UAVCAN_NODE_SUBSCRIBER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/timer.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/timer.hpp new file mode 100644 index 000000000000..3973146ff345 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/timer.hpp @@ -0,0 +1,148 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_TIMER_HPP_INCLUDED +#define UAVCAN_NODE_TIMER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +#if !defined(UAVCAN_CPP11) || !defined(UAVCAN_CPP_VERSION) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ + +class UAVCAN_EXPORT TimerBase; + +/** + * Objects of this type will be supplied into timer callbacks. + */ +struct UAVCAN_EXPORT TimerEvent +{ + MonotonicTime scheduled_time; ///< Time when the timer callback was expected to be invoked + MonotonicTime real_time; ///< True time when the timer callback was invoked + + TimerEvent(MonotonicTime arg_scheduled_time, MonotonicTime arg_real_time) + : scheduled_time(arg_scheduled_time) + , real_time(arg_real_time) + { } +}; + +/** + * Inherit this class if you need a timer callback method in your class. + */ +class UAVCAN_EXPORT TimerBase : private DeadlineHandler +{ + MonotonicDuration period_; + + virtual void handleDeadline(MonotonicTime current) override; + +public: + using DeadlineHandler::stop; + using DeadlineHandler::isRunning; + using DeadlineHandler::getDeadline; + using DeadlineHandler::getScheduler; + + explicit TimerBase(INode& node) + : DeadlineHandler(node.getScheduler()) + , period_(MonotonicDuration::getInfinite()) + { } + + /** + * Various ways to start the timer - periodically or once. + * If it is running already, it will be restarted. + * If the deadline is in the past, the event will fire immediately. + * In periodic mode the timer does not accumulate error over time. + */ + void startOneShotWithDeadline(MonotonicTime deadline); + void startOneShotWithDelay(MonotonicDuration delay); + void startPeriodic(MonotonicDuration period); + + /** + * Returns period if the timer is in periodic mode. + * Returns infinite duration if the timer is in one-shot mode or stopped. + */ + MonotonicDuration getPeriod() const { return period_; } + + /** + * Implement this method in your class to receive callbacks. + */ + virtual void handleTimerEvent(const TimerEvent& event) = 0; +}; + +/** + * Wrapper over TimerBase that forwards callbacks into arbitrary handlers, like + * functor objects, member functions or static functions. + * + * Callback must be set before the first event; otherwise the event will generate a fatal error. + * + * Also take a look at @ref MethodBinder<>, which may come useful if C++11 features are not available. + * + * @tparam Callback_ Callback type. Shall accept const reference to TimerEvent as its argument. + */ +template +class UAVCAN_EXPORT TimerEventForwarder : public TimerBase +{ +public: + typedef Callback_ Callback; + +private: + Callback callback_; + + virtual void handleTimerEvent(const TimerEvent& event) + { + if (coerceOrFallback(callback_, true)) + { + callback_(event); + } + else + { + handleFatalError("Invalid timer callback"); + } + } + +public: + explicit TimerEventForwarder(INode& node) + : TimerBase(node) + , callback_() + { } + + TimerEventForwarder(INode& node, const Callback& callback) + : TimerBase(node) + , callback_(callback) + { } + + /** + * Get/set the callback object. + * Callback must be set before the first event happens; otherwise the event will generate a fatal error. + */ + const Callback& getCallback() const { return callback_; } + void setCallback(const Callback& callback) { callback_ = callback; } +}; + + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +/** + * Use this timer in C++11 mode. + * Callback type is std::function<>. + */ +typedef TimerEventForwarder > Timer; + +#endif + +} + +#endif // UAVCAN_NODE_TIMER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/README.md b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/README.md new file mode 100644 index 000000000000..1b29688f366b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/README.md @@ -0,0 +1,13 @@ +High-level protocol logic +========================= + +Classes defined in this directory implement some high-level functions of UAVCAN. + +Since most applications won't use all of them at the same time, their definitions are provided right in the header +files rather than in source files, in order to not pollute the build outputs with unused code (which is also a +requirement for most safety-critical software). + +However, some of the high-level functions that are either always used by the library itself or those that are extremely +likely to be used by the application are defined in .cpp files. + +When adding a new class here, please think twice before putting its definition into a .cpp file. diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp new file mode 100644 index 000000000000..3da0ca790b81 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp @@ -0,0 +1,137 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DATA_TYPE_INFO_PROVIDER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DATA_TYPE_INFO_PROVIDER_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class implements the standard services for data type introspection. + * Once started it does not require any attention from the application. + * The user does not need to deal with it directly - it's started by the node class. + */ +class UAVCAN_EXPORT DataTypeInfoProvider : Noncopyable +{ + typedef MethodBinder GetDataTypeInfoCallback; + + ServiceServer gdti_srv_; + + INode& getNode() { return gdti_srv_.getNode(); } + + static bool isValidDataTypeKind(DataTypeKind kind) + { + return (kind == DataTypeKindMessage) || (kind == DataTypeKindService); + } + + void handleGetDataTypeInfoRequest(const protocol::GetDataTypeInfo::Request& request, + protocol::GetDataTypeInfo::Response& response) + { + /* + * Asking the Global Data Type Registry for the matching type descriptor, either by name or by ID + */ + const DataTypeDescriptor* desc = UAVCAN_NULLPTR; + + if (request.name.empty()) + { + response.id = request.id; // Pre-setting the fields so they have meaningful values even in + response.kind = request.kind; // ...case of failure. + + if (!isValidDataTypeKind(DataTypeKind(request.kind.value))) + { + UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request with invalid DataTypeKind %i", + static_cast(request.kind.value)); + return; + } + + desc = GlobalDataTypeRegistry::instance().find(DataTypeKind(request.kind.value), request.id); + } + else + { + response.name = request.name; + + desc = GlobalDataTypeRegistry::instance().find(request.name.c_str()); + } + + if (desc == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("DataTypeInfoProvider", + "Cannot process GetDataTypeInfo for nonexistent type: dtid=%i dtk=%i name='%s'", + static_cast(request.id), static_cast(request.kind.value), request.name.c_str()); + return; + } + + UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request for %s", desc->toString().c_str()); + + /* + * Filling the response struct + */ + response.signature = desc->getSignature().get(); + response.id = desc->getID().get(); + response.kind.value = desc->getKind(); + response.flags = protocol::GetDataTypeInfo::Response::FLAG_KNOWN; + response.name = desc->getFullName(); + + const Dispatcher& dispatcher = getNode().getDispatcher(); + + if (desc->getKind() == DataTypeKindService) + { + if (dispatcher.hasServer(desc->getID().get())) + { + response.flags |= protocol::GetDataTypeInfo::Response::FLAG_SERVING; + } + } + else if (desc->getKind() == DataTypeKindMessage) + { + if (dispatcher.hasSubscriber(desc->getID().get())) + { + response.flags |= protocol::GetDataTypeInfo::Response::FLAG_SUBSCRIBED; + } + if (dispatcher.hasPublisher(desc->getID().get())) + { + response.flags |= protocol::GetDataTypeInfo::Response::FLAG_PUBLISHING; + } + } + else + { + UAVCAN_ASSERT(0); // That means that GDTR somehow found a type of an unknown kind. The horror. + } + } + +public: + explicit DataTypeInfoProvider(INode& node) : + gdti_srv_(node) + { } + + int start() + { + int res = 0; + + res = gdti_srv_.start(GetDataTypeInfoCallback(this, &DataTypeInfoProvider::handleGetDataTypeInfoRequest)); + if (res < 0) + { + goto fail; + } + + UAVCAN_ASSERT(res >= 0); + return res; + + fail: + UAVCAN_ASSERT(res < 0); + gdti_srv_.stop(); + return res; + } +}; + +} + +#endif // UAVCAN_PROTOCOL_DATA_TYPE_INFO_PROVIDER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp new file mode 100644 index 000000000000..31e2c16a3dbf --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class implements client-side logic of dynamic node ID allocation procedure. + * + * Once started, the object will be publishing dynamic node ID allocation requests at the default frequency defined + * by the specification, until a Node ID is granted by the allocator. + * + * If the local node is equipped with redundant CAN interfaces, all of them will be used for publishing requests + * and listening for responses. + * + * Once dynamic allocation is complete (or not needed anymore), the object can be deleted. + * + * Note that this class uses std::rand(), which must be correctly seeded before use. + */ +class UAVCAN_EXPORT DynamicNodeIDClient : private TimerBase +{ + typedef MethodBinder&)> + AllocationCallback; + + enum Mode + { + ModeWaitingForTimeSlot, + ModeDelayBeforeFollowup, + NumModes + }; + + Publisher dnida_pub_; + Subscriber dnida_sub_; + + uint8_t unique_id_[protocol::HardwareVersion::FieldTypes::unique_id::MaxSize]; + uint8_t size_of_received_unique_id_; + + NodeID preferred_node_id_; + NodeID allocated_node_id_; + NodeID allocator_node_id_; + + void terminate(); + + static MonotonicDuration getRandomDuration(uint32_t lower_bound_msec, uint32_t upper_bound_msec); + + void restartTimer(const Mode mode); + + virtual void handleTimerEvent(const TimerEvent&) override; + + void handleAllocation(const ReceivedDataStructure& msg); + +public: + typedef protocol::HardwareVersion::FieldTypes::unique_id UniqueID; + + DynamicNodeIDClient(INode& node) + : TimerBase(node) + , dnida_pub_(node) + , dnida_sub_(node) + , size_of_received_unique_id_(0) + { } + + /** + * @param unique_id Unique ID of the local node. Must be the same as in the hardware version struct. + * @param preferred_node_id Node ID that the application would like to take; set to broadcast (zero) if + * the application doesn't have any preference (this is default). + * @param transfer_priority Transfer priority, Normal by default. + * @return Zero on success + * Negative error code on failure + * -ErrLogic if 1. the node is not in passive mode or 2. the client is already started + */ + int start(const UniqueID& unique_id, + const NodeID preferred_node_id = NodeID::Broadcast, + const TransferPriority transfer_priority = TransferPriority::OneHigherThanLowest); + + /** + * Use this method to determine when allocation is complete. + */ + bool isAllocationComplete() const { return getAllocatedNodeID().isUnicast(); } + + /** + * This method allows to retrieve the node ID that was allocated to the local node. + * If no node ID was allocated yet, the returned node ID will be invalid (non-unicast). + * @return If allocation is complete, a valid unicast node ID will be returned. + * If allocation is not complete yet, a non-unicast node ID will be returned. + */ + NodeID getAllocatedNodeID() const { return allocated_node_id_; } + + /** + * This method allows to retrieve node ID of the allocator that granted our Node ID. + * If no node ID was allocated yet, the returned node ID will be invalid (non-unicast). + * @return If allocation is complete, a valid unicast node ID will be returned. + * If allocation is not complete yet, an non-unicast node ID will be returned. + */ + NodeID getAllocatorNodeID() const { return allocator_node_id_; } +}; + +} + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/abstract_server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/abstract_server.hpp new file mode 100644 index 000000000000..cf61e8771a96 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/abstract_server.hpp @@ -0,0 +1,114 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ + +class AbstractServer : protected IAllocationRequestHandler + , protected INodeDiscoveryHandler +{ + UniqueID own_unique_id_; + MonotonicTime started_at_; + +protected: + INode& node_; + IEventTracer& tracer_; + AllocationRequestManager allocation_request_manager_; + NodeDiscoverer node_discoverer_; + + AbstractServer(INode& node, + IEventTracer& tracer) : + node_(node), + tracer_(tracer), + allocation_request_manager_(node, tracer, *this), + node_discoverer_(node, tracer, *this) + { } + + const UniqueID& getOwnUniqueID() const { return own_unique_id_; } + + int init(const UniqueID& own_unique_id, const TransferPriority priority) + { + int res = 0; + + own_unique_id_ = own_unique_id; + + res = allocation_request_manager_.init(priority); + if (res < 0) + { + return res; + } + + res = node_discoverer_.init(priority); + if (res < 0) + { + return res; + } + + started_at_ = node_.getMonotonicTime(); + + return 0; + } + +public: + /** + * This can be used to guess if there are any un-allocated dynamic nodes left in the network. + */ + bool guessIfAllDynamicNodesAreAllocated( + const MonotonicDuration& allocation_activity_timeout = + MonotonicDuration::fromMSec(Allocation::MAX_REQUEST_PERIOD_MS * 2), + const MonotonicDuration& min_uptime = MonotonicDuration::fromMSec(6000)) const + { + const MonotonicTime ts = node_.getMonotonicTime(); + + /* + * If uptime is not large enough, the allocator may be unaware about some nodes yet. + */ + const MonotonicDuration uptime = ts - started_at_; + if (uptime < max(allocation_activity_timeout, min_uptime)) + { + return false; + } + + /* + * If there are any undiscovered nodes, assume that allocation is still happening. + */ + if (node_discoverer_.hasUnknownNodes()) + { + return false; + } + + /* + * Lastly, check if there wasn't any allocation messages detected on the bus in the specified amount of time. + */ + const MonotonicDuration since_allocation_activity = + ts - allocation_request_manager_.getTimeOfLastAllocationActivity(); + if (since_allocation_activity < allocation_activity_timeout) + { + return false; + } + + return true; + } + + /** + * This is useful for debugging/testing/monitoring. + */ + const NodeDiscoverer& getNodeDiscoverer() const { return node_discoverer_; } +}; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp new file mode 100644 index 000000000000..a16346b9a04c --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -0,0 +1,286 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * The main allocator must implement this interface. + */ +class IAllocationRequestHandler +{ +public: + /** + * Allocation request manager uses this method to detect if it is allowed to publish follow-up responses. + */ + virtual bool canPublishFollowupAllocationResponse() const = 0; + + /** + * This method will be invoked when a new allocation request is received. + */ + virtual void handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) = 0; + + virtual ~IAllocationRequestHandler() { } +}; + +/** + * This class manages communication with allocation clients. + * Three-stage unique ID exchange is implemented here, as well as response publication. + */ +class AllocationRequestManager +{ + typedef MethodBinder&)> + AllocationCallback; + + const MonotonicDuration stage_timeout_; + + MonotonicTime last_message_timestamp_; + MonotonicTime last_activity_timestamp_; + Allocation::FieldTypes::unique_id current_unique_id_; + + IAllocationRequestHandler& handler_; + IEventTracer& tracer_; + + Subscriber allocation_sub_; + Publisher allocation_pub_; + + enum { InvalidStage = 0 }; + + void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); } + + static uint8_t detectRequestStage(const Allocation& msg) + { + const uint8_t max_bytes_per_request = Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST; + + if ((msg.unique_id.size() != max_bytes_per_request) && + (msg.unique_id.size() != (msg.unique_id.capacity() - max_bytes_per_request * 2U)) && + (msg.unique_id.size() != msg.unique_id.capacity())) // Future proofness for CAN FD + { + return InvalidStage; + } + if (msg.first_part_of_unique_id) + { + return 1; // Note that CAN FD frames can deliver the unique ID in one stage! + } + if (msg.unique_id.size() == Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + { + return 2; + } + if (msg.unique_id.size() < Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + { + return 3; + } + return InvalidStage; + } + + uint8_t getExpectedStage() const + { + if (current_unique_id_.empty()) + { + return 1; + } + if (current_unique_id_.size() >= (Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2)) + { + return 3; + } + if (current_unique_id_.size() >= Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + { + return 2; + } + return InvalidStage; + } + + void publishFollowupAllocationResponse() + { + Allocation msg; + msg.unique_id = current_unique_id_; + UAVCAN_ASSERT(msg.unique_id.size() < msg.unique_id.capacity()); + + UAVCAN_TRACE("AllocationRequestManager", "Intermediate response with %u bytes of unique ID", + unsigned(msg.unique_id.size())); + + trace(TraceAllocationFollowupResponse, msg.unique_id.size()); + + const int res = allocation_pub_.broadcast(msg); + if (res < 0) + { + trace(TraceError, res); + allocation_pub_.getNode().registerInternalFailure("Dynamic allocation broadcast"); + } + } + + void handleAllocation(const ReceivedDataStructure& msg) + { + trace(TraceAllocationActivity, msg.getSrcNodeID().get()); + last_activity_timestamp_ = msg.getMonotonicTimestamp(); + + if (!msg.isAnonymousTransfer()) + { + return; // This is a response from another allocator, ignore + } + + /* + * Reset the expected stage on timeout + */ + if (msg.getMonotonicTimestamp() > (last_message_timestamp_ + stage_timeout_)) + { + UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset"); + current_unique_id_.clear(); + trace(TraceAllocationFollowupTimeout, (msg.getMonotonicTimestamp() - last_message_timestamp_).toUSec()); + } + + /* + * Checking if request stage matches the expected stage + */ + const uint8_t request_stage = detectRequestStage(msg); + if (request_stage == InvalidStage) + { + trace(TraceAllocationBadRequest, msg.unique_id.size()); + return; // Malformed request - ignore without resetting + } + + const uint8_t expected_stage = getExpectedStage(); + if (expected_stage == InvalidStage) + { + UAVCAN_ASSERT(0); + return; + } + + if (request_stage != expected_stage) + { + trace(TraceAllocationUnexpectedStage, request_stage); + return; // Ignore - stage mismatch + } + + const uint8_t max_expected_bytes = + static_cast(current_unique_id_.capacity() - current_unique_id_.size()); + UAVCAN_ASSERT(max_expected_bytes > 0); + if (msg.unique_id.size() > max_expected_bytes) + { + trace(TraceAllocationBadRequest, msg.unique_id.size()); + return; // Malformed request + } + + /* + * Updating the local state + */ + for (uint8_t i = 0; i < msg.unique_id.size(); i++) + { + current_unique_id_.push_back(msg.unique_id[i]); + } + + trace(TraceAllocationRequestAccepted, current_unique_id_.size()); + + /* + * Proceeding with allocation if possible + * Note that single-frame CAN FD allocation requests will be delivered to the server even if it's not leader. + */ + if (current_unique_id_.size() == current_unique_id_.capacity()) + { + UAVCAN_TRACE("AllocationRequestManager", "Allocation request received; preferred node ID: %d", + int(msg.node_id)); + + UniqueID unique_id; + copy(current_unique_id_.begin(), current_unique_id_.end(), unique_id.begin()); + current_unique_id_.clear(); + + { + uint64_t event_agrument = 0; + for (uint8_t i = 0; i < 8; i++) + { + event_agrument |= static_cast(unique_id[i]) << (56U - i * 8U); + } + trace(TraceAllocationExchangeComplete, static_cast(event_agrument)); + } + + handler_.handleAllocationRequest(unique_id, msg.node_id); + } + else + { + if (handler_.canPublishFollowupAllocationResponse()) + { + publishFollowupAllocationResponse(); + } + else + { + trace(TraceAllocationFollowupDenied, 0); + current_unique_id_.clear(); + } + } + + /* + * It is super important to update timestamp only if the request has been processed successfully. + */ + last_message_timestamp_ = msg.getMonotonicTimestamp(); + } + +public: + AllocationRequestManager(INode& node, IEventTracer& tracer, IAllocationRequestHandler& handler) + : stage_timeout_(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS)) + , handler_(handler) + , tracer_(tracer) + , allocation_sub_(node) + , allocation_pub_(node) + { } + + int init(const TransferPriority priority) + { + int res = allocation_pub_.init(priority); + if (res < 0) + { + return res; + } + allocation_pub_.setTxTimeout(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS)); + + res = allocation_sub_.start(AllocationCallback(this, &AllocationRequestManager::handleAllocation)); + if (res < 0) + { + return res; + } + allocation_sub_.allowAnonymousTransfers(); + + return 0; + } + + int broadcastAllocationResponse(const UniqueID& unique_id, NodeID allocated_node_id) + { + Allocation msg; + + msg.unique_id.resize(msg.unique_id.capacity()); + copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin()); + + msg.node_id = allocated_node_id.get(); + + trace(TraceAllocationResponse, msg.node_id); + last_activity_timestamp_ = allocation_pub_.getNode().getMonotonicTime(); + + return allocation_pub_.broadcast(msg); + } + + /** + * When the last allocation activity was registered. + * This value can be used to heuristically determine whether there are any unallocated nodes left in the network. + */ + MonotonicTime getTimeOfLastAllocationActivity() const { return last_activity_timestamp_; } +}; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp new file mode 100644 index 000000000000..e0f9e5bcae3a --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp @@ -0,0 +1,20 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED + +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ + +typedef centralized::Server CentralizedServer; + +} +} + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp new file mode 100644 index 000000000000..fea436bfb704 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp @@ -0,0 +1,180 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace centralized +{ +/** + * This server is an alternative to @ref DistributedServer with the following differences: + * - It is not distributed, so using it means introducing a single point of failure into the system. + * - It takes less code space and requires less RAM, which makes it suitable for resource-constrained applications. + * + * This version is suitable only for simple non-critical systems. + */ +class Server : public AbstractServer +{ + Storage storage_; + + /* + * Private methods + */ + bool isNodeIDTaken(const NodeID node_id) const + { + return storage_.isNodeIDOccupied(node_id); + } + + void tryPublishAllocationResult(const NodeID node_id, const UniqueID& unique_id) + { + const int res = allocation_request_manager_.broadcastAllocationResponse(unique_id, node_id); + if (res < 0) + { + tracer_.onEvent(TraceError, res); + node_.registerInternalFailure("Dynamic allocation response"); + } + } + + /* + * Methods of IAllocationRequestHandler + */ + virtual bool canPublishFollowupAllocationResponse() const override + { + return true; // Because there's only one Centralized server in the system + } + + virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id) override + { + const NodeID existing_node_id = storage_.getNodeIDForUniqueID(unique_id); + if (existing_node_id.isValid()) + { + tryPublishAllocationResult(existing_node_id, unique_id); + } + else + { + const NodeID allocated_node_id = + NodeIDSelector(this, &Server::isNodeIDTaken).findFreeNodeID(preferred_node_id); + + if (allocated_node_id.isUnicast()) + { + const int res = storage_.add(allocated_node_id, unique_id); + if (res >= 0) + { + tryPublishAllocationResult(allocated_node_id, unique_id); + } + else + { + tracer_.onEvent(TraceError, res); + node_.registerInternalFailure("CentralizedServer storage add"); + } + } + else + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "Request ignored - no free node ID left"); + } + } + } + + /* + * Methods of INodeDiscoveryHandler + */ + virtual bool canDiscoverNewNodes() const override + { + return true; // Because there's only one Centralized server in the system + } + + virtual NodeAwareness checkNodeAwareness(NodeID node_id) const override + { + return storage_.isNodeIDOccupied(node_id) ? NodeAwarenessKnownAndCommitted : NodeAwarenessUnknown; + } + + virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) override + { + if (storage_.isNodeIDOccupied(node_id)) + { + UAVCAN_ASSERT(0); // Such node is already known, the class that called this method should have known that + return; + } + + const int res = storage_.add(node_id, (unique_id_or_null == UAVCAN_NULLPTR) ? UniqueID() : *unique_id_or_null); + if (res < 0) + { + tracer_.onEvent(TraceError, res); + node_.registerInternalFailure("CentralizedServer storage add"); + } + } + +public: + Server(INode& node, + IStorageBackend& storage, + IEventTracer& tracer) + : AbstractServer(node, tracer) + , storage_(storage) + { } + + int init(const UniqueID& own_unique_id, + const TransferPriority priority = TransferPriority::OneHigherThanLowest) + { + /* + * Initializing storage first, because the next step requires it to be loaded + */ + int res = storage_.init(); + if (res < 0) + { + return res; + } + + /* + * Common logic + */ + res = AbstractServer::init(own_unique_id, priority); + if (res < 0) + { + return res; + } + + /* + * Making sure that the server is started with the same node ID + */ + { + const NodeID stored_own_node_id = storage_.getNodeIDForUniqueID(getOwnUniqueID()); + if (stored_own_node_id.isValid()) + { + if (stored_own_node_id != node_.getNodeID()) + { + return -ErrInvalidConfiguration; + } + } + else + { + res = storage_.add(node_.getNodeID(), getOwnUniqueID()); + if (res < 0) + { + return res; + } + } + } + + return 0; + } + + uint8_t getNumAllocations() const { return storage_.getSize(); } +}; + +} +} +} + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp new file mode 100644 index 000000000000..51123a896300 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp @@ -0,0 +1,150 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_STORAGE_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_STORAGE_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace centralized +{ +/** + * This class transparently replicates its state to the storage backend, keeping the most recent state in memory. + * Writes are slow, reads are instantaneous. + */ +class Storage +{ + typedef BitSet OccupationMask; + typedef Array, ArrayModeStatic, + BitLenToByteLen::Result> + OccupationMaskArray; + + IStorageBackend& storage_; + OccupationMask occupation_mask_; + + static IStorageBackend::String getOccupationMaskKey() { return "occupation_mask"; } + + static OccupationMask maskFromArray(const OccupationMaskArray& array) + { + OccupationMask mask; + for (uint8_t byte = 0; byte < array.size(); byte++) + { + for (uint8_t bit = 0; bit < 8; bit++) + { + mask[byte * 8U + bit] = (array[byte] & (1U << bit)) != 0; + } + } + return mask; + } + + static OccupationMaskArray maskToArray(const OccupationMask& mask) + { + OccupationMaskArray array; + for (uint8_t byte = 0; byte < array.size(); byte++) + { + for (uint8_t bit = 0; bit < 8; bit++) + { + if (mask[byte * 8U + bit]) + { + array[byte] = static_cast(array[byte] | (1U << bit)); + } + } + } + return array; + } + +public: + Storage(IStorageBackend& storage) : + storage_(storage) + { } + + /** + * This method reads only the occupation mask from the storage. + */ + int init() + { + StorageMarshaller io(storage_); + OccupationMaskArray array; + io.get(getOccupationMaskKey(), array); + occupation_mask_ = maskFromArray(array); + return 0; + } + + /** + * This method invokes storage IO. + * Returned value indicates whether the entry was successfully appended. + */ + int add(const NodeID node_id, const UniqueID& unique_id) + { + if (!node_id.isUnicast()) + { + return -ErrInvalidParam; + } + + StorageMarshaller io(storage_); + + // If next operations fail, we'll get a dangling entry, but it's absolutely OK. + { + uint32_t node_id_int = node_id.get(); + int res = io.setAndGetBack(StorageMarshaller::convertUniqueIDToHex(unique_id), node_id_int); + if (res < 0) + { + return res; + } + if (node_id_int != node_id.get()) + { + return -ErrFailure; + } + } + + // Updating the mask in the storage + OccupationMask new_occupation_mask = occupation_mask_; + new_occupation_mask[node_id.get()] = true; + OccupationMaskArray occupation_array = maskToArray(new_occupation_mask); + + int res = io.setAndGetBack(getOccupationMaskKey(), occupation_array); + if (res < 0) + { + return res; + } + if (occupation_array != maskToArray(new_occupation_mask)) + { + return -ErrFailure; + } + + // Updating the cached mask only if the storage was updated successfully + occupation_mask_ = new_occupation_mask; + + return 0; + } + + /** + * Returns an invalid node ID if there's no such allocation. + */ + NodeID getNodeIDForUniqueID(const UniqueID& unique_id) const + { + StorageMarshaller io(storage_); + uint32_t node_id = 0; + io.get(StorageMarshaller::convertUniqueIDToHex(unique_id), node_id); + return (node_id > 0 && node_id <= NodeID::Max) ? NodeID(static_cast(node_id)) : NodeID(); + } + + bool isNodeIDOccupied(NodeID node_id) const { return occupation_mask_[node_id.get()]; } + + uint8_t getSize() const { return static_cast(occupation_mask_.count()); } +}; + +} +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp new file mode 100644 index 000000000000..822f757522c1 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp @@ -0,0 +1,20 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED + +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ + +typedef distributed::Server DistributedServer; + +} +} + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp new file mode 100644 index 000000000000..b4e573b29d93 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp @@ -0,0 +1,433 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_CLUSTER_MANAGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_CLUSTER_MANAGER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * This class maintains the cluster state. + */ +class ClusterManager : private TimerBase +{ +public: + enum { MaxClusterSize = Discovery::FieldTypes::known_nodes::MaxSize }; + +private: + typedef MethodBinder&)> + DiscoveryCallback; + + struct Server + { + NodeID node_id; + Log::Index next_index; + Log::Index match_index; + + Server() + : next_index(0) + , match_index(0) + { } + + void resetIndices(const Log& log) + { + next_index = Log::Index(log.getLastIndex() + 1U); + match_index = 0; + } + }; + + IStorageBackend& storage_; + IEventTracer& tracer_; + const Log& log_; + + Subscriber discovery_sub_; + mutable Publisher discovery_pub_; + + Server servers_[MaxClusterSize - 1]; ///< Minus one because the local server is not listed there. + + uint8_t cluster_size_; + uint8_t num_known_servers_; + + static IStorageBackend::String getStorageKeyForClusterSize() { return "cluster_size"; } + + INode& getNode() { return discovery_sub_.getNode(); } + const INode& getNode() const { return discovery_sub_.getNode(); } + + const Server* findServer(NodeID node_id) const { return const_cast(this)->findServer(node_id); } + Server* findServer(NodeID node_id) + { + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + if (servers_[i].node_id == node_id) + { + return &servers_[i]; + } + } + return UAVCAN_NULLPTR; + } + + virtual void handleTimerEvent(const TimerEvent&) + { + UAVCAN_ASSERT(num_known_servers_ < cluster_size_); + + tracer_.onEvent(TraceRaftDiscoveryBroadcast, num_known_servers_); + + /* + * Filling the message + */ + Discovery msg; + msg.configured_cluster_size = cluster_size_; + + msg.known_nodes.push_back(getNode().getNodeID().get()); // Putting ourselves at index 0 + + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + msg.known_nodes.push_back(servers_[i].node_id.get()); + } + + UAVCAN_ASSERT(msg.known_nodes.size() == (num_known_servers_ + 1)); + + /* + * Broadcasting + */ + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", + "Broadcasting Discovery message; known nodes: %d of %d", + int(msg.known_nodes.size()), int(cluster_size_)); + + const int res = discovery_pub_.broadcast(msg); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Discovery broadcst failed: %d", res); + getNode().registerInternalFailure("Raft discovery broadcast"); + } + + /* + * Termination condition + */ + if (isClusterDiscovered()) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", + "Discovery broadcasting timer stopped"); + stop(); + } + } + + void handleDiscovery(const ReceivedDataStructure& msg) + { + tracer_.onEvent(TraceRaftDiscoveryReceived, msg.getSrcNodeID().get()); + + /* + * Validating cluster configuration + * If there's a case of misconfiguration, the message will be ignored. + */ + if (msg.configured_cluster_size != cluster_size_) + { + tracer_.onEvent(TraceRaftBadClusterSizeReceived, msg.configured_cluster_size); + getNode().registerInternalFailure("Bad Raft cluster size"); + return; + } + + /* + * Updating the set of known servers + */ + for (uint8_t i = 0; i < msg.known_nodes.size(); i++) + { + if (isClusterDiscovered()) + { + break; + } + + const NodeID node_id(msg.known_nodes[i]); + if (node_id.isUnicast() && !isKnownServer(node_id)) + { + addServer(node_id); + } + } + + /* + * Publishing a new Discovery request if the publishing server needs to learn about more servers. + */ + if (msg.configured_cluster_size > msg.known_nodes.size()) + { + startDiscoveryPublishingTimerIfNotRunning(); + } + } + + void startDiscoveryPublishingTimerIfNotRunning() + { + if (!isRunning()) + { + startPeriodic(MonotonicDuration::fromMSec(Discovery::BROADCASTING_PERIOD_MS)); + } + } + +public: + enum { ClusterSizeUnknown = 0 }; + + /** + * @param node Needed to publish and subscribe to Discovery message + * @param storage Needed to read the cluster size parameter from the storage + * @param log Needed to initialize nextIndex[] values after elections + */ + ClusterManager(INode& node, IStorageBackend& storage, const Log& log, IEventTracer& tracer) + : TimerBase(node) + , storage_(storage) + , tracer_(tracer) + , log_(log) + , discovery_sub_(node) + , discovery_pub_(node) + , cluster_size_(0) + , num_known_servers_(0) + { } + + /** + * If cluster_size is set to ClusterSizeUnknown, the class will try to read this parameter from the + * storage backend using key 'cluster_size'. + * Returns negative error code. + */ + int init(const uint8_t init_cluster_size, const TransferPriority priority) + { + /* + * Figuring out the cluster size + */ + if (init_cluster_size == ClusterSizeUnknown) + { + // Reading from the storage + StorageMarshaller io(storage_); + uint32_t value = 0; + int res = io.get(getStorageKeyForClusterSize(), value); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", + "Cluster size is neither configured nor stored in the storage"); + return res; + } + if ((value == 0) || (value > MaxClusterSize)) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Cluster size is invalid"); + return -ErrInvalidConfiguration; + } + cluster_size_ = static_cast(value); + } + else + { + if ((init_cluster_size == 0) || (init_cluster_size > MaxClusterSize)) + { + return -ErrInvalidParam; + } + cluster_size_ = init_cluster_size; + + // Writing the storage + StorageMarshaller io(storage_); + uint32_t value = init_cluster_size; + int res = io.setAndGetBack(getStorageKeyForClusterSize(), value); + if ((res < 0) || (value != init_cluster_size)) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Failed to store cluster size"); + return -ErrFailure; + } + } + + tracer_.onEvent(TraceRaftClusterSizeInited, cluster_size_); + + UAVCAN_ASSERT(cluster_size_ > 0); + UAVCAN_ASSERT(cluster_size_ <= MaxClusterSize); + + /* + * Initializing pub/sub and timer + */ + int res = discovery_pub_.init(priority); + if (res < 0) + { + return res; + } + + res = discovery_sub_.start(DiscoveryCallback(this, &ClusterManager::handleDiscovery)); + if (res < 0) + { + return res; + } + + startDiscoveryPublishingTimerIfNotRunning(); + + /* + * Misc + */ + resetAllServerIndices(); + return 0; + } + + /** + * Adds once server regardless of the discovery logic. + */ + void addServer(NodeID node_id) + { + UAVCAN_ASSERT((num_known_servers_ + 1) < MaxClusterSize); + if (!isKnownServer(node_id) && node_id.isUnicast()) + { + tracer_.onEvent(TraceRaftNewServerDiscovered, node_id.get()); + servers_[num_known_servers_].node_id = node_id; + servers_[num_known_servers_].resetIndices(log_); + num_known_servers_ = static_cast(num_known_servers_ + 1U); + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * Whether such server has been discovered. + */ + bool isKnownServer(NodeID node_id) const + { + if (node_id == getNode().getNodeID()) + { + return true; + } + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + UAVCAN_ASSERT(servers_[i].node_id != getNode().getNodeID()); + if (servers_[i].node_id == node_id) + { + return true; + } + } + return false; + } + + /** + * An invalid node ID will be returned if there's no such server. + * The local server is not listed there. + */ + NodeID getRemoteServerNodeIDAtIndex(uint8_t index) const + { + if (index < num_known_servers_) + { + return servers_[index].node_id; + } + return NodeID(); + } + + /** + * See next_index[] in Raft paper. + */ + Log::Index getServerNextIndex(NodeID server_node_id) const + { + const Server* const s = findServer(server_node_id); + if (s != UAVCAN_NULLPTR) + { + return s->next_index; + } + UAVCAN_ASSERT(0); + return 0; + } + + void incrementServerNextIndexBy(NodeID server_node_id, Log::Index increment) + { + Server* const s = findServer(server_node_id); + if (s != UAVCAN_NULLPTR) + { + s->next_index = Log::Index(s->next_index + increment); + } + else + { + UAVCAN_ASSERT(0); + } + } + + void decrementServerNextIndex(NodeID server_node_id) + { + Server* const s = findServer(server_node_id); + if (s != UAVCAN_NULLPTR) + { + s->next_index--; + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * See match_index[] in Raft paper. + */ + Log::Index getServerMatchIndex(NodeID server_node_id) const + { + const Server* const s = findServer(server_node_id); + if (s != UAVCAN_NULLPTR) + { + return s->match_index; + } + UAVCAN_ASSERT(0); + return 0; + } + + void setServerMatchIndex(NodeID server_node_id, Log::Index match_index) + { + Server* const s = findServer(server_node_id); + if (s != UAVCAN_NULLPTR) + { + s->match_index = match_index; + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * This method must be called when the current server becomes leader. + */ + void resetAllServerIndices() + { + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + servers_[i].resetIndices(log_); + } + } + + /** + * Number of known servers can only grow, and it never exceeds the cluster size value. + * This number does not include the local server. + */ + uint8_t getNumKnownServers() const { return num_known_servers_; } + + /** + * Cluster size and quorum size are constant. + */ + uint8_t getClusterSize() const { return cluster_size_; } + uint8_t getQuorumSize() const { return static_cast(cluster_size_ / 2U + 1U); } + + bool isClusterDiscovered() const { return num_known_servers_ == (cluster_size_ - 1); } +}; + +} +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp new file mode 100644 index 000000000000..3ef89b31ee81 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp @@ -0,0 +1,310 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_LOG_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_LOG_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * Raft log. + * This class transparently replicates its state to the storage backend, keeping the most recent state in memory. + * Writes are slow, reads are instantaneous. + */ +class Log +{ +public: + typedef uint8_t Index; + + enum { Capacity = NodeID::Max + 1 }; + +private: + IStorageBackend& storage_; + IEventTracer& tracer_; + Entry entries_[Capacity]; + Index last_index_; // Index zero always contains an empty entry + + static IStorageBackend::String getLastIndexKey() { return "log_last_index"; } + + static IStorageBackend::String makeEntryKey(Index index, const char* postfix) + { + IStorageBackend::String str; + // "log0_foobar" + str += "log"; + str.appendFormatted("%d", int(index)); + str += "_"; + str += postfix; + return str; + } + + int readEntryFromStorage(Index index, Entry& out_entry) + { + const StorageMarshaller io(storage_); + + // Term + if (io.get(makeEntryKey(index, "term"), out_entry.term) < 0) + { + return -ErrFailure; + } + + // Unique ID + if (io.get(makeEntryKey(index, "unique_id"), out_entry.unique_id) < 0) + { + return -ErrFailure; + } + + // Node ID + uint32_t node_id = 0; + if (io.get(makeEntryKey(index, "node_id"), node_id) < 0) + { + return -ErrFailure; + } + if (node_id > NodeID::Max) + { + return -ErrFailure; + } + out_entry.node_id = static_cast(node_id); + + return 0; + } + + int writeEntryToStorage(Index index, const Entry& entry) + { + Entry temp = entry; + + StorageMarshaller io(storage_); + + // Term + if (io.setAndGetBack(makeEntryKey(index, "term"), temp.term) < 0) + { + return -ErrFailure; + } + + // Unique ID + if (io.setAndGetBack(makeEntryKey(index, "unique_id"), temp.unique_id) < 0) + { + return -ErrFailure; + } + + // Node ID + uint32_t node_id = entry.node_id; + if (io.setAndGetBack(makeEntryKey(index, "node_id"), node_id) < 0) + { + return -ErrFailure; + } + temp.node_id = static_cast(node_id); + + return (temp == entry) ? 0 : -ErrFailure; + } + + int initEmptyLogStorage() + { + StorageMarshaller io(storage_); + + /* + * Writing the zero entry - it must always be default-initialized + */ + entries_[0] = Entry(); + int res = writeEntryToStorage(0, entries_[0]); + if (res < 0) + { + return res; + } + + /* + * Initializing last index + * Last index must be written AFTER the zero entry, otherwise if the write fails here the storage will be + * left in an inconsistent state. + */ + last_index_ = 0; + uint32_t stored_index = 0; + res = io.setAndGetBack(getLastIndexKey(), stored_index); + if (res < 0) + { + return res; + } + if (stored_index != 0) + { + return -ErrFailure; + } + + return 0; + } + +public: + Log(IStorageBackend& storage, IEventTracer& tracer) + : storage_(storage) + , tracer_(tracer) + , last_index_(0) + { } + + int init() + { + StorageMarshaller io(storage_); + + // Reading max index + { + uint32_t value = 0; + if (io.get(getLastIndexKey(), value) < 0) + { + if (storage_.get(getLastIndexKey()).empty()) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Initializing empty storage"); + return initEmptyLogStorage(); + } + else + { + // There's some data in the storage, but it cannot be parsed - reporting an error + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Failed to read last index"); + return -ErrFailure; + } + } + if (value >= Capacity) + { + return -ErrFailure; + } + last_index_ = Index(value); + } + + tracer_.onEvent(TraceRaftLogLastIndexRestored, last_index_); + + // Restoring log entries - note that index 0 always exists + for (Index index = 0; index <= last_index_; index++) + { + const int result = readEntryFromStorage(index, entries_[index]); + if (result < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Failed to read entry at index %u: %d", + unsigned(index), result); + return result; + } + } + + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Restored %u log entries", unsigned(last_index_)); + return 0; + } + + /** + * This method invokes storage IO. + * Returned value indicates whether the entry was successfully appended. + */ + int append(const Entry& entry) + { + if ((last_index_ + 1) >= Capacity) + { + return -ErrLogic; + } + + tracer_.onEvent(TraceRaftLogAppend, last_index_ + 1U); + + // If next operations fail, we'll get a dangling entry, but it's absolutely OK. + int res = writeEntryToStorage(Index(last_index_ + 1), entry); + if (res < 0) + { + return res; + } + + // Updating the last index + StorageMarshaller io(storage_); + uint32_t new_last_index = last_index_ + 1U; + res = io.setAndGetBack(getLastIndexKey(), new_last_index); + if (res < 0) + { + return res; + } + if (new_last_index != last_index_ + 1U) + { + return -ErrFailure; + } + entries_[new_last_index] = entry; + last_index_ = Index(new_last_index); + + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "New entry, index %u, node ID %u, term %u", + unsigned(last_index_), unsigned(entry.node_id), unsigned(entry.term)); + return 0; + } + + /** + * This method invokes storage IO. + * Returned value indicates whether the requested operation has been carried out successfully. + */ + int removeEntriesWhereIndexGreaterOrEqual(Index index) + { + UAVCAN_ASSERT(last_index_ < Capacity); + + if (((index) >= Capacity) || (index <= 0)) + { + return -ErrLogic; + } + + uint32_t new_last_index = index - 1U; + + tracer_.onEvent(TraceRaftLogRemove, new_last_index); + + if (new_last_index != last_index_) + { + StorageMarshaller io(storage_); + int res = io.setAndGetBack(getLastIndexKey(), new_last_index); + if (res < 0) + { + return res; + } + if (new_last_index != index - 1U) + { + return -ErrFailure; + } + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Entries removed, last index %u --> %u", + unsigned(last_index_), unsigned(new_last_index)); + last_index_ = Index(new_last_index); + } + + // Removal operation leaves dangling entries in storage, it's OK + return 0; + } + + int removeEntriesWhereIndexGreater(Index index) + { + return removeEntriesWhereIndexGreaterOrEqual(Index(index + 1U)); + } + + /** + * Returns nullptr if there's no such index. + * This method does not use storage IO. + */ + const Entry* getEntryAtIndex(Index index) const + { + UAVCAN_ASSERT(last_index_ < Capacity); + return (index <= last_index_) ? &entries_[index] : UAVCAN_NULLPTR; + } + + Index getLastIndex() const { return last_index_; } + + bool isOtherLogUpToDate(Index other_last_index, Term other_last_term) const + { + UAVCAN_ASSERT(last_index_ < Capacity); + // Terms are different - the one with higher term is more up-to-date + if (other_last_term != entries_[last_index_].term) + { + return other_last_term > entries_[last_index_].term; + } + // Terms are equal - longer log wins + return other_last_index >= last_index_; + } +}; + +} +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp new file mode 100644 index 000000000000..a1ca44ea7990 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp @@ -0,0 +1,237 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_PERSISTENT_STATE_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_PERSISTENT_STATE_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * This class is a convenient container for persistent state variables defined by Raft. + * Writes are slow, reads are instantaneous. + */ +class PersistentState +{ + IStorageBackend& storage_; + IEventTracer& tracer_; + + Term current_term_; + NodeID voted_for_; + Log log_; + + static IStorageBackend::String getCurrentTermKey() { return "current_term"; } + static IStorageBackend::String getVotedForKey() { return "voted_for"; } + +public: + PersistentState(IStorageBackend& storage, IEventTracer& tracer) + : storage_(storage) + , tracer_(tracer) + , current_term_(0) + , log_(storage, tracer) + { } + + /** + * Initialization is performed as follows (every step may fail and return an error): + * 1. Log is restored or initialized. + * 2. Current term is restored. If there was no current term stored and the log is empty, it will be initialized + * with zero. + * 3. VotedFor value is restored. If there was no VotedFor value stored, the log is empty, and the current term is + * zero, the value will be initialized with zero. + */ + int init() + { + /* + * Reading log + */ + int res = log_.init(); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", "Log init failed: %d", res); + return res; + } + + const Entry* const last_entry = log_.getEntryAtIndex(log_.getLastIndex()); + if (last_entry == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + + const bool log_is_empty = (log_.getLastIndex() == 0) && (last_entry->term == 0); + + StorageMarshaller io(storage_); + + /* + * Reading currentTerm + */ + if (storage_.get(getCurrentTermKey()).empty() && log_is_empty) + { + // First initialization + current_term_ = 0; + res = io.setAndGetBack(getCurrentTermKey(), current_term_); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", + "Failed to init current term: %d", res); + return res; + } + if (current_term_ != 0) + { + return -ErrFailure; + } + } + else + { + // Restoring + res = io.get(getCurrentTermKey(), current_term_); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", + "Failed to read current term: %d", res); + return res; + } + } + + tracer_.onEvent(TraceRaftCurrentTermRestored, current_term_); + + if (current_term_ < last_entry->term) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", + "Persistent storage is damaged: current term is less than term of the last log entry (%u < %u)", + unsigned(current_term_), unsigned(last_entry->term)); + return -ErrLogic; + } + + /* + * Reading votedFor + */ + if (storage_.get(getVotedForKey()).empty() && log_is_empty && (current_term_ == 0)) + { + // First initialization + voted_for_ = NodeID(0); + uint32_t stored_voted_for = 0; + res = io.setAndGetBack(getVotedForKey(), stored_voted_for); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", + "Failed to init votedFor: %d", res); + return res; + } + if (stored_voted_for != 0) + { + return -ErrFailure; + } + } + else + { + // Restoring + uint32_t stored_voted_for = 0; + res = io.get(getVotedForKey(), stored_voted_for); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", + "Failed to read votedFor: %d", res); + return res; + } + if (stored_voted_for > NodeID::Max) + { + return -ErrInvalidConfiguration; + } + voted_for_ = NodeID(uint8_t(stored_voted_for)); + } + + tracer_.onEvent(TraceRaftVotedForRestored, voted_for_.get()); + + return 0; + } + + Term getCurrentTerm() const { return current_term_; } + + NodeID getVotedFor() const { return voted_for_; } + bool isVotedForSet() const { return voted_for_.isUnicast(); } + + Log& getLog() { return log_; } + const Log& getLog() const { return log_; } + + /** + * Invokes storage IO. + */ + int setCurrentTerm(Term term) + { + if (term < current_term_) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + + tracer_.onEvent(TraceRaftCurrentTermUpdate, term); + + StorageMarshaller io(storage_); + + Term tmp = term; + int res = io.setAndGetBack(getCurrentTermKey(), tmp); + if (res < 0) + { + return res; + } + + if (tmp != term) + { + return -ErrFailure; + } + + current_term_ = term; + return 0; + } + + /** + * Invokes storage IO. + */ + int setVotedFor(NodeID node_id) + { + if (!node_id.isValid()) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + + tracer_.onEvent(TraceRaftVotedForUpdate, node_id.get()); + + StorageMarshaller io(storage_); + + uint32_t tmp = node_id.get(); + int res = io.setAndGetBack(getVotedForKey(), tmp); + if (res < 0) + { + return res; + } + + if (node_id.get() != tmp) + { + return -ErrFailure; + } + + voted_for_ = node_id; + return 0; + } + + int resetVotedFor() { return setVotedFor(NodeID(0)); } +}; + +} +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp new file mode 100644 index 000000000000..b446e3ee4523 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -0,0 +1,917 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_RAFT_CORE_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_RAFT_CORE_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * Allocator has to implement this interface so the RaftCore can inform it when a new entry gets committed to the log. + */ +class IRaftLeaderMonitor +{ +public: + /** + * This method will be invoked when a new log entry is committed (only if the local server is the current Leader). + */ + virtual void handleLogCommitOnLeader(const Entry& committed_entry) = 0; + + /** + * Invoked by the Raft core when the local node becomes a leader or ceases to be one. + * By default the local node is not leader. + * It is possible to commit to the log right from this method. + */ + virtual void handleLocalLeadershipChange(bool local_node_is_leader) = 0; + + virtual ~IRaftLeaderMonitor() { } +}; + +/** + * This class implements log replication and voting. + * It does not implement client-server interaction at all; instead it just exposes a public method for adding + * allocation entries. + * + * Note that this class uses std::rand(), so the RNG must be properly seeded by the application. + * + * Activity registration: + * - persistent state update error + * - switch to candidate (this defines timeout between reelections) + * - newer term in response (also switch to follower) + * - append entries request with term >= currentTerm + * - vote granted + */ +class RaftCore : private TimerBase +{ +public: + enum ServerState + { + ServerStateFollower, + ServerStateCandidate, + ServerStateLeader + }; + +private: + typedef MethodBinder&, + ServiceResponseDataStructure&)> + AppendEntriesCallback; + + typedef MethodBinder&)> + AppendEntriesResponseCallback; + + typedef MethodBinder&, + ServiceResponseDataStructure&)> + RequestVoteCallback; + + typedef MethodBinder&)> + RequestVoteResponseCallback; + + struct PendingAppendEntriesFields + { + Log::Index prev_log_index; + Log::Index num_entries; + + PendingAppendEntriesFields() + : prev_log_index(0) + , num_entries(0) + { } + }; + + /* + * Constants + */ + enum { MaxNumFollowers = ClusterManager::MaxClusterSize - 1 }; + + IEventTracer& tracer_; + IRaftLeaderMonitor& leader_monitor_; + + /* + * States + */ + PersistentState persistent_state_; + ClusterManager cluster_; + Log::Index commit_index_; + + MonotonicTime last_activity_timestamp_; + MonotonicDuration randomized_activity_timeout_; + ServerState server_state_; + + uint8_t next_server_index_; ///< Next server to query AE from + uint8_t num_votes_received_in_this_campaign_; + + PendingAppendEntriesFields pending_append_entries_fields_; + + /* + * Transport + */ + ServiceServer append_entries_srv_; + ServiceClient append_entries_client_; + ServiceServer request_vote_srv_; + ServiceClient request_vote_client_; + + /* + * Methods + */ + void trace(TraceCode event, int64_t argument) { tracer_.onEvent(event, argument); } + + INode& getNode() { return append_entries_srv_.getNode(); } + const INode& getNode() const { return append_entries_srv_.getNode(); } + + void checkInvariants() const + { + // Commit index + UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex()); + + // Term + UAVCAN_ASSERT(persistent_state_.getLog().getEntryAtIndex(persistent_state_.getLog().getLastIndex()) != + UAVCAN_NULLPTR); + UAVCAN_ASSERT(persistent_state_.getLog().getEntryAtIndex(persistent_state_.getLog().getLastIndex())->term <= + persistent_state_.getCurrentTerm()); + + // Elections + UAVCAN_ASSERT(server_state_ != ServerStateCandidate || !request_vote_client_.hasPendingCalls() || + persistent_state_.getVotedFor() == getNode().getNodeID()); + UAVCAN_ASSERT(num_votes_received_in_this_campaign_ <= cluster_.getClusterSize()); + + // Transport + UAVCAN_ASSERT(append_entries_client_.getNumPendingCalls() <= 1); + UAVCAN_ASSERT(request_vote_client_.getNumPendingCalls() <= cluster_.getNumKnownServers()); + UAVCAN_ASSERT(server_state_ != ServerStateCandidate || !append_entries_client_.hasPendingCalls()); + UAVCAN_ASSERT(server_state_ != ServerStateLeader || !request_vote_client_.hasPendingCalls()); + UAVCAN_ASSERT(server_state_ != ServerStateFollower || + (!append_entries_client_.hasPendingCalls() && !request_vote_client_.hasPendingCalls())); + } + + void registerActivity() + { + last_activity_timestamp_ = getNode().getMonotonicTime(); + + static const int32_t randomization_range_msec = AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS - + AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS; + // coverity[dont_call] + const int32_t random_msec = (std::rand() % randomization_range_msec) + 1; + + randomized_activity_timeout_ = + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS + random_msec); + + UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() > AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS); + UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() <= AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS); + } + + bool isActivityTimedOut() const + { + return getNode().getMonotonicTime() > (last_activity_timestamp_ + randomized_activity_timeout_); + } + + void handlePersistentStateUpdateError(int error) + { + UAVCAN_ASSERT(error < 0); + trace(TraceRaftPersistStateUpdateError, error); + switchState(ServerStateFollower); + registerActivity(); // Deferring reelections + } + + void updateFollower() + { + if (isActivityTimedOut()) + { + switchState(ServerStateCandidate); + registerActivity(); + } + } + + void updateCandidate() + { + if (num_votes_received_in_this_campaign_ > 0) + { + trace(TraceRaftElectionComplete, num_votes_received_in_this_campaign_); + const bool won = num_votes_received_in_this_campaign_ >= cluster_.getQuorumSize(); + + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "Election complete, won: %d", int(won)); + + switchState(won ? ServerStateLeader : ServerStateFollower); // Start over or become leader + } + else + { + // Set votedFor, abort on failure + int res = persistent_state_.setVotedFor(getNode().getNodeID()); + if (res < 0) + { + handlePersistentStateUpdateError(res); + return; + } + + // Increment current term, abort on failure + res = persistent_state_.setCurrentTerm(persistent_state_.getCurrentTerm() + 1U); + if (res < 0) + { + handlePersistentStateUpdateError(res); + return; + } + + num_votes_received_in_this_campaign_ = 1; // Voting for self + + RequestVote::Request req; + req.last_log_index = persistent_state_.getLog().getLastIndex(); + req.last_log_term = persistent_state_.getLog().getEntryAtIndex(req.last_log_index)->term; + req.term = persistent_state_.getCurrentTerm(); + + for (uint8_t i = 0; i < MaxNumFollowers; i++) + { + const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(i); + if (!node_id.isUnicast()) + { + break; + } + + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", + "Requesting vote from %d", int(node_id.get())); + trace(TraceRaftVoteRequestInitiation, node_id.get()); + + res = request_vote_client_.call(node_id, req); + if (res < 0) + { + trace(TraceError, res); + } + } + } + } + + void updateLeader() + { + if (append_entries_client_.hasPendingCalls()) + { + append_entries_client_.cancelAllCalls(); // Refer to the response callback to learn why + } + + if (cluster_.getClusterSize() > 1) + { + const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_); + UAVCAN_ASSERT(node_id.isUnicast()); + + next_server_index_++; + if (next_server_index_ >= cluster_.getNumKnownServers()) + { + next_server_index_ = 0; + } + + AppendEntries::Request req; + req.term = persistent_state_.getCurrentTerm(); + req.leader_commit = commit_index_; + + req.prev_log_index = Log::Index(cluster_.getServerNextIndex(node_id) - 1U); + + const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(req.prev_log_index); + if (entry == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + handlePersistentStateUpdateError(-ErrLogic); + return; + } + + req.prev_log_term = entry->term; + + for (Log::Index index = cluster_.getServerNextIndex(node_id); + index <= persistent_state_.getLog().getLastIndex(); + index++) + { + req.entries.push_back(*persistent_state_.getLog().getEntryAtIndex(index)); + if (req.entries.size() == req.entries.capacity()) + { + break; + } + } + + pending_append_entries_fields_.num_entries = req.entries.size(); + pending_append_entries_fields_.prev_log_index = req.prev_log_index; + + const int res = append_entries_client_.call(node_id, req); + if (res < 0) + { + trace(TraceRaftAppendEntriesCallFailure, res); + } + } + + propagateCommitIndex(); + } + + void switchState(ServerState new_state) + { + if (server_state_ == new_state) + { + return; + } + + /* + * Logging + */ + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "State switch: %d --> %d", + int(server_state_), int(new_state)); + trace(TraceRaftStateSwitch, new_state); + + /* + * Updating the current state + */ + const ServerState old_state = server_state_; + server_state_ = new_state; + + /* + * Resetting specific states + */ + cluster_.resetAllServerIndices(); + + next_server_index_ = 0; + num_votes_received_in_this_campaign_ = 0; + + request_vote_client_.cancelAllCalls(); + append_entries_client_.cancelAllCalls(); + + /* + * Calling the switch handler + * Note that the handler may commit to the log directly + */ + if ((old_state == ServerStateLeader) || + (new_state == ServerStateLeader)) + { + leader_monitor_.handleLocalLeadershipChange(new_state == ServerStateLeader); + } + } + + void tryIncrementCurrentTermFromResponse(Term new_term) + { + trace(TraceRaftNewerTermInResponse, new_term); + const int res = persistent_state_.setCurrentTerm(new_term); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + } + registerActivity(); // Deferring future elections + switchState(ServerStateFollower); + } + + void propagateCommitIndex() + { + // Objective is to estimate whether we can safely increment commit index value + UAVCAN_ASSERT(server_state_ == ServerStateLeader); + UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex()); + + if (commit_index_ < persistent_state_.getLog().getLastIndex()) + { + /* + * Not all local entries are committed. + * Deciding if it is safe to increment commit index. + */ + uint8_t num_nodes_with_next_log_entry_available = 1; // Local node + for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++) + { + const Log::Index match_index = cluster_.getServerMatchIndex(cluster_.getRemoteServerNodeIDAtIndex(i)); + if (match_index > commit_index_) + { + num_nodes_with_next_log_entry_available++; + } + } + + if (num_nodes_with_next_log_entry_available >= cluster_.getQuorumSize()) + { + commit_index_++; + UAVCAN_ASSERT(commit_index_ > 0); // Index 0 is always committed + trace(TraceRaftNewEntryCommitted, commit_index_); + + // AT THIS POINT ALLOCATION IS COMPLETE + leader_monitor_.handleLogCommitOnLeader(*persistent_state_.getLog().getEntryAtIndex(commit_index_)); + } + } + } + + void handleAppendEntriesRequest(const ReceivedDataStructure& request, + ServiceResponseDataStructure& response) + { + checkInvariants(); + + if (!cluster_.isKnownServer(request.getSrcNodeID())) + { + if (cluster_.isClusterDiscovered()) + { + trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); + response.setResponseEnabled(false); + return; + } + else + { + cluster_.addServer(request.getSrcNodeID()); + } + } + + UAVCAN_ASSERT(response.isResponseEnabled()); // This is default + + /* + * Checking if our current state is up to date. + * The request will be ignored if persistent state cannot be updated. + */ + if (request.term > persistent_state_.getCurrentTerm()) + { + int res = persistent_state_.setCurrentTerm(request.term); + if (res < 0) + { + handlePersistentStateUpdateError(res); + response.setResponseEnabled(false); + return; + } + + res = persistent_state_.resetVotedFor(); + if (res < 0) + { + handlePersistentStateUpdateError(res); + response.setResponseEnabled(false); + return; + } + } + + /* + * Preparing the response + */ + response.term = persistent_state_.getCurrentTerm(); + response.success = false; + + /* + * Step 1 (see Raft paper) + * Reject the request if the leader has stale term number. + */ + if (request.term < persistent_state_.getCurrentTerm()) + { + response.setResponseEnabled(true); + return; + } + + registerActivity(); + switchState(ServerStateFollower); + + /* + * Step 2 + * Reject the request if the assumed log index does not exist on the local node. + */ + const Entry* const prev_entry = persistent_state_.getLog().getEntryAtIndex(request.prev_log_index); + if (prev_entry == UAVCAN_NULLPTR) + { + response.setResponseEnabled(true); + return; + } + + /* + * Step 3 + * Drop log entries if term number does not match. + * Ignore the request if the persistent state cannot be updated. + */ + if (prev_entry->term != request.prev_log_term) + { + const int res = persistent_state_.getLog().removeEntriesWhereIndexGreaterOrEqual(request.prev_log_index); + response.setResponseEnabled(res >= 0); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + } + return; + } + + /* + * Step 4 + * Update the log with new entries - this will possibly require to rewrite existing entries. + * Ignore the request if the persistent state cannot be updated. + */ + if (request.prev_log_index != persistent_state_.getLog().getLastIndex()) + { + const int res = persistent_state_.getLog().removeEntriesWhereIndexGreater(request.prev_log_index); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + response.setResponseEnabled(false); + return; + } + } + + for (uint8_t i = 0; i < request.entries.size(); i++) + { + const int res = persistent_state_.getLog().append(request.entries[i]); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + response.setResponseEnabled(false); + return; // Response will not be sent, the server will assume that we're dead + } + } + + /* + * Step 5 + * Update the commit index. + */ + if (request.leader_commit > commit_index_) + { + commit_index_ = min(request.leader_commit, persistent_state_.getLog().getLastIndex()); + trace(TraceRaftCommitIndexUpdate, commit_index_); + } + + response.setResponseEnabled(true); + response.success = true; + } + + void handleAppendEntriesResponse(const ServiceCallResult& result) + { + UAVCAN_ASSERT(server_state_ == ServerStateLeader); // When state switches, all requests must be cancelled + checkInvariants(); + + if (!result.isSuccessful()) + { + return; + } + + if (result.getResponse().term > persistent_state_.getCurrentTerm()) + { + tryIncrementCurrentTermFromResponse(result.getResponse().term); + } + else + { + if (result.getResponse().success) + { + cluster_.incrementServerNextIndexBy(result.getCallID().server_node_id, + pending_append_entries_fields_.num_entries); + cluster_.setServerMatchIndex(result.getCallID().server_node_id, + Log::Index(pending_append_entries_fields_.prev_log_index + + pending_append_entries_fields_.num_entries)); + } + else + { + cluster_.decrementServerNextIndex(result.getCallID().server_node_id); + trace(TraceRaftAppendEntriesRespUnsucfl, result.getCallID().server_node_id.get()); + } + } + + pending_append_entries_fields_ = PendingAppendEntriesFields(); + // Rest of the logic is implemented in periodic update handlers. + } + + void handleRequestVoteRequest(const ReceivedDataStructure& request, + ServiceResponseDataStructure& response) + { + checkInvariants(); + trace(TraceRaftVoteRequestReceived, request.getSrcNodeID().get()); + + if (!cluster_.isKnownServer(request.getSrcNodeID())) + { + trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); + response.setResponseEnabled(false); + return; + } + + UAVCAN_ASSERT(response.isResponseEnabled()); // This is default + + /* + * Checking if our current state is up to date. + * The request will be ignored if persistent state cannot be updated. + */ + if (request.term > persistent_state_.getCurrentTerm()) + { + switchState(ServerStateFollower); // Our term is stale, so we can't serve as leader + + int res = persistent_state_.setCurrentTerm(request.term); + if (res < 0) + { + handlePersistentStateUpdateError(res); + response.setResponseEnabled(false); + return; + } + + res = persistent_state_.resetVotedFor(); + if (res < 0) + { + handlePersistentStateUpdateError(res); + response.setResponseEnabled(false); + return; + } + } + + /* + * Preparing the response + */ + response.term = persistent_state_.getCurrentTerm(); + + if (request.term < response.term) + { + response.vote_granted = false; + } + else + { + const bool can_vote = !persistent_state_.isVotedForSet() || + (persistent_state_.getVotedFor() == request.getSrcNodeID()); + const bool log_is_up_to_date = + persistent_state_.getLog().isOtherLogUpToDate(request.last_log_index, request.last_log_term); + + response.vote_granted = can_vote && log_is_up_to_date; + + if (response.vote_granted) + { + switchState(ServerStateFollower); // Avoiding race condition when Candidate + registerActivity(); // This is necessary to avoid excessive elections + + const int res = persistent_state_.setVotedFor(request.getSrcNodeID()); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + response.setResponseEnabled(false); + return; + } + } + } + } + + void handleRequestVoteResponse(const ServiceCallResult& result) + { + UAVCAN_ASSERT(server_state_ == ServerStateCandidate); // When state switches, all requests must be cancelled + checkInvariants(); + + if (!result.isSuccessful()) + { + return; + } + + trace(TraceRaftVoteRequestSucceeded, result.getCallID().server_node_id.get()); + + if (result.getResponse().term > persistent_state_.getCurrentTerm()) + { + tryIncrementCurrentTermFromResponse(result.getResponse().term); + } + else + { + if (result.getResponse().vote_granted) + { + num_votes_received_in_this_campaign_++; + } + } + // Rest of the logic is implemented in periodic update handlers. + // I'm no fan of asynchronous programming. At all. + } + + virtual void handleTimerEvent(const TimerEvent&) + { + checkInvariants(); + + switch (server_state_) + { + case ServerStateFollower: + { + updateFollower(); + break; + } + case ServerStateCandidate: + { + updateCandidate(); + break; + } + case ServerStateLeader: + { + updateLeader(); + break; + } + default: + { + UAVCAN_ASSERT(0); + break; + } + } + } + +public: + RaftCore(INode& node, + IStorageBackend& storage, + IEventTracer& tracer, + IRaftLeaderMonitor& leader_monitor) + : TimerBase(node) + , tracer_(tracer) + , leader_monitor_(leader_monitor) + , persistent_state_(storage, tracer) + , cluster_(node, storage, persistent_state_.getLog(), tracer) + , commit_index_(0) // Per Raft paper, commitIndex must be initialized to zero + , last_activity_timestamp_(node.getMonotonicTime()) + , randomized_activity_timeout_( + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS)) + , server_state_(ServerStateFollower) + , next_server_index_(0) + , num_votes_received_in_this_campaign_(0) + , append_entries_srv_(node) + , append_entries_client_(node) + , request_vote_srv_(node) + , request_vote_client_(node) + { } + + /** + * Once started, the logic runs in the background until destructor is called. + * @param cluster_size If set, this value will be used and stored in the persistent storage. If not set, + * value from the persistent storage will be used. If not set and there's no such key + * in the persistent storage, initialization will fail. + */ + int init(const uint8_t cluster_size, const TransferPriority priority) + { + /* + * Initializing state variables + */ + server_state_ = ServerStateFollower; + next_server_index_ = 0; + num_votes_received_in_this_campaign_ = 0; + commit_index_ = 0; + + registerActivity(); + + /* + * Initializing internals + */ + int res = persistent_state_.init(); + if (res < 0) + { + return res; + } + + res = cluster_.init(cluster_size, priority); + if (res < 0) + { + return res; + } + + res = append_entries_srv_.start(AppendEntriesCallback(this, &RaftCore::handleAppendEntriesRequest)); + if (res < 0) + { + return res; + } + + res = request_vote_srv_.start(RequestVoteCallback(this, &RaftCore::handleRequestVoteRequest)); + if (res < 0) + { + return res; + } + + res = append_entries_client_.init(priority); + if (res < 0) + { + return res; + } + append_entries_client_.setCallback(AppendEntriesResponseCallback(this, + &RaftCore::handleAppendEntriesResponse)); + + res = request_vote_client_.init(priority); + if (res < 0) + { + return res; + } + request_vote_client_.setCallback(RequestVoteResponseCallback(this, &RaftCore::handleRequestVoteResponse)); + + /* + * Initializing timing constants + * Refer to the specification for the formula + */ + const uint8_t num_followers = static_cast(cluster_.getClusterSize() - 1); + + const MonotonicDuration update_interval = + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS / + 2 / max(static_cast(2), num_followers)); + + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", + "Update interval: %ld msec", static_cast(update_interval.toMSec())); + + append_entries_client_.setRequestTimeout(min(append_entries_client_.getDefaultRequestTimeout(), + update_interval)); + + request_vote_client_.setRequestTimeout(min(request_vote_client_.getDefaultRequestTimeout(), + update_interval)); + + startPeriodic(update_interval); + + trace(TraceRaftCoreInited, update_interval.toUSec()); + + UAVCAN_ASSERT(res >= 0); + return 0; + } + + /** + * This function is mostly needed for testing. + */ + Log::Index getCommitIndex() const { return commit_index_; } + + /** + * This essentially indicates whether the server could replicate log since last allocation. + */ + bool areAllLogEntriesCommitted() const { return commit_index_ == persistent_state_.getLog().getLastIndex(); } + + /** + * Only the leader can call @ref appendLog(). + */ + bool isLeader() const { return server_state_ == ServerStateLeader; } + + /** + * Inserts one entry into log. + * This method will trigger an assertion failure and return error if the current node is not the leader. + * If operation fails, the node may give up its Leader status. + */ + void appendLog(const Entry::FieldTypes::unique_id& unique_id, NodeID node_id) + { + if (isLeader()) + { + Entry entry; + entry.node_id = node_id.get(); + entry.unique_id = unique_id; + entry.term = persistent_state_.getCurrentTerm(); + + trace(TraceRaftNewLogEntry, entry.node_id); + const int res = persistent_state_.getLog().append(entry); + if (res < 0) + { + handlePersistentStateUpdateError(res); + } + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * This class is used to perform log searches. + */ + struct LogEntryInfo + { + Entry entry; + bool committed; + + LogEntryInfo(const Entry& arg_entry, bool arg_committed) + : entry(arg_entry) + , committed(arg_committed) + { } + }; + + /** + * This method is used by the allocator to query existence of certain entries in the Raft log. + * Predicate is a callable of the following prototype: + * bool (const LogEntryInfo& entry) + * Once the predicate returns true, the loop will be terminated and the method will return an initialized lazy + * contructor with the last visited entry; otherwise the constructor will not be initialized. + * In this case, lazy constructor is used as boost::optional. + * The log is always traversed from HIGH to LOW index values, i.e. entry 0 will be traversed last. + */ + template + inline LazyConstructor traverseLogFromEndUntil(const Predicate& predicate) const + { + UAVCAN_ASSERT(coerceOrFallback(predicate, true)); + for (int index = static_cast(persistent_state_.getLog().getLastIndex()); index >= 0; index--) + { + const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(Log::Index(index)); + UAVCAN_ASSERT(entry != UAVCAN_NULLPTR); + const LogEntryInfo info(*entry, Log::Index(index) <= commit_index_); + if (predicate(info)) + { + LazyConstructor ret; + ret.template construct(info); + return ret; + } + } + return LazyConstructor(); + } + + Log::Index getNumAllocations() const + { + // Remember that index zero contains a special-purpose entry that doesn't count as allocation + return persistent_state_.getLog().getLastIndex(); + } + + /** + * These accessors are needed for debugging, visualization and testing. + */ + const PersistentState& getPersistentState() const { return persistent_state_; } + const ClusterManager& getClusterManager() const { return cluster_; } + MonotonicTime getLastActivityTimestamp() const { return last_activity_timestamp_; } + ServerState getServerState() const { return server_state_; } + MonotonicDuration getUpdateInterval() const { return getPeriod(); } + MonotonicDuration getRandomizedTimeout() const { return randomized_activity_timeout_; } +}; + +} +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp new file mode 100644 index 000000000000..52f712553ad9 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -0,0 +1,354 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * This class implements the top-level allocation logic and server API. + */ +class UAVCAN_EXPORT Server : public AbstractServer + , IRaftLeaderMonitor +{ + struct UniqueIDLogPredicate + { + const UniqueID unique_id; + + UniqueIDLogPredicate(const UniqueID& uid) + : unique_id(uid) + { } + + bool operator()(const RaftCore::LogEntryInfo& info) const + { + return info.entry.unique_id == unique_id; + } + }; + + struct NodeIDLogPredicate + { + const NodeID node_id; + + NodeIDLogPredicate(const NodeID& nid) + : node_id(nid) + { } + + bool operator()(const RaftCore::LogEntryInfo& info) const + { + return info.entry.node_id == node_id.get(); + } + }; + + /* + * States + */ + RaftCore raft_core_; + + /* + * Methods of IAllocationRequestHandler + */ + virtual bool canPublishFollowupAllocationResponse() const + { + /* + * The server is allowed to publish follow-up allocation responses only if both conditions are met: + * - The server is leader. + * - The last allocation request has been completed successfully. + * + * Why second condition? Imagine a case when there's two Raft nodes that don't hear each other - A and B, + * both of them are leaders (but only A can commit to the log, B is in a minor partition); then there's a + * client X that can exchange with both leaders, and a client Y that can exchange only with A. Such a + * situation can occur in case of a very unlikely failure of redundant interfaces. + * + * Both clients X and Y initially send a first-stage Allocation request; A responds to Y with a first-stage + * response, whereas B responds to X. Both X and Y will issue a follow-up second-stage requests, which may + * cause A to mix second-stage Allocation requests from different nodes, leading to reception of an invalid + * unique ID. When both leaders receive full unique IDs (A will receive an invalid one, B will receive a valid + * unique ID of X), only A will be able to make a commit, because B is in a minority. Since both clients were + * unable to receive node ID values in this round, they will try again later. + * + * Now, in order to prevent B from disrupting client-server communication second time around, we introduce this + * second restriction: the server cannot exchange with clients as long as its log contains uncommitted entries. + * + * Note that this restriction does not apply to allocation requests sent via CAN FD frames, as in this case + * no follow-up responses are necessary. So only CAN FD can offer reliable Allocation exchange. + */ + return raft_core_.isLeader() && raft_core_.areAllLogEntriesCommitted(); + } + + virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id) + { + /* + * Note that it is possible that the local node is not leader. We will still perform the log search + * and try to find the node that requested allocation. If the node is found, response will be sent; + * otherwise the request will be ignored because only leader can add new allocations. + */ + const LazyConstructor result = + raft_core_.traverseLogFromEndUntil(UniqueIDLogPredicate(unique_id)); + + if (result.isConstructed()) + { + if (result->committed) + { + tryPublishAllocationResult(result->entry); + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", + "Allocation request served with existing allocation; node ID %d", + int(result->entry.node_id)); + } + else + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", + "Allocation request ignored - allocation exists but not committed yet; node ID %d", + int(result->entry.node_id)); + } + } + else + { + if (raft_core_.isLeader() && !node_discoverer_.hasUnknownNodes()) + { + allocateNewNode(unique_id, preferred_node_id); + } + } + } + + /* + * Methods of INodeDiscoveryHandler + */ + virtual bool canDiscoverNewNodes() const + { + return raft_core_.isLeader(); + } + + virtual NodeAwareness checkNodeAwareness(NodeID node_id) const + { + const LazyConstructor result = + raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id)); + if (result.isConstructed()) + { + return result->committed ? NodeAwarenessKnownAndCommitted : NodeAwarenessKnownButNotCommitted; + } + else + { + return NodeAwarenessUnknown; + } + } + + virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) + { + if (raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id)).isConstructed()) + { + UAVCAN_ASSERT(0); // Such node is already known, the class that called this method should have known that + return; + } + + const UniqueID uid = (unique_id_or_null == UAVCAN_NULLPTR) ? UniqueID() : *unique_id_or_null; + + if (raft_core_.isLeader()) + { + raft_core_.appendLog(uid, node_id); + } + } + + /* + * Methods of IRaftLeaderMonitor + */ + virtual void handleLogCommitOnLeader(const protocol::dynamic_node_id::server::Entry& entry) + { + /* + * Maybe this node did not request allocation at all, we don't care, we publish anyway. + */ + tryPublishAllocationResult(entry); + } + + virtual void handleLocalLeadershipChange(bool local_node_is_leader) + { + if (!local_node_is_leader) + { + return; + } + + const LazyConstructor result = + raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_.getNodeID())); + + if (!result.isConstructed()) + { + raft_core_.appendLog(getOwnUniqueID(), node_.getNodeID()); + } + } + + /* + * Private methods + */ + bool isNodeIDTaken(const NodeID node_id) const + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", + "Testing if node ID %d is taken", int(node_id.get())); + return raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id)); + } + + void allocateNewNode(const UniqueID& unique_id, const NodeID preferred_node_id) + { + const NodeID allocated_node_id = + NodeIDSelector(this, &Server::isNodeIDTaken).findFreeNodeID(preferred_node_id); + if (!allocated_node_id.isUnicast()) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "Request ignored - no free node ID left"); + return; + } + + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "New node ID allocated: %d", + int(allocated_node_id.get())); + raft_core_.appendLog(unique_id, allocated_node_id); + } + + void tryPublishAllocationResult(const protocol::dynamic_node_id::server::Entry& entry) + { + const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id); + if (res < 0) + { + tracer_.onEvent(TraceError, res); + node_.registerInternalFailure("Dynamic allocation response"); + } + } + +public: + Server(INode& node, + IStorageBackend& storage, + IEventTracer& tracer) + : AbstractServer(node, tracer) + , raft_core_(node, storage, tracer, *this) + { } + + int init(const UniqueID& own_unique_id, + const uint8_t cluster_size = ClusterManager::ClusterSizeUnknown, + const TransferPriority priority = TransferPriority::OneHigherThanLowest) + { + /* + * Initializing Raft core first, because the next step requires Log to be loaded + */ + int res = raft_core_.init(cluster_size, priority); + if (res < 0) + { + return res; + } + + /* + * Common logic + */ + res = AbstractServer::init(own_unique_id, priority); + if (res < 0) + { + return res; + } + + /* + * Making sure that the server is started with the same node ID + */ + const LazyConstructor own_log_entry = + raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_.getNodeID())); + + if (own_log_entry.isConstructed()) + { + if (own_log_entry->entry.unique_id != getOwnUniqueID()) + { + return -ErrInvalidConfiguration; + } + } + + return 0; + } + + Log::Index getNumAllocations() const { return raft_core_.getNumAllocations(); } + + /** + * These accessors are needed for debugging, visualization and testing. + */ + const RaftCore& getRaftCore() const { return raft_core_; } +}; + +/** + * This structure represents immediate state of the server. + * It can be used for state visualization and debugging. + */ +struct StateReport +{ + uint8_t cluster_size; + + RaftCore::ServerState state; + + Log::Index last_log_index; + Log::Index commit_index; + + Term last_log_term; + Term current_term; + + NodeID voted_for; + + MonotonicTime last_activity_timestamp; + MonotonicDuration randomized_timeout; + + uint8_t num_unknown_nodes; + + struct FollowerState + { + NodeID node_id; + Log::Index next_index; + Log::Index match_index; + + FollowerState() + : next_index(0) + , match_index(0) + { } + } followers[ClusterManager::MaxClusterSize - 1]; + + StateReport(const Server& s) + : cluster_size (s.getRaftCore().getClusterManager().getClusterSize()) + , state (s.getRaftCore().getServerState()) + , last_log_index (s.getRaftCore().getPersistentState().getLog().getLastIndex()) + , commit_index (s.getRaftCore().getCommitIndex()) + , last_log_term (0) // See below + , current_term (s.getRaftCore().getPersistentState().getCurrentTerm()) + , voted_for (s.getRaftCore().getPersistentState().getVotedFor()) + , last_activity_timestamp(s.getRaftCore().getLastActivityTimestamp()) + , randomized_timeout (s.getRaftCore().getRandomizedTimeout()) + , num_unknown_nodes (s.getNodeDiscoverer().getNumUnknownNodes()) + { + const Entry* const e = s.getRaftCore().getPersistentState().getLog().getEntryAtIndex(last_log_index); + UAVCAN_ASSERT(e != UAVCAN_NULLPTR); + if (e != UAVCAN_NULLPTR) + { + last_log_term = e->term; + } + + for (uint8_t i = 0; i < (cluster_size - 1U); i++) + { + const ClusterManager& mgr = s.getRaftCore().getClusterManager(); + const NodeID node_id = mgr.getRemoteServerNodeIDAtIndex(i); + if (node_id.isUnicast()) + { + followers[i].node_id = node_id; + followers[i].next_index = mgr.getServerNextIndex(node_id); + followers[i].match_index = mgr.getServerMatchIndex(node_id); + } + } + } +}; + +} +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/types.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/types.hpp new file mode 100644 index 000000000000..ed3ba2f873fb --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/types.hpp @@ -0,0 +1,29 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_TYPES_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_TYPES_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ + +using namespace ::uavcan::protocol::dynamic_node_id::server; + +/** + * Raft term + */ +typedef StorageType::Type Term; + +} +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp new file mode 100644 index 000000000000..7f56f2a82c11 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -0,0 +1,176 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_EVENT_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_EVENT_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * @ref IEventTracer. + * Event codes cannot be changed, only new ones can be added. + */ +enum UAVCAN_EXPORT TraceCode +{ + // Event name Argument + // 0 + TraceError, // error code (may be negated) + TraceRaftLogLastIndexRestored, // recovered last index value + TraceRaftLogAppend, // index of new entry + TraceRaftLogRemove, // new last index value + TraceRaftCurrentTermRestored, // current term + // 5 + TraceRaftCurrentTermUpdate, // current term + TraceRaftVotedForRestored, // value of votedFor + TraceRaftVotedForUpdate, // value of votedFor + TraceRaftDiscoveryBroadcast, // number of known servers + TraceRaftNewServerDiscovered, // node ID of the new server + // 10 + TraceRaftDiscoveryReceived, // node ID of the sender + TraceRaftClusterSizeInited, // cluster size + TraceRaftBadClusterSizeReceived, // received cluster size + TraceRaftCoreInited, // update interval in usec + TraceRaftStateSwitch, // 0 - Follower, 1 - Candidate, 2 - Leader + // 15 + Trace0, + TraceRaftNewLogEntry, // node ID value + TraceRaftRequestIgnored, // node ID of the client + TraceRaftVoteRequestReceived, // node ID of the client + TraceRaftVoteRequestSucceeded, // node ID of the server + // 20 + TraceRaftVoteRequestInitiation, // node ID of the server + TraceRaftPersistStateUpdateError, // negative error code + TraceRaftCommitIndexUpdate, // new commit index value + TraceRaftNewerTermInResponse, // new term value + TraceRaftNewEntryCommitted, // new commit index value + // 25 + TraceRaftAppendEntriesCallFailure, // error code (may be negated) + TraceRaftElectionComplete, // number of votes collected + TraceRaftAppendEntriesRespUnsucfl, // node ID of the client + Trace2, + Trace3, + // 30 + TraceAllocationFollowupResponse, // number of unique ID bytes in this response + TraceAllocationFollowupDenied, // reason code (see sources for details) + TraceAllocationFollowupTimeout, // timeout value in microseconds + TraceAllocationBadRequest, // number of unique ID bytes in this request + TraceAllocationUnexpectedStage, // stage number in the request - 1, 2, or 3 + // 35 + TraceAllocationRequestAccepted, // number of bytes of unique ID after request + TraceAllocationExchangeComplete, // first 8 bytes of unique ID interpreted as signed 64 bit big endian + TraceAllocationResponse, // allocated node ID + TraceAllocationActivity, // source node ID of the message + Trace12, + // 40 + TraceDiscoveryNewNodeFound, // node ID + TraceDiscoveryCommitCacheUpdated, // node ID marked as committed + TraceDiscoveryNodeFinalized, // node ID in lower 7 bits, bit 8 (256, 0x100) is set if unique ID is known + TraceDiscoveryGetNodeInfoFailure, // node ID + TraceDiscoveryTimerStart, // interval in microseconds + // 45 + TraceDiscoveryTimerStop, // reason code (see sources for details) + TraceDiscoveryGetNodeInfoRequest, // target node ID + TraceDiscoveryNodeRestartDetected, // node ID + TraceDiscoveryNodeRemoved, // node ID + Trace22, + // 50 + + NumTraceCodes +}; + +/** + * This interface allows the application to trace events that happen in the server. + */ +class UAVCAN_EXPORT IEventTracer +{ +public: +#if UAVCAN_TOSTRING + /** + * It is safe to call this function with any argument. + * If the event code is out of range, an assertion failure will be triggered and an error text will be returned. + */ + static const char* getEventName(TraceCode code) + { + // import re;m = lambda s:',\n'.join('"%s"' % x for x in re.findall(r'\ {4}Trace[0-9]*([A-Za-z0-9]*),',s)) + static const char* const Strings[] = + { + "Error", + "RaftLogLastIndexRestored", + "RaftLogAppend", + "RaftLogRemove", + "RaftCurrentTermRestored", + "RaftCurrentTermUpdate", + "RaftVotedForRestored", + "RaftVotedForUpdate", + "RaftDiscoveryBroadcast", + "RaftNewServerDiscovered", + "RaftDiscoveryReceived", + "RaftClusterSizeInited", + "RaftBadClusterSizeReceived", + "RaftCoreInited", + "RaftStateSwitch", + "", + "RaftNewLogEntry", + "RaftRequestIgnored", + "RaftVoteRequestReceived", + "RaftVoteRequestSucceeded", + "RaftVoteRequestInitiation", + "RaftPersistStateUpdateError", + "RaftCommitIndexUpdate", + "RaftNewerTermInResponse", + "RaftNewEntryCommitted", + "RaftAppendEntriesCallFailure", + "RaftElectionComplete", + "RaftAppendEntriesRespUnsucfl", + "", + "", + "AllocationFollowupResponse", + "AllocationFollowupDenied", + "AllocationFollowupTimeout", + "AllocationBadRequest", + "AllocationUnexpectedStage", + "AllocationRequestAccepted", + "AllocationExchangeComplete", + "AllocationResponse", + "AllocationActivity", + "", + "DiscoveryNewNodeFound", + "DiscoveryCommitCacheUpdated", + "DiscoveryNodeFinalized", + "DiscoveryGetNodeInfoFailure", + "DiscoveryTimerStart", + "DiscoveryTimerStop", + "DiscoveryGetNodeInfoRequest", + "DiscoveryNodeRestartDetected", + "DiscoveryNodeRemoved", + "" + }; + uavcan::StaticAssert::check(); + UAVCAN_ASSERT(code < NumTraceCodes); + // coverity[dead_error_line] + return (code < NumTraceCodes) ? Strings[static_cast(code)] : "INVALID_EVENT_CODE"; + } +#endif + + /** + * The server invokes this method every time it believes that a noteworthy event has happened. + * It is guaranteed that event code values will never change, but new ones can be added in future. This ensures + * full backward compatibility. + * @param event_code Event code, see the sources for the enum with values. + * @param event_argument Value associated with the event; its meaning depends on the event code. + */ + virtual void onEvent(TraceCode event_code, int64_t event_argument) = 0; + + virtual ~IEventTracer() { } +}; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp new file mode 100644 index 000000000000..958328ba6016 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -0,0 +1,348 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_DISCOVERER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_DISCOVERER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * The allocator must implement this interface. + */ +class INodeDiscoveryHandler +{ +public: + /** + * In order to avoid bus congestion, normally only leader can discover new nodes. + */ + virtual bool canDiscoverNewNodes() const = 0; + + /** + * These values represent the level of awareness of a certain node by the server. + */ + enum NodeAwareness + { + NodeAwarenessUnknown, + NodeAwarenessKnownButNotCommitted, + NodeAwarenessKnownAndCommitted + }; + + /** + * It is OK to do full log traversal in this method, because the unique ID collector will cache the + * result when possible. + */ + virtual NodeAwareness checkNodeAwareness(NodeID node_id) const = 0; + + /** + * This method will be called when a new node responds to GetNodeInfo request. + * If this method fails to register the node, the node will be queried again later and this method will be + * invoked again. + * Unique ID will be UAVCAN_NULLPTR if the node is assumed to not implement the GetNodeInfo service. + */ + virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) = 0; + + virtual ~INodeDiscoveryHandler() { } +}; + +/** + * This class listens to NodeStatus messages from other nodes and retrieves their unique ID if they are not + * known to the allocator. + */ +class NodeDiscoverer : TimerBase +{ + typedef MethodBinder&)> + GetNodeInfoResponseCallback; + + typedef MethodBinder&)> + NodeStatusCallback; + + struct NodeData + { + uint32_t last_seen_uptime; + uint8_t num_get_node_info_attempts; + + NodeData() + : last_seen_uptime(0) + , num_get_node_info_attempts(0) + { } + }; + + typedef Map NodeMap; + + /** + * When this number of attempts has been made, the discoverer will give up and assume that the node + * does not implement this service. + */ + enum { MaxAttemptsToGetNodeInfo = 5 }; + + enum { TimerPollIntervalMs = 170 }; // ~ ceil(500 ms service timeout / 3) + + /* + * States + */ + INodeDiscoveryHandler& handler_; + IEventTracer& tracer_; + + BitSet committed_node_mask_; ///< Nodes that are marked will not be queried + NodeMap node_map_; + + ServiceClient get_node_info_client_; + Subscriber node_status_sub_; + + /* + * Methods + */ + void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); } + + INode& getNode() { return node_status_sub_.getNode(); } + + void removeNode(const NodeID node_id) + { + node_map_.remove(node_id); + trace(TraceDiscoveryNodeRemoved, node_id.get()); + } + + NodeID pickNextNodeToQuery() const + { + // This essentially means that we pick first available node. Remember that the map is unordered. + const NodeMap::KVPair* const pair = node_map_.getByIndex(0); + return (pair == UAVCAN_NULLPTR) ? NodeID() : pair->key; + } + + bool needToQuery(NodeID node_id) + { + UAVCAN_ASSERT(node_id.isUnicast()); + + /* + * Fast check + */ + if (committed_node_mask_[node_id.get()]) + { + return false; + } + + /* + * Slow check - may involve full log search + */ + const INodeDiscoveryHandler::NodeAwareness awareness = handler_.checkNodeAwareness(node_id); + + if (awareness == INodeDiscoveryHandler::NodeAwarenessUnknown) + { + return true; + } + else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownButNotCommitted) + { + removeNode(node_id); + return false; + } + else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownAndCommitted) + { + trace(TraceDiscoveryCommitCacheUpdated, node_id.get()); + committed_node_mask_[node_id.get()] = true; + removeNode(node_id); + return false; + } + else + { + UAVCAN_ASSERT(0); + return false; + } + } + + NodeID pickNextNodeToQueryAndCleanupMap() + { + NodeID node_id; + do + { + node_id = pickNextNodeToQuery(); + if (node_id.isUnicast()) + { + if (needToQuery(node_id)) + { + return node_id; + } + else + { + removeNode(node_id); + } + } + } + while (node_id.isUnicast()); + return NodeID(); + } + + void finalizeNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) + { + trace(TraceDiscoveryNodeFinalized, node_id.get() | ((unique_id_or_null == UAVCAN_NULLPTR) ? 0U : 0x100U)); + removeNode(node_id); + /* + * It is paramount to check if the server is still interested to receive this data. + * Otherwise, if the node appeared in the log while we were waiting for response, we'd end up with + * duplicate node ID in the log. + */ + if (needToQuery(node_id)) + { + handler_.handleNewNodeDiscovery(unique_id_or_null, node_id); + } + } + + void handleGetNodeInfoResponse(const ServiceCallResult& result) + { + if (result.isSuccessful()) + { + UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "GetNodeInfo response from %d", + int(result.getCallID().server_node_id.get())); + finalizeNodeDiscovery(&result.getResponse().hardware_version.unique_id, result.getCallID().server_node_id); + } + else + { + trace(TraceDiscoveryGetNodeInfoFailure, result.getCallID().server_node_id.get()); + + NodeData* const data = node_map_.access(result.getCallID().server_node_id); + if (data == UAVCAN_NULLPTR) + { + return; // Probably it is a known node now + } + + UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", + "GetNodeInfo request to %d has timed out, %d attempts", + int(result.getCallID().server_node_id.get()), int(data->num_get_node_info_attempts)); + data->num_get_node_info_attempts++; + if (data->num_get_node_info_attempts >= MaxAttemptsToGetNodeInfo) + { + finalizeNodeDiscovery(UAVCAN_NULLPTR, result.getCallID().server_node_id); + // Warning: data pointer is invalidated now + } + } + } + + void handleTimerEvent(const TimerEvent&) override + { + if (get_node_info_client_.hasPendingCalls()) + { + return; + } + + const NodeID node_id = pickNextNodeToQueryAndCleanupMap(); + if (!node_id.isUnicast()) + { + trace(TraceDiscoveryTimerStop, 0); + stop(); + return; + } + + if (!handler_.canDiscoverNewNodes()) + { + return; // Timer must continue to run in order to not stuck when it unlocks + } + + trace(TraceDiscoveryGetNodeInfoRequest, node_id.get()); + + UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d", + int(node_id.get())); + const int res = get_node_info_client_.call(node_id, protocol::GetNodeInfo::Request()); + if (res < 0) + { + getNode().registerInternalFailure("NodeDiscoverer GetNodeInfo call"); + } + } + + void handleNodeStatus(const ReceivedDataStructure& msg) + { + if (!needToQuery(msg.getSrcNodeID())) + { + return; + } + + NodeData* data = node_map_.access(msg.getSrcNodeID()); + if (data == UAVCAN_NULLPTR) + { + trace(TraceDiscoveryNewNodeFound, msg.getSrcNodeID().get()); + + data = node_map_.insert(msg.getSrcNodeID(), NodeData()); + if (data == UAVCAN_NULLPTR) + { + getNode().registerInternalFailure("NodeDiscoverer OOM"); + return; + } + } + UAVCAN_ASSERT(data != UAVCAN_NULLPTR); + + if (msg.uptime_sec < data->last_seen_uptime) + { + trace(TraceDiscoveryNodeRestartDetected, msg.getSrcNodeID().get()); + data->num_get_node_info_attempts = 0; + } + data->last_seen_uptime = msg.uptime_sec; + + if (!isRunning()) + { + startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs)); + trace(TraceDiscoveryTimerStart, getPeriod().toUSec()); + } + } + +public: + NodeDiscoverer(INode& node, IEventTracer& tracer, INodeDiscoveryHandler& handler) + : TimerBase(node) + , handler_(handler) + , tracer_(tracer) + , node_map_(node.getAllocator()) + , get_node_info_client_(node) + , node_status_sub_(node) + { } + + int init(const TransferPriority priority) + { + int res = get_node_info_client_.init(priority); + if (res < 0) + { + return res; + } + get_node_info_client_.setCallback( + GetNodeInfoResponseCallback(this, &NodeDiscoverer::handleGetNodeInfoResponse)); + + res = node_status_sub_.start(NodeStatusCallback(this, &NodeDiscoverer::handleNodeStatus)); + if (res < 0) + { + return res; + } + + // Note: the timer starts ad-hoc from the node status callback, not here. + + return 0; + } + + /** + * Returns true if there's at least one node with pending GetNodeInfo. + */ + bool hasUnknownNodes() const { return !node_map_.isEmpty(); } + + /** + * Returns number of nodes that are being queried at the moment. + * This method is needed for testing and state visualization. + */ + uint8_t getNumUnknownNodes() const { return static_cast(node_map_.getSize()); } +}; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp new file mode 100644 index 000000000000..42e85926e8dc --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_ID_SELECTOR_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_ID_SELECTOR_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * Node ID allocation logic + */ +template +class NodeIDSelector +{ + typedef bool (Owner::*IsNodeIDTakenMethod)(const NodeID node_id) const; + + const Owner* const owner_; + const IsNodeIDTakenMethod is_node_id_taken_; + +public: + NodeIDSelector(const Owner* owner, IsNodeIDTakenMethod is_node_id_taken) + : owner_(owner) + , is_node_id_taken_(is_node_id_taken) + { + UAVCAN_ASSERT(owner_ != UAVCAN_NULLPTR); + UAVCAN_ASSERT(is_node_id_taken_ != UAVCAN_NULLPTR); + } + + /** + * Reutrns a default-constructed (invalid) node ID if a free one could not be found. + */ + NodeID findFreeNodeID(const NodeID preferred) const + { + uint8_t candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes; + + // Up + while (candidate <= NodeID::MaxRecommendedForRegularNodes) + { + if (!(owner_->*is_node_id_taken_)(candidate)) + { + return candidate; + } + candidate++; + } + + candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes; + + // Down + while (candidate > 0) + { + if (!(owner_->*is_node_id_taken_)(candidate)) + { + return candidate; + } + candidate--; + } + + return NodeID(); + } +}; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp new file mode 100644 index 000000000000..9e2395c92a27 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_BACKEND_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_BACKEND_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * This interface is used by the server to read and write stable storage. + * The storage is represented as a key-value container, where keys and values are ASCII strings up to 32 + * characters long, not including the termination byte. Fixed block size allows for absolutely straightforward + * and efficient implementation of storage backends, e.g. based on text files. + * Keys and values may contain only non-whitespace, non-formatting printable characters. + */ +class UAVCAN_EXPORT IStorageBackend +{ +public: + /** + * Maximum length of keys and values. One pair takes twice as much space. + */ + enum { MaxStringLength = 32 }; + + /** + * It is guaranteed that the server will never require more than this number of key/value pairs. + * Total storage space needed is (MaxKeyValuePairs * MaxStringLength * 2), not including storage overhead. + */ + enum { MaxKeyValuePairs = 512 }; + + /** + * This type is used to exchange data chunks with the backend. + * It doesn't use any dynamic memory; please refer to the Array<> class for details. + */ + typedef MakeString::Type String; + + /** + * Read one value from the storage. + * If such key does not exist, or if read failed, an empty string will be returned. + * This method should not block for more than 50 ms. + */ + virtual String get(const String& key) const = 0; + + /** + * Create or update value for the given key. Empty value should be regarded as a request to delete the key. + * This method should not block for more than 50 ms. + * Failures will be ignored. + */ + virtual void set(const String& key, const String& value) = 0; + + virtual ~IStorageBackend() { } +}; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp new file mode 100644 index 000000000000..83f68e4f2ace --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp @@ -0,0 +1,190 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_MARSHALLER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_MARSHALLER_HPP_INCLUDED + +#include +#include +#include +#include +#include + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * This class extends the storage backend interface with serialization/deserialization functionality. + */ +class StorageMarshaller +{ + IStorageBackend& storage_; + + static uint8_t convertLowerCaseHexCharToNibble(char ch) + { + const uint8_t ret = (ch > '9') ? static_cast(ch - 'a' + 10) : static_cast(ch - '0'); + UAVCAN_ASSERT(ret < 16); + return ret; + } + +public: + StorageMarshaller(IStorageBackend& storage) + : storage_(storage) + { } + + /** + * Turns a unique ID into a hex string that can be used as a key or as a value. + */ + static IStorageBackend::String convertUniqueIDToHex(const UniqueID& key) + { + IStorageBackend::String serialized; + for (uint8_t i = 0; i < UniqueID::MaxSize; i++) + { + serialized.appendFormatted("%02x", key.at(i)); + } + UAVCAN_ASSERT(serialized.size() == UniqueID::MaxSize * 2); + return serialized; + } + + /** + * These methods set the value and then immediately read it back. + * 1. Serialize the value. + * 2. Update the value on the backend. + * 3. Call get() with the same value argument. + * The caller then is supposed to check whether the argument has the desired value. + */ + int setAndGetBack(const IStorageBackend::String& key, uint32_t& inout_value) + { + IStorageBackend::String serialized; + serialized.appendFormatted("%lu", inout_value); + + UAVCAN_TRACE("StorageMarshaller", "Set %s = %s", key.c_str(), serialized.c_str()); + storage_.set(key, serialized); + + return get(key, inout_value); + } + + int setAndGetBack(const IStorageBackend::String& key, UniqueID& inout_value) + { + const IStorageBackend::String serialized = convertUniqueIDToHex(inout_value); + + UAVCAN_TRACE("StorageMarshaller", "Set %s = %s", key.c_str(), serialized.c_str()); + storage_.set(key, serialized); + + return get(key, inout_value); + } + + /** + * Getters simply read and deserialize the value. + * 1. Read the value back from the backend; return false if read fails. + * 2. Deserealize the newly read value; return false if deserialization fails. + * 3. Update the argument with deserialized value. + * 4. Return true. + */ + int get(const IStorageBackend::String& key, uint32_t& out_value) const + { + /* + * Reading the storage + */ + const IStorageBackend::String val = storage_.get(key); + if (val.empty()) + { + return -ErrFailure; + } + + /* + * Per MISRA C++ recommendations, checking the inputs instead of relying solely on errno. + * The value must contain only numeric characters. + */ + for (IStorageBackend::String::const_iterator it = val.begin(); it != val.end(); ++it) + { + if (static_cast(*it) < '0' || static_cast(*it) > '9') + { + return -ErrFailure; + } + } + + if (val.size() > 10) // len(str(0xFFFFFFFF)) + { + return -ErrFailure; + } + + /* + * Conversion is carried out here + */ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + errno = 0; +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + const unsigned long long x = std::strtoull(val.c_str(), UAVCAN_NULLPTR, 10); +#else + // There was no strtoull() before C++11, so we need to resort to strtoul() + StaticAssert<(sizeof(unsigned long) >= sizeof(uint32_t))>::check(); + const unsigned long x = std::strtoul(val.c_str(), UAVCAN_NULLPTR, 10); +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + if (errno != 0) + { + return -ErrFailure; + } +#endif + + out_value = static_cast(x); + return 0; + } + + int get(const IStorageBackend::String& key, UniqueID& out_value) const + { + static const uint8_t NumBytes = UniqueID::MaxSize; + + /* + * Reading the storage + */ + IStorageBackend::String val = storage_.get(key); + if (val.size() != NumBytes * 2) + { + return -ErrFailure; + } + + /* + * The value must contain only hexadecimal numbers. + */ + val.convertToLowerCaseASCII(); + for (IStorageBackend::String::const_iterator it = val.begin(); it != val.end(); ++it) + { + if ((static_cast(*it) < '0' || static_cast(*it) > '9') && + (static_cast(*it) < 'a' || static_cast(*it) > 'f')) + { + return -ErrFailure; + } + } + + /* + * Conversion is carried out here + */ + IStorageBackend::String::const_iterator it = val.begin(); + + for (uint8_t byte_index = 0; byte_index < NumBytes; byte_index++) + { + out_value[byte_index] = + static_cast(convertLowerCaseHexCharToNibble(static_cast(*it++)) << 4); + out_value[byte_index] = + static_cast(convertLowerCaseHexCharToNibble(static_cast(*it++)) | out_value[byte_index]); + } + + return 0; + } +}; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp new file mode 100644 index 000000000000..360a93aee6fb --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp @@ -0,0 +1,27 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_TYPES_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_TYPES_HPP_INCLUDED + +#include +// UAVCAN types +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ + +using namespace ::uavcan::protocol::dynamic_node_id; + +/** + * Node Unique ID + */ +typedef protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id UniqueID; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/file_server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/file_server.hpp new file mode 100644 index 000000000000..167be48fa95d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/file_server.hpp @@ -0,0 +1,321 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +// UAVCAN types +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * The file server backend should implement this interface. + * Note that error codes returned by these methods are defined in uavcan.protocol.file.Error; these are + * not the same as libuavcan-internal error codes defined in uavcan.error.hpp. + */ +class UAVCAN_EXPORT IFileServerBackend +{ + +public: + typedef protocol::file::Path::FieldTypes::path Path; + typedef protocol::file::EntryType EntryType; + typedef protocol::file::Error Error; + + IFileServerBackend::Path root_path_; + IFileServerBackend::Path alt_root_path_; + + /** + * All read operations must return this number of bytes, unless end of file is reached. + */ + enum { ReadSize = protocol::file::Read::Response::FieldTypes::data::MaxSize }; + + /** + * Shortcut for uavcan.protocol.file.Path.SEPARATOR. + */ + static char getPathSeparator() { return static_cast(protocol::file::Path::SEPARATOR); } + + /** + * Set a base path to the files. + */ + void setRootPath(const char * path) + { + if (path) + { + root_path_.clear(); + root_path_ = path; + if (root_path_.back() != getPathSeparator()) + { + root_path_.push_back(getPathSeparator()); + } + } + } + + void setAltRootPath(const char * path) + { + if (path) + { + alt_root_path_.clear(); + alt_root_path_ = path; + if (alt_root_path_.back() != getPathSeparator()) + { + alt_root_path_.push_back(getPathSeparator()); + } + } + } + + /** + * Get a base path to the files. + */ + Path& getRootPath() + { + return root_path_; + } + + /** + * Get a base path to the files. + */ + Path& getAltRootPath() + { + return alt_root_path_; + } + + /** + * Backend for uavcan.protocol.file.GetInfo. + * Refer to uavcan.protocol.file.EntryType for the list of available bit flags. + * Implementation of this method is required. + * On success the method must return zero. + */ + virtual int16_t getInfo(const Path& path, uint64_t& out_size, EntryType& out_type) = 0; + + /** + * Backend for uavcan.protocol.file.Read. + * Implementation of this method is required. + * @ref inout_size is set to @ref ReadSize; read operation is required to return exactly this amount, except + * if the end of file is reached. + * On success the method must return zero. + */ + virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size) = 0; + + // Methods below are optional. + + /** + * Backend for uavcan.protocol.file.Write. + * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED. + * On success the method must return zero. + */ + virtual int16_t write(const Path& path, const uint64_t offset, const uint8_t* buffer, const uint16_t size) + { + (void)path; + (void)offset; + (void)buffer; + (void)size; + return Error::NOT_IMPLEMENTED; + } + + /** + * Backend for uavcan.protocol.file.Delete. ('delete' is a C++ keyword, so 'remove' is used instead) + * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED. + * On success the method must return zero. + */ + virtual int16_t remove(const Path& path) + { + (void)path; + return Error::NOT_IMPLEMENTED; + } + + /** + * Backend for uavcan.protocol.file.GetDirectoryEntryInfo. + * Refer to uavcan.protocol.file.EntryType for the list of available bit flags. + * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED. + * On success the method must return zero. + */ + virtual int16_t getDirectoryEntryInfo(const Path& directory_path, const uint32_t entry_index, + EntryType& out_type, Path& out_entry_full_path) + { + (void)directory_path; + (void)entry_index; + (void)out_type; + (void)out_entry_full_path; + return Error::NOT_IMPLEMENTED; + } + + virtual ~IFileServerBackend() { } +}; + +/** + * Basic file server implements only the following services: + * uavcan.protocol.file.GetInfo + * uavcan.protocol.file.Read + * Also see @ref IFileServerBackend. + */ +class BasicFileServer +{ + typedef MethodBinder + GetInfoCallback; + + typedef MethodBinder + ReadCallback; + + ServiceServer get_info_srv_; + ServiceServer read_srv_; + + void handleGetInfo(const protocol::file::GetInfo::Request& req, protocol::file::GetInfo::Response& resp) + { + resp.error.value = backend_.getInfo(req.path.path, resp.size, resp.entry_type); + } + + void handleRead(const protocol::file::Read::Request& req, protocol::file::Read::Response& resp) + { + uint16_t inout_size = resp.data.capacity(); + + resp.data.resize(inout_size); + + resp.error.value = backend_.read(req.path.path, req.offset, resp.data.begin(), inout_size); + + if (resp.error.value != protocol::file::Error::OK) + { + inout_size = 0; + } + + if (inout_size > resp.data.capacity()) + { + UAVCAN_ASSERT(0); + resp.error.value = protocol::file::Error::UNKNOWN_ERROR; + } + else + { + resp.data.resize(inout_size); + } + } + +protected: + IFileServerBackend& backend_; ///< Derived types can use it + +public: + BasicFileServer(INode& node, IFileServerBackend& backend) + : get_info_srv_(node) + , read_srv_(node) + , backend_(backend) + { } + + int start(const char* server_root = UAVCAN_NULLPTR, const char* server_alt_root = UAVCAN_NULLPTR) + { + backend_.setRootPath(server_root); + backend_.setAltRootPath(server_alt_root); + + int res = get_info_srv_.start(GetInfoCallback(this, &BasicFileServer::handleGetInfo)); + if (res < 0) + { + return res; + } + + res = read_srv_.start(ReadCallback(this, &BasicFileServer::handleRead)); + if (res < 0) + { + return res; + } + + return 0; + } +}; + +/** + * Full file server implements all file services: + * uavcan.protocol.file.GetInfo + * uavcan.protocol.file.Read + * uavcan.protocol.file.Write + * uavcan.protocol.file.Delete + * uavcan.protocol.file.GetDirectoryEntryInfo + * Also see @ref IFileServerBackend. + */ +class FileServer : protected BasicFileServer +{ + typedef MethodBinder + WriteCallback; + + typedef MethodBinder + DeleteCallback; + + typedef MethodBinder + GetDirectoryEntryInfoCallback; + + ServiceServer write_srv_; + ServiceServer delete_srv_; + ServiceServer get_directory_entry_info_srv_; + + void handleWrite(const protocol::file::Write::Request& req, protocol::file::Write::Response& resp) + { + resp.error.value = backend_.write(req.path.path, req.offset, req.data.begin(), req.data.size()); + } + + void handleDelete(const protocol::file::Delete::Request& req, protocol::file::Delete::Response& resp) + { + resp.error.value = backend_.remove(req.path.path); + } + + void handleGetDirectoryEntryInfo(const protocol::file::GetDirectoryEntryInfo::Request& req, + protocol::file::GetDirectoryEntryInfo::Response& resp) + { + resp.error.value = backend_.getDirectoryEntryInfo(req.directory_path.path, req.entry_index, + resp.entry_type, resp.entry_full_path.path); + } + + +public: + FileServer(INode& node, IFileServerBackend& backend) + : BasicFileServer(node, backend) + , write_srv_(node) + , delete_srv_(node) + , get_directory_entry_info_srv_(node) + { } + + int start() + { + int res = BasicFileServer::start(); + if (res < 0) + { + return res; + } + + res = write_srv_.start(WriteCallback(this, &FileServer::handleWrite)); + if (res < 0) + { + return res; + } + + res = delete_srv_.start(DeleteCallback(this, &FileServer::handleDelete)); + if (res < 0) + { + return res; + } + + res = get_directory_entry_info_srv_.start( + GetDirectoryEntryInfoCallback(this, &FileServer::handleGetDirectoryEntryInfo)); + if (res < 0) + { + return res; + } + + return 0; + } +}; + +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp new file mode 100644 index 000000000000..04de3be0d203 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -0,0 +1,472 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include + +namespace uavcan +{ +/** + * Application-specific firmware version checking logic. + * Refer to @ref FirmwareUpdateTrigger for details. + */ +class IFirmwareVersionChecker +{ +public: + /** + * This value is limited by the pool block size minus some extra data. Please refer to the Map<> documentation + * for details. If this size is set too high, the compilation will fail in the Map<> template. + */ + enum { MaxFirmwareFilePathLength = 40 }; + + /** + * This type is used to store path to firmware file that the target node will retrieve using the + * service uavcan.protocol.file.Read. Note that the maximum length is limited due to some specifics of + * libuavcan (@ref MaxFirmwareFilePathLength), this is NOT a protocol-level limitation. + */ + typedef MakeString::Type FirmwareFilePath; + + /** + * This method will be invoked when the class obtains a response to GetNodeInfo request. + * + * @param node_id Node ID that this GetNodeInfo response was received from. + * + * @param node_info Actual node info structure; refer to uavcan.protocol.GetNodeInfo for details. + * + * @param out_firmware_file_path The implementation should return the firmware image path via this argument. + * Note that this path must be reachable via uavcan.protocol.file.Read service. + * Refer to @ref FileServer and @ref BasicFileServer for details. + * + * @return True - the class will begin sending update requests. + * False - the node will be ignored, no request will be sent. + */ + virtual bool shouldRequestFirmwareUpdate(NodeID node_id, const protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) = 0; + + /** + * This method will be invoked when a node responds to the update request with an error. If the request simply + * times out, this method will not be invoked. + * Note that if by the time of arrival of the response the node is already removed, this method will not be called. + * + * SPECIAL CASE: If the node responds with ERROR_IN_PROGRESS, the class will assume that further requesting + * is not needed anymore. This method will not be invoked. + * + * @param node_id Node ID that returned this error. + * + * @param error_response Contents of the error response. It contains error code and text. + * + * @param out_firmware_file_path New firmware path if a retry is needed. Note that this argument will be + * initialized with old path, so if the same path needs to be reused, this + * argument should be left unchanged. + * + * @return True - the class will continue sending update requests with new firmware path. + * False - the node will be forgotten, new requests will not be sent. + */ + virtual bool shouldRetryFirmwareUpdate(NodeID node_id, + const protocol::file::BeginFirmwareUpdate::Response& error_response, + FirmwareFilePath& out_firmware_file_path) = 0; + + /** + * This node is invoked when the node responds to the update request with confirmation. + * Note that if by the time of arrival of the response the node is already removed, this method will not be called. + * + * Implementation is optional; default one does nothing. + * + * @param node_id Node ID that confirmed the request. + * + * @param response Actual response. + */ + virtual void handleFirmwareUpdateConfirmation(NodeID node_id, + const protocol::file::BeginFirmwareUpdate::Response& response) + { + (void)node_id; + (void)response; + } + + virtual ~IFirmwareVersionChecker() { } +}; + +/** + * This class subscribes to updates from @ref NodeInfoRetriever in order to detect nodes that need firmware + * updates. The decision process of whether a firmware update is needed is relayed to the application via + * @ref IFirmwareVersionChecker. If the application confirms that the update is needed, this class will begin + * sending uavcan.protocol.file.BeginFirmwareUpdate periodically (period is configurable) to every node that + * needs an update in a round-robin fashion. There are the following termination conditions for the periodical + * sending process: + * + * - The node responds with confirmation. In this case the class will forget about the node on the assumption + * that its job is done here. Confirmation will be reported to the application via the interface. + * + * - The node responds with an error, and the error code is not ERROR_IN_PROGRESS. In this case the class will + * request the application via the interface mentioned above about its further actions - either give up or + * retry, possibly with a different firmware. + * + * - The node responds with error ERROR_IN_PROGRESS. In this case the class behaves exactly in the same way as if + * response was successful (because the firmware is alredy being updated, so the goal is fulfilled). + * Confirmation will be reported to the application via the interface. + * + * - The node goes offline or restarts. In this case the node will be immediately forgotten, and the process + * will repeat again later because the node info retriever re-queries GetNodeInfo every time when a node restarts. + * + * Since the target node (i.e. node that is being updated) will try to retrieve the specified firmware file using + * the file services (uavcan.protocol.file.*), the provided firmware path must be reachable for the file server + * (@ref FileServer, @ref BasicFileServer). Normally, an application that serves as UAVCAN firmware update server + * will include at least the following components: + * - this firmware update trigger; + * - dynamic node ID allocation server; + * - file server. + * + * Implementation details: the class uses memory pool to keep the list of nodes that have not responded yet, which + * limits the maximum length of the path to the firmware file, which is covered in @ref IFirmwareVersionChecker. + * To somewhat relieve the maximum path length limitation, the class can be supplied with a common prefix that + * will be prepended to firmware pathes before sending requests. + * Interval at which requests are being sent is configurable, but the default value should cover the needs of + * virtually all use cases (as always). + */ +class FirmwareUpdateTrigger : public INodeInfoListener, + private TimerBase +{ + typedef MethodBinder&)> + BeginFirmwareUpdateResponseCallback; + + typedef IFirmwareVersionChecker::FirmwareFilePath FirmwareFilePath; + + enum { DefaultRequestIntervalMs = 1000 }; ///< Shall not be less than default service response timeout. + + struct NextNodeIDSearchPredicate : ::uavcan::Noncopyable + { + enum { DefaultOutput = 0xFFU }; + + const FirmwareUpdateTrigger& owner; + uint8_t output; + + NextNodeIDSearchPredicate(const FirmwareUpdateTrigger& arg_owner) + : owner(arg_owner) + , output(DefaultOutput) + { } + + bool operator()(const NodeID node_id, const FirmwareFilePath&) + { + if (node_id.get() > owner.last_queried_node_id_ && + !owner.begin_fw_update_client_.hasPendingCallToServer(node_id)) + { + output = min(output, node_id.get()); + } + return false; + } + }; + + /* + * State + */ + ServiceClient begin_fw_update_client_; + + IFirmwareVersionChecker& checker_; + + NodeInfoRetriever* node_info_retriever_; + + Map pending_nodes_; + + MonotonicDuration request_interval_; + + FirmwareFilePath common_path_prefix_; + + mutable uint8_t last_queried_node_id_; + + /* + * Methods of INodeInfoListener + */ + virtual void handleNodeInfoUnavailable(NodeID node_id) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d could not provide GetNodeInfo response", int(node_id.get())); + pending_nodes_.remove(node_id); // For extra paranoia + } + + virtual void handleNodeInfoRetrieved(const NodeID node_id, const protocol::GetNodeInfo::Response& node_info) + { + FirmwareFilePath firmware_file_path; + const bool update_needed = checker_.shouldRequestFirmwareUpdate(node_id, node_info, firmware_file_path); + if (update_needed) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d requires update; file path: %s", + int(node_id.get()), firmware_file_path.c_str()); + trySetPendingNode(node_id, firmware_file_path); + } + else + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d does not need update", int(node_id.get())); + pending_nodes_.remove(node_id); + } + } + + virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event) + { + if (event.status.mode == protocol::NodeStatus::MODE_OFFLINE) + { + pending_nodes_.remove(event.node_id); + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d is offline hence forgotten", int(event.node_id.get())); + } + } + + /* + * Own methods + */ + INode& getNode() { return begin_fw_update_client_.getNode(); } + + void trySetPendingNode(const NodeID node_id, const FirmwareFilePath& path) + { + if (UAVCAN_NULLPTR != pending_nodes_.insert(node_id, path)) + { + if (!TimerBase::isRunning()) + { + TimerBase::startPeriodic(request_interval_); + UAVCAN_TRACE("FirmwareUpdateTrigger", "Timer started"); + } + } + else + { + getNode().registerInternalFailure("FirmwareUpdateTrigger OOM"); + } + } + + NodeID pickNextNodeID() const + { + // We can't do index search because indices are unstable in Map<> + // First try - from the current node up + NextNodeIDSearchPredicate s1(*this); + (void)pending_nodes_.find(s1); + + if (s1.output != NextNodeIDSearchPredicate::DefaultOutput) + { + last_queried_node_id_ = s1.output; + } + else if (last_queried_node_id_ != 0) + { + // Nothing was found, resetting the selector and trying again + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node selector reset, last value: %d", int(last_queried_node_id_)); + last_queried_node_id_ = 0; + + NextNodeIDSearchPredicate s2(*this); + (void)pending_nodes_.find(s2); + + if (s2.output != NextNodeIDSearchPredicate::DefaultOutput) + { + last_queried_node_id_ = s2.output; + } + } + else + { + ; // Hopeless + } + UAVCAN_TRACE("FirmwareUpdateTrigger", "Next node ID to query: %d, pending nodes: %u, pending calls: %u", + int(last_queried_node_id_), pending_nodes_.getSize(), + begin_fw_update_client_.getNumPendingCalls()); + return last_queried_node_id_; + } + + void handleBeginFirmwareUpdateResponse(const ServiceCallResult& result) + { + if (!result.isSuccessful()) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d has timed out, will retry", + int(result.getCallID().server_node_id.get())); + return; + } + + FirmwareFilePath* const old_path = pending_nodes_.access(result.getCallID().server_node_id); + if (old_path == UAVCAN_NULLPTR) + { + // The entry has been removed, assuming that it's not needed anymore + return; + } + + const bool confirmed = + result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_OK || + result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; + + if (confirmed) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d confirmed the update request", + int(result.getCallID().server_node_id.get())); + pending_nodes_.remove(result.getCallID().server_node_id); + checker_.handleFirmwareUpdateConfirmation(result.getCallID().server_node_id, result.getResponse()); + } + else + { + UAVCAN_ASSERT(old_path != UAVCAN_NULLPTR); + UAVCAN_ASSERT(TimerBase::isRunning()); + // We won't have to call trySetPendingNode(), because we'll directly update the old path via the pointer + + const bool update_needed = + checker_.shouldRetryFirmwareUpdate(result.getCallID().server_node_id, result.getResponse(), *old_path); + + if (!update_needed) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d does not need retry", + int(result.getCallID().server_node_id.get())); + pending_nodes_.remove(result.getCallID().server_node_id); + } + } + } + + virtual void handleTimerEvent(const TimerEvent&) + { + if (pending_nodes_.isEmpty()) + { + TimerBase::stop(); + UAVCAN_TRACE("FirmwareUpdateTrigger", "Timer stopped"); + return; + } + + const NodeID node_id = pickNextNodeID(); + if (!node_id.isUnicast()) + { + return; + } + + FirmwareFilePath* const path = pending_nodes_.access(node_id); + if (path == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); // pickNextNodeID() returned a node ID that is not present in the map + return; + } + + protocol::file::BeginFirmwareUpdate::Request req; + + req.source_node_id = getNode().getNodeID().get(); + req.image_file_remote_path.path = path->c_str(); + + UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d with path: %s", + int(node_id.get()), req.image_file_remote_path.path.c_str()); + + const int call_res = begin_fw_update_client_.call(node_id, req); + if (call_res < 0) + { + getNode().registerInternalFailure("FirmwareUpdateTrigger call"); + } + } + +public: + FirmwareUpdateTrigger(INode& node, IFirmwareVersionChecker& checker) + : TimerBase(node) + , begin_fw_update_client_(node) + , checker_(checker) + , node_info_retriever_(UAVCAN_NULLPTR) + , pending_nodes_(node.getAllocator()) + , request_interval_(MonotonicDuration::fromMSec(DefaultRequestIntervalMs)) + , last_queried_node_id_(0) + { } + + ~FirmwareUpdateTrigger() + { + if (node_info_retriever_ != UAVCAN_NULLPTR) + { + node_info_retriever_->removeListener(this); + } + } + + /** + * Starts the object. Once started, it can't be stopped unless destroyed. + * + * @param node_info_retriever The object will register itself against this retriever. + * When the destructor is called, the object will unregister itself. + * + * @param common_path_prefix If set, this path will be prefixed to all firmware pathes provided by the + * application interface. The prefix does not need to end with path separator; + * the last trailing one will be removed (so use '//' if you need '/'). + * By default the prefix is empty. + * + * @return Negative error code. + */ + int start(NodeInfoRetriever& node_info_retriever, + const FirmwareFilePath& arg_common_path_prefix = FirmwareFilePath(), + const FirmwareFilePath& arg_alt_path_prefix = FirmwareFilePath(), + const TransferPriority priority = TransferPriority::OneHigherThanLowest) + { + /* + * Configuring the node info retriever + */ + node_info_retriever_ = &node_info_retriever; + + int res = node_info_retriever_->addListener(this); + if (res < 0) + { + return res; + } + + /* + * Initializing the prefix, removing only the last trailing path separator. + */ + common_path_prefix_ = arg_common_path_prefix; + + if (!common_path_prefix_.empty() && + *(common_path_prefix_.end() - 1) == protocol::file::Path::SEPARATOR) + { + common_path_prefix_.resize(uint8_t(common_path_prefix_.size() - 1U)); + } + + /* + * Initializing the client + */ + res = begin_fw_update_client_.init(priority); + if (res < 0) + { + return res; + } + begin_fw_update_client_.setCallback( + BeginFirmwareUpdateResponseCallback(this, &FirmwareUpdateTrigger::handleBeginFirmwareUpdateResponse)); + + // The timer will be started ad-hoc + return 0; + } + + /** + * Interval at which uavcan.protocol.file.BeginFirmwareUpdate requests are being sent. + * Note that default value should be OK for any use case. + */ + MonotonicDuration getRequestInterval() const { return request_interval_; } + void setRequestInterval(const MonotonicDuration interval) + { + if (interval.isPositive()) + { + request_interval_ = interval; + if (TimerBase::isRunning()) // Restarting with new interval + { + TimerBase::startPeriodic(request_interval_); + } + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * This method is mostly needed for testing. + * When triggering is not in progress, the class consumes zero CPU time. + */ + bool isTimerRunning() const { return TimerBase::isRunning(); } + + unsigned getNumPendingNodes() const + { + const unsigned ret = pending_nodes_.getSize(); + UAVCAN_ASSERT((ret > 0) ? isTimerRunning() : true); + return ret; + } + +}; + +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp new file mode 100644 index 000000000000..e64fb478338b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp @@ -0,0 +1,262 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Please read the specs to learn how the time synchronization works. + * + * No more than one object of this class is allowed per node; otherwise a disaster is bound to happen. + * + * NOTE: In order for this class to work, the platform driver must implement + * CAN bus TX loopback with both UTC and monotonic timestamping. + * + * Ref. M. Gergeleit, H. Streich - "Implementing a Distributed High-Resolution Real-Time Clock using the CAN-Bus" + * + * TODO: Enforce max one master per node + */ +class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase +{ + class IfaceMaster + { + Publisher pub_; + MonotonicTime iface_prev_pub_mono_; + UtcTime prev_tx_utc_; + const uint8_t iface_index_; + + public: + IfaceMaster(INode& node, uint8_t iface_index) + : pub_(node) + , iface_index_(iface_index) + { + UAVCAN_ASSERT(iface_index < MaxCanIfaces); + } + + int init(TransferPriority priority) + { + const int res = pub_.init(priority); + if (res >= 0) + { + pub_.getTransferSender().setIfaceMask(uint8_t(1 << iface_index_)); + pub_.getTransferSender().setCanIOFlags(CanIOFlagLoopback); + } + return res; + } + + void setTxTimestamp(UtcTime ts) + { + if (ts.isZero()) + { + UAVCAN_ASSERT(0); + pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster zero TX ts"); + return; + } + if (!prev_tx_utc_.isZero()) + { + prev_tx_utc_ = UtcTime(); // Reset again, because there's something broken in the driver and we don't trust it + pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster pub conflict"); + return; + } + prev_tx_utc_ = ts; + } + + int publish(TransferID tid, MonotonicTime current_time) + { + UAVCAN_ASSERT(pub_.getTransferSender().getCanIOFlags() == CanIOFlagLoopback); + UAVCAN_ASSERT(pub_.getTransferSender().getIfaceMask() == (1 << iface_index_)); + + const MonotonicDuration since_prev_pub = current_time - iface_prev_pub_mono_; + iface_prev_pub_mono_ = current_time; + UAVCAN_ASSERT(since_prev_pub.isPositive()); + const bool long_period = since_prev_pub.toMSec() >= protocol::GlobalTimeSync::MAX_BROADCASTING_PERIOD_MS; + + protocol::GlobalTimeSync msg; + msg.previous_transmission_timestamp_usec = long_period ? 0 : prev_tx_utc_.toUSec(); + prev_tx_utc_ = UtcTime(); + + UAVCAN_TRACE("GlobalTimeSyncMaster", "Publishing %llu iface=%i tid=%i", + static_cast(msg.previous_transmission_timestamp_usec), + int(iface_index_), int(tid.get())); + return pub_.broadcast(msg, tid); + } + }; + + INode& node_; + LazyConstructor iface_masters_[MaxCanIfaces]; + MonotonicTime prev_pub_mono_; + DataTypeID dtid_; + bool initialized_; + + virtual void handleLoopbackFrame(const RxFrame& frame) + { + const uint8_t iface = frame.getIfaceIndex(); + if (initialized_ && iface < MaxCanIfaces) + { + if (frame.getDataTypeID() == dtid_ && + frame.getTransferType() == TransferTypeMessageBroadcast && + frame.isStartOfTransfer() && frame.isEndOfTransfer() && + frame.getSrcNodeID() == node_.getNodeID()) + { + iface_masters_[iface]->setTxTimestamp(frame.getUtcTimestamp()); + } + } + else + { + UAVCAN_ASSERT(0); + } + } + + int getNextTransferID(TransferID& tid) + { + const MonotonicDuration max_transfer_interval = + MonotonicDuration::fromMSec(protocol::GlobalTimeSync::MAX_BROADCASTING_PERIOD_MS); + + const OutgoingTransferRegistryKey otr_key(dtid_, TransferTypeMessageBroadcast, NodeID::Broadcast); + const MonotonicTime otr_deadline = node_.getMonotonicTime() + max_transfer_interval; + TransferID* const tid_ptr = + node_.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline); + if (tid_ptr == UAVCAN_NULLPTR) + { + return -ErrMemory; + } + + tid = *tid_ptr; + tid_ptr->increment(); + return 0; + } + +public: + explicit GlobalTimeSyncMaster(INode& node) + : LoopbackFrameListenerBase(node.getDispatcher()) + , node_(node) + , initialized_(false) + { } + + /** + * Merely prepares the class to work, doesn't do anything else. + * Must be called before the master can be used. + * Returns negative error code. + */ + int init(const TransferPriority priority = TransferPriority::OneLowerThanHighest) + { + if (initialized_) + { + return 0; + } + + // Data type ID + const DataTypeDescriptor* const desc = + GlobalDataTypeRegistry::instance().find(DataTypeKindMessage, protocol::GlobalTimeSync::getDataTypeFullName()); + if (desc == UAVCAN_NULLPTR) + { + return -ErrUnknownDataType; + } + dtid_ = desc->getID(); + + // Iface master array + int res = -ErrLogic; + for (uint8_t i = 0; i < MaxCanIfaces; i++) + { + if (!iface_masters_[i].isConstructed()) + { + iface_masters_[i].construct(node_, i); + } + res = iface_masters_[i]->init(priority); + if (res < 0) + { + break; + } + } + + // Loopback listener + initialized_ = res >= 0; + if (initialized_) + { + LoopbackFrameListenerBase::startListening(); + } + return res; + } + + /** + * Whether the master instance has been initialized. + */ + bool isInitialized() const { return initialized_; } + + /** + * Publishes one sync message. + * + * Every call to this method hints the master to publish the next sync message once. Exact time will + * be obtained from the TX loopback timestamp field. + * + * This method shall be called with a proper interval - refer to the time sync message definition + * for min/max interval values. + * + * Returns negative error code. + */ + int publish() + { + if (!initialized_) + { + const int res = init(); + if (res < 0) + { + return res; + } + } + + /* + * Enforce max frequency + */ + const MonotonicTime current_time = node_.getMonotonicTime(); + { + const MonotonicDuration since_prev_pub = current_time - prev_pub_mono_; + UAVCAN_ASSERT(since_prev_pub.isPositive()); + if (since_prev_pub.toMSec() < protocol::GlobalTimeSync::MIN_BROADCASTING_PERIOD_MS) + { + UAVCAN_TRACE("GlobalTimeSyncMaster", "Publication skipped"); + return 0; + } + prev_pub_mono_ = current_time; + } + + /* + * Obtain common Transfer ID for all masters + */ + TransferID tid; + { + const int tid_res = getNextTransferID(tid); + if (tid_res < 0) + { + return tid_res; + } + } + + for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) + { + const int res = iface_masters_[i]->publish(tid, current_time); + if (res < 0) + { + return res; + } + } + return 0; + } +}; + +} + +#endif // UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp new file mode 100644 index 000000000000..770cd721ad6a --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp @@ -0,0 +1,198 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED +#define UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Please read the specs to learn how the time synchronization works. + * + * No more than one object of this class is allowed per node; otherwise a disaster is bound to happen. + * + * NOTE: In order for this class to work, the platform driver must implement: + * - CAN bus RX UTC timestamping; + * - Clock adjustment method in the system clock interface @ref ISystemClock::adjustUtc(). + * + * Ref. M. Gergeleit, H. Streich - "Implementing a Distributed High-Resolution Real-Time Clock using the CAN-Bus" + * http://modecs.cs.uni-salzburg.at/results/related_documents/CAN_clock.pdf + */ +class UAVCAN_EXPORT GlobalTimeSyncSlave : Noncopyable +{ + typedef MethodBinder&)> + GlobalTimeSyncCallback; + + Subscriber sub_; + + UtcTime prev_ts_utc_; + MonotonicTime prev_ts_mono_; + MonotonicTime last_adjustment_ts_; + enum State { Update, Adjust } state_; + NodeID master_nid_; + TransferID prev_tid_; + uint8_t prev_iface_index_; + bool suppressed_; + + ISystemClock& getSystemClock() const { return sub_.getNode().getSystemClock(); } + + void adjustFromMsg(const ReceivedDataStructure& msg) + { + UAVCAN_ASSERT(msg.previous_transmission_timestamp_usec > 0); + const UtcDuration adjustment = UtcTime::fromUSec(msg.previous_transmission_timestamp_usec) - prev_ts_utc_; + + UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment: usec=%lli snid=%i iface=%i suppress=%i", + static_cast(adjustment.toUSec()), + int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()), int(suppressed_)); + + if (!suppressed_) + { + getSystemClock().adjustUtc(adjustment); + } + last_adjustment_ts_ = msg.getMonotonicTimestamp(); + state_ = Update; + } + + void updateFromMsg(const ReceivedDataStructure& msg) + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Update: snid=%i iface=%i", + int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex())); + + prev_ts_utc_ = msg.getUtcTimestamp(); + prev_ts_mono_ = msg.getMonotonicTimestamp(); + master_nid_ = msg.getSrcNodeID(); + prev_iface_index_ = msg.getIfaceIndex(); + prev_tid_ = msg.getTransferID(); + state_ = Adjust; + } + + void processMsg(const ReceivedDataStructure& msg) + { + const MonotonicDuration since_prev_msg = msg.getMonotonicTimestamp() - prev_ts_mono_; + UAVCAN_ASSERT(!since_prev_msg.isNegative()); + + const bool needs_init = !master_nid_.isValid() || prev_ts_mono_.isZero(); + const bool switch_master = msg.getSrcNodeID() < master_nid_; + // TODO: Make configurable + const bool pub_timeout = since_prev_msg.toMSec() > protocol::GlobalTimeSync::RECOMMENDED_BROADCASTER_TIMEOUT_MS; + + if (switch_master || pub_timeout || needs_init) + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i pub_timeout=%i", + int(needs_init), int(switch_master), int(pub_timeout)); + updateFromMsg(msg); + } + else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_) + { + if (state_ == Adjust) + { + const bool msg_invalid = msg.previous_transmission_timestamp_usec == 0; + const bool wrong_tid = prev_tid_.computeForwardDistance(msg.getTransferID()) != 1; + const bool wrong_timing = since_prev_msg.toMSec() > protocol::GlobalTimeSync::MAX_BROADCASTING_PERIOD_MS; + if (msg_invalid || wrong_tid || wrong_timing) + { + UAVCAN_TRACE("GlobalTimeSyncSlave", + "Adjustment skipped: msg_invalid=%i wrong_tid=%i wrong_timing=%i", + int(msg_invalid), int(wrong_tid), int(wrong_timing)); + state_ = Update; + } + } + if (state_ == Adjust) + { + adjustFromMsg(msg); + } + else + { + updateFromMsg(msg); + } + } + else + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Ignored: snid=%i iface=%i", + int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex())); + } + } + + void handleGlobalTimeSync(const ReceivedDataStructure& msg) + { + if (msg.getTransferType() == TransferTypeMessageBroadcast) + { + processMsg(msg); + } + else + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Invalid transfer type %i", int(msg.getTransferType())); + } + } + +public: + explicit GlobalTimeSyncSlave(INode& node) + : sub_(node) + , state_(Update) + , prev_iface_index_(0xFF) + , suppressed_(false) + { } + + /** + * Starts the time sync slave. Once started, it works on its own and does not require any + * attention from the application, other than to handle a clock adjustment request occasionally. + * Returns negative error code. + */ + int start() + { + return sub_.start(GlobalTimeSyncCallback(this, &GlobalTimeSyncSlave::handleGlobalTimeSync)); + } + + /** + * Enable or disable the suppressed mode. + * + * In suppressed mode the slave will continue tracking time sync masters in the network, but will not + * perform local clock adjustments. So it's kind of a dry run - all the time sync logic works except + * the local clock will not receive adjustments. + * + * Suppressed mode is useful for nodes that can act as a back-up clock sync masters - as long as the + * node sees a higher priority time sync master in the network, its slave will be NOT suppressed + * in order to sync the local clock with the global master. As soon as all other higher priority + * masters go down, the local node will suppress its time sync slave instance and become a new master. + * + * Suppressed mode is disabled by default. + */ + void suppress(bool suppressed) { suppressed_ = suppressed; } + bool isSuppressed() const { return suppressed_; } + + /** + * If the clock sync slave sees any clock sync masters in the network, it is ACTIVE. + * When the last master times out (PUBLISHER_TIMEOUT), the slave will be INACTIVE. + * Note that immediately after start up the slave will be INACTIVE until it finds a master. + * Please read the specs to learn more. + */ + bool isActive() const + { + const MonotonicDuration since_prev_adj = getSystemClock().getMonotonic() - last_adjustment_ts_; + return !last_adjustment_ts_.isZero() && + (since_prev_adj.toMSec() <= protocol::GlobalTimeSync::RECOMMENDED_BROADCASTER_TIMEOUT_MS); + } + + /** + * Node ID of the master the slave is currently locked on. + * Returns an invalid Node ID if there's no active master. + */ + NodeID getMasterNodeID() const { return isActive() ? master_nid_ : NodeID(); } + + /** + * Last time when the local clock adjustment was performed. + */ + MonotonicTime getLastAdjustmentTime() const { return last_adjustment_ts_; } +}; + +} + +#endif // UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/logger.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/logger.hpp new file mode 100644 index 000000000000..07a6ba6a5689 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/logger.hpp @@ -0,0 +1,286 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_LOGGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_LOGGER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +namespace uavcan +{ +/** + * External log sink interface. + * External log sink allows the application to install a hook on the logger output. + * This can be used for application-wide logging. + * Please refer to the @ref Logger class docs. + */ +class UAVCAN_EXPORT ILogSink +{ +public: + typedef typename StorageType::Type LogLevel; + + virtual ~ILogSink() { } + + /** + * Logger will not sink messages with a severity level lower than returned by this method. + * Default level is DEBUG. + */ + virtual LogLevel getLogLevel() const { return protocol::debug::LogLevel::DEBUG; } + + /** + * Logger will call this method for every log message which severity level + * is not less than the current level of this sink. + */ + virtual void log(const protocol::debug::LogMessage& message) = 0; +}; + +/** + * Node logging convenience class. + * + * This class is based on the standard UAVCAN message type for logging - uavcan.protocol.debug.LogMessage. + * + * Provides logging methods of different severity; implements two sinks for the log messages: + * - Broadcast via the UAVCAN bus; + * - Sink into the application via @ref ILogSink. + * + * For each sink an individual severity threshold filter can be configured. + */ +class UAVCAN_EXPORT Logger +{ +public: + typedef ILogSink::LogLevel LogLevel; + + /** + * This value is higher than any valid severity value. + * Use it to completely suppress the output. + */ + static LogLevel getLogLevelAboveAll() { return (1U << protocol::debug::LogLevel::FieldTypes::value::BitLen) - 1U; } + +private: + enum { DefaultTxTimeoutMs = 2000 }; + + Publisher logmsg_pub_; + protocol::debug::LogMessage msg_buf_; + LogLevel level_; + ILogSink* external_sink_; + + LogLevel getExternalSinkLevel() const + { + return (external_sink_ == UAVCAN_NULLPTR) ? getLogLevelAboveAll() : external_sink_->getLogLevel(); + } + +public: + explicit Logger(INode& node) + : logmsg_pub_(node) + , external_sink_(UAVCAN_NULLPTR) + { + level_ = protocol::debug::LogLevel::ERROR; + setTxTimeout(MonotonicDuration::fromMSec(DefaultTxTimeoutMs)); + UAVCAN_ASSERT(getTxTimeout() == MonotonicDuration::fromMSec(DefaultTxTimeoutMs)); + } + + /** + * Initializes the logger, does not perform any network activity. + * Must be called once before use. + * Returns negative error code. + */ + int init(const TransferPriority priority = TransferPriority::Lowest) + { + const int res = logmsg_pub_.init(priority); + if (res < 0) + { + return res; + } + return 0; + } + + /** + * Logs one message. Please consider using helper methods instead of this one. + * + * The message will be broadcasted via the UAVCAN bus if the severity level of the + * message is >= severity level of the logger. + * + * The message will be reported into the external log sink if the external sink is + * installed and the severity level of the message is >= severity level of the external sink. + * + * Returns negative error code. + */ + int log(const protocol::debug::LogMessage& message) + { + int retval = 0; + if (message.level.value >= getExternalSinkLevel()) + { + external_sink_->log(message); + } + if (message.level.value >= level_) + { + retval = logmsg_pub_.broadcast(message); + } + return retval; + } + + /** + * Severity filter for UAVCAN broadcasting. + * Log message will be broadcasted via the UAVCAN network only if its severity is >= getLevel(). + * This does not affect the external sink. + * Default level is ERROR. + */ + LogLevel getLevel() const { return level_; } + void setLevel(LogLevel level) { level_ = level; } + + /** + * External log sink allows the application to install a hook on the logger output. + * This can be used for application-wide logging. + * Null pointer means that there's no log sink (can be used to remove it). + * By default there's no log sink. + */ + ILogSink* getExternalSink() const { return external_sink_; } + void setExternalSink(ILogSink* sink) { external_sink_ = sink; } + + /** + * Log message broadcast transmission timeout. + * The default value should be acceptable for any use case. + */ + MonotonicDuration getTxTimeout() const { return logmsg_pub_.getTxTimeout(); } + void setTxTimeout(MonotonicDuration val) { logmsg_pub_.setTxTimeout(val); } + + /** + * Helper methods for various severity levels and with formatting support. + * These methods build a formatted log message and pass it into the method @ref log(). + * + * Format string usage is a bit unusual: use "%*" for any argument type, use "%%" to print a percent character. + * No other formating options are supported. Insufficient/extra arguments are ignored. + * + * Example format string: + * "What do you get if you %* %* by %*? %*. Extra arguments: %* %* %%" + * ...with the following arguments: + * "multiply", 6, 9.0F 4.2e1 + * ...will likely produce this (floating point representation is platform dependent): + * "What do you get if you multiply 6 by 9.000000? 42.000000. Extra arguments: %* %* %" + * + * Formatting is not supported in C++03 mode. + * @{ + */ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + + template + int log(LogLevel level, const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT; + + template + inline int logDebug(const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::DEBUG, source, format, args...); + } + + template + inline int logInfo(const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::INFO, source, format, args...); + } + + template + inline int logWarning(const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::WARNING, source, format, args...); + } + + template + inline int logError(const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::ERROR, source, format, args...); + } + +#else + + int log(LogLevel level, const char* source, const char* text) UAVCAN_NOEXCEPT + { + #if UAVCAN_EXCEPTIONS + try + #endif + { + if (level >= level_ || level >= getExternalSinkLevel()) + { + msg_buf_.level.value = level; + msg_buf_.source = source; + msg_buf_.text = text; + return log(msg_buf_); + } + return 0; + } + #if UAVCAN_EXCEPTIONS + catch (...) + { + return -ErrFailure; + } + #endif + } + + int logDebug(const char* source, const char* text) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::DEBUG, source, text); + } + + int logInfo(const char* source, const char* text) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::INFO, source, text); + } + + int logWarning(const char* source, const char* text) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::WARNING, source, text); + } + + int logError(const char* source, const char* text) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::ERROR, source, text); + } + +#endif + /** + * @} + */ +}; + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +template +int Logger::log(LogLevel level, const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT +{ +#if UAVCAN_EXCEPTIONS + try +#endif + { + if (level >= level_ || level >= getExternalSinkLevel()) + { + msg_buf_.level.value = level; + msg_buf_.source = source; + msg_buf_.text.clear(); + CharArrayFormatter formatter(msg_buf_.text); + formatter.write(format, args...); + return log(msg_buf_); + } + return 0; + } +#if UAVCAN_EXCEPTIONS + catch (...) + { + return -ErrFailure; + } +#endif +} + +#endif + +} + +#endif // UAVCAN_PROTOCOL_LOGGER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_info_retriever.hpp new file mode 100644 index 000000000000..a1549f763c74 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -0,0 +1,464 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Classes that need to receive GetNodeInfo responses should implement this interface. + */ +class UAVCAN_EXPORT INodeInfoListener +{ +public: + /** + * Called when a response to GetNodeInfo request is received. This happens shortly after the node restarts or + * becomes online for the first time. + * @param node_id Node ID of the node + * @param response Node info struct + */ + virtual void handleNodeInfoRetrieved(NodeID node_id, const protocol::GetNodeInfo::Response& node_info) = 0; + + /** + * Called when the retriever decides that the node does not support the GetNodeInfo service. + * This method will never be called if the number of attempts is unlimited. + */ + virtual void handleNodeInfoUnavailable(NodeID node_id) = 0; + + /** + * This call is routed directly from @ref NodeStatusMonitor. + * Default implementation does nothing. + * @param event Node status change event + */ + virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event) + { + (void)event; + } + + /** + * This call is routed directly from @ref NodeStatusMonitor. + * Default implementation does nothing. + * @param msg Node status message + */ + virtual void handleNodeStatusMessage(const ReceivedDataStructure& msg) + { + (void)msg; + } + + virtual ~INodeInfoListener() { } +}; + +/** + * This class automatically retrieves a response to GetNodeInfo once a node appears online or restarts. + * It does a number of attempts in case if there's a communication failure before assuming that the node does not + * implement the GetNodeInfo service. All parameters are pre-configured with sensible default values that should fit + * virtually any use case, but they can be overriden if needed - refer to the setter methods below for details. + * + * Defaults are pre-configured so that the class is able to query 123 nodes (node ID 1..125, where 1 is our local + * node and 1 is one node that implements GetNodeInfo service, hence 123) of which none implements GetNodeInfo + * service in under 5 seconds. The 5 second limitation is imposed by UAVCAN-compatible bootloaders, which are + * unlikely to wait for more than that before continuing to boot. In case if this default value is not appropriate + * for the end application, the request interval can be overriden via @ref setRequestInterval(). + * + * Following the above explained requirements, the default request interval is defined as follows: + * request interval [ms] = floor(5000 [ms] bootloader timeout / 123 nodes) + * Which yields 40 ms. + * + * Given default service timeout 1000 ms and the defined above request frequency 40 ms, the maximum number of + * concurrent requests will be: + * max concurrent requests = ceil(1000 [ms] timeout / 40 [ms] request interval) + * Which yields 25 requests. + * + * Keep the above equations in mind when changing the default request interval. + * + * Obviously, if all calls are completing in under (request interval), the number of concurrent requests will never + * exceed one. This is actually the most likely scenario. + * + * Note that all nodes are queried in a round-robin fashion, regardless of their uptime, number of requests made, etc. + * + * Events from this class can be routed to many listeners, @ref INodeInfoListener. + */ +class UAVCAN_EXPORT NodeInfoRetriever : public NodeStatusMonitor + , TimerBase +{ +public: + enum { MaxNumRequestAttempts = 254 }; + enum { UnlimitedRequestAttempts = 0 }; + +private: + typedef MethodBinder&)> + GetNodeInfoResponseCallback; + + struct Entry + { + uint32_t uptime_sec; + uint8_t num_attempts_made; + bool request_needed; ///< Always false for unknown nodes + bool updated_since_last_attempt; ///< Always false for unknown nodes + + Entry() + : uptime_sec(0) + , num_attempts_made(0) + , request_needed(false) + , updated_since_last_attempt(false) + { +#if UAVCAN_DEBUG + StaticAssert::check(); +#endif + } + }; + + struct NodeInfoRetrievedHandlerCaller + { + const NodeID node_id; + const protocol::GetNodeInfo::Response& node_info; + + NodeInfoRetrievedHandlerCaller(NodeID arg_node_id, const protocol::GetNodeInfo::Response& arg_node_info) + : node_id(arg_node_id) + , node_info(arg_node_info) + { } + + bool operator()(INodeInfoListener* key) + { + UAVCAN_ASSERT(key != UAVCAN_NULLPTR); + key->handleNodeInfoRetrieved(node_id, node_info); + return false; + } + }; + + template + struct GenericHandlerCaller + { + void (INodeInfoListener::* const method)(Event); + Event event; + + GenericHandlerCaller(void (INodeInfoListener::*arg_method)(Event), Event arg_event) + : method(arg_method) + , event(arg_event) + { } + + bool operator()(INodeInfoListener* key) + { + UAVCAN_ASSERT(key != UAVCAN_NULLPTR); + (key->*method)(event); + return false; + } + }; + + enum { DefaultNumRequestAttempts = 16 }; + enum { DefaultTimerIntervalMSec = 40 }; ///< Read explanation in the class documentation + + /* + * State + */ + Entry entries_[NodeID::Max]; // [1, NodeID::Max] + + Multiset listeners_; + + ServiceClient get_node_info_client_; + + MonotonicDuration request_interval_; + + mutable uint8_t last_picked_node_; + + uint8_t num_attempts_; + + /* + * Methods + */ + const Entry& getEntry(NodeID node_id) const { return const_cast(this)->getEntry(node_id); } + Entry& getEntry(NodeID node_id) + { + if (node_id.get() < 1 || node_id.get() > NodeID::Max) + { + handleFatalError("NodeInfoRetriever NodeID"); + } + return entries_[node_id.get() - 1]; + } + + void startTimerIfNotRunning() + { + if (!TimerBase::isRunning()) + { + TimerBase::startPeriodic(request_interval_); + UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %s sec", request_interval_.toString().c_str()); + } + } + + NodeID pickNextNodeToQuery(bool& out_at_least_one_request_needed) const + { + out_at_least_one_request_needed = false; + + for (unsigned iter_cnt_ = 0; iter_cnt_ < (sizeof(entries_) / sizeof(entries_[0])); iter_cnt_++) // Round-robin + { + last_picked_node_++; + if (last_picked_node_ > NodeID::Max) + { + last_picked_node_ = 1; + } + UAVCAN_ASSERT((last_picked_node_ >= 1) && + (last_picked_node_ <= NodeID::Max)); + + const Entry& entry = getEntry(last_picked_node_); + + if (entry.request_needed) + { + out_at_least_one_request_needed = true; + + if (entry.updated_since_last_attempt && + !get_node_info_client_.hasPendingCallToServer(last_picked_node_)) + { + UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_)); + return NodeID(last_picked_node_); + } + } + } + + return NodeID(); // No node could be found + } + + virtual void handleTimerEvent(const TimerEvent&) + { + bool at_least_one_request_needed = false; + const NodeID next = pickNextNodeToQuery(at_least_one_request_needed); + + if (next.isUnicast()) + { + UAVCAN_ASSERT(at_least_one_request_needed); + getEntry(next).updated_since_last_attempt = false; + const int res = get_node_info_client_.call(next, protocol::GetNodeInfo::Request()); + if (res < 0) + { + get_node_info_client_.getNode().registerInternalFailure("NodeInfoRetriever GetNodeInfo call"); + } + } + else + { + if (!at_least_one_request_needed) + { + TimerBase::stop(); + UAVCAN_TRACE("NodeInfoRetriever", "Timer stopped"); + } + } + } + + virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event) + { + const bool was_offline = !event.was_known || + (event.old_status.mode == protocol::NodeStatus::MODE_OFFLINE); + + const bool offline_now = event.status.mode == protocol::NodeStatus::MODE_OFFLINE; + + if (was_offline || offline_now) + { + Entry& entry = getEntry(event.node_id); + + entry.request_needed = !offline_now; + entry.num_attempts_made = 0; + + UAVCAN_TRACE("NodeInfoRetriever", "Offline status change: node ID %d, request needed: %d", + int(event.node_id.get()), int(entry.request_needed)); + + if (entry.request_needed) + { + startTimerIfNotRunning(); + } + } + + listeners_.forEach( + GenericHandlerCaller(&INodeInfoListener::handleNodeStatusChange, event)); + } + + virtual void handleNodeStatusMessage(const ReceivedDataStructure& msg) + { + Entry& entry = getEntry(msg.getSrcNodeID()); + + if (msg.uptime_sec < entry.uptime_sec) + { + entry.request_needed = true; + entry.num_attempts_made = 0; + + startTimerIfNotRunning(); + } + entry.uptime_sec = msg.uptime_sec; + entry.updated_since_last_attempt = true; + + listeners_.forEach(GenericHandlerCaller&>( + &INodeInfoListener::handleNodeStatusMessage, msg)); + } + + void handleGetNodeInfoResponse(const ServiceCallResult& result) + { + Entry& entry = getEntry(result.getCallID().server_node_id); + + if (result.isSuccessful()) + { + /* + * Updating the uptime here allows to properly handle a corner case where the service response arrives + * after the device has restarted and published its new NodeStatus (although it's unlikely to happen). + */ + entry.uptime_sec = result.getResponse().status.uptime_sec; + entry.request_needed = false; + listeners_.forEach(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id, + result.getResponse())); + } + else + { + if (num_attempts_ != UnlimitedRequestAttempts) + { + entry.num_attempts_made++; + if (entry.num_attempts_made >= num_attempts_) + { + entry.request_needed = false; + listeners_.forEach(GenericHandlerCaller(&INodeInfoListener::handleNodeInfoUnavailable, + result.getCallID().server_node_id)); + } + } + } + } + +public: + NodeInfoRetriever(INode& node) + : NodeStatusMonitor(node) + , TimerBase(node) + , listeners_(node.getAllocator()) + , get_node_info_client_(node) + , request_interval_(MonotonicDuration::fromMSec(DefaultTimerIntervalMSec)) + , last_picked_node_(1) + , num_attempts_(DefaultNumRequestAttempts) + { } + + /** + * Starts the retriever. + * Destroy the object to stop it. + * Returns negative error code. + */ + int start(const TransferPriority priority = TransferPriority::OneHigherThanLowest) + { + int res = NodeStatusMonitor::start(); + if (res < 0) + { + return res; + } + + res = get_node_info_client_.init(priority); + if (res < 0) + { + return res; + } + get_node_info_client_.setCallback(GetNodeInfoResponseCallback(this, + &NodeInfoRetriever::handleGetNodeInfoResponse)); + // Note: the timer will be started ad-hoc + return 0; + } + + /** + * This method forces the class to re-request uavcan.protocol.GetNodeInfo from all nodes as if they + * have just appeared in the network. + */ + void invalidateAll() + { + NodeStatusMonitor::forgetAllNodes(); + get_node_info_client_.cancelAllCalls(); + + for (unsigned i = 0; i < (sizeof(entries_) / sizeof(entries_[0])); i++) + { + entries_[i] = Entry(); + } + // It is not necessary to reset the last picked node index + } + + /** + * Adds one listener. Does nothing if such listener already exists. + * May return -ErrMemory if there's no space to add the listener. + */ + int addListener(INodeInfoListener* listener) + { + if (listener != UAVCAN_NULLPTR) + { + removeListener(listener); + return (UAVCAN_NULLPTR == listeners_.emplace(listener)) ? -ErrMemory : 0; + } + else + { + return -ErrInvalidParam; + } + } + + /** + * Removes the listener. + * If the listener was not registered, nothing will be done. + */ + void removeListener(INodeInfoListener* listener) + { + if (listener != UAVCAN_NULLPTR) + { + listeners_.removeAll(listener); + } + else + { + UAVCAN_ASSERT(0); + } + } + + unsigned getNumListeners() const { return listeners_.getSize(); } + + /** + * Number of attempts to retrieve GetNodeInfo response before giving up on the assumption that the service is + * not implemented. + * Zero is a special value that can be used to set unlimited number of attempts, @ref UnlimitedRequestAttempts. + */ + uint8_t getNumRequestAttempts() const { return num_attempts_; } + void setNumRequestAttempts(const uint8_t num) + { + num_attempts_ = min(static_cast(MaxNumRequestAttempts), num); + } + + /** + * Request interval also implicitly defines the maximum number of concurrent requests. + * Read the class documentation for details. + */ + MonotonicDuration getRequestInterval() const { return request_interval_; } + void setRequestInterval(const MonotonicDuration interval) + { + if (interval.isPositive()) + { + request_interval_ = interval; + if (TimerBase::isRunning()) + { + TimerBase::startPeriodic(request_interval_); + } + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * These methods are needed mostly for testing. + */ + bool isRetrievingInProgress() const { return TimerBase::isRunning(); } + + uint8_t getNumPendingRequests() const + { + const unsigned num = get_node_info_client_.getNumPendingCalls(); + UAVCAN_ASSERT(num <= 0xFF); + return static_cast(num); + } +}; + +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp new file mode 100644 index 000000000000..c915ec6d104b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp @@ -0,0 +1,320 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED +#define UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class implements the core functionality of a network monitor. + * It can be extended by inheritance to add more complex logic, or used directly as is. + */ +class UAVCAN_EXPORT NodeStatusMonitor +{ +public: + struct NodeStatus + { + uint8_t health : 2; + uint8_t mode : 3; + uint8_t sub_mode : 3; + + NodeStatus() : + health(protocol::NodeStatus::HEALTH_CRITICAL), + mode(protocol::NodeStatus::MODE_OFFLINE), + sub_mode(0) + { + StaticAssert::check(); + StaticAssert::check(); + StaticAssert::check(); + } + + bool operator!=(const NodeStatus rhs) const { return !operator==(rhs); } + bool operator==(const NodeStatus rhs) const + { + return std::memcmp(this, &rhs, sizeof(NodeStatus)) == 0; + } + +#if UAVCAN_TOSTRING + std::string toString() const + { + char buf[40]; + (void)snprintf(buf, sizeof(buf), "health=%d mode=%d sub_mode=%d", int(health), int(mode), int(sub_mode)); + return std::string(buf); + } +#endif + }; + + struct NodeStatusChangeEvent + { + NodeID node_id; + NodeStatus status; + NodeStatus old_status; + bool was_known; + + NodeStatusChangeEvent() : + was_known(false) + { } + }; + +private: + enum { TimerPeriodMs100 = 2 }; + + typedef MethodBinder&)> + NodeStatusCallback; + + typedef MethodBinder TimerCallback; + + Subscriber sub_; + + TimerEventForwarder timer_; + + struct Entry + { + NodeStatus status; + int8_t time_since_last_update_ms100; + Entry() : + time_since_last_update_ms100(-1) + { } + }; + + mutable Entry entries_[NodeID::Max]; // [1, NodeID::Max] + + Entry& getEntry(NodeID node_id) const + { + if (node_id.get() < 1 || node_id.get() > NodeID::Max) + { + handleFatalError("NodeStatusMonitor NodeID"); + } + return entries_[node_id.get() - 1]; + } + + void changeNodeStatus(const NodeID node_id, const Entry new_entry_value) + { + Entry& entry = getEntry(node_id); + if (entry.status != new_entry_value.status) + { + NodeStatusChangeEvent event; + event.node_id = node_id; + event.old_status = entry.status; + event.status = new_entry_value.status; + event.was_known = entry.time_since_last_update_ms100 >= 0; + + UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: [%s] --> [%s]", int(node_id.get()), + (event.was_known ? "known" : "new"), + event.old_status.toString().c_str(), event.status.toString().c_str()); + + handleNodeStatusChange(event); + } + entry = new_entry_value; + } + + void handleNodeStatus(const ReceivedDataStructure& msg) + { + Entry new_entry; + new_entry.time_since_last_update_ms100 = 0; + new_entry.status.health = msg.health & ((1 << protocol::NodeStatus::FieldTypes::health::BitLen) - 1); + new_entry.status.mode = msg.mode & ((1 << protocol::NodeStatus::FieldTypes::mode::BitLen) - 1); + new_entry.status.sub_mode = msg.sub_mode & ((1 << protocol::NodeStatus::FieldTypes::sub_mode::BitLen) - 1); + + changeNodeStatus(msg.getSrcNodeID(), new_entry); + + handleNodeStatusMessage(msg); + } + + void handleTimerEvent(const TimerEvent&) + { + const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100; + + for (uint8_t i = 1; i <= NodeID::Max; i++) + { + Entry& entry = getEntry(i); + if (entry.time_since_last_update_ms100 >= 0 && + entry.status.mode != protocol::NodeStatus::MODE_OFFLINE) + { + entry.time_since_last_update_ms100 = + int8_t(entry.time_since_last_update_ms100 + int8_t(TimerPeriodMs100)); + + if (entry.time_since_last_update_ms100 > OfflineTimeoutMs100) + { + Entry new_entry_value = entry; + new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100; + new_entry_value.status.mode = protocol::NodeStatus::MODE_OFFLINE; + changeNodeStatus(i, new_entry_value); + } + } + } + } + +protected: + /** + * Called when a node becomes online, changes status or goes offline. + * Refer to uavcan.protocol.NodeStatus for the offline timeout value. + * Overriding is not required. + */ + virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event) + { + (void)event; + } + + /** + * Called for every received message uavcan.protocol.NodeStatus after handleNodeStatusChange(), even + * if the status code did not change. + * Overriding is not required. + */ + virtual void handleNodeStatusMessage(const ReceivedDataStructure& msg) + { + (void)msg; + } + +public: + explicit NodeStatusMonitor(INode& node) + : sub_(node) + , timer_(node) + { } + + virtual ~NodeStatusMonitor() { } + + /** + * Starts the monitor. + * Destroy the object to stop it. + * Returns negative error code. + */ + int start() + { + const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus)); + if (res >= 0) + { + timer_.setCallback(TimerCallback(this, &NodeStatusMonitor::handleTimerEvent)); + timer_.startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100)); + } + return res; + } + + /** + * Make the node unknown. + */ + void forgetNode(NodeID node_id) + { + if (node_id.isValid()) + { + Entry& entry = getEntry(node_id); + entry = Entry(); + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * Make all nodes unknown. + */ + void forgetAllNodes() + { + for (unsigned i = 0; i < (sizeof(entries_) / sizeof(entries_[0])); i++) + { + entries_[i] = Entry(); + } + } + + /** + * Returns status of a given node. + * Unknown nodes are considered offline. + * Complexity O(1). + */ + NodeStatus getNodeStatus(NodeID node_id) const + { + if (!node_id.isValid()) + { + UAVCAN_ASSERT(0); + return NodeStatus(); + } + + const Entry& entry = getEntry(node_id); + if (entry.time_since_last_update_ms100 >= 0) + { + return entry.status; + } + else + { + return NodeStatus(); + } + } + + /** + * Whether the class has observed this node at least once since initialization. + * Complexity O(1). + */ + bool isNodeKnown(NodeID node_id) const + { + if (!node_id.isValid()) + { + UAVCAN_ASSERT(0); + return false; + } + return getEntry(node_id).time_since_last_update_ms100 >= 0; + } + + /** + * This helper method allows to quickly estimate the overall network health. + * Health of the local node is not considered. + * Returns an invalid Node ID value if there's no known nodes in the network. + */ + NodeID findNodeWithWorstHealth() const + { + NodeID nid_with_worst_health; + uint8_t worst_health = protocol::NodeStatus::HEALTH_OK; + + for (uint8_t i = 1; i <= NodeID::Max; i++) + { + const NodeID nid(i); + UAVCAN_ASSERT(nid.isUnicast()); + const Entry& entry = getEntry(nid); + if (entry.time_since_last_update_ms100 >= 0) + { + if (entry.status.health > worst_health || !nid_with_worst_health.isValid()) + { + nid_with_worst_health = nid; + worst_health = entry.status.health; + } + } + } + + return nid_with_worst_health; + } + + /** + * Calls the operator for every known node. + * Operator signature: + * void (NodeID, NodeStatus) + */ + template + void forEachNode(Operator op) const + { + for (uint8_t i = 1; i <= NodeID::Max; i++) + { + const NodeID nid(i); + UAVCAN_ASSERT(nid.isUnicast()); + const Entry& entry = getEntry(nid); + if (entry.time_since_last_update_ms100 >= 0) + { + op(nid, entry.status); + } + } + } +}; + +} + +#endif // UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_provider.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_provider.hpp new file mode 100644 index 000000000000..b39319f080f5 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_provider.hpp @@ -0,0 +1,174 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_NODE_STATUS_PROVIDER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_NODE_STATUS_PROVIDER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This optional interface can be implemented by the user in order to update the node status as necessary, + * immediately before the next NodeStatus message is emitted by @ref NodeStatusProvider. + */ +class IAdHocNodeStatusUpdater +{ +public: + /** + * This method is invoked by the library from @ref NodeStatusProvider from the library's thread immediately + * before the next NodeStatus message is transmitted. The application can implement this method to perform + * node status updates only as necessary. + * The application is expected to invoke the methods of @ref NodeStatusProvider to update the status + * from this method. + * Note that this method is only invoked when publication is happening by the timer event. + * It will NOT be invoked if the following methods are used to trigger node status publication: + * - @ref NodeStatusProvider::startAndPublish() + * - @ref NodeStatusProvider::forcePublish() + */ + virtual void updateNodeStatus() = 0; + + virtual ~IAdHocNodeStatusUpdater() { } +}; + +/** + * Provides the status and basic information about this node to other network participants. + * + * Usually the application does not need to deal with this class directly - it's instantiated by the node class. + * + * Default values: + * - health - OK + * - mode - INITIALIZATION + */ +class UAVCAN_EXPORT NodeStatusProvider : private TimerBase +{ + typedef MethodBinder GetNodeInfoCallback; + + const MonotonicTime creation_timestamp_; + + Publisher node_status_pub_; + ServiceServer gni_srv_; + + protocol::GetNodeInfo::Response node_info_; + + IAdHocNodeStatusUpdater* ad_hoc_status_updater_; + + INode& getNode() { return node_status_pub_.getNode(); } + + bool isNodeInfoInitialized() const; + + int publish(); + + virtual void handleTimerEvent(const TimerEvent&) override; + void handleGetNodeInfoRequest(const protocol::GetNodeInfo::Request&, protocol::GetNodeInfo::Response& rsp); + +public: + typedef typename StorageType::Type + VendorSpecificStatusCode; + + typedef typename StorageType::Type NodeName; + + explicit NodeStatusProvider(INode& node) + : TimerBase(node) + , creation_timestamp_(node.getMonotonicTime()) + , node_status_pub_(node) + , gni_srv_(node) + , ad_hoc_status_updater_(UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(!creation_timestamp_.isZero()); + + node_info_.status.mode = protocol::NodeStatus::MODE_INITIALIZATION; + + node_info_.status.health = protocol::NodeStatus::HEALTH_OK; + } + + /** + * Starts the provider and immediately broadcasts uavcan.protocol.NodeStatus. + * Returns negative error code. + */ + int startAndPublish(const TransferPriority priority = TransferPriority::Default); + + /** + * Publish the message uavcan.protocol.NodeStatus right now, out of schedule. + * Returns negative error code. + */ + int forcePublish() { return publish(); } + + /** + * Allows to override default publishing rate for uavcan.protocol.NodeStatus. + * Refer to the DSDL definition of uavcan.protocol.NodeStatus to see what is the default rate. + * Doesn't fail; if the value is outside of acceptable range, a closest valid value will be used instead. + */ + void setStatusPublicationPeriod(uavcan::MonotonicDuration period); + uavcan::MonotonicDuration getStatusPublicationPeriod() const; + + /** + * Configure the optional handler that is invoked before every node status message is emitted. + * By default no handler is installed. + * It is allowed to pass a null pointer, that will disable the ad-hoc update feature. + * @ref IAdHocNodeStatusUpdater + */ + void setAdHocNodeStatusUpdater(IAdHocNodeStatusUpdater* updater); + IAdHocNodeStatusUpdater* getAdHocNodeStatusUpdater() const { return ad_hoc_status_updater_; } + + /** + * Local node health code control. + */ + uint8_t getHealth() const { return node_info_.status.health; } + void setHealth(uint8_t code); + void setHealthOk() { setHealth(protocol::NodeStatus::HEALTH_OK); } + void setHealthWarning() { setHealth(protocol::NodeStatus::HEALTH_WARNING); } + void setHealthError() { setHealth(protocol::NodeStatus::HEALTH_ERROR); } + void setHealthCritical() { setHealth(protocol::NodeStatus::HEALTH_CRITICAL); } + + /** + * Local node mode code control. + */ + uint8_t getMode() const { return node_info_.status.mode; } + void setMode(uint8_t code); + void setModeOperational() { setMode(protocol::NodeStatus::MODE_OPERATIONAL); } + void setModeInitialization() { setMode(protocol::NodeStatus::MODE_INITIALIZATION); } + void setModeMaintenance() { setMode(protocol::NodeStatus::MODE_MAINTENANCE); } + void setModeSoftwareUpdate() { setMode(protocol::NodeStatus::MODE_SOFTWARE_UPDATE); } + void setModeOffline() { setMode(protocol::NodeStatus::MODE_OFFLINE); } + + /** + * Local node vendor-specific status code control. + */ + void setVendorSpecificStatusCode(VendorSpecificStatusCode code); + VendorSpecificStatusCode getVendorSpecificStatusCode() const + { + return node_info_.status.vendor_specific_status_code; + } + + /** + * Local node name control. + * Can be set only once before the provider is started. + * The provider will refuse to start if the node name is not set. + */ + const NodeName& getName() const { return node_info_.name; } + void setName(const NodeName& name); + + /** + * Node version information. + * Can be set only once before the provider is started. + */ + const protocol::SoftwareVersion& getSoftwareVersion() const { return node_info_.software_version; } + const protocol::HardwareVersion& getHardwareVersion() const { return node_info_.hardware_version; } + void setSoftwareVersion(const protocol::SoftwareVersion& version); + void setHardwareVersion(const protocol::HardwareVersion& version); +}; + +} + +#endif // UAVCAN_PROTOCOL_NODE_STATUS_PROVIDER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp new file mode 100644 index 000000000000..28c9b32c1dae --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Helper for broadcasting the message uavcan.protocol.Panic. + */ +class UAVCAN_EXPORT PanicBroadcaster : private TimerBase +{ + Publisher pub_; + protocol::Panic msg_; + + void publishOnce() + { + const int res = pub_.broadcast(msg_); + if (res < 0) + { + pub_.getNode().registerInternalFailure("Panic pub failed"); + } + } + + virtual void handleTimerEvent(const TimerEvent&) + { + publishOnce(); + } + +public: + explicit PanicBroadcaster(INode& node) + : TimerBase(node) + , pub_(node) + { } + + /** + * This method does not block and can't fail. + * @param short_reason Short ASCII string that describes the reason of the panic, 7 characters max. + * If the string exceeds 7 characters, it will be truncated. + * @param broadcasting_period Broadcasting period. Optional. + * @param priority Transfer priority. Optional. + */ + void panic(const char* short_reason_description, + MonotonicDuration broadcasting_period = MonotonicDuration::fromMSec(100), + const TransferPriority priority = TransferPriority::Default) + { + msg_.reason_text.clear(); + const char* p = short_reason_description; + while (p && *p) + { + if (msg_.reason_text.size() == msg_.reason_text.capacity()) + { + break; + } + msg_.reason_text.push_back(protocol::Panic::FieldTypes::reason_text::ValueType(*p)); + p++; + } + + UAVCAN_TRACE("PanicBroadcaster", "Panicking with reason '%s'", getReason().c_str()); + + pub_.setTxTimeout(broadcasting_period); + pub_.setPriority(priority); + + publishOnce(); + startPeriodic(broadcasting_period); + } + + /** + * Stop broadcasting immediately. + */ + void dontPanic() // Where's my towel + { + stop(); + msg_.reason_text.clear(); + } + + bool isPanicking() const { return isRunning(); } + + const typename protocol::Panic::FieldTypes::reason_text& getReason() const + { + return msg_.reason_text; + } +}; + +} + +#endif // UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/panic_listener.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/panic_listener.hpp new file mode 100644 index 000000000000..69a0e71bd7c6 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/panic_listener.hpp @@ -0,0 +1,128 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_PANIC_LISTENER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_PANIC_LISTENER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ +/** + * This class implements proper panic detector. + * Refer to uavcan.protocol.Panic for details. + * The listener can be stopped from the callback. + * + * @tparam Callback Possible callback prototypes: + * void (const ReceivedDataStructure&) + * void (const protocol::Panic&) + */ +template < +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + typename Callback = std::function&)> +#else + typename Callback = void (*)(const ReceivedDataStructure&) +#endif + > +class UAVCAN_EXPORT PanicListener : Noncopyable +{ + typedef MethodBinder&)> + PanicMsgCallback; + + Subscriber sub_; + MonotonicTime prev_msg_timestamp_; + Callback callback_; + uint8_t num_subsequent_msgs_; + + void invokeCallback(const ReceivedDataStructure& msg) + { + if (coerceOrFallback(callback_, true)) + { + callback_(msg); + } + else + { + UAVCAN_ASSERT(0); // This is a logic error because normally we shouldn't start with an invalid callback + sub_.getNode().registerInternalFailure("PanicListener invalid callback"); + } + } + + void handleMsg(const ReceivedDataStructure& msg) + { + UAVCAN_TRACE("PanicListener", "Received panic from snid=%i reason=%s", + int(msg.getSrcNodeID().get()), msg.reason_text.c_str()); + if (prev_msg_timestamp_.isZero()) + { + num_subsequent_msgs_ = 1; + prev_msg_timestamp_ = msg.getMonotonicTimestamp(); + } + else + { + const MonotonicDuration diff = msg.getMonotonicTimestamp() - prev_msg_timestamp_; + UAVCAN_ASSERT(diff.isPositive() || diff.isZero()); + if (diff.toMSec() > protocol::Panic::MAX_INTERVAL_MS) + { + num_subsequent_msgs_ = 1; + } + else + { + num_subsequent_msgs_++; + } + prev_msg_timestamp_ = msg.getMonotonicTimestamp(); + if (num_subsequent_msgs_ >= protocol::Panic::MIN_MESSAGES) + { + num_subsequent_msgs_ = protocol::Panic::MIN_MESSAGES; + invokeCallback(msg); // The application can stop us from the callback + } + } + } + +public: + explicit PanicListener(INode& node) + : sub_(node) + , callback_() + , num_subsequent_msgs_(0) + { } + + /** + * Start the listener. + * Once started it does not require further attention. + * Returns negative error code. + */ + int start(const Callback& callback) + { + stop(); + if (!coerceOrFallback(callback, true)) + { + UAVCAN_TRACE("PanicListener", "Invalid callback"); + return -ErrInvalidParam; + } + callback_ = callback; + return sub_.start(PanicMsgCallback(this, &PanicListener::handleMsg)); + } + + void stop() + { + sub_.stop(); + num_subsequent_msgs_ = 0; + prev_msg_timestamp_ = MonotonicTime(); + } +}; + +} + +#endif // UAVCAN_PROTOCOL_PANIC_LISTENER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/param_server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/param_server.hpp new file mode 100644 index 000000000000..d3a6651ffb4f --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/param_server.hpp @@ -0,0 +1,192 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_PARAM_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_PARAM_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Implement this interface in the application to support the standard remote reconfiguration services. + * Refer to @ref ParamServer. + */ +class UAVCAN_EXPORT IParamManager +{ +public: + typedef typename StorageType::Type Name; + typedef typename StorageType::Type Index; + typedef protocol::param::Value Value; + typedef protocol::param::NumericValue NumericValue; + + virtual ~IParamManager() { } + + /** + * Copy the parameter name to @ref out_name if it exists, otherwise do nothing. + */ + virtual void getParamNameByIndex(Index index, Name& out_name) const = 0; + + /** + * Assign by name if exists. + */ + virtual void assignParamValue(const Name& name, const Value& value) = 0; + + /** + * Read by name if exists, otherwise do nothing. + */ + virtual void readParamValue(const Name& name, Value& out_value) const = 0; + + /** + * Read param's default/max/min if available. + * Note that min/max are only applicable to numeric params. + * Implementation is optional. + */ + virtual void readParamDefaultMaxMin(const Name& name, Value& out_default, + NumericValue& out_max, NumericValue& out_min) const + { + (void)name; + (void)out_default; + (void)out_max; + (void)out_min; + } + + /** + * Save all params to non-volatile storage. + * @return Negative if failed. + */ + virtual int saveAllParams() = 0; + + /** + * Clear the non-volatile storage. + * @return Negative if failed. + */ + virtual int eraseAllParams() = 0; +}; + +/** + * Convenience class for supporting the standard configuration services. + * Highly recommended to use. + */ +class UAVCAN_EXPORT ParamServer +{ + typedef MethodBinder GetSetCallback; + + typedef MethodBinder ExecuteOpcodeCallback; + + ServiceServer get_set_srv_; + ServiceServer save_erase_srv_; + IParamManager* manager_; + + void handleGetSet(const protocol::param::GetSet::Request& in, protocol::param::GetSet::Response& out) + { + UAVCAN_ASSERT(manager_ != UAVCAN_NULLPTR); + + // Recover the name from index + if (in.name.empty()) + { + manager_->getParamNameByIndex(in.index, out.name); + UAVCAN_TRACE("ParamServer", "GetSet: Index %i --> '%s'", int(in.index), out.name.c_str()); + } + else + { + out.name = in.name; + } + + if (out.name.empty()) + { + UAVCAN_TRACE("ParamServer", "GetSet: Can't resolve parameter name, index=%i", int(in.index)); + return; + } + + // Assign if needed, read back + if (!in.value.is(protocol::param::Value::Tag::empty)) + { + manager_->assignParamValue(out.name, in.value); + } + manager_->readParamValue(out.name, out.value); + + // Check if the value is OK, otherwise reset the name to indicate that we have no idea what is it all about + if (!out.value.is(protocol::param::Value::Tag::empty)) + { + manager_->readParamDefaultMaxMin(out.name, out.default_value, out.max_value, out.min_value); + } + else + { + UAVCAN_TRACE("ParamServer", "GetSet: Unknown param: index=%i name='%s'", int(in.index), out.name.c_str()); + out.name.clear(); + } + } + + void handleExecuteOpcode(const protocol::param::ExecuteOpcode::Request& in, + protocol::param::ExecuteOpcode::Response& out) + { + UAVCAN_ASSERT(manager_ != UAVCAN_NULLPTR); + + if (in.opcode == protocol::param::ExecuteOpcode::Request::OPCODE_SAVE) + { + out.ok = manager_->saveAllParams() >= 0; + } + else if (in.opcode == protocol::param::ExecuteOpcode::Request::OPCODE_ERASE) + { + out.ok = manager_->eraseAllParams() >= 0; + } + else + { + UAVCAN_TRACE("ParamServer", "ExecuteOpcode: invalid opcode %i", int(in.opcode)); + out.ok = false; + } + } + +public: + explicit ParamServer(INode& node) + : get_set_srv_(node) + , save_erase_srv_(node) + , manager_(UAVCAN_NULLPTR) + { } + + /** + * Starts the parameter server with given param manager instance. + * Returns negative error code. + */ + int start(IParamManager* manager) + { + if (manager == UAVCAN_NULLPTR) + { + return -ErrInvalidParam; + } + manager_ = manager; + + int res = get_set_srv_.start(GetSetCallback(this, &ParamServer::handleGetSet)); + if (res < 0) + { + return res; + } + + res = save_erase_srv_.start(ExecuteOpcodeCallback(this, &ParamServer::handleExecuteOpcode)); + if (res < 0) + { + get_set_srv_.stop(); + } + return res; + } + + /** + * @ref IParamManager + */ + IParamManager* getParamManager() const { return manager_; } +}; + +} + +#endif // UAVCAN_PROTOCOL_PARAM_SERVER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/restart_request_server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/restart_request_server.hpp new file mode 100644 index 000000000000..62031fe983cd --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/restart_request_server.hpp @@ -0,0 +1,92 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Implement this interface in the application to support the standard node restart service. + */ +class UAVCAN_EXPORT IRestartRequestHandler +{ +public: + virtual ~IRestartRequestHandler() { } + + /** + * This method shall do either: + * - restart the local node immediately; + * - initiate the restart procedure to complete it asynchronously; + * - reject the restart request and return false. + * + * If the restart requets was accepted, this method shall either return true or don't return at all. + */ + virtual bool handleRestartRequest(NodeID request_source) = 0; +}; + +/** + * Convenience class for supporting the standard node restart service. + * Highly recommended to use. + */ +class UAVCAN_EXPORT RestartRequestServer : Noncopyable +{ + typedef MethodBinder&, + protocol::RestartNode::Response&) const> RestartNodeCallback; + + ServiceServer srv_; + IRestartRequestHandler* handler_; + + void handleRestartNode(const ReceivedDataStructure& request, + protocol::RestartNode::Response& response) const + { + UAVCAN_TRACE("RestartRequestServer", "Request from snid=%i", int(request.getSrcNodeID().get())); + response.ok = false; + if (request.magic_number == protocol::RestartNode::Request::MAGIC_NUMBER) + { + if (handler_) + { + response.ok = handler_->handleRestartRequest(request.getSrcNodeID()); + } + UAVCAN_TRACE("RestartRequestServer", "%s", (response.ok ? "Accepted" : "Rejected")); + } + else + { + UAVCAN_TRACE("RestartRequestServer", "Invalid magic number 0x%llx", + static_cast(request.magic_number)); + } + } + +public: + explicit RestartRequestServer(INode& node) + : srv_(node) + , handler_(UAVCAN_NULLPTR) + { } + + /** + * Restart request handler configuration. + * All restart requests will be explicitly rejected if there's no handler installed. + */ + IRestartRequestHandler* getHandler() const { return handler_; } + void setHandler(IRestartRequestHandler* handler) { handler_ = handler; } + + /** + * Starts the server. + * Returns negative error code. + */ + int start() + { + return srv_.start(RestartNodeCallback(this, &RestartRequestServer::handleRestartNode)); + } +}; + +} + +#endif // UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/transport_stats_provider.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/transport_stats_provider.hpp new file mode 100644 index 000000000000..34f8aaa0b051 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/transport_stats_provider.hpp @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class provides statistics about the transport layer performance on the local node. + * The user's application does not deal with this class directly because it's instantiated by the node class. + */ +class UAVCAN_EXPORT TransportStatsProvider : Noncopyable +{ + typedef MethodBinder + GetTransportStatsCallback; + + ServiceServer srv_; + + void handleGetTransportStats(const protocol::GetTransportStats::Request&, + protocol::GetTransportStats::Response& resp) const + { + const TransferPerfCounter& perf = srv_.getNode().getDispatcher().getTransferPerfCounter(); + resp.transfer_errors = perf.getErrorCount(); + resp.transfers_tx = perf.getTxTransferCount(); + resp.transfers_rx = perf.getRxTransferCount(); + + const CanIOManager& canio = srv_.getNode().getDispatcher().getCanIOManager(); + for (uint8_t i = 0; i < canio.getNumIfaces(); i++) + { + const CanIfacePerfCounters can_perf = canio.getIfacePerfCounters(i); + protocol::CANIfaceStats stats; + stats.errors = can_perf.errors; + stats.frames_tx = can_perf.frames_tx; + stats.frames_rx = can_perf.frames_rx; + resp.can_iface_stats.push_back(stats); + } + } + +public: + explicit TransportStatsProvider(INode& node) + : srv_(node) + { } + + /** + * Once started, this class requires no further attention. + * Returns negative error code. + */ + int start() + { + return srv_.start(GetTransportStatsCallback(this, &TransportStatsProvider::handleGetTransportStats)); + } +}; + +} + +#endif // UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/std.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/std.hpp new file mode 100644 index 000000000000..a1e63ea46959 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/std.hpp @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_STD_HPP_INCLUDED +#define UAVCAN_STD_HPP_INCLUDED + +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +# include +# include + +namespace uavcan +{ + +typedef std::uint8_t uint8_t; +typedef std::uint16_t uint16_t; +typedef std::uint32_t uint32_t; +typedef std::uint64_t uint64_t; + +typedef std::int8_t int8_t; +typedef std::int16_t int16_t; +typedef std::int32_t int32_t; +typedef std::int64_t int64_t; + +} + +#else + +# include // Standard integer types from C library +# include // vsnprintf() from the C library + +namespace uavcan +{ + +typedef ::uint8_t uint8_t; +typedef ::uint16_t uint16_t; +typedef ::uint32_t uint32_t; +typedef ::uint64_t uint64_t; + +typedef ::int8_t int8_t; +typedef ::int16_t int16_t; +typedef ::int32_t int32_t; +typedef ::int64_t int64_t; + +} + +#endif + +namespace uavcan +{ +/** + * Wrapper over the standard snprintf(). This wrapper is needed because different standards and different + * implementations of C++ do not agree whether snprintf() should be defined in std:: or in ::. The solution + * is to use 'using namespace std' hack inside the wrapper, so the compiler will be able to pick whatever + * definition is available in the standard library. Alternatively, the user's application can provide a + * custom implementation of uavcan::snprintf(). + */ +#if __GNUC__ +__attribute__ ((format(printf, 3, 4))) +#endif +extern int snprintf(char* out, std::size_t maxlen, const char* format, ...); + +#if !UAVCAN_USE_EXTERNAL_SNPRINTF +inline int snprintf(char* out, std::size_t maxlen, const char* format, ...) +{ + using namespace std; // This way we can pull vsnprintf() either from std:: or from ::. + va_list args; + va_start(args, format); + const int return_value = vsnprintf(out, maxlen, format, args); + va_end(args); + return return_value; +} +#endif + +} + +#endif // UAVCAN_STD_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/time.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/time.hpp new file mode 100644 index 000000000000..40bcb99449d1 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/time.hpp @@ -0,0 +1,289 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TIME_HPP_INCLUDED +#define UAVCAN_TIME_HPP_INCLUDED + +#include +#include +#include +#include +#include + + +namespace uavcan +{ + +template +class DurationBase +{ + int64_t usec_; + +protected: + ~DurationBase() { } + + DurationBase() + : usec_(0) + { + StaticAssert<(sizeof(D) == 8)>::check(); + } + +public: + static D getInfinite() { return fromUSec(NumericTraits::max()); } + + static D fromUSec(int64_t us) + { + D d; + d.usec_ = us; + return d; + } + static D fromMSec(int64_t ms) { return fromUSec(ms * 1000); } + + int64_t toUSec() const { return usec_; } + int64_t toMSec() const { return usec_ / 1000; } + + D getAbs() const { return D::fromUSec((usec_ < 0) ? (-usec_) : usec_); } + + bool isPositive() const { return usec_ > 0; } + bool isNegative() const { return usec_ < 0; } + bool isZero() const { return usec_ == 0; } + + bool operator==(const D& r) const { return usec_ == r.usec_; } + bool operator!=(const D& r) const { return !operator==(r); } + + bool operator<(const D& r) const { return usec_ < r.usec_; } + bool operator>(const D& r) const { return usec_ > r.usec_; } + bool operator<=(const D& r) const { return usec_ <= r.usec_; } + bool operator>=(const D& r) const { return usec_ >= r.usec_; } + + D operator+(const D& r) const { return fromUSec(usec_ + r.usec_); } // TODO: overflow check + D operator-(const D& r) const { return fromUSec(usec_ - r.usec_); } // ditto + + D operator-() const { return fromUSec(-usec_); } + + D& operator+=(const D& r) + { + *this = *this + r; + return *static_cast(this); + } + D& operator-=(const D& r) + { + *this = *this - r; + return *static_cast(this); + } + + template + D operator*(Scale scale) const { return fromUSec(usec_ * scale); } + + template + D& operator*=(Scale scale) + { + *this = *this * scale; + return *static_cast(this); + } + + static const unsigned StringBufSize = 32; + void toString(char buf[StringBufSize]) const; ///< Prints time in seconds with microsecond resolution +#if UAVCAN_TOSTRING + std::string toString() const; ///< Prints time in seconds with microsecond resolution +#endif +}; + + +template +class TimeBase +{ + uint64_t usec_; + +protected: + ~TimeBase() { } + + TimeBase() + : usec_(0) + { + StaticAssert<(sizeof(T) == 8)>::check(); + StaticAssert<(sizeof(D) == 8)>::check(); + } + +public: + static T getMax() { return fromUSec(NumericTraits::max()); } + + static T fromUSec(uint64_t us) + { + T d; + d.usec_ = us; + return d; + } + static T fromMSec(uint64_t ms) { return fromUSec(ms * 1000); } + + uint64_t toUSec() const { return usec_; } + uint64_t toMSec() const { return usec_ / 1000; } + + bool isZero() const { return usec_ == 0; } + + bool operator==(const T& r) const { return usec_ == r.usec_; } + bool operator!=(const T& r) const { return !operator==(r); } + + bool operator<(const T& r) const { return usec_ < r.usec_; } + bool operator>(const T& r) const { return usec_ > r.usec_; } + bool operator<=(const T& r) const { return usec_ <= r.usec_; } + bool operator>=(const T& r) const { return usec_ >= r.usec_; } + + T operator+(const D& r) const + { + if (r.isNegative()) + { + if (uint64_t(r.getAbs().toUSec()) > usec_) + { + return fromUSec(0); + } + } + else + { + if (uint64_t(int64_t(usec_) + r.toUSec()) < usec_) + { + return fromUSec(NumericTraits::max()); + } + } + return fromUSec(uint64_t(int64_t(usec_) + r.toUSec())); + } + + T operator-(const D& r) const + { + return *static_cast(this) + (-r); + } + D operator-(const T& r) const + { + return D::fromUSec((usec_ > r.usec_) ? int64_t(usec_ - r.usec_) : -int64_t(r.usec_ - usec_)); + } + + T& operator+=(const D& r) + { + *this = *this + r; + return *static_cast(this); + } + T& operator-=(const D& r) + { + *this = *this - r; + return *static_cast(this); + } + + static const unsigned StringBufSize = 32; + void toString(char buf[StringBufSize]) const; ///< Prints time in seconds with microsecond resolution +#if UAVCAN_TOSTRING + std::string toString() const; ///< Prints time in seconds with microsecond resolution +#endif +}; + +/* + * Monotonic + */ +class UAVCAN_EXPORT MonotonicDuration : public DurationBase { }; + +class UAVCAN_EXPORT MonotonicTime : public TimeBase { }; + +/* + * UTC + */ +class UAVCAN_EXPORT UtcDuration : public DurationBase { }; + +class UAVCAN_EXPORT UtcTime : public TimeBase /// Implicitly convertible to/from uavcan.Timestamp +{ +public: + UtcTime() { } + + UtcTime(const Timestamp& ts) // Implicit + { + operator=(ts); + } + + UtcTime& operator=(const Timestamp& ts) + { + *this = fromUSec(ts.usec); + return *this; + } + + operator Timestamp() const + { + Timestamp ts; + ts.usec = toUSec(); + return ts; + } +}; + +// ---------------------------------------------------------------------------- + +template +const unsigned DurationBase::StringBufSize; + +template +const unsigned TimeBase::StringBufSize; + +template +void DurationBase::toString(char buf[StringBufSize]) const +{ + char* ptr = buf; + if (isNegative()) + { + *ptr++ = '-'; + } + (void)snprintf(ptr, StringBufSize - 1, "%llu.%06lu", + static_cast(getAbs().toUSec() / 1000000L), + static_cast(getAbs().toUSec() % 1000000L)); +} + + +template +void TimeBase::toString(char buf[StringBufSize]) const +{ + (void)snprintf(buf, StringBufSize, "%llu.%06lu", + static_cast(toUSec() / 1000000UL), + static_cast(toUSec() % 1000000UL)); +} + + +#if UAVCAN_TOSTRING + +template +std::string DurationBase::toString() const +{ + char buf[StringBufSize]; + toString(buf); + return std::string(buf); +} + +template +std::string TimeBase::toString() const +{ + char buf[StringBufSize]; + toString(buf); + return std::string(buf); +} + +#endif + + +template +UAVCAN_EXPORT +Stream& operator<<(Stream& s, const DurationBase& d) +{ + char buf[DurationBase::StringBufSize]; + d.toString(buf); + s << buf; + return s; +} + +template +UAVCAN_EXPORT +Stream& operator<<(Stream& s, const TimeBase& t) +{ + char buf[TimeBase::StringBufSize]; + t.toString(buf); + s << buf; + return s; +} + +} + +#endif // UAVCAN_TIME_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/abstract_transfer_buffer.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/abstract_transfer_buffer.hpp new file mode 100644 index 000000000000..e4a276d1cea9 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/abstract_transfer_buffer.hpp @@ -0,0 +1,27 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_ABSTRACT_TRANSFER_BUFFER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_ABSTRACT_TRANSFER_BUFFER_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +/** + * API for transfer buffer users. + */ +class UAVCAN_EXPORT ITransferBuffer +{ +public: + virtual ~ITransferBuffer() { } + + virtual int read(unsigned offset, uint8_t* data, unsigned len) const = 0; + virtual int write(unsigned offset, const uint8_t* data, unsigned len) = 0; +}; + +} + +#endif // UAVCAN_TRANSPORT_ABSTRACT_TRANSFER_BUFFER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp new file mode 100644 index 000000000000..22b98809e038 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp @@ -0,0 +1,171 @@ +/* + * Copyright (C) 2015 Pavel Kirienko , + * Ilia Sheremet + */ + +#ifndef UAVCAN_TRANSPORT_CAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED +#define UAVCAN_TRANSPORT_CAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class configures hardware acceptance filters (if this feature is present on the particular CAN driver) to + * preclude reception of irrelevant CAN frames on the hardware level. + * + * Configuration starts by creating an object of class @ref CanAcceptanceFilterConfigurator on the stack. + * By means of computeConfiguration() method the class determines the number of available HW filters and the number + * of listeners. In case if custom configuration required, it is possible to add it through addFilterConfig(). + * Subsequently obtained configurations are then loaded into the CAN driver by calling the applyConfiguration() method. + * If the cumulative number of configurations obtained by computeConfiguration() and addFilterConfig() is higher than + * the number of available HW filters, configurations will be merged automatically in the most efficient way. + * + * Note that if the application adds additional server or subscriber objects after the filters have been configured, + * the configuration procedure will have to be performed again. + * + * The maximum number of CAN acceptance filters is predefined in uavcan/build_config.hpp through a constant + * @ref MaxCanAcceptanceFilters. The algorithm doesn't allow to have higher number of HW filters configurations than + * defined by MaxCanAcceptanceFilters. You can change this value according to the number specified in your CAN driver + * datasheet. + */ +class CanAcceptanceFilterConfigurator +{ +public: + /** + * These arguments defines whether acceptance filter configuration has anonymous messages or not + */ + enum AnonymousMessages + { + AcceptAnonymousMessages, + IgnoreAnonymousMessages + }; + +private: + /** + * Below constants based on UAVCAN transport layer specification. Masks and ID's depends on message + * TypeID, TransferID (RequestNotResponse - for service types, ServiceNotMessage - for all types of messages). + * For more details refer to uavcan.org/CAN_bus_transport_layer_specification. + * For clarity let's represent "i" as Data Type ID and "d" as Destination Node Id + * DefaultFilterMsgMask = 00000 11111111 11111111 10000000 + * DefaultFilterMsgID = 00000 iiiiiiii iiiiiiii 00000000, no need to explicitly define, since MsgID initialized + * as 0. + * DefaultFilterServiceMask = 00000 00000000 01111111 10000000 + * DefaultFilterServiceID = 00000 00000000 0ddddddd 10000000, all Service Response Frames are accepted by + * HW filters. + * DefaultAnonMsgMask = 00000 00000000 00000000 11111111 + * DefaultAnonMsgID = 00000 00000000 00000000 00000000, by default the config is added to accept all anonymous + * frames. In case there are no anonymous messages, invoke computeConfiguration(IgnoreAnonymousMessages). + */ + static const unsigned DefaultFilterMsgMask = 0xFFFF80; + static const unsigned DefaultFilterServiceMask = 0x7F80; + static const unsigned DefaultFilterServiceID = 0x80; + static const unsigned DefaultAnonMsgMask = 0xFF; + static const unsigned DefaultAnonMsgID = 0x0; + + typedef uavcan::Multiset MultisetConfigContainer; + + static CanFilterConfig mergeFilters(CanFilterConfig& a_, CanFilterConfig& b_); + static uint8_t countBits(uint32_t n_); + uint16_t getNumFilters() const; + + /** + * Fills the multiset_configs_ to proceed it with mergeConfigurations() + */ + int16_t loadInputConfiguration(AnonymousMessages load_mode); + + /** + * This method merges several listeners's filter configurations by predetermined algorithm + * if number of available hardware acceptance filters less than number of listeners + */ + int16_t mergeConfigurations(); + + INode& node_; //< Node reference is needed for access to ICanDriver and Dispatcher + MultisetConfigContainer multiset_configs_; + uint16_t filters_number_; + +public: + /** + * @param node Libuavcan node whose subscribers/servers/etc will be used to configure the filters. + * + * @param filters_number Allows to override the maximum number of hardware filters to use. + * If set to zero (which is default), the class will obtain the number of available + * filters from the CAN driver via @ref ICanIface::getNumFilters(). + */ + explicit CanAcceptanceFilterConfigurator(INode& node, uint16_t filters_number = 0) + : node_(node) + , multiset_configs_(node.getAllocator()) + , filters_number_(filters_number) + { } + + /** + * This method invokes loadInputConfiguration() and mergeConfigurations() consequently + * in order to comute optimal filter configurations for the current hardware. + * + * It can only be invoked when all of the subscriber and server objects are initialized. + * If new subscriber or server objects are added later, the filters will have to be reconfigured again. + * + * @param mode Either: AcceptAnonymousMessages - the filters will accept all anonymous messages (this is default) + * IgnoreAnonymousMessages - anonymous messages will be ignored + * @return 0 = success, negative for error. + */ + int computeConfiguration(AnonymousMessages mode = AcceptAnonymousMessages); + + /** + * Add an additional filter configuration. + * This method must not be invoked after @ref computeConfiguration(). + * @return 0 = success, negative for error. + */ + int addFilterConfig(const CanFilterConfig& config); + + /** + * This method loads the configuration computed with mergeConfigurations() or explicitly added by addFilterConfig() + * to the CAN driver. Must be called after computeConfiguration() and addFilterConfig(). + * @return 0 = success, negative for error. + */ + int applyConfiguration(); + + /** + * Returns the configuration computed with mergeConfigurations() or added by addFilterConfig(). + * If mergeConfigurations() or addFilterConfig() have not been called yet, an empty configuration will be returned. + */ + const MultisetConfigContainer& getConfiguration() const + { + return multiset_configs_; + } +}; + +/** + * This function is a shortcut for @ref CanAcceptanceFilterConfigurator. + * It allows to compute filter configuration and then apply it in just one step. + * It implements only the most common use case; if you have special requirements, + * use @ref CanAcceptanceFilterConfigurator directly. + * + * @param node Refer to @ref CanAcceptanceFilterConfigurator constructor for explanation. + * @param mode Refer to @ref CanAcceptanceFilterConfigurator::computeConfiguration() for explanation. + * @return non-negative on success, negative error code on error. + */ +static inline int configureCanAcceptanceFilters(INode& node, + CanAcceptanceFilterConfigurator::AnonymousMessages mode + = CanAcceptanceFilterConfigurator::AcceptAnonymousMessages) +{ + CanAcceptanceFilterConfigurator cfger(node); + + const int compute_res = cfger.computeConfiguration(mode); + if (compute_res < 0) + { + return compute_res; + } + + return cfger.applyConfiguration(); +} + +} + +#endif diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_io.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_io.hpp new file mode 100644 index 000000000000..11752f0f8abf --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_io.hpp @@ -0,0 +1,186 @@ +/* + * CAN bus IO logic. + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_CAN_IO_HPP_INCLUDED +#define UAVCAN_TRANSPORT_CAN_IO_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +struct UAVCAN_EXPORT CanRxFrame : public CanFrame +{ + MonotonicTime ts_mono; + UtcTime ts_utc; + uint8_t iface_index; + + CanRxFrame() + : iface_index(0) + { } + +#if UAVCAN_TOSTRING + std::string toString(StringRepresentation mode = StrTight) const; +#endif +}; + + +class UAVCAN_EXPORT CanTxQueue : Noncopyable +{ +public: + enum Qos { Volatile, Persistent }; + + struct Entry : public LinkedListNode // Not required to be packed - fits the block in any case + { + MonotonicTime deadline; + CanFrame frame; + uint8_t qos; + CanIOFlags flags; + + Entry(const CanFrame& arg_frame, MonotonicTime arg_deadline, Qos arg_qos, CanIOFlags arg_flags) + : deadline(arg_deadline) + , frame(arg_frame) + , qos(uint8_t(arg_qos)) + , flags(arg_flags) + { + UAVCAN_ASSERT((qos == Volatile) || (qos == Persistent)); + IsDynamicallyAllocatable::check(); + } + + static void destroy(Entry*& obj, IPoolAllocator& allocator); + + bool isExpired(MonotonicTime timestamp) const { return timestamp > deadline; } + + bool qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const; + bool qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const; + bool qosHigherThan(const Entry& rhs) const { return qosHigherThan(rhs.frame, Qos(rhs.qos)); } + bool qosLowerThan(const Entry& rhs) const { return qosLowerThan(rhs.frame, Qos(rhs.qos)); } + +#if UAVCAN_TOSTRING + std::string toString() const; +#endif + }; + +private: + class PriorityInsertionComparator + { + const CanFrame& frm_; + public: + explicit PriorityInsertionComparator(const CanFrame& frm) : frm_(frm) { } + bool operator()(const Entry* entry) + { + UAVCAN_ASSERT(entry); + return frm_.priorityHigherThan(entry->frame); + } + }; + + LinkedListRoot queue_; + LimitedPoolAllocator allocator_; + ISystemClock& sysclock_; + uint32_t rejected_frames_cnt_; + + void registerRejectedFrame(); + +public: + CanTxQueue(IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t allocator_quota) + : allocator_(allocator, allocator_quota) + , sysclock_(sysclock) + , rejected_frames_cnt_(0) + { } + + ~CanTxQueue(); + + void push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags); + + Entry* peek(); // Modifier + void remove(Entry*& entry); + const CanFrame* getTopPriorityPendingFrame() const; + + /// The 'or equal' condition is necessary to avoid frame reordering. + bool topPriorityHigherOrEqual(const CanFrame& rhs_frame) const; + + uint32_t getRejectedFrameCount() const { return rejected_frames_cnt_; } + + bool isEmpty() const { return queue_.isEmpty(); } +}; + + +struct UAVCAN_EXPORT CanIfacePerfCounters +{ + uint64_t frames_tx; + uint64_t frames_rx; + uint64_t errors; + + CanIfacePerfCounters() + : frames_tx(0) + , frames_rx(0) + , errors(0) + { } +}; + + +class UAVCAN_EXPORT CanIOManager : Noncopyable +{ + struct IfaceFrameCounters + { + uint64_t frames_tx; + uint64_t frames_rx; + + IfaceFrameCounters() + : frames_tx(0) + , frames_rx(0) + { } + }; + + ICanDriver& driver_; + ISystemClock& sysclock_; + + LazyConstructor tx_queues_[MaxCanIfaces]; + IfaceFrameCounters counters_[MaxCanIfaces]; + + const uint8_t num_ifaces_; + + int sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags); + int sendFromTxQueue(uint8_t iface_index); + int callSelect(CanSelectMasks& inout_masks, const CanFrame* (& pending_tx)[MaxCanIfaces], + MonotonicTime blocking_deadline); + +public: + CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock, + std::size_t mem_blocks_per_iface = 0); + + uint8_t getNumIfaces() const { return num_ifaces_; } + + CanIfacePerfCounters getIfacePerfCounters(uint8_t iface_index) const; + + const ICanDriver& getCanDriver() const { return driver_; } + ICanDriver& getCanDriver() { return driver_; } + + uint8_t makePendingTxMask() const; + + /** + * Returns: + * 0 - rejected/timedout/enqueued + * 1+ - sent/received + * negative - failure + */ + int send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, + uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags); + int receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags); +}; + +} + +#endif // UAVCAN_TRANSPORT_CAN_IO_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/crc.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/crc.hpp new file mode 100644 index 000000000000..aae2c4d9648d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/crc.hpp @@ -0,0 +1,80 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_CRC_HPP_INCLUDED +#define UAVCAN_TRANSPORT_CRC_HPP_INCLUDED + +#include +#include +#include + +namespace uavcan +{ + +/** + * CRC-16-CCITT + * Initial value: 0xFFFF + * Poly: 0x1021 + * Reverse: no + * Output xor: 0 + * + * import crcmod + * crc = crcmod.predefined.Crc('crc-ccitt-false') + * crc.update('123456789') + * crc.hexdigest() + * '29B1' + */ +class UAVCAN_EXPORT TransferCRC +{ +#if !UAVCAN_TINY + static const uint16_t Table[256]; +#endif + + uint16_t value_; + +public: + enum { NumBytes = 2 }; + + TransferCRC() + : value_(0xFFFFU) + { } + +#if UAVCAN_TINY + void add(uint8_t byte) + { + value_ ^= uint16_t(uint16_t(byte) << 8); + for (uint8_t bit = 8; bit > 0; --bit) + { + if (value_ & 0x8000U) + { + value_ = uint16_t(uint16_t(value_ << 1) ^ 0x1021U); + } + else + { + value_ = uint16_t(value_ << 1); + } + } + } +#else + void add(uint8_t byte) + { + value_ = uint16_t(uint16_t((value_ << 8) ^ Table[uint16_t((value_ >> 8) ^ byte) & 0xFFU]) & 0xFFFFU); + } +#endif + + void add(const uint8_t* bytes, unsigned len) + { + UAVCAN_ASSERT(bytes); + while (len--) + { + add(*bytes++); + } + } + + uint16_t get() const { return value_; } +}; + +} + +#endif // UAVCAN_TRANSPORT_CRC_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/dispatcher.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/dispatcher.hpp new file mode 100644 index 000000000000..53bc2e203f2a --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -0,0 +1,242 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_DISPATCHER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_DISPATCHER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class UAVCAN_EXPORT Dispatcher; + +#if !UAVCAN_TINY +/** + * Inherit this class to receive notifications about all TX CAN frames that were transmitted with the loopback flag. + */ +class UAVCAN_EXPORT LoopbackFrameListenerBase : public LinkedListNode +{ + Dispatcher& dispatcher_; + +protected: + explicit LoopbackFrameListenerBase(Dispatcher& dispatcher) + : dispatcher_(dispatcher) + { } + + virtual ~LoopbackFrameListenerBase() { stopListening(); } + + void startListening(); + void stopListening(); + bool isListening() const; + + Dispatcher& getDispatcher() { return dispatcher_; } + +public: + virtual void handleLoopbackFrame(const RxFrame& frame) = 0; +}; + + +class UAVCAN_EXPORT LoopbackFrameListenerRegistry : Noncopyable +{ + LinkedListRoot listeners_; + +public: + void add(LoopbackFrameListenerBase* listener); + void remove(LoopbackFrameListenerBase* listener); + bool doesExist(const LoopbackFrameListenerBase* listener) const; + unsigned getNumListeners() const { return listeners_.getLength(); } + + void invokeListeners(RxFrame& frame); +}; + +/** + * Implement this interface to receive notifications about all incoming CAN frames, including loopback. + */ +class UAVCAN_EXPORT IRxFrameListener +{ +public: + virtual ~IRxFrameListener() { } + + /** + * Make sure to filter out loopback frames if they are not wanted. + */ + virtual void handleRxFrame(const CanRxFrame& frame, CanIOFlags flags) = 0; +}; +#endif + +/** + * This class performs low-level CAN frame routing. + */ +class UAVCAN_EXPORT Dispatcher : Noncopyable +{ + CanIOManager canio_; + ISystemClock& sysclock_; + OutgoingTransferRegistry outgoing_transfer_reg_; + TransferPerfCounter perf_; + + class ListenerRegistry + { + LinkedListRoot list_; + + class DataTypeIDInsertionComparator + { + const DataTypeID id_; + public: + explicit DataTypeIDInsertionComparator(DataTypeID id) : id_(id) { } + bool operator()(const TransferListener* listener) const + { + UAVCAN_ASSERT(listener); + return id_ > listener->getDataTypeDescriptor().getID(); + } + }; + + public: + enum Mode { UniqueListener, ManyListeners }; + + bool add(TransferListener* listener, Mode mode); + void remove(TransferListener* listener); + bool exists(DataTypeID dtid) const; + void cleanup(MonotonicTime ts); + void handleFrame(const RxFrame& frame); + + unsigned getNumEntries() const { return list_.getLength(); } + + const LinkedListRoot& getList() const { return list_; } + }; + + ListenerRegistry lmsg_; + ListenerRegistry lsrv_req_; + ListenerRegistry lsrv_resp_; + +#if !UAVCAN_TINY + LoopbackFrameListenerRegistry loopback_listeners_; + IRxFrameListener* rx_listener_; +#endif + + NodeID self_node_id_; + bool self_node_id_is_set_; + + void handleFrame(const CanRxFrame& can_frame); + + void handleLoopbackFrame(const CanRxFrame& can_frame); + + void notifyRxFrameListener(const CanRxFrame& can_frame, CanIOFlags flags); + +public: + Dispatcher(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock) + : canio_(driver, allocator, sysclock) + , sysclock_(sysclock) + , outgoing_transfer_reg_(allocator) +#if !UAVCAN_TINY + , rx_listener_(UAVCAN_NULLPTR) +#endif + , self_node_id_(NodeID::Broadcast) // Default + , self_node_id_is_set_(false) + { } + + /** + * This version returns strictly when the deadline is reached. + */ + int spin(MonotonicTime deadline); + + /** + * This version does not return until all available frames are processed. + */ + int spinOnce(); + + /** + * Refer to CanIOManager::send() for the parameter description + */ + int send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos, + CanIOFlags flags, uint8_t iface_mask); + + void cleanup(MonotonicTime ts); + + bool registerMessageListener(TransferListener* listener); + bool registerServiceRequestListener(TransferListener* listener); + bool registerServiceResponseListener(TransferListener* listener); + + void unregisterMessageListener(TransferListener* listener); + void unregisterServiceRequestListener(TransferListener* listener); + void unregisterServiceResponseListener(TransferListener* listener); + + bool hasSubscriber(DataTypeID dtid) const; + bool hasPublisher(DataTypeID dtid) const; + bool hasServer(DataTypeID dtid) const; + + unsigned getNumMessageListeners() const { return lmsg_.getNumEntries(); } + unsigned getNumServiceRequestListeners() const { return lsrv_req_.getNumEntries(); } + unsigned getNumServiceResponseListeners() const { return lsrv_resp_.getNumEntries(); } + + /** + * These methods can be used to retreive lists of messages, service requests and service responses the + * dispatcher is currently listening to. + * Note that the list of service response listeners is very volatile, because a response listener will be + * removed from this list as soon as the corresponding service call is complete. + * @{ + */ + const LinkedListRoot& getListOfMessageListeners() const + { + return lmsg_.getList(); + } + const LinkedListRoot& getListOfServiceRequestListeners() const + { + return lsrv_req_.getList(); + } + const LinkedListRoot& getListOfServiceResponseListeners() const + { + return lsrv_resp_.getList(); + } + /** + * @} + */ + + OutgoingTransferRegistry& getOutgoingTransferRegistry() { return outgoing_transfer_reg_; } + +#if !UAVCAN_TINY + LoopbackFrameListenerRegistry& getLoopbackFrameListenerRegistry() { return loopback_listeners_; } + + IRxFrameListener* getRxFrameListener() const { return rx_listener_; } + void removeRxFrameListener() { rx_listener_ = UAVCAN_NULLPTR; } + void installRxFrameListener(IRxFrameListener* listener) + { + UAVCAN_ASSERT(listener != UAVCAN_NULLPTR); + rx_listener_ = listener; + } +#endif + + /** + * Node ID can be set only once. + * Non-unicast Node ID puts the node into passive mode. + */ + NodeID getNodeID() const { return self_node_id_; } + bool setNodeID(NodeID nid); + + /** + * Refer to the specs to learn more about passive mode. + */ + bool isPassiveMode() const { return !getNodeID().isUnicast(); } + + const ISystemClock& getSystemClock() const { return sysclock_; } + ISystemClock& getSystemClock() { return sysclock_; } + + const CanIOManager& getCanIOManager() const { return canio_; } + CanIOManager& getCanIOManager() { return canio_; } + + const TransferPerfCounter& getTransferPerfCounter() const { return perf_; } + TransferPerfCounter& getTransferPerfCounter() { return perf_; } +}; + +} + +#endif // UAVCAN_TRANSPORT_DISPATCHER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/frame.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/frame.hpp new file mode 100644 index 000000000000..8c95beab895e --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/frame.hpp @@ -0,0 +1,144 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_FRAME_HPP_INCLUDED +#define UAVCAN_TRANSPORT_FRAME_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class UAVCAN_EXPORT Frame +{ + enum { PayloadCapacity = 7 }; // Will be redefined when CAN FD is available + + uint8_t payload_[PayloadCapacity]; + TransferPriority transfer_priority_; + TransferType transfer_type_; + DataTypeID data_type_id_; + uint_fast8_t payload_len_; + NodeID src_node_id_; + NodeID dst_node_id_; + TransferID transfer_id_; + bool start_of_transfer_; + bool end_of_transfer_; + bool toggle_; + +public: + Frame() : + transfer_type_(TransferType(NumTransferTypes)), // Invalid value + payload_len_(0), + start_of_transfer_(false), + end_of_transfer_(false), + toggle_(false) + { } + + Frame(DataTypeID data_type_id, + TransferType transfer_type, + NodeID src_node_id, + NodeID dst_node_id, + TransferID transfer_id) : + transfer_priority_(TransferPriority::Default), + transfer_type_(transfer_type), + data_type_id_(data_type_id), + payload_len_(0), + src_node_id_(src_node_id), + dst_node_id_(dst_node_id), + transfer_id_(transfer_id), + start_of_transfer_(false), + end_of_transfer_(false), + toggle_(false) + { + UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast()); + UAVCAN_ASSERT(data_type_id.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type))); + UAVCAN_ASSERT(src_node_id.isUnicast() ? (src_node_id != dst_node_id) : true); + } + + void setPriority(TransferPriority priority) { transfer_priority_ = priority; } + TransferPriority getPriority() const { return transfer_priority_; } + + /** + * Max payload length depends on the transfer type and frame index. + */ + uint8_t getPayloadCapacity() const { return PayloadCapacity; } + uint8_t setPayload(const uint8_t* data, unsigned len); + + unsigned getPayloadLen() const { return payload_len_; } + const uint8_t* getPayloadPtr() const { return payload_; } + + TransferType getTransferType() const { return transfer_type_; } + DataTypeID getDataTypeID() const { return data_type_id_; } + NodeID getSrcNodeID() const { return src_node_id_; } + NodeID getDstNodeID() const { return dst_node_id_; } + TransferID getTransferID() const { return transfer_id_; } + + void setStartOfTransfer(bool x) { start_of_transfer_ = x; } + void setEndOfTransfer(bool x) { end_of_transfer_ = x; } + + bool isStartOfTransfer() const { return start_of_transfer_; } + bool isEndOfTransfer() const { return end_of_transfer_; } + + void flipToggle() { toggle_ = !toggle_; } + bool getToggle() const { return toggle_; } + + bool parse(const CanFrame& can_frame); + bool compile(CanFrame& can_frame) const; + + bool isValid() const; + + bool operator!=(const Frame& rhs) const { return !operator==(rhs); } + bool operator==(const Frame& rhs) const; + +#if UAVCAN_TOSTRING + std::string toString() const; +#endif +}; + + +class UAVCAN_EXPORT RxFrame : public Frame +{ + MonotonicTime ts_mono_; + UtcTime ts_utc_; + uint8_t iface_index_; + +public: + RxFrame() + : iface_index_(0) + { } + + RxFrame(const Frame& frame, MonotonicTime ts_mono, UtcTime ts_utc, uint8_t iface_index) + : ts_mono_(ts_mono) + , ts_utc_(ts_utc) + , iface_index_(iface_index) + { + *static_cast(this) = frame; + } + + bool parse(const CanRxFrame& can_frame); + + /** + * Can't be zero. + */ + MonotonicTime getMonotonicTimestamp() const { return ts_mono_; } + + /** + * Can be zero if not supported by the platform driver. + */ + UtcTime getUtcTimestamp() const { return ts_utc_; } + + uint8_t getIfaceIndex() const { return iface_index_; } + +#if UAVCAN_TOSTRING + std::string toString() const; +#endif +}; + +} + +#endif // UAVCAN_TRANSPORT_FRAME_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp new file mode 100644 index 000000000000..f92636e54053 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp @@ -0,0 +1,130 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_OUTGOING_TRANSFER_REGISTRY_HPP_INCLUDED +#define UAVCAN_TRANSPORT_OUTGOING_TRANSFER_REGISTRY_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class UAVCAN_EXPORT OutgoingTransferRegistryKey +{ + DataTypeID data_type_id_; + uint8_t transfer_type_; + NodeID destination_node_id_; ///< Not applicable for message broadcasting + +public: + OutgoingTransferRegistryKey() + : transfer_type_(0xFF) + { } + + OutgoingTransferRegistryKey(DataTypeID data_type_id, TransferType transfer_type, NodeID destination_node_id) + : data_type_id_(data_type_id) + , transfer_type_(transfer_type) + , destination_node_id_(destination_node_id) + { + UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == destination_node_id.isBroadcast()); + /* + * Service response transfers must use the same Transfer ID as matching service request transfer, + * so this registry is not applicable for service response transfers at all. + */ + UAVCAN_ASSERT(transfer_type != TransferTypeServiceResponse); + } + + DataTypeID getDataTypeID() const { return data_type_id_; } + TransferType getTransferType() const { return TransferType(transfer_type_); } + + bool operator==(const OutgoingTransferRegistryKey& rhs) const + { + return + (data_type_id_ == rhs.data_type_id_) && + (transfer_type_ == rhs.transfer_type_) && + (destination_node_id_ == rhs.destination_node_id_); + } + +#if UAVCAN_TOSTRING + std::string toString() const; +#endif +}; + +/** + * Outgoing transfer registry keeps track of Transfer ID values for all currently existing local transfer senders. + * If a local transfer sender was inactive for a sufficiently long time, the outgoing transfer registry will + * remove the respective Transfer ID tracking object. + */ +class UAVCAN_EXPORT OutgoingTransferRegistry : Noncopyable +{ + struct Value + { + MonotonicTime deadline; + TransferID tid; + }; + + class DeadlineExpiredPredicate + { + const MonotonicTime ts_; + + public: + explicit DeadlineExpiredPredicate(MonotonicTime ts) + : ts_(ts) + { } + + bool operator()(const OutgoingTransferRegistryKey& key, const Value& value) const + { + (void)key; + UAVCAN_ASSERT(!value.deadline.isZero()); + const bool expired = value.deadline <= ts_; + if (expired) + { + UAVCAN_TRACE("OutgoingTransferRegistry", "Expired %s tid=%i", + key.toString().c_str(), int(value.tid.get())); + } + return expired; + } + }; + + class ExistenceCheckingPredicate + { + const DataTypeID dtid_; + const TransferType tt_; + + public: + ExistenceCheckingPredicate(DataTypeID dtid, TransferType tt) + : dtid_(dtid) + , tt_(tt) + { } + + bool operator()(const OutgoingTransferRegistryKey& key, const Value&) const + { + return dtid_ == key.getDataTypeID() && tt_ == key.getTransferType(); + } + }; + + Map map_; + +public: + static const MonotonicDuration MinEntryLifetime; + + explicit OutgoingTransferRegistry(IPoolAllocator& allocator) + : map_(allocator) + { } + + TransferID* accessOrCreate(const OutgoingTransferRegistryKey& key, MonotonicTime new_deadline); + + bool exists(DataTypeID dtid, TransferType tt) const; + + void cleanup(MonotonicTime ts); +}; + +} + +#endif // UAVCAN_TRANSPORT_OUTGOING_TRANSFER_REGISTRY_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/perf_counter.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/perf_counter.hpp new file mode 100644 index 000000000000..e056f3c0b769 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/perf_counter.hpp @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_PERF_COUNTER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_PERF_COUNTER_HPP_INCLUDED + +#include +#include +#include + +namespace uavcan +{ + +#if UAVCAN_TINY + +class UAVCAN_EXPORT TransferPerfCounter : Noncopyable +{ +public: + void addTxTransfer() { } + void addRxTransfer() { } + void addError() { } + void addErrors(unsigned) { } + uint64_t getTxTransferCount() const { return 0; } + uint64_t getRxTransferCount() const { return 0; } + uint64_t getErrorCount() const { return 0; } +}; + +#else + +/** + * The class is declared noncopyable for two reasons: + * - to prevent accidental pass-by-value into a mutator + * - to make the addresses of the counters fixed and exposable to the user of the library + */ +class UAVCAN_EXPORT TransferPerfCounter : Noncopyable +{ + uint64_t transfers_tx_; + uint64_t transfers_rx_; + uint64_t errors_; + +public: + TransferPerfCounter() + : transfers_tx_(0) + , transfers_rx_(0) + , errors_(0) + { } + + void addTxTransfer() { transfers_tx_++; } + void addRxTransfer() { transfers_rx_++; } + + void addError() { errors_++; } + + void addErrors(unsigned errors) + { + errors_ += errors; + } + + /** + * Returned references are guaranteed to be valid as long as this instance of Node exists. + * This is enforced by virtue of the class being Noncopyable. + */ + const uint64_t& getTxTransferCount() const { return transfers_tx_; } + const uint64_t& getRxTransferCount() const { return transfers_rx_; } + const uint64_t& getErrorCount() const { return errors_; } +}; + +#endif + +} + +#endif // UAVCAN_TRANSPORT_PERF_COUNTER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer.hpp new file mode 100644 index 000000000000..7e5d5b88215c --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer.hpp @@ -0,0 +1,149 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_TRANSFER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_TRANSFER_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ + +static const unsigned GuaranteedPayloadLenPerFrame = 7; ///< Guaranteed for all transfers, all CAN standards + +enum TransferType +{ + TransferTypeServiceResponse = 0, + TransferTypeServiceRequest = 1, + TransferTypeMessageBroadcast = 2 +}; + +static const uint8_t NumTransferTypes = 3; + + +class UAVCAN_EXPORT TransferPriority +{ + uint8_t value_; + +public: + static const uint8_t BitLen = 5U; + static const uint8_t NumericallyMax = (1U << BitLen) - 1; + static const uint8_t NumericallyMin = 0; + + /// This priority is used by default + static const TransferPriority Default; + static const TransferPriority MiddleLower; + static const TransferPriority OneHigherThanLowest; + static const TransferPriority OneLowerThanHighest; + static const TransferPriority Lowest; + + TransferPriority() : value_(0xFF) { } + + TransferPriority(uint8_t value) // Implicit + : value_(value) + { + UAVCAN_ASSERT(isValid()); + } + + template + static TransferPriority fromPercent() + { + StaticAssert<(Percent <= 100)>::check(); + enum { Result = ((100U - Percent) * NumericallyMax) / 100U }; + StaticAssert<(Result <= NumericallyMax)>::check(); + StaticAssert<(Result >= NumericallyMin)>::check(); + return TransferPriority(Result); + } + + uint8_t get() const { return value_; } + + bool isValid() const { return value_ < (1U << BitLen); } + + bool operator!=(TransferPriority rhs) const { return value_ != rhs.value_; } + bool operator==(TransferPriority rhs) const { return value_ == rhs.value_; } +}; + + +class UAVCAN_EXPORT TransferID +{ + uint8_t value_; + +public: + static const uint8_t BitLen = 5U; + static const uint8_t Max = (1U << BitLen) - 1U; + static const uint8_t Half = (1U << BitLen) / 2U; + + TransferID() + : value_(0) + { } + + TransferID(uint8_t value) // implicit + : value_(value) + { + value_ &= Max; + UAVCAN_ASSERT(value == value_); + } + + bool operator!=(TransferID rhs) const { return !operator==(rhs); } + bool operator==(TransferID rhs) const { return get() == rhs.get(); } + + void increment() + { + value_ = (value_ + 1) & Max; + } + + uint8_t get() const + { + UAVCAN_ASSERT(value_ <= Max); + return value_; + } + + /** + * Amount of increment() calls to reach rhs value. + */ + int computeForwardDistance(TransferID rhs) const; +}; + + +class UAVCAN_EXPORT NodeID +{ + static const uint8_t ValueBroadcast = 0; + static const uint8_t ValueInvalid = 0xFF; + uint8_t value_; + +public: + static const uint8_t BitLen = 7U; + static const uint8_t Max = (1U << BitLen) - 1U; + static const uint8_t MaxRecommendedForRegularNodes = Max - 2; + static const NodeID Broadcast; + + NodeID() : value_(ValueInvalid) { } + + NodeID(uint8_t value) // Implicit + : value_(value) + { + UAVCAN_ASSERT(isValid()); + } + + uint8_t get() const { return value_; } + + bool isValid() const { return value_ <= Max; } + bool isBroadcast() const { return value_ == ValueBroadcast; } + bool isUnicast() const { return (value_ <= Max) && (value_ != ValueBroadcast); } + + bool operator!=(NodeID rhs) const { return !operator==(rhs); } + bool operator==(NodeID rhs) const { return value_ == rhs.value_; } + + bool operator<(NodeID rhs) const { return value_ < rhs.value_; } + bool operator>(NodeID rhs) const { return value_ > rhs.value_; } + bool operator<=(NodeID rhs) const { return value_ <= rhs.value_; } + bool operator>=(NodeID rhs) const { return value_ >= rhs.value_; } +}; + +} + +#endif // UAVCAN_TRANSPORT_TRANSFER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_buffer.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_buffer.hpp new file mode 100644 index 000000000000..8d7bb873dc13 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_buffer.hpp @@ -0,0 +1,199 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_TRANSFER_BUFFER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_TRANSFER_BUFFER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Standalone static buffer + */ +class StaticTransferBufferImpl : public ITransferBuffer +{ + uint8_t* const data_; + const uint16_t size_; + uint16_t max_write_pos_; + +public: + StaticTransferBufferImpl(uint8_t* buf, uint16_t buf_size) : + data_(buf), + size_(buf_size), + max_write_pos_(0) + { } + + virtual int read(unsigned offset, uint8_t* data, unsigned len) const override; + virtual int write(unsigned offset, const uint8_t* data, unsigned len) override; + + void reset(); + + uint16_t getSize() const { return size_; } + + uint8_t* getRawPtr() { return data_; } + const uint8_t* getRawPtr() const { return data_; } + + uint16_t getMaxWritePos() const { return max_write_pos_; } + void setMaxWritePos(uint16_t value) { max_write_pos_ = value; } +}; + +template +class UAVCAN_EXPORT StaticTransferBuffer : public StaticTransferBufferImpl +{ + uint8_t buffer_[Size]; +public: + StaticTransferBuffer() : StaticTransferBufferImpl(buffer_, Size) + { + StaticAssert<(Size > 0)>::check(); + } +}; + +/** + * Internal for TransferBufferManager + */ +class UAVCAN_EXPORT TransferBufferManagerKey +{ + NodeID node_id_; + uint8_t transfer_type_; + +public: + TransferBufferManagerKey() + : transfer_type_(TransferType(0)) + { + UAVCAN_ASSERT(isEmpty()); + } + + TransferBufferManagerKey(NodeID node_id, TransferType ttype) + : node_id_(node_id) + , transfer_type_(ttype) + { + UAVCAN_ASSERT(!isEmpty()); + } + + bool operator==(const TransferBufferManagerKey& rhs) const + { + return node_id_ == rhs.node_id_ && transfer_type_ == rhs.transfer_type_; + } + + bool isEmpty() const { return !node_id_.isValid(); } + + NodeID getNodeID() const { return node_id_; } + TransferType getTransferType() const { return TransferType(transfer_type_); } + +#if UAVCAN_TOSTRING + std::string toString() const; +#endif +}; + +/** + * Resizable gather/scatter storage. + * reset() call releases all memory blocks. + * Supports unordered write operations - from higher to lower offsets + */ +class UAVCAN_EXPORT TransferBufferManagerEntry : public ITransferBuffer + , public LinkedListNode +{ + struct Block : LinkedListNode + { + enum { Size = MemPoolBlockSize - sizeof(LinkedListNode) }; + uint8_t data[static_cast(Size)]; + + static Block* instantiate(IPoolAllocator& allocator); + static void destroy(Block*& obj, IPoolAllocator& allocator); + + void read(uint8_t*& outptr, unsigned target_offset, + unsigned& total_offset, unsigned& left_to_read); + void write(const uint8_t*& inptr, unsigned target_offset, + unsigned& total_offset, unsigned& left_to_write); + }; + + IPoolAllocator& allocator_; + LinkedListRoot blocks_; // Blocks are ordered from lower to higher buffer offset + uint16_t max_write_pos_; + const uint16_t max_size_; + TransferBufferManagerKey key_; + +public: + TransferBufferManagerEntry(IPoolAllocator& allocator, uint16_t max_size) : + allocator_(allocator), + max_write_pos_(0), + max_size_(max_size) + { + StaticAssert<(Block::Size > 8)>::check(); + IsDynamicallyAllocatable::check(); + IsDynamicallyAllocatable::check(); + } + + virtual ~TransferBufferManagerEntry() { reset(); } + + static TransferBufferManagerEntry* instantiate(IPoolAllocator& allocator, uint16_t max_size); + static void destroy(TransferBufferManagerEntry*& obj, IPoolAllocator& allocator); + + virtual int read(unsigned offset, uint8_t* data, unsigned len) const override; + virtual int write(unsigned offset, const uint8_t* data, unsigned len) override; + + void reset(const TransferBufferManagerKey& key = TransferBufferManagerKey()); + + const TransferBufferManagerKey& getKey() const { return key_; } + bool isEmpty() const { return key_.isEmpty(); } +}; + +/** + * Buffer manager implementation. + */ +class TransferBufferManager : public Noncopyable +{ + LinkedListRoot buffers_; + IPoolAllocator& allocator_; + const uint16_t max_buf_size_; + + TransferBufferManagerEntry* findFirst(const TransferBufferManagerKey& key); + +public: + TransferBufferManager(uint16_t max_buf_size, IPoolAllocator& allocator) : + allocator_(allocator), + max_buf_size_(max_buf_size) + { } + + ~TransferBufferManager(); + + ITransferBuffer* access(const TransferBufferManagerKey& key); + ITransferBuffer* create(const TransferBufferManagerKey& key); + void remove(const TransferBufferManagerKey& key); + bool isEmpty() const; + + unsigned getNumBuffers() const; +}; + +/** + * Convinience class. + */ +class UAVCAN_EXPORT TransferBufferAccessor +{ + TransferBufferManager& bufmgr_; + const TransferBufferManagerKey key_; + +public: + TransferBufferAccessor(TransferBufferManager& bufmgr, TransferBufferManagerKey key) : + bufmgr_(bufmgr), + key_(key) + { + UAVCAN_ASSERT(!key.isEmpty()); + } + ITransferBuffer* access() { return bufmgr_.access(key_); } + ITransferBuffer* create() { return bufmgr_.create(key_); } + void remove() { bufmgr_.remove(key_); } +}; + +} + +#endif // UAVCAN_TRANSPORT_TRANSFER_BUFFER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_listener.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_listener.hpp new file mode 100644 index 000000000000..4ccf36a72fca --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_listener.hpp @@ -0,0 +1,194 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_TRANSFER_LISTENER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_TRANSFER_LISTENER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Container for received transfer. + */ +class UAVCAN_EXPORT IncomingTransfer : public ITransferBuffer +{ + MonotonicTime ts_mono_; + UtcTime ts_utc_; + TransferPriority transfer_priority_; + TransferType transfer_type_; + TransferID transfer_id_; + NodeID src_node_id_; + uint8_t iface_index_; + + /// That's a no-op, asserts in debug builds + virtual int write(unsigned offset, const uint8_t* data, unsigned len) override; + +protected: + IncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc, TransferPriority transfer_priority, + TransferType transfer_type, TransferID transfer_id, NodeID source_node_id, + uint8_t iface_index) + : ts_mono_(ts_mono) + , ts_utc_(ts_utc) + , transfer_priority_(transfer_priority) + , transfer_type_(transfer_type) + , transfer_id_(transfer_id) + , src_node_id_(source_node_id) + , iface_index_(iface_index) + { } + +public: + /** + * Dispose the payload buffer. Further calls to read() will not be possible. + */ + virtual void release() { } + + /** + * Whether this is a anonymous transfer + */ + virtual bool isAnonymousTransfer() const { return false; } + + MonotonicTime getMonotonicTimestamp() const { return ts_mono_; } + UtcTime getUtcTimestamp() const { return ts_utc_; } + TransferPriority getPriority() const { return transfer_priority_; } + TransferType getTransferType() const { return transfer_type_; } + TransferID getTransferID() const { return transfer_id_; } + NodeID getSrcNodeID() const { return src_node_id_; } + uint8_t getIfaceIndex() const { return iface_index_; } +}; + +/** + * Internal. + */ +class UAVCAN_EXPORT SingleFrameIncomingTransfer : public IncomingTransfer +{ + const uint8_t* const payload_; + const uint8_t payload_len_; +public: + explicit SingleFrameIncomingTransfer(const RxFrame& frm); + virtual int read(unsigned offset, uint8_t* data, unsigned len) const override; + virtual bool isAnonymousTransfer() const override; +}; + +/** + * Internal. + */ +class UAVCAN_EXPORT MultiFrameIncomingTransfer : public IncomingTransfer, Noncopyable +{ + TransferBufferAccessor& buf_acc_; +public: + MultiFrameIncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc, const RxFrame& last_frame, + TransferBufferAccessor& tba); + virtual int read(unsigned offset, uint8_t* data, unsigned len) const override; + virtual void release() override { buf_acc_.remove(); } +}; + +/** + * Internal, refer to the transport dispatcher class. + */ +class UAVCAN_EXPORT TransferListener : public LinkedListNode +{ + const DataTypeDescriptor& data_type_; + TransferBufferManager bufmgr_; + Map receivers_; + TransferPerfCounter& perf_; + const TransferCRC crc_base_; ///< Pre-initialized with data type hash, thus constant + bool allow_anonymous_transfers_; + + class TimedOutReceiverPredicate + { + const MonotonicTime ts_; + TransferBufferManager& parent_bufmgr_; + + public: + TimedOutReceiverPredicate(MonotonicTime arg_ts, TransferBufferManager& arg_bufmgr) + : ts_(arg_ts) + , parent_bufmgr_(arg_bufmgr) + { } + + bool operator()(const TransferBufferManagerKey& key, const TransferReceiver& value) const; + }; + + bool checkPayloadCrc(const uint16_t compare_with, const ITransferBuffer& tbb) const; + +protected: + void handleReception(TransferReceiver& receiver, const RxFrame& frame, TransferBufferAccessor& tba); + void handleAnonymousTransferReception(const RxFrame& frame); + + virtual void handleIncomingTransfer(IncomingTransfer& transfer) = 0; + +public: + TransferListener(TransferPerfCounter& perf, const DataTypeDescriptor& data_type, + uint16_t max_buffer_size, IPoolAllocator& allocator) + : data_type_(data_type) + , bufmgr_(max_buffer_size, allocator) + , receivers_(allocator) + , perf_(perf) + , crc_base_(data_type.getSignature().toTransferCRC()) + , allow_anonymous_transfers_(false) + { } + + virtual ~TransferListener(); + + const DataTypeDescriptor& getDataTypeDescriptor() const { return data_type_; } + + /** + * By default, anonymous transfers will be ignored. + * This option allows to enable reception of anonymous transfers. + */ + void allowAnonymousTransfers() { allow_anonymous_transfers_ = true; } + + void cleanup(MonotonicTime ts); + + virtual void handleFrame(const RxFrame& frame); +}; + +/** + * This class is used by transfer listener to decide if the frame should be accepted or ignored. + */ +class ITransferAcceptanceFilter +{ +public: + /** + * If it returns false, the frame will be ignored, otherwise accepted. + */ + virtual bool shouldAcceptFrame(const RxFrame& frame) const = 0; + + virtual ~ITransferAcceptanceFilter() { } +}; + +/** + * This class should be derived by callers. + */ +class UAVCAN_EXPORT TransferListenerWithFilter : public TransferListener +{ + const ITransferAcceptanceFilter* filter_; + + virtual void handleFrame(const RxFrame& frame) override; + +public: + TransferListenerWithFilter(TransferPerfCounter& perf, const DataTypeDescriptor& data_type, + uint16_t max_buffer_size, IPoolAllocator& allocator) + : TransferListener(perf, data_type, max_buffer_size, allocator) + , filter_(UAVCAN_NULLPTR) + { } + + void installAcceptanceFilter(const ITransferAcceptanceFilter* acceptance_filter) + { + filter_ = acceptance_filter; + } +}; + +} + +#endif // UAVCAN_TRANSPORT_TRANSFER_LISTENER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_receiver.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_receiver.hpp new file mode 100644 index 000000000000..20c4b41eeab4 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_receiver.hpp @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_TRANSFER_RECEIVER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_TRANSFER_RECEIVER_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ + +class UAVCAN_EXPORT TransferReceiver +{ +public: + enum ResultCode { ResultNotComplete, ResultComplete, ResultSingleFrame }; + + static const uint16_t MinTransferIntervalMSec = 1; + static const uint16_t MaxTransferIntervalMSec = 0xFFFF; + static const uint16_t DefaultTransferIntervalMSec = 1000; + static const uint16_t DefaultTidTimeoutMSec = 1000; + + static MonotonicDuration getDefaultTransferInterval() + { + return MonotonicDuration::fromMSec(DefaultTransferIntervalMSec); + } + static MonotonicDuration getMinTransferInterval() { return MonotonicDuration::fromMSec(MinTransferIntervalMSec); } + static MonotonicDuration getMaxTransferInterval() { return MonotonicDuration::fromMSec(MaxTransferIntervalMSec); } + +private: + enum { IfaceIndexNotSet = MaxCanIfaces }; + + enum { ErrorCntMask = 31 }; + enum { IfaceIndexMask = MaxCanIfaces }; + + MonotonicTime prev_transfer_ts_; + MonotonicTime this_transfer_ts_; + UtcTime first_frame_ts_; + uint16_t transfer_interval_msec_; + uint16_t this_transfer_crc_; + + uint16_t buffer_write_pos_; + + TransferID tid_; // 1 byte field + + // 1 byte aligned bitfields: + uint8_t next_toggle_ : 1; + uint8_t iface_index_ : 2; + mutable uint8_t error_cnt_ : 5; + + bool isInitialized() const { return iface_index_ != IfaceIndexNotSet; } + + bool isMidTransfer() const { return buffer_write_pos_ > 0; } + + MonotonicDuration getIfaceSwitchDelay() const; + MonotonicDuration getTidTimeout() const; + + void registerError() const; + + void updateTransferTimings(); + void prepareForNextTransfer(); + + bool validate(const RxFrame& frame) const; + bool writePayload(const RxFrame& frame, ITransferBuffer& buf); + ResultCode receive(const RxFrame& frame, TransferBufferAccessor& tba); + +public: + TransferReceiver() : + transfer_interval_msec_(DefaultTransferIntervalMSec), + this_transfer_crc_(0), + buffer_write_pos_(0), + next_toggle_(false), + iface_index_(IfaceIndexNotSet), + error_cnt_(0) + { } + + bool isTimedOut(MonotonicTime current_ts) const; + + ResultCode addFrame(const RxFrame& frame, TransferBufferAccessor& tba); + + uint8_t yieldErrorCount(); + + MonotonicTime getLastTransferTimestampMonotonic() const { return prev_transfer_ts_; } + UtcTime getLastTransferTimestampUtc() const { return first_frame_ts_; } + + uint16_t getLastTransferCrc() const { return this_transfer_crc_; } + + MonotonicDuration getInterval() const { return MonotonicDuration::fromMSec(transfer_interval_msec_); } +}; + +} + +#endif // UAVCAN_TRANSPORT_TRANSFER_RECEIVER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_sender.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_sender.hpp new file mode 100644 index 000000000000..462e51ed397b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_sender.hpp @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_TRANSFER_SENDER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_TRANSFER_SENDER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class UAVCAN_EXPORT TransferSender +{ + const MonotonicDuration max_transfer_interval_; + + Dispatcher& dispatcher_; + + TransferPriority priority_; + CanTxQueue::Qos qos_; + TransferCRC crc_base_; + DataTypeID data_type_id_; + CanIOFlags flags_; + uint8_t iface_mask_; + bool allow_anonymous_transfers_; + + void registerError() const; + +public: + enum { AllIfacesMask = 0xFF }; + + static MonotonicDuration getDefaultMaxTransferInterval() + { + return MonotonicDuration::fromMSec(60 * 1000); + } + + TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos, + MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval()) + : max_transfer_interval_(max_transfer_interval) + , dispatcher_(dispatcher) + , priority_(TransferPriority::Default) + , qos_(CanTxQueue::Qos()) + , flags_(CanIOFlags(0)) + , iface_mask_(AllIfacesMask) + , allow_anonymous_transfers_(false) + { + init(data_type, qos); + } + + TransferSender(Dispatcher& dispatcher, MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval()) + : max_transfer_interval_(max_transfer_interval) + , dispatcher_(dispatcher) + , priority_(TransferPriority::Default) + , qos_(CanTxQueue::Qos()) + , flags_(CanIOFlags(0)) + , iface_mask_(AllIfacesMask) + , allow_anonymous_transfers_(false) + { } + + void init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos); + + bool isInitialized() const { return data_type_id_ != DataTypeID(); } + + CanIOFlags getCanIOFlags() const { return flags_; } + void setCanIOFlags(CanIOFlags flags) { flags_ = flags; } + + uint8_t getIfaceMask() const { return iface_mask_; } + void setIfaceMask(uint8_t iface_mask) + { + UAVCAN_ASSERT(iface_mask); + iface_mask_ = iface_mask; + } + + TransferPriority getPriority() const { return priority_; } + void setPriority(TransferPriority prio) { priority_ = prio; } + + /** + * Anonymous transfers (i.e. transfers that don't carry a valid Source Node ID) can be sent if + * the local node is configured in passive mode (i.e. the node doesn't have a valid Node ID). + * By default, this class will return an error if it is asked to send a transfer while the + * node is configured in passive mode. However, if this option is enabled, it will be possible + * to send anonymous transfers from passive mode. + */ + void allowAnonymousTransfers() { allow_anonymous_transfers_ = true; } + + /** + * Send with explicit Transfer ID. + * Should be used only for service responses, where response TID should match request TID. + */ + int send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline, + MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id, + TransferID tid) const; + + /** + * Send with automatic Transfer ID. + * + * Note that as long as the local node operates in passive mode, the + * flag @ref CanIOFlagAbortOnError will be set implicitly for all outgoing frames. + * + * TID is managed by OutgoingTransferRegistry. + */ + int send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline, + MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id) const; +}; + +} + +#endif // UAVCAN_TRANSPORT_TRANSFER_SENDER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/uavcan.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/uavcan.hpp new file mode 100644 index 000000000000..9a0139aeb305 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/uavcan.hpp @@ -0,0 +1,27 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + * + * This header should be included by the user application. + */ + +#ifndef UAVCAN_UAVCAN_HPP_INCLUDED +#define UAVCAN_UAVCAN_HPP_INCLUDED + +#include +#include + +// High-level node logic +#include +#include +#include +#include +#include +#include +#include + +// Util +#include +#include +#include + +#endif // UAVCAN_UAVCAN_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/bitset.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/bitset.hpp new file mode 100644 index 000000000000..3ef06494bbca --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/bitset.hpp @@ -0,0 +1,189 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_BITSET_HPP_INCLUDED +#define UAVCAN_UTIL_BITSET_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ +/** + * STL-like bitset + */ +template +class UAVCAN_EXPORT BitSet +{ + enum { NumBytes = (NumBits + 7) / 8 }; + + static std::size_t getByteNum(std::size_t bit_num) { return bit_num / 8; } + + static std::size_t getBitNum(const std::size_t bit_num) { return bit_num % 8; } + + static void validatePos(std::size_t& inout_pos) + { + if (inout_pos >= NumBits) + { + UAVCAN_ASSERT(0); + inout_pos = NumBits - 1; + } + } + + char data_[NumBytes]; + +public: + class Reference + { + friend class BitSet; + + BitSet* const parent_; + const std::size_t bitpos_; + + Reference(BitSet* arg_parent, std::size_t arg_bitpos) + : parent_(arg_parent) + , bitpos_(arg_bitpos) + { } + + public: + Reference& operator=(bool x) + { + parent_->set(bitpos_, x); + return *this; + } + + Reference& operator=(const Reference& x) + { + parent_->set(bitpos_, x); + return *this; + } + + bool operator~() const + { + return !parent_->test(bitpos_); + } + + operator bool() const + { + return parent_->test(bitpos_); + } + }; + + BitSet() + : data_() + { + reset(); + } + + BitSet& reset() + { + std::memset(data_, 0, NumBytes); + return *this; + } + + BitSet& set() + { + std::memset(data_, 0xFF, NumBytes); + return *this; + } + + BitSet& set(std::size_t pos, bool val = true) + { + validatePos(pos); + if (val) + { + data_[getByteNum(pos)] = char(data_[getByteNum(pos)] | (1 << getBitNum(pos))); + } + else + { + data_[getByteNum(pos)] = char(data_[getByteNum(pos)] & ~(1 << getBitNum(pos))); + } + return *this; + } + + bool test(std::size_t pos) const + { + return (data_[getByteNum(pos)] & (1 << getBitNum(pos))) != 0; + } + + bool any() const + { + for (std::size_t i = 0; i < NumBits; ++i) + { + if (test(i)) + { + return true; + } + } + return false; + } + + std::size_t count() const + { + std::size_t retval = 0; + for (std::size_t i = 0; i < NumBits; ++i) + { + retval += test(i) ? 1U : 0U; + } + return retval; + } + + std::size_t size() const { return NumBits; } + + bool operator[](std::size_t pos) const + { + return test(pos); + } + + Reference operator[](std::size_t pos) + { + validatePos(pos); + return Reference(this, pos); + } + + BitSet& operator=(const BitSet & rhs) + { + if (&rhs == this) + { + return *this; + } + for (std::size_t i = 0; i < NumBytes; ++i) + { + data_[i] = rhs.data_[i]; + } + return *this; + } + + bool operator!=(const BitSet& rhs) const { return !operator==(rhs); } + bool operator==(const BitSet& rhs) const + { + for (std::size_t i = 0; i < NumBits; ++i) + { + if (test(i) != rhs.test(i)) + { + return false; + } + } + return true; + } +}; + +template <> class BitSet<0>; ///< Invalid instantiation + + +template +Stream& operator<<(Stream& s, const BitSet& x) +{ + for (std::size_t i = NumBits; i > 0; --i) + { + s << (x.test(i-1) ? "1" : "0"); + } + return s; +} + +} + +#endif // UAVCAN_UTIL_BITSET_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/comparison.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/comparison.hpp new file mode 100644 index 000000000000..587d8625aa11 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/comparison.hpp @@ -0,0 +1,271 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_COMPARISON_HPP_INCLUDED +#define UAVCAN_UTIL_COMPARISON_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +/** + * Exact comparison of two floats that suppresses the compiler warnings. + */ +template +UAVCAN_EXPORT +inline bool areFloatsExactlyEqual(const T& left, const T& right) +{ + return (left <= right) && (left >= right); +} + +/** + * This function performs fuzzy comparison of two floating point numbers. + * Type of T can be either float, double or long double. + * For details refer to http://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ + * See also: @ref UAVCAN_FLOAT_COMPARISON_EPSILON_MULT. + */ +template +UAVCAN_EXPORT +inline bool areFloatsClose(T a, T b, const T& absolute_epsilon, const T& relative_epsilon) +{ + // NAN + if (isNaN(a) || isNaN(b)) + { + return false; + } + + // Infinity + if (isInfinity(a) || isInfinity(b)) + { + return areFloatsExactlyEqual(a, b); + } + + // Close numbers near zero + const T diff = std::fabs(a - b); + if (diff <= absolute_epsilon) + { + return true; + } + + // General case + a = std::fabs(a); + b = std::fabs(b); + const T largest = (b > a) ? b : a; + return (diff <= largest * relative_epsilon); +} + +/** + * This namespace contains implementation details for areClose(). + * Don't try this at home. + */ +namespace are_close_impl_ +{ + +struct Applicable { char foo[1]; }; +struct NotApplicable { long long foo[16]; }; + +template +struct HasIsCloseMethod +{ + template struct ConstRef { }; + template struct ByValue { }; + + template static Applicable test(ConstRef*); + template static Applicable test(ByValue*); + + template static NotApplicable test(...); + + enum { Result = sizeof(test(UAVCAN_NULLPTR)) }; +}; + +/// First stage: bool L::isClose(R) +template +UAVCAN_EXPORT +inline bool areCloseImplFirst(const L& left, const R& right, IntToType) +{ + return left.isClose(right); +} + +/// Second stage: bool R::isClose(L) +template +UAVCAN_EXPORT +inline bool areCloseImplSecond(const L& left, const R& right, IntToType) +{ + return right.isClose(left); +} + +/// Second stage: L == R +template +UAVCAN_EXPORT +inline bool areCloseImplSecond(const L& left, const R& right, IntToType) +{ + return left == right; +} + +/// First stage: select either L == R or bool R::isClose(L) +template +UAVCAN_EXPORT +inline bool areCloseImplFirst(const L& left, const R& right, IntToType) +{ + return are_close_impl_::areCloseImplSecond(left, right, + IntToType::Result>()); +} + +} // namespace are_close_impl_ + +/** + * Generic fuzzy comparison function. + * + * This function properly handles floating point comparison, including mixed floating point type comparison, + * e.g. float with long double. + * + * Two objects of types A and B will be fuzzy comparable if either method is defined: + * - bool A::isClose(const B&) const + * - bool A::isClose(B) const + * - bool B::isClose(const A&) const + * - bool B::isClose(A) const + * + * Call areClose(A, B) will be dispatched as follows: + * + * - If A and B are both floating point types (float, double, long double) - possibly different - the call will be + * dispatched to @ref areFloatsClose(). If A and B are different types, value of the larger type will be coerced + * to the smaller type, e.g. areClose(long double, float) --> areClose(float, float). + * + * - If A defines isClose() that accepts B, the call will be dispatched there. + * + * - If B defines isClose() that accepts A, the call will be dispatched there (A/B swapped). + * + * - Last resort is A == B. + * + * Alternatively, a custom behavior can be implemented via template specialization. + * + * See also: @ref UAVCAN_FLOAT_COMPARISON_EPSILON_MULT. + * + * Examples: + * areClose(1.0, 1.0F) --> true + * areClose(1.0, 1.0F + std::numeric_limits::epsilon()) --> true + * areClose(1.0, 1.1) --> false + * areClose("123", std::string("123")) --> true (using std::string's operator ==) + * areClose(inf, inf) --> true + * areClose(inf, -inf) --> false + * areClose(nan, nan) --> false (any comparison with nan returns false) + * areClose(123, "123") --> compilation error: operator == is not defined + */ +template +UAVCAN_EXPORT +inline bool areClose(const L& left, const R& right) +{ + return are_close_impl_::areCloseImplFirst(left, right, + IntToType::Result>()); +} + +/* + * Float comparison specializations + */ +template <> +UAVCAN_EXPORT +inline bool areClose(const float& left, const float& right) +{ + return areFloatsClose(left, right, NumericTraits::epsilon(), + NumericTraits::epsilon() * FloatComparisonEpsilonMult); +} + +template <> +UAVCAN_EXPORT +inline bool areClose(const double& left, const double& right) +{ + return areFloatsClose(left, right, NumericTraits::epsilon(), + NumericTraits::epsilon() * FloatComparisonEpsilonMult); +} + +template <> +UAVCAN_EXPORT +inline bool areClose(const long double& left, const long double& right) +{ + return areFloatsClose(left, right, NumericTraits::epsilon(), + NumericTraits::epsilon() * FloatComparisonEpsilonMult); +} + +/* + * Mixed floating type comparison - coercing larger type to smaller type + */ +template <> +UAVCAN_EXPORT +inline bool areClose(const float& left, const double& right) +{ + return areClose(left, static_cast(right)); +} + +template <> +UAVCAN_EXPORT +inline bool areClose(const double& left, const float& right) +{ + return areClose(static_cast(left), right); +} + +template <> +UAVCAN_EXPORT +inline bool areClose(const float& left, const long double& right) +{ + return areClose(left, static_cast(right)); +} + +template <> +UAVCAN_EXPORT +inline bool areClose(const long double& left, const float& right) +{ + return areClose(static_cast(left), right); +} + +template <> +UAVCAN_EXPORT +inline bool areClose(const double& left, const long double& right) +{ + return areClose(left, static_cast(right)); +} + +template <> +UAVCAN_EXPORT +inline bool areClose(const long double& left, const double& right) +{ + return areClose(static_cast(left), right); +} + +/** + * Comparison against zero. + * Helps to compare a floating point number against zero if the exact type is unknown. + * For non-floating point types performs exact comparison against integer zero. + */ +template +UAVCAN_EXPORT +inline bool isCloseToZero(const T& x) +{ + return x == 0; +} + +template <> +UAVCAN_EXPORT +inline bool isCloseToZero(const float& x) +{ + return areClose(x, static_cast(0.0F)); +} + +template <> +UAVCAN_EXPORT +inline bool isCloseToZero(const double& x) +{ + return areClose(x, static_cast(0.0)); +} + +template <> +UAVCAN_EXPORT +inline bool isCloseToZero(const long double& x) +{ + return areClose(x, static_cast(0.0L)); +} + +} + +#endif // UAVCAN_UTIL_COMPARISON_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/lazy_constructor.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/lazy_constructor.hpp new file mode 100644 index 000000000000..c2076701deb1 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/lazy_constructor.hpp @@ -0,0 +1,182 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_LAZY_CONSTRUCTOR_HPP_INCLUDED +#define UAVCAN_UTIL_LAZY_CONSTRUCTOR_HPP_INCLUDED + +#include +#include +#include +#include + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif + +namespace uavcan +{ +/** + * This class allows to postpone the object contruction. + * It statically allocates a pool of memory of just enough size to fit the object being constructed. + * Later call to construct<>() calls the constructor of the object. + * The object will be destroyed automatically when the container class destructor is called. + * The memory pool is aligned at the size of the largest primitive type (long double or long long int). + */ +template +class UAVCAN_EXPORT LazyConstructor +{ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + struct + { + alignas(T) unsigned char pool[sizeof(T)]; + } data_; +#else + union + { + unsigned char pool[sizeof(T)]; + long double _aligner1; + long long _aligner2; + } data_; +#endif + + T* ptr_; + + void ensureConstructed() const + { + if (!ptr_) + { + handleFatalError("LazyConstructor"); + } + } + + void ensureNotConstructed() const + { + if (ptr_) + { + handleFatalError("LazyConstructor"); + } + } + +public: + LazyConstructor() + : ptr_(UAVCAN_NULLPTR) + { + fill(data_.pool, data_.pool + sizeof(T), uint8_t(0)); + } + + LazyConstructor(const LazyConstructor& rhs) // Implicit + : ptr_(UAVCAN_NULLPTR) + { + fill(data_.pool, data_.pool + sizeof(T), uint8_t(0)); + if (rhs) + { + construct(*rhs); // Invoke copy constructor + } + } + + ~LazyConstructor() { destroy(); } + + LazyConstructor& operator=(const LazyConstructor& rhs) + { + destroy(); + if (rhs) + { + construct(*rhs); // Invoke copy constructor + } + return *this; + } + + bool isConstructed() const { return ptr_ != UAVCAN_NULLPTR; } + + operator T*() const { return ptr_; } + + const T* operator->() const { ensureConstructed(); return ptr_; } + T* operator->() { ensureConstructed(); return ptr_; } + + const T& operator*() const { ensureConstructed(); return *ptr_; } + T& operator*() { ensureConstructed(); return *ptr_; } + + void destroy() + { + if (ptr_) + { + ptr_->~T(); + } + ptr_ = UAVCAN_NULLPTR; + fill(data_.pool, data_.pool + sizeof(T), uint8_t(0)); + } + + void construct() + { + ensureNotConstructed(); + ptr_ = new (static_cast(data_.pool)) T(); + } + +// MAX_ARGS = 6 +// TEMPLATE = ''' +// template <%s> +// void construct(%s) +// { +// ensureNotConstructed(); +// ptr_ = new (static_cast(data_.pool)) T(%s); +// }''' +// for nargs in range(1, MAX_ARGS + 1): +// nums = [(x + 1) for x in range(nargs)] +// l1 = ['typename P%d' % x for x in nums] +// l2 = ['typename ParameterType::Type p%d' % (x, x) for x in nums] +// l3 = ['p%d' % x for x in nums] +// print(TEMPLATE % (', '.join(l1), ', '.join(l2), ', '.join(l3))) + + template + void construct(typename ParameterType::Type p1) + { + ensureNotConstructed(); + ptr_ = new (static_cast(data_.pool)) T(p1); + } + + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2) + { + ensureNotConstructed(); + ptr_ = new (static_cast(data_.pool)) T(p1, p2); + } + + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3) + { + ensureNotConstructed(); + ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3); + } + + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3, typename ParameterType::Type p4) + { + ensureNotConstructed(); + ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3, p4); + } + + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3, typename ParameterType::Type p4, + typename ParameterType::Type p5) + { + ensureNotConstructed(); + ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3, p4, p5); + } + + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3, typename ParameterType::Type p4, + typename ParameterType::Type p5, typename ParameterType::Type p6) + { + ensureNotConstructed(); + ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3, p4, p5, p6); + } +}; + +} + +#endif // UAVCAN_UTIL_LAZY_CONSTRUCTOR_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/linked_list.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/linked_list.hpp new file mode 100644 index 000000000000..915eb9c7e92d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/linked_list.hpp @@ -0,0 +1,176 @@ +/* + * Singly-linked list. + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_LINKED_LIST_HPP_INCLUDED +#define UAVCAN_UTIL_LINKED_LIST_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Classes that are supposed to be linked-listed should derive this. + */ +template +class UAVCAN_EXPORT LinkedListNode : Noncopyable +{ + T* next_; + +protected: + LinkedListNode() + : next_(UAVCAN_NULLPTR) + { } + + ~LinkedListNode() { } + +public: + T* getNextListNode() const { return next_; } + + void setNextListNode(T* node) + { + next_ = node; + } +}; + +/** + * Linked list root. + */ +template +class UAVCAN_EXPORT LinkedListRoot : Noncopyable +{ + T* root_; + +public: + LinkedListRoot() + : root_(UAVCAN_NULLPTR) + { } + + T* get() const { return root_; } + bool isEmpty() const { return get() == UAVCAN_NULLPTR; } + + /** + * Complexity: O(N) + */ + unsigned getLength() const; + + /** + * Inserts the node to the beginning of the list. + * If the node is already present in the list, it will be relocated to the beginning. + * Complexity: O(N) + */ + void insert(T* node); + + /** + * Inserts the node immediately before the node X where predicate(X) returns true. + * If the node is already present in the list, it can be relocated to a new position. + * Complexity: O(2N) (calls remove()) + */ + template + void insertBefore(T* node, Predicate predicate); + + /** + * Removes only the first occurence of the node. + * Complexity: O(N) + */ + void remove(const T* node); +}; + +// ---------------------------------------------------------------------------- + +/* + * LinkedListRoot<> + */ +template +unsigned LinkedListRoot::getLength() const +{ + T* node = root_; + unsigned cnt = 0; + while (node) + { + cnt++; + node = node->getNextListNode(); + } + return cnt; +} + +template +void LinkedListRoot::insert(T* node) +{ + if (node == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + return; + } + remove(node); // Making sure there will be no loops + node->setNextListNode(root_); + root_ = node; +} + +template +template +void LinkedListRoot::insertBefore(T* node, Predicate predicate) +{ + if (node == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + return; + } + + remove(node); + + if (root_ == UAVCAN_NULLPTR || predicate(root_)) + { + node->setNextListNode(root_); + root_ = node; + } + else + { + T* p = root_; + while (p->getNextListNode()) + { + if (predicate(p->getNextListNode())) + { + break; + } + p = p->getNextListNode(); + } + node->setNextListNode(p->getNextListNode()); + p->setNextListNode(node); + } +} + +template +void LinkedListRoot::remove(const T* node) +{ + if (root_ == UAVCAN_NULLPTR || node == UAVCAN_NULLPTR) + { + return; + } + + if (root_ == node) + { + root_ = root_->getNextListNode(); + } + else + { + T* p = root_; + while (p->getNextListNode()) + { + if (p->getNextListNode() == node) + { + p->setNextListNode(p->getNextListNode()->getNextListNode()); + break; + } + p = p->getNextListNode(); + } + } +} + +} + +#endif // UAVCAN_UTIL_LINKED_LIST_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/map.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/map.hpp new file mode 100644 index 000000000000..59ce72da74d8 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/map.hpp @@ -0,0 +1,388 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_MAP_HPP_INCLUDED +#define UAVCAN_UTIL_MAP_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Slow but memory efficient KV container. + * + * KV pairs will be allocated in the node's memory pool. + * + * Please be aware that this container does not perform any speed optimizations to minimize memory footprint, + * so the complexity of most operations is O(N). + * + * Type requirements: + * Both key and value must be copyable, assignable and default constructible. + * Key must implement a comparison operator. + * Key's default constructor must initialize the object into invalid state. + * Size of Key + Value + padding must not exceed MemPoolBlockSize. + */ +template +class UAVCAN_EXPORT Map : Noncopyable +{ +public: + struct KVPair + { + Value value; // Key and value are swapped because this may allow to reduce padding (depending on types) + Key key; + + KVPair() : + value(), + key() + { } + + KVPair(const Key& arg_key, const Value& arg_value) : + value(arg_value), + key(arg_key) + { } + + bool match(const Key& rhs) const { return rhs == key; } + }; + +private: + struct KVGroup : LinkedListNode + { + enum { NumKV = (MemPoolBlockSize - sizeof(LinkedListNode)) / sizeof(KVPair) }; + KVPair kvs[NumKV]; + + KVGroup() + { + StaticAssert<(static_cast(NumKV) > 0)>::check(); + IsDynamicallyAllocatable::check(); + } + + static KVGroup* instantiate(IPoolAllocator& allocator) + { + void* const praw = allocator.allocate(sizeof(KVGroup)); + if (praw == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + return new (praw) KVGroup(); + } + + static void destroy(KVGroup*& obj, IPoolAllocator& allocator) + { + if (obj != UAVCAN_NULLPTR) + { + obj->~KVGroup(); + allocator.deallocate(obj); + obj = UAVCAN_NULLPTR; + } + } + + KVPair* find(const Key& key) + { + for (unsigned i = 0; i < static_cast(NumKV); i++) + { + if (kvs[i].match(key)) + { + return kvs + i; + } + } + return UAVCAN_NULLPTR; + } + }; + + LinkedListRoot list_; + IPoolAllocator& allocator_; + + KVPair* findKey(const Key& key); + + void compact(); + + struct YesPredicate + { + bool operator()(const Key&, const Value&) const { return true; } + }; + +public: + Map(IPoolAllocator& allocator) : + allocator_(allocator) + { + UAVCAN_ASSERT(Key() == Key()); + } + + ~Map() + { + clear(); + } + + /** + * Returns null pointer if there's no such entry. + */ + Value* access(const Key& key); + + /** + * If entry with the same key already exists, it will be replaced + */ + Value* insert(const Key& key, const Value& value); + + /** + * Does nothing if there's no such entry. + */ + void remove(const Key& key); + + /** + * Removes entries where the predicate returns true. + * Predicate prototype: + * bool (Key& key, Value& value) + */ + template + void removeAllWhere(Predicate predicate); + + /** + * Returns first entry where the predicate returns true. + * Predicate prototype: + * bool (const Key& key, const Value& value) + */ + template + const Key* find(Predicate predicate) const; + + /** + * Removes all items. + */ + void clear(); + + /** + * Returns a key-value pair located at the specified position from the beginning. + * Note that any insertion or deletion may greatly disturb internal ordering, so use with care. + * If index is greater than or equal the number of pairs, null pointer will be returned. + */ + KVPair* getByIndex(unsigned index); + const KVPair* getByIndex(unsigned index) const; + + /** + * Complexity is O(1). + */ + bool isEmpty() const { return find(YesPredicate()) == UAVCAN_NULLPTR; } + + /** + * Complexity is O(N). + */ + unsigned getSize() const; +}; + +// ---------------------------------------------------------------------------- + +/* + * Map<> + */ +template +typename Map::KVPair* Map::findKey(const Key& key) +{ + KVGroup* p = list_.get(); + while (p) + { + KVPair* const kv = p->find(key); + if (kv) + { + return kv; + } + p = p->getNextListNode(); + } + return UAVCAN_NULLPTR; +} + +template +void Map::compact() +{ + KVGroup* p = list_.get(); + while (p) + { + KVGroup* const next = p->getNextListNode(); + bool remove_this = true; + for (int i = 0; i < KVGroup::NumKV; i++) + { + if (!p->kvs[i].match(Key())) + { + remove_this = false; + break; + } + } + if (remove_this) + { + list_.remove(p); + KVGroup::destroy(p, allocator_); + } + p = next; + } +} + +template +Value* Map::access(const Key& key) +{ + UAVCAN_ASSERT(!(key == Key())); + KVPair* const kv = findKey(key); + return kv ? &kv->value : UAVCAN_NULLPTR; +} + +template +Value* Map::insert(const Key& key, const Value& value) +{ + UAVCAN_ASSERT(!(key == Key())); + remove(key); + + KVPair* const kv = findKey(Key()); + if (kv) + { + *kv = KVPair(key, value); + return &kv->value; + } + + KVGroup* const kvg = KVGroup::instantiate(allocator_); + if (kvg == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + list_.insert(kvg); + kvg->kvs[0] = KVPair(key, value); + return &kvg->kvs[0].value; +} + +template +void Map::remove(const Key& key) +{ + UAVCAN_ASSERT(!(key == Key())); + KVPair* const kv = findKey(key); + if (kv) + { + *kv = KVPair(); + compact(); + } +} + +template +template +void Map::removeAllWhere(Predicate predicate) +{ + unsigned num_removed = 0; + + KVGroup* p = list_.get(); + while (p != UAVCAN_NULLPTR) + { + KVGroup* const next_group = p->getNextListNode(); + + for (int i = 0; i < KVGroup::NumKV; i++) + { + const KVPair* const kv = p->kvs + i; + if (!kv->match(Key())) + { + if (predicate(kv->key, kv->value)) + { + num_removed++; + p->kvs[i] = KVPair(); + } + } + } + + p = next_group; + } + + if (num_removed > 0) + { + compact(); + } +} + +template +template +const Key* Map::find(Predicate predicate) const +{ + KVGroup* p = list_.get(); + while (p != UAVCAN_NULLPTR) + { + KVGroup* const next_group = p->getNextListNode(); + + for (int i = 0; i < KVGroup::NumKV; i++) + { + const KVPair* const kv = p->kvs + i; + if (!kv->match(Key())) + { + if (predicate(kv->key, kv->value)) + { + return &p->kvs[i].key; + } + } + } + + p = next_group; + } + return UAVCAN_NULLPTR; +} + +template +void Map::clear() +{ + removeAllWhere(YesPredicate()); +} + +template +typename Map::KVPair* Map::getByIndex(unsigned index) +{ + // Slowly crawling through the dynamic storage + KVGroup* p = list_.get(); + while (p != UAVCAN_NULLPTR) + { + KVGroup* const next_group = p->getNextListNode(); + + for (int i = 0; i < KVGroup::NumKV; i++) + { + KVPair* const kv = p->kvs + i; + if (!kv->match(Key())) + { + if (index == 0) + { + return kv; + } + index--; + } + } + + p = next_group; + } + + return UAVCAN_NULLPTR; +} + +template +const typename Map::KVPair* Map::getByIndex(unsigned index) const +{ + return const_cast*>(this)->getByIndex(index); +} + +template +unsigned Map::getSize() const +{ + unsigned num = 0; + KVGroup* p = list_.get(); + while (p) + { + for (int i = 0; i < KVGroup::NumKV; i++) + { + const KVPair* const kv = p->kvs + i; + if (!kv->match(Key())) + { + num++; + } + } + p = p->getNextListNode(); + } + return num; +} + +} + +#endif // UAVCAN_UTIL_MAP_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/method_binder.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/method_binder.hpp new file mode 100644 index 000000000000..f3bbf424b40d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/method_binder.hpp @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_METHOD_BINDER_HPP_INCLUDED +#define UAVCAN_UTIL_METHOD_BINDER_HPP_INCLUDED + +#include +#include +#include + +namespace uavcan +{ +/** + * Use this to call member functions as callbacks in C++03 mode. + * + * In C++11 or newer you don't need it because you can use std::function<>/std::bind<> instead. + */ +template +class UAVCAN_EXPORT MethodBinder +{ + ObjectPtr obj_; + MemFunPtr fun_; + + void validateBeforeCall() const + { + if (!operator bool()) + { + handleFatalError("Null binder"); + } + } + +public: + MethodBinder() + : obj_() + , fun_() + { } + + MethodBinder(ObjectPtr o, MemFunPtr f) + : obj_(o) + , fun_(f) + { } + + /** + * Returns true if the binder is initialized (doesn't contain null pointers). + */ + operator bool() const + { + return coerceOrFallback(obj_, true) && coerceOrFallback(fun_, true); + } + + /** + * Will raise a fatal error if either method pointer or object pointer are null. + */ + void operator()() + { + validateBeforeCall(); + (obj_->*fun_)(); + } + + /** + * Will raise a fatal error if either method pointer or object pointer are null. + */ + template + void operator()(Par1& p1) + { + validateBeforeCall(); + (obj_->*fun_)(p1); + } + + /** + * Will raise a fatal error if either method pointer or object pointer are null. + */ + template + void operator()(Par1& p1, Par2& p2) + { + validateBeforeCall(); + (obj_->*fun_)(p1, p2); + } +}; + +} + +#endif // UAVCAN_UTIL_METHOD_BINDER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/multiset.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/multiset.hpp new file mode 100644 index 000000000000..5813466c62f7 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/multiset.hpp @@ -0,0 +1,478 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_MULTISET_HPP_INCLUDED +#define UAVCAN_UTIL_MULTISET_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +namespace uavcan +{ +/** + * Slow but memory efficient unordered multiset. Unlike Map<>, this container does not move objects, so + * they don't have to be copyable. + * + * Items will be allocated in the node's memory pool. + */ +template +class UAVCAN_EXPORT Multiset : Noncopyable +{ + struct Item : ::uavcan::Noncopyable + { + T* ptr; + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + alignas(T) unsigned char pool[sizeof(T)]; ///< Memory efficient version +#else + union + { + unsigned char pool[sizeof(T)]; + /* + * Such alignment does not guarantee safety for all types (only for libuavcan internal ones); + * however, increasing it is too memory inefficient. So it is recommended to use C++11, where + * this issue is resolved with alignas() (see above). + */ + void* _aligner1_; + long long _aligner2_; + }; +#endif + + Item() + : ptr(UAVCAN_NULLPTR) + { + fill_n(pool, sizeof(pool), static_cast(0)); + } + + ~Item() { destroy(); } + + bool isConstructed() const { return ptr != UAVCAN_NULLPTR; } + + void destroy() + { + if (ptr != UAVCAN_NULLPTR) + { + ptr->~T(); + ptr = UAVCAN_NULLPTR; + fill_n(pool, sizeof(pool), static_cast(0)); + } + } + }; + +private: + struct Chunk : LinkedListNode + { + enum { NumItems = (MemPoolBlockSize - sizeof(LinkedListNode)) / sizeof(Item) }; + Item items[NumItems]; + + Chunk() + { + StaticAssert<(static_cast(NumItems) > 0)>::check(); + IsDynamicallyAllocatable::check(); + UAVCAN_ASSERT(!items[0].isConstructed()); + } + + static Chunk* instantiate(IPoolAllocator& allocator) + { + void* const praw = allocator.allocate(sizeof(Chunk)); + if (praw == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + return new (praw) Chunk(); + } + + static void destroy(Chunk*& obj, IPoolAllocator& allocator) + { + if (obj != UAVCAN_NULLPTR) + { + obj->~Chunk(); + allocator.deallocate(obj); + obj = UAVCAN_NULLPTR; + } + } + + Item* findFreeSlot() + { + for (unsigned i = 0; i < static_cast(NumItems); i++) + { + if (!items[i].isConstructed()) + { + return items + i; + } + } + return UAVCAN_NULLPTR; + } + }; + + /* + * Data + */ + LinkedListRoot list_; + IPoolAllocator& allocator_; + + /* + * Methods + */ + Item* findOrCreateFreeSlot(); + + void compact(); + + enum RemoveStrategy { RemoveOne, RemoveAll }; + + template + void removeWhere(Predicate predicate, RemoveStrategy strategy); + + struct YesPredicate + { + bool operator()(const T&) const { return true; } + }; + + struct IndexPredicate : ::uavcan::Noncopyable + { + unsigned index; + IndexPredicate(unsigned target_index) + : index(target_index) + { } + + bool operator()(const T&) + { + return index-- == 0; + } + }; + + struct ComparingPredicate + { + const T& reference; + + ComparingPredicate(const T& ref) + : reference(ref) + { } + + bool operator()(const T& sample) + { + return reference == sample; + } + }; + + template + struct OperatorToFalsePredicateAdapter : ::uavcan::Noncopyable + { + Operator oper; + + OperatorToFalsePredicateAdapter(Operator o) + : oper(o) + { } + + bool operator()(T& item) + { + oper(item); + return false; + } + + bool operator()(const T& item) const + { + oper(item); + return false; + } + }; + +public: + Multiset(IPoolAllocator& allocator) + : allocator_(allocator) + { } + + ~Multiset() + { + clear(); + } + + /** + * Creates one item in-place and returns a pointer to it. + * If creation fails due to lack of memory, UAVCAN_NULLPTR will be returned. + * Complexity is O(N). + */ + T* emplace() + { + Item* const item = findOrCreateFreeSlot(); + if (item == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + UAVCAN_ASSERT(item->ptr == UAVCAN_NULLPTR); + item->ptr = new (item->pool) T(); + return item->ptr; + } + + template + T* emplace(P1 p1) + { + Item* const item = findOrCreateFreeSlot(); + if (item == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + UAVCAN_ASSERT(item->ptr == UAVCAN_NULLPTR); + item->ptr = new (item->pool) T(p1); + return item->ptr; + } + + template + T* emplace(P1 p1, P2 p2) + { + Item* const item = findOrCreateFreeSlot(); + if (item == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + UAVCAN_ASSERT(item->ptr == UAVCAN_NULLPTR); + item->ptr = new (item->pool) T(p1, p2); + return item->ptr; + } + + template + T* emplace(P1 p1, P2 p2, P3 p3) + { + Item* const item = findOrCreateFreeSlot(); + if (item == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + UAVCAN_ASSERT(item->ptr == UAVCAN_NULLPTR); + item->ptr = new (item->pool) T(p1, p2, p3); + return item->ptr; + } + + /** + * Removes entries where the predicate returns true. + * Predicate prototype: + * bool (T& item) + */ + template + void removeAllWhere(Predicate predicate) { removeWhere(predicate, RemoveAll); } + + template + void removeFirstWhere(Predicate predicate) { removeWhere(predicate, RemoveOne); } + + void removeFirst(const T& ref) { removeFirstWhere(ComparingPredicate(ref)); } + + void removeAll(const T& ref) { removeAllWhere(ComparingPredicate(ref)); } + + void clear() { removeAllWhere(YesPredicate()); } + + /** + * Returns first entry where the predicate returns true. + * Predicate prototype: + * bool (const T& item) + */ + template + T* find(Predicate predicate); + + template + const T* find(Predicate predicate) const + { + return const_cast(this)->find(predicate); + } + + /** + * Calls Operator for each item of the set. + * Operator prototype: + * void (T& item) + * void (const T& item) - const overload + */ + template + void forEach(Operator oper) + { + OperatorToFalsePredicateAdapter adapter(oper); + (void)find&>(adapter); + } + + template + void forEach(Operator oper) const + { + const OperatorToFalsePredicateAdapter adapter(oper); + (void)find&>(adapter); + } + + /** + * Returns an item located at the specified position from the beginning. + * Note that addition and removal operations invalidate indices. + * If index is greater than or equal the number of items, null pointer will be returned. + * Complexity is O(N). + */ + T* getByIndex(unsigned index) + { + IndexPredicate predicate(index); + return find(predicate); + } + + const T* getByIndex(unsigned index) const + { + return const_cast(this)->getByIndex(index); + } + + /** + * Complexity is O(1). + */ + bool isEmpty() const { return find(YesPredicate()) == UAVCAN_NULLPTR; } + + /** + * Counts number of items stored. + * Best case complexity is O(N). + */ + unsigned getSize() const; +}; + +// ---------------------------------------------------------------------------- + +/* + * Multiset<> + */ +template +typename Multiset::Item* Multiset::findOrCreateFreeSlot() +{ + // Search + { + Chunk* p = list_.get(); + while (p) + { + Item* const dyn = p->findFreeSlot(); + if (dyn != UAVCAN_NULLPTR) + { + return dyn; + } + p = p->getNextListNode(); + } + } + + // Create new chunk + Chunk* const chunk = Chunk::instantiate(allocator_); + if (chunk == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + list_.insert(chunk); + return &chunk->items[0]; +} + +template +void Multiset::compact() +{ + Chunk* p = list_.get(); + while (p) + { + Chunk* const next = p->getNextListNode(); + bool remove_this = true; + for (int i = 0; i < Chunk::NumItems; i++) + { + if (p->items[i].isConstructed()) + { + remove_this = false; + break; + } + } + if (remove_this) + { + list_.remove(p); + Chunk::destroy(p, allocator_); + } + p = next; + } +} + +template +template +void Multiset::removeWhere(Predicate predicate, const RemoveStrategy strategy) +{ + unsigned num_removed = 0; + + Chunk* p = list_.get(); + while (p != UAVCAN_NULLPTR) + { + Chunk* const next_chunk = p->getNextListNode(); // For the case if the current entry gets modified + + if ((num_removed > 0) && (strategy == RemoveOne)) + { + break; + } + + for (int i = 0; i < Chunk::NumItems; i++) + { + Item& item = p->items[i]; + if (item.isConstructed()) + { + if (predicate(*item.ptr)) + { + num_removed++; + item.destroy(); + if (strategy == RemoveOne) + { + break; + } + } + } + } + + p = next_chunk; + } + + if (num_removed > 0) + { + compact(); + } +} + +template +template +T* Multiset::find(Predicate predicate) +{ + Chunk* p = list_.get(); + while (p != UAVCAN_NULLPTR) + { + Chunk* const next_chunk = p->getNextListNode(); // For the case if the current entry gets modified + + for (int i = 0; i < Chunk::NumItems; i++) + { + if (p->items[i].isConstructed()) + { + if (predicate(*p->items[i].ptr)) + { + return p->items[i].ptr; + } + } + } + + p = next_chunk; + } + return UAVCAN_NULLPTR; +} + +template +unsigned Multiset::getSize() const +{ + unsigned num = 0; + Chunk* p = list_.get(); + while (p) + { + for (int i = 0; i < Chunk::NumItems; i++) + { + num += p->items[i].isConstructed() ? 1U : 0U; + } + p = p->getNextListNode(); + } + return num; +} + +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/placement_new.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/placement_new.hpp new file mode 100644 index 000000000000..8c133081113b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/placement_new.hpp @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_PLACEMENT_NEW_HPP_INCLUDED +#define UAVCAN_UTIL_PLACEMENT_NEW_HPP_INCLUDED + +#include +#include + +/* + * Some embedded C++ implementations don't implement the placement new operator. + * Define UAVCAN_IMPLEMENT_PLACEMENT_NEW to apply this workaround. + */ + +#ifndef UAVCAN_IMPLEMENT_PLACEMENT_NEW +# error UAVCAN_IMPLEMENT_PLACEMENT_NEW +#endif + +#if UAVCAN_IMPLEMENT_PLACEMENT_NEW + +inline void* operator new (std::size_t, void* ptr) throw() +{ + return ptr; +} +inline void* operator new[](std::size_t, void* ptr) throw() +{ + return ptr; +} + +inline void operator delete (void*, void*) throw() { } +inline void operator delete[](void*, void*) throw() { } + +#else +# include +#endif + +#endif // UAVCAN_UTIL_PLACEMENT_NEW_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/templates.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/templates.hpp new file mode 100644 index 000000000000..00abb7c6785f --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/templates.hpp @@ -0,0 +1,557 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_TEMPLATES_HPP_INCLUDED +#define UAVCAN_UTIL_TEMPLATES_HPP_INCLUDED + +#include +#include +#include +#include + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif +#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 +# include // cfloat may not be available +#else +# include // C++11 mode assumes that all standard headers are available +#endif + +namespace uavcan +{ +/** + * Usage: + * StaticAssert::check(); + */ +template +struct UAVCAN_EXPORT StaticAssert; + +template <> +struct UAVCAN_EXPORT StaticAssert +{ + static void check() { } +}; + +/** + * Usage: + * ShowIntegerAsError::foobar(); + */ +template struct ShowIntegerAsError; + +/** + * Prevents copying when inherited + */ +class UAVCAN_EXPORT Noncopyable +{ + Noncopyable(const Noncopyable&); + Noncopyable& operator=(const Noncopyable&); +protected: + Noncopyable() { } + ~Noncopyable() { } +}; + +/** + * Compile time conditions + */ +template +struct UAVCAN_EXPORT EnableIf { }; + +template +struct UAVCAN_EXPORT EnableIf +{ + typedef T Type; +}; + +/** + * Lightweight type categorization. + */ +template +struct UAVCAN_EXPORT EnableIfType +{ + typedef R Type; +}; + +/** + * Compile-time type selection (Alexandrescu) + */ +template +struct UAVCAN_EXPORT Select; + +template +struct UAVCAN_EXPORT Select +{ + typedef TrueType Result; +}; + +template +struct UAVCAN_EXPORT Select +{ + typedef FalseType Result; +}; + +/** + * Checks if two identifiers refer to the same type. + */ +template +struct IsSameType +{ + enum { Result = 0 }; +}; + +template +struct IsSameType +{ + enum { Result = 1 }; +}; + +/** + * Remove reference as in + */ +template struct RemoveReference { typedef T Type; }; +template struct RemoveReference { typedef T Type; }; +#if UAVCAN_CPP_VERSION > UAVCAN_CPP03 +template struct RemoveReference { typedef T Type; }; +#endif + +/** + * Parameter types + */ +template struct ParameterType { typedef const U& Type; }; +template struct ParameterType { typedef U& Type; }; +#if UAVCAN_CPP_VERSION > UAVCAN_CPP03 +template struct ParameterType { typedef U&& Type; }; +#endif + +/** + * Value types + */ +template struct UAVCAN_EXPORT BooleanType { }; +typedef BooleanType TrueType; +typedef BooleanType FalseType; + +template struct IntToType { }; + +/** + * Relations + */ +template +class UAVCAN_EXPORT IsImplicitlyConvertibleFromTo +{ + template static U returner(); + + struct True_ { char x[2]; }; + struct False_ { }; + + static True_ test(const T2 &); + static False_ test(...); + +public: + enum { Result = sizeof(True_) == sizeof(IsImplicitlyConvertibleFromTo::test(returner())) }; +}; + +/** + * coerceOrFallback(From) + * coerceOrFallback(From, To) + * @{ + */ +template +struct UAVCAN_EXPORT CoerceOrFallbackImpl +{ + static To impl(const From& from, const To&, TrueType) { return To(from); } + static To impl(const From&, const To& default_, FalseType) { return default_; } +}; + +/** + * If possible, performs an implicit cast from the type From to the type To. + * If the cast is not possible, returns default_ of type To. + */ +template +UAVCAN_EXPORT +To coerceOrFallback(const From& from, const To& default_) +{ + return CoerceOrFallbackImpl::impl(from, default_, + BooleanType::Result>()); +} + +/** + * If possible, performs an implicit cast from the type From to the type To. + * If the cast is not possible, returns a default constructed object of the type To. + */ +template +UAVCAN_EXPORT +To coerceOrFallback(const From& from) +{ + return CoerceOrFallbackImpl::impl(from, To(), + BooleanType::Result>()); +} +/** + * @} + */ + +/** + * Selects smaller value + */ +template +struct EnumMin +{ + enum { Result = (A < B) ? A : B }; +}; + +/** + * Selects larger value + */ +template +struct EnumMax +{ + enum { Result = (A > B) ? A : B }; +}; + +/** + * Compile time square root for integers. + * Useful for operations on square matrices. + */ +template struct UAVCAN_EXPORT CompileTimeIntSqrt; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<1> { enum { Result = 1 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<4> { enum { Result = 2 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<9> { enum { Result = 3 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<16> { enum { Result = 4 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<25> { enum { Result = 5 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<36> { enum { Result = 6 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<49> { enum { Result = 7 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<64> { enum { Result = 8 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<81> { enum { Result = 9 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<100> { enum { Result = 10 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<121> { enum { Result = 11 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<144> { enum { Result = 12 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<169> { enum { Result = 13 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<196> { enum { Result = 14 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<225> { enum { Result = 15 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<256> { enum { Result = 16 }; }; + +/** + * Replacement for std::copy(..) + */ +template +UAVCAN_EXPORT +OutputIt copy(InputIt first, InputIt last, OutputIt result) +{ + while (first != last) + { + *result = *first; + ++first; + ++result; + } + return result; +} + +/** + * Replacement for std::fill(..) + */ +template +UAVCAN_EXPORT +void fill(ForwardIt first, ForwardIt last, const T& value) +{ + while (first != last) + { + *first = value; + ++first; + } +} + +/** + * Replacement for std::fill_n(..) + */ +template +UAVCAN_EXPORT +void fill_n(OutputIt first, std::size_t n, const T& value) +{ + while (n--) + { + *first++ = value; + } +} + +/** + * Replacement for std::min(..) + */ +template +UAVCAN_EXPORT +const T& min(const T& a, const T& b) +{ + return (b < a) ? b : a; +} + +/** + * Replacement for std::max(..) + */ +template +UAVCAN_EXPORT +const T& max(const T& a, const T& b) +{ + return (a < b) ? b : a; +} + +/** + * Replacement for std::lexicographical_compare(..) + */ +template +UAVCAN_EXPORT +bool lexicographical_compare(InputIt1 first1, InputIt1 last1, InputIt2 first2, InputIt2 last2) +{ + while ((first1 != last1) && (first2 != last2)) + { + if (*first1 < *first2) + { + return true; + } + if (*first2 < *first1) + { + return false; + } + ++first1; + ++first2; + } + return (first1 == last1) && (first2 != last2); +} + +/** + * Replacement for std::equal(..) + */ +template +UAVCAN_EXPORT +bool equal(InputIt1 first1, InputIt1 last1, InputIt2 first2) +{ + while (first1 != last1) + { + if (*first1 != *first2) + { + return false; + } + ++first1; + ++first2; + } + return true; +} + +/** + * Numeric traits, like std::numeric_limits<> + */ +template +struct UAVCAN_EXPORT NumericTraits; + +/// bool +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 0 }; + enum { IsInteger = 1 }; + static bool max() { return true; } + static bool min() { return false; } +}; + +/// char +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 1 }; + static char max() { return CHAR_MAX; } + static char min() { return CHAR_MIN; } +}; +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 1 }; + static signed char max() { return SCHAR_MAX; } + static signed char min() { return SCHAR_MIN; } +}; +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 0 }; + enum { IsInteger = 1 }; + static unsigned char max() { return UCHAR_MAX; } + static unsigned char min() { return 0; } +}; + +/// short +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 1 }; + static short max() { return SHRT_MAX; } + static short min() { return SHRT_MIN; } +}; +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 0 }; + enum { IsInteger = 1 }; + static unsigned short max() { return USHRT_MAX; } + static unsigned short min() { return 0; } +}; + +/// int +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 1 }; + static int max() { return INT_MAX; } + static int min() { return INT_MIN; } +}; +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 0 }; + enum { IsInteger = 1 }; + static unsigned int max() { return UINT_MAX; } + static unsigned int min() { return 0; } +}; + +/// long +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 1 }; + static long max() { return LONG_MAX; } + static long min() { return LONG_MIN; } +}; +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 0 }; + enum { IsInteger = 1 }; + static unsigned long max() { return ULONG_MAX; } + static unsigned long min() { return 0; } +}; + +/// long long +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 1 }; + static long long max() { return LLONG_MAX; } + static long long min() { return LLONG_MIN; } +}; +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 0 }; + enum { IsInteger = 1 }; + static unsigned long long max() { return ULLONG_MAX; } + static unsigned long long min() { return 0; } +}; + +/// float +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 0 }; + static float max() { return FLT_MAX; } + static float min() { return FLT_MIN; } + static float infinity() { return INFINITY; } + static float epsilon() { return FLT_EPSILON; } +}; + +/// double +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 0 }; + static double max() { return DBL_MAX; } + static double min() { return DBL_MIN; } + static double infinity() { return static_cast(INFINITY) * static_cast(INFINITY); } + static double epsilon() { return DBL_EPSILON; } +}; + +#if defined(LDBL_MAX) && defined(LDBL_MIN) && defined(LDBL_EPSILON) +/// long double +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 0 }; + static long double max() { return LDBL_MAX; } + static long double min() { return LDBL_MIN; } + static long double infinity() { return static_cast(INFINITY) * static_cast(INFINITY); } + static long double epsilon() { return LDBL_EPSILON; } +}; +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# undef isnan +# undef isinf +# undef signbit +#endif + +/** + * Replacement for std::isnan(). + * Note that direct float comparison (==, !=) is intentionally avoided. + */ +template +inline bool isNaN(T arg) +{ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + return std::isnan(arg); +#else + // coverity[same_on_both_sides : FALSE] + // cppcheck-suppress duplicateExpression + return !(arg <= arg); +#endif +} + +/** + * Replacement for std::isinf(). + * Note that direct float comparison (==, !=) is intentionally avoided. + */ +template +inline bool isInfinity(T arg) +{ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + return std::isinf(arg); +#else + return (arg >= NumericTraits::infinity()) || (arg <= -NumericTraits::infinity()); +#endif +} + +/** + * Replacement for std::isfinite(). + * Note that direct float comparison (==, !=) is intentionally avoided. + */ +template +inline bool isFinite(T arg) +{ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + return std::isfinite(arg); +#else + return !isNaN(arg) && !isInfinity(arg); +#endif +} + +/** + * Replacement for std::signbit(). + * Note that direct float comparison (==, !=) is intentionally avoided. + */ +template +inline bool getSignBit(T arg) +{ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + return std::signbit(arg); +#else + // coverity[divide_by_zero : FALSE] + return arg < T(0) || (((arg <= T(0)) && (arg >= T(0))) && (T(1) / arg < T(0))); +#endif +} + +} + +#endif // UAVCAN_UTIL_TEMPLATES_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/driver/uc_can.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/driver/uc_can.cpp new file mode 100644 index 000000000000..739740343bd3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/driver/uc_can.cpp @@ -0,0 +1,118 @@ +/* + * CAN bus driver interface. + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +namespace uavcan +{ + +const uint32_t CanFrame::MaskStdID; +const uint32_t CanFrame::MaskExtID; +const uint32_t CanFrame::FlagEFF; +const uint32_t CanFrame::FlagRTR; +const uint32_t CanFrame::FlagERR; +const uint8_t CanFrame::MaxDataLen; + +bool CanFrame::priorityHigherThan(const CanFrame& rhs) const +{ + const uint32_t clean_id = id & MaskExtID; + const uint32_t rhs_clean_id = rhs.id & MaskExtID; + + /* + * STD vs EXT - if 11 most significant bits are the same, EXT loses. + */ + const bool ext = id & FlagEFF; + const bool rhs_ext = rhs.id & FlagEFF; + if (ext != rhs_ext) + { + const uint32_t arb11 = ext ? (clean_id >> 18) : clean_id; + const uint32_t rhs_arb11 = rhs_ext ? (rhs_clean_id >> 18) : rhs_clean_id; + if (arb11 != rhs_arb11) + { + return arb11 < rhs_arb11; + } + else + { + return rhs_ext; + } + } + + /* + * RTR vs Data frame - if frame identifiers and frame types are the same, RTR loses. + */ + const bool rtr = id & FlagRTR; + const bool rhs_rtr = rhs.id & FlagRTR; + if (clean_id == rhs_clean_id && rtr != rhs_rtr) + { + return rhs_rtr; + } + + /* + * Plain ID arbitration - greater value loses. + */ + return clean_id < rhs_clean_id; +} + +#if UAVCAN_TOSTRING +std::string CanFrame::toString(StringRepresentation mode) const +{ + UAVCAN_ASSERT(mode == StrTight || mode == StrAligned); + + static const unsigned AsciiColumnOffset = 36U; + + char buf[50]; + char* wpos = buf; + char* const epos = buf + sizeof(buf); + fill(buf, buf + sizeof(buf), '\0'); + + if (id & FlagEFF) + { + wpos += snprintf(wpos, unsigned(epos - wpos), "0x%08x ", unsigned(id & MaskExtID)); + } + else + { + const char* const fmt = (mode == StrAligned) ? " 0x%03x " : "0x%03x "; + wpos += snprintf(wpos, unsigned(epos - wpos), fmt, unsigned(id & MaskStdID)); + } + + if (id & FlagRTR) + { + wpos += snprintf(wpos, unsigned(epos - wpos), " RTR"); + } + else if (id & FlagERR) + { + wpos += snprintf(wpos, unsigned(epos - wpos), " ERR"); + } + else + { + for (int dlen = 0; dlen < dlc; dlen++) // hex bytes + { + wpos += snprintf(wpos, unsigned(epos - wpos), " %02x", unsigned(data[dlen])); + } + + while ((mode == StrAligned) && (wpos < buf + AsciiColumnOffset)) // alignment + { + *wpos++ = ' '; + } + + wpos += snprintf(wpos, unsigned(epos - wpos), " \'"); // ascii + for (int dlen = 0; dlen < dlc; dlen++) + { + uint8_t ch = data[dlen]; + if (ch < 0x20 || ch > 0x7E) + { + ch = '.'; + } + wpos += snprintf(wpos, unsigned(epos - wpos), "%c", ch); + } + wpos += snprintf(wpos, unsigned(epos - wpos), "\'"); + } + (void)wpos; + return std::string(buf); +} +#endif + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_bit_array_copy.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_bit_array_copy.cpp new file mode 100644 index 000000000000..5657655a453f --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_bit_array_copy.cpp @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include + +namespace uavcan +{ +void bitarrayCopy(const unsigned char* src, std::size_t src_offset, std::size_t src_len, + unsigned char* dst, std::size_t dst_offset) +{ + /* + * Should never be called on a zero-length buffer. The caller will also ensure that the bit + * offsets never exceed one byte. + */ + + UAVCAN_ASSERT(src_len > 0U); + UAVCAN_ASSERT(src_offset < 8U && dst_offset < 8U); + + const std::size_t last_bit = src_offset + src_len; + while (last_bit - src_offset) + { + const uint8_t src_bit_offset = src_offset % 8U; + const uint8_t dst_bit_offset = dst_offset % 8U; + + // The number of bits to copy + const uint8_t max_offset = uavcan::max(src_bit_offset, dst_bit_offset); + const std::size_t copy_bits = uavcan::min(last_bit - src_offset, std::size_t(8U - max_offset)); + + /* + * The mask indicating which bits of dest to update: + * dst_byte_offset copy_bits write_mask + * 0 8 11111111 + * 0 7 11111110 + * ... + * 0 1 10000000 + * ... + * 4 4 00001111 + * 4 3 00001110 + * 4 2 00001100 + * 4 1 00001000 + * ... + * 7 1 00000001 + */ + const uint8_t write_mask = uint8_t(uint8_t(0xFF00U >> copy_bits) >> dst_bit_offset); + + // The value to be extracted from src, shifted into the dst location + const uint8_t src_data = uint8_t((src[src_offset / 8U] << src_bit_offset) >> dst_bit_offset); + + dst[dst_offset / 8U] = uint8_t((dst[dst_offset / 8U] & ~write_mask) | (src_data & write_mask)); + + src_offset += copy_bits; + dst_offset += copy_bits; + } +} +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_bit_stream.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_bit_stream.cpp new file mode 100644 index 000000000000..fba0153fb2e2 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_bit_stream.cpp @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ + +const unsigned BitStream::MaxBytesPerRW; +const unsigned BitStream::MaxBitsPerRW; + +int BitStream::write(const uint8_t* bytes, const unsigned bitlen) +{ + // Temporary buffer is needed to merge new bits with cached unaligned bits from the last write() (see byte_cache_) + uint8_t tmp[MaxBytesPerRW + 1]; + + // Tmp space must be large enough to accomodate new bits AND unaligned bits from the last write() + const unsigned bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8)); + UAVCAN_ASSERT(MaxBytesPerRW >= bytelen); + tmp[0] = tmp[bytelen - 1] = 0; + + fill(tmp, tmp + bytelen, uint8_t(0)); + copyBitArrayAlignedToUnaligned(bytes, bitlen, tmp, bit_offset_ % 8); + + const unsigned new_bit_offset = bit_offset_ + bitlen; + + // Bitcopy algorithm resets skipped bits in the first byte. Restore them back. + tmp[0] |= byte_cache_; + + // (new_bit_offset % 8 == 0) means that this write was perfectly aligned. + byte_cache_ = uint8_t((new_bit_offset % 8) ? tmp[bytelen - 1] : 0); + + /* + * Dump the data into the destination buffer. + * Note that if this write was unaligned, last written byte in the buffer will be rewritten with updated value + * within the next write() operation. + */ + const int write_res = buf_.write(bit_offset_ / 8, tmp, bytelen); + if (write_res < 0) + { + return write_res; + } + if (static_cast(write_res) < bytelen) + { + return ResultOutOfBuffer; + } + + bit_offset_ = new_bit_offset; + return ResultOk; +} + +int BitStream::read(uint8_t* bytes, const unsigned bitlen) +{ + uint8_t tmp[MaxBytesPerRW + 1]; + + const unsigned bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8)); + UAVCAN_ASSERT(MaxBytesPerRW >= bytelen); + + const int read_res = buf_.read(bit_offset_ / 8, tmp, bytelen); + if (read_res < 0) + { + return read_res; + } + if (static_cast(read_res) < bytelen) + { + return ResultOutOfBuffer; + } + + fill(bytes, bytes + bitlenToBytelen(bitlen), uint8_t(0)); + copyBitArrayUnalignedToAligned(tmp, bit_offset_ % 8, bitlen, bytes); + bit_offset_ += bitlen; + return ResultOk; +} + +#if UAVCAN_TOSTRING +std::string BitStream::toString() const +{ + std::string out; + out.reserve(128); + + for (unsigned offset = 0; true; offset++) + { + uint8_t byte = 0; + if (1 != buf_.read(offset, &byte, 1U)) + { + break; + } + for (int i = 7; i >= 0; i--) // Most significant goes first + { + out += (byte & (1 << i)) ? '1' : '0'; + } + out += ' '; + } + if (out.length() > 0) + { + (void)out.erase(out.length() - 1, 1); + } + return out; +} +#endif + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_float_spec.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_float_spec.cpp new file mode 100644 index 000000000000..267e0a6bc994 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_float_spec.cpp @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ + +#if !UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION + +union Fp32 +{ + uint32_t u; + float f; +}; + +/* + * IEEE754Converter + */ +uint16_t IEEE754Converter::nativeIeeeToHalf(float value) +{ + /* + * https://gist.github.com/rygorous/2156668 + * Public domain, by Fabian "ryg" Giesen + */ + const Fp32 f32infty = { 255U << 23 }; + const Fp32 f16infty = { 31U << 23 }; + const Fp32 magic = { 15U << 23 }; + const uint32_t sign_mask = 0x80000000U; + const uint32_t round_mask = ~0xFFFU; + + Fp32 in; + uint16_t out; + + in.f = value; + + uint32_t sign = in.u & sign_mask; + in.u ^= sign; + + if (in.u >= f32infty.u) /* Inf or NaN (all exponent bits set) */ + { + /* NaN->sNaN and Inf->Inf */ + out = (in.u > f32infty.u) ? 0x7FFFU : 0x7C00U; + } + else /* (De)normalized number or zero */ + { + in.u &= round_mask; + in.f *= magic.f; + in.u -= round_mask; + if (in.u > f16infty.u) + { + in.u = f16infty.u; /* Clamp to signed infinity if overflowed */ + } + + out = uint16_t(in.u >> 13); /* Take the bits! */ + } + + out = uint16_t(out | (sign >> 16)); + + return out; +} + +float IEEE754Converter::halfToNativeIeee(uint16_t value) +{ + /* + * https://gist.github.com/rygorous/2144712 + * Public domain, by Fabian "ryg" Giesen + */ + const Fp32 magic = { (254U - 15U) << 23 }; + const Fp32 was_infnan = { (127U + 16U) << 23 }; + Fp32 out; + + out.u = (value & 0x7FFFU) << 13; /* exponent/mantissa bits */ + out.f *= magic.f; /* exponent adjust */ + if (out.f >= was_infnan.f) /* make sure Inf/NaN survive */ + { + out.u |= 255U << 23; + } + out.u |= (value & 0x8000U) << 16; /* sign bit */ + + return out.f; +} + +#endif // !UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_scalar_codec.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_scalar_codec.cpp new file mode 100644 index 000000000000..1a7485698ce7 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_scalar_codec.cpp @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ + +void ScalarCodec::swapByteOrder(uint8_t* const bytes, const unsigned len) +{ + UAVCAN_ASSERT(bytes); + for (unsigned i = 0, j = len - 1; i < j; i++, j--) + { + const uint8_t c = bytes[i]; + bytes[i] = bytes[j]; + bytes[j] = c; + } +} + +int ScalarCodec::encodeBytesImpl(uint8_t* const bytes, const unsigned bitlen) +{ + UAVCAN_ASSERT(bytes); + // Underlying stream class assumes that more significant bits have lower index, so we need to shift some. + if (bitlen % 8) + { + bytes[bitlen / 8] = uint8_t(bytes[bitlen / 8] << ((8 - (bitlen % 8)) & 7)); + } + return stream_.write(bytes, bitlen); +} + +int ScalarCodec::decodeBytesImpl(uint8_t* const bytes, const unsigned bitlen) +{ + UAVCAN_ASSERT(bytes); + const int read_res = stream_.read(bytes, bitlen); + if (read_res > 0) + { + if (bitlen % 8) + { + bytes[bitlen / 8] = uint8_t(bytes[bitlen / 8] >> ((8 - (bitlen % 8)) & 7)); // As in encode(), vice versa + } + } + return read_res; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_generic_publisher.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_generic_publisher.cpp new file mode 100644 index 000000000000..2c42cfb0ffd9 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_generic_publisher.cpp @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ + +bool GenericPublisherBase::isInited() const +{ + return sender_.isInitialized(); +} + +int GenericPublisherBase::doInit(DataTypeKind dtkind, const char* dtname, CanTxQueue::Qos qos) +{ + if (isInited()) + { + return 0; + } + + GlobalDataTypeRegistry::instance().freeze(); + + const DataTypeDescriptor* const descr = GlobalDataTypeRegistry::instance().find(dtkind, dtname); + if (descr == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("GenericPublisher", "Type [%s] is not registered", dtname); + return -ErrUnknownDataType; + } + + sender_.init(*descr, qos); + + return 0; +} + +MonotonicTime GenericPublisherBase::getTxDeadline() const +{ + return node_.getMonotonicTime() + tx_timeout_; +} + +int GenericPublisherBase::genericPublish(const StaticTransferBufferImpl& buffer, TransferType transfer_type, + NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline) +{ + if (tid) + { + return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(), + blocking_deadline, transfer_type, dst_node_id, *tid); + } + else + { + return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(), + blocking_deadline, transfer_type, dst_node_id); + } +} + +void GenericPublisherBase::setTxTimeout(MonotonicDuration tx_timeout) +{ + tx_timeout = max(tx_timeout, getMinTxTimeout()); + tx_timeout = min(tx_timeout, getMaxTxTimeout()); + tx_timeout_ = tx_timeout; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_generic_subscriber.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_generic_subscriber.cpp new file mode 100644 index 000000000000..6f5adb66ab10 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_generic_subscriber.cpp @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ + +int GenericSubscriberBase::genericStart(TransferListener* listener, + bool (Dispatcher::*registration_method)(TransferListener*)) +{ + if (listener == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + stop(listener); + if (!(node_.getDispatcher().*registration_method)(listener)) + { + UAVCAN_TRACE("GenericSubscriber", "Failed to register transfer listener"); + return -ErrInvalidTransferListener; + } + return 0; +} + +void GenericSubscriberBase::stop(TransferListener* listener) +{ + if (listener != UAVCAN_NULLPTR) + { + node_.getDispatcher().unregisterMessageListener(listener); + node_.getDispatcher().unregisterServiceRequestListener(listener); + node_.getDispatcher().unregisterServiceResponseListener(listener); + } +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_global_data_type_registry.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_global_data_type_registry.cpp new file mode 100644 index 000000000000..7586201e2bdf --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_global_data_type_registry.cpp @@ -0,0 +1,199 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include + +namespace uavcan +{ + +GlobalDataTypeRegistry::List* GlobalDataTypeRegistry::selectList(DataTypeKind kind) const +{ + if (kind == DataTypeKindMessage) + { + return &msgs_; + } + else if (kind == DataTypeKindService) + { + return &srvs_; + } + else + { + UAVCAN_ASSERT(0); + return UAVCAN_NULLPTR; + } +} + +GlobalDataTypeRegistry::RegistrationResult GlobalDataTypeRegistry::remove(Entry* dtd) +{ + if (!dtd) + { + UAVCAN_ASSERT(0); + return RegistrationResultInvalidParams; + } + if (isFrozen()) + { + return RegistrationResultFrozen; + } + + List* list = selectList(dtd->descriptor.getKind()); + if (!list) + { + return RegistrationResultInvalidParams; + } + + list->remove(dtd); // If this call came from regist<>(), that would be enough + Entry* p = list->get(); // But anyway + while (p) + { + Entry* const next = p->getNextListNode(); + if (p->descriptor.match(dtd->descriptor.getKind(), dtd->descriptor.getFullName())) + { + list->remove(p); + } + p = next; + } + return RegistrationResultOk; +} + +GlobalDataTypeRegistry::RegistrationResult GlobalDataTypeRegistry::registImpl(Entry* dtd) +{ + if (!dtd || !dtd->descriptor.isValid()) + { + UAVCAN_ASSERT(0); + return RegistrationResultInvalidParams; + } + if (isFrozen()) + { + return RegistrationResultFrozen; + } + + List* list = selectList(dtd->descriptor.getKind()); + if (!list) + { + return RegistrationResultInvalidParams; + } + + { // Collision check + Entry* p = list->get(); + while (p) + { + if (p->descriptor.getID() == dtd->descriptor.getID()) // ID collision + { + return RegistrationResultCollision; + } + if (!std::strncmp(p->descriptor.getFullName(), dtd->descriptor.getFullName(), + DataTypeDescriptor::MaxFullNameLen)) // Name collision + { + return RegistrationResultCollision; + } + p = p->getNextListNode(); + } + } +#if UAVCAN_DEBUG + const unsigned len_before = list->getLength(); +#endif + list->insertBefore(dtd, EntryInsertionComparator(dtd)); + +#if UAVCAN_DEBUG + { // List integrity check + const unsigned len_after = list->getLength(); + if ((len_before + 1) != len_after) + { + UAVCAN_ASSERT(0); + std::abort(); + } + } + { // Order check + Entry* p = list->get(); + int id = -1; + while (p) + { + if (id >= p->descriptor.getID().get()) + { + UAVCAN_ASSERT(0); + std::abort(); + } + id = p->descriptor.getID().get(); + p = p->getNextListNode(); + } + } +#endif + return RegistrationResultOk; +} + +GlobalDataTypeRegistry& GlobalDataTypeRegistry::instance() +{ + static GlobalDataTypeRegistry singleton; + return singleton; +} + +void GlobalDataTypeRegistry::freeze() +{ + if (!frozen_) + { + frozen_ = true; + UAVCAN_TRACE("GlobalDataTypeRegistry", "Frozen; num msgs: %u, num srvs: %u", + getNumMessageTypes(), getNumServiceTypes()); + } +} + +const DataTypeDescriptor* GlobalDataTypeRegistry::find(const char* name) const +{ + const DataTypeDescriptor* desc = find(DataTypeKindMessage, name); + if (desc == UAVCAN_NULLPTR) + { + desc = find(DataTypeKindService, name); + } + return desc; +} + +const DataTypeDescriptor* GlobalDataTypeRegistry::find(DataTypeKind kind, const char* name) const +{ + if (!name) + { + UAVCAN_ASSERT(0); + return UAVCAN_NULLPTR; + } + const List* list = selectList(kind); + if (!list) + { + UAVCAN_ASSERT(0); + return UAVCAN_NULLPTR; + } + Entry* p = list->get(); + while (p) + { + if (p->descriptor.match(kind, name)) + { + return &p->descriptor; + } + p = p->getNextListNode(); + } + return UAVCAN_NULLPTR; +} + +const DataTypeDescriptor* GlobalDataTypeRegistry::find(DataTypeKind kind, DataTypeID dtid) const +{ + const List* list = selectList(kind); + if (!list) + { + UAVCAN_ASSERT(0); + return UAVCAN_NULLPTR; + } + Entry* p = list->get(); + while (p) + { + if (p->descriptor.match(kind, dtid)) + { + return &p->descriptor; + } + p = p->getNextListNode(); + } + return UAVCAN_NULLPTR; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_scheduler.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_scheduler.cpp new file mode 100644 index 000000000000..529d94ccc6c3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_scheduler.cpp @@ -0,0 +1,208 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ +/* + * MonotonicDeadlineHandler + */ +void DeadlineHandler::startWithDeadline(MonotonicTime deadline) +{ + UAVCAN_ASSERT(!deadline.isZero()); + stop(); + deadline_ = deadline; + scheduler_.getDeadlineScheduler().add(this); +} + +void DeadlineHandler::startWithDelay(MonotonicDuration delay) +{ + startWithDeadline(scheduler_.getMonotonicTime() + delay); +} + +void DeadlineHandler::stop() +{ + scheduler_.getDeadlineScheduler().remove(this); +} + +bool DeadlineHandler::isRunning() const +{ + return scheduler_.getDeadlineScheduler().doesExist(this); +} + +/* + * MonotonicDeadlineScheduler + */ +struct MonotonicDeadlineHandlerInsertionComparator +{ + const MonotonicTime ts; + explicit MonotonicDeadlineHandlerInsertionComparator(MonotonicTime arg_ts) : ts(arg_ts) { } + bool operator()(const DeadlineHandler* t) const + { + return t->getDeadline() > ts; + } +}; + +void DeadlineScheduler::add(DeadlineHandler* mdh) +{ + UAVCAN_ASSERT(mdh); + handlers_.insertBefore(mdh, MonotonicDeadlineHandlerInsertionComparator(mdh->getDeadline())); +} + +void DeadlineScheduler::remove(DeadlineHandler* mdh) +{ + UAVCAN_ASSERT(mdh); + handlers_.remove(mdh); +} + +bool DeadlineScheduler::doesExist(const DeadlineHandler* mdh) const +{ + UAVCAN_ASSERT(mdh); + const DeadlineHandler* p = handlers_.get(); +#if UAVCAN_DEBUG + MonotonicTime prev_deadline; +#endif + while (p) + { +#if UAVCAN_DEBUG + if (prev_deadline > p->getDeadline()) // Self check + { + std::abort(); + } + prev_deadline = p->getDeadline(); +#endif + if (p == mdh) + { + return true; + } + p = p->getNextListNode(); + } + return false; +} + +MonotonicTime DeadlineScheduler::pollAndGetMonotonicTime(ISystemClock& sysclock) +{ + while (true) + { + DeadlineHandler* const mdh = handlers_.get(); + if (!mdh) + { + return sysclock.getMonotonic(); + } +#if UAVCAN_DEBUG + if (mdh->getNextListNode()) // Order check + { + UAVCAN_ASSERT(mdh->getDeadline() <= mdh->getNextListNode()->getDeadline()); + } +#endif + + const MonotonicTime ts = sysclock.getMonotonic(); + if (ts < mdh->getDeadline()) + { + return ts; + } + + handlers_.remove(mdh); + mdh->handleDeadline(ts); // This handler can be re-registered immediately + } + UAVCAN_ASSERT(0); + return MonotonicTime(); +} + +MonotonicTime DeadlineScheduler::getEarliestDeadline() const +{ + const DeadlineHandler* const mdh = handlers_.get(); + if (mdh) + { + return mdh->getDeadline(); + } + return MonotonicTime::getMax(); +} + +/* + * Scheduler + */ +MonotonicTime Scheduler::computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const +{ + const MonotonicTime earliest = min(deadline_scheduler_.getEarliestDeadline(), spin_deadline); + const MonotonicTime ts = getMonotonicTime(); + if (earliest > ts) + { + if (earliest - ts > deadline_resolution_) + { + return ts + deadline_resolution_; + } + } + return earliest; +} + +void Scheduler::pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin) +{ + // cleanup will be performed less frequently if the stack handles more frames per second + const MonotonicTime deadline = prev_cleanup_ts_ + cleanup_period_ * (num_frames_processed_with_last_spin + 1); + if (mono_ts > deadline) + { + //UAVCAN_TRACE("Scheduler", "Cleanup with %u processed frames", num_frames_processed_with_last_spin); + prev_cleanup_ts_ = mono_ts; + dispatcher_.cleanup(mono_ts); + } +} + +int Scheduler::spin(MonotonicTime deadline) +{ + if (inside_spin_) // Preventing recursive calls + { + UAVCAN_ASSERT(0); + return -ErrRecursiveCall; + } + InsideSpinSetter iss(*this); + UAVCAN_ASSERT(inside_spin_); + + int retval = 0; + while (true) + { + const MonotonicTime dl = computeDispatcherSpinDeadline(deadline); + retval = dispatcher_.spin(dl); + if (retval < 0) + { + break; + } + + const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTime(getSystemClock()); + pollCleanup(ts, unsigned(retval)); + if (ts >= deadline) + { + break; + } + } + + return retval; +} + +int Scheduler::spinOnce() +{ + if (inside_spin_) // Preventing recursive calls + { + UAVCAN_ASSERT(0); + return -ErrRecursiveCall; + } + InsideSpinSetter iss(*this); + UAVCAN_ASSERT(inside_spin_); + + const int retval = dispatcher_.spinOnce(); + if (retval < 0) + { + return retval; + } + + const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTime(getSystemClock()); + pollCleanup(ts, unsigned(retval)); + + return retval; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_service_client.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_service_client.cpp new file mode 100644 index 000000000000..cfe14555b048 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_service_client.cpp @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ +/* + * ServiceClientBase::CallState + */ +void ServiceClientBase::CallState::handleDeadline(MonotonicTime) +{ + UAVCAN_TRACE("ServiceClient::CallState", "Timeout from nid=%d, tid=%d, dtname=%s", + int(id_.server_node_id.get()), int(id_.transfer_id.get()), + (owner_.data_type_descriptor_ == UAVCAN_NULLPTR) ? "???" : owner_.data_type_descriptor_->getFullName()); + /* + * What we're doing here is relaying execution from this call stack to a different one. + * We need it because call registry cannot release memory from this callback, because this will destroy the + * object method of which we're executing now. + */ + UAVCAN_ASSERT(timed_out_ == false); + timed_out_ = true; + owner_.generateDeadlineImmediately(); + UAVCAN_TRACE("ServiceClient::CallState", "Relaying execution to the owner's handler via timer callback"); +} + +/* + * ServiceClientBase + */ +int ServiceClientBase::prepareToCall(INode& node, + const char* dtname, + NodeID server_node_id, + ServiceCallID& out_call_id) +{ + /* + * Making sure we're not going to get transport error because of invalid input data + */ + if (!server_node_id.isUnicast() || (server_node_id == node.getNodeID())) + { + UAVCAN_TRACE("ServiceClient", "Invalid Server Node ID"); + return -ErrInvalidParam; + } + out_call_id.server_node_id = server_node_id; + + /* + * Determining the Data Type ID + */ + if (data_type_descriptor_ == UAVCAN_NULLPTR) + { + GlobalDataTypeRegistry::instance().freeze(); + data_type_descriptor_ = GlobalDataTypeRegistry::instance().find(DataTypeKindService, dtname); + if (data_type_descriptor_ == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("ServiceClient", "Type [%s] is not registered", dtname); + return -ErrUnknownDataType; + } + UAVCAN_TRACE("ServiceClient", "Data type descriptor inited: %s", data_type_descriptor_->toString().c_str()); + } + UAVCAN_ASSERT(data_type_descriptor_ != UAVCAN_NULLPTR); + + /* + * Determining the Transfer ID + */ + const OutgoingTransferRegistryKey otr_key(data_type_descriptor_->getID(), + TransferTypeServiceRequest, server_node_id); + const MonotonicTime otr_deadline = node.getMonotonicTime() + TransferSender::getDefaultMaxTransferInterval(); + TransferID* const otr_tid = + node.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline); + if (!otr_tid) + { + UAVCAN_TRACE("ServiceClient", "OTR access failure, dtd=%s", data_type_descriptor_->toString().c_str()); + return -ErrMemory; + } + out_call_id.transfer_id = *otr_tid; + otr_tid->increment(); + + return 0; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_timer.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_timer.cpp new file mode 100644 index 000000000000..0230198d8692 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_timer.cpp @@ -0,0 +1,50 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +namespace uavcan +{ +/* + * TimerBase + */ +void TimerBase::handleDeadline(MonotonicTime current) +{ + UAVCAN_ASSERT(!isRunning()); + + const MonotonicTime scheduled_time = getDeadline(); + + if (period_ < MonotonicDuration::getInfinite()) + { + startWithDeadline(scheduled_time + period_); + } + + // Application can re-register the timer with different params, it's OK + handleTimerEvent(TimerEvent(scheduled_time, current)); +} + +void TimerBase::startOneShotWithDeadline(MonotonicTime deadline) +{ + stop(); + period_ = MonotonicDuration::getInfinite(); + DeadlineHandler::startWithDeadline(deadline); +} + +void TimerBase::startOneShotWithDelay(MonotonicDuration delay) +{ + stop(); + period_ = MonotonicDuration::getInfinite(); + DeadlineHandler::startWithDelay(delay); +} + +void TimerBase::startPeriodic(MonotonicDuration period) +{ + UAVCAN_ASSERT(period < MonotonicDuration::getInfinite()); + stop(); + period_ = period; + DeadlineHandler::startWithDelay(period); +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp new file mode 100644 index 000000000000..2889cbadee61 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp @@ -0,0 +1,210 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include + +namespace uavcan +{ + +void DynamicNodeIDClient::terminate() +{ + UAVCAN_TRACE("DynamicNodeIDClient", "Client terminated"); + stop(); + dnida_sub_.stop(); +} + +MonotonicDuration DynamicNodeIDClient::getRandomDuration(uint32_t lower_bound_msec, uint32_t upper_bound_msec) +{ + UAVCAN_ASSERT(upper_bound_msec > lower_bound_msec); + // coverity[dont_call] + return MonotonicDuration::fromMSec(lower_bound_msec + + static_cast(std::rand()) % (upper_bound_msec - lower_bound_msec)); +} + +void DynamicNodeIDClient::restartTimer(const Mode mode) +{ + UAVCAN_ASSERT(mode < NumModes); + UAVCAN_ASSERT((mode == ModeWaitingForTimeSlot) == (size_of_received_unique_id_ == 0)); + + const MonotonicDuration delay = (mode == ModeWaitingForTimeSlot) ? + getRandomDuration(protocol::dynamic_node_id::Allocation::MIN_REQUEST_PERIOD_MS, + protocol::dynamic_node_id::Allocation::MAX_REQUEST_PERIOD_MS) : + getRandomDuration(protocol::dynamic_node_id::Allocation::MIN_FOLLOWUP_DELAY_MS, + protocol::dynamic_node_id::Allocation::MAX_FOLLOWUP_DELAY_MS); + + startOneShotWithDelay(delay); + + UAVCAN_TRACE("DynamicNodeIDClient", "Restart mode %d, delay %d ms", + static_cast(mode), static_cast(delay.toMSec())); +} + +void DynamicNodeIDClient::handleTimerEvent(const TimerEvent&) +{ + UAVCAN_ASSERT(preferred_node_id_.isValid()); + UAVCAN_ASSERT(size_of_received_unique_id_ < protocol::dynamic_node_id::Allocation::FieldTypes::unique_id::MaxSize); + + if (isAllocationComplete()) + { + UAVCAN_ASSERT(0); + terminate(); + return; + } + + /* + * Filling the message. + */ + protocol::dynamic_node_id::Allocation tx; + tx.node_id = preferred_node_id_.get(); + tx.first_part_of_unique_id = (size_of_received_unique_id_ == 0); + + const uint8_t size_of_unique_id_in_request = + min(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST, + static_cast(tx.unique_id.capacity() - size_of_received_unique_id_)); + + tx.unique_id.resize(size_of_unique_id_in_request); + copy(unique_id_ + size_of_received_unique_id_, + unique_id_ + size_of_received_unique_id_ + size_of_unique_id_in_request, + tx.unique_id.begin()); + + UAVCAN_ASSERT(equal(tx.unique_id.begin(), tx.unique_id.end(), unique_id_ + size_of_received_unique_id_)); + + /* + * Resetting the state - this way we can continue with a first stage request on the next attempt. + */ + size_of_received_unique_id_ = 0; + restartTimer(ModeWaitingForTimeSlot); + + /* + * Broadcasting the message. + */ + UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting; preferred ID %d, size of UID %d", + static_cast(preferred_node_id_.get()), + static_cast(tx.unique_id.size())); + const int res = dnida_pub_.broadcast(tx); + if (res < 0) + { + dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed"); + } +} + +void DynamicNodeIDClient::handleAllocation(const ReceivedDataStructure& msg) +{ + UAVCAN_ASSERT(preferred_node_id_.isValid()); + if (isAllocationComplete()) + { + UAVCAN_ASSERT(0); + terminate(); + return; + } + + UAVCAN_TRACE("DynamicNodeIDClient", "Allocation message from %d, %d bytes of unique ID, node ID %d", + static_cast(msg.getSrcNodeID().get()), static_cast(msg.unique_id.size()), + static_cast(msg.node_id)); + + /* + * Switching to passive state by default; will switch to active state if response matches. + */ + size_of_received_unique_id_ = 0; + restartTimer(ModeWaitingForTimeSlot); + + /* + * Filtering out anonymous and invalid messages. + */ + const bool full_response = (msg.unique_id.size() == msg.unique_id.capacity()); + + if (msg.isAnonymousTransfer() || + msg.unique_id.empty() || + (full_response && (msg.node_id == 0))) + { + UAVCAN_TRACE("DynamicNodeIDClient", "Message from %d ignored", static_cast(msg.getSrcNodeID().get())); + return; + } + + /* + * If matches, either switch to active mode or complete the allocation. + */ + if (equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_)) + { + if (full_response) + { + allocated_node_id_ = msg.node_id; + allocator_node_id_ = msg.getSrcNodeID(); + terminate(); + UAVCAN_ASSERT(isAllocationComplete()); + UAVCAN_TRACE("DynamicNodeIDClient", "Allocation complete, node ID %d provided by %d", + static_cast(allocated_node_id_.get()), static_cast(allocator_node_id_.get())); + } + else + { + size_of_received_unique_id_ = msg.unique_id.size(); + restartTimer(ModeDelayBeforeFollowup); + } + } +} + +int DynamicNodeIDClient::start(const UniqueID& unique_id, + const NodeID preferred_node_id, + const TransferPriority transfer_priority) +{ + terminate(); + + // Allocation is not possible if node ID is already set + if (dnida_pub_.getNode().getNodeID().isUnicast()) + { + return -ErrLogic; + } + + // Unique ID initialization & validation + copy(unique_id.begin(), unique_id.end(), unique_id_); + bool unique_id_is_zero = true; + for (uint8_t i = 0; i < sizeof(unique_id_); i++) + { + if (unique_id_[i] != 0) + { + unique_id_is_zero = false; + break; + } + } + + if (unique_id_is_zero) + { + return -ErrInvalidParam; + } + + if (!preferred_node_id.isValid()) // Only broadcast and unicast are allowed + { + return -ErrInvalidParam; + } + + // Initializing the fields + preferred_node_id_ = preferred_node_id; + allocated_node_id_ = NodeID(); + allocator_node_id_ = NodeID(); + UAVCAN_ASSERT(preferred_node_id_.isValid()); + UAVCAN_ASSERT(!allocated_node_id_.isValid()); + UAVCAN_ASSERT(!allocator_node_id_.isValid()); + + // Initializing node objects - Rule A + int res = dnida_pub_.init(); + if (res < 0) + { + return res; + } + dnida_pub_.allowAnonymousTransfers(); + dnida_pub_.setPriority(transfer_priority); + + res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDClient::handleAllocation)); + if (res < 0) + { + return res; + } + dnida_sub_.allowAnonymousTransfers(); + + restartTimer(ModeWaitingForTimeSlot); + + return 0; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/protocol/uc_node_status_provider.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/protocol/uc_node_status_provider.cpp new file mode 100644 index 000000000000..a24034716c20 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/protocol/uc_node_status_provider.cpp @@ -0,0 +1,161 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ + +bool NodeStatusProvider::isNodeInfoInitialized() const +{ + // Hardware/Software versions are not required + return !node_info_.name.empty(); +} + +int NodeStatusProvider::publish() +{ + const MonotonicDuration uptime = getNode().getMonotonicTime() - creation_timestamp_; + UAVCAN_ASSERT(uptime.isPositive()); + node_info_.status.uptime_sec = uint32_t(uptime.toMSec() / 1000); + + UAVCAN_ASSERT(node_info_.status.health <= protocol::NodeStatus::FieldTypes::health::max()); + + return node_status_pub_.broadcast(node_info_.status); +} + +void NodeStatusProvider::handleTimerEvent(const TimerEvent&) +{ + if (getNode().isPassiveMode()) + { + UAVCAN_TRACE("NodeStatusProvider", "NodeStatus pub skipped - passive mode"); + } + else + { + if (ad_hoc_status_updater_ != UAVCAN_NULLPTR) + { + ad_hoc_status_updater_->updateNodeStatus(); + } + + const int res = publish(); + if (res < 0) + { + getNode().registerInternalFailure("NodeStatus pub failed"); + } + } +} + +void NodeStatusProvider::handleGetNodeInfoRequest(const protocol::GetNodeInfo::Request&, + protocol::GetNodeInfo::Response& rsp) +{ + UAVCAN_TRACE("NodeStatusProvider", "Got GetNodeInfo request"); + UAVCAN_ASSERT(isNodeInfoInitialized()); + rsp = node_info_; +} + +int NodeStatusProvider::startAndPublish(const TransferPriority priority) +{ + if (!isNodeInfoInitialized()) + { + UAVCAN_TRACE("NodeStatusProvider", "Node info was not initialized"); + return -ErrNotInited; + } + + int res = -1; + + node_status_pub_.setPriority(priority); + + if (!getNode().isPassiveMode()) + { + res = publish(); + if (res < 0) // Initial broadcast + { + goto fail; + } + } + + res = gni_srv_.start(GetNodeInfoCallback(this, &NodeStatusProvider::handleGetNodeInfoRequest)); + if (res < 0) + { + goto fail; + } + + setStatusPublicationPeriod(MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_BROADCASTING_PERIOD_MS)); + + return res; + +fail: + UAVCAN_ASSERT(res < 0); + gni_srv_.stop(); + TimerBase::stop(); + return res; +} + +void NodeStatusProvider::setStatusPublicationPeriod(uavcan::MonotonicDuration period) +{ + const MonotonicDuration maximum = MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_BROADCASTING_PERIOD_MS); + const MonotonicDuration minimum = MonotonicDuration::fromMSec(protocol::NodeStatus::MIN_BROADCASTING_PERIOD_MS); + + period = min(period, maximum); + period = max(period, minimum); + TimerBase::startPeriodic(period); + + const MonotonicDuration tx_timeout = period - MonotonicDuration::fromUSec(period.toUSec() / 20); + node_status_pub_.setTxTimeout(tx_timeout); + + UAVCAN_TRACE("NodeStatusProvider", "Status pub period: %s, TX timeout: %s", + period.toString().c_str(), node_status_pub_.getTxTimeout().toString().c_str()); +} + +uavcan::MonotonicDuration NodeStatusProvider::getStatusPublicationPeriod() const +{ + return TimerBase::getPeriod(); +} + +void NodeStatusProvider::setAdHocNodeStatusUpdater(IAdHocNodeStatusUpdater* updater) +{ + ad_hoc_status_updater_ = updater; // Can be nullptr, that's okay +} + +void NodeStatusProvider::setHealth(uint8_t code) +{ + node_info_.status.health = code; +} + +void NodeStatusProvider::setMode(uint8_t code) +{ + node_info_.status.mode = code; +} + +void NodeStatusProvider::setVendorSpecificStatusCode(VendorSpecificStatusCode code) +{ + node_info_.status.vendor_specific_status_code = code; +} + +void NodeStatusProvider::setName(const NodeName& name) +{ + if (node_info_.name.empty()) + { + node_info_.name = name; + } +} + +void NodeStatusProvider::setSoftwareVersion(const protocol::SoftwareVersion& version) +{ + if (node_info_.software_version == protocol::SoftwareVersion()) + { + node_info_.software_version = version; + } +} + +void NodeStatusProvider::setHardwareVersion(const protocol::HardwareVersion& version) +{ + if (node_info_.hardware_version == protocol::HardwareVersion()) + { + node_info_.hardware_version = version; + } +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp new file mode 100644 index 000000000000..e468b3ca85b1 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp @@ -0,0 +1,256 @@ +/* + * Copyright (C) 2015 Pavel Kirienko , + * Ilia Sheremet + */ + +#include +#include + +namespace uavcan +{ +const unsigned CanAcceptanceFilterConfigurator::DefaultFilterMsgMask; +const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceID; +const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceMask; +const unsigned CanAcceptanceFilterConfigurator::DefaultAnonMsgMask; +const unsigned CanAcceptanceFilterConfigurator::DefaultAnonMsgID; + +int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonymousMessages load_mode) +{ + multiset_configs_.clear(); + + if (load_mode == AcceptAnonymousMessages) + { + CanFilterConfig anon_frame_cfg; + anon_frame_cfg.id = DefaultAnonMsgID | CanFrame::FlagEFF; + anon_frame_cfg.mask = DefaultAnonMsgMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR; + if (multiset_configs_.emplace(anon_frame_cfg) == UAVCAN_NULLPTR) + { + return -ErrMemory; + } + } + + CanFilterConfig service_cfg; + service_cfg.id = DefaultFilterServiceID; + service_cfg.id |= (static_cast(node_.getNodeID().get()) << 8) | CanFrame::FlagEFF; + service_cfg.mask = DefaultFilterServiceMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR; + if (multiset_configs_.emplace(service_cfg) == UAVCAN_NULLPTR) + { + return -ErrMemory; + } + + const TransferListener* p = node_.getDispatcher().getListOfMessageListeners().get(); + while (p != UAVCAN_NULLPTR) + { + CanFilterConfig cfg; + cfg.id = (static_cast(p->getDataTypeDescriptor().getID().get()) << 8) | CanFrame::FlagEFF; + cfg.mask = DefaultFilterMsgMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR; + if (multiset_configs_.emplace(cfg) == UAVCAN_NULLPTR) + { + return -ErrMemory; + } + p = p->getNextListNode(); + } + + if (multiset_configs_.getSize() == 0) + { + return -ErrLogic; + } + + return 0; +} + +int16_t CanAcceptanceFilterConfigurator::mergeConfigurations() +{ + const uint16_t acceptance_filters_number = getNumFilters(); + if (acceptance_filters_number == 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available"); + return -ErrDriver; + } + UAVCAN_ASSERT(multiset_configs_.getSize() != 0); + + while (acceptance_filters_number < multiset_configs_.getSize()) + { + uint16_t i_rank = 0, j_rank = 0; + uint8_t best_rank = 0; + + const uint16_t multiset_array_size = static_cast(multiset_configs_.getSize()); + + for (uint16_t i_ind = 0; i_ind < multiset_array_size - 1; i_ind++) + { + for (uint16_t j_ind = static_cast(i_ind + 1); j_ind < multiset_array_size; j_ind++) + { + CanFilterConfig temp_config = mergeFilters(*multiset_configs_.getByIndex(i_ind), + *multiset_configs_.getByIndex(j_ind)); + uint8_t rank = countBits(temp_config.mask); + if (rank > best_rank) + { + best_rank = rank; + i_rank = i_ind; + j_rank = j_ind; + } + } + } + + *multiset_configs_.getByIndex(j_rank) = mergeFilters(*multiset_configs_.getByIndex(i_rank), + *multiset_configs_.getByIndex(j_rank)); + multiset_configs_.removeFirst(*multiset_configs_.getByIndex(i_rank)); + } + + UAVCAN_ASSERT(acceptance_filters_number >= multiset_configs_.getSize()); + + return 0; +} + +int CanAcceptanceFilterConfigurator::applyConfiguration(void) +{ + CanFilterConfig filter_conf_array[MaxCanAcceptanceFilters]; + unsigned int filter_array_size = multiset_configs_.getSize(); + + const uint16_t acceptance_filters_number = getNumFilters(); + if (acceptance_filters_number == 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available"); + return -ErrDriver; + } + + if (filter_array_size > acceptance_filters_number) + { + UAVCAN_TRACE("CanAcceptanceFilter", "Too many filter configurations. Executing computeConfiguration()"); + mergeConfigurations(); + filter_array_size = multiset_configs_.getSize(); + } + + if (filter_array_size > MaxCanAcceptanceFilters) + { + UAVCAN_ASSERT(0); + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); + return -ErrLogic; + } + + for (uint16_t i = 0; i < filter_array_size; i++) + { + CanFilterConfig temp_filter_config = *multiset_configs_.getByIndex(i); + + filter_conf_array[i] = temp_filter_config; + } + +#if UAVCAN_DEBUG + for (uint16_t i = 0; i < multiset_configs_.getSize(); i++) + { + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.ID [%u] = %u", i, + multiset_configs_.getByIndex(i)->id); + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.MK [%u] = %u", i, + multiset_configs_.getByIndex(i)->mask); + } +#endif + + ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver(); + for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) + { + ICanIface* iface = can_driver.getIface(i); + if (iface == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); + return -ErrDriver; + } + int16_t num = iface->configureFilters(reinterpret_cast(&filter_conf_array), + static_cast(filter_array_size)); + if (num < 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); + return -ErrDriver; + } + } + + return 0; +} + +int CanAcceptanceFilterConfigurator::computeConfiguration(AnonymousMessages mode) +{ + if (getNumFilters() == 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available"); + return -ErrDriver; + } + + int16_t fill_array_error = loadInputConfiguration(mode); + if (fill_array_error != 0) + { + UAVCAN_TRACE("CanAcceptanceFilter::loadInputConfiguration", "Failed to execute loadInputConfiguration()"); + return fill_array_error; + } + + int16_t merge_configurations_error = mergeConfigurations(); + if (merge_configurations_error != 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to compute optimal acceptance fliter's configuration"); + return merge_configurations_error; + } + + return 0; +} + +uint16_t CanAcceptanceFilterConfigurator::getNumFilters() const +{ + if (filters_number_ == 0) + { + static const uint16_t InvalidOut = 0xFFFF; + uint16_t out = InvalidOut; + ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver(); + + for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) + { + const ICanIface* iface = can_driver.getIface(i); + if (iface == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + out = 0; + break; + } + const uint16_t num = iface->getNumFilters(); + out = min(out, num); + if (out > MaxCanAcceptanceFilters) + { + out = MaxCanAcceptanceFilters; + } + } + + return (out == InvalidOut) ? 0 : out; + } + else + { + return filters_number_; + } +} + +int CanAcceptanceFilterConfigurator::addFilterConfig(const CanFilterConfig& config) +{ + if (multiset_configs_.emplace(config) == UAVCAN_NULLPTR) + { + return -ErrMemory; + } + + return 0; +} + +CanFilterConfig CanAcceptanceFilterConfigurator::mergeFilters(CanFilterConfig& a_, CanFilterConfig& b_) +{ + CanFilterConfig temp_arr; + temp_arr.mask = a_.mask & b_.mask & ~(a_.id ^ b_.id); + temp_arr.id = a_.id & temp_arr.mask; + + return temp_arr; +} + +uint8_t CanAcceptanceFilterConfigurator::countBits(uint32_t n_) +{ + uint8_t c_; // c accumulates the total bits set in v + for (c_ = 0; n_; c_++) + { + n_ &= n_ - 1; // clear the least significant bit set + } + + return c_; +} +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_io.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_io.cpp new file mode 100644 index 000000000000..bcca883477e5 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_io.cpp @@ -0,0 +1,509 @@ +/* + * CAN bus IO logic. + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ +/* + * CanRxFrame + */ +#if UAVCAN_TOSTRING +std::string CanRxFrame::toString(StringRepresentation mode) const +{ + std::string out = CanFrame::toString(mode); + out.reserve(128); + out += " ts_m=" + ts_mono.toString(); + out += " ts_utc=" + ts_utc.toString(); + out += " iface="; + out += char('0' + iface_index); + return out; +} +#endif + +/* + * CanTxQueue::Entry + */ +void CanTxQueue::Entry::destroy(Entry*& obj, IPoolAllocator& allocator) +{ + if (obj != UAVCAN_NULLPTR) + { + obj->~Entry(); + allocator.deallocate(obj); + obj = UAVCAN_NULLPTR; + } +} + +bool CanTxQueue::Entry::qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const +{ + if (qos != rhs_qos) + { + return qos > rhs_qos; + } + return frame.priorityHigherThan(rhs_frame); +} + +bool CanTxQueue::Entry::qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const +{ + if (qos != rhs_qos) + { + return qos < rhs_qos; + } + return frame.priorityLowerThan(rhs_frame); +} + +#if UAVCAN_TOSTRING +std::string CanTxQueue::Entry::toString() const +{ + std::string str_qos; + switch (qos) + { + case Volatile: + { + str_qos = " "; + break; + } + case Persistent: + { + str_qos = " "; + break; + } + default: + { + UAVCAN_ASSERT(0); + str_qos = " "; + break; + } + } + return str_qos + frame.toString(); +} +#endif + +/* + * CanTxQueue + */ +CanTxQueue::~CanTxQueue() +{ + Entry* p = queue_.get(); + while (p) + { + Entry* const next = p->getNextListNode(); + remove(p); + p = next; + } +} + +void CanTxQueue::registerRejectedFrame() +{ + if (rejected_frames_cnt_ < NumericTraits::max()) + { + rejected_frames_cnt_++; + } +} + +void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags) +{ + const MonotonicTime timestamp = sysclock_.getMonotonic(); + + if (timestamp >= tx_deadline) + { + UAVCAN_TRACE("CanTxQueue", "Push rejected: already expired"); + registerRejectedFrame(); + return; + } + + void* praw = allocator_.allocate(sizeof(Entry)); + if (praw == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("CanTxQueue", "Push OOM #1, cleanup"); + // No memory left in the pool, so we try to remove expired frames + Entry* p = queue_.get(); + while (p) + { + Entry* const next = p->getNextListNode(); + if (p->isExpired(timestamp)) + { + UAVCAN_TRACE("CanTxQueue", "Push: Expired %s", p->toString().c_str()); + registerRejectedFrame(); + remove(p); + } + p = next; + } + praw = allocator_.allocate(sizeof(Entry)); // Try again + } + + if (praw == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("CanTxQueue", "Push OOM #2, QoS arbitration"); + registerRejectedFrame(); + + // Find a frame with lowest QoS + Entry* p = queue_.get(); + if (p == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("CanTxQueue", "Push rejected: Nothing to replace"); + return; + } + Entry* lowestqos = p; + while (p) + { + if (lowestqos->qosHigherThan(*p)) + { + lowestqos = p; + } + p = p->getNextListNode(); + } + // Note that frame with *equal* QoS will be replaced too. + if (lowestqos->qosHigherThan(frame, qos)) // Frame that we want to transmit has lowest QoS + { + UAVCAN_TRACE("CanTxQueue", "Push rejected: low QoS"); + return; // What a loser. + } + UAVCAN_TRACE("CanTxQueue", "Push: Replacing %s", lowestqos->toString().c_str()); + remove(lowestqos); + praw = allocator_.allocate(sizeof(Entry)); // Try again + } + + if (praw == UAVCAN_NULLPTR) + { + return; // Seems that there is no memory at all. + } + Entry* entry = new (praw) Entry(frame, tx_deadline, qos, flags); + UAVCAN_ASSERT(entry); + queue_.insertBefore(entry, PriorityInsertionComparator(frame)); +} + +CanTxQueue::Entry* CanTxQueue::peek() +{ + const MonotonicTime timestamp = sysclock_.getMonotonic(); + Entry* p = queue_.get(); + while (p) + { + if (p->isExpired(timestamp)) + { + UAVCAN_TRACE("CanTxQueue", "Peek: Expired %s", p->toString().c_str()); + Entry* const next = p->getNextListNode(); + registerRejectedFrame(); + remove(p); + p = next; + } + else + { + return p; + } + } + return UAVCAN_NULLPTR; +} + +void CanTxQueue::remove(Entry*& entry) +{ + if (entry == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + return; + } + queue_.remove(entry); + Entry::destroy(entry, allocator_); +} + +const CanFrame* CanTxQueue::getTopPriorityPendingFrame() const +{ + return (queue_.get() == UAVCAN_NULLPTR) ? UAVCAN_NULLPTR : &queue_.get()->frame; +} + +bool CanTxQueue::topPriorityHigherOrEqual(const CanFrame& rhs_frame) const +{ + const Entry* entry = queue_.get(); + if (entry == UAVCAN_NULLPTR) + { + return false; + } + return !rhs_frame.priorityHigherThan(entry->frame); +} + +/* + * CanIOManager + */ +int CanIOManager::sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags) +{ + UAVCAN_ASSERT(iface_index < MaxCanIfaces); + ICanIface* const iface = driver_.getIface(iface_index); + if (iface == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); // Nonexistent interface + return -ErrLogic; + } + const int res = iface->send(frame, tx_deadline, flags); + if (res != 1) + { + UAVCAN_TRACE("CanIOManager", "Send failed: code %i, iface %i, frame %s", + res, iface_index, frame.toString().c_str()); + } + if (res > 0) + { + counters_[iface_index].frames_tx += unsigned(res); + } + return res; +} + +int CanIOManager::sendFromTxQueue(uint8_t iface_index) +{ + UAVCAN_ASSERT(iface_index < MaxCanIfaces); + CanTxQueue::Entry* entry = tx_queues_[iface_index]->peek(); + if (entry == UAVCAN_NULLPTR) + { + return 0; + } + const int res = sendToIface(iface_index, entry->frame, entry->deadline, entry->flags); + if (res > 0) + { + tx_queues_[iface_index]->remove(entry); + } + return res; +} + +int CanIOManager::callSelect(CanSelectMasks& inout_masks, const CanFrame* (& pending_tx)[MaxCanIfaces], + MonotonicTime blocking_deadline) +{ + const CanSelectMasks in_masks = inout_masks; + + const int res = driver_.select(inout_masks, pending_tx, blocking_deadline); + if (res < 0) + { + return -ErrDriver; + } + + inout_masks.read &= in_masks.read; // Driver is not required to clean the masks + inout_masks.write &= in_masks.write; + return res; +} + +CanIOManager::CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock, + std::size_t mem_blocks_per_iface) + : driver_(driver) + , sysclock_(sysclock) + , num_ifaces_(driver.getNumIfaces()) +{ + if (num_ifaces_ < 1 || num_ifaces_ > MaxCanIfaces) + { + handleFatalError("Num ifaces"); + } + + if (mem_blocks_per_iface == 0) + { + mem_blocks_per_iface = allocator.getBlockCapacity() / (num_ifaces_ + 1U) + 1U; + } + UAVCAN_TRACE("CanIOManager", "Memory blocks per iface: %u, total: %u", + unsigned(mem_blocks_per_iface), unsigned(allocator.getBlockCapacity())); + + for (int i = 0; i < num_ifaces_; i++) + { + tx_queues_[i].construct + (allocator, sysclock, mem_blocks_per_iface); + } +} + +uint8_t CanIOManager::makePendingTxMask() const +{ + uint8_t write_mask = 0; + for (uint8_t i = 0; i < getNumIfaces(); i++) + { + if (!tx_queues_[i]->isEmpty()) + { + write_mask = uint8_t(write_mask | (1 << i)); + } + } + return write_mask; +} + +CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) const +{ + ICanIface* const iface = driver_.getIface(iface_index); + if (iface == UAVCAN_NULLPTR || iface_index >= MaxCanIfaces) + { + UAVCAN_ASSERT(0); + return CanIfacePerfCounters(); + } + CanIfacePerfCounters cnt; + cnt.errors = iface->getErrorCount() + tx_queues_[iface_index]->getRejectedFrameCount(); + cnt.frames_rx = counters_[iface_index].frames_rx; + cnt.frames_tx = counters_[iface_index].frames_tx; + return cnt; +} + +int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, + uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags) +{ + const uint8_t num_ifaces = getNumIfaces(); + const uint8_t all_ifaces_mask = uint8_t((1U << num_ifaces) - 1); + iface_mask &= all_ifaces_mask; + + if (blocking_deadline > tx_deadline) + { + blocking_deadline = tx_deadline; + } + + int retval = 0; + + while (true) // Somebody please refactor this. + { + if (iface_mask == 0) + { + break; + } + + CanSelectMasks masks; + masks.write = iface_mask | makePendingTxMask(); + { + // Building the list of next pending frames per iface. + // The driver will give them a scrutinizing look before deciding whether he wants to accept them. + const CanFrame* pending_tx[MaxCanIfaces] = {}; + for (int i = 0; i < num_ifaces; i++) + { + CanTxQueue& q = *tx_queues_[i]; + if (iface_mask & (1 << i)) // I hate myself so much right now. + { + pending_tx[i] = q.topPriorityHigherOrEqual(frame) ? q.getTopPriorityPendingFrame() : &frame; + } + else + { + pending_tx[i] = q.getTopPriorityPendingFrame(); + } + } + + const int select_res = callSelect(masks, pending_tx, blocking_deadline); + if (select_res < 0) + { + return -ErrDriver; + } + UAVCAN_ASSERT(masks.read == 0); + } + + // Transmission + for (uint8_t i = 0; i < num_ifaces; i++) + { + if (masks.write & (1 << i)) + { + int res = 0; + if (iface_mask & (1 << i)) + { + if (tx_queues_[i]->topPriorityHigherOrEqual(frame)) + { + res = sendFromTxQueue(i); // May return 0 if nothing to transmit (e.g. expired) + } + if (res <= 0) + { + res = sendToIface(i, frame, tx_deadline, flags); + if (res > 0) + { + iface_mask &= uint8_t(~(1 << i)); // Mark transmitted + } + } + } + else + { + res = sendFromTxQueue(i); + } + if (res > 0) + { + retval++; + } + } + } + + // Timeout. Enqueue the frame if wasn't transmitted and leave. + const bool timed_out = sysclock_.getMonotonic() >= blocking_deadline; + if (masks.write == 0 || timed_out) + { + if (!timed_out) + { + UAVCAN_TRACE("CanIOManager", "Send: Premature timeout in select(), will try again"); + continue; + } + for (uint8_t i = 0; i < num_ifaces; i++) + { + if (iface_mask & (1 << i)) + { + tx_queues_[i]->push(frame, tx_deadline, qos, flags); + } + } + break; + } + } + return retval; +} + +int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags) +{ + const uint8_t num_ifaces = getNumIfaces(); + + while (true) + { + CanSelectMasks masks; + masks.write = makePendingTxMask(); + masks.read = uint8_t((1 << num_ifaces) - 1); + { + const CanFrame* pending_tx[MaxCanIfaces] = {}; + for (int i = 0; i < num_ifaces; i++) // Dear compiler, kindly unroll this. Thanks. + { + pending_tx[i] = tx_queues_[i]->getTopPriorityPendingFrame(); + } + + const int select_res = callSelect(masks, pending_tx, blocking_deadline); + if (select_res < 0) + { + return -ErrDriver; + } + } + + // Write - if buffers are not empty, one frame will be sent for each iface per one receive() call + for (uint8_t i = 0; i < num_ifaces; i++) + { + if (masks.write & (1 << i)) + { + (void)sendFromTxQueue(i); // It may fail, we don't care. Requested operation was receive, not send. + } + } + + // Read + for (uint8_t i = 0; i < num_ifaces; i++) + { + if (masks.read & (1 << i)) + { + ICanIface* const iface = driver_.getIface(i); + if (iface == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); // Nonexistent interface + continue; + } + + const int res = iface->receive(out_frame, out_frame.ts_mono, out_frame.ts_utc, out_flags); + if (res == 0) + { + UAVCAN_ASSERT(0); // select() reported that iface has pending RX frames, but receive() returned none + continue; + } + out_frame.iface_index = i; + + if ((res > 0) && !(out_flags & CanIOFlagLoopback)) + { + counters_[i].frames_rx += 1; + } + return (res < 0) ? -ErrDriver : res; + } + } + + // Timeout checked in the last order - this way we can operate with expired deadline: + if (sysclock_.getMonotonic() >= blocking_deadline) + { + break; + } + } + return 0; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_crc.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_crc.cpp new file mode 100644 index 000000000000..a854c4ca5d14 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_crc.cpp @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ + +#if !UAVCAN_TINY + +// print ', '.join(map(lambda x: '%04x' % x, map(lambda x: int(x, 0), c.crc_ccitt_tab))) +const uint16_t TransferCRC::Table[256] = +{ + 0x0000U, 0x1021U, 0x2042U, 0x3063U, 0x4084U, 0x50A5U, 0x60C6U, 0x70E7U, + 0x8108U, 0x9129U, 0xA14AU, 0xB16BU, 0xC18CU, 0xD1ADU, 0xE1CEU, 0xF1EFU, + 0x1231U, 0x0210U, 0x3273U, 0x2252U, 0x52B5U, 0x4294U, 0x72F7U, 0x62D6U, + 0x9339U, 0x8318U, 0xB37BU, 0xA35AU, 0xD3BDU, 0xC39CU, 0xF3FFU, 0xE3DEU, + 0x2462U, 0x3443U, 0x0420U, 0x1401U, 0x64E6U, 0x74C7U, 0x44A4U, 0x5485U, + 0xA56AU, 0xB54BU, 0x8528U, 0x9509U, 0xE5EEU, 0xF5CFU, 0xC5ACU, 0xD58DU, + 0x3653U, 0x2672U, 0x1611U, 0x0630U, 0x76D7U, 0x66F6U, 0x5695U, 0x46B4U, + 0xB75BU, 0xA77AU, 0x9719U, 0x8738U, 0xF7DFU, 0xE7FEU, 0xD79DU, 0xC7BCU, + 0x48C4U, 0x58E5U, 0x6886U, 0x78A7U, 0x0840U, 0x1861U, 0x2802U, 0x3823U, + 0xC9CCU, 0xD9EDU, 0xE98EU, 0xF9AFU, 0x8948U, 0x9969U, 0xA90AU, 0xB92BU, + 0x5AF5U, 0x4AD4U, 0x7AB7U, 0x6A96U, 0x1A71U, 0x0A50U, 0x3A33U, 0x2A12U, + 0xDBFDU, 0xCBDCU, 0xFBBFU, 0xEB9EU, 0x9B79U, 0x8B58U, 0xBB3BU, 0xAB1AU, + 0x6CA6U, 0x7C87U, 0x4CE4U, 0x5CC5U, 0x2C22U, 0x3C03U, 0x0C60U, 0x1C41U, + 0xEDAEU, 0xFD8FU, 0xCDECU, 0xDDCDU, 0xAD2AU, 0xBD0BU, 0x8D68U, 0x9D49U, + 0x7E97U, 0x6EB6U, 0x5ED5U, 0x4EF4U, 0x3E13U, 0x2E32U, 0x1E51U, 0x0E70U, + 0xFF9FU, 0xEFBEU, 0xDFDDU, 0xCFFCU, 0xBF1BU, 0xAF3AU, 0x9F59U, 0x8F78U, + 0x9188U, 0x81A9U, 0xB1CAU, 0xA1EBU, 0xD10CU, 0xC12DU, 0xF14EU, 0xE16FU, + 0x1080U, 0x00A1U, 0x30C2U, 0x20E3U, 0x5004U, 0x4025U, 0x7046U, 0x6067U, + 0x83B9U, 0x9398U, 0xA3FBU, 0xB3DAU, 0xC33DU, 0xD31CU, 0xE37FU, 0xF35EU, + 0x02B1U, 0x1290U, 0x22F3U, 0x32D2U, 0x4235U, 0x5214U, 0x6277U, 0x7256U, + 0xB5EAU, 0xA5CBU, 0x95A8U, 0x8589U, 0xF56EU, 0xE54FU, 0xD52CU, 0xC50DU, + 0x34E2U, 0x24C3U, 0x14A0U, 0x0481U, 0x7466U, 0x6447U, 0x5424U, 0x4405U, + 0xA7DBU, 0xB7FAU, 0x8799U, 0x97B8U, 0xE75FU, 0xF77EU, 0xC71DU, 0xD73CU, + 0x26D3U, 0x36F2U, 0x0691U, 0x16B0U, 0x6657U, 0x7676U, 0x4615U, 0x5634U, + 0xD94CU, 0xC96DU, 0xF90EU, 0xE92FU, 0x99C8U, 0x89E9U, 0xB98AU, 0xA9ABU, + 0x5844U, 0x4865U, 0x7806U, 0x6827U, 0x18C0U, 0x08E1U, 0x3882U, 0x28A3U, + 0xCB7DU, 0xDB5CU, 0xEB3FU, 0xFB1EU, 0x8BF9U, 0x9BD8U, 0xABBBU, 0xBB9AU, + 0x4A75U, 0x5A54U, 0x6A37U, 0x7A16U, 0x0AF1U, 0x1AD0U, 0x2AB3U, 0x3A92U, + 0xFD2EU, 0xED0FU, 0xDD6CU, 0xCD4DU, 0xBDAAU, 0xAD8BU, 0x9DE8U, 0x8DC9U, + 0x7C26U, 0x6C07U, 0x5C64U, 0x4C45U, 0x3CA2U, 0x2C83U, 0x1CE0U, 0x0CC1U, + 0xEF1FU, 0xFF3EU, 0xCF5DU, 0xDF7CU, 0xAF9BU, 0xBFBAU, 0x8FD9U, 0x9FF8U, + 0x6E17U, 0x7E36U, 0x4E55U, 0x5E74U, 0x2E93U, 0x3EB2U, 0x0ED1U, 0x1EF0U +}; + +#endif + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_dispatcher.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_dispatcher.cpp new file mode 100644 index 000000000000..f304689bc4ef --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_dispatcher.cpp @@ -0,0 +1,385 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ +#if !UAVCAN_TINY +/* + * LoopbackFrameListenerBase + */ +void LoopbackFrameListenerBase::startListening() +{ + dispatcher_.getLoopbackFrameListenerRegistry().add(this); +} + +void LoopbackFrameListenerBase::stopListening() +{ + dispatcher_.getLoopbackFrameListenerRegistry().remove(this); +} + +bool LoopbackFrameListenerBase::isListening() const +{ + return dispatcher_.getLoopbackFrameListenerRegistry().doesExist(this); +} + +/* + * LoopbackFrameListenerRegistry + */ +void LoopbackFrameListenerRegistry::add(LoopbackFrameListenerBase* listener) +{ + UAVCAN_ASSERT(listener); + listeners_.insert(listener); +} + +void LoopbackFrameListenerRegistry::remove(LoopbackFrameListenerBase* listener) +{ + UAVCAN_ASSERT(listener); + listeners_.remove(listener); +} + +bool LoopbackFrameListenerRegistry::doesExist(const LoopbackFrameListenerBase* listener) const +{ + UAVCAN_ASSERT(listener); + const LoopbackFrameListenerBase* p = listeners_.get(); + while (p) + { + if (p == listener) + { + return true; + } + p = p->getNextListNode(); + } + return false; +} + +void LoopbackFrameListenerRegistry::invokeListeners(RxFrame& frame) +{ + LoopbackFrameListenerBase* p = listeners_.get(); + while (p) + { + LoopbackFrameListenerBase* const next = p->getNextListNode(); + p->handleLoopbackFrame(frame); // p may be modified + p = next; + } +} +#endif + +/* + * Dispatcher::ListenerRegister + */ +bool Dispatcher::ListenerRegistry::add(TransferListener* listener, Mode mode) +{ + if (mode == UniqueListener) + { + TransferListener* p = list_.get(); + while (p) + { + if (p->getDataTypeDescriptor().getID() == listener->getDataTypeDescriptor().getID()) + { + return false; + } + p = p->getNextListNode(); + } + } + // Objective is to arrange entries by Data Type ID in ascending order from root. + list_.insertBefore(listener, DataTypeIDInsertionComparator(listener->getDataTypeDescriptor().getID())); + return true; +} + +void Dispatcher::ListenerRegistry::remove(TransferListener* listener) +{ + list_.remove(listener); +} + +bool Dispatcher::ListenerRegistry::exists(DataTypeID dtid) const +{ + TransferListener* p = list_.get(); + while (p) + { + if (p->getDataTypeDescriptor().getID() == dtid) + { + return true; + } + p = p->getNextListNode(); + } + return false; +} + +void Dispatcher::ListenerRegistry::cleanup(MonotonicTime ts) +{ + TransferListener* p = list_.get(); + while (p) + { + TransferListener* const next = p->getNextListNode(); + p->cleanup(ts); // p may be modified + p = next; + } +} + +void Dispatcher::ListenerRegistry::handleFrame(const RxFrame& frame) +{ + TransferListener* p = list_.get(); + while (p) + { + TransferListener* const next = p->getNextListNode(); + if (p->getDataTypeDescriptor().getID() == frame.getDataTypeID()) + { + p->handleFrame(frame); // p may be modified + } + else if (p->getDataTypeDescriptor().getID() < frame.getDataTypeID()) // Listeners are ordered by data type id! + { + break; + } + else + { + ; // Nothing to do with this one + } + p = next; + } +} + +/* + * Dispatcher + */ +void Dispatcher::handleFrame(const CanRxFrame& can_frame) +{ + RxFrame frame; + if (!frame.parse(can_frame)) + { + // This is not counted as a transport error + UAVCAN_TRACE("Dispatcher", "Invalid CAN frame received: %s", can_frame.toString().c_str()); + return; + } + + if ((frame.getDstNodeID() != NodeID::Broadcast) && + (frame.getDstNodeID() != getNodeID())) + { + return; + } + + switch (frame.getTransferType()) + { + case TransferTypeMessageBroadcast: + { + lmsg_.handleFrame(frame); + break; + } + case TransferTypeServiceRequest: + { + lsrv_req_.handleFrame(frame); + break; + } + case TransferTypeServiceResponse: + { + lsrv_resp_.handleFrame(frame); + break; + } + default: + { + UAVCAN_ASSERT(0); + break; + } + } +} + +#if UAVCAN_TINY +void Dispatcher::handleLoopbackFrame(const CanRxFrame&) +{ +} + +void Dispatcher::notifyRxFrameListener(const CanRxFrame&, CanIOFlags) +{ +} +#else +void Dispatcher::handleLoopbackFrame(const CanRxFrame& can_frame) +{ + RxFrame frame; + if (!frame.parse(can_frame)) + { + UAVCAN_TRACE("Dispatcher", "Invalid loopback CAN frame: %s", can_frame.toString().c_str()); + UAVCAN_ASSERT(0); // No way! + return; + } + UAVCAN_ASSERT(frame.getSrcNodeID() == getNodeID()); + loopback_listeners_.invokeListeners(frame); +} + +void Dispatcher::notifyRxFrameListener(const CanRxFrame& can_frame, CanIOFlags flags) +{ + if (rx_listener_ != UAVCAN_NULLPTR) + { + rx_listener_->handleRxFrame(can_frame, flags); + } +} +#endif + +int Dispatcher::spin(MonotonicTime deadline) +{ + int num_frames_processed = 0; + do + { + CanIOFlags flags = 0; + CanRxFrame frame; + const int res = canio_.receive(frame, deadline, flags); + if (res < 0) + { + return res; + } + if (res > 0) + { + if (flags & CanIOFlagLoopback) + { + handleLoopbackFrame(frame); + } + else + { + num_frames_processed++; + handleFrame(frame); + } + notifyRxFrameListener(frame, flags); + } + } + while (sysclock_.getMonotonic() < deadline); + + return num_frames_processed; +} + +int Dispatcher::spinOnce() +{ + int num_frames_processed = 0; + + while (true) + { + CanIOFlags flags = 0; + CanRxFrame frame; + const int res = canio_.receive(frame, MonotonicTime(), flags); + if (res < 0) + { + return res; + } + else if (res > 0) + { + if (flags & CanIOFlagLoopback) + { + handleLoopbackFrame(frame); + } + else + { + num_frames_processed++; + handleFrame(frame); + } + notifyRxFrameListener(frame, flags); + } + else + { + break; // No frames left + } + } + + return num_frames_processed; +} + +int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, + CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask) +{ + if (frame.getSrcNodeID() != getNodeID()) + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + + CanFrame can_frame; + if (!frame.compile(can_frame)) + { + UAVCAN_TRACE("Dispatcher", "Unable to send: frame is malformed: %s", frame.toString().c_str()); + UAVCAN_ASSERT(0); + return -ErrLogic; + } + return canio_.send(can_frame, tx_deadline, blocking_deadline, iface_mask, qos, flags); +} + +void Dispatcher::cleanup(MonotonicTime ts) +{ + outgoing_transfer_reg_.cleanup(ts); + lmsg_.cleanup(ts); + lsrv_req_.cleanup(ts); + lsrv_resp_.cleanup(ts); +} + +bool Dispatcher::registerMessageListener(TransferListener* listener) +{ + if (listener->getDataTypeDescriptor().getKind() != DataTypeKindMessage) + { + UAVCAN_ASSERT(0); + return false; + } + return lmsg_.add(listener, ListenerRegistry::ManyListeners); // Multiple subscribers are OK +} + +bool Dispatcher::registerServiceRequestListener(TransferListener* listener) +{ + if (listener->getDataTypeDescriptor().getKind() != DataTypeKindService) + { + UAVCAN_ASSERT(0); + return false; + } + return lsrv_req_.add(listener, ListenerRegistry::UniqueListener); // Only one server per data type +} + +bool Dispatcher::registerServiceResponseListener(TransferListener* listener) +{ + if (listener->getDataTypeDescriptor().getKind() != DataTypeKindService) + { + UAVCAN_ASSERT(0); + return false; + } + return lsrv_resp_.add(listener, ListenerRegistry::ManyListeners); // Multiple callers may call same srv +} + +void Dispatcher::unregisterMessageListener(TransferListener* listener) +{ + lmsg_.remove(listener); +} + +void Dispatcher::unregisterServiceRequestListener(TransferListener* listener) +{ + lsrv_req_.remove(listener); +} + +void Dispatcher::unregisterServiceResponseListener(TransferListener* listener) +{ + lsrv_resp_.remove(listener); +} + +bool Dispatcher::hasSubscriber(DataTypeID dtid) const +{ + return lmsg_.exists(dtid); +} + +bool Dispatcher::hasPublisher(DataTypeID dtid) const +{ + return outgoing_transfer_reg_.exists(dtid, TransferTypeMessageBroadcast); +} + +bool Dispatcher::hasServer(DataTypeID dtid) const +{ + return lsrv_req_.exists(dtid); +} + +bool Dispatcher::setNodeID(NodeID nid) +{ + if (!self_node_id_is_set_) + { + self_node_id_ = nid; + self_node_id_is_set_ = true; + return true; + } + return false; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_frame.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_frame.cpp new file mode 100644 index 000000000000..d97c34f0914a --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_frame.cpp @@ -0,0 +1,334 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Frame + */ +uint8_t Frame::setPayload(const uint8_t* data, unsigned len) +{ + const uint8_t maxlen = getPayloadCapacity(); + len = min(unsigned(maxlen), len); + (void)copy(data, data + len, payload_); + payload_len_ = uint_fast8_t(len); + return static_cast(len); +} + +template +inline static uint32_t bitunpack(uint32_t val) +{ + StaticAssert<(OFFSET >= 0)>::check(); + StaticAssert<(WIDTH > 0)>::check(); + StaticAssert<((OFFSET + WIDTH) <= 29)>::check(); + return (val >> OFFSET) & ((1UL << WIDTH) - 1); +} + +bool Frame::parse(const CanFrame& can_frame) +{ + if (can_frame.isErrorFrame() || can_frame.isRemoteTransmissionRequest() || !can_frame.isExtended()) + { + UAVCAN_TRACE("Frame", "Parsing failed at line %d", __LINE__); + return false; + } + + if (can_frame.dlc > sizeof(can_frame.data)) + { + UAVCAN_ASSERT(0); // This is not a protocol error, so UAVCAN_ASSERT() is ok + return false; + } + + if (can_frame.dlc < 1) + { + UAVCAN_TRACE("Frame", "Parsing failed at line %d", __LINE__); + return false; + } + + /* + * CAN ID parsing + */ + const uint32_t id = can_frame.id & CanFrame::MaskExtID; + + transfer_priority_ = static_cast(bitunpack<24, 5>(id)); + src_node_id_ = static_cast(bitunpack<0, 7>(id)); + + const bool service_not_message = bitunpack<7, 1>(id) != 0U; + if (service_not_message) + { + const bool request_not_response = bitunpack<15, 1>(id) != 0U; + transfer_type_ = request_not_response ? TransferTypeServiceRequest : TransferTypeServiceResponse; + + dst_node_id_ = static_cast(bitunpack<8, 7>(id)); + data_type_id_ = static_cast(bitunpack<16, 8>(id)); + } + else + { + transfer_type_ = TransferTypeMessageBroadcast; + dst_node_id_ = NodeID::Broadcast; + + data_type_id_ = static_cast(bitunpack<8, 16>(id)); + + if (src_node_id_.isBroadcast()) + { + // Removing the discriminator + data_type_id_ = static_cast(data_type_id_.get() & 3U); + } + } + + /* + * CAN payload parsing + */ + payload_len_ = static_cast(can_frame.dlc - 1U); + (void)copy(can_frame.data, can_frame.data + payload_len_, payload_); + + const uint8_t tail = can_frame.data[can_frame.dlc - 1U]; + + start_of_transfer_ = (tail & (1U << 7)) != 0; + end_of_transfer_ = (tail & (1U << 6)) != 0; + toggle_ = (tail & (1U << 5)) != 0; + + transfer_id_ = tail & TransferID::Max; + + return isValid(); +} + +template +inline static uint32_t bitpack(uint32_t field) +{ + StaticAssert<(OFFSET >= 0)>::check(); + StaticAssert<(WIDTH > 0)>::check(); + StaticAssert<((OFFSET + WIDTH) <= 29)>::check(); + UAVCAN_ASSERT((field & ((1UL << WIDTH) - 1)) == field); + return uint32_t((field & ((1UL << WIDTH) - 1)) << OFFSET); +} + +bool Frame::compile(CanFrame& out_can_frame) const +{ + if (!isValid()) + { + UAVCAN_ASSERT(0); // This is an application error, so we need to maximize it. + return false; + } + + /* + * CAN ID field + */ + out_can_frame.id = CanFrame::FlagEFF | + bitpack<0, 7>(src_node_id_.get()) | + bitpack<24, 5>(transfer_priority_.get()); + + if (transfer_type_ == TransferTypeMessageBroadcast) + { + out_can_frame.id |= + bitpack<7, 1>(0U) | + bitpack<8, 16>(data_type_id_.get()); + } + else + { + const bool request_not_response = transfer_type_ == TransferTypeServiceRequest; + out_can_frame.id |= + bitpack<7, 1>(1U) | + bitpack<8, 7>(dst_node_id_.get()) | + bitpack<15, 1>(request_not_response ? 1U : 0U) | + bitpack<16, 8>(data_type_id_.get()); + } + + /* + * Payload + */ + uint8_t tail = transfer_id_.get(); + if (start_of_transfer_) + { + tail |= (1U << 7); + } + if (end_of_transfer_) + { + tail |= (1U << 6); + } + if (toggle_) + { + tail |= (1U << 5); + } + + UAVCAN_ASSERT(payload_len_ < sizeof(static_cast(UAVCAN_NULLPTR)->data)); + + out_can_frame.dlc = static_cast(payload_len_); + (void)copy(payload_, payload_ + payload_len_, out_can_frame.data); + + out_can_frame.data[out_can_frame.dlc] = tail; + out_can_frame.dlc++; + + /* + * Discriminator + */ + if (src_node_id_.isBroadcast()) + { + TransferCRC crc; + crc.add(out_can_frame.data, out_can_frame.dlc); + out_can_frame.id |= bitpack<10, 14>(crc.get() & ((1U << 14) - 1U)); + } + + return true; +} + +bool Frame::isValid() const +{ + /* + * Toggle + */ + if (start_of_transfer_ && toggle_) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + /* + * Node ID + */ + if (!src_node_id_.isValid() || !dst_node_id_.isValid()) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + if (src_node_id_.isUnicast() && (src_node_id_ == dst_node_id_)) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + /* + * Transfer type + */ + if (transfer_type_ >= NumTransferTypes) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + if ((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast()) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + // Anonymous transfers + if (src_node_id_.isBroadcast() && + (!start_of_transfer_ || !end_of_transfer_ || (transfer_type_ != TransferTypeMessageBroadcast))) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + /* + * Payload + */ + if (payload_len_ > getPayloadCapacity()) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + /* + * Data type ID + */ + if (!data_type_id_.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type_))) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + /* + * Priority + */ + if (!transfer_priority_.isValid()) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + return true; +} + +bool Frame::operator==(const Frame& rhs) const +{ + return + (transfer_priority_ == rhs.transfer_priority_) && + (transfer_type_ == rhs.transfer_type_) && + (data_type_id_ == rhs.data_type_id_) && + (src_node_id_ == rhs.src_node_id_) && + (dst_node_id_ == rhs.dst_node_id_) && + (transfer_id_ == rhs.transfer_id_) && + (toggle_ == rhs.toggle_) && + (start_of_transfer_ == rhs.start_of_transfer_) && + (end_of_transfer_ == rhs.end_of_transfer_) && + (payload_len_ == rhs.payload_len_) && + equal(payload_, payload_ + payload_len_, rhs.payload_); +} + +#if UAVCAN_TOSTRING +std::string Frame::toString() const +{ + static const int BUFLEN = 100; + char buf[BUFLEN]; + int ofs = snprintf(buf, BUFLEN, "prio=%d dtid=%d tt=%d snid=%d dnid=%d sot=%d eot=%d togl=%d tid=%d payload=[", + int(transfer_priority_.get()), int(data_type_id_.get()), int(transfer_type_), + int(src_node_id_.get()), int(dst_node_id_.get()), + int(start_of_transfer_), int(end_of_transfer_), int(toggle_), int(transfer_id_.get())); + + for (unsigned i = 0; i < payload_len_; i++) + { + // Coverity Scan complains about payload_ being not default initialized. This is OK. + // coverity[read_parm_fld] + ofs += snprintf(buf + ofs, unsigned(BUFLEN - ofs), "%02x", payload_[i]); + if ((i + 1) < payload_len_) + { + ofs += snprintf(buf + ofs, unsigned(BUFLEN - ofs), " "); + } + } + (void)snprintf(buf + ofs, unsigned(BUFLEN - ofs), "]"); + return std::string(buf); +} +#endif + +/** + * RxFrame + */ +bool RxFrame::parse(const CanRxFrame& can_frame) +{ + if (!Frame::parse(can_frame)) + { + return false; + } + if (can_frame.ts_mono.isZero()) // Monotonic timestamps are mandatory. + { + UAVCAN_ASSERT(0); // If it is not set, it's a driver failure. + return false; + } + ts_mono_ = can_frame.ts_mono; + ts_utc_ = can_frame.ts_utc; + iface_index_ = can_frame.iface_index; + return true; +} + +#if UAVCAN_TOSTRING +std::string RxFrame::toString() const +{ + std::string out = Frame::toString(); + out.reserve(128); + out += " ts_m=" + ts_mono_.toString(); + out += " ts_utc=" + ts_utc_.toString(); + out += " iface="; + out += char('0' + iface_index_); + return out; +} +#endif + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_outgoing_transfer_registry.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_outgoing_transfer_registry.cpp new file mode 100644 index 000000000000..fc49f65aa3d1 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_outgoing_transfer_registry.cpp @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ +/* + * OutgoingTransferRegistryKey + */ +#if UAVCAN_TOSTRING +std::string OutgoingTransferRegistryKey::toString() const +{ + char buf[40]; + (void)snprintf(buf, sizeof(buf), "dtid=%u tt=%u dnid=%u", + int(data_type_id_.get()), int(transfer_type_), int(destination_node_id_.get())); + return std::string(buf); +} +#endif + +/* + * OutgoingTransferRegistry + */ +const MonotonicDuration OutgoingTransferRegistry::MinEntryLifetime = MonotonicDuration::fromMSec(2000); + +TransferID* OutgoingTransferRegistry::accessOrCreate(const OutgoingTransferRegistryKey& key, + MonotonicTime new_deadline) +{ + UAVCAN_ASSERT(!new_deadline.isZero()); + Value* p = map_.access(key); + if (p == UAVCAN_NULLPTR) + { + p = map_.insert(key, Value()); + if (p == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + UAVCAN_TRACE("OutgoingTransferRegistry", "Created %s", key.toString().c_str()); + } + p->deadline = new_deadline; + return &p->tid; +} + +bool OutgoingTransferRegistry::exists(DataTypeID dtid, TransferType tt) const +{ + return UAVCAN_NULLPTR != map_.find(ExistenceCheckingPredicate(dtid, tt)); +} + +void OutgoingTransferRegistry::cleanup(MonotonicTime ts) +{ + map_.removeAllWhere(DeadlineExpiredPredicate(ts)); +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer.cpp new file mode 100644 index 000000000000..58778ef16883 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer.cpp @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ +/** + * TransferPriority + */ +const uint8_t TransferPriority::BitLen; +const uint8_t TransferPriority::NumericallyMax; +const uint8_t TransferPriority::NumericallyMin; + +const TransferPriority TransferPriority::Default((1U << BitLen) / 2); +const TransferPriority TransferPriority::MiddleLower((1U << BitLen) / 2 + (1U << BitLen) / 4); +const TransferPriority TransferPriority::OneHigherThanLowest(NumericallyMax - 1); +const TransferPriority TransferPriority::OneLowerThanHighest(NumericallyMin + 1); +const TransferPriority TransferPriority::Lowest(NumericallyMax); + +/** + * TransferID + */ +const uint8_t TransferID::BitLen; +const uint8_t TransferID::Max; +const uint8_t TransferID::Half; + +/** + * NodeID + */ +const uint8_t NodeID::ValueBroadcast; +const uint8_t NodeID::ValueInvalid; +const uint8_t NodeID::BitLen; +const uint8_t NodeID::Max; +const uint8_t NodeID::MaxRecommendedForRegularNodes; +const NodeID NodeID::Broadcast(ValueBroadcast); + +/** + * TransferID + */ +int TransferID::computeForwardDistance(TransferID rhs) const +{ + int d = int(rhs.get()) - int(get()); + if (d < 0) + { + d += 1 << BitLen; + } + + UAVCAN_ASSERT(((get() + d) & Max) == rhs.get()); + return d; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_buffer.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_buffer.cpp new file mode 100644 index 000000000000..5e670cf0ebec --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_buffer.cpp @@ -0,0 +1,362 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ +/* + * StaticTransferBufferImpl + */ +int StaticTransferBufferImpl::read(unsigned offset, uint8_t* data, unsigned len) const +{ + if (!data) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + if (offset >= max_write_pos_) + { + return 0; + } + if ((offset + len) > max_write_pos_) + { + len = max_write_pos_ - offset; + } + UAVCAN_ASSERT((offset + len) <= max_write_pos_); + (void)copy(data_ + offset, data_ + offset + len, data); + return int(len); +} + +int StaticTransferBufferImpl::write(unsigned offset, const uint8_t* data, unsigned len) +{ + if (!data) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + if (offset >= size_) + { + return 0; + } + if ((offset + len) > size_) + { + len = size_ - offset; + } + UAVCAN_ASSERT((offset + len) <= size_); + (void)copy(data, data + len, data_ + offset); + max_write_pos_ = max(uint16_t(offset + len), uint16_t(max_write_pos_)); + return int(len); +} + +void StaticTransferBufferImpl::reset() +{ + max_write_pos_ = 0; +#if UAVCAN_DEBUG + fill(data_, data_ + size_, uint8_t(0)); +#endif +} + +/* + * TransferBufferManagerKey + */ +#if UAVCAN_TOSTRING +std::string TransferBufferManagerKey::toString() const +{ + char buf[24]; + (void)snprintf(buf, sizeof(buf), "nid=%i tt=%i", int(node_id_.get()), int(transfer_type_)); + return std::string(buf); +} +#endif + +/* + * DynamicTransferBuffer::Block + */ +TransferBufferManagerEntry::Block* +TransferBufferManagerEntry::Block::instantiate(IPoolAllocator& allocator) +{ + void* const praw = allocator.allocate(sizeof(Block)); + if (praw == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + return new (praw) Block; +} + +void TransferBufferManagerEntry::Block::destroy(Block*& obj, IPoolAllocator& allocator) +{ + if (obj != UAVCAN_NULLPTR) + { + obj->~Block(); + allocator.deallocate(obj); + obj = UAVCAN_NULLPTR; + } +} + +void TransferBufferManagerEntry::Block::read(uint8_t*& outptr, unsigned target_offset, + unsigned& total_offset, unsigned& left_to_read) +{ + UAVCAN_ASSERT(outptr); + for (unsigned i = 0; (i < Block::Size) && (left_to_read > 0); i++, total_offset++) + { + if (total_offset >= target_offset) + { + *outptr++ = data[i]; + left_to_read--; + } + } +} + +void TransferBufferManagerEntry::Block::write(const uint8_t*& inptr, unsigned target_offset, + unsigned& total_offset, unsigned& left_to_write) +{ + UAVCAN_ASSERT(inptr); + for (unsigned i = 0; (i < Block::Size) && (left_to_write > 0); i++, total_offset++) + { + if (total_offset >= target_offset) + { + data[i] = *inptr++; + left_to_write--; + } + } +} + +/* + * DynamicTransferBuffer + */ +TransferBufferManagerEntry* TransferBufferManagerEntry::instantiate(IPoolAllocator& allocator, + uint16_t max_size) +{ + void* const praw = allocator.allocate(sizeof(TransferBufferManagerEntry)); + if (praw == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + return new (praw) TransferBufferManagerEntry(allocator, max_size); +} + +void TransferBufferManagerEntry::destroy(TransferBufferManagerEntry*& obj, IPoolAllocator& allocator) +{ + if (obj != UAVCAN_NULLPTR) + { + obj->~TransferBufferManagerEntry(); + allocator.deallocate(obj); + obj = UAVCAN_NULLPTR; + } +} + +int TransferBufferManagerEntry::read(unsigned offset, uint8_t* data, unsigned len) const +{ + if (!data) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + if (offset >= max_write_pos_) + { + return 0; + } + if ((offset + len) > max_write_pos_) + { + len = max_write_pos_ - offset; + } + UAVCAN_ASSERT((offset + len) <= max_write_pos_); + + // This shall be optimized. + unsigned total_offset = 0; + unsigned left_to_read = len; + uint8_t* outptr = data; + Block* p = blocks_.get(); + while (p) + { + p->read(outptr, offset, total_offset, left_to_read); + if (left_to_read == 0) + { + break; + } + p = p->getNextListNode(); + } + + UAVCAN_ASSERT(left_to_read == 0); + return int(len); +} + +int TransferBufferManagerEntry::write(unsigned offset, const uint8_t* data, unsigned len) +{ + if (!data) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + + if (offset >= max_size_) + { + return 0; + } + if ((offset + len) > max_size_) + { + len = max_size_ - offset; + } + UAVCAN_ASSERT((offset + len) <= max_size_); + + unsigned total_offset = 0; + unsigned left_to_write = len; + const uint8_t* inptr = data; + Block* p = blocks_.get(); + Block* last_written_block = UAVCAN_NULLPTR; + + // First we need to write the part that is already allocated + while (p) + { + last_written_block = p; + p->write(inptr, offset, total_offset, left_to_write); + if (left_to_write == 0) + { + break; + } + p = p->getNextListNode(); + } + + // Then we need to append new chunks until all data is written + while (left_to_write > 0) + { + // cppcheck-suppress nullPointer + UAVCAN_ASSERT(p == UAVCAN_NULLPTR); + + // Allocating the chunk + Block* new_block = Block::instantiate(allocator_); + if (new_block == UAVCAN_NULLPTR) + { + break; // We're in deep shit. + } + // Appending the chain with the new block + if (last_written_block != UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(last_written_block->getNextListNode() == UAVCAN_NULLPTR); // Because it is last in the chain + last_written_block->setNextListNode(new_block); + new_block->setNextListNode(UAVCAN_NULLPTR); + } + else + { + blocks_.insert(new_block); + } + last_written_block = new_block; + + // Writing the data + new_block->write(inptr, offset, total_offset, left_to_write); + } + + UAVCAN_ASSERT(len >= left_to_write); + const unsigned actually_written = len - left_to_write; + max_write_pos_ = max(uint16_t(offset + actually_written), uint16_t(max_write_pos_)); + return int(actually_written); +} + +void TransferBufferManagerEntry::reset(const TransferBufferManagerKey& key) +{ + key_ = key; + max_write_pos_ = 0; + Block* p = blocks_.get(); + while (p) + { + Block* const next = p->getNextListNode(); + blocks_.remove(p); + Block::destroy(p, allocator_); + p = next; + } +} + +/* + * TransferBufferManager + */ +TransferBufferManagerEntry* TransferBufferManager::findFirst(const TransferBufferManagerKey& key) +{ + TransferBufferManagerEntry* dyn = buffers_.get(); + while (dyn) + { + UAVCAN_ASSERT(!dyn->isEmpty()); + if (dyn->getKey() == key) + { + return dyn; + } + dyn = dyn->getNextListNode(); + } + return UAVCAN_NULLPTR; +} + +TransferBufferManager::~TransferBufferManager() +{ + TransferBufferManagerEntry* dyn = buffers_.get(); + while (dyn) + { + TransferBufferManagerEntry* const next = dyn->getNextListNode(); + buffers_.remove(dyn); + TransferBufferManagerEntry::destroy(dyn, allocator_); + dyn = next; + } +} + +ITransferBuffer* TransferBufferManager::access(const TransferBufferManagerKey& key) +{ + if (key.isEmpty()) + { + UAVCAN_ASSERT(0); + return UAVCAN_NULLPTR; + } + return findFirst(key); +} + +ITransferBuffer* TransferBufferManager::create(const TransferBufferManagerKey& key) +{ + if (key.isEmpty()) + { + UAVCAN_ASSERT(0); + return UAVCAN_NULLPTR; + } + remove(key); + + TransferBufferManagerEntry* tbme = TransferBufferManagerEntry::instantiate(allocator_, max_buf_size_); + if (tbme == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; // Epic fail. + } + + buffers_.insert(tbme); + + UAVCAN_TRACE("TransferBufferManager", "Buffer created [num=%u], %s", getNumBuffers(), key.toString().c_str()); + + if (tbme != UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(tbme->isEmpty()); + tbme->reset(key); + } + return tbme; +} + +void TransferBufferManager::remove(const TransferBufferManagerKey& key) +{ + UAVCAN_ASSERT(!key.isEmpty()); + + TransferBufferManagerEntry* dyn = findFirst(key); + if (dyn != UAVCAN_NULLPTR) + { + UAVCAN_TRACE("TransferBufferManager", "Buffer deleted, %s", key.toString().c_str()); + buffers_.remove(dyn); + TransferBufferManagerEntry::destroy(dyn, allocator_); + } +} + +bool TransferBufferManager::isEmpty() const +{ + return getNumBuffers() == 0; +} + +unsigned TransferBufferManager::getNumBuffers() const +{ + return buffers_.getLength(); +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_listener.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_listener.cpp new file mode 100644 index 000000000000..304e67126a66 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_listener.cpp @@ -0,0 +1,258 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include + +namespace uavcan +{ +/* + * IncomingTransfer + */ +int IncomingTransfer::write(unsigned, const uint8_t*, unsigned) +{ + UAVCAN_ASSERT(0); // Incoming transfer container is read-only + return -ErrLogic; +} + +/* + * SingleFrameIncomingTransfer + */ +SingleFrameIncomingTransfer::SingleFrameIncomingTransfer(const RxFrame& frm) + : IncomingTransfer(frm.getMonotonicTimestamp(), frm.getUtcTimestamp(), frm.getPriority(), + frm.getTransferType(), frm.getTransferID(), frm.getSrcNodeID(), frm.getIfaceIndex()) + , payload_(frm.getPayloadPtr()) + , payload_len_(uint8_t(frm.getPayloadLen())) +{ + UAVCAN_ASSERT(frm.isValid()); +} + +int SingleFrameIncomingTransfer::read(unsigned offset, uint8_t* data, unsigned len) const +{ + if (data == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + if (offset >= payload_len_) + { + return 0; + } + if ((offset + len) > payload_len_) + { + len = payload_len_ - offset; + } + UAVCAN_ASSERT((offset + len) <= payload_len_); + (void)copy(payload_ + offset, payload_ + offset + len, data); + return int(len); +} + +bool SingleFrameIncomingTransfer::isAnonymousTransfer() const +{ + return (getTransferType() == TransferTypeMessageBroadcast) && getSrcNodeID().isBroadcast(); +} + +/* + * MultiFrameIncomingTransfer + */ +MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc, + const RxFrame& last_frame, TransferBufferAccessor& tba) + : IncomingTransfer(ts_mono, ts_utc, last_frame.getPriority(), last_frame.getTransferType(), + last_frame.getTransferID(), last_frame.getSrcNodeID(), last_frame.getIfaceIndex()) + , buf_acc_(tba) +{ + UAVCAN_ASSERT(last_frame.isValid()); + UAVCAN_ASSERT(last_frame.isEndOfTransfer()); +} + +int MultiFrameIncomingTransfer::read(unsigned offset, uint8_t* data, unsigned len) const +{ + const ITransferBuffer* const tbb = const_cast(buf_acc_).access(); + if (tbb == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("MultiFrameIncomingTransfer", "Read failed: no such buffer"); + return -ErrLogic; + } + return tbb->read(offset, data, len); +} + +/* + * TransferListener::TimedOutReceiverPredicate + */ +bool TransferListener::TimedOutReceiverPredicate::operator()(const TransferBufferManagerKey& key, + const TransferReceiver& value) const +{ + if (value.isTimedOut(ts_)) + { + UAVCAN_TRACE("TransferListener", "Timed out receiver: %s", key.toString().c_str()); + /* + * TransferReceivers do not own their buffers - this helps the Map<> container to copy them + * around quickly and safely (using default assignment operator). Downside is that we need to + * destroy the buffers manually. + * Maybe it is not good that the predicate has side effects, but I ran out of better ideas. + */ + parent_bufmgr_.remove(key); + return true; + } + return false; +} + +/* + * TransferListener + */ +bool TransferListener::checkPayloadCrc(const uint16_t compare_with, const ITransferBuffer& tbb) const +{ + TransferCRC crc = crc_base_; + unsigned offset = 0; + while (true) + { + uint8_t buf[16]; + const int res = tbb.read(offset, buf, sizeof(buf)); + if (res < 0) + { + UAVCAN_TRACE("TransferListener", "Failed to check CRC: Buffer read failure %i", res); + return false; + } + if (res == 0) + { + break; + } + offset += unsigned(res); + crc.add(buf, unsigned(res)); + } + if (crc.get() != compare_with) + { + UAVCAN_TRACE("TransferListener", "CRC mismatch, expected=0x%04x, got=0x%04x", + int(compare_with), int(crc.get())); + return false; + } + return true; +} + +void TransferListener::handleReception(TransferReceiver& receiver, const RxFrame& frame, + TransferBufferAccessor& tba) +{ + switch (receiver.addFrame(frame, tba)) + { + case TransferReceiver::ResultNotComplete: + { + perf_.addErrors(receiver.yieldErrorCount()); + break; + } + case TransferReceiver::ResultSingleFrame: + { + perf_.addRxTransfer(); + SingleFrameIncomingTransfer it(frame); + handleIncomingTransfer(it); + break; + } + case TransferReceiver::ResultComplete: + { + perf_.addRxTransfer(); + const ITransferBuffer* tbb = tba.access(); + if (tbb == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("TransferListener", "Buffer access failure, last frame: %s", frame.toString().c_str()); + break; + } + if (!checkPayloadCrc(receiver.getLastTransferCrc(), *tbb)) + { + UAVCAN_TRACE("TransferListener", "CRC error, last frame: %s", frame.toString().c_str()); + break; + } + MultiFrameIncomingTransfer it(receiver.getLastTransferTimestampMonotonic(), + receiver.getLastTransferTimestampUtc(), frame, tba); + handleIncomingTransfer(it); + it.release(); + break; + } + default: + { + UAVCAN_ASSERT(0); + break; + } + } +} + +void TransferListener::handleAnonymousTransferReception(const RxFrame& frame) +{ + if (allow_anonymous_transfers_) + { + perf_.addRxTransfer(); + SingleFrameIncomingTransfer it(frame); + handleIncomingTransfer(it); + } +} + +TransferListener::~TransferListener() +{ + // Map must be cleared before bufmgr is destroyed + receivers_.clear(); +} + +void TransferListener::cleanup(MonotonicTime ts) +{ + receivers_.removeAllWhere(TimedOutReceiverPredicate(ts, bufmgr_)); + UAVCAN_ASSERT(receivers_.isEmpty() ? bufmgr_.isEmpty() : 1); +} + +void TransferListener::handleFrame(const RxFrame& frame) +{ + if (frame.getSrcNodeID().isUnicast()) // Normal transfer + { + const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType()); + + TransferReceiver* recv = receivers_.access(key); + if (recv == UAVCAN_NULLPTR) + { + if (!frame.isStartOfTransfer()) + { + return; + } + + TransferReceiver new_recv; + recv = receivers_.insert(key, new_recv); + if (recv == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str()); + return; + } + } + TransferBufferAccessor tba(bufmgr_, key); + handleReception(*recv, frame, tba); + } + else if (frame.getSrcNodeID().isBroadcast() && + frame.isStartOfTransfer() && + frame.isEndOfTransfer() && + frame.getDstNodeID().isBroadcast()) // Anonymous transfer + { + handleAnonymousTransferReception(frame); + } + else + { + UAVCAN_TRACE("TransferListener", "Invalid frame: %s", frame.toString().c_str()); // Invalid frame + } +} + +/* + * TransferListenerWithFilter + */ +void TransferListenerWithFilter::handleFrame(const RxFrame& frame) +{ + if (filter_ != UAVCAN_NULLPTR) + { + if (filter_->shouldAcceptFrame(frame)) + { + TransferListener::handleFrame(frame); + } + } + else + { + UAVCAN_ASSERT(0); + } +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_receiver.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_receiver.cpp new file mode 100644 index 000000000000..d60e92861d12 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_receiver.cpp @@ -0,0 +1,248 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include + +namespace uavcan +{ + +const uint16_t TransferReceiver::MinTransferIntervalMSec; +const uint16_t TransferReceiver::MaxTransferIntervalMSec; +const uint16_t TransferReceiver::DefaultTransferIntervalMSec; +const uint16_t TransferReceiver::DefaultTidTimeoutMSec; + +MonotonicDuration TransferReceiver::getIfaceSwitchDelay() const +{ + return MonotonicDuration::fromMSec(transfer_interval_msec_); +} + +MonotonicDuration TransferReceiver::getTidTimeout() const +{ + return MonotonicDuration::fromMSec(DefaultTidTimeoutMSec); +} + +void TransferReceiver::registerError() const +{ + error_cnt_ = static_cast(error_cnt_ + 1) & ErrorCntMask; +} + +void TransferReceiver::updateTransferTimings() +{ + UAVCAN_ASSERT(!this_transfer_ts_.isZero()); + + const MonotonicTime prev_prev_ts = prev_transfer_ts_; + prev_transfer_ts_ = this_transfer_ts_; + + if ((!prev_prev_ts.isZero()) && (!prev_transfer_ts_.isZero()) && (prev_transfer_ts_ >= prev_prev_ts)) + { + uint64_t interval_msec = uint64_t((prev_transfer_ts_ - prev_prev_ts).toMSec()); + interval_msec = min(interval_msec, uint64_t(MaxTransferIntervalMSec)); + interval_msec = max(interval_msec, uint64_t(MinTransferIntervalMSec)); + transfer_interval_msec_ = static_cast((uint64_t(transfer_interval_msec_) * 7U + interval_msec) / 8U); + } +} + +void TransferReceiver::prepareForNextTransfer() +{ + tid_.increment(); + next_toggle_ = false; + buffer_write_pos_ = 0; +} + +bool TransferReceiver::validate(const RxFrame& frame) const +{ + if (iface_index_ != frame.getIfaceIndex()) + { + return false; + } + if (frame.isStartOfTransfer() && !frame.isEndOfTransfer() && (frame.getPayloadLen() < TransferCRC::NumBytes)) + { + UAVCAN_TRACE("TransferReceiver", "CRC expected, %s", frame.toString().c_str()); + registerError(); + return false; + } + if (frame.isStartOfTransfer() && frame.getToggle()) + { + UAVCAN_TRACE("TransferReceiver", "Toggle bit is not cleared, %s", frame.toString().c_str()); + registerError(); + return false; + } + if (frame.isStartOfTransfer() && isMidTransfer()) + { + UAVCAN_TRACE("TransferReceiver", "Unexpected start of transfer, %s", frame.toString().c_str()); + registerError(); + } + if (frame.getToggle() != next_toggle_) + { + UAVCAN_TRACE("TransferReceiver", "Unexpected toggle bit (not %i), %s", + int(next_toggle_), frame.toString().c_str()); + registerError(); + return false; + } + if (frame.getTransferID() != tid_) + { + UAVCAN_TRACE("TransferReceiver", "Unexpected TID (current %i), %s", tid_.get(), frame.toString().c_str()); + registerError(); + return false; + } + return true; +} + +bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf) +{ + const uint8_t* const payload = frame.getPayloadPtr(); + const unsigned payload_len = frame.getPayloadLen(); + + if (frame.isStartOfTransfer()) // First frame contains CRC, we need to extract it now + { + if (frame.getPayloadLen() < TransferCRC::NumBytes) + { + return false; // Must have been validated earlier though. I think I'm paranoid. + } + this_transfer_crc_ = static_cast(payload[0] & 0xFF); + this_transfer_crc_ |= static_cast(static_cast(payload[1] & 0xFF) << 8); // Little endian. + + const unsigned effective_payload_len = payload_len - TransferCRC::NumBytes; + const int res = buf.write(buffer_write_pos_, payload + TransferCRC::NumBytes, effective_payload_len); + const bool success = res == static_cast(effective_payload_len); + if (success) + { + buffer_write_pos_ = static_cast(buffer_write_pos_ + effective_payload_len); + } + return success; + } + else + { + const int res = buf.write(buffer_write_pos_, payload, payload_len); + const bool success = res == static_cast(payload_len); + if (success) + { + buffer_write_pos_ = static_cast(buffer_write_pos_ + payload_len); + } + return success; + } +} + +TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, TransferBufferAccessor& tba) +{ + // Transfer timestamps are derived from the first frame + if (frame.isStartOfTransfer()) + { + this_transfer_ts_ = frame.getMonotonicTimestamp(); + first_frame_ts_ = frame.getUtcTimestamp(); + } + + if (frame.isStartOfTransfer() && frame.isEndOfTransfer()) + { + tba.remove(); + updateTransferTimings(); + prepareForNextTransfer(); + this_transfer_crc_ = 0; // SFT has no CRC + return ResultSingleFrame; + } + + // Payload write + ITransferBuffer* buf = tba.access(); + if (buf == UAVCAN_NULLPTR) + { + buf = tba.create(); + } + if (buf == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("TransferReceiver", "Failed to access the buffer, %s", frame.toString().c_str()); + prepareForNextTransfer(); + registerError(); + return ResultNotComplete; + } + if (!writePayload(frame, *buf)) + { + UAVCAN_TRACE("TransferReceiver", "Payload write failed, %s", frame.toString().c_str()); + tba.remove(); + prepareForNextTransfer(); + registerError(); + return ResultNotComplete; + } + next_toggle_ = !next_toggle_; + + if (frame.isEndOfTransfer()) + { + updateTransferTimings(); + prepareForNextTransfer(); + return ResultComplete; + } + return ResultNotComplete; +} + +bool TransferReceiver::isTimedOut(MonotonicTime current_ts) const +{ + return (current_ts - this_transfer_ts_) > getTidTimeout(); +} + +TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, TransferBufferAccessor& tba) +{ + if ((frame.getMonotonicTimestamp().isZero()) || + (frame.getMonotonicTimestamp() < prev_transfer_ts_) || + (frame.getMonotonicTimestamp() < this_transfer_ts_)) + { + UAVCAN_TRACE("TransferReceiver", "Invalid frame, %s", frame.toString().c_str()); + return ResultNotComplete; + } + + const bool not_initialized = !isInitialized(); + const bool tid_timed_out = isTimedOut(frame.getMonotonicTimestamp()); + const bool same_iface = frame.getIfaceIndex() == iface_index_; + const bool first_frame = frame.isStartOfTransfer(); + const bool non_wrapped_tid = tid_.computeForwardDistance(frame.getTransferID()) < TransferID::Half; + const bool not_previous_tid = frame.getTransferID().computeForwardDistance(tid_) > 1; + const bool iface_switch_allowed = (frame.getMonotonicTimestamp() - this_transfer_ts_) > getIfaceSwitchDelay(); + + // FSM, the hard way + const bool need_restart = + (not_initialized) || + (tid_timed_out) || + (same_iface && first_frame && not_previous_tid) || + (iface_switch_allowed && first_frame && non_wrapped_tid); + + if (need_restart) + { + if (!not_initialized && (tid_ != frame.getTransferID())) + { + registerError(); + } + UAVCAN_TRACE("TransferReceiver", "Restart [ni=%d, isa=%d, tt=%d, si=%d, ff=%d, nwtid=%d, nptid=%d, tid=%d], %s", + int(not_initialized), int(iface_switch_allowed), int(tid_timed_out), int(same_iface), + int(first_frame), int(non_wrapped_tid), int(not_previous_tid), int(tid_.get()), + frame.toString().c_str()); + tba.remove(); + iface_index_ = frame.getIfaceIndex() & IfaceIndexMask; + tid_ = frame.getTransferID(); + next_toggle_ = false; + buffer_write_pos_ = 0; + this_transfer_crc_ = 0; + if (!first_frame) + { + tid_.increment(); + return ResultNotComplete; + } + } + + if (!validate(frame)) + { + return ResultNotComplete; + } + return receive(frame, tba); +} + +uint8_t TransferReceiver::yieldErrorCount() +{ + const uint8_t ret = error_cnt_; + error_cnt_ = 0; + return ret; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_sender.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_sender.cpp new file mode 100644 index 000000000000..f5d69d9b9ed0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_sender.cpp @@ -0,0 +1,173 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + + +namespace uavcan +{ + +void TransferSender::registerError() const +{ + dispatcher_.getTransferPerfCounter().addError(); +} + +void TransferSender::init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos) +{ + UAVCAN_ASSERT(!isInitialized()); + + qos_ = qos; + data_type_id_ = dtid.getID(); + crc_base_ = dtid.getSignature().toTransferCRC(); +} + +int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline, + MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id, + TransferID tid) const +{ + Frame frame(data_type_id_, transfer_type, dispatcher_.getNodeID(), dst_node_id, tid); + + frame.setPriority(priority_); + frame.setStartOfTransfer(true); + + UAVCAN_TRACE("TransferSender", "%s", frame.toString().c_str()); + + /* + * Checking if we're allowed to send. + * In passive mode we can send only anonymous transfers, if they are enabled. + */ + if (dispatcher_.isPassiveMode()) + { + const bool allow = allow_anonymous_transfers_ && + (transfer_type == TransferTypeMessageBroadcast) && + (int(payload_len) <= frame.getPayloadCapacity()); + if (!allow) + { + return -ErrPassiveMode; + } + } + + dispatcher_.getTransferPerfCounter().addTxTransfer(); + + /* + * Sending frames + */ + if (frame.getPayloadCapacity() >= payload_len) // Single Frame Transfer + { + const int res = frame.setPayload(payload, payload_len); + if (res != int(payload_len)) + { + UAVCAN_ASSERT(0); + UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", res); + registerError(); + return (res < 0) ? res : -ErrLogic; + } + + frame.setEndOfTransfer(true); + UAVCAN_ASSERT(frame.isStartOfTransfer() && frame.isEndOfTransfer() && !frame.getToggle()); + + const CanIOFlags flags = frame.getSrcNodeID().isUnicast() ? flags_ : (flags_ | CanIOFlagAbortOnError); + + return dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags, iface_mask_); + } + else // Multi Frame Transfer + { + UAVCAN_ASSERT(!dispatcher_.isPassiveMode()); + UAVCAN_ASSERT(frame.getSrcNodeID().isUnicast()); + + int offset = 0; + { + TransferCRC crc = crc_base_; + crc.add(payload, payload_len); + + static const int BUFLEN = sizeof(static_cast(0)->data); + uint8_t buf[BUFLEN]; + + buf[0] = uint8_t(crc.get() & 0xFFU); // Transfer CRC, little endian + buf[1] = uint8_t((crc.get() >> 8) & 0xFF); + (void)copy(payload, payload + BUFLEN - 2, buf + 2); + + const int write_res = frame.setPayload(buf, BUFLEN); + if (write_res < 2) + { + UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res); + registerError(); + return write_res; + } + offset = write_res - 2; + UAVCAN_ASSERT(int(payload_len) > offset); + } + + int num_sent = 0; + + while (true) + { + const int send_res = dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags_, iface_mask_); + if (send_res < 0) + { + registerError(); + return send_res; + } + + num_sent++; + if (frame.isEndOfTransfer()) + { + return num_sent; // Number of frames transmitted + } + + frame.setStartOfTransfer(false); + frame.flipToggle(); + + UAVCAN_ASSERT(offset >= 0); + const int write_res = frame.setPayload(payload + offset, payload_len - unsigned(offset)); + if (write_res < 0) + { + UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res); + registerError(); + return write_res; + } + + offset += write_res; + UAVCAN_ASSERT(offset <= int(payload_len)); + if (offset >= int(payload_len)) + { + frame.setEndOfTransfer(true); + } + } + } + + UAVCAN_ASSERT(0); + return -ErrLogic; // Return path analysis is apparently broken. There should be no warning, this 'return' is unreachable. +} + +int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline, + MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id) const +{ + /* + * TODO: TID is not needed for anonymous transfers, this part of the code can be skipped? + */ + const OutgoingTransferRegistryKey otr_key(data_type_id_, transfer_type, dst_node_id); + + UAVCAN_ASSERT(!tx_deadline.isZero()); + const MonotonicTime otr_deadline = tx_deadline + max(max_transfer_interval_ * 2, + OutgoingTransferRegistry::MinEntryLifetime); + + TransferID* const tid = dispatcher_.getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline); + if (tid == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("TransferSender", "OTR access failure, dtid=%d tt=%i", + int(data_type_id_.get()), int(transfer_type)); + return -ErrMemory; + } + + const TransferID this_tid = tid->get(); + tid->increment(); + + return send(payload, payload_len, tx_deadline, blocking_deadline, transfer_type, + dst_node_id, this_tid); +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/uc_data_type.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/uc_data_type.cpp new file mode 100644 index 000000000000..5f605d60ff2e --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/uc_data_type.cpp @@ -0,0 +1,157 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include + +namespace uavcan +{ +/* + * DataTypeID + */ +const uint16_t DataTypeID::MaxServiceDataTypeIDValue; +const uint16_t DataTypeID::MaxMessageDataTypeIDValue; +const uint16_t DataTypeID::MaxPossibleDataTypeIDValue; + +DataTypeID DataTypeID::getMaxValueForDataTypeKind(const DataTypeKind dtkind) +{ + if (dtkind == DataTypeKindService) + { + return MaxServiceDataTypeIDValue; + } + else if (dtkind == DataTypeKindMessage) + { + return MaxMessageDataTypeIDValue; + } + else + { + UAVCAN_ASSERT(0); + return DataTypeID(0); + } +} + +/* + * DataTypeSignatureCRC + */ +DataTypeSignatureCRC DataTypeSignatureCRC::extend(uint64_t crc) +{ + DataTypeSignatureCRC ret; + ret.crc_ = crc ^ 0xFFFFFFFFFFFFFFFF; + return ret; +} + +void DataTypeSignatureCRC::add(uint8_t byte) +{ + static const uint64_t Poly = 0x42F0E1EBA9EA3693; + crc_ ^= uint64_t(byte) << 56; + for (int i = 0; i < 8; i++) + { + crc_ = (crc_ & (uint64_t(1) << 63)) ? (crc_ << 1) ^ Poly : crc_ << 1; + } +} + +void DataTypeSignatureCRC::add(const uint8_t* bytes, unsigned len) +{ + UAVCAN_ASSERT(bytes); + while (len--) + { + add(*bytes++); + } +} + +/* + * DataTypeSignature + */ +void DataTypeSignature::mixin64(uint64_t x) +{ + DataTypeSignatureCRC crc = DataTypeSignatureCRC::extend(value_); + for (int i = 0; i < 64; i += 8) // LSB first + { + crc.add((x >> i) & 0xFF); + } + value_ = crc.get(); +} + +void DataTypeSignature::extend(DataTypeSignature dts) +{ + const uint64_t y = value_; + mixin64(dts.get()); + mixin64(y); +} + +TransferCRC DataTypeSignature::toTransferCRC() const +{ + TransferCRC tcrc; + for (int i = 0; i < 64; i += 8) // LSB first + { + tcrc.add((value_ >> i) & 0xFF); + } + return tcrc; +} + +/* + * DataTypeDescriptor + */ +const unsigned DataTypeDescriptor::MaxFullNameLen; + +bool DataTypeDescriptor::isValid() const +{ + return id_.isValidForDataTypeKind(kind_) && + (full_name_ != UAVCAN_NULLPTR) && + (*full_name_ != '\0'); +} + +bool DataTypeDescriptor::match(DataTypeKind kind, const char* name) const +{ + return (kind_ == kind) && !std::strncmp(full_name_, name, MaxFullNameLen); +} + +bool DataTypeDescriptor::match(DataTypeKind kind, DataTypeID id) const +{ + return (kind_ == kind) && (id_ == id); +} + +#if UAVCAN_TOSTRING +std::string DataTypeDescriptor::toString() const +{ + char kindch = '?'; + switch (kind_) + { + case DataTypeKindMessage: + { + kindch = 'm'; + break; + } + case DataTypeKindService: + { + kindch = 's'; + break; + } + default: + { + UAVCAN_ASSERT(0); + break; + } + } + + char buf[128]; + (void)snprintf(buf, sizeof(buf), "%s:%u%c:%016llx", + full_name_, static_cast(id_.get()), kindch, + static_cast(signature_.get())); + return std::string(buf); +} +#endif + +bool DataTypeDescriptor::operator==(const DataTypeDescriptor& rhs) const +{ + return + (kind_ == rhs.kind_) && + (id_ == rhs.id_) && + (signature_ == rhs.signature_) && + !std::strncmp(full_name_, rhs.full_name_, MaxFullNameLen); +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/uc_dynamic_memory.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/uc_dynamic_memory.cpp new file mode 100644 index 000000000000..e806d5de9eaf --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/uc_dynamic_memory.cpp @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ +/* + * LimitedPoolAllocator + */ +void* LimitedPoolAllocator::allocate(std::size_t size) +{ + if (used_blocks_ < max_blocks_) + { + used_blocks_++; + return allocator_.allocate(size); + } + else + { + return UAVCAN_NULLPTR; + } +} + +void LimitedPoolAllocator::deallocate(const void* ptr) +{ + allocator_.deallocate(ptr); + + UAVCAN_ASSERT(used_blocks_ > 0); + if (used_blocks_ > 0) + { + used_blocks_--; + } +} + +uint16_t LimitedPoolAllocator::getBlockCapacity() const +{ + return min(max_blocks_, allocator_.getBlockCapacity()); +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/uc_error.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/uc_error.cpp new file mode 100644 index 000000000000..59b1aad88b26 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/uc_error.cpp @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +#ifndef UAVCAN_EXCEPTIONS +# error UAVCAN_EXCEPTIONS +#endif + +#if UAVCAN_EXCEPTIONS +# include +#endif + +namespace uavcan +{ + +void handleFatalError(const char* msg) +{ +#if UAVCAN_EXCEPTIONS + throw std::runtime_error(msg); +#else + (void)msg; + UAVCAN_ASSERT(0); + std::abort(); +#endif +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/clock.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/clock.hpp new file mode 100644 index 000000000000..623321bae7a1 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/clock.hpp @@ -0,0 +1,106 @@ +/* + * Copyright (C) 2014 + */ + +#pragma once + +#include +#include +#include +#include + +class SystemClockMock : public uavcan::ISystemClock +{ +public: + mutable uint64_t monotonic; + mutable uint64_t utc; + uavcan::UtcDuration last_adjustment; + uint64_t monotonic_auto_advance; + bool preserve_utc; + + SystemClockMock(uint64_t initial = 0) + : monotonic(initial) + , utc(initial) + , monotonic_auto_advance(0) + , preserve_utc(false) + { } + + void advance(uint64_t usec) const + { + monotonic += usec; + if (!preserve_utc) + { + utc += usec; + } + } + + virtual uavcan::MonotonicTime getMonotonic() const + { + assert(this); + const uint64_t res = monotonic; + advance(monotonic_auto_advance); + return uavcan::MonotonicTime::fromUSec(res); + } + + virtual uavcan::UtcTime getUtc() const + { + assert(this); + return uavcan::UtcTime::fromUSec(utc); + } + + virtual void adjustUtc(uavcan::UtcDuration adjustment) + { + assert(this); + const uint64_t prev_utc = utc; + utc = uint64_t(int64_t(utc) + adjustment.toUSec()); + last_adjustment = adjustment; + std::cout << "Clock adjustment " << prev_utc << " --> " << utc << std::endl; + } +}; + + +class SystemClockDriver : public uavcan::ISystemClock +{ +public: + uavcan::UtcDuration utc_adjustment; + + virtual uavcan::MonotonicTime getMonotonic() const + { + struct timespec ts; + const int ret = clock_gettime(CLOCK_MONOTONIC, &ts); + if (ret != 0) + { + assert(0); + return uavcan::MonotonicTime(); + } + return uavcan::MonotonicTime::fromUSec(uint64_t(int64_t(ts.tv_sec) * 1000000L + int64_t(ts.tv_nsec / 1000L))); + } + + virtual uavcan::UtcTime getUtc() const + { + struct timeval tv; + const int ret = gettimeofday(&tv, UAVCAN_NULLPTR); + if (ret != 0) + { + assert(0); + return uavcan::UtcTime(); + } + return uavcan::UtcTime::fromUSec(uint64_t(int64_t(tv.tv_sec) * 1000000L + tv.tv_usec)) + utc_adjustment; + } + + virtual void adjustUtc(uavcan::UtcDuration adjustment) + { + utc_adjustment += adjustment; + } +}; + +inline uavcan::MonotonicTime tsMono(uint64_t usec) { return uavcan::MonotonicTime::fromUSec(usec); } +inline uavcan::UtcTime tsUtc(uint64_t usec) { return uavcan::UtcTime::fromUSec(usec); } + +inline uavcan::MonotonicDuration durMono(int64_t usec) { return uavcan::MonotonicDuration::fromUSec(usec); } + +template +static bool areTimestampsClose(const T& a, const T& b, int64_t precision_usec = 100000) +{ + return (a - b).getAbs().toUSec() < precision_usec; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/data_type.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/data_type.cpp new file mode 100644 index 000000000000..a95bdbb32f17 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/data_type.cpp @@ -0,0 +1,162 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + + +TEST(DataTypeSignatureCRC, Correctness) +{ + uavcan::DataTypeSignatureCRC crc; + + ASSERT_EQ(0xFFFFFFFFFFFFFFFF ^ 0xFFFFFFFFFFFFFFFF, crc.get()); + + crc.add('1'); + crc.add('2'); + crc.add('3'); + crc.add(reinterpret_cast("456789"), 6); + + ASSERT_EQ(0x62EC59E3F1A4F00A, crc.get()); +} + + +TEST(DataTypeSignatureCRC, Extension) +{ + uavcan::DataTypeSignatureCRC crc1; + + crc1.add('1'); + crc1.add('2'); + crc1.add('3'); + + uavcan::DataTypeSignatureCRC crc2 = uavcan::DataTypeSignatureCRC::extend(crc1.get()); + + crc2.add(reinterpret_cast("456789"), 6); + + ASSERT_EQ(0x62EC59E3F1A4F00A, crc2.get()); +} + + +TEST(DataTypeSignature, Correctness) +{ + using uavcan::DataTypeSignature; + using uavcan::DataTypeSignatureCRC; + + DataTypeSignature signature; + ASSERT_EQ(0, signature.get()); + + /* + * First extension + */ + signature.extend(DataTypeSignature(0x123456789abcdef0)); + + DataTypeSignatureCRC crc; + crc.add(0xF0); + crc.add(0xDE); + crc.add(0xBC); + crc.add(0x9A); + crc.add(0x78); + crc.add(0x56); + crc.add(0x34); + crc.add(0x12); + for (int i = 0; i < 8; i++) + { + crc.add(0); + } + + ASSERT_EQ(crc.get(), signature.get()); + + const uint64_t old_signature = signature.get(); + + /* + * Second extension + */ + signature.extend(DataTypeSignature(0xfedcba9876543210)); + crc.add(0x10); + crc.add(0x32); + crc.add(0x54); + crc.add(0x76); + crc.add(0x98); + crc.add(0xba); + crc.add(0xdc); + crc.add(0xfe); + for (int i = 0; i < 64; i += 8) + { + crc.add((old_signature >> i) & 0xFF); + } + + ASSERT_EQ(crc.get(), signature.get()); + + /* + * Comparison + */ + ASSERT_TRUE(signature == DataTypeSignature(signature.get())); + ASSERT_FALSE(signature == DataTypeSignature()); + ASSERT_FALSE(signature != DataTypeSignature(signature.get())); + ASSERT_TRUE(signature != DataTypeSignature()); +} + + +TEST(DataTypeDescriptor, ToString) +{ + uavcan::DataTypeDescriptor desc; + ASSERT_EQ(":65535s:0000000000000000", desc.toString()); + + desc = uavcan::DataTypeDescriptor(uavcan::DataTypeKindMessage, 123, + uavcan::DataTypeSignature(0xdeadbeef1234), "Bar"); + ASSERT_EQ("Bar:123m:0000deadbeef1234", desc.toString()); + + // Max length - 80 chars + desc = uavcan::DataTypeDescriptor(uavcan::DataTypeKindMessage, 1023, uavcan::DataTypeSignature(0xdeadbeef12345678), + "sirius_cybernetics_corporation.marvin.model_a.LongDataTypeName123456789abcdefghi"); + ASSERT_EQ("sirius_cybernetics_corporation.marvin.model_a.LongDataTypeName123456789abcdefghi:1023m:deadbeef12345678", + desc.toString()); +} + + +TEST(DataTypeDescriptor, Match) +{ + uavcan::DataTypeDescriptor desc(uavcan::DataTypeKindMessage, 123, + uavcan::DataTypeSignature(0xdeadbeef1234), "namespace.TypeName"); + ASSERT_TRUE(desc.match(uavcan::DataTypeKindMessage, "namespace.TypeName")); + ASSERT_FALSE(desc.match(uavcan::DataTypeKindMessage, "boo")); + ASSERT_FALSE(desc.match(uavcan::DataTypeKindService, "namespace.TypeName")); +} + + +TEST(DataTypeID, Basic) +{ + uavcan::DataTypeID id; + + ASSERT_EQ(0xFFFF, id.get()); + ASSERT_FALSE(id.isValidForDataTypeKind(uavcan::DataTypeKindMessage)); + ASSERT_FALSE(id.isValidForDataTypeKind(uavcan::DataTypeKindService)); + + id = 123; + uavcan::DataTypeID id2 = 255; + + ASSERT_EQ(123, id.get()); + ASSERT_EQ(255, id2.get()); + + ASSERT_TRUE(id.isValidForDataTypeKind(uavcan::DataTypeKindMessage)); + ASSERT_TRUE(id.isValidForDataTypeKind(uavcan::DataTypeKindService)); + ASSERT_TRUE(id2.isValidForDataTypeKind(uavcan::DataTypeKindMessage)); + ASSERT_TRUE(id2.isValidForDataTypeKind(uavcan::DataTypeKindService)); + + ASSERT_TRUE(id < id2); + ASSERT_TRUE(id <= id2); + ASSERT_TRUE(id2 > id); + ASSERT_TRUE(id2 >= id); + ASSERT_TRUE(id != id2); + + id = id2; + ASSERT_FALSE(id < id2); + ASSERT_TRUE(id <= id2); + ASSERT_FALSE(id2 > id); + ASSERT_TRUE(id2 >= id); + ASSERT_TRUE(id == id2); + + id = 1024; + ASSERT_TRUE(id.isValidForDataTypeKind(uavcan::DataTypeKindMessage)); + ASSERT_FALSE(id.isValidForDataTypeKind(uavcan::DataTypeKindService)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_const_1.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_const_1.cpp new file mode 100644 index 000000000000..593b9ade9a76 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_const_1.cpp @@ -0,0 +1,18 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +/* + * Objective of this test is to make sure that the generated types are being linked correcly. + * This test requests the address of some static const member variables to make sure that there + * will be no duplicated symbol linking errors. + */ +TEST(DsdlConst1, Timestamp) +{ + using uavcan::Timestamp; + + std::cout << &Timestamp::UNKNOWN << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_const_2.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_const_2.cpp new file mode 100644 index 000000000000..477db1fa75e2 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_const_2.cpp @@ -0,0 +1,14 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + + +TEST(DsdlConst2, Timestamp) +{ + using uavcan::Timestamp; + + std::cout << &Timestamp::UNKNOWN << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_test.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_test.cpp new file mode 100644 index 000000000000..fdaaac4859d1 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_test.cpp @@ -0,0 +1,152 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +TEST(Dsdl, EmptyServices) +{ + uavcan::StaticTransferBuffer<100> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + root_ns_b::ServiceWithEmptyRequest::Request req; + ASSERT_EQ(1, root_ns_b::ServiceWithEmptyRequest::Request::encode(req, sc_wr)); + ASSERT_EQ("", bs_wr.toString()); + + root_ns_b::ServiceWithEmptyRequest::Response resp; + ASSERT_EQ(1, root_ns_b::ServiceWithEmptyRequest::Response::encode(resp, sc_wr)); + ASSERT_EQ("", bs_wr.toString()); + + resp.covariance.push_back(-2); + resp.covariance.push_back(65504); + root_ns_b::ServiceWithEmptyRequest::Response::encode(resp, sc_wr); + ASSERT_EQ("00000000 11000000 11111111 01111011", bs_wr.toString()); + + resp.covariance.push_back(42); + resp.covariance[0] = 999; + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + ASSERT_EQ(1, root_ns_b::ServiceWithEmptyRequest::Response::decode(resp, sc_rd)); + + ASSERT_EQ(2, resp.covariance.size()); + ASSERT_EQ(-2, resp.covariance[0]); + ASSERT_EQ(65504, resp.covariance[1]); +} + + +TEST(Dsdl, Signature) +{ + ASSERT_EQ(0xe74617107a34aa9c, root_ns_a::EmptyService::getDataTypeSignature().get()); + ASSERT_STREQ("root_ns_a.EmptyService", root_ns_a::EmptyService::getDataTypeFullName()); + ASSERT_EQ(uavcan::DataTypeKindService, root_ns_a::EmptyService::DataTypeKind); + + ASSERT_EQ(0x99604d7066e0d713, root_ns_a::NestedMessage::getDataTypeSignature().get()); // Computed manually + ASSERT_STREQ("root_ns_a.NestedMessage", root_ns_a::NestedMessage::getDataTypeFullName()); + ASSERT_EQ(uavcan::DataTypeKindMessage, root_ns_a::NestedMessage::DataTypeKind); +} + + +TEST(Dsdl, Operators) +{ + { + root_ns_a::EmptyService::Request a, b; + ASSERT_TRUE(a == b); + ASSERT_FALSE(a != b); + } + { + root_ns_a::NestedMessage c, d; + ASSERT_TRUE(c == d); + ASSERT_FALSE(c != d); + + c.field = 1; + ASSERT_FALSE(c == d); + ASSERT_TRUE(c != d); + } +} + + +TEST(Dsdl, CloseComparison) +{ + root_ns_a::A first, second; + + ASSERT_TRUE(first == second); + + first.vector[1].vector[1] = std::numeric_limits::epsilon(); + ASSERT_TRUE(first.isClose(second)); // Still close + ASSERT_FALSE(first == second); // But not exactly + + first.vector[1].vector[1] = std::numeric_limits::epsilon(); + ASSERT_FALSE(first.isClose(second)); // Nope + ASSERT_FALSE(first == second); // Ditto +} + +/* + * This test assumes that it will be executed before other GDTR tests; otherwise it fails. + * TODO: Probably it needs to be called directly from main() + */ +//TEST(Dsdl, Registration) +//{ +// using uavcan::GlobalDataTypeRegistry; +// /* +// * Descriptors +// */ +// const uavcan::DataTypeDescriptor* desc = UAVCAN_NULLPTR; +// +// desc = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, "root_ns_a.EmptyMessage"); +// ASSERT_TRUE(desc); +// ASSERT_EQ(root_ns_a::EmptyMessage::DefaultDataTypeID, desc->getID()); +// ASSERT_EQ(root_ns_a::EmptyMessage::DataTypeKind, desc->getKind()); +// ASSERT_EQ(root_ns_a::EmptyMessage::getDataTypeSignature(), desc->getSignature()); +// ASSERT_STREQ(root_ns_a::EmptyMessage::getDataTypeFullName(), desc->getFullName()); +// +// desc = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, "root_ns_a.EmptyService"); +// ASSERT_TRUE(desc); +// ASSERT_EQ(root_ns_a::EmptyService::DefaultDataTypeID, desc->getID()); +// ASSERT_EQ(root_ns_a::EmptyService::DataTypeKind, desc->getKind()); +// ASSERT_EQ(root_ns_a::EmptyService::getDataTypeSignature(), desc->getSignature()); +// ASSERT_STREQ(root_ns_a::EmptyService::getDataTypeFullName(), desc->getFullName()); +// +// desc = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, "root_ns_a.ReportBackSoldier"); +// ASSERT_TRUE(desc); +// ASSERT_EQ(root_ns_a::ReportBackSoldier::DefaultDataTypeID, desc->getID()); +// ASSERT_EQ(root_ns_a::ReportBackSoldier::DataTypeKind, desc->getKind()); +// ASSERT_EQ(root_ns_a::ReportBackSoldier::getDataTypeSignature(), desc->getSignature()); +// ASSERT_STREQ(root_ns_a::ReportBackSoldier::getDataTypeFullName(), desc->getFullName()); +// +// /* +// * Mask +// */ +// uavcan::DataTypeIDMask mask; +// +// GlobalDataTypeRegistry::instance().getDataTypeIDMask(uavcan::DataTypeKindMessage, mask); +// ASSERT_TRUE(mask[8]); +// mask[8] = false; +// +// GlobalDataTypeRegistry::instance().getDataTypeIDMask(uavcan::DataTypeKindService, mask); +// ASSERT_TRUE(mask[1]); +// ASSERT_TRUE(mask[3]); +// mask[1] = mask[3] = false; +// +// /* +// * Reset +// */ +// GlobalDataTypeRegistry::instance().reset(); +// ASSERT_FALSE(GlobalDataTypeRegistry::instance().isFrozen()); +//} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_uavcan_compilability.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_uavcan_compilability.cpp new file mode 100644 index 000000000000..079ab96e294b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_uavcan_compilability.cpp @@ -0,0 +1,292 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +template +static bool validateYaml(const T& obj, const std::string& reference) +{ + uavcan::OStream::instance() << "Validating YAML:\n" << obj << "\n" << uavcan::OStream::endl; + + std::ostringstream os; + os << obj; + if (os.str() == reference) + { + return true; + } + else + { + std::cout << "INVALID YAML:\n" + << "EXPECTED:\n" + << "===\n" + << reference + << "\n===\n" + << "ACTUAL:\n" + << "\n===\n" + << os.str() + << "\n===\n" << std::endl; + return false; + } +} + +TEST(Dsdl, Streaming) +{ + EXPECT_TRUE(validateYaml(uavcan::protocol::GetNodeInfo::Response(), + "status: \n" + " uptime_sec: 0\n" + " health: 0\n" + " mode: 0\n" + " sub_mode: 0\n" + " vendor_specific_status_code: 0\n" + "software_version: \n" + " major: 0\n" + " minor: 0\n" + " optional_field_flags: 0\n" + " vcs_commit: 0\n" + " image_crc: 0\n" + "hardware_version: \n" + " major: 0\n" + " minor: 0\n" + " unique_id: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n" + " certificate_of_authenticity: \"\"\n" + "name: \"\"")); + + root_ns_a::Deep ps; + ps.a.resize(1); + EXPECT_TRUE(validateYaml(ps, + "c: 0\n" + "str: \"\"\n" + "a: \n" + " - \n" + " scalar: 0\n" + " vector: \n" + " - \n" + " vector: [0, 0]\n" + " bools: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n" + " - \n" + " vector: [0, 0]\n" + " bools: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n" + "b: \n" + " - \n" + " vector: [0, 0]\n" + " bools: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n" + " - \n" + " vector: [0, 0]\n" + " bools: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]")); +} + + +template +static bool encodeDecodeValidate(const T& obj, const std::string& reference_bit_string) +{ + uavcan::StaticTransferBuffer<256> buf; + + { + uavcan::BitStream bits(buf); + uavcan::ScalarCodec codec(bits); + /* + * Coding + */ + if (0 > T::encode(obj, codec)) + { + std::cout << "Failed to encode" << std::endl; + return false; + } + + /* + * Validating the encoded bitstream + */ + const std::string result = bits.toString(); + if (result != reference_bit_string) + { + std::cout << "ENCODED VALUE DOESN'T MATCH THE REFERENCE:\nEXPECTED:\n" + << reference_bit_string << "\nACTUAL:\n" + << result << std::endl; + return false; + } + } + + /* + * Decoding back and comparing + */ + uavcan::BitStream bits(buf); + uavcan::ScalarCodec codec(bits); + + T decoded; + + if (0 > T::decode(decoded, codec)) + { + std::cout << "Failed to decode" << std::endl; + return false; + } + + if (!decoded.isClose(obj)) + { + std::cout << "DECODED OBJECT DOESN'T MATCH THE REFERENCE:\nEXPECTED:\n" + << obj << "\nACTUAL:\n" + << decoded << std::endl; + return false; + } + return true; +} + + +TEST(Dsdl, Union) +{ + using root_ns_a::UnionTest; + using root_ns_a::NestedInUnion; + + ASSERT_EQ(3, UnionTest::MinBitLen); + ASSERT_EQ(16, UnionTest::MaxBitLen); + ASSERT_EQ(13, NestedInUnion::MinBitLen); + ASSERT_EQ(13, NestedInUnion::MaxBitLen); + + UnionTest s; + + EXPECT_TRUE(validateYaml(s, "z: ")); + encodeDecodeValidate(s, "00000000"); + + s.to() = 16; + EXPECT_TRUE(validateYaml(s, "a: 16")); + EXPECT_TRUE(encodeDecodeValidate(s, "00110000")); + + s.to() = 31; + EXPECT_TRUE(validateYaml(s, "b: 31")); + EXPECT_TRUE(encodeDecodeValidate(s, "01011111")); + + s.to() = 256; + EXPECT_TRUE(validateYaml(s, "c: 256")); + EXPECT_TRUE(encodeDecodeValidate(s, "01100000 00000001")); + + s.to().push_back(true); + s.to().push_back(false); + s.to().push_back(true); + s.to().push_back(true); + s.to().push_back(false); + s.to().push_back(false); + s.to().push_back(true); + s.to().push_back(true); + s.to().push_back(true); + ASSERT_EQ(9, s.to().size()); + EXPECT_TRUE(validateYaml(s, "d: [1, 0, 1, 1, 0, 0, 1, 1, 1]")); + EXPECT_TRUE(encodeDecodeValidate(s, "10010011 01100111")); + + s.to().array[0] = 0; + s.to().array[1] = 1; + s.to().array[2] = 2; + s.to().array[3] = 3; + EXPECT_TRUE(validateYaml(s, "e: \n array: [0, 1, 2, 3]")); + EXPECT_TRUE(encodeDecodeValidate(s, "10100000 11011000")); +} + + +TEST(Dsdl, UnionTagWidth) +{ + using root_ns_a::UnionTest4; + + ASSERT_EQ(2, UnionTest4::MinBitLen); + ASSERT_EQ(8, UnionTest4::MaxBitLen); + + UnionTest4 s; + + { + uavcan::StaticTransferBuffer<100> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, UnionTest4::encode(s, sc_wr)); + ASSERT_EQ("00000000", bs_wr.toString()); + } + + { + uavcan::StaticTransferBuffer<100> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + s.to() = 1U << 5U; // 32, 0b100000 + + ASSERT_EQ(1, UnionTest4::encode(s, sc_wr)); + ASSERT_EQ("10100000", bs_wr.toString()); + } +} + + +TEST(Dsdl, ParamGetSetRequestUnion) +{ + uavcan::protocol::param::GetSet::Request req; + + req.index = 8191; + req.name = "123"; // 49, 50, 51 // 00110001, 00110010, 00110011 + EXPECT_TRUE(encodeDecodeValidate(req, "11111111 11111000 00110001 00110010 00110011")); + + req.value.to() = "abc"; // 01100001, 01100010, 01100011 + EXPECT_TRUE(encodeDecodeValidate(req, + "11111111 11111100 " // Index, Union tag + "00000011 " // Array length + "01100001 01100010 01100011 " // Payload + "00110001 00110010 00110011")); // Name + + EXPECT_TRUE(validateYaml(req, + "index: 8191\n" + "value: \n" + " string_value: \"abc\"\n" + "name: \"123\"")); + + req.value.to() = 1; + EXPECT_TRUE(encodeDecodeValidate(req, + "11111111 11111001 " // Index, Union tag + "00000001 00000000 00000000 00000000 00000000 00000000 00000000 00000000 " // Payload + "00110001 00110010 00110011")); // Name +} + + +TEST(Dsdl, ParamGetSetResponseUnion) +{ + uavcan::protocol::param::GetSet::Response res; + + res.value.to() = "abc"; + res.default_value.to(); // Empty + res.name = "123"; + EXPECT_TRUE(encodeDecodeValidate(res, + "00000100 " // Value union tag + "00000011 " // Value array length + "01100001 01100010 01100011 " // Value array payload + "00000100 " // Default union tag + "00000000 " // Default array length + "00000000 " // Max value tag + "00000000 " // Min value tag + "00110001 00110010 00110011")); // Name + + res.value.to() = true; + res.default_value.to(); // False + res.name = "123"; + EXPECT_TRUE(encodeDecodeValidate(res, + "00000011 " // Value union tag + "00000001 " // Value + "00000011 " // Default union tag + "00000000 " // Default value + "00000000 " // Max value tag + "00000000 " // Min value tag + "00110001 00110010 00110011")); // Name +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/128.EmptyService.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/128.EmptyService.uavcan new file mode 100644 index 000000000000..ed97d539c095 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/128.EmptyService.uavcan @@ -0,0 +1 @@ +--- diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/129.ReportBackSoldier.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/129.ReportBackSoldier.uavcan new file mode 100644 index 000000000000..ba1328449279 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/129.ReportBackSoldier.uavcan @@ -0,0 +1,9 @@ +bool TRUE = true +bool NOT_TRUE = false * true +uint64 BIG = (1 << 64) - 1 +float64 FLOAT_CONSTANT = 3.14 +uint8[<128] string_request +--- +float64 FLOAT_CONSTANT = 1.79769313486231570815e+308 +root_ns_b.SuperIntelligentShadeOfBlue blue +uint8[<128] string_response diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/20000.MavlinkMessage.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/20000.MavlinkMessage.uavcan new file mode 100644 index 000000000000..01297a904101 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/20000.MavlinkMessage.uavcan @@ -0,0 +1,10 @@ +# +# This thing is only needed for testing +# + +uint8 seq +uint8 sysid +uint8 compid +uint8 msgid + +uint8[<256] payload diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/32768.EmptyMessage.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/32768.EmptyMessage.uavcan new file mode 100644 index 000000000000..8d1c8b69c3fc --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/32768.EmptyMessage.uavcan @@ -0,0 +1 @@ + diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/99.StringService.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/99.StringService.uavcan new file mode 100644 index 000000000000..5bca3a34d2c0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/99.StringService.uavcan @@ -0,0 +1,3 @@ +uint8[<=64] string_request +--- +uint8[<=64] string_response diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/A.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/A.uavcan new file mode 100644 index 000000000000..9f69a474e808 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/A.uavcan @@ -0,0 +1,8 @@ +# +# Nested type. +# Energy and capacity are expressed in watt hours (Joules are infeasible because of range limitations). +# Unknown values should be represented as NAN. +# + +float32 scalar +B[2] vector diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/B.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/B.uavcan new file mode 100644 index 000000000000..fd41dbd0c416 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/B.uavcan @@ -0,0 +1,2 @@ +float64[2] vector +bool[16] bools diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/Deep.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/Deep.uavcan new file mode 100644 index 000000000000..425e9d26c828 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/Deep.uavcan @@ -0,0 +1,8 @@ +# +# Info for all battery packs of the primary power supply. +# + +bool c +uint8[<20] str +A[<2] a +B[2] b diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/Empty.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/Empty.uavcan new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/NestedInUnion.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/NestedInUnion.uavcan new file mode 100644 index 000000000000..ca6785ed66cc --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/NestedInUnion.uavcan @@ -0,0 +1,4 @@ +# This should be 13 bits long +void2 # 2 bits +uint2[4] array # 8 bits +void3 # 3 bits diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/NestedMessage.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/NestedMessage.uavcan new file mode 100644 index 000000000000..d4875f15f202 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/NestedMessage.uavcan @@ -0,0 +1,5 @@ + +float32 VALUE = 2 +( 2 *2) +bool BOOLEAN = true + false +int2 field +EmptyMessage empty diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/UnionTest.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/UnionTest.uavcan new file mode 100644 index 000000000000..b143a2735f78 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/UnionTest.uavcan @@ -0,0 +1,8 @@ +# Max bit len 16 bits +@union # 3 bits +Empty z +uint5 a +uint5 b # Conflicting with a +uint13 c +bool[<=9] d # 4 bit length field + 9 bit payload +NestedInUnion e diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/UnionTest4.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/UnionTest4.uavcan new file mode 100644 index 000000000000..05bfb5c98c53 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/UnionTest4.uavcan @@ -0,0 +1,6 @@ +# A union of four items; tag 2 bits wide, total length 1 byte +@union # 2 bits +Empty first # Tag value 0 +uint5 second # Tag value 1 +uint6 third # Tag value 2 +int2 fourth # Tag value 3 diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/ServiceWithEmptyRequest.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/ServiceWithEmptyRequest.uavcan new file mode 100644 index 000000000000..731fe88f853f --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/ServiceWithEmptyRequest.uavcan @@ -0,0 +1,2 @@ +--- +float16[<=9] covariance diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/ServiceWithEmptyResponse.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/ServiceWithEmptyResponse.uavcan new file mode 100644 index 000000000000..875562aa2bc2 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/ServiceWithEmptyResponse.uavcan @@ -0,0 +1,2 @@ +float16[<=9] covariance +--- diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/SuperIntelligentShadeOfBlue.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/SuperIntelligentShadeOfBlue.uavcan new file mode 100644 index 000000000000..2badbc9ee26b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/SuperIntelligentShadeOfBlue.uavcan @@ -0,0 +1,2 @@ +float16[<32] array_f16 +root_ns_a.NestedMessage[3] nested_message diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/T.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/T.uavcan new file mode 100644 index 000000000000..31a901612e73 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/T.uavcan @@ -0,0 +1,2 @@ +bool T = true +bool F = false diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dynamic_memory.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/dynamic_memory.cpp new file mode 100644 index 000000000000..2a4b17b9dc0d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dynamic_memory.cpp @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +TEST(DynamicMemory, Basic) +{ + uavcan::PoolAllocator<128, 32> pool32; + EXPECT_EQ(4, pool32.getNumFreeBlocks()); + EXPECT_EQ(0, pool32.getPeakNumUsedBlocks()); + const void* ptr1 = pool32.allocate(16); + ASSERT_TRUE(ptr1); + EXPECT_EQ(1, pool32.getNumUsedBlocks()); + EXPECT_FALSE(pool32.allocate(120)); + EXPECT_EQ(1, pool32.getNumUsedBlocks()); + pool32.deallocate(ptr1); + EXPECT_EQ(0, pool32.getNumUsedBlocks()); + EXPECT_EQ(1, pool32.getPeakNumUsedBlocks()); +} + +TEST(DynamicMemory, OutOfMemory) +{ + uavcan::PoolAllocator<64, 32> pool32; + + EXPECT_EQ(2, pool32.getNumFreeBlocks()); + EXPECT_EQ(0, pool32.getNumUsedBlocks()); + EXPECT_EQ(0, pool32.getPeakNumUsedBlocks()); + + const void* ptr1 = pool32.allocate(32); + ASSERT_TRUE(ptr1); + EXPECT_EQ(1, pool32.getNumUsedBlocks()); + EXPECT_EQ(1, pool32.getPeakNumUsedBlocks()); + + const void* ptr2 = pool32.allocate(32); + ASSERT_TRUE(ptr2); + EXPECT_EQ(2, pool32.getNumUsedBlocks()); + EXPECT_EQ(2, pool32.getPeakNumUsedBlocks()); + + ASSERT_FALSE(pool32.allocate(32)); // No free blocks left --> UAVCAN_NULLPTR + EXPECT_EQ(2, pool32.getNumUsedBlocks()); + EXPECT_EQ(0, pool32.getNumFreeBlocks()); + EXPECT_EQ(2, pool32.getPeakNumUsedBlocks()); +} + +TEST(DynamicMemory, LimitedPoolAllocator) +{ + uavcan::PoolAllocator<128, 32> pool32; + uavcan::LimitedPoolAllocator lim(pool32, 2); + + EXPECT_EQ(2, lim.getBlockCapacity()); + EXPECT_EQ(0, pool32.getPeakNumUsedBlocks()); + + const void* ptr1 = lim.allocate(1); + const void* ptr2 = lim.allocate(1); + const void* ptr3 = lim.allocate(1); + + EXPECT_TRUE(ptr1); + EXPECT_TRUE(ptr2); + EXPECT_FALSE(ptr3); + + lim.deallocate(ptr2); + const void* ptr4 = lim.allocate(1); + lim.deallocate(ptr1); + const void* ptr5 = lim.allocate(1); + const void* ptr6 = lim.allocate(1); + + EXPECT_TRUE(ptr4); + EXPECT_TRUE(ptr5); + EXPECT_FALSE(ptr6); + + EXPECT_EQ(2, pool32.getPeakNumUsedBlocks()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/helpers/heap_based_pool_allocator.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/helpers/heap_based_pool_allocator.cpp new file mode 100644 index 000000000000..52dbb364b008 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/helpers/heap_based_pool_allocator.cpp @@ -0,0 +1,193 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#ifdef __linux__ +#include + +#else +#include +#endif + +TEST(HeapBasedPoolAllocator, Basic) +{ +#ifdef __linux__ + std::cout << ">>> HEAP BEFORE:" << std::endl; + malloc_stats(); +#endif + + uavcan::HeapBasedPoolAllocator al(0xEEEE); + + ASSERT_EQ(0, al.getNumReservedBlocks()); + ASSERT_EQ(0, al.getNumAllocatedBlocks()); + + ASSERT_EQ(0xEEEE, al.getBlockCapacity()); + ASSERT_EQ(0xFFFF, al.getBlockCapacityHardLimit()); + + void* a = al.allocate(10); + void* b = al.allocate(10); + void* c = al.allocate(10); + void* d = al.allocate(10); + + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(4, al.getNumAllocatedBlocks()); + + al.deallocate(a); + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(3, al.getNumAllocatedBlocks()); + + al.deallocate(b); + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(2, al.getNumAllocatedBlocks()); + + al.deallocate(c); + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(1, al.getNumAllocatedBlocks()); + + a = al.allocate(10); + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(2, al.getNumAllocatedBlocks()); + ASSERT_EQ(c, a); + + al.deallocate(a); + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(1, al.getNumAllocatedBlocks()); + + al.shrink(); + ASSERT_EQ(1, al.getNumReservedBlocks()); + ASSERT_EQ(1, al.getNumAllocatedBlocks()); + + al.deallocate(d); + ASSERT_EQ(1, al.getNumReservedBlocks()); + ASSERT_EQ(0, al.getNumAllocatedBlocks()); + + al.shrink(); + ASSERT_EQ(0, al.getNumReservedBlocks()); + ASSERT_EQ(0, al.getNumAllocatedBlocks()); + +#ifdef __linux__ + std::cout << ">>> HEAP AFTER:" << std::endl; + malloc_stats(); +#endif +} + + +TEST(HeapBasedPoolAllocator, Limits) +{ + uavcan::HeapBasedPoolAllocator al(2); + + ASSERT_EQ(2, al.getBlockCapacity()); + ASSERT_EQ(4, al.getBlockCapacityHardLimit()); + + ASSERT_EQ(0, al.getNumReservedBlocks()); + ASSERT_EQ(0, al.getNumAllocatedBlocks()); + + void* a = al.allocate(10); + void* b = al.allocate(10); + void* c = al.allocate(10); + void* d = al.allocate(10); + + ASSERT_TRUE(a); + ASSERT_TRUE(b); + ASSERT_TRUE(c); + ASSERT_TRUE(d); + + ASSERT_FALSE(al.allocate(10)); + + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(4, al.getNumAllocatedBlocks()); + + al.deallocate(a); + al.deallocate(b); + al.deallocate(c); + al.deallocate(d); + + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(0, al.getNumAllocatedBlocks()); +} + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +#include +#include + +struct RaiiSynchronizer +{ + static std::mutex mutex; + std::lock_guard guard{mutex}; +}; + +std::mutex RaiiSynchronizer::mutex; + +TEST(HeapBasedPoolAllocator, Concurrency) +{ +#ifdef __linux__ + std::cout << ">>> HEAP BEFORE:" << std::endl; + malloc_stats(); + +#endif + uavcan::HeapBasedPoolAllocator al(1000); + + ASSERT_EQ(1000, al.getBlockCapacity()); + ASSERT_EQ(2000, al.getBlockCapacityHardLimit()); + + volatile bool terminate = false; + + /* + * Starting the testing threads + */ + std::thread threads[3]; + + for (auto& x : threads) + { + x = std::thread([&al, &terminate]() + { + while (!terminate) + { + auto a = al.allocate(1); + auto b = al.allocate(1); + auto c = al.allocate(1); + al.deallocate(al.allocate(1)); + al.deallocate(a); + al.deallocate(b); + al.deallocate(c); + } + }); + } + + /* + * Running the threads for some time, then terminating + */ + std::this_thread::sleep_for(std::chrono::seconds(1)); + + terminate = true; + std::cout << "Terminating workers..." << std::endl; + + for (auto& x : threads) + { + x.join(); + } + std::cout << "All workers joined" << std::endl; + + /* + * Now, there must not be any leaked memory, because the worker threads deallocate everything before completion. + */ + std::cout << "Allocated: " << al.getNumAllocatedBlocks() << std::endl; + std::cout << "Reserved: " << al.getNumReservedBlocks() << std::endl; + +#ifdef __linux__ + std::cout << ">>> HEAP BEFORE SHRINK:" << std::endl; + malloc_stats(); + +#endif + al.shrink(); + +#ifdef __linux__ + std::cout << ">>> HEAP AFTER SHRINK:" << std::endl; + malloc_stats(); +#endif +} + +#endif diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/array.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/array.cpp new file mode 100644 index 000000000000..d8e4d6f83b66 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/array.cpp @@ -0,0 +1,1419 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wfloat-equal" +# pragma GCC diagnostic ignored "-Wdouble-promotion" +#endif + +#include +#include +#include + +using uavcan::Array; +using uavcan::ArrayModeDynamic; +using uavcan::ArrayModeStatic; +using uavcan::IntegerSpec; +using uavcan::FloatSpec; +using uavcan::SignednessSigned; +using uavcan::SignednessUnsigned; +using uavcan::CastModeSaturate; +using uavcan::CastModeTruncate; + +struct CustomType +{ + typedef uavcan::IntegerSpec<8, uavcan::SignednessSigned, uavcan::CastModeTruncate> A; + typedef uavcan::FloatSpec<16, uavcan::CastModeSaturate> B; + // Dynamic array of max len 5 --> 3 bits for len, 5 bits for data --> 1 byte max len + typedef uavcan::Array, + uavcan::ArrayModeDynamic, 5> C; + + enum { MinBitLen = A::MinBitLen + B::MinBitLen + C::MinBitLen }; + enum { MaxBitLen = A::MaxBitLen + B::MaxBitLen + C::MaxBitLen }; + + typename uavcan::StorageType::Type a; + typename uavcan::StorageType::Type b; + typename uavcan::StorageType::Type c; + + CustomType() + : a() + , b() + , c() + { } + + bool operator==(const CustomType& rhs) const + { + return a == rhs.a && + uavcan::areFloatsExactlyEqual(b, rhs.b) && + c == rhs.c; + } + + static int encode(const CustomType& obj, uavcan::ScalarCodec& codec, + uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled) + { + int res = 0; + res = A::encode(obj.a, codec, uavcan::TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + res = B::encode(obj.b, codec, uavcan::TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + res = C::encode(obj.c, codec, tao_mode); + if (res <= 0) + { + return res; + } + return 1; + } + + static int decode(CustomType& obj, uavcan::ScalarCodec& codec, + uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled) + { + int res = 0; + res = A::decode(obj.a, codec, uavcan::TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + res = B::decode(obj.b, codec, uavcan::TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + res = C::decode(obj.c, codec, tao_mode); + if (res <= 0) + { + return res; + } + return 1; + } +}; + + +TEST(Array, Basic) +{ + typedef Array, ArrayModeStatic, 4> A1; + typedef Array, ArrayModeStatic, 2> A2; + typedef Array A3; + + A1 a1; + A2 a2; + A3 a3; + + ASSERT_EQ(1, A3::ValueType::C::RawValueType::BitLen); + + ASSERT_EQ(8 * 4, A1::MaxBitLen); + ASSERT_EQ(16 * 2, A2::MaxBitLen); + ASSERT_EQ((8 + 16 + 5 + 3) * 2, A3::MaxBitLen); + + /* + * Zero initialization check + */ + ASSERT_FALSE(a1.empty()); + for (A1::const_iterator it = a1.begin(); it != a1.end(); ++it) + { + ASSERT_EQ(0, *it); + } + + ASSERT_FALSE(a2.empty()); + for (A2::const_iterator it = a2.begin(); it != a2.end(); ++it) + { + ASSERT_EQ(0, *it); + } + + for (A3::const_iterator it = a3.begin(); it != a3.end(); ++it) + { + ASSERT_EQ(0, it->a); + ASSERT_EQ(0, it->b); + ASSERT_EQ(0, it->c.size()); + ASSERT_TRUE(it->c.empty()); + } + + /* + * Modification with known values; array lengths are hard coded. + */ + for (uint8_t i = 0; i < 4; i++) + { + a1.at(i) = int8_t(i); + } + for (uint8_t i = 0; i < 2; i++) + { + a2.at(i) = i; + } + for (uint8_t i = 0; i < 2; i++) + { + a3[i].a = int8_t(i); + a3[i].b = i; + for (uint8_t i2 = 0; i2 < 5; i2++) + { + a3[i].c.push_back(i2 & 1); + } + ASSERT_EQ(5, a3[i].c.size()); + ASSERT_FALSE(a3[i].c.empty()); + } + + /* + * Representation check + * Note that TAO in A3 is not possible because A3::C has less than one byte per item + */ + uavcan::StaticTransferBuffer<16> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, A1::encode(a1, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, A2::encode(a2, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, A3::encode(a3, sc_wr, uavcan::TailArrayOptEnabled)); + + ASSERT_EQ(0, A3::encode(a3, sc_wr, uavcan::TailArrayOptEnabled)); // Out of buffer space + + static const std::string Reference = + "00000000 00000001 00000010 00000011 " // A1 (0, 1, 2, 3) + "00000000 00000000 00000000 00111100 " // A2 (0, 1) + "00000000 00000000 00000000 10101010 " // A3[0] (0, 0, bool[5]) + "00000001 00000000 00111100 10101010"; // A3[1] (1, 1, bool[5]) + + ASSERT_EQ(Reference, bs_wr.toString()); + + /* + * Read back + */ + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + + A1 a1_; + A2 a2_; + A3 a3_; + + ASSERT_EQ(1, A1::decode(a1_, sc_rd, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, A2::decode(a2_, sc_rd, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, A3::decode(a3_, sc_rd, uavcan::TailArrayOptEnabled)); + + ASSERT_EQ(a1_, a1); + ASSERT_EQ(a2_, a2); + ASSERT_EQ(a3_, a3); + + for (uint8_t i = 0; i < 4; i++) + { + ASSERT_EQ(a1[i], a1_[i]); + } + for (uint8_t i = 0; i < 2; i++) + { + ASSERT_EQ(a2[i], a2_[i]); + } + for (uint8_t i = 0; i < 2; i++) + { + ASSERT_EQ(a3[i].a, a3_[i].a); + ASSERT_EQ(a3[i].b, a3_[i].b); + ASSERT_EQ(a3[i].c, a3_[i].c); + } + + ASSERT_EQ(0, A3::decode(a3_, sc_rd, uavcan::TailArrayOptEnabled)); // Out of buffer space + + /* + * STL compatibility + */ + std::vector v1; + v1.push_back(0); + v1.push_back(1); + v1.push_back(2); + v1.push_back(3); + + ASSERT_TRUE(a1 == v1); + ASSERT_FALSE(a1 != v1); + ASSERT_TRUE(v1 == a1); + ASSERT_FALSE(v1 != a1); + ASSERT_FALSE(a1 < v1); + + v1[0] = 9000; + ASSERT_FALSE(a1 == v1); + ASSERT_TRUE(a1 != v1); + ASSERT_TRUE(a1 < v1); + + ASSERT_EQ(0, a1.front()); + ASSERT_EQ(3, a1.back()); + + // Boolean vector + std::vector v2; + v2.push_back(false); + v2.push_back(true); + v2.push_back(false); + v2.push_back(true); + v2.push_back(false); + + ASSERT_TRUE(a3[0].c == v2); + ASSERT_FALSE(a3[0].c == v1); + ASSERT_FALSE(a3[0].c != v2); + ASSERT_TRUE(a3[0].c != v1); + + v2[0] = true; + ASSERT_TRUE(a3[0].c != v2); + ASSERT_FALSE(a3[0].c == v2); +} + + +TEST(Array, Dynamic) +{ + typedef Array, ArrayModeDynamic, 5> A; + typedef Array, ArrayModeDynamic, 255> B; + + A a; + B b; + B b2; + + ASSERT_EQ(3 + 5, A::MaxBitLen); + ASSERT_EQ(8 + 255 * 8, B::MaxBitLen); + + ASSERT_TRUE(a.empty()); + ASSERT_TRUE(b.empty()); + ASSERT_TRUE(b2.empty()); + + { + uavcan::StaticTransferBuffer<16> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::encode(b, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::encode(b2, sc_wr, uavcan::TailArrayOptEnabled)); + + ASSERT_EQ("000" "00000 000" "00000", bs_wr.toString()); // Last array was optimized away completely + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + + ASSERT_EQ(1, A::decode(a, sc_rd, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::decode(b, sc_rd, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::decode(b2, sc_rd, uavcan::TailArrayOptEnabled)); + + ASSERT_TRUE(a.empty()); + ASSERT_TRUE(b.empty()); + ASSERT_TRUE(b2.empty()); + } + + a.push_back(true); + a.push_back(false); + a.push_back(true); + a.push_back(false); + a.push_back(true); + + b.push_back(42); + b.push_back(-42); + + b2.push_back(123); + b2.push_back(72); + + { + uavcan::StaticTransferBuffer<16> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::encode(b, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::encode(b2, sc_wr, uavcan::TailArrayOptEnabled)); // No length field + + // A B len B[0] B[1] B2[0] B2[1] + ASSERT_EQ("10110101 00000010 00101010 11010110 01111011 01001000", bs_wr.toString()); + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + + a.clear(); + b.clear(); + b2.clear(); + ASSERT_TRUE(a.empty()); + ASSERT_TRUE(b.empty()); + ASSERT_TRUE(b2.empty()); + + ASSERT_EQ(1, A::decode(a, sc_rd, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::decode(b, sc_rd, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::decode(b2, sc_rd, uavcan::TailArrayOptEnabled)); + + ASSERT_EQ(5, a.size()); + ASSERT_EQ(2, b.size()); + ASSERT_EQ(2, b2.size()); + + ASSERT_TRUE(a[0]); + ASSERT_FALSE(a[1]); + ASSERT_TRUE(a[2]); + ASSERT_FALSE(a[3]); + ASSERT_TRUE(a[4]); + + ASSERT_EQ(42, b[0]); + ASSERT_EQ(-42, b[1]); + + ASSERT_EQ(123, b2[0]); + ASSERT_EQ(72, b2[1]); + } + + ASSERT_FALSE(a == b); + ASSERT_FALSE(b == a); + ASSERT_TRUE(a != b); + ASSERT_TRUE(b != a); + + a.resize(0); + b.resize(0); + ASSERT_TRUE(a.empty()); + ASSERT_TRUE(b.empty()); + + a.resize(5, true); + b.resize(255, 72); + ASSERT_EQ(5, a.size()); + ASSERT_EQ(255, b.size()); + + for (uint8_t i = 0; i < 5; i++) + { + ASSERT_TRUE(a[i]); + } + for (uint8_t i = 0; i < 255; i++) + { + ASSERT_EQ(72, b[i]); + } +} + + +template +struct CustomType2 +{ + typedef uavcan::FloatSpec<16, uavcan::CastModeSaturate> A; + + enum { MinBitLen = A::MinBitLen + B::MinBitLen }; + enum { MaxBitLen = A::MaxBitLen + B::MaxBitLen }; + + typename uavcan::StorageType::Type a; + typename uavcan::StorageType::Type b; + + CustomType2() + : a() + , b() + { } + + bool operator==(const CustomType2& rhs) const + { + return uavcan::areFloatsExactlyEqual(a, rhs.a) && + b == rhs.b; + } + + static int encode(const CustomType2& obj, uavcan::ScalarCodec& codec, + uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled) + { + int res = 0; + res = A::encode(obj.a, codec, uavcan::TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + res = B::encode(obj.b, codec, tao_mode); + if (res <= 0) + { + return res; + } + return 1; + } + + static int decode(CustomType2& obj, uavcan::ScalarCodec& codec, + uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled) + { + int res = 0; + res = A::decode(obj.a, codec, uavcan::TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + res = B::decode(obj.b, codec, tao_mode); + if (res <= 0) + { + return res; + } + return 1; + } +}; + + +template +static std::string runEncodeDecode(const typename uavcan::StorageType::Type& value, + const uavcan::TailArrayOptimizationMode tao_mode) +{ + uavcan::StaticTransferBuffer<(T::MaxBitLen + 7) / 8> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + EXPECT_EQ(1, T::encode(value, sc_wr, tao_mode)); + + typename uavcan::StorageType::Type value2 = typename uavcan::StorageType::Type(); + // Decode multiple times to make sure that the decoded type is being correctly de-initialized + for (int i = 0; i < 3; i++) + { + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + EXPECT_EQ(1, T::decode(value2, sc_rd, tao_mode)); + EXPECT_EQ(value, value2); + } + return bs_wr.toString(); +} + + +TEST(Array, TailArrayOptimization) +{ + typedef Array, ArrayModeDynamic, 5> OneBitArray; + typedef Array, ArrayModeDynamic, 255> EightBitArray; + typedef CustomType2 > A; + typedef CustomType2 > B; + typedef CustomType2 C; + + A a; + B b; + C c; + + /* + * Empty + */ + // a LSB a MSB b len + ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode(a, uavcan::TailArrayOptEnabled)); + ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode(a, uavcan::TailArrayOptDisabled)); + + // a LSB a MSB b len + ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode(b, uavcan::TailArrayOptEnabled)); + ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode(b, uavcan::TailArrayOptDisabled)); + + // a LSB a MSB + ASSERT_EQ("00000000 00000000", runEncodeDecode(c, uavcan::TailArrayOptEnabled)); + ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode(c, uavcan::TailArrayOptDisabled)); + + /* + * A + */ + a.b.resize(2); + a.b[0].push_back(true); + a.b[0].push_back(false); + // a.b[1] remains empty + // a LSB a MSB b len b: len(2), 1, 0, len(0) + ASSERT_EQ("00000000 00000000 00000010 01010000", runEncodeDecode(a, uavcan::TailArrayOptEnabled)); + ASSERT_EQ("00000000 00000000 00000010 01010000", runEncodeDecode(a, uavcan::TailArrayOptDisabled)); + + /* + * B + */ + b.b.resize(3); + b.b[0].push_back(42); + b.b[0].push_back(72); + // b.b[1] remains empty + b.b[2].push_back(123); + b.b[2].push_back(99); + // a LSB a MSB b len b[0]len 42 72 b[1]len 123 99 (b[2] len optimized out) + ASSERT_EQ("00000000 00000000 00000011 00000010 00101010 01001000 00000000 01111011 01100011", + runEncodeDecode(b, uavcan::TailArrayOptEnabled)); + // Same as above, but b[2] len is present v here v + ASSERT_EQ("00000000 00000000 00000011 00000010 00101010 01001000 00000000 00000010 01111011 01100011", + runEncodeDecode(b, uavcan::TailArrayOptDisabled)); + + /* + * C + */ + c.a = 1; + c.b.push_back(1); + c.b.push_back(2); + c.b.push_back(3); + // a LSB a MSB 1 2 3 + ASSERT_EQ("00000000 00111100 00000001 00000010 00000011", + runEncodeDecode(c, uavcan::TailArrayOptEnabled)); + // a LSB a MSB b len 1 2 3 + ASSERT_EQ("00000000 00111100 00000011 00000001 00000010 00000011", + runEncodeDecode(c, uavcan::TailArrayOptDisabled)); +} + + +TEST(Array, TailArrayOptimizationErrors) +{ + typedef Array, ArrayModeDynamic, 5> A; + + A a; + ASSERT_TRUE(a.empty()); + ASSERT_EQ("", runEncodeDecode(a, uavcan::TailArrayOptEnabled)); + ASSERT_EQ("00000000", runEncodeDecode(a, uavcan::TailArrayOptDisabled)); + + // Correct decode/encode + a.push_back(1); + a.push_back(126); + a.push_back(5); + ASSERT_FALSE(a.empty()); + ASSERT_EQ("00000001 01111110 00000101", runEncodeDecode(a, uavcan::TailArrayOptEnabled)); + ASSERT_EQ("01100000 00101111 11000000 10100000", runEncodeDecode(a, uavcan::TailArrayOptDisabled)); + + // Invalid decode - length field is out of range + uavcan::StaticTransferBuffer<7> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, sc_wr.encode<3>(uint8_t(6))); // Length - more than 5 items, error + ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(42))); + ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(72))); + ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(126))); + ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(1))); + ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(2))); + ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(3))); // Out of range - only 5 items allowed + + // 197 73 15 192 32 ... + ASSERT_EQ("11000101 01001001 00001111 11000000 00100000 01000000 01100000", bs_wr.toString()); + + { + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + A a2; + a2.push_back(56); // Garbage + ASSERT_EQ(1, a2.size()); + // Will fail - declared length is more than 5 items + ASSERT_GT(0, A::decode(a2, sc_rd, uavcan::TailArrayOptDisabled)); + // Must be cleared + ASSERT_TRUE(a2.empty()); + } + { + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + A a2; + a2.push_back(56); // Garbage + ASSERT_EQ(1, a2.size()); + // Will fail - no length field, but the stream is too long + ASSERT_GT(0, A::decode(a2, sc_rd, uavcan::TailArrayOptEnabled)); + // Will contain some garbage + ASSERT_EQ(5, a2.size()); + // Interpreted stream - see the values above + ASSERT_EQ(197, a2[0]); + ASSERT_EQ(73, a2[1]); + ASSERT_EQ(15, a2[2]); + ASSERT_EQ(192, a2[3]); + ASSERT_EQ(32, a2[4]); + } +} + + +TEST(Array, DynamicEncodeDecodeErrors) +{ + typedef CustomType2, + ArrayModeDynamic, 255>, + ArrayModeDynamic, 255> > A; + A a; + a.b.resize(2); + a.b[0].push_back(55); + a.b[0].push_back(66); + { + uavcan::StaticTransferBuffer<4> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + ASSERT_EQ(0, A::encode(a, sc_wr, uavcan::TailArrayOptEnabled)); // Not enough buffer space + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + ASSERT_EQ(0, A::decode(a, sc_rd, uavcan::TailArrayOptEnabled)); + } + { + uavcan::StaticTransferBuffer<4> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + ASSERT_EQ(0, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); // Not enough buffer space + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + ASSERT_EQ(0, A::decode(a, sc_rd, uavcan::TailArrayOptDisabled)); + } +} + + +TEST(Array, StaticEncodeDecodeErrors) +{ + typedef CustomType2, + ArrayModeStatic, 2>, + ArrayModeStatic, 2> > A; + A a; + a.a = 1.0; + a.b[0][0] = 0x11; + a.b[0][1] = 0x22; + a.b[1][0] = 0x33; + a.b[1][1] = 0x44; + { // Just enough buffer space - 6 bytes + uavcan::StaticTransferBuffer<6> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + ASSERT_EQ(1, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); + + ASSERT_EQ("00000000 00111100 00010001 00100010 00110011 01000100", bs_wr.toString()); + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + ASSERT_EQ(1, A::decode(a, sc_rd, uavcan::TailArrayOptEnabled)); + } + { // Not enough space + uavcan::StaticTransferBuffer<5> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + ASSERT_EQ(0, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); + + ASSERT_EQ("00000000 00111100 00010001 00100010 00110011", bs_wr.toString()); + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + ASSERT_EQ(0, A::decode(a, sc_rd, uavcan::TailArrayOptEnabled)); + } +} + + +TEST(Array, Copyability) +{ + typedef Array, ArrayModeDynamic, 5> OneBitArray; + typedef Array, ArrayModeDynamic, 255> EightBitArray; + typedef Array A; + typedef Array B; + typedef EightBitArray C; + + A a; + B b; + C c; + + A a2 = a; + B b2 = b; + C c2 = c; + + ASSERT_TRUE(a == a2); + ASSERT_TRUE(b == b2); + ASSERT_TRUE(c == c2); + + a.push_back(OneBitArray()); + b.push_back(EightBitArray()); + c.push_back(42); + + ASSERT_TRUE(a != a2); + ASSERT_TRUE(b != b2); + ASSERT_TRUE(c != c2); + + a2 = a; + b2 = b; + c2 = c; + + ASSERT_TRUE(a2 == a); + ASSERT_TRUE(b2 == b); + ASSERT_TRUE(c2 == c); +} + + +TEST(Array, Appending) +{ + typedef Array, ArrayModeDynamic, 2> A; + typedef Array, ArrayModeDynamic, 257> B; + A a; + B b; + + a.push_back(1); + a.push_back(2); + a += b; + ASSERT_EQ(2, a.size()); + ASSERT_EQ(1, a[0]); + ASSERT_EQ(2, a[1]); + + b += a; + ASSERT_TRUE(b == a); + b += a; + ASSERT_EQ(4, b.size()); + ASSERT_EQ(1, b[0]); + ASSERT_EQ(2, b[1]); + ASSERT_EQ(1, b[2]); + ASSERT_EQ(2, b[3]); +} + + +TEST(Array, Strings) +{ + typedef Array, ArrayModeDynamic, 32> A8; + typedef Array, ArrayModeDynamic, 32> A7; + + A8 a8; + A8 a8_2; + A7 a7; + + ASSERT_TRUE(a8 == a7); + // cppcheck-suppress duplicateExpression + ASSERT_TRUE(a8 == a8); + // cppcheck-suppress duplicateExpression + ASSERT_TRUE(a7 == a7); + ASSERT_TRUE(a8 == ""); + ASSERT_TRUE(a7 == ""); + + a8 = "Hello world!"; + a7 = "123"; + ASSERT_TRUE(a8 == "Hello world!"); + ASSERT_TRUE(a7 == "123"); + + a8 = "Our sun is dying."; + a7 = "456"; + ASSERT_TRUE("Our sun is dying." == a8); + ASSERT_TRUE("456" == a7); + + a8 += " 123456"; + a8 += "-789"; + ASSERT_TRUE("Our sun is dying. 123456-789" == a8); + + ASSERT_TRUE(a8_2 == ""); + ASSERT_TRUE(a8_2.empty()); + ASSERT_TRUE(a8_2 != a8); + a8_2 = a8; + ASSERT_TRUE(a8_2 == "Our sun is dying. 123456-789"); + ASSERT_TRUE(a8_2 == a8); + + /* + * c_str() + */ + ASSERT_STREQ("", A8().c_str()); + ASSERT_STREQ("", A7().c_str()); + ASSERT_STREQ("Our sun is dying. 123456-789", a8_2.c_str()); + ASSERT_STREQ("Our sun is dying. 123456-789", a8.c_str()); + ASSERT_STREQ("456", a7.c_str()); + + /* + * String constructor + */ + A8 a8_3("123"); + A7 a7_3 = "456"; + ASSERT_EQ(3, a8_3.size()); + ASSERT_EQ(3, a7_3.size()); + ASSERT_STREQ("123", a8_3.c_str()); + ASSERT_STREQ("456", a7_3.c_str()); +} + + +TEST(Array, AppendFormatted) +{ + typedef Array, ArrayModeDynamic, 45> A8; + + A8 a; + + ASSERT_TRUE("" == a); + + a.appendFormatted("%4.1f", 12.3); // 4 + a += " "; // 1 + a.appendFormatted("%li", -123456789L); // 10 + a.appendFormatted("%s", " TOTAL PERSPECTIVE VORTEX "); // 26 + a.appendFormatted("0x%X", 0xDEADBEEF); // 10 --> 4 + + ASSERT_STREQ("12.3 -123456789 TOTAL PERSPECTIVE VORTEX 0xDE", a.c_str()); +} + + +TEST(Array, FlatStreaming) +{ + typedef Array, ArrayModeDynamic, 32> A8D; + typedef Array, ArrayModeDynamic, 16> AF16D; + typedef Array, ArrayModeStatic, 3> AF16S; + + A8D a1; + a1 = "12\n3\x44\xa5\xde\xad\x79"; + uavcan::YamlStreamer::stream(std::cout, a1, 0); + std::cout << std::endl; + + A8D a2; + a2 = "Hello"; + uavcan::YamlStreamer::stream(std::cout, a2, 0); + std::cout << std::endl; + + AF16D af16d1; + af16d1.push_back(1.23F); + af16d1.push_back(4.56F); + uavcan::YamlStreamer::stream(std::cout, af16d1, 0); + std::cout << std::endl; + + AF16D af16d2; + uavcan::YamlStreamer::stream(std::cout, af16d2, 0); + std::cout << std::endl; + + AF16S af16s; + uavcan::YamlStreamer::stream(std::cout, af16s, 0); + std::cout << std::endl; +} + + +TEST(Array, MultidimensionalStreaming) +{ + typedef Array, ArrayModeDynamic, 16> Float16Array; + typedef Array TwoDimensional; + typedef Array ThreeDimensional; + + ThreeDimensional threedee; + threedee.resize(3); + for (uint8_t x = 0; x < threedee.size(); x++) + { + threedee[x].resize(3); + for (uint8_t y = 0; y < threedee[x].size(); y++) + { + threedee[x][y].resize(3); + for (uint8_t z = 0; z < threedee[x][y].size(); z++) + { + threedee[x][y][z] = 1.0F / (float(x + y + z) + 1.0F); + } + } + } + + uavcan::YamlStreamer::stream(std::cout, threedee, 0); + std::cout << std::endl; +} + + +TEST(Array, SquareMatrixPacking) +{ + Array, ArrayModeDynamic, 9> m3x3s; + Array, ArrayModeDynamic, 4> m2x2f; + Array, ArrayModeDynamic, 36> m6x6d; + + // NAN will be reduced to empty array + { + const double nans3x3[] = + { + NAN, NAN, NAN, + NAN, NAN, NAN, + NAN, NAN, NAN + }; + m3x3s.packSquareMatrix(nans3x3); + ASSERT_EQ(0, m3x3s.size()); + + // Empty array will be decoded as zero matrix + double nans3x3_out[9]; + m3x3s.unpackSquareMatrix(nans3x3_out); + for (uint8_t i = 0; i < 9; i++) + { + ASSERT_DOUBLE_EQ(0, nans3x3_out[i]); + } + } + { + std::vector empty; + m3x3s.packSquareMatrix(empty); + ASSERT_EQ(0, m3x3s.size()); + + empty.resize(9); + m3x3s.unpackSquareMatrix(empty); + for (uint8_t i = 0; i < 9; i++) + { + ASSERT_DOUBLE_EQ(0, empty.at(i)); + } + } + + // Scalar matrix will be reduced to a single value + { + std::vector scalar2x2(4); + scalar2x2[0] = scalar2x2[3] = 3.14F; + m2x2f.packSquareMatrix(scalar2x2); + ASSERT_EQ(1, m2x2f.size()); + ASSERT_FLOAT_EQ(3.14F, m2x2f[0]); + + m2x2f.unpackSquareMatrix(scalar2x2); + const float reference[] = + { + 3.14F, 0.0F, + 0.0F, 3.14F + }; + ASSERT_TRUE(std::equal(scalar2x2.begin(), scalar2x2.end(), reference)); + } + { + const float scalar6x6[] = + { + -18, 0, 0, 0, 0, 0, + 0, -18, 0, 0, 0, 0, + 0, 0, -18, 0, 0, 0, + 0, 0, 0, -18, 0, 0, + 0, 0, 0, 0, -18, 0, + 0, 0, 0, 0, 0, -18 + }; + m6x6d.packSquareMatrix(scalar6x6); + ASSERT_EQ(1, m6x6d.size()); + ASSERT_DOUBLE_EQ(-18, m6x6d[0]); + + std::vector output(36); + m6x6d.unpackSquareMatrix(output); + ASSERT_TRUE(std::equal(output.begin(), output.end(), scalar6x6)); + } + + // Diagonal matrix will be reduced to an array of length Width + { + const float diagonal6x6[] = + { + 1, 0, 0, 0, 0, 0, + 0, -2, 0, 0, 0, 0, + 0, 0, 3, 0, 0, 0, + 0, 0, 0, -4, 0, 0, + 0, 0, 0, 0, 5, 0, + 0, 0, 0, 0, 0, -6 + }; + m6x6d.packSquareMatrix(diagonal6x6); + ASSERT_EQ(6, m6x6d.size()); + ASSERT_DOUBLE_EQ(1, m6x6d[0]); + ASSERT_DOUBLE_EQ(-2, m6x6d[1]); + ASSERT_DOUBLE_EQ(3, m6x6d[2]); + ASSERT_DOUBLE_EQ(-4, m6x6d[3]); + ASSERT_DOUBLE_EQ(5, m6x6d[4]); + ASSERT_DOUBLE_EQ(-6, m6x6d[5]); + + std::vector output(36); + m6x6d.unpackSquareMatrix(output); + ASSERT_TRUE(std::equal(output.begin(), output.end(), diagonal6x6)); + } + + // A matrix filled with random values will not be compressed + { + std::vector full3x3(9); + for (uint8_t i = 0; i < 9; i++) + { + full3x3[i] = float(i); + } + m3x3s.packSquareMatrix(full3x3); + ASSERT_EQ(9, m3x3s.size()); + for (uint8_t i = 0; i < 9; i++) + { + ASSERT_FLOAT_EQ(float(i), m3x3s[i]); + } + + long output[9]; + m3x3s.unpackSquareMatrix(output); + ASSERT_TRUE(std::equal(full3x3.begin(), full3x3.end(), output)); + } + + // This will be represented as diagonal - NANs are exceptional + { + const double scalarnan3x3[] = + { + NAN, 0, 0, + 0, NAN, 0, + 0, 0, NAN + }; + m3x3s.packSquareMatrix(scalarnan3x3); + ASSERT_EQ(3, m3x3s.size()); + ASSERT_FALSE(m3x3s[0] <= m3x3s[0]); // NAN + ASSERT_FALSE(m3x3s[1] <= m3x3s[1]); // NAN + ASSERT_FALSE(m3x3s[2] <= m3x3s[2]); // NAN + + float output[9]; + m3x3s.unpackSquareMatrix(output); + ASSERT_FALSE(output[0] <= output[0]); // NAN + ASSERT_EQ(0, output[1]); + ASSERT_EQ(0, output[2]); + ASSERT_EQ(0, output[3]); + ASSERT_FALSE(output[4] <= output[4]); // NAN + ASSERT_EQ(0, output[5]); + ASSERT_EQ(0, output[6]); + ASSERT_EQ(0, output[7]); + ASSERT_FALSE(output[8] <= output[8]); // NAN + } + + // This is a full matrix too (notice the NAN) + { + const float full2x2[] = + { + 1, NAN, + 0, -2 + }; + m2x2f.packSquareMatrix(full2x2); + ASSERT_EQ(4, m2x2f.size()); + ASSERT_FLOAT_EQ(1, m2x2f[0]); + ASSERT_FALSE(m2x2f[1] <= m2x2f[1]); // NAN + ASSERT_FLOAT_EQ(0, m2x2f[2]); + ASSERT_FLOAT_EQ(-2, m2x2f[3]); + + float output[4]; + m2x2f.unpackSquareMatrix(output); + ASSERT_EQ(1, output[0]); + ASSERT_FALSE(output[1] <= output[1]); // NAN + ASSERT_EQ(0, output[2]); + ASSERT_EQ(-2, output[3]); + } + + // Zero matrix will be represented as scalar matrix + { + const float zero2x2[] = + { + 0, 0, + 0, 0 + }; + m2x2f.packSquareMatrix(zero2x2); + ASSERT_EQ(1, m2x2f.size()); + ASSERT_FLOAT_EQ(0, m2x2f[0]); + } + + // Symmetric matrix will contain only upper-right triangle + { + const float sym2x2[] = + { + 1, 2, + 2, 1 + }; + m2x2f.packSquareMatrix(sym2x2); + ASSERT_EQ(3, m2x2f.size()); + + float sym2x2_out[4]; + m2x2f.unpackSquareMatrix(sym2x2_out); + ASSERT_FLOAT_EQ(1, sym2x2_out[0]); + ASSERT_FLOAT_EQ(2, sym2x2_out[1]); + ASSERT_FLOAT_EQ(2, sym2x2_out[2]); + ASSERT_FLOAT_EQ(1, sym2x2_out[3]); + } + { + const float sym3x3[] = + { + 1, 2, 3, + 2, 4, 5, + 3, 5, 6 + }; + m3x3s.packSquareMatrix(sym3x3); + ASSERT_EQ(6, m3x3s.size()); + ASSERT_EQ(1, m3x3s[0]); + ASSERT_EQ(2, m3x3s[1]); + ASSERT_EQ(3, m3x3s[2]); + ASSERT_EQ(4, m3x3s[3]); + ASSERT_EQ(5, m3x3s[4]); + ASSERT_EQ(6, m3x3s[5]); + + float sym3x3_out[9]; + m3x3s.unpackSquareMatrix(sym3x3_out); + + for (int i = 0; i < 9; i++) + { + ASSERT_FLOAT_EQ(sym3x3[i], sym3x3_out[i]); + } + } + { + const double sym6x6[] = + { + 1, 2, 3, 4, 5, 6, + 2, 7, 8, 9, 10, 11, + 3, 8, 12, 13, 14, 15, + 4, 9, 13, 16, 17, 18, + 5, 10, 14, 17, 19, 20, + 6, 11, 15, 18, 20, 21 + }; + m6x6d.packSquareMatrix(sym6x6); + ASSERT_EQ(21, m6x6d.size()); + for (uavcan::uint8_t i = 0; i < 21; i++) + { + ASSERT_DOUBLE_EQ(double(i + 1), m6x6d[i]); + } + + double sym6x6_out[36]; + m6x6d.unpackSquareMatrix(sym6x6_out); + + for (int i = 0; i < 36; i++) + { + ASSERT_DOUBLE_EQ(sym6x6[i], sym6x6_out[i]); + } + } +} + + +TEST(Array, FuzzySquareMatrixPacking) +{ + Array, ArrayModeDynamic, 36> m6x6d; + + // Diagonal matrix will be reduced to an array of length Width + { + float diagonal6x6[] = + { + 1, 0, 0, 0, 0, 0, + 0, -2, 0, 0, 0, 0, + 0, 0, 3, 0, 0, 0, + 0, 0, 0, -4, 0, 0, + 0, 0, 0, 0, 5, 0, + 0, 0, 0, 0, 0, -6 + }; + + // Some almost-zeroes + diagonal6x6[1] = std::numeric_limits::epsilon(); + diagonal6x6[4] = -std::numeric_limits::epsilon(); + diagonal6x6[34] = -std::numeric_limits::epsilon(); + + m6x6d.packSquareMatrix(diagonal6x6); + ASSERT_EQ(6, m6x6d.size()); + ASSERT_DOUBLE_EQ(1, m6x6d[0]); + ASSERT_DOUBLE_EQ(-2, m6x6d[1]); + ASSERT_DOUBLE_EQ(3, m6x6d[2]); + ASSERT_DOUBLE_EQ(-4, m6x6d[3]); + ASSERT_DOUBLE_EQ(5, m6x6d[4]); + ASSERT_DOUBLE_EQ(-6, m6x6d[5]); + + std::vector output(36); + m6x6d.unpackSquareMatrix(output); + + // This comparison will fail because epsilons + ASSERT_FALSE(std::equal(output.begin(), output.end(), diagonal6x6)); + + // This comparison will be ok + ASSERT_TRUE(std::equal(output.begin(), output.end(), diagonal6x6, &uavcan::areClose)); + } +} + + +TEST(Array, SquareMatrixPackingIntegers) +{ + Array, ArrayModeDynamic, 9> m3x3int; + { + const long scalar[] = + { + 42, 0, 0, + 0, 42, 0, + 0, 0, 42 + }; + m3x3int.packSquareMatrix(scalar); + ASSERT_EQ(1, m3x3int.size()); + ASSERT_EQ(42, m3x3int[0]); + + std::vector output(9); + m3x3int.unpackSquareMatrix(output); + ASSERT_TRUE(std::equal(output.begin(), output.end(), scalar)); + } + { + std::vector diagonal(9); + diagonal[0] = 6; + diagonal[4] = -57; + diagonal[8] = 1139; + m3x3int.packSquareMatrix(diagonal); + ASSERT_EQ(3, m3x3int.size()); + ASSERT_EQ(6, m3x3int[0]); + ASSERT_EQ(-57, m3x3int[1]); + ASSERT_EQ(1139, m3x3int[2]); + } + { + std::vector full(9); + for (uint8_t i = 0; i < 9; i++) + { + full[i] = i; + } + m3x3int.packSquareMatrix(full); + ASSERT_EQ(9, m3x3int.size()); + for (uint8_t i = 0; i < 9; i++) + { + ASSERT_EQ(i, m3x3int[i]); + } + } +} + +#if UAVCAN_EXCEPTIONS +TEST(Array, SquareMatrixPackingErrors) +{ + Array, ArrayModeDynamic, 9> m3x3s; + + std::vector ill_formed_row_major(8); + ASSERT_THROW(m3x3s.packSquareMatrix(ill_formed_row_major), std::out_of_range); + + ASSERT_THROW(m3x3s.unpackSquareMatrix(ill_formed_row_major), std::out_of_range); +} +#endif + +TEST(Array, SquareMatrixPackingInPlace) +{ + Array, ArrayModeDynamic, 9> m3x3s; + + // Will do nothing - matrix is empty + m3x3s.packSquareMatrix(); + ASSERT_TRUE(m3x3s.empty()); + + // Will fill with zeros - matrix is empty + m3x3s.unpackSquareMatrix(); + ASSERT_EQ(9, m3x3s.size()); + for (uint8_t i = 0; i < 9; i++) + { + ASSERT_EQ(0, m3x3s[i]); + } + + // Fill an unpackaple matrix + m3x3s.clear(); + m3x3s.push_back(11); + m3x3s.push_back(12); + m3x3s.push_back(13); + +#if UAVCAN_EXCEPTIONS + // Shall throw - matrix is ill-formed + ASSERT_THROW(m3x3s.packSquareMatrix(), std::out_of_range); +#endif + + m3x3s.push_back(21); + m3x3s.push_back(22); + m3x3s.push_back(23); + m3x3s.push_back(31); + m3x3s.push_back(32); + m3x3s.push_back(33); + + // Will pack/unpack successfully + ASSERT_EQ(9, m3x3s.size()); + m3x3s.packSquareMatrix(); + ASSERT_EQ(9, m3x3s.size()); + m3x3s.unpackSquareMatrix(); + + // Make sure it was unpacked properly + ASSERT_EQ(11, m3x3s[0]); + ASSERT_EQ(12, m3x3s[1]); + ASSERT_EQ(13, m3x3s[2]); + ASSERT_EQ(21, m3x3s[3]); + ASSERT_EQ(22, m3x3s[4]); + ASSERT_EQ(23, m3x3s[5]); + ASSERT_EQ(31, m3x3s[6]); + ASSERT_EQ(32, m3x3s[7]); + ASSERT_EQ(33, m3x3s[8]); + + // Try again with a scalar matrix + m3x3s.clear(); + for (unsigned i = 0; i < 9; i++) + { + const bool diagonal = (i == 0) || (i == 4) || (i == 8); + m3x3s.push_back(diagonal ? 123 : 0); + } + + ASSERT_EQ(9, m3x3s.size()); + m3x3s.packSquareMatrix(); + ASSERT_EQ(1, m3x3s.size()); + m3x3s.unpackSquareMatrix(); + ASSERT_EQ(9, m3x3s.size()); + + for (uint8_t i = 0; i < 9; i++) + { + const bool diagonal = (i == 0) || (i == 4) || (i == 8); + ASSERT_EQ((diagonal ? 123 : 0), m3x3s[i]); + } + + // Try again with symmetric matrix + /* + * Full matrix: + * 1 2 3 + * 2 4 5 + * 3 5 6 + * Compressed triangle: + * 1 2 3 + * 4 5 + * 6 + */ + m3x3s.clear(); + m3x3s.push_back(1); + m3x3s.push_back(2); + m3x3s.push_back(3); + m3x3s.push_back(4); + m3x3s.push_back(5); + m3x3s.push_back(6); + // Unpacking + ASSERT_EQ(6, m3x3s.size()); + m3x3s.unpackSquareMatrix(); + ASSERT_EQ(9, m3x3s.size()); + // Validating + ASSERT_EQ(1, m3x3s[0]); + ASSERT_EQ(2, m3x3s[1]); + ASSERT_EQ(3, m3x3s[2]); + ASSERT_EQ(2, m3x3s[3]); + ASSERT_EQ(4, m3x3s[4]); + ASSERT_EQ(5, m3x3s[5]); + ASSERT_EQ(3, m3x3s[6]); + ASSERT_EQ(5, m3x3s[7]); + ASSERT_EQ(6, m3x3s[8]); + // Packing back + m3x3s.packSquareMatrix(); + ASSERT_EQ(6, m3x3s.size()); + // Validating + ASSERT_EQ(1, m3x3s[0]); + ASSERT_EQ(2, m3x3s[1]); + ASSERT_EQ(3, m3x3s[2]); + ASSERT_EQ(4, m3x3s[3]); + ASSERT_EQ(5, m3x3s[4]); + ASSERT_EQ(6, m3x3s[5]); +} + +TEST(Array, FuzzyComparison) +{ + typedef Array, ArrayModeStatic, 2>, + ArrayModeStatic, 2>, + ArrayModeStatic, 2> ArrayStatic32; + + typedef Array, ArrayModeDynamic, 2>, + ArrayModeDynamic, 2>, + ArrayModeDynamic, 2> ArrayDynamic64; + + ArrayStatic32 array_s32; + + ArrayDynamic64 array_d64; + + array_d64.resize(2); + array_d64[0].resize(2); + array_d64[1].resize(2); + array_d64[0][0].resize(2); + array_d64[0][1].resize(2); + array_d64[1][0].resize(2); + array_d64[1][1].resize(2); + + std::cout << "One:"; + uavcan::YamlStreamer::stream(std::cout, array_s32, 0); + std::cout << std::endl << "------"; + uavcan::YamlStreamer::stream(std::cout, array_d64, 0); + std::cout << std::endl; + + // Both are equal right now + ASSERT_TRUE(array_d64 == array_s32); + ASSERT_TRUE(array_d64.isClose(array_s32)); + ASSERT_TRUE(array_s32.isClose(array_d64)); + + // Slightly modifying - still close enough + array_s32[0][0][0] = 123.456F + uavcan::NumericTraits::epsilon() * 123.0F; + array_s32[0][0][1] = uavcan::NumericTraits::infinity(); + array_s32[0][1][0] = uavcan::NumericTraits::epsilon(); + array_s32[0][1][1] = -uavcan::NumericTraits::epsilon(); + + array_d64[0][0][0] = 123.456; + array_d64[0][0][1] = uavcan::NumericTraits::infinity(); + array_d64[0][1][0] = -uavcan::NumericTraits::epsilon(); // Note that the sign is inverted + array_d64[0][1][1] = uavcan::NumericTraits::epsilon(); + + std::cout << "Two:"; + uavcan::YamlStreamer::stream(std::cout, array_s32, 0); + std::cout << std::endl << "------"; + uavcan::YamlStreamer::stream(std::cout, array_d64, 0); + std::cout << std::endl; + + // They are close bot not exactly equal + ASSERT_FALSE(array_d64 == array_s32); + ASSERT_TRUE(array_d64.isClose(array_s32)); + ASSERT_TRUE(array_s32.isClose(array_d64)); + + // Not close + array_d64[0][0][0] = 123.457; + + ASSERT_FALSE(array_d64 == array_s32); + ASSERT_FALSE(array_d64.isClose(array_s32)); + ASSERT_FALSE(array_s32.isClose(array_d64)); + + // Values are close, but lengths differ + array_d64[0][0][0] = 123.456; + + ASSERT_FALSE(array_d64 == array_s32); + ASSERT_TRUE(array_d64.isClose(array_s32)); + ASSERT_TRUE(array_s32.isClose(array_d64)); + + array_d64[0][0].resize(1); + + ASSERT_FALSE(array_d64 == array_s32); + ASSERT_FALSE(array_d64.isClose(array_s32)); + ASSERT_FALSE(array_s32.isClose(array_d64)); + + std::cout << "Three:"; + uavcan::YamlStreamer::stream(std::cout, array_s32, 0); + std::cout << std::endl << "------"; + uavcan::YamlStreamer::stream(std::cout, array_d64, 0); + std::cout << std::endl; +} + +TEST(Array, CaseConversion) +{ + Array, ArrayModeDynamic, 30> str; + + str.convertToLowerCaseASCII(); + str.convertToUpperCaseASCII(); + + ASSERT_STREQ("", str.c_str()); + + str = "Hello World!"; + + ASSERT_STREQ("Hello World!", str.c_str()); + str.convertToLowerCaseASCII(); + ASSERT_STREQ("hello world!", str.c_str()); + str.convertToUpperCaseASCII(); + ASSERT_STREQ("HELLO WORLD!", str.c_str()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/bit_stream.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/bit_stream.cpp new file mode 100644 index 000000000000..85ad2010fc29 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/bit_stream.cpp @@ -0,0 +1,194 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + + +TEST(BitStream, ToString) +{ + { + uavcan::StaticTransferBuffer<8> buf; + uavcan::BitStream bs(buf); + ASSERT_EQ("", bs.toString()); + ASSERT_EQ("", bs.toString()); + } + { + const uint8_t data[] = {0xad}; // 10101101 + uavcan::StaticTransferBuffer<8> buf; + uavcan::BitStream bs(buf); + ASSERT_EQ(1, bs.write(data, 8)); // all 8 + ASSERT_EQ("10101101", bs.toString()); + } + { + const uint8_t data[] = {0xad, 0xbe}; // 10101101 10111110 + uavcan::StaticTransferBuffer<8> buf; + uavcan::BitStream bs(buf); + ASSERT_EQ(1, bs.write(data, 16)); // all 16 + ASSERT_EQ("10101101 10111110", bs.toString()); + } + { + const uint8_t data[] = {0xad, 0xbe, 0xfc}; // 10101101 10111110 11111100 + uavcan::StaticTransferBuffer<8> buf; + uavcan::BitStream bs(buf); + ASSERT_EQ(1, bs.write(data, 20)); // 10101101 10111110 1111 + ASSERT_EQ("10101101 10111110 11110000", bs.toString()); + } +} + + +TEST(BitStream, BitOrderSimple) +{ + /* + * a = 1010 + * b = 1011 + * c = 1100 + * d = 1101 + * e = 1110 + * f = 1111 + */ + uavcan::StaticTransferBuffer<32> buf; + { // Write + const uint8_t data[] = {0xad, 0xbe}; // adbe + uavcan::BitStream bs(buf); + ASSERT_EQ(1, bs.write(data, 12)); // adb0 + ASSERT_EQ("10101101 10110000", bs.toString()); // adb0 + } + { // Read + uavcan::BitStream bs(buf); + ASSERT_EQ("10101101 10110000", bs.toString()); // Same data + uint8_t data[] = {0xFF, 0xFF}; // Uninitialized + ASSERT_EQ(1, bs.read(data, 12)); + ASSERT_EQ(0xad, data[0]); + ASSERT_EQ(0xb0, data[1]); + } +} + + +TEST(BitStream, BitOrderComplex) +{ + static const std::string REFERENCE = + "10101101 10111111 11101111 01010110 11011111 01000100 10001101 00010101 10011110 00100110 10101111 00110111 10111100 00000100"; + + uavcan::StaticTransferBuffer<32> buf; + { // Write + const uint8_t data1[] = {0xad, 0xbe}; // 10101101 10111110 + const uint8_t data2[] = {0xfc}; // 11111100 + const uint8_t data3[] = {0xde, 0xad, 0xbe, 0xef}; // 11011110 10101101 10111110 11101111 + const uint8_t data4[] = {0x12, 0x34, 0x56, 0x78, // 00010010 00110100 01010110 01111000 + 0x9a, 0xbc, 0xde, 0xf0}; // 10011010 10111100 11011110 11110000 + + uavcan::BitStream bs(buf); + ASSERT_EQ(1, bs.write(data1, 11)); // 10101101 101 + std::cout << bs.toString() << std::endl; + ASSERT_EQ(1, bs.write(data2, 6)); // 11111 1 + std::cout << bs.toString() << std::endl; + ASSERT_EQ(1, bs.write(data3, 25)); // 1101111 01010110 11011111 01 + std::cout << bs.toString() << std::endl; + ASSERT_EQ(1, bs.write(data4, 64)); // all 64, total 42 + 64 = 106 + std::cout << bs.toString() << std::endl; + ASSERT_EQ(1, bs.write(data4, 4)); // 0001 + std::cout << bs.toString() << std::endl; + + std::cout << "Reference:\n" << REFERENCE << std::endl; + + ASSERT_EQ(REFERENCE, bs.toString()); + } + { // Read back in the same order + uint8_t data[8]; + std::fill(data, data + sizeof(data), 0xA5); // Filling with garbage + uavcan::BitStream bs(buf); + ASSERT_EQ(REFERENCE, bs.toString()); + + ASSERT_EQ(1, bs.read(data, 11)); // 10101101 10100000 + ASSERT_EQ(0xad, data[0]); + ASSERT_EQ(0xa0, data[1]); + + ASSERT_EQ(1, bs.read(data, 6)); // 11111100 + ASSERT_EQ(0xfc, data[0]); + + ASSERT_EQ(1, bs.read(data, 25)); // 11011110 10101101 10111110 10000000 + ASSERT_EQ(0xde, data[0]); + ASSERT_EQ(0xad, data[1]); + ASSERT_EQ(0xbe, data[2]); + ASSERT_EQ(0x80, data[3]); + + ASSERT_EQ(1, bs.read(data, 64)); // Data - see above + ASSERT_EQ(0x12, data[0]); + ASSERT_EQ(0x34, data[1]); + ASSERT_EQ(0x56, data[2]); + ASSERT_EQ(0x78, data[3]); + ASSERT_EQ(0x9a, data[4]); + ASSERT_EQ(0xbc, data[5]); + ASSERT_EQ(0xde, data[6]); + ASSERT_EQ(0xf0, data[7]); + } +} + + +TEST(BitStream, BitByBit) +{ + static const int NUM_BYTES = 1024; + uavcan::StaticTransferBuffer buf; + uavcan::BitStream bs_wr(buf); + + std::string binary_string; + unsigned counter = 0; + for (int byte = 0; byte < NUM_BYTES; byte++) + { + for (int bit = 0; bit < 8; bit++, counter++) + { + const bool value = counter % 3 == 0; + binary_string.push_back(value ? '1' : '0'); + const uint8_t data[] = { uint8_t(value << 7) }; + ASSERT_EQ(1, bs_wr.write(data, 1)); + } + binary_string.push_back(' '); + } + binary_string.erase(binary_string.length() - 1, 1); + + /* + * Currently we have no free buffer space, so next write() must fail + */ + const uint8_t dummy_data_wr[] = { 0xFF }; + ASSERT_EQ(0, bs_wr.write(dummy_data_wr, 1)); + + /* + * Bitstream content validation + */ +// std::cout << bs.toString() << std::endl; +// std::cout << "Reference:\n" << binary_string << std::endl; + ASSERT_EQ(binary_string, bs_wr.toString()); + + /* + * Read back + */ + uavcan::BitStream bs_rd(buf); + counter = 0; + for (int byte = 0; byte < NUM_BYTES; byte++) + { + for (int bit = 0; bit < 8; bit++, counter++) + { + const bool value = counter % 3 == 0; + uint8_t data[1]; + ASSERT_EQ(1, bs_rd.read(data, 1)); + if (value) + { + ASSERT_EQ(0x80, data[0]); + } + else + { + ASSERT_EQ(0, data[0]); + } + } + } + + /* + * Making sure that reading out of buffer range will fail with error + */ + uint8_t dummy_data_rd[] = { 0xFF }; + ASSERT_EQ(0, bs_wr.read(dummy_data_rd, 1)); + ASSERT_EQ(0xFF, dummy_data_rd[0]); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/char_array_formatter.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/char_array_formatter.cpp new file mode 100644 index 000000000000..134d87039295 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/char_array_formatter.cpp @@ -0,0 +1,80 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +using uavcan::Array; +using uavcan::ArrayModeDynamic; +using uavcan::ArrayModeStatic; +using uavcan::IntegerSpec; +using uavcan::SignednessSigned; +using uavcan::SignednessUnsigned; +using uavcan::CastModeSaturate; +using uavcan::CastModeTruncate; +using uavcan::CharArrayFormatter; + +TEST(CharArrayFormatter, Basic) +{ + typedef Array, ArrayModeDynamic, 45> A8; + A8 a; + + CharArrayFormatter f(a); + ASSERT_STREQ("", f.getArray().c_str()); + + f.write("Don't %s.", "Panic"); + ASSERT_STREQ("Don't Panic.", f.getArray().c_str()); + + f.write(" abc%idef ", 123); + ASSERT_STREQ("Don't Panic. abc123def ", f.getArray().c_str()); + + f.write("%g", 0.0); + ASSERT_STREQ("Don't Panic. abc123def 0", f.getArray().c_str()); + + a.clear(); + ASSERT_STREQ("", f.getArray().c_str()); + + f.write("123456789"); + ASSERT_STREQ("123456789", f.getArray().c_str()); +} + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +TEST(CharArrayFormatter, Hardcore) +{ + typedef Array, ArrayModeDynamic, 150> A8; + A8 a; + + CharArrayFormatter f(a); + + f.write( + "%% char='%*' double='%*' long='%*' unsigned long='%*' int='%s' long double='%*' bool='%*' const char='%*' %%", + '%', -12.3456, -123456789123456789L, 987654321, -123456789, 0.000000001L, true, "Don't Panic."); + + static const std::string Reference = + "% char='%' double='-12.3456' long='-123456789123456789' unsigned long='987654321' int='-123456789' " + "long double='1e-09' bool='1' const char='Don't Pani"; // few chars truncated! + + ASSERT_STREQ(Reference.c_str(), f.getArray().c_str()); + + a.clear(); + + f.write(""); + ASSERT_STREQ("", f.getArray().c_str()); + + f.write("%%"); // Nothing to format --> "%%" is not expanded + ASSERT_STREQ("%%", f.getArray().c_str()); + + f.write("%*", "Test", 123, true); // Extra args ignored + ASSERT_STREQ("%%Test", f.getArray().c_str()); + + f.write("%% %* %* %% %*", true); // Insufficient args are OK; second "%%" is not expanded + ASSERT_STREQ("%%Test% 1 %* %% %*", f.getArray().c_str()); +} + +#endif diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/float_spec.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/float_spec.cpp new file mode 100644 index 000000000000..eacc0522fee3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/float_spec.cpp @@ -0,0 +1,184 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include + + +TEST(FloatSpec, Sizes) +{ + uavcan::StaticAssert::Type) == 4>::check(); + uavcan::StaticAssert::Type) == 4>::check(); + uavcan::StaticAssert::Type) == 8>::check(); + uavcan::StaticAssert::Type) >= 10>::check(); +} + +TEST(FloatSpec, Limits) +{ + using uavcan::FloatSpec; + using uavcan::CastModeSaturate; + using uavcan::CastModeTruncate; + + typedef FloatSpec<16, CastModeSaturate> F16S; + typedef FloatSpec<32, CastModeTruncate> F32T; + typedef FloatSpec<64, CastModeSaturate> F64S; + + ASSERT_FALSE(F16S::IsExactRepresentation); + ASSERT_FLOAT_EQ(65504.0F, F16S::max()); + ASSERT_FLOAT_EQ(9.77e-04F, F16S::epsilon()); + + ASSERT_TRUE(F32T::IsExactRepresentation); + ASSERT_FLOAT_EQ(std::numeric_limits::max(), F32T::max()); + ASSERT_FLOAT_EQ(std::numeric_limits::epsilon(), F32T::epsilon()); + + ASSERT_TRUE(F64S::IsExactRepresentation); + ASSERT_DOUBLE_EQ(std::numeric_limits::max(), F64S::max()); + ASSERT_DOUBLE_EQ(std::numeric_limits::epsilon(), F64S::epsilon()); +} + +TEST(FloatSpec, Basic) +{ + using uavcan::FloatSpec; + using uavcan::CastModeSaturate; + using uavcan::CastModeTruncate; + using uavcan::StorageType; + + typedef FloatSpec<16, CastModeSaturate> F16S; + typedef FloatSpec<16, CastModeTruncate> F16T; + typedef FloatSpec<32, CastModeSaturate> F32S; + typedef FloatSpec<32, CastModeTruncate> F32T; + typedef FloatSpec<64, CastModeSaturate> F64S; + typedef FloatSpec<64, CastModeTruncate> F64T; + + static const long double Values[] = + { + 0.0, + 1.0, + M_PI, + 123, + -123, + 99999, + -999999, + std::numeric_limits::max(), + -std::numeric_limits::max(), + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + nanl("") + }; + static const int NumValues = int(sizeof(Values) / sizeof(Values[0])); + + static const long double ValuesF16S[] = + { + 0.0, + 1.0, + 3.140625, + 123, + -123, + F16S::max(), + -F16S::max(), + F16S::max(), + -F16S::max(), + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + nanl("") + }; + static const long double ValuesF16T[] = + { + 0.0, + 1.0, + 3.140625, + 123, + -123, + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + nanl("") + }; + + /* + * Writing + */ + uavcan::StaticTransferBuffer buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + for (int i = 0; i < NumValues; i++) + { + ASSERT_EQ(1, F16S::encode(float(Values[i]), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F16T::encode(float(Values[i]), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F32S::encode(float(Values[i]), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F32T::encode(float(Values[i]), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F64S::encode(double(Values[i]), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F64T::encode(double(Values[i]), sc_wr, uavcan::TailArrayOptDisabled)); + } + + ASSERT_EQ(0, F16S::encode(0, sc_wr, uavcan::TailArrayOptDisabled)); // Out of buffer space now + + /* + * Reading + */ + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + +#define CHECK(FloatType, expected_value) \ + do { \ + StorageType::Type var = StorageType::Type(); \ + ASSERT_EQ(1, FloatType::decode(var, sc_rd, uavcan::TailArrayOptDisabled)); \ + if (!std::isnan(expected_value)) { \ + ASSERT_DOUBLE_EQ(expected_value, var); } \ + else { \ + ASSERT_EQ(!!std::isnan(expected_value), !!std::isnan(var)); } \ + } while (0) + + for (int i = 0; i < NumValues; i++) + { + CHECK(F16S, float(ValuesF16S[i])); + CHECK(F16T, float(ValuesF16T[i])); + CHECK(F32S, float(Values[i])); + CHECK(F32T, float(Values[i])); + CHECK(F64S, double(Values[i])); + CHECK(F64T, double(Values[i])); + } + +#undef CHECK +} + +TEST(FloatSpec, Float16Representation) +{ + using uavcan::FloatSpec; + using uavcan::CastModeSaturate; + using uavcan::CastModeTruncate; + + typedef FloatSpec<16, CastModeSaturate> F16S; + typedef FloatSpec<16, CastModeTruncate> F16T; + + uavcan::StaticTransferBuffer<2 * 6> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, F16S::encode(0.0, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F16S::encode(1.0, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F16S::encode(-2.0, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F16T::encode(999999, sc_wr, uavcan::TailArrayOptDisabled)); // +inf + ASSERT_EQ(1, F16S::encode(-999999, sc_wr, uavcan::TailArrayOptDisabled)); // -max + ASSERT_EQ(1, F16S::encode(float(nan("")), sc_wr, uavcan::TailArrayOptDisabled)); // nan + + ASSERT_EQ(0, F16S::encode(0, sc_wr, uavcan::TailArrayOptDisabled)); // Out of buffer space now + + // Keep in mind that this is LITTLE ENDIAN representation + static const std::string Reference = + "00000000 00000000 " // 0.0 + "00000000 00111100 " // 1.0 + "00000000 11000000 " // -2.0 + "00000000 01111100 " // +inf + "11111111 11111011 " // -max + "11111111 01111111"; // nan + + ASSERT_EQ(Reference, bs_wr.toString()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/integer_spec.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/integer_spec.cpp new file mode 100644 index 000000000000..abda00a2e7b3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/integer_spec.cpp @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + + +TEST(IntegerSpec, Limits) +{ + using uavcan::IntegerSpec; + using uavcan::SignednessSigned; + using uavcan::SignednessUnsigned; + using uavcan::CastModeSaturate; + using uavcan::CastModeTruncate; + + typedef IntegerSpec<8, SignednessUnsigned, CastModeSaturate> UInt8; + typedef IntegerSpec<4, SignednessSigned, CastModeTruncate> SInt4; + typedef IntegerSpec<32, SignednessUnsigned, CastModeTruncate> UInt32; + typedef IntegerSpec<40, SignednessUnsigned, CastModeSaturate> UInt40; + typedef IntegerSpec<64, SignednessUnsigned, CastModeTruncate> UInt64; + typedef IntegerSpec<64, SignednessSigned, CastModeSaturate> SInt64; + typedef IntegerSpec<63, SignednessUnsigned, CastModeSaturate> UInt63; + + ASSERT_EQ(255, UInt8::max()); + ASSERT_EQ(0, UInt8::min()); + + ASSERT_EQ(7, SInt4::max()); + ASSERT_EQ(-8, SInt4::min()); + + ASSERT_EQ(0xFFFFFFFF, UInt32::max()); + ASSERT_EQ(0, UInt32::min()); + + ASSERT_EQ(0xFFFFFFFFFF, UInt40::max()); + ASSERT_EQ(0, UInt40::min()); + + ASSERT_EQ(0xFFFFFFFFFFFFFFFF, UInt64::max()); + ASSERT_EQ(0, UInt64::min()); + + ASSERT_EQ(0x7FFFFFFFFFFFFFFF, SInt64::max()); + ASSERT_EQ(-0x8000000000000000, SInt64::min()); + + ASSERT_EQ(0x7FFFFFFFFFFFFFFF, UInt63::max()); + ASSERT_EQ(0, UInt63::min()); + + ASSERT_EQ(SInt64::max(), UInt63::max()); +} + + +TEST(IntegerSpec, Basic) +{ + using uavcan::IntegerSpec; + using uavcan::SignednessSigned; + using uavcan::SignednessUnsigned; + using uavcan::CastModeSaturate; + using uavcan::CastModeTruncate; + using uavcan::StorageType; + + uavcan::StaticTransferBuffer<100> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + typedef IntegerSpec<8, SignednessUnsigned, CastModeSaturate> UInt8S; + typedef IntegerSpec<4, SignednessSigned, CastModeTruncate> SInt4T; + typedef IntegerSpec<32, SignednessUnsigned, CastModeTruncate> UInt32T; + typedef IntegerSpec<40, SignednessUnsigned, CastModeSaturate> UInt40S; + typedef IntegerSpec<64, SignednessUnsigned, CastModeTruncate> UInt64T; + typedef IntegerSpec<58, SignednessSigned, CastModeSaturate> SInt58S; + typedef IntegerSpec<63, SignednessUnsigned, CastModeSaturate> UInt63S; + typedef IntegerSpec<10, SignednessSigned, CastModeSaturate> SInt10S; + typedef IntegerSpec<1, SignednessUnsigned, CastModeSaturate> UInt1S; + + ASSERT_EQ(1, UInt8S::encode(UInt8S::StorageType(123), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, SInt4T::encode(SInt4T::StorageType(-0x44), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, UInt32T::encode(UInt32T::StorageType(0xFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, UInt40S::encode(UInt40S::StorageType(0xFFFFFFFFFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, UInt64T::encode(UInt64T::StorageType(0xFFFFFFFFFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, SInt58S::encode(SInt58S::StorageType(0xFFFFFFFFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, UInt63S::encode(UInt63S::StorageType(0xFFFFFFFFFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, SInt10S::encode(SInt10S::StorageType(-30000), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, UInt1S::encode(UInt1S::StorageType(42), sc_wr, uavcan::TailArrayOptDisabled)); + + std::cout << bs_wr.toString() << std::endl; + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + +#define CHECK(IntType, expected_value) \ + do { \ + StorageType::Type var = StorageType::Type(); \ + ASSERT_EQ(1, IntType::decode(var, sc_rd, uavcan::TailArrayOptDisabled)); \ + ASSERT_EQ(expected_value, var); \ + } while (0) + + CHECK(UInt8S, 123); + CHECK(SInt4T, -4); + CHECK(UInt32T, 0xFFFFFFFF); + CHECK(UInt40S, 0xFFFFFFFFFF); + CHECK(UInt64T, 0xFFFFFFFFFFFFFFFF); + CHECK(SInt58S, 0x1FFFFFFFFFFFFFF); + CHECK(UInt63S, 0x7FFFFFFFFFFFFFFF); + CHECK(SInt10S, -512); + CHECK(UInt1S, 1); + +#undef CHECK + + StorageType::Type var; + ASSERT_EQ(0, UInt1S::decode(var, sc_rd, uavcan::TailArrayOptDisabled)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/scalar_codec.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/scalar_codec.cpp new file mode 100644 index 000000000000..e9012f71b5ef --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/scalar_codec.cpp @@ -0,0 +1,98 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include + + +TEST(ScalarCodec, Basic) +{ + uavcan::StaticTransferBuffer<38> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + { + uint64_t u64 = 0; + ASSERT_EQ(0, sc_wr.decode<64>(u64)); // Out of buffer space + } + + /* + * Encoding some variables + */ + ASSERT_EQ(1, sc_wr.encode<12>(uint16_t(0xbeda))); // --> 0xeda + ASSERT_EQ(1, sc_wr.encode<1>(uint8_t(1))); + ASSERT_EQ(1, sc_wr.encode<1>(uint16_t(0))); + ASSERT_EQ(1, sc_wr.encode<4>(uint8_t(8))); + ASSERT_EQ(1, sc_wr.encode<32>(uint32_t(0xdeadbeef))); + ASSERT_EQ(1, sc_wr.encode<3>(int8_t(-1))); + ASSERT_EQ(1, sc_wr.encode<4>(int8_t(-6))); + ASSERT_EQ(1, sc_wr.encode<20>(int32_t(-123456))); + ASSERT_EQ(1, sc_wr.encode<64>(std::numeric_limits::min())); + ASSERT_EQ(1, sc_wr.encode<64>(std::numeric_limits::max())); + ASSERT_EQ(1, sc_wr.encode<15>(int16_t(-1))); + ASSERT_EQ(1, sc_wr.encode<2>(int16_t(-1))); + ASSERT_EQ(1, sc_wr.encode<16>(std::numeric_limits::min())); + ASSERT_EQ(1, sc_wr.encode<64>(std::numeric_limits::max())); // Total 302 bit (38 bytes) + + ASSERT_EQ(0, sc_wr.encode<64>(std::numeric_limits::min())); // Out of buffer space + + /* + * Decoding back + */ + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + + uint8_t u8 = 0; + int8_t i8 = 0; + uint16_t u16 = 0; + int16_t i16 = 0; + uint32_t u32 = 0; + int32_t i32 = 0; + uint64_t u64 = 0; + int64_t i64 = 0; + +#define CHECK(bitlen, variable, expected_value) \ + do { \ + ASSERT_EQ(1, sc_rd.decode(variable)); \ + ASSERT_EQ(expected_value, variable); \ + } while (0) + + CHECK(12, u16, 0xeda); + CHECK(1, u8, 1); + CHECK(1, u16, 0); + CHECK(4, u8, 8); + CHECK(32, u32, 0xdeadbeef); + CHECK(3, i8, -1); + CHECK(4, i8, -6); + CHECK(20, i32, -123456); + CHECK(64, i64, std::numeric_limits::min()); + CHECK(64, i64, std::numeric_limits::max()); + CHECK(15, i16, -1); + CHECK(2, i8, -1); + CHECK(16, i16, std::numeric_limits::min()); + CHECK(64, u64, std::numeric_limits::max()); + +#undef CHECK + + ASSERT_EQ(0, sc_rd.decode<64>(u64)); // Out of buffer space +} + +TEST(ScalarCodec, RepresentationCorrectness) +{ + uavcan::StaticTransferBuffer<4> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, sc_wr.encode<12>(uint16_t(0xbeda))); // --> 0xeda + ASSERT_EQ(1, sc_wr.encode<3>(int8_t(-1))); + ASSERT_EQ(1, sc_wr.encode<4>(int8_t(-5))); + ASSERT_EQ(1, sc_wr.encode<2>(int16_t(-1))); + ASSERT_EQ(1, sc_wr.encode<4>(uint8_t(0x88))); // --> 8 + + // This representation was carefully crafted and triple checked: + static const std::string REFERENCE = "11011010 11101111 01111100 00000000"; + ASSERT_EQ(REFERENCE, bs_wr.toString()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/type_util.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/type_util.cpp new file mode 100644 index 000000000000..e35b730e52dd --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/type_util.cpp @@ -0,0 +1,30 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + + +TEST(MarshalTypeUtil, IntegerBitLen) +{ + using uavcan::IntegerBitLen; + + ASSERT_EQ(0, IntegerBitLen<0>::Result); + ASSERT_EQ(1, IntegerBitLen<1>::Result); + ASSERT_EQ(6, IntegerBitLen<42>::Result); + ASSERT_EQ(8, IntegerBitLen<232>::Result); + ASSERT_EQ(32, IntegerBitLen<0x81234567>::Result); +} + + +TEST(MarshalTypeUtil, BitLenToByteLen) +{ + using uavcan::BitLenToByteLen; + + ASSERT_EQ(2, BitLenToByteLen<16>::Result); + ASSERT_EQ(1, BitLenToByteLen<8>::Result); + ASSERT_EQ(1, BitLenToByteLen<7>::Result); + ASSERT_EQ(1, BitLenToByteLen<1>::Result); + ASSERT_EQ(2, BitLenToByteLen<9>::Result); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/global_data_type_registry.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/global_data_type_registry.cpp new file mode 100644 index 000000000000..f81b2902852a --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/global_data_type_registry.cpp @@ -0,0 +1,168 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +namespace +{ + +struct DataTypeAMessage +{ + enum { DefaultDataTypeID = 0 }; + enum { DataTypeKind = uavcan::DataTypeKindMessage }; + static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(123); } + static const char* getDataTypeFullName() { return "my_namespace.DataTypeA"; } +}; + +struct DataTypeAService +{ + enum { DefaultDataTypeID = 0 }; + enum { DataTypeKind = uavcan::DataTypeKindService }; + static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(789); } + static const char* getDataTypeFullName() { return "my_namespace.DataTypeA"; } +}; + +struct DataTypeB +{ + enum { DefaultDataTypeID = 42 }; + enum { DataTypeKind = uavcan::DataTypeKindMessage }; + static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(456); } + static const char* getDataTypeFullName() { return "my_namespace.DataTypeB"; } +}; + +struct DataTypeC +{ + enum { DefaultDataTypeID = 1023 }; + enum { DataTypeKind = uavcan::DataTypeKindMessage }; + static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(654); } + static const char* getDataTypeFullName() { return "foo.DataTypeC"; } +}; + +struct DataTypeD +{ + enum { DefaultDataTypeID = 43 }; + enum { DataTypeKind = uavcan::DataTypeKindService }; + static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(987); } + static const char* getDataTypeFullName() { return "foo.DataTypeD"; } +}; + +template +uavcan::DataTypeDescriptor extractDescriptor(uint16_t dtid = Type::DefaultDataTypeID) +{ + return uavcan::DataTypeDescriptor(uavcan::DataTypeKind(Type::DataTypeKind), dtid, + Type::getDataTypeSignature(), Type::getDataTypeFullName()); +} + +} + + +TEST(GlobalDataTypeRegistry, Basic) +{ + using uavcan::GlobalDataTypeRegistry; + using uavcan::DataTypeSignature; + + GlobalDataTypeRegistry::instance().reset(); + ASSERT_FALSE(GlobalDataTypeRegistry::instance().isFrozen()); + ASSERT_EQ(0, GlobalDataTypeRegistry::instance().getNumMessageTypes()); + ASSERT_EQ(0, GlobalDataTypeRegistry::instance().getNumServiceTypes()); + + /* + * Static registrations + */ + uavcan::DefaultDataTypeRegistrator reg_DataTypeAMessage; + uavcan::DefaultDataTypeRegistrator reg_DataTypeB; + + ASSERT_EQ(2, GlobalDataTypeRegistry::instance().getNumMessageTypes()); + ASSERT_EQ(0, GlobalDataTypeRegistry::instance().getNumServiceTypes()); + + /* + * Runtime registrations + */ + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultOk, + GlobalDataTypeRegistry::instance().registerDataType( + DataTypeAService::DefaultDataTypeID)); + + ASSERT_EQ(2, GlobalDataTypeRegistry::instance().getNumMessageTypes()); + ASSERT_EQ(1, GlobalDataTypeRegistry::instance().getNumServiceTypes()); + + /* + * Runtime re-registration + */ + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultOk, + GlobalDataTypeRegistry::instance().registerDataType(147)); + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultOk, + GlobalDataTypeRegistry::instance().registerDataType(741)); + + ASSERT_EQ(2, GlobalDataTypeRegistry::instance().getNumMessageTypes()); + ASSERT_EQ(1, GlobalDataTypeRegistry::instance().getNumServiceTypes()); + + /* + * These types will be necessary for the aggregate signature test + */ + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultCollision, + GlobalDataTypeRegistry::instance().registerDataType(741)); // ID COLLISION + + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultOk, + GlobalDataTypeRegistry::instance().registerDataType(DataTypeC::DefaultDataTypeID)); + uavcan::DefaultDataTypeRegistrator reg_DataTypeD; + + /* + * Frozen state + */ + GlobalDataTypeRegistry::instance().freeze(); + + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultFrozen, + GlobalDataTypeRegistry::instance().registerDataType(555)); // Rejected + + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultFrozen, + GlobalDataTypeRegistry::instance().registerDataType(999)); // Rejected + + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultFrozen, + GlobalDataTypeRegistry::instance().registerDataType(888)); // Rejected + + /* + * Searching + */ + const uavcan::DataTypeDescriptor* pdtd = UAVCAN_NULLPTR; + ASSERT_FALSE(GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, "Nonexistent")); + ASSERT_FALSE(GlobalDataTypeRegistry::instance().find("Nonexistent")); + ASSERT_FALSE(GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, 987)); + // Asking for service, but this is a message: + ASSERT_FALSE(GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, "my_namespace.DataTypeB")); + ASSERT_FALSE(GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, 42)); + + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, + "my_namespace.DataTypeB"))); + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find("my_namespace.DataTypeB"))); + ASSERT_EQ(extractDescriptor(741), *pdtd); + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, 741))); + ASSERT_EQ(extractDescriptor(741), *pdtd); + + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, + "my_namespace.DataTypeA"))); + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find("my_namespace.DataTypeA"))); + ASSERT_EQ(extractDescriptor(), *pdtd); + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, uavcan::DataTypeID(0)))); + ASSERT_EQ(extractDescriptor(), *pdtd); + + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, + "my_namespace.DataTypeA"))); + ASSERT_EQ(extractDescriptor(147), *pdtd); + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, 147))); + ASSERT_EQ(extractDescriptor(147), *pdtd); +} + + +TEST(GlobalDataTypeRegistry, Reset) +{ + using uavcan::GlobalDataTypeRegistry; + + /* + * Since we're dealing with singleton, we need to reset it for other tests to use + */ + ASSERT_TRUE(GlobalDataTypeRegistry::instance().isFrozen()); + GlobalDataTypeRegistry::instance().reset(); + ASSERT_FALSE(GlobalDataTypeRegistry::instance().isFrozen()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/node.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/node.cpp new file mode 100644 index 000000000000..cc4a4ff03102 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/node.cpp @@ -0,0 +1,90 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include // Compilability test +#include +#include "test_node.hpp" +#include "../protocol/helpers.hpp" + +static void registerTypes() +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + uavcan::DefaultDataTypeRegistrator _reg6; + uavcan::DefaultDataTypeRegistrator _reg7; + uavcan::DefaultDataTypeRegistrator _reg8; +} + + +TEST(Node, Basic) +{ + registerTypes(); + InterlinkedTestNodesWithSysClock nodes; + + uavcan::protocol::SoftwareVersion swver; + swver.major = 0; + swver.minor = 1; + swver.vcs_commit = 0xDEADBEEF; + + std::cout << "sizeof(uavcan::Node<0>): " << sizeof(uavcan::Node<0>) << std::endl; + + /* + * uavcan::Node + */ + uavcan::Node<1024> node1(nodes.can_a, nodes.clock_a); + node1.setName("com.example"); + node1.setNodeID(1); + node1.setSoftwareVersion(swver); + + /* + * Companion test node + */ + uavcan::Node<1024> node2(nodes.can_b, nodes.clock_b); + node2.setName("foobar"); + node2.setNodeID(2); + node2.setSoftwareVersion(swver); + + BackgroundSpinner bgspinner(node2, node1); + bgspinner.startPeriodic(uavcan::MonotonicDuration::fromMSec(10)); + + uavcan::NodeStatusMonitor node_status_monitor(node2); + ASSERT_LE(0, node_status_monitor.start()); + + /* + * Init the second node - network is empty + */ + ASSERT_LE(0, node2.start()); + ASSERT_FALSE(node_status_monitor.findNodeWithWorstHealth().isValid()); + + /* + * Init the first node + */ + ASSERT_FALSE(node1.isStarted()); + ASSERT_EQ(-uavcan::ErrNotInited, node1.spin(uavcan::MonotonicDuration::fromMSec(20))); + ASSERT_LE(0, node1.start()); + ASSERT_TRUE(node1.isStarted()); + + ASSERT_LE(0, node1.spin(uavcan::MonotonicDuration::fromMSec(2000))); + + ASSERT_EQ(1, node_status_monitor.findNodeWithWorstHealth().get()); + + /* + * Some logging + */ + SubscriberWithCollector log_sub(node2); + ASSERT_LE(0, log_sub.start()); + + node1.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + node1.logInfo("test", "6 * 9 = 42"); + + ASSERT_LE(0, node1.spin(uavcan::MonotonicDuration::fromMSec(20))); + ASSERT_LE(0, node2.spin(uavcan::MonotonicDuration::fromMSec(20))); + + ASSERT_TRUE(log_sub.collector.msg.get()); + std::cout << *log_sub.collector.msg << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/publisher.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/publisher.cpp new file mode 100644 index 000000000000..f3332a4e0e30 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/publisher.cpp @@ -0,0 +1,97 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "../clock.hpp" +#include "../transport/can/can.hpp" +#include "test_node.hpp" + + +TEST(Publisher, Basic) +{ + SystemClockMock clock_mock(100); + CanDriverMock can_driver(2, clock_mock); + TestNode node(can_driver, clock_mock, 1); + + uavcan::Publisher publisher(node); + + ASSERT_FALSE(publisher.getTransferSender().isInitialized()); + + std::cout << + "sizeof(uavcan::Publisher): " << + sizeof(uavcan::Publisher) << std::endl; + + // Manual type registration - we can't rely on the GDTR state + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + /* + * Message layout: + * uint8 seq + * uint8 sysid + * uint8 compid + * uint8 msgid + * uint8[<256] payload + */ + root_ns_a::MavlinkMessage msg; + msg.seq = 0x42; + msg.sysid = 0x72; + msg.compid = 0x08; + msg.msgid = 0xa5; + msg.payload = "Msg"; + + const uint8_t expected_transfer_payload[] = {0x42, 0x72, 0x08, 0xa5, 'M', 's', 'g'}; + const uint64_t tx_timeout_usec = uint64_t(publisher.getDefaultTxTimeout().toUSec()); + + /* + * Broadcast + */ + { + ASSERT_LT(0, publisher.broadcast(msg)); + + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false + uavcan::Frame expected_frame(root_ns_a::MavlinkMessage::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + node.getNodeID(), uavcan::NodeID::Broadcast, 0); + expected_frame.setPayload(expected_transfer_payload, 7); + expected_frame.setStartOfTransfer(true); + expected_frame.setEndOfTransfer(true); + + uavcan::CanFrame expected_can_frame; + ASSERT_TRUE(expected_frame.compile(expected_can_frame)); + + ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100)); + ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100)); + ASSERT_TRUE(can_driver.ifaces[0].tx.empty()); + ASSERT_TRUE(can_driver.ifaces[1].tx.empty()); + + // Second shot - checking the transfer ID + publisher.setPriority(10); + ASSERT_LT(0, publisher.broadcast(msg)); + + expected_frame = uavcan::Frame(root_ns_a::MavlinkMessage::DefaultDataTypeID, + uavcan::TransferTypeMessageBroadcast, + node.getNodeID(), uavcan::NodeID::Broadcast, 1); + expected_frame.setStartOfTransfer(true); + expected_frame.setEndOfTransfer(true); + expected_frame.setPayload(expected_transfer_payload, 7); + expected_frame.setPriority(10); + ASSERT_TRUE(expected_frame.compile(expected_can_frame)); + + ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100)); + ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100)); + ASSERT_TRUE(can_driver.ifaces[0].tx.empty()); + ASSERT_TRUE(can_driver.ifaces[1].tx.empty()); + } + + clock_mock.advance(1000); + + /* + * Misc + */ + ASSERT_TRUE(uavcan::GlobalDataTypeRegistry::instance().isFrozen()); + ASSERT_TRUE(publisher.getTransferSender().isInitialized()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/scheduler.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/scheduler.cpp new file mode 100644 index 000000000000..6e8fad28644f --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/scheduler.cpp @@ -0,0 +1,116 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "../clock.hpp" +#include "../transport/can/can.hpp" +#include "test_node.hpp" + +#if !defined(UAVCAN_CPP11) || !defined(UAVCAN_CPP_VERSION) +# error UAVCAN_CPP_VERSION +#endif + +struct TimerCallCounter +{ + std::vector events_a; + std::vector events_b; + + void callA(const uavcan::TimerEvent& ev) { events_a.push_back(ev); } + void callB(const uavcan::TimerEvent& ev) { events_b.push_back(ev); } + + typedef uavcan::MethodBinder Binder; + + Binder bindA() { return Binder(this, &TimerCallCounter::callA); } + Binder bindB() { return Binder(this, &TimerCallCounter::callB); } +}; + +/* + * This test can fail on a non real time system. That's kinda sad but okay. + */ +TEST(Scheduler, Timers) +{ + SystemClockDriver clock_driver; + CanDriverMock can_driver(2, clock_driver); + TestNode node(can_driver, clock_driver, 1); + + /* + * Registration + */ + { + TimerCallCounter tcc; + uavcan::TimerEventForwarder a(node, tcc.bindA()); + uavcan::TimerEventForwarder b(node, tcc.bindB()); + + ASSERT_EQ(0, node.getScheduler().getDeadlineScheduler().getNumHandlers()); + + const uavcan::MonotonicTime start_ts = clock_driver.getMonotonic(); + + a.startOneShotWithDeadline(start_ts + durMono(100000)); + b.startPeriodic(durMono(1000)); + + ASSERT_EQ(2, node.getScheduler().getDeadlineScheduler().getNumHandlers()); + + /* + * Spinning + */ + ASSERT_EQ(0, node.spin(start_ts + durMono(1000000))); + + ASSERT_EQ(1, tcc.events_a.size()); + ASSERT_TRUE(areTimestampsClose(tcc.events_a[0].scheduled_time, start_ts + durMono(100000))); + ASSERT_TRUE(areTimestampsClose(tcc.events_a[0].scheduled_time, tcc.events_a[0].real_time)); + + ASSERT_LT(900, tcc.events_b.size()); + ASSERT_GT(1100, tcc.events_b.size()); + { + uavcan::MonotonicTime next_expected_deadline = start_ts + durMono(1000); + for (unsigned i = 0; i < tcc.events_b.size(); i++) + { + ASSERT_TRUE(areTimestampsClose(tcc.events_b[i].scheduled_time, next_expected_deadline)); + ASSERT_TRUE(areTimestampsClose(tcc.events_b[i].scheduled_time, tcc.events_b[i].real_time)); + next_expected_deadline += durMono(1000); + } + } + + /* + * Deinitialization + */ + ASSERT_EQ(1, node.getScheduler().getDeadlineScheduler().getNumHandlers()); + + ASSERT_FALSE(a.isRunning()); + ASSERT_EQ(uavcan::MonotonicDuration::getInfinite(), a.getPeriod()); + + ASSERT_TRUE(b.isRunning()); + ASSERT_EQ(1000, b.getPeriod().toUSec()); + } + + ASSERT_EQ(0, node.getScheduler().getDeadlineScheduler().getNumHandlers()); // Both timers were destroyed by now + ASSERT_EQ(0, node.spin(durMono(1000))); // Spin some more without timers +} + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +TEST(Scheduler, TimerCpp11) +{ + SystemClockDriver clock_driver; + CanDriverMock can_driver(2, clock_driver); + TestNode node(can_driver, clock_driver, 1); + + int count = 0; + + uavcan::Timer tm(node, [&count](const uavcan::TimerEvent&) { count++; }); + + ASSERT_EQ(0, node.getScheduler().getDeadlineScheduler().getNumHandlers()); + tm.startPeriodic(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_EQ(1, node.getScheduler().getDeadlineScheduler().getNumHandlers()); + + ASSERT_EQ(0, node.spin(uavcan::MonotonicDuration::fromMSec(100))); + + std::cout << count << std::endl; + ASSERT_LE(5, count); + ASSERT_GE(15, count); +} + +#endif diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/service_client.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/service_client.cpp new file mode 100644 index 000000000000..28014eecc35f --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/service_client.cpp @@ -0,0 +1,454 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_node.hpp" + + +template +struct ServiceCallResultHandler +{ + typedef typename uavcan::ServiceCallResult::Status StatusType; + StatusType last_status; + uavcan::NodeID last_server_node_id; + typename DataType::Response last_response; + std::queue responses; + + void handleResponse(const uavcan::ServiceCallResult& result) + { + std::cout << result << std::endl; + last_status = result.getStatus(); + last_response = result.getResponse(); + last_server_node_id = result.getCallID().server_node_id; + responses.push(result.getResponse()); + } + + bool match(StatusType status, uavcan::NodeID server_node_id, const typename DataType::Response& response) const + { + if (status == last_status && + server_node_id == last_server_node_id && + response == last_response) + { + return true; + } + else + { + std::cout << "MISMATCH: status=" << int(last_status) << ", last_server_node_id=" + << int(last_server_node_id.get()) << ", last response:\n" << last_response << std::endl; + return false; + } + } + + typedef uavcan::MethodBinder&)> Binder; + + Binder bind() { return Binder(this, &ServiceCallResultHandler::handleResponse); } +}; + + +static void stringServiceServerCallback(const uavcan::ReceivedDataStructure& req, + uavcan::ServiceResponseDataStructure& rsp) +{ + rsp.string_response = "Request string: "; + rsp.string_response += req.string_request; +} + +static void rejectingStringServiceServerCallback( + const uavcan::ReceivedDataStructure& req, + uavcan::ServiceResponseDataStructure& rsp) +{ + rsp.string_response = "Request string: "; + rsp.string_response += req.string_request; + ASSERT_TRUE(rsp.isResponseEnabled()); + rsp.setResponseEnabled(false); + ASSERT_FALSE(rsp.isResponseEnabled()); +} + +static void emptyServiceServerCallback(const uavcan::ReceivedDataStructure&, + uavcan::ServiceResponseDataStructure&) +{ + // Nothing to do - the service is empty +} + + +TEST(ServiceClient, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + // Type registration + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + // Server + uavcan::ServiceServer server(nodes.a); + ASSERT_EQ(0, server.start(stringServiceServerCallback)); + + { + // Caller + typedef uavcan::ServiceCallResult ResultType; + typedef uavcan::ServiceClient::Binder > ClientType; + ServiceCallResultHandler handler; + + ClientType client1(nodes.b); + ClientType client2(nodes.b); + ClientType client3(nodes.b); + + ASSERT_EQ(0, client1.getNumPendingCalls()); + ASSERT_EQ(0, client2.getNumPendingCalls()); + ASSERT_EQ(0, client3.getNumPendingCalls()); + + ASSERT_FALSE(client1.hasPendingCallToServer(1)); + + client1.setCallback(handler.bind()); + client2.setCallback(client1.getCallback()); + client3.setCallback(client1.getCallback()); + client3.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100)); + + ASSERT_EQ(1, nodes.a.getDispatcher().getNumServiceRequestListeners()); + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // NOT listening! + + root_ns_a::StringService::Request request; + request.string_request = "Hello world"; + + std::cout << "!!! Calling!" << std::endl; + + ASSERT_LT(0, client1.call(1, request)); // OK + ASSERT_LT(0, client1.call(1, request)); // OK - second request + ASSERT_LT(0, client2.call(1, request)); // OK + ASSERT_LT(0, client3.call(99, request)); // Will timeout! + ASSERT_LT(0, client3.call(1, request)); // OK - second request + + ASSERT_TRUE(client1.hasPendingCallToServer(1)); + ASSERT_TRUE(client2.hasPendingCallToServer(1)); + ASSERT_TRUE(client3.hasPendingCallToServer(99)); + ASSERT_TRUE(client3.hasPendingCallToServer(1)); + + std::cout << "!!! Spinning!" << std::endl; + + ASSERT_EQ(3, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Listening now! + + ASSERT_TRUE(client1.hasPendingCalls()); + ASSERT_TRUE(client2.hasPendingCalls()); + ASSERT_TRUE(client3.hasPendingCalls()); + + ASSERT_EQ(2, client1.getNumPendingCalls()); + ASSERT_EQ(1, client2.getNumPendingCalls()); + ASSERT_EQ(2, client3.getNumPendingCalls()); + + ASSERT_EQ(uavcan::NodeID(1), client2.getCallIDByIndex(0).server_node_id); + ASSERT_EQ(uavcan::NodeID(), client2.getCallIDByIndex(1).server_node_id); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20)); + + std::cout << "!!! Spin finished!" << std::endl; + + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third is still listening! + + ASSERT_FALSE(client1.hasPendingCalls()); + ASSERT_FALSE(client2.hasPendingCalls()); + ASSERT_TRUE(client3.hasPendingCalls()); + + ASSERT_EQ(0, client1.getNumPendingCalls()); + ASSERT_EQ(0, client2.getNumPendingCalls()); + ASSERT_EQ(1, client3.getNumPendingCalls()); // The one addressed to 99 is still waiting + + // Validating + root_ns_a::StringService::Response expected_response; + expected_response.string_response = "Request string: Hello world"; + ASSERT_TRUE(handler.match(ResultType::Success, 1, expected_response)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200)); + + ASSERT_FALSE(client1.hasPendingCalls()); + ASSERT_FALSE(client2.hasPendingCalls()); + ASSERT_FALSE(client3.hasPendingCalls()); + + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third has timed out :( + + // Validating + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response())); + + // Stray request + ASSERT_LT(0, client3.call(99, request)); // Will timeout! + ASSERT_TRUE(client3.hasPendingCalls()); + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); + } + + // All destroyed - nobody listening + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); +} + + +TEST(ServiceClient, Rejection) +{ + InterlinkedTestNodesWithSysClock nodes; + + // Type registration + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + // Server + uavcan::ServiceServer server(nodes.a); + ASSERT_EQ(0, server.start(rejectingStringServiceServerCallback)); + + // Caller + typedef uavcan::ServiceCallResult ResultType; + typedef uavcan::ServiceClient::Binder > ClientType; + ServiceCallResultHandler handler; + + ClientType client1(nodes.b); + client1.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100)); + client1.setCallback(handler.bind()); + + root_ns_a::StringService::Request request; + request.string_request = "Hello world"; + + ASSERT_LT(0, client1.call(1, request)); + + ASSERT_EQ(uavcan::NodeID(1), client1.getCallIDByIndex(0).server_node_id); + ASSERT_EQ(uavcan::NodeID(), client1.getCallIDByIndex(1).server_node_id); + + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); + ASSERT_TRUE(client1.hasPendingCalls()); + ASSERT_TRUE(client1.hasPendingCallToServer(1)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200)); + ASSERT_FALSE(client1.hasPendingCalls()); + ASSERT_FALSE(client1.hasPendingCallToServer(1)); + + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Timed out + + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 1, root_ns_a::StringService::Response())); +} + + +TEST(ServiceClient, ConcurrentCalls) +{ + InterlinkedTestNodesWithSysClock nodes; + + // Type registration + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + // Server + uavcan::ServiceServer server(nodes.a); + ASSERT_EQ(0, server.start(stringServiceServerCallback)); + + // Caller + typedef uavcan::ServiceCallResult ResultType; + typedef uavcan::ServiceClient::Binder > ClientType; + ServiceCallResultHandler handler; + + /* + * Initializing + */ + ClientType client(nodes.b); + + ASSERT_EQ(0, client.getNumPendingCalls()); + + client.setCallback(handler.bind()); + + ASSERT_EQ(1, nodes.a.getDispatcher().getNumServiceRequestListeners()); + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // NOT listening! + + ASSERT_FALSE(client.hasPendingCalls()); + ASSERT_EQ(0, client.getNumPendingCalls()); + + /* + * Calling ten requests, the last one will be cancelled + * Note that there will be non-unique transfer ID values; the client must handle that correctly + */ + uavcan::ServiceCallID last_call_id; + for (int i = 0; i < 10; i++) + { + std::ostringstream os; + os << i; + root_ns_a::StringService::Request request; + request.string_request = os.str().c_str(); + ASSERT_LT(0, client.call(1, request, last_call_id)); + } + + ASSERT_LT(0, client.call(99, root_ns_a::StringService::Request())); // Will timeout in 1000 ms + + client.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100)); + + ASSERT_LT(0, client.call(88, root_ns_a::StringService::Request())); // Will timeout in 100 ms + + ASSERT_TRUE(client.hasPendingCalls()); + ASSERT_EQ(12, client.getNumPendingCalls()); + + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Listening + + /* + * Cancelling one + */ + client.cancelCall(last_call_id); + + ASSERT_TRUE(client.hasPendingCalls()); + ASSERT_EQ(11, client.getNumPendingCalls()); + + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Still listening + + /* + * Spinning + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20)); + + ASSERT_TRUE(client.hasPendingCalls()); + ASSERT_EQ(2, client.getNumPendingCalls()); // Two still pending + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Still listening + + /* + * Validating the ones that didn't timeout + */ + root_ns_a::StringService::Response last_response; + for (int i = 0; i < 9; i++) + { + std::ostringstream os; + os << "Request string: " << i; + last_response.string_response = os.str().c_str(); + + ASSERT_FALSE(handler.responses.empty()); + + ASSERT_STREQ(last_response.string_response.c_str(), handler.responses.front().string_response.c_str()); + + handler.responses.pop(); + } + + ASSERT_TRUE(handler.responses.empty()); + + /* + * Validating the 100 ms timeout + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + + ASSERT_TRUE(client.hasPendingCalls()); + ASSERT_EQ(1, client.getNumPendingCalls()); // One dropped + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Still listening + + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 88, root_ns_a::StringService::Response())); + + /* + * Validating the 1000 ms timeout + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + + ASSERT_FALSE(client.hasPendingCalls()); + ASSERT_EQ(0, client.getNumPendingCalls()); // All finished + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Not listening + + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response())); +} + + +TEST(ServiceClient, Empty) +{ + InterlinkedTestNodesWithSysClock nodes; + + // Type registration + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + // Server + uavcan::ServiceServer server(nodes.a); + ASSERT_EQ(0, server.start(emptyServiceServerCallback)); + + { + // Caller + typedef uavcan::ServiceClient::Binder > ClientType; + ServiceCallResultHandler handler; + + ClientType client(nodes.b); + + client.setCallback(handler.bind()); + + root_ns_a::EmptyService::Request request; + + ASSERT_LT(0, client.call(1, request)); // OK + } + + // All destroyed - nobody listening + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); +} + + +TEST(ServiceClient, Priority) +{ + InterlinkedTestNodesWithSysClock nodes; + + // Type registration + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + // Initializing + typedef uavcan::ServiceClient::Binder > ClientType; + ServiceCallResultHandler handler; + ClientType client(nodes.b); + client.setCallback(handler.bind()); + + // Calling + root_ns_a::EmptyService::Request request; + + client.setPriority(0); + ASSERT_LT(0, client.call(1, request)); // OK + + client.setPriority(10); + ASSERT_LT(0, client.call(1, request)); // OK + + client.setPriority(20); + ASSERT_LT(0, client.call(1, request)); // OK + + client.setPriority(31); + ASSERT_LT(0, client.call(1, request)); // OK + + // Validating + ASSERT_EQ(4, nodes.can_a.read_queue.size()); + + uavcan::Frame frame; + + ASSERT_TRUE(frame.parse(nodes.can_a.read_queue.front())); + nodes.can_a.read_queue.pop(); + ASSERT_EQ(0, frame.getPriority().get()); + + ASSERT_TRUE(frame.parse(nodes.can_a.read_queue.front())); + nodes.can_a.read_queue.pop(); + ASSERT_EQ(10, frame.getPriority().get()); + + ASSERT_TRUE(frame.parse(nodes.can_a.read_queue.front())); + nodes.can_a.read_queue.pop(); + ASSERT_EQ(20, frame.getPriority().get()); + + ASSERT_TRUE(frame.parse(nodes.can_a.read_queue.front())); + nodes.can_a.read_queue.pop(); + ASSERT_EQ(31, frame.getPriority().get()); +} + + +TEST(ServiceClient, Sizes) +{ + using namespace uavcan; + + std::cout << "GetDataTypeInfo server: " << + sizeof(ServiceServer) << std::endl; + + std::cout << "RestartNode server: " << + sizeof(ServiceServer) << std::endl; + + std::cout << "GetDataTypeInfo client: " << + sizeof(ServiceClient) << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/service_server.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/service_server.cpp new file mode 100644 index 000000000000..3d1efcbe9b76 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/service_server.cpp @@ -0,0 +1,156 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include "../clock.hpp" +#include "../transport/can/can.hpp" +#include "test_node.hpp" + + +struct StringServerImpl +{ + const char* string_response; + + StringServerImpl(const char* string_response) : string_response(string_response) { } + + void handleRequest(const uavcan::ReceivedDataStructure& request, + root_ns_a::StringService::Response& response) + { + std::cout << request << std::endl; + response.string_response = request.string_request; + response.string_response += " --> "; + response.string_response += string_response; + std::cout << response << std::endl; + } + + typedef uavcan::MethodBinder&, + root_ns_a::StringService::Response&)> Binder; + + Binder bind() { return Binder(this, &StringServerImpl::handleRequest); } +}; + + +struct EmptyServerImpl +{ + void handleRequest(const uavcan::ReceivedDataStructure& request, + root_ns_a::EmptyService::Response&) + { + std::cout << request << std::endl; + } + + typedef uavcan::MethodBinder&, + root_ns_a::EmptyService::Response&)> Binder; + + Binder bind() { return Binder(this, &EmptyServerImpl::handleRequest); } +}; + + +TEST(ServiceServer, Basic) +{ + // Manual type registration - we can't rely on the GDTR state + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + SystemClockDriver clock_driver; + CanDriverMock can_driver(1, clock_driver); + TestNode node(can_driver, clock_driver, 1); + + StringServerImpl impl("456"); + + { + uavcan::ServiceServer server(node); + + ASSERT_EQ(0, node.getDispatcher().getNumServiceRequestListeners()); + server.start(impl.bind()); + ASSERT_EQ(1, node.getDispatcher().getNumServiceRequestListeners()); + + /* + * Request frames + */ + for (uint8_t i = 0; i < 2; i++) + { + uavcan::Frame frame(root_ns_a::StringService::DefaultDataTypeID, uavcan::TransferTypeServiceRequest, + uavcan::NodeID(uint8_t(i + 0x10)), 1, i); + + const uint8_t req[] = {'r', 'e', 'q', uint8_t(i + '0')}; + frame.setPayload(req, sizeof(req)); + + frame.setStartOfTransfer(true); + frame.setEndOfTransfer(true); + frame.setPriority(i); + + uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0); + can_driver.ifaces[0].pushRx(rx_frame); + } + + node.spin(clock_driver.getMonotonic() + uavcan::MonotonicDuration::fromUSec(10000)); + + /* + * Responses (MFT) + */ + ASSERT_EQ(4, can_driver.ifaces[0].tx.size()); + for (int i = 0; i < 2; i++) + { + char payloads[2][8]; + std::snprintf(payloads[0], 8, "req%i ", i); + std::snprintf(payloads[1], 8, "--> 456"); + + // First frame + uavcan::Frame fr; + ASSERT_TRUE(fr.parse(can_driver.ifaces[0].popTxFrame())); + std::cout << fr.toString() << std::endl; + ASSERT_FALSE(std::strncmp(payloads[0], reinterpret_cast(fr.getPayloadPtr() + 2), 5)); // No CRC + + ASSERT_EQ(i, fr.getTransferID().get()); + ASSERT_EQ(uavcan::TransferTypeServiceResponse, fr.getTransferType()); + ASSERT_EQ(i + 0x10, fr.getDstNodeID().get()); + ASSERT_EQ(i, fr.getPriority().get()); + + // Second frame + ASSERT_TRUE(fr.parse(can_driver.ifaces[0].popTxFrame())); + std::cout << fr.toString() << std::endl; + // cppcheck-suppress arrayIndexOutOfBounds + ASSERT_FALSE(std::strncmp(payloads[1], reinterpret_cast(fr.getPayloadPtr()), 7)); + + ASSERT_EQ(i, fr.getTransferID().get()); + ASSERT_EQ(uavcan::TransferTypeServiceResponse, fr.getTransferType()); + ASSERT_EQ(i + 0x10, fr.getDstNodeID().get()); + } + + ASSERT_EQ(0, server.getRequestFailureCount()); + ASSERT_EQ(0, server.getResponseFailureCount()); + + ASSERT_EQ(1, node.getDispatcher().getNumServiceRequestListeners()); + } + ASSERT_EQ(0, node.getDispatcher().getNumServiceRequestListeners()); +} + + +TEST(ServiceServer, Empty) +{ + // Manual type registration - we can't rely on the GDTR state + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + SystemClockDriver clock_driver; + CanDriverMock can_driver(1, clock_driver); + TestNode node(can_driver, clock_driver, 1); + + EmptyServerImpl impl; + + uavcan::ServiceServer server(node); + + std::cout << "sizeof(ServiceServer): " + << sizeof(uavcan::ServiceServer) << std::endl; + + ASSERT_EQ(0, node.getDispatcher().getNumServiceRequestListeners()); + ASSERT_GE(0, server.start(impl.bind())); + ASSERT_EQ(1, node.getDispatcher().getNumServiceRequestListeners()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/sub_node.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/sub_node.cpp new file mode 100644 index 000000000000..3cb04df8dd43 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/sub_node.cpp @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include +#include "test_node.hpp" +#include "../protocol/helpers.hpp" + +static void registerTypes() +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + uavcan::DefaultDataTypeRegistrator _reg6; + uavcan::DefaultDataTypeRegistrator _reg7; + uavcan::DefaultDataTypeRegistrator _reg8; +} + + +TEST(SubNode, Basic) +{ + registerTypes(); + InterlinkedTestNodesWithSysClock nodes; + + uavcan::protocol::SoftwareVersion swver; + swver.major = 0; + swver.minor = 1; + swver.vcs_commit = 0xDEADBEEF; + + std::cout << "sizeof(uavcan::SubNode<0>): " << sizeof(uavcan::SubNode<0>) << std::endl; + + /* + * uavcan::Node + */ + uavcan::Node<1024> node1(nodes.can_a, nodes.clock_a); + node1.setName("com.example"); + node1.setNodeID(1); + node1.setSoftwareVersion(swver); + + /* + * uavcan::SubNode + */ + uavcan::SubNode<1024> node2(nodes.can_b, nodes.clock_b); + + BackgroundSpinner bgspinner(node2, node1); + bgspinner.startPeriodic(uavcan::MonotonicDuration::fromMSec(10)); + + uavcan::NodeStatusMonitor node_status_monitor(node2); + ASSERT_LE(0, node_status_monitor.start()); + + /* + * Init the first node + */ + ASSERT_FALSE(node1.isStarted()); + ASSERT_EQ(-uavcan::ErrNotInited, node1.spin(uavcan::MonotonicDuration::fromMSec(20))); + ASSERT_LE(0, node1.start()); + ASSERT_TRUE(node1.isStarted()); + + ASSERT_LE(0, node1.spin(uavcan::MonotonicDuration::fromMSec(2000))); + + ASSERT_EQ(1, node_status_monitor.findNodeWithWorstHealth().get()); + + /* + * Some logging + */ + SubscriberWithCollector log_sub(node2); + ASSERT_LE(0, log_sub.start()); + + node1.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + node1.logInfo("test", "6 * 9 = 42"); + + ASSERT_LE(0, node1.spin(uavcan::MonotonicDuration::fromMSec(20))); + ASSERT_LE(0, node2.spin(uavcan::MonotonicDuration::fromMSec(20))); + + ASSERT_TRUE(log_sub.collector.msg.get()); + std::cout << *log_sub.collector.msg << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/subscriber.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/subscriber.cpp new file mode 100644 index 000000000000..40be485af9a1 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/subscriber.cpp @@ -0,0 +1,282 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include "../clock.hpp" +#include "../transport/can/can.hpp" +#include "test_node.hpp" + + +template +struct SubscriptionListener +{ + typedef uavcan::ReceivedDataStructure ReceivedDataStructure; + + struct ReceivedDataStructureCopy + { + uavcan::MonotonicTime ts_monotonic; + uavcan::UtcTime ts_utc; + uavcan::TransferType transfer_type; + uavcan::TransferID transfer_id; + uavcan::NodeID src_node_id; + uavcan::uint8_t iface_index; + DataType msg; + + ReceivedDataStructureCopy(const ReceivedDataStructure& s) + : ts_monotonic(s.getMonotonicTimestamp()) + , ts_utc(s.getUtcTimestamp()) + , transfer_type(s.getTransferType()) + , transfer_id(s.getTransferID()) + , src_node_id(s.getSrcNodeID()) + , iface_index(s.getIfaceIndex()) + , msg(s) + { } + }; + + std::vector simple; + std::vector extended; + + void receiveExtended(ReceivedDataStructure& msg) + { + extended.push_back(msg); + } + + void receiveSimple(DataType& msg) + { + simple.push_back(msg); + } + + typedef SubscriptionListener SelfType; + typedef uavcan::MethodBinder ExtendedBinder; + typedef uavcan::MethodBinder SimpleBinder; + + ExtendedBinder bindExtended() { return ExtendedBinder(this, &SelfType::receiveExtended); } + SimpleBinder bindSimple() { return SimpleBinder(this, &SelfType::receiveSimple); } +}; + + +TEST(Subscriber, Basic) +{ + // Manual type registration - we can't rely on the GDTR state + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + SystemClockDriver clock_driver; + CanDriverMock can_driver(2, clock_driver); + TestNode node(can_driver, clock_driver, 1); + + typedef SubscriptionListener Listener; + + uavcan::Subscriber sub_extended(node); + uavcan::Subscriber sub_extended2(node); // Not used + uavcan::Subscriber sub_simple(node); + uavcan::Subscriber sub_simple2(node); // Not used + + std::cout << + "sizeof(uavcan::Subscriber): " << + sizeof(uavcan::Subscriber) << std::endl; + + // Null binder - will fail + ASSERT_EQ(-uavcan::ErrInvalidParam, sub_extended.start(Listener::ExtendedBinder(UAVCAN_NULLPTR, UAVCAN_NULLPTR))); + + Listener listener; + + /* + * Message layout: + * uint8 seq + * uint8 sysid + * uint8 compid + * uint8 msgid + * uint8[<256] payload + */ + root_ns_a::MavlinkMessage expected_msg; + expected_msg.seq = 0x42; + expected_msg.sysid = 0x72; + expected_msg.compid = 0x08; + expected_msg.msgid = 0xa5; + expected_msg.payload = "Msg"; + + const uint8_t transfer_payload[] = {0x42, 0x72, 0x08, 0xa5, 'M', 's', 'g'}; + + /* + * RxFrame generation + */ + std::vector rx_frames; + for (uint8_t i = 0; i < 4; i++) + { + uavcan::TransferType tt = uavcan::TransferTypeMessageBroadcast; + uavcan::NodeID dni = (tt == uavcan::TransferTypeMessageBroadcast) ? + uavcan::NodeID::Broadcast : node.getDispatcher().getNodeID(); + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame + uavcan::Frame frame(root_ns_a::MavlinkMessage::DefaultDataTypeID, tt, uavcan::NodeID(uint8_t(i + 100)), + dni, i); + frame.setStartOfTransfer(true); + frame.setEndOfTransfer(true); + frame.setPayload(transfer_payload, 7); + uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0); + rx_frames.push_back(rx_frame); + } + + /* + * Reception + */ + ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners()); + + ASSERT_EQ(0, sub_extended.start(listener.bindExtended())); + ASSERT_EQ(0, sub_extended2.start(listener.bindExtended())); + ASSERT_EQ(0, sub_simple.start(listener.bindSimple())); + ASSERT_EQ(0, sub_simple2.start(listener.bindSimple())); + + ASSERT_EQ(4, node.getDispatcher().getNumMessageListeners()); + + sub_extended2.stop(); // These are not used - making sure they aren't receiving anything + sub_simple2.stop(); + + ASSERT_EQ(2, node.getDispatcher().getNumMessageListeners()); + + for (unsigned i = 0; i < rx_frames.size(); i++) + { + can_driver.ifaces[0].pushRx(rx_frames[i]); + can_driver.ifaces[1].pushRx(rx_frames[i]); + } + + ASSERT_LE(0, node.spin(clock_driver.getMonotonic() + durMono(10000))); + + /* + * Validation + */ + ASSERT_EQ(listener.extended.size(), rx_frames.size()); + for (unsigned i = 0; i < rx_frames.size(); i++) + { + const Listener::ReceivedDataStructureCopy s = listener.extended.at(i); + ASSERT_TRUE(s.msg == expected_msg); + ASSERT_EQ(rx_frames[i].getSrcNodeID(), s.src_node_id); + ASSERT_EQ(rx_frames[i].getTransferID(), s.transfer_id); + ASSERT_EQ(rx_frames[i].getTransferType(), s.transfer_type); + ASSERT_EQ(rx_frames[i].getMonotonicTimestamp(), s.ts_monotonic); + ASSERT_EQ(rx_frames[i].getIfaceIndex(), s.iface_index); + } + + ASSERT_EQ(listener.simple.size(), rx_frames.size()); + for (unsigned i = 0; i < rx_frames.size(); i++) + { + ASSERT_TRUE(listener.simple.at(i) == expected_msg); + } + + ASSERT_EQ(0, sub_extended.getFailureCount()); + ASSERT_EQ(0, sub_simple.getFailureCount()); + + /* + * Unregistration + */ + ASSERT_EQ(2, node.getDispatcher().getNumMessageListeners()); + + sub_extended.stop(); + sub_extended2.stop(); + sub_simple.stop(); + sub_simple2.stop(); + + ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners()); +} + + +static void panickingSink(const uavcan::ReceivedDataStructure&) +{ + FAIL() << "I just went mad"; +} + + +TEST(Subscriber, FailureCount) +{ + // Manual type registration - we can't rely on the GDTR state + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + SystemClockDriver clock_driver; + CanDriverMock can_driver(2, clock_driver); + TestNode node(can_driver, clock_driver, 1); + + { + uavcan::Subscriber sub(node); + ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners()); + sub.start(panickingSink); + ASSERT_EQ(1, node.getDispatcher().getNumMessageListeners()); + + ASSERT_EQ(0, sub.getFailureCount()); + + for (uint8_t i = 0; i < 4; i++) + { + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame + uavcan::Frame frame(root_ns_a::MavlinkMessage::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + uavcan::NodeID(uint8_t(i + 100)), uavcan::NodeID::Broadcast, i); + frame.setStartOfTransfer(true); + frame.setEndOfTransfer(true); + // No payload - broken transfer + uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0); + can_driver.ifaces[0].pushRx(rx_frame); + can_driver.ifaces[1].pushRx(rx_frame); + } + + ASSERT_LE(0, node.spin(clock_driver.getMonotonic() + durMono(10000))); + + ASSERT_EQ(4, sub.getFailureCount()); + + ASSERT_EQ(1, node.getDispatcher().getNumMessageListeners()); // Still there + } + ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners()); // Removed +} + + +TEST(Subscriber, SingleFrameTransfer) +{ + // Manual type registration - we can't rely on the GDTR state + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + SystemClockDriver clock_driver; + CanDriverMock can_driver(2, clock_driver); + TestNode node(can_driver, clock_driver, 1); + + typedef SubscriptionListener Listener; + + uavcan::Subscriber sub(node); + + std::cout << + "sizeof(uavcan::Subscriber): " << + sizeof(uavcan::Subscriber) << std::endl; + + Listener listener; + + sub.start(listener.bindSimple()); + + for (uint8_t i = 0; i < 4; i++) + { + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame + uavcan::Frame frame(root_ns_a::EmptyMessage::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + uavcan::NodeID(uint8_t(i + 100)), uavcan::NodeID::Broadcast, i); + frame.setStartOfTransfer(true); + frame.setEndOfTransfer(true); + // No payload - message is empty + uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0); + can_driver.ifaces[0].pushRx(rx_frame); + can_driver.ifaces[1].pushRx(rx_frame); + } + + ASSERT_LE(0, node.spin(clock_driver.getMonotonic() + durMono(10000))); + + ASSERT_EQ(0, sub.getFailureCount()); + + ASSERT_EQ(4, listener.simple.size()); + for (unsigned i = 0; i < 4; i++) + { + ASSERT_TRUE(listener.simple.at(i) == root_ns_a::EmptyMessage()); + } +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/test_node.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/test_node.hpp new file mode 100644 index 000000000000..a8e6fc84fb3e --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/test_node.hpp @@ -0,0 +1,268 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#include +#include +#include +#include +#include +#include "../transport/can/can.hpp" +#include +#include + +struct TestNode : public uavcan::INode +{ + /* + * This class used to use the simple pool allocator instead: + * uavcan::PoolAllocator pool; + * It has been replaced because unlike the simple allocator, heap-based one is not tested as extensively. + * Moreover, heap based allocator prints and error message upon destruction if some memory has not been freed. + */ + uavcan::HeapBasedPoolAllocator pool; + uavcan::Scheduler scheduler; + uint64_t internal_failure_count; + + TestNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock_driver, uavcan::NodeID self_node_id) : + pool(1024), + scheduler(can_driver, pool, clock_driver), + internal_failure_count(0) + { + setNodeID(self_node_id); + } + + virtual void registerInternalFailure(const char* msg) + { + std::cout << "TestNode internal failure: " << msg << std::endl; + internal_failure_count++; + } + + virtual uavcan::IPoolAllocator& getAllocator() { return pool; } + virtual uavcan::Scheduler& getScheduler() { return scheduler; } + virtual const uavcan::Scheduler& getScheduler() const { return scheduler; } +}; + + +struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface +{ + uavcan::ISystemClock& clock; + std::set others; + std::queue read_queue; + std::queue loopback_queue; + uint64_t error_count; + + PairableCanDriver(uavcan::ISystemClock& clock) + : clock(clock) + , error_count(0) + { } + + void linkTogether(PairableCanDriver* with) + { + this->others.insert(with); + with->others.insert(this); + others.erase(this); + } + + virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) + { + return (iface_index == 0) ? this : UAVCAN_NULLPTR; + } + virtual const uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) const + { + return (iface_index == 0) ? this : UAVCAN_NULLPTR; + } + + virtual uavcan::uint8_t getNumIfaces() const { return 1; } + + virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, + const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces], + uavcan::MonotonicTime blocking_deadline) + { + if (inout_masks.read == 1) + { + inout_masks.read = (!read_queue.empty() || !loopback_queue.empty()) ? 1 : 0; + } + if (inout_masks.read || inout_masks.write) + { + return 1; + } + while (clock.getMonotonic() < blocking_deadline) + { + usleep(1000); + } + return 0; + } + + virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime, uavcan::CanIOFlags flags) + { + assert(!others.empty()); + for (std::set::iterator it = others.begin(); it != others.end(); ++it) + { + (*it)->read_queue.push(frame); + } + if (flags & uavcan::CanIOFlagLoopback) + { + loopback_queue.push(frame); + } + return 1; + } + + virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, + uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) + { + out_flags = 0; + if (loopback_queue.empty()) + { + assert(read_queue.size()); + out_frame = read_queue.front(); + read_queue.pop(); + } + else + { + out_flags |= uavcan::CanIOFlagLoopback; + out_frame = loopback_queue.front(); + loopback_queue.pop(); + } + out_ts_monotonic = clock.getMonotonic(); + out_ts_utc = clock.getUtc(); + return 1; + } + + void pushRxToAllIfaces(const uavcan::CanFrame& can_frame) + { + read_queue.push(can_frame); + } + + virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig*, uavcan::uint16_t) { return -1; } + virtual uavcan::uint16_t getNumFilters() const { return 0; } + virtual uavcan::uint64_t getErrorCount() const { return error_count; } +}; + + +template +struct InterlinkedTestNodes +{ + ClockType clock_a; + ClockType clock_b; + PairableCanDriver can_a; + PairableCanDriver can_b; + TestNode a; + TestNode b; + + InterlinkedTestNodes(uavcan::NodeID nid_first, uavcan::NodeID nid_second) + : can_a(clock_a) + , can_b(clock_b) + , a(can_a, clock_a, nid_first) + , b(can_b, clock_b, nid_second) + { + can_a.linkTogether(&can_b); + } + + InterlinkedTestNodes() + : can_a(clock_a) + , can_b(clock_b) + , a(can_a, clock_a, 1) + , b(can_b, clock_b, 2) + { + can_a.linkTogether(&can_b); + } + + int spinBoth(uavcan::MonotonicDuration duration) + { + assert(!duration.isNegative()); + unsigned nspins2 = unsigned(duration.toMSec() / 2); + nspins2 = nspins2 ? nspins2 : 1; + while (nspins2 --> 0) + { + int ret = a.spin(uavcan::MonotonicDuration::fromMSec(1)); + if (ret < 0) + { + return ret; + } + ret = b.spin(uavcan::MonotonicDuration::fromMSec(1)); + if (ret < 0) + { + return ret; + } + } + return 0; + } +}; + + +typedef InterlinkedTestNodes InterlinkedTestNodesWithSysClock; +typedef InterlinkedTestNodes InterlinkedTestNodesWithClockMock; + + +template +struct TestNetwork +{ + struct NodeEnvironment + { + SystemClockDriver clock; + PairableCanDriver can_driver; + TestNode node; + + NodeEnvironment(uavcan::NodeID node_id) + : can_driver(clock) + , node(can_driver, clock, node_id) + { } + }; + + std::unique_ptr nodes[NumNodes]; + + TestNetwork(uavcan::uint8_t first_node_id = 1) + { + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + nodes[i].reset(new NodeEnvironment(uint8_t(first_node_id + i))); + } + + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + for (uavcan::uint8_t k = 0; k < NumNodes; k++) + { + nodes[i]->can_driver.linkTogether(&nodes[k]->can_driver); + } + } + + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + assert(nodes[i]->can_driver.others.size() == (NumNodes - 1)); + } + } + + int spinAll(uavcan::MonotonicDuration duration) + { + assert(!duration.isNegative()); + unsigned nspins = unsigned(duration.toMSec() / NumNodes); + nspins = nspins ? nspins : 1; + while (nspins --> 0) + { + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + int ret = nodes[i]->node.spin(uavcan::MonotonicDuration::fromMSec(1)); + if (ret < 0) + { + return ret; + } + } + } + return 0; + } + + TestNode& operator[](unsigned index) + { + if (index >= NumNodes) + { + throw std::out_of_range("No such test node"); + } + return nodes[index]->node; + } +}; diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/test_node_test.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/test_node_test.cpp new file mode 100644 index 000000000000..b40357be2153 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/test_node_test.cpp @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include "test_node.hpp" + + +TEST(TestNode, TestNetwork) +{ + TestNetwork<4> nwk; + + uavcan::CanFrame frame; + for (uint8_t i = 0; i < 8; i++) + { + frame.data[i] = i; + } + frame.id = 1234U; + + ASSERT_EQ(1, nwk.nodes[0]->can_driver.send(frame, uavcan::MonotonicTime(), uavcan::CanIOFlags())); + + for (int i = 1; i < 4; i++) + { + uavcan::CanFrame rx; + uavcan::MonotonicTime ts_mono; + uavcan::UtcTime ts_utc; + uavcan::CanIOFlags flags = 0; + ASSERT_EQ(1, nwk.nodes[i]->can_driver.receive(rx, ts_mono, ts_utc, flags)); + + ASSERT_TRUE(rx == frame); + } +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/data_type_info_provider.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/data_type_info_provider.cpp new file mode 100644 index 000000000000..75e704b86c83 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/data_type_info_provider.cpp @@ -0,0 +1,172 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include "helpers.hpp" + +using uavcan::protocol::GetDataTypeInfo; +using uavcan::protocol::NodeStatus; +using uavcan::protocol::DataTypeKind; +using uavcan::ServiceCallResult; +using uavcan::DataTypeInfoProvider; +using uavcan::GlobalDataTypeRegistry; +using uavcan::DefaultDataTypeRegistrator; +using uavcan::MonotonicDuration; + +template +static bool validateDataTypeInfoResponse(const std::unique_ptr::Result>& resp, + unsigned flags) +{ + if (!resp.get()) + { + std::cout << "Null response" << std::endl; + return false; + } + if (!resp->isSuccessful()) + { + std::cout << "Request was not successful" << std::endl; + return false; + } + if (resp->getResponse().name != DataType::getDataTypeFullName()) + { + std::cout << "Type name mismatch: '" + << resp->getResponse().name.c_str() << "' '" + << DataType::getDataTypeFullName() << "'" << std::endl; + return false; + } + if (DataType::getDataTypeSignature().get() != resp->getResponse().signature) + { + std::cout << "Signature mismatch" << std::endl; + return false; + } + if (resp->getResponse().flags != flags) + { + std::cout << "Mask mismatch" << std::endl; + return false; + } + if (resp->getResponse().kind.value != DataType::DataTypeKind) + { + std::cout << "Kind mismatch" << std::endl; + return false; + } + if (resp->getResponse().id != DataType::DefaultDataTypeID) + { + std::cout << "DTID mismatch" << std::endl; + return false; + } + return true; +} + + +TEST(DataTypeInfoProvider, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + DataTypeInfoProvider dtip(nodes.a); + + GlobalDataTypeRegistry::instance().reset(); + DefaultDataTypeRegistrator _reg1; + DefaultDataTypeRegistrator _reg3; + + ASSERT_LE(0, dtip.start()); + + ServiceClientWithCollector gdti_cln(nodes.b); + + /* + * GetDataTypeInfo request for GetDataTypeInfo + */ + GetDataTypeInfo::Request gdti_request; + gdti_request.id = GetDataTypeInfo::DefaultDataTypeID; + gdti_request.kind.value = DataTypeKind::SERVICE; + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(validateDataTypeInfoResponse(gdti_cln.collector.result, + GetDataTypeInfo::Response::FLAG_KNOWN | + GetDataTypeInfo::Response::FLAG_SERVING)); + ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get()); + + /* + * GetDataTypeInfo request for GetDataTypeInfo by name + */ + gdti_request = GetDataTypeInfo::Request(); + gdti_request.id = 999; // Intentionally wrong + gdti_request.kind.value = DataTypeKind::MESSAGE; // Intentionally wrong + gdti_request.name = "uavcan.protocol.GetDataTypeInfo"; + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(validateDataTypeInfoResponse(gdti_cln.collector.result, + GetDataTypeInfo::Response::FLAG_KNOWN | + GetDataTypeInfo::Response::FLAG_SERVING)); + ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get()); + + /* + * GetDataTypeInfo request for NodeStatus - not used yet + */ + gdti_request = GetDataTypeInfo::Request(); + gdti_request.id = NodeStatus::DefaultDataTypeID; + gdti_request.kind.value = DataTypeKind::MESSAGE; + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(validateDataTypeInfoResponse(gdti_cln.collector.result, + GetDataTypeInfo::Response::FLAG_KNOWN)); + + /* + * Starting publisher and subscriber for NodeStatus, requesting info again + */ + uavcan::Publisher ns_pub(nodes.a); + SubscriberWithCollector ns_sub(nodes.a); + + ASSERT_LE(0, ns_pub.broadcast(NodeStatus())); + ASSERT_LE(0, ns_sub.start()); + + // Request again + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(validateDataTypeInfoResponse(gdti_cln.collector.result, + GetDataTypeInfo::Response::FLAG_KNOWN | + GetDataTypeInfo::Response::FLAG_PUBLISHING | + GetDataTypeInfo::Response::FLAG_SUBSCRIBED)); + + /* + * Requesting a non-existent type + */ + gdti_request = GetDataTypeInfo::Request(); + gdti_request.id = 20000; + gdti_request.kind.value = 3; // INVALID VALUE + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(gdti_cln.collector.result.get()); + ASSERT_TRUE(gdti_cln.collector.result->isSuccessful()); + ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get()); + ASSERT_EQ(0, gdti_cln.collector.result->getResponse().flags); + ASSERT_TRUE(gdti_cln.collector.result->getResponse().name.empty()); // Empty name + ASSERT_EQ(gdti_request.id, gdti_cln.collector.result->getResponse().id); + ASSERT_EQ(gdti_request.kind.value, gdti_cln.collector.result->getResponse().kind.value); + + /* + * Requesting a non-existent type by name + */ + gdti_request = GetDataTypeInfo::Request(); + gdti_request.id = 999; // Intentionally wrong + gdti_request.kind.value = 3; // Intentionally wrong + gdti_request.name = "uavcan.equipment.gnss.Fix"; + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(gdti_cln.collector.result.get()); + ASSERT_TRUE(gdti_cln.collector.result->isSuccessful()); + ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get()); + ASSERT_EQ(0, gdti_cln.collector.result->getResponse().flags); + ASSERT_EQ("uavcan.equipment.gnss.Fix", gdti_cln.collector.result->getResponse().name); + ASSERT_EQ(0, gdti_cln.collector.result->getResponse().id); + ASSERT_EQ(0, gdti_cln.collector.result->getResponse().kind.value); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_client.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_client.cpp new file mode 100644 index 000000000000..eff92c6d724b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_client.cpp @@ -0,0 +1,179 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +TEST(DynamicNodeIDClient, Basic) +{ + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + uavcan::DynamicNodeIDClient dnidac(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + (void)_reg1; + + /* + * Client initialization + */ + uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id; + + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(unique_id)); // Empty hardware version is not allowed + + for (uavcan::uint8_t i = 0; i < unique_id.size(); i++) + { + unique_id[i] = i; + } + + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(unique_id, uavcan::NodeID())); + + const uavcan::NodeID PreferredNodeID = 42; + ASSERT_LE(0, dnidac.start(unique_id, PreferredNodeID)); + + ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); + ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + ASSERT_FALSE(dnidac.isAllocationComplete()); + + /* + * Subscriber (server emulation) + */ + SubscriberWithCollector dynid_sub(nodes.a); + ASSERT_LE(0, dynid_sub.start()); + dynid_sub.subscriber.allowAnonymousTransfers(); + + /* + * Monitoring requests + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500)); + ASSERT_TRUE(dynid_sub.collector.msg.get()); + std::cout << "First-stage request:\n" << *dynid_sub.collector.msg << std::endl; + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + ASSERT_TRUE(dynid_sub.collector.msg->first_part_of_unique_id); + ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(), + dynid_sub.collector.msg->unique_id.end(), + unique_id.begin())); + dynid_sub.collector.msg.reset(); + + // Second - rate is no lower than 0.5 Hz + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500)); + ASSERT_TRUE(dynid_sub.collector.msg.get()); + dynid_sub.collector.msg.reset(); + + ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); + ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + ASSERT_FALSE(dnidac.isAllocationComplete()); + + /* + * Publisher (server emulation) + */ + uavcan::Publisher dynid_pub(nodes.a); + ASSERT_LE(0, dynid_pub.init()); + + /* + * Sending some some Allocation messages - the timer will keep restarting + */ + for (int i = 0; i < 10; i++) + { + uavcan::protocol::dynamic_node_id::Allocation msg; // Contents of the message doesn't matter + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(210)); + ASSERT_FALSE(dynid_sub.collector.msg.get()); + } + + /* + * Responding with partially matching unique ID - the client will respond with second-stage request immediately + */ + const uint8_t BytesPerRequest = uavcan::protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST; + { + uavcan::protocol::dynamic_node_id::Allocation msg; + msg.unique_id.resize(BytesPerRequest); + uavcan::copy(unique_id.begin(), unique_id.begin() + BytesPerRequest, msg.unique_id.begin()); + + std::cout << "First-stage offer:\n" << msg << std::endl; + + ASSERT_FALSE(dynid_sub.collector.msg.get()); + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500)); + + ASSERT_TRUE(dynid_sub.collector.msg.get()); + std::cout << "Second-stage request:\n" << *dynid_sub.collector.msg << std::endl; + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id); + ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(), + dynid_sub.collector.msg->unique_id.end(), + unique_id.begin() + BytesPerRequest)); + dynid_sub.collector.msg.reset(); + } + + /* + * Responding with second-stage offer, expecting the last request back + */ + { + uavcan::protocol::dynamic_node_id::Allocation msg; + msg.unique_id.resize(BytesPerRequest * 2); + uavcan::copy(unique_id.begin(), unique_id.begin() + BytesPerRequest * 2, msg.unique_id.begin()); + + std::cout << "Second-stage offer:\n" << msg << std::endl; + + ASSERT_FALSE(dynid_sub.collector.msg.get()); + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500)); + + ASSERT_TRUE(dynid_sub.collector.msg.get()); + std::cout << "Last request:\n" << *dynid_sub.collector.msg << std::endl; + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id); + ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(), + dynid_sub.collector.msg->unique_id.end(), + unique_id.begin() + BytesPerRequest * 2)); + dynid_sub.collector.msg.reset(); + } + + ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); + ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + ASSERT_FALSE(dnidac.isAllocationComplete()); + + /* + * Now we have full unique ID for this client received, and it is possible to grant allocation + */ + { + uavcan::protocol::dynamic_node_id::Allocation msg; + msg.unique_id.resize(16); + msg.node_id = 72; + uavcan::copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin()); + + ASSERT_FALSE(dynid_sub.collector.msg.get()); + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + ASSERT_FALSE(dynid_sub.collector.msg.get()); + } + + ASSERT_EQ(uavcan::NodeID(72), dnidac.getAllocatedNodeID()); + ASSERT_EQ(uavcan::NodeID(10), dnidac.getAllocatorNodeID()); + ASSERT_TRUE(dnidac.isAllocationComplete()); +} + + +TEST(DynamicNodeIDClient, NonPassiveMode) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::DynamicNodeIDClient dnidac(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + (void)_reg1; + + uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id; + for (uavcan::uint8_t i = 0; i < unique_id.size(); i++) + { + unique_id[i] = i; + } + + ASSERT_LE(-uavcan::ErrLogic, dnidac.start(unique_id)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp new file mode 100644 index 000000000000..07894133dc3e --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp @@ -0,0 +1,117 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include "event_tracer.hpp" +#include "../helpers.hpp" + + +using uavcan::dynamic_node_id_server::UniqueID; + +class AllocationRequestHandler : public uavcan::dynamic_node_id_server::IAllocationRequestHandler +{ + std::vector > requests_; + +public: + bool can_followup; + + AllocationRequestHandler() : can_followup(false) { } + + virtual void handleAllocationRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) + { + requests_.push_back(std::pair(unique_id, preferred_node_id)); + } + + virtual bool canPublishFollowupAllocationResponse() const + { + return can_followup; + } + + bool matchAndPopLastRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) + { + if (requests_.empty()) + { + std::cout << "No pending requests" << std::endl; + return false; + } + + const std::pair pair = requests_.at(requests_.size() - 1U); + requests_.pop_back(); + + if (pair.first != unique_id) + { + std::cout << "Unique ID mismatch" << std::endl; + return false; + } + + if (pair.second != preferred_node_id) + { + std::cout << "Node ID mismatch (" << pair.second.get() << ", " << preferred_node_id.get() << ")" + << std::endl; + return false; + } + + return true; + } + + void reset() { requests_.clear(); } +}; + + +TEST(dynamic_node_id_server_AllocationRequestManager, Basic) +{ + using namespace uavcan::protocol::dynamic_node_id; + using namespace uavcan::protocol::dynamic_node_id::server; + using namespace uavcan::dynamic_node_id_server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + uavcan::DynamicNodeIDClient client(nodes.b); + + /* + * Client initialization + */ + uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id; + for (uavcan::uint8_t i = 0; i < unique_id.size(); i++) + { + unique_id[i] = i; + } + const uavcan::NodeID PreferredNodeID = 42; + ASSERT_LE(0, client.start(unique_id, PreferredNodeID)); + + /* + * Request manager initialization + */ + EventTracer tracer; + AllocationRequestHandler handler; + handler.can_followup = true; + + AllocationRequestManager manager(nodes.a, tracer, handler); + + ASSERT_LE(0, manager.init(uavcan::TransferPriority::OneHigherThanLowest)); + + /* + * Allocation + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + ASSERT_TRUE(handler.matchAndPopLastRequest(unique_id, PreferredNodeID)); + + ASSERT_LE(0, manager.broadcastAllocationResponse(unique_id, PreferredNodeID)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + + /* + * Checking the client + */ + ASSERT_TRUE(client.isAllocationComplete()); + + ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp new file mode 100644 index 000000000000..932bf61c0664 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include "../../helpers.hpp" +#include "../event_tracer.hpp" +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + +using uavcan::dynamic_node_id_server::UniqueID; + + +TEST(dynamic_node_id_server_centralized_Server, Basic) +{ + using namespace uavcan::dynamic_node_id_server; + using namespace uavcan::protocol::dynamic_node_id; + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + + EventTracer tracer; + MemoryStorageBackend storage; + + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + UniqueID own_unique_id; + own_unique_id[0] = 0xAA; + own_unique_id[3] = 0xCC; + own_unique_id[7] = 0xEE; + own_unique_id[9] = 0xBB; + + /* + * Server + */ + uavcan::dynamic_node_id_server::CentralizedServer server(nodes.a, storage, tracer); + + ASSERT_LE(0, server.init(own_unique_id)); + + ASSERT_EQ(1, server.getNumAllocations()); // Server's own node ID + + /* + * Client + */ + uavcan::DynamicNodeIDClient client(nodes.b); + uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id; + for (uavcan::uint8_t i = 0; i < unique_id.size(); i++) + { + unique_id[i] = i; + } + const uavcan::NodeID PreferredNodeID = 42; + ASSERT_LE(0, client.start(unique_id, PreferredNodeID)); + + /* + * Fire + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(15000)); + + ASSERT_TRUE(client.isAllocationComplete()); + ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); + + ASSERT_EQ(2, server.getNumAllocations()); // Server's own node ID + client +} + + +TEST(dynamic_node_id_server_centralized, ObjectSizes) +{ + using namespace uavcan::dynamic_node_id_server; + std::cout << "centralized::Storage: " << sizeof(centralized::Storage) << std::endl; + std::cout << "centralized::Server: " << sizeof(centralized::Server) << std::endl; + std::cout << "NodeDiscoverer: " << sizeof(NodeDiscoverer) << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp new file mode 100644 index 000000000000..c41115653dc3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp @@ -0,0 +1,130 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + + +TEST(dynamic_node_id_server_centralized_Storage, Initialization) +{ + using namespace uavcan::dynamic_node_id_server::centralized; + using namespace uavcan::dynamic_node_id_server; + + // No data in the storage - initializing empty + { + MemoryStorageBackend storage; + Storage stor(storage); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, stor.init()); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_EQ(0, stor.getSize()); + + ASSERT_FALSE(stor.isNodeIDOccupied(1)); + ASSERT_FALSE(stor.isNodeIDOccupied(0)); + } + // Nonempty storage, many items + { + MemoryStorageBackend storage; + Storage stor(storage); + + storage.set("occupation_mask", "0e000000000000000000000000000000"); // node ID 1, 2, 3 + ASSERT_LE(0, stor.init()); // OK + + ASSERT_EQ(1, storage.getNumKeys()); + ASSERT_EQ(3, stor.getSize()); + + ASSERT_TRUE(stor.isNodeIDOccupied(1)); + ASSERT_TRUE(stor.isNodeIDOccupied(2)); + ASSERT_TRUE(stor.isNodeIDOccupied(3)); + ASSERT_FALSE(stor.isNodeIDOccupied(0)); + ASSERT_FALSE(stor.isNodeIDOccupied(4)); + + UniqueID uid_1; + uid_1[0] = 1; + + UniqueID uid_2; + uid_2[0] = 2; + + UniqueID uid_3; + uid_3[0] = 3; + + ASSERT_FALSE(stor.getNodeIDForUniqueID(uid_1).isValid()); + ASSERT_FALSE(stor.getNodeIDForUniqueID(uid_2).isValid()); + ASSERT_FALSE(stor.getNodeIDForUniqueID(uid_3).isValid()); + ASSERT_FALSE(stor.isNodeIDOccupied(127)); + + storage.set("01000000000000000000000000000000", "1"); + storage.set("02000000000000000000000000000000", "2"); + storage.set("03000000000000000000000000000000", "3"); + + ASSERT_EQ(1, stor.getNodeIDForUniqueID(uid_1).get()); + ASSERT_EQ(2, stor.getNodeIDForUniqueID(uid_2).get()); + ASSERT_EQ(3, stor.getNodeIDForUniqueID(uid_3).get()); + ASSERT_FALSE(stor.isNodeIDOccupied(127)); + } +} + + +TEST(dynamic_node_id_server_centralized_Storage, Basic) +{ + using namespace uavcan::dynamic_node_id_server::centralized; + + MemoryStorageBackend storage; + Storage stor(storage); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, stor.init()); + storage.print(); + ASSERT_EQ(0, storage.getNumKeys()); + + /* + * Adding one entry to the log, making sure it appears in the storage + */ + uavcan::dynamic_node_id_server::UniqueID unique_id; + unique_id[0] = 1; + ASSERT_LE(0, stor.add(1, unique_id)); + + ASSERT_EQ("02000000000000000000000000000000", storage.get("occupation_mask")); + ASSERT_EQ("1", storage.get("01000000000000000000000000000000")); + + ASSERT_EQ(2, storage.getNumKeys()); + ASSERT_EQ(1, stor.getSize()); + + /* + * Adding another entry while storage is failing + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(2, storage.getNumKeys()); + + unique_id[0] = 2; + ASSERT_GT(0, stor.add(2, unique_id)); + + ASSERT_EQ(2, storage.getNumKeys()); // No new entries, we failed + + ASSERT_EQ(1, stor.getSize()); + + /* + * Adding a lot of entries + */ + storage.failOnSetCalls(false); + + uavcan::NodeID node_id(2); + while (stor.getSize() < 127) + { + ASSERT_LE(0, stor.add(node_id, unique_id)); + + ASSERT_TRUE(stor.getNodeIDForUniqueID(unique_id).isValid()); + ASSERT_TRUE(stor.isNodeIDOccupied(node_id)); + + node_id = uint8_t(uavcan::min(node_id.get() + 1U, 127U)); + unique_id[0] = uint8_t(unique_id[0] + 1U); + } + + storage.print(); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp new file mode 100644 index 000000000000..4b13b34dc71d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp @@ -0,0 +1,262 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "../event_tracer.hpp" +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + +TEST(dynamic_node_id_server_ClusterManager, Initialization) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + const unsigned MaxClusterSize = + uavcan::protocol::dynamic_node_id::server::Discovery::FieldTypes::known_nodes::MaxSize; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + EventTracer tracer; + + /* + * Simple initialization + */ + { + MemoryStorageBackend storage; + Log log(storage, tracer); + InterlinkedTestNodesWithSysClock nodes; + + ClusterManager mgr(nodes.a, storage, log, tracer); + + // Too big + ASSERT_GT(0, mgr.init(MaxClusterSize + 1, uavcan::TransferPriority::OneHigherThanLowest)); + ASSERT_EQ(0, storage.getNumKeys()); + + // OK + ASSERT_LE(0, mgr.init(5, uavcan::TransferPriority::OneHigherThanLowest)); + ASSERT_EQ(1, storage.getNumKeys()); + ASSERT_EQ("5", storage.get("cluster_size")); + + // Testing other states + ASSERT_EQ(0, mgr.getNumKnownServers()); + ASSERT_EQ(5, mgr.getClusterSize()); + ASSERT_EQ(3, mgr.getQuorumSize()); + ASSERT_FALSE(mgr.getRemoteServerNodeIDAtIndex(0).isValid()); + } + /* + * Recovery from the storage + */ + { + MemoryStorageBackend storage; + Log log(storage, tracer); + InterlinkedTestNodesWithSysClock nodes; + + ClusterManager mgr(nodes.a, storage, log, tracer); + + // Not configured + ASSERT_GT(0, mgr.init(0, uavcan::TransferPriority::OneHigherThanLowest)); + ASSERT_EQ(0, storage.getNumKeys()); + + // OK + storage.set("cluster_size", "5"); + ASSERT_LE(0, mgr.init(0, uavcan::TransferPriority::OneHigherThanLowest)); + ASSERT_EQ(1, storage.getNumKeys()); + } +} + + +TEST(dynamic_node_id_server_ClusterManager, OneServer) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + EventTracer tracer; + MemoryStorageBackend storage; + Log log(storage, tracer); + InterlinkedTestNodesWithSysClock nodes; + + ClusterManager mgr(nodes.a, storage, log, tracer); + + /* + * Pub and sub + */ + SubscriberWithCollector sub(nodes.b); + uavcan::Publisher pub(nodes.b); + + ASSERT_LE(0, sub.start()); + ASSERT_LE(0, pub.init()); + + /* + * Starting + */ + ASSERT_LE(0, mgr.init(1, uavcan::TransferPriority::OneHigherThanLowest)); + + ASSERT_EQ(0, mgr.getNumKnownServers()); + ASSERT_TRUE(mgr.isClusterDiscovered()); + + ASSERT_EQ(0, nodes.a.internal_failure_count); + + /* + * Broadcasting discovery with wrong cluster size, it will be reported as internal failure + */ + uavcan::protocol::dynamic_node_id::server::Discovery msg; + msg.configured_cluster_size = 2; + msg.known_nodes.push_back(2U); + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_EQ(1, nodes.a.internal_failure_count); + + /* + * Discovery rate limiting test + */ + ASSERT_FALSE(sub.collector.msg.get()); + + msg = uavcan::protocol::dynamic_node_id::server::Discovery(); + msg.configured_cluster_size = 1; // Correct value + ASSERT_LE(0, pub.broadcast(msg)); // List of known nodes is empty, intentionally + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(1, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); + + // Rinse repeat + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(1, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); +} + + +TEST(dynamic_node_id_server_ClusterManager, ThreeServers) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + EventTracer tracer; + MemoryStorageBackend storage; + Log log(storage, tracer); + InterlinkedTestNodesWithSysClock nodes; + + ClusterManager mgr(nodes.a, storage, log, tracer); + + /* + * Pub and sub + */ + SubscriberWithCollector sub(nodes.b); + uavcan::Publisher pub(nodes.b); + + ASSERT_LE(0, sub.start()); + ASSERT_LE(0, pub.init()); + + /* + * Starting + */ + ASSERT_LE(0, mgr.init(3, uavcan::TransferPriority::OneHigherThanLowest)); + + ASSERT_EQ(0, mgr.getNumKnownServers()); + ASSERT_FALSE(mgr.isClusterDiscovered()); + + /* + * Discovery publishing rate check + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); + + /* + * Discovering other nodes + */ + uavcan::protocol::dynamic_node_id::server::Discovery msg; + msg.configured_cluster_size = 3; + msg.known_nodes.push_back(2U); + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1050)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(2, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + ASSERT_EQ(2, sub.collector.msg->known_nodes[1]); + sub.collector.msg.reset(); + + ASSERT_FALSE(mgr.isClusterDiscovered()); + + // This will complete the discovery + msg.known_nodes.push_back(127U); + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1050)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(3, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + ASSERT_EQ(2, sub.collector.msg->known_nodes[1]); + ASSERT_EQ(127, sub.collector.msg->known_nodes[2]); + sub.collector.msg.reset(); + + // Making sure discovery is now terminated + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500)); + ASSERT_FALSE(sub.collector.msg.get()); + + /* + * Checking Raft states + */ + ASSERT_EQ(uavcan::NodeID(2), mgr.getRemoteServerNodeIDAtIndex(0)); + ASSERT_EQ(uavcan::NodeID(127), mgr.getRemoteServerNodeIDAtIndex(1)); + ASSERT_EQ(uavcan::NodeID(), mgr.getRemoteServerNodeIDAtIndex(2)); + + ASSERT_EQ(0, mgr.getServerMatchIndex(2)); + ASSERT_EQ(0, mgr.getServerMatchIndex(127)); + + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2)); + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127)); + + mgr.setServerMatchIndex(2, 10); + ASSERT_EQ(10, mgr.getServerMatchIndex(2)); + + mgr.incrementServerNextIndexBy(2, 5); + ASSERT_EQ(log.getLastIndex() + 1 + 5, mgr.getServerNextIndex(2)); + mgr.decrementServerNextIndex(2); + ASSERT_EQ(log.getLastIndex() + 1 + 5 - 1, mgr.getServerNextIndex(2)); + + mgr.resetAllServerIndices(); + + ASSERT_EQ(0, mgr.getServerMatchIndex(2)); + ASSERT_EQ(0, mgr.getServerMatchIndex(127)); + + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2)); + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp new file mode 100644 index 000000000000..e5d7339410b9 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp @@ -0,0 +1,243 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "../event_tracer.hpp" +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + + +static const unsigned NumEntriesInStorageWithEmptyLog = 4; // last index + 3 items per log entry + + +TEST(dynamic_node_id_server_Log, Initialization) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + // No log data in the storage - initializing empty log + { + MemoryStorageBackend storage; + uavcan::dynamic_node_id_server::distributed::Log log(storage, tracer); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, log.init()); + ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id); + ASSERT_EQ(uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id(), + log.getEntryAtIndex(0)->unique_id); + } + // Nonempty storage, one item + { + MemoryStorageBackend storage; + Log log(storage, tracer); + + storage.set("log_last_index", "0"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Expected one entry, none found + ASSERT_EQ(1, storage.getNumKeys()); + + storage.set("log0_term", "0"); + storage.set("log0_unique_id", "00000000000000000000000000000000"); + storage.set("log0_node_id", "0"); + ASSERT_LE(0, log.init()); // OK now + ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + } + // Nonempty storage, broken data + { + MemoryStorageBackend storage; + Log log(storage, tracer); + + storage.set("log_last_index", "foobar"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value + + storage.set("log_last_index", "128"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value + + storage.set("log_last_index", "0"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // No log items + ASSERT_EQ(1, storage.getNumKeys()); + + storage.set("log0_term", "0"); + storage.set("log0_unique_id", "00000000000000000000000000000000"); + storage.set("log0_node_id", "128"); // Bad value (127 max) + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Failed + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + ASSERT_EQ(4, storage.getNumKeys()); + } + // Nonempty storage, many items + { + MemoryStorageBackend storage; + Log log(storage, tracer); + + storage.set("log_last_index", "1"); // 2 items - 0, 1 + storage.set("log0_term", "0"); + storage.set("log0_unique_id", "00000000000000000000000000000000"); + storage.set("log0_node_id", "0"); + storage.set("log1_term", "1"); + storage.set("log1_unique_id", "0123456789abcdef0123456789abcdef"); + storage.set("log1_node_id", "127"); + + ASSERT_LE(0, log.init()); // OK now + ASSERT_EQ(7, storage.getNumKeys()); + ASSERT_EQ(1, log.getLastIndex()); + + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id); + ASSERT_EQ(uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id(), + log.getEntryAtIndex(0)->unique_id); + + ASSERT_EQ(1, log.getEntryAtIndex(1)->term); + ASSERT_EQ(127, log.getEntryAtIndex(1)->node_id); + uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id uid; + uid[0] = 0x01; + uid[1] = 0x23; + uid[2] = 0x45; + uid[3] = 0x67; + uid[4] = 0x89; + uid[5] = 0xab; + uid[6] = 0xcd; + uid[7] = 0xef; + uavcan::copy(uid.begin(), uid.begin() + 8, uid.begin() + 8); + ASSERT_EQ(uid, log.getEntryAtIndex(1)->unique_id); + } +} + + +TEST(dynamic_node_id_server_Log, Append) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + MemoryStorageBackend storage; + Log log(storage, tracer); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, log.init()); + storage.print(); + ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); + + /* + * Entry at the index 0 always exists, and it's always zero-initialized. + */ + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("log0_term")); + ASSERT_EQ("00000000000000000000000000000000", storage.get("log0_unique_id")); + ASSERT_EQ("0", storage.get("log0_node_id")); + + /* + * Adding one entry to the log, making sure it appears in the storage + */ + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + ASSERT_LE(0, log.append(entry)); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("1", storage.get("log1_term")); + ASSERT_EQ("01000000000000000000000000000000", storage.get("log1_unique_id")); + ASSERT_EQ("1", storage.get("log1_node_id")); + + ASSERT_EQ(1, log.getLastIndex()); + ASSERT_TRUE(entry == *log.getEntryAtIndex(1)); + + /* + * Adding another entry while storage is failing + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(7, storage.getNumKeys()); + + entry.term = 2; + entry.node_id = 2; + entry.unique_id[0] = 2; + ASSERT_GT(0, log.append(entry)); + + ASSERT_EQ(7, storage.getNumKeys()); // No new entries, we failed + + ASSERT_EQ(1, log.getLastIndex()); + + /* + * Making sure append() fails when the log is full + */ + storage.failOnSetCalls(false); + + while (log.getLastIndex() < (log.Capacity - 1)) + { + ASSERT_LE(0, log.append(entry)); + ASSERT_TRUE(entry == *log.getEntryAtIndex(log.getLastIndex())); + + entry.term += 1; + entry.node_id = uint8_t(entry.node_id + 1U); + entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U); + } + + ASSERT_GT(0, log.append(entry)); // Failing because full + + storage.print(); +} + + +TEST(dynamic_node_id_server_Log, Remove) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + MemoryStorageBackend storage; + Log log(storage, tracer); + + /* + * Filling the log fully + */ + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + + while (log.getLastIndex() < (log.Capacity - 1)) + { + ASSERT_LE(0, log.append(entry)); + ASSERT_TRUE(entry == *log.getEntryAtIndex(log.getLastIndex())); + + entry.term += 1; + entry.node_id = uint8_t(entry.node_id + 1U); + entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U); + } + + /* + * Removal will fail as the storage is failing to update + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); + ASSERT_GT(0, log.removeEntriesWhereIndexGreaterOrEqual(60)); // Failing + ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); + + /* + * Now removal must work + */ + storage.failOnSetCalls(false); + + ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); + + ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(60)); + ASSERT_EQ(59, log.getLastIndex()); + ASSERT_EQ("59", storage.get("log_last_index")); + + ASSERT_LE(0, log.removeEntriesWhereIndexGreater(30)); + ASSERT_EQ(30, log.getLastIndex()); + ASSERT_EQ("30", storage.get("log_last_index")); + + ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(1)); + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ("0", storage.get("log_last_index")); + + storage.print(); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp new file mode 100644 index 000000000000..a47fa8158506 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp @@ -0,0 +1,209 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "../event_tracer.hpp" +#include "../memory_storage_backend.hpp" + + +TEST(dynamic_node_id_server_PersistentState, Initialization) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + /* + * First initialization + */ + { + MemoryStorageBackend storage; + PersistentState pers(storage, tracer); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, pers.init()); + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } + /* + * Partial recovery - only empty log is recovered + */ + { + MemoryStorageBackend storage; + + { + // This log is used to initialize the storage + Log log(storage, tracer); + ASSERT_LE(0, log.init()); + } + ASSERT_LE(1, storage.getNumKeys()); + + PersistentState pers(storage, tracer); + + ASSERT_LE(0, pers.init()); + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } + /* + * Partial recovery - log and current term are recovered + */ + { + MemoryStorageBackend storage; + + { + // This log is used to initialize the storage + Log log(storage, tracer); + ASSERT_LE(0, log.init()); + } + ASSERT_LE(1, storage.getNumKeys()); + + storage.set("current_term", "1"); + + PersistentState pers(storage, tracer); + + ASSERT_GT(0, pers.init()); // Fails because current term is not zero + + storage.set("current_term", "0"); + + ASSERT_LE(0, pers.init()); // OK now + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } + /* + * Full recovery + */ + { + MemoryStorageBackend storage; + + { + // This log is used to initialize the storage + Log log(storage, tracer); + ASSERT_LE(0, log.init()); + + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + ASSERT_LE(0, log.append(entry)); + } + ASSERT_LE(4, storage.getNumKeys()); + + PersistentState pers(storage, tracer); + + ASSERT_GT(0, pers.init()); // Fails because log is not empty + + storage.set("current_term", "0"); + storage.set("voted_for", "0"); + ASSERT_GT(0, pers.init()); // Fails because of bad currentTerm + + storage.set("current_term", "1"); // OK + storage.set("voted_for", "128"); // Invalid value + ASSERT_GT(0, pers.init()); // Fails because of bad votedFor + + storage.set("voted_for", "0"); // OK now + ASSERT_LE(0, pers.init()); + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("1", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } +} + + +TEST(dynamic_node_id_server_PersistentState, Basic) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + MemoryStorageBackend storage; + PersistentState pers(storage, tracer); + + /* + * Initializing + */ + ASSERT_LE(0, pers.init()); + + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + + /* + * Inserting some log entries + */ + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + ASSERT_LE(0, pers.getLog().append(entry)); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + + /* + * Changing current term + */ + ASSERT_EQ(0, pers.getCurrentTerm()); + ASSERT_LE(0, pers.setCurrentTerm(2)); + ASSERT_EQ(2, pers.getCurrentTerm()); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("2", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + + /* + * Changing votedFor + */ + ASSERT_FALSE(pers.isVotedForSet()); + ASSERT_EQ(0, pers.getVotedFor().get()); + ASSERT_LE(0, pers.setVotedFor(0)); + ASSERT_EQ(0, pers.getVotedFor().get()); + ASSERT_LE(0, pers.setVotedFor(45)); + ASSERT_EQ(45, pers.getVotedFor().get()); + ASSERT_TRUE(pers.isVotedForSet()); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("2", storage.get("current_term")); + ASSERT_EQ("45", storage.get("voted_for")); + + ASSERT_LE(0, pers.resetVotedFor()); + ASSERT_EQ(0, pers.getVotedFor().get()); + ASSERT_FALSE(pers.isVotedForSet()); + ASSERT_EQ("0", storage.get("voted_for")); + + ASSERT_LE(0, pers.setVotedFor(45)); + ASSERT_TRUE(pers.isVotedForSet()); + ASSERT_EQ("45", storage.get("voted_for")); + + /* + * Handling errors + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(2, pers.getCurrentTerm()); + ASSERT_GT(0, pers.setCurrentTerm(7893)); + ASSERT_EQ(2, pers.getCurrentTerm()); + + ASSERT_EQ(45, pers.getVotedFor().get()); + ASSERT_GT(0, pers.setVotedFor(78)); + ASSERT_EQ(45, pers.getVotedFor().get()); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("2", storage.get("current_term")); + ASSERT_EQ("45", storage.get("voted_for")); + + /* + * Final checks + */ + ASSERT_GT(10, storage.getNumKeys()); // Making sure there's some sane number of keys in the storage +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp new file mode 100644 index 000000000000..e09fadda5296 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -0,0 +1,193 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#include +#include +#include +#include +#include "../event_tracer.hpp" +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + +using uavcan::dynamic_node_id_server::UniqueID; + + +class CommitHandler : public uavcan::dynamic_node_id_server::distributed::IRaftLeaderMonitor +{ + const std::string id_; + + virtual void handleLogCommitOnLeader(const uavcan::protocol::dynamic_node_id::server::Entry& entry) + { + std::cout << "ENTRY COMMITTED [" << id_ << "]\n" << entry << std::endl; + } + + virtual void handleLocalLeadershipChange(bool local_node_is_leader) + { + std::cout << "I AM LEADER [" << id_ << "]: " << (local_node_is_leader ? "YES" : "NOT ANYMORE") << std::endl; + } + +public: + CommitHandler(const std::string& id) : id_(id) { } +}; + + +TEST(dynamic_node_id_server_RaftCore, Basic) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + + EventTracer tracer_a("a"); + EventTracer tracer_b("b"); + MemoryStorageBackend storage_a; + MemoryStorageBackend storage_b; + CommitHandler commit_handler_a("a"); + CommitHandler commit_handler_b("b"); + + InterlinkedTestNodesWithSysClock nodes; + + std::unique_ptr raft_a(new RaftCore(nodes.a, storage_a, tracer_a, commit_handler_a)); + std::unique_ptr raft_b(new RaftCore(nodes.b, storage_b, tracer_b, commit_handler_b)); + + /* + * Initialization + */ + ASSERT_LE(0, raft_a->init(2, uavcan::TransferPriority::OneHigherThanLowest)); + ASSERT_LE(0, raft_b->init(2, uavcan::TransferPriority::OneHigherThanLowest)); + + /* + * Running and trying not to fall + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000)); + + // Either must become a leader + ASSERT_TRUE(raft_a->isLeader() || raft_b->isLeader()); + + ASSERT_EQ(0, raft_a->getCommitIndex()); + ASSERT_EQ(0, raft_b->getCommitIndex()); + + /* + * Adding some stuff + */ + Entry::FieldTypes::unique_id unique_id; + uavcan::fill_n(unique_id.begin(), 16, uint8_t(0xAA)); + + (raft_a->isLeader() ? raft_a : raft_b)->appendLog(unique_id, uavcan::NodeID(1)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000)); + + ASSERT_EQ(1, raft_a->getCommitIndex()); + ASSERT_EQ(1, raft_b->getCommitIndex()); + + /* + * Terminating the leader + */ + raft_a.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000)); + + /* + * Reinitializing the leader - current Follower will become the new Leader + */ + storage_a.reset(); + + raft_a.reset(new RaftCore(nodes.a, storage_a, tracer_a, commit_handler_a)); + ASSERT_LE(0, raft_a->init(2, uavcan::TransferPriority::OneHigherThanLowest)); + ASSERT_EQ(0, raft_a->getCommitIndex()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000)); + + ASSERT_FALSE(raft_a->isLeader()); + ASSERT_TRUE(raft_b->isLeader()); + + ASSERT_EQ(1, raft_a->getCommitIndex()); + ASSERT_EQ(1, raft_b->getCommitIndex()); +} + + +TEST(dynamic_node_id_server_Server, Basic) +{ + using namespace uavcan::dynamic_node_id_server; + using namespace uavcan::protocol::dynamic_node_id; + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + uavcan::DefaultDataTypeRegistrator _reg6; + + EventTracer tracer; + MemoryStorageBackend storage; + + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + UniqueID own_unique_id; + own_unique_id[0] = 0xAA; + own_unique_id[3] = 0xCC; + own_unique_id[7] = 0xEE; + own_unique_id[9] = 0xBB; + + /* + * Server + */ + DistributedServer server(nodes.a, storage, tracer); + + ASSERT_LE(0, server.init(own_unique_id, 1)); + + ASSERT_EQ(0, server.getNumAllocations()); + + /* + * Client + */ + uavcan::DynamicNodeIDClient client(nodes.b); + uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id; + for (uavcan::uint8_t i = 0; i < unique_id.size(); i++) + { + unique_id[i] = i; + } + const uavcan::NodeID PreferredNodeID = 42; + ASSERT_LE(0, client.start(unique_id, PreferredNodeID)); + + /* + * Fire + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(15000)); + + ASSERT_TRUE(client.isAllocationComplete()); + ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); + + ASSERT_EQ(2, server.getNumAllocations()); // Server's own node ID + client +} + + +TEST(dynamic_node_id_server, ObjectSizes) +{ + using namespace uavcan; + using namespace uavcan::protocol::dynamic_node_id::server; + using namespace uavcan::dynamic_node_id_server; + + std::cout << "distributed::Log: " << sizeof(distributed::Log) << std::endl; + std::cout << "distributed::PersistentState: " << sizeof(distributed::PersistentState) << std::endl; + std::cout << "distributed::ClusterManager: " << sizeof(distributed::ClusterManager) << std::endl; + std::cout << "distributed::RaftCore: " << sizeof(distributed::RaftCore) << std::endl; + std::cout << "distributed::Server: " << sizeof(distributed::Server) << std::endl; + std::cout << "AllocationRequestManager: " << sizeof(AllocationRequestManager) << std::endl; + + std::cout << "ServiceServer: " << sizeof(ServiceServer) << std::endl; + std::cout << "ServiceClient: " << sizeof(ServiceClient) << std::endl; + std::cout << "ServiceServer: " << sizeof(ServiceServer) << std::endl; + std::cout << "ServiceClient: " << sizeof(ServiceClient) << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/event.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/event.cpp new file mode 100644 index 000000000000..7405f5d85033 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/event.cpp @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "event_tracer.hpp" + + +TEST(dynamic_node_id_server_EventTracer, EventCodeToString) +{ + using namespace uavcan::dynamic_node_id_server; + + // Simply checking some error codes + ASSERT_STREQ("Error", IEventTracer::getEventName(TraceError)); + ASSERT_STREQ("RaftAppendEntriesCallFailure", IEventTracer::getEventName(TraceRaftAppendEntriesCallFailure)); + ASSERT_STREQ("RaftDiscoveryReceived", IEventTracer::getEventName(TraceRaftDiscoveryReceived)); + ASSERT_STREQ("DiscoveryNodeRestartDetected", IEventTracer::getEventName(TraceDiscoveryNodeRestartDetected)); + ASSERT_STREQ("AllocationUnexpectedStage", IEventTracer::getEventName(TraceAllocationUnexpectedStage)); +} + + +TEST(dynamic_node_id_server_EventTracer, EnvironmentSelfTest) +{ + using namespace uavcan::dynamic_node_id_server; + + EventTracer tracer; + + ASSERT_EQ(0, tracer.getNumEvents()); + + tracer.onEvent(TraceRaftAppendEntriesCallFailure, 123); + ASSERT_EQ(1, tracer.getNumEvents()); + tracer.onEvent(TraceRaftAppendEntriesCallFailure, -456); + ASSERT_EQ(2, tracer.getNumEvents()); + tracer.onEvent(TraceError, -0xFFFFFFFFFFFFFFFLL); + ASSERT_EQ(3, tracer.getNumEvents()); + + ASSERT_EQ(0, tracer.countEvents(TraceAllocationActivity)); + ASSERT_EQ(2, tracer.countEvents(TraceRaftAppendEntriesCallFailure)); + ASSERT_EQ(1, tracer.countEvents(TraceError)); + + ASSERT_EQ(-456, tracer.getLastEventArgumentOrFail(TraceRaftAppendEntriesCallFailure)); + + ASSERT_EQ(-0xFFFFFFFFFFFFFFFLL, tracer.getLastEventArgumentOrFail(TraceError)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp new file mode 100644 index 000000000000..b2c30ce6ab56 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp @@ -0,0 +1,77 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include "../../clock.hpp" + + +class EventTracer : public uavcan::dynamic_node_id_server::IEventTracer +{ + struct EventLogEntry + { + const uavcan::dynamic_node_id_server::TraceCode code; + const uavcan::int64_t argument; + + EventLogEntry(uavcan::dynamic_node_id_server::TraceCode arg_code, uavcan::int64_t arg_argument) + : code(arg_code) + , argument(arg_argument) + { } + }; + + const std::string id_; + const uavcan::MonotonicTime startup_ts_; + std::list event_log_; + +public: + EventTracer() : + startup_ts_(SystemClockDriver().getMonotonic()) + { } + + EventTracer(const std::string& id) : + id_(id), + startup_ts_(SystemClockDriver().getMonotonic()) + { } + + virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) + { + const uavcan::MonotonicDuration ts = SystemClockDriver().getMonotonic() - startup_ts_; + std::cout << "EVENT [" << id_ << "]\t" << ts.toString() << "\t" + << int(code) << "\t" << getEventName(code) << "\t" << argument << std::endl; + event_log_.push_back(EventLogEntry(code, argument)); + } + + unsigned countEvents(const uavcan::dynamic_node_id_server::TraceCode code) const + { + unsigned count = 0; + for (std::list::const_iterator it = event_log_.begin(); it != event_log_.end(); ++it) + { + count += (it->code == code) ? 1U : 0U; + } + return count; + } + + uavcan::int64_t getLastEventArgumentOrFail(const uavcan::dynamic_node_id_server::TraceCode code) const + { + for (std::list::const_reverse_iterator it = event_log_.rbegin(); it != event_log_.rend(); ++it) + { + if (it->code == code) + { + return it->argument; + } + } + + std::cout << "No such event in the event log, code " << int(code) << ", log length " << event_log_.size() + << std::endl; + + throw std::runtime_error("EventTracer::getLastEventArgumentOrFail()"); + } + + unsigned getNumEvents() const { return static_cast(event_log_.size()); } +}; diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp new file mode 100644 index 000000000000..e1da5b02baa0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include + +class GetNodeInfoMockServer +{ + typedef uavcan::MethodBinder&, + uavcan::protocol::GetNodeInfo::Response&) const> + GetNodeInfoCallback; + + uavcan::ServiceServer server_; + + void handleRequest(const uavcan::ReceivedDataStructure& req, + uavcan::protocol::GetNodeInfo::Response& res) const + { + res = response; + std::cout << "GET NODE INFO:\nREQUEST\n" << req << "RESPONSE\n" << res << std::endl; + } + +public: + uavcan::protocol::GetNodeInfo::Response response; + + GetNodeInfoMockServer(uavcan::INode& node) : server_(node) { } + + int start() { return server_.start(GetNodeInfoCallback(this, &GetNodeInfoMockServer::handleRequest)); } +}; diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/memory_storage_backend.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/memory_storage_backend.hpp new file mode 100644 index 000000000000..516c33e994d3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/memory_storage_backend.hpp @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#pragma once + +#include +#include + +class MemoryStorageBackend : public uavcan::dynamic_node_id_server::IStorageBackend +{ + typedef std::map Container; + Container container_; + + bool fail_; + +public: + MemoryStorageBackend() + : fail_(false) + { } + + virtual String get(const String& key) const + { + const Container::const_iterator it = container_.find(key); + if (it == container_.end()) + { + return String(); + } + return it->second; + } + + virtual void set(const String& key, const String& value) + { + if (!fail_) + { + container_[key] = value; + } + } + + void failOnSetCalls(bool really) { fail_ = really; } + + void reset() { container_.clear(); } + + unsigned getNumKeys() const { return unsigned(container_.size()); } + + void print() const + { + for (Container::const_iterator it = container_.begin(); it != container_.end(); ++it) + { + std::cout << it->first.c_str() << "\t" << it->second.c_str() << std::endl; + } + } +}; diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp new file mode 100644 index 000000000000..64df33baec0b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -0,0 +1,284 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#include +#include +#include +#include +#include "event_tracer.hpp" +#include "get_node_info_mock_server.hpp" +#include "../helpers.hpp" + +using namespace uavcan::dynamic_node_id_server; + + +class NodeDiscoveryHandler : public uavcan::dynamic_node_id_server::INodeDiscoveryHandler +{ +public: + struct NodeInfo + { + UniqueID unique_id; + uavcan::NodeID node_id; + bool committed; + + NodeInfo() : committed(false) { } + }; + + bool can_discover; + std::vector nodes; + + NodeDiscoveryHandler() : can_discover(false) { } + + virtual bool canDiscoverNewNodes() const + { + return can_discover; + } + + virtual NodeAwareness checkNodeAwareness(uavcan::NodeID node_id) const + { + const NodeInfo* const ni = const_cast(this)->findNode(node_id); + if (ni == UAVCAN_NULLPTR) + { + return NodeAwarenessUnknown; + } + return ni->committed ? NodeAwarenessKnownAndCommitted : NodeAwarenessKnownButNotCommitted; + } + + virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, uavcan::NodeID node_id) + { + NodeInfo info; + if (unique_id_or_null != UAVCAN_NULLPTR) + { + info.unique_id = *unique_id_or_null; + } + info.node_id = node_id; + nodes.push_back(info); + } + + NodeInfo* findNode(const UniqueID& unique_id) + { + for (unsigned i = 0; i < nodes.size(); i++) + { + if (nodes.at(i).unique_id == unique_id) + { + return &nodes.at(i); + } + } + return UAVCAN_NULLPTR; + } + + NodeInfo* findNode(const uavcan::NodeID& node_id) + { + for (unsigned i = 0; i < nodes.size(); i++) + { + if (nodes.at(i).node_id == node_id) + { + return &nodes.at(i); + } + } + return UAVCAN_NULLPTR; + } +}; + + +TEST(dynamic_node_id_server_NodeDiscoverer, Basic) +{ + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + EventTracer tracer; + InterlinkedTestNodesWithSysClock nodes; + NodeDiscoveryHandler handler; + + NodeDiscoverer disc(nodes.a, tracer, handler); + + /* + * Initialization + */ + ASSERT_LE(0, disc.init(uavcan::TransferPriority::OneHigherThanLowest)); + + ASSERT_FALSE(disc.hasUnknownNodes()); + + /* + * Publishing NodeStatus, discovery is disabled + */ + std::cout << "!!! Publishing NodeStatus, discovery is disabled" << std::endl; + handler.can_discover = false; + + uavcan::Publisher node_status_pub(nodes.b); + ASSERT_LE(0, node_status_pub.init()); + + uavcan::protocol::NodeStatus node_status; + node_status.uptime_sec = 0; + ASSERT_LE(0, node_status_pub.broadcast(node_status)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); // The timer runs as long as there are unknown nodes + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); // Querying is disabled! + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_TRUE(disc.hasUnknownNodes()); + + /* + * Enabling discovery - the querying will continue despite the fact that NodeStatus messages are not arriving + */ + std::cout << "!!! Enabling discovery" << std::endl; + handler.can_discover = true; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1150)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); + ASSERT_EQ(2, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_TRUE(disc.hasUnknownNodes()); + + /* + * Publishing NodeStatus + */ + std::cout << "!!! Publishing NodeStatus" << std::endl; + + node_status.uptime_sec += 5U; + ASSERT_LE(0, node_status_pub.broadcast(node_status)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1250)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); + ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_TRUE(disc.hasUnknownNodes()); + + /* + * Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized + */ + std::cout << "!!! Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized" << std::endl; + + GetNodeInfoMockServer get_node_info_server(nodes.b); + get_node_info_server.response.hardware_version.unique_id[0] = 123; // Arbitrary data + get_node_info_server.response.hardware_version.unique_id[6] = 213; + get_node_info_server.response.hardware_version.unique_id[14] = 52; + ASSERT_LE(0, get_node_info_server.start()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop)); + ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized)); + ASSERT_FALSE(disc.hasUnknownNodes()); + + /* + * Checking the results + */ + ASSERT_TRUE(handler.findNode(get_node_info_server.response.hardware_version.unique_id)); + ASSERT_EQ(2, handler.findNode(get_node_info_server.response.hardware_version.unique_id)->node_id.get()); +} + + +TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts) +{ + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + EventTracer tracer; + InterlinkedTestNodesWithSysClock nodes; + NodeDiscoveryHandler handler; + + NodeDiscoverer disc(nodes.a, tracer, handler); + + /* + * Initialization + */ + ASSERT_LE(0, disc.init(uavcan::TransferPriority::OneHigherThanLowest)); + + ASSERT_FALSE(disc.hasUnknownNodes()); + + /* + * Publishing NodeStatus once to trigger querying + * Querying for 2 seconds, no responses will be sent (there's no server) + */ + handler.can_discover = true; + + uavcan::Publisher node_status_pub(nodes.b); + ASSERT_LE(0, node_status_pub.init()); + + uavcan::protocol::NodeStatus node_status; + node_status.uptime_sec = 10; // Nonzero + ASSERT_LE(0, node_status_pub.broadcast(node_status)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); + ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeRestartDetected)); + ASSERT_TRUE(disc.hasUnknownNodes()); + + /* + * Emulating node restart + */ + node_status.uptime_sec = 9; // Less than previous + ASSERT_LE(0, node_status_pub.broadcast(node_status)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); + ASSERT_EQ(7, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(6, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected)); + ASSERT_TRUE(disc.hasUnknownNodes()); + + /* + * Waiting for timeout + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop)); + ASSERT_EQ(8, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(8, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected)); + ASSERT_FALSE(disc.hasUnknownNodes()); + + /* + * Checking the results + */ + ASSERT_TRUE(handler.findNode(UniqueID())); + ASSERT_EQ(2, handler.findNode(UniqueID())->node_id.get()); +} + + +TEST(dynamic_node_id_server_NodeDiscoverer, Sizes) +{ + using namespace uavcan; + + std::cout << "BitSet: " << sizeof(BitSet) << std::endl; + std::cout << "ServiceClient: " << sizeof(ServiceClient) << std::endl; + std::cout << "protocol::GetNodeInfo::Response: " << sizeof(protocol::GetNodeInfo::Response) << std::endl; + std::cout << "Subscriber: " << sizeof(Subscriber) << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/storage_marshaller.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/storage_marshaller.cpp new file mode 100644 index 000000000000..53be55825934 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/storage_marshaller.cpp @@ -0,0 +1,88 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "memory_storage_backend.hpp" + +TEST(dynamic_node_id_server_StorageMarshaller, Basic) +{ + MemoryStorageBackend st; + + uavcan::dynamic_node_id_server::StorageMarshaller marshaler(st); + + uavcan::dynamic_node_id_server::IStorageBackend::String key; + + /* + * uint32 + */ + uint32_t u32 = 0; + + key = "foo"; + u32 = 0; + ASSERT_LE(0, marshaler.setAndGetBack(key, u32)); + ASSERT_EQ(0, u32); + + key = "bar"; + u32 = 0xFFFFFFFF; + ASSERT_LE(0, marshaler.setAndGetBack(key, u32)); + ASSERT_EQ(0xFFFFFFFF, u32); + ASSERT_LE(0, marshaler.get(key, u32)); + ASSERT_EQ(0xFFFFFFFF, u32); + + key = "foo"; + ASSERT_LE(0, marshaler.get(key, u32)); + ASSERT_EQ(0, u32); + + key = "the_cake_is_a_lie"; + ASSERT_GT(0, marshaler.get(key, u32)); + ASSERT_EQ(0, u32); + + /* + * uint8[16] + */ + uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id array; + + key = "a"; + // Set zero + ASSERT_LE(0, marshaler.setAndGetBack(key, array)); + for (uint8_t i = 0; i < 16; i++) + { + ASSERT_EQ(0, array[i]); + } + + // Make sure this will not be interpreted as uint32 + ASSERT_GT(0, marshaler.get(key, u32)); + ASSERT_EQ(0, u32); + + // Set pattern + for (uint8_t i = 0; i < 16; i++) + { + array[i] = uint8_t(i + 1); + } + ASSERT_LE(0, marshaler.setAndGetBack(key, array)); + for (uint8_t i = 0; i < 16; i++) + { + ASSERT_EQ(i + 1, array[i]); + } + + // Set another pattern + for (uint8_t i = 0; i < 16; i++) + { + array[i] = uint8_t(i | (i << 4)); + } + ASSERT_LE(0, marshaler.setAndGetBack(key, array)); + for (uint8_t i = 0; i < 16; i++) + { + ASSERT_EQ(uint8_t(i | (i << 4)), array[i]); + } + + // Make sure uint32 cannot be interpreted as an array + key = "foo"; + ASSERT_GT(0, marshaler.get(key, array)); + + // Nonexistent key + key = "the_cake_is_a_lie"; + ASSERT_GT(0, marshaler.get(key, array)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/file_server.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/file_server.cpp new file mode 100644 index 000000000000..be970b4d0409 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/file_server.cpp @@ -0,0 +1,136 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +class TestFileServerBackend : public uavcan::IFileServerBackend +{ +public: + static const std::string file_name; + static const std::string file_data; + + virtual int16_t getInfo(const Path& path, uint64_t& out_size, EntryType& out_type) + { + if (path == file_name) + { + out_size = uint16_t(file_data.length()); + out_type.flags |= EntryType::FLAG_FILE; + out_type.flags |= EntryType::FLAG_READABLE; + return 0; + } + else + { + return Error::NOT_FOUND; + } + } + + virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size) + { + if (path == file_name) + { + if (offset < file_data.length()) + { + inout_size = uint16_t(file_data.length() - offset); + std::memcpy(out_buffer, file_data.c_str() + offset, inout_size); + } + else + { + inout_size = 0; + } + return 0; + } + else + { + return Error::NOT_FOUND; + } + } +}; + +const std::string TestFileServerBackend::file_name = "test"; +const std::string TestFileServerBackend::file_data = "123456789"; + +TEST(BasicFileServer, Basic) +{ + using namespace uavcan::protocol::file; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + InterlinkedTestNodesWithSysClock nodes; + + TestFileServerBackend backend; + + uavcan::BasicFileServer serv(nodes.a, backend); + std::cout << "sizeof(uavcan::BasicFileServer): " << sizeof(uavcan::BasicFileServer) << std::endl; + + ASSERT_LE(0, serv.start()); + + /* + * GetInfo, existing file + */ + { + ServiceClientWithCollector get_info(nodes.b); + + GetInfo::Request get_info_req; + get_info_req.path.path = "test"; + + ASSERT_LE(0, get_info.call(1, get_info_req)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(get_info.collector.result.get()); + ASSERT_TRUE(get_info.collector.result->isSuccessful()); + + ASSERT_EQ(0, get_info.collector.result->getResponse().error.value); + ASSERT_EQ(9, get_info.collector.result->getResponse().size); + ASSERT_EQ(EntryType::FLAG_FILE | EntryType::FLAG_READABLE, + get_info.collector.result->getResponse().entry_type.flags); + } + + /* + * Read, existing file + */ + { + ServiceClientWithCollector read(nodes.b); + + Read::Request read_req; + read_req.path.path = "test"; + + ASSERT_LE(0, read.call(1, read_req)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(read.collector.result.get()); + ASSERT_TRUE(read.collector.result->isSuccessful()); + + ASSERT_EQ("123456789", read.collector.result->getResponse().data); + ASSERT_EQ(0, read.collector.result->getResponse().error.value); + } +} + + +TEST(FileServer, Basic) +{ + using namespace uavcan::protocol::file; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + + InterlinkedTestNodesWithSysClock nodes; + + TestFileServerBackend backend; + + uavcan::FileServer serv(nodes.a, backend); + std::cout << "sizeof(uavcan::FileServer): " << sizeof(uavcan::FileServer) << std::endl; + + ASSERT_LE(0, serv.start()); + + // TODO TEST +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/firmware_update_trigger.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/firmware_update_trigger.cpp new file mode 100644 index 000000000000..a17e92c4423f --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -0,0 +1,336 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + +using namespace uavcan::protocol::file; + +struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker +{ + unsigned should_request_cnt; + unsigned should_retry_cnt; + unsigned confirmation_cnt; + + std::string firmware_path; + + int retry_quota; + std::string expected_node_name_to_update; + + BeginFirmwareUpdate::Response last_error_response; + + FirmwareVersionChecker() + : should_request_cnt(0) + , should_retry_cnt(0) + , confirmation_cnt(0) + , retry_quota(0) + { } + + virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, + const uavcan::protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) + { + should_request_cnt++; + std::cout << "REQUEST? " << int(node_id.get()) << "\n" << node_info << std::endl; + out_firmware_file_path = firmware_path.c_str(); + return node_info.name == expected_node_name_to_update; + } + + virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID node_id, + const BeginFirmwareUpdate::Response& error_response, + FirmwareFilePath& out_firmware_file_path) + { + last_error_response = error_response; + std::cout << "RETRY? " << int(node_id.get()) << "\n" << error_response << std::endl; + should_retry_cnt++; + + EXPECT_STREQ(firmware_path.c_str(), out_firmware_file_path.c_str()); + + if (retry_quota > 0) + { + retry_quota--; + return true; + } + else + { + return false; + } + } + + virtual void handleFirmwareUpdateConfirmation(uavcan::NodeID node_id, + const BeginFirmwareUpdate::Response& response) + { + confirmation_cnt++; + std::cout << "CONFIRMED " << int(node_id.get()) << "\n" << response << std::endl; + } +}; + +struct BeginFirmwareUpdateServer +{ + uint8_t response_error_code; + + BeginFirmwareUpdateServer() : response_error_code(0) { } + + void handleRequest(const uavcan::ReceivedDataStructure& req, + uavcan::ServiceResponseDataStructure& res) const + { + std::cout << "REQUEST\n" << req << std::endl; + res.error = response_error_code; + res.optional_error_message = "foobar"; + } + + typedef uavcan::MethodBinder&, + uavcan::ServiceResponseDataStructure&) const> Callback; + + Callback makeCallback() { return Callback(this, &BeginFirmwareUpdateServer::handleRequest); } +}; + + +TEST(FirmwareUpdateTrigger, Basic) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + + InterlinkedTestNodesWithSysClock nodes; + + FirmwareVersionChecker checker; + + uavcan::NodeInfoRetriever node_info_retriever(nodes.a); // On the same node + + uavcan::FirmwareUpdateTrigger trigger(nodes.a, checker); + std::cout << "sizeof(uavcan::FirmwareUpdateTrigger): " << sizeof(uavcan::FirmwareUpdateTrigger) << std::endl; + + std::unique_ptr provider(new uavcan::NodeStatusProvider(nodes.b)); // Other node + + /* + * Initializing + */ + ASSERT_LE(0, trigger.start(node_info_retriever, "/path_prefix/")); + + ASSERT_LE(0, node_info_retriever.start()); + ASSERT_EQ(1, node_info_retriever.getNumListeners()); + + uavcan::protocol::HardwareVersion hwver; + hwver.unique_id[0] = 123; + hwver.unique_id[4] = 213; + hwver.unique_id[8] = 45; + + provider->setName("Ivan"); + provider->setHardwareVersion(hwver); + + ASSERT_LE(0, provider->startAndPublish()); + + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + /* + * Updating one node + * The server that can confirm the request is not running yet + */ + checker.firmware_path = "firmware_path"; + checker.expected_node_name_to_update = "Ivan"; + checker.retry_quota = 1000; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(1, trigger.getNumPendingNodes()); + + ASSERT_EQ(1, checker.should_request_cnt); + ASSERT_EQ(0, checker.should_retry_cnt); + ASSERT_EQ(0, checker.confirmation_cnt); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + // Still running! + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(1, trigger.getNumPendingNodes()); + + /* + * Starting the firmware update server that returns an error + * The checker will instruct the trigger to repeat + */ + uavcan::ServiceServer srv(nodes.b); + BeginFirmwareUpdateServer srv_impl; + + ASSERT_LE(0, srv.start(srv_impl.makeCallback())); + + srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_UNKNOWN; + checker.retry_quota = 1000; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + + ASSERT_EQ(1, checker.should_request_cnt); + ASSERT_EQ(1, checker.should_retry_cnt); + ASSERT_EQ(0, checker.confirmation_cnt); + + // Still running! + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(1, trigger.getNumPendingNodes()); + + /* + * Trying again, this time with ERROR_IN_PROGRESS + */ + srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; + checker.retry_quota = 0; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100)); + + ASSERT_EQ(1, checker.should_request_cnt); + ASSERT_EQ(1, checker.should_retry_cnt); + ASSERT_EQ(1, checker.confirmation_cnt); + + // Stopped! + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + /* + * Restarting the node info provider + * Now it doesn't need an update + */ + provider.reset(new uavcan::NodeStatusProvider(nodes.b)); + + provider->setName("Dmitry"); + provider->setHardwareVersion(hwver); + + ASSERT_LE(0, provider->startAndPublish()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100)); + + ASSERT_EQ(2, checker.should_request_cnt); + ASSERT_EQ(1, checker.should_retry_cnt); + ASSERT_EQ(1, checker.confirmation_cnt); + + // Stopped! + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + /* + * Final checks + */ + ASSERT_EQ(0, nodes.a.internal_failure_count); + ASSERT_EQ(0, nodes.b.internal_failure_count); +} + + +TEST(FirmwareUpdateTrigger, MultiNode) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + + TestNetwork<5> nodes; + + // The trigger node + FirmwareVersionChecker checker; + uavcan::NodeInfoRetriever node_info_retriever(nodes[0]); + uavcan::FirmwareUpdateTrigger trigger(nodes[0], checker); + + // The client nodes + std::unique_ptr provider_a(new uavcan::NodeStatusProvider(nodes[1])); + std::unique_ptr provider_b(new uavcan::NodeStatusProvider(nodes[2])); + std::unique_ptr provider_c(new uavcan::NodeStatusProvider(nodes[3])); + std::unique_ptr provider_d(new uavcan::NodeStatusProvider(nodes[4])); + + uavcan::protocol::HardwareVersion hwver; + + /* + * Initializing + */ + ASSERT_LE(0, trigger.start(node_info_retriever, "/path_prefix/")); + + ASSERT_LE(0, node_info_retriever.start()); + ASSERT_EQ(1, node_info_retriever.getNumListeners()); + + hwver.unique_id[0] = 0xAA; + provider_a->setHardwareVersion(hwver); + provider_a->setName("Victor"); + ASSERT_LE(0, provider_a->startAndPublish()); + + hwver.unique_id[0] = 0xBB; + provider_b->setHardwareVersion(hwver); + provider_b->setName("Victor"); + ASSERT_LE(0, provider_b->startAndPublish()); + + hwver.unique_id[0] = 0xCC; + provider_c->setHardwareVersion(hwver); + provider_c->setName("Alexey"); + ASSERT_LE(0, provider_c->startAndPublish()); + + hwver.unique_id[0] = 0xDD; + provider_d->setHardwareVersion(hwver); + provider_d->setName("Victor"); + ASSERT_LE(0, provider_d->startAndPublish()); + + checker.expected_node_name_to_update = "Victor"; // Victors will get updated, others will not + checker.firmware_path = "abc"; + + /* + * Running - 3 will timout, 1 will be ignored + */ + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + nodes.spinAll(uavcan::MonotonicDuration::fromMSec(4100)); + + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(3, trigger.getNumPendingNodes()); + + ASSERT_EQ(4, checker.should_request_cnt); + ASSERT_EQ(0, checker.should_retry_cnt); + ASSERT_EQ(0, checker.confirmation_cnt); + + /* + * Initializing the BeginFirmwareUpdate servers + */ + uavcan::ServiceServer srv_a(nodes[1]); + uavcan::ServiceServer srv_b(nodes[2]); + uavcan::ServiceServer srv_c(nodes[3]); + uavcan::ServiceServer srv_d(nodes[4]); + + BeginFirmwareUpdateServer srv_a_impl; + BeginFirmwareUpdateServer srv_b_impl; + BeginFirmwareUpdateServer srv_c_impl; + BeginFirmwareUpdateServer srv_d_impl; + + ASSERT_LE(0, srv_a.start(srv_a_impl.makeCallback())); + ASSERT_LE(0, srv_b.start(srv_b_impl.makeCallback())); + ASSERT_LE(0, srv_c.start(srv_c_impl.makeCallback())); + ASSERT_LE(0, srv_d.start(srv_d_impl.makeCallback())); + + srv_a_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // retry + srv_b_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // retry + srv_c_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // ignore (see below) + srv_d_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_OK; // OK + + /* + * Spinning, now we're getting some errors + * This also checks correctness of the round-robin selector + */ + checker.retry_quota = 2; + nodes.spinAll(uavcan::MonotonicDuration::fromMSec(4200)); // Two will retry, one drop, one confirm + + ASSERT_TRUE(trigger.isTimerRunning()); + + nodes.spinAll(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_EQ(0, trigger.getNumPendingNodes()); // All removed now + + EXPECT_EQ(4, checker.should_request_cnt); + EXPECT_EQ(4, checker.should_retry_cnt); + EXPECT_EQ(1, checker.confirmation_cnt); + + /* + * Waiting for the timer to stop + */ + nodes.spinAll(uavcan::MonotonicDuration::fromMSec(1100)); + + ASSERT_FALSE(trigger.isTimerRunning()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/global_time_sync_master.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/global_time_sync_master.cpp new file mode 100644 index 000000000000..bed4d908dc59 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/global_time_sync_master.cpp @@ -0,0 +1,131 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + +struct GlobalTimeSyncMasterTestNode +{ + SystemClockDriver clock; + PairableCanDriver can; + TestNode node; + + GlobalTimeSyncMasterTestNode(uavcan::NodeID nid) + : can(clock) + , node(can, clock, nid) + { } +}; + +struct GlobalTimeSyncTestNetwork +{ + GlobalTimeSyncMasterTestNode slave; + GlobalTimeSyncMasterTestNode master_low; + GlobalTimeSyncMasterTestNode master_high; + + GlobalTimeSyncTestNetwork() + : slave(64) + , master_low(120) + , master_high(8) + { + slave.can.others.insert(&master_low.can); + master_low.can.others.insert(&slave.can); + master_high.can.others.insert(&slave.can); + } + + void spinAll(uavcan::MonotonicDuration duration = uavcan::MonotonicDuration::fromMSec(9)) + { + assert(!duration.isNegative()); + unsigned nspins3 = unsigned(duration.toMSec() / 3); + nspins3 = nspins3 ? nspins3 : 2; + while (nspins3 --> 0) + { + ASSERT_LE(0, slave.node.spin(uavcan::MonotonicDuration::fromMSec(1))); + ASSERT_LE(0, master_low.node.spin(uavcan::MonotonicDuration::fromMSec(1))); + ASSERT_LE(0, master_high.node.spin(uavcan::MonotonicDuration::fromMSec(1))); + } + } +}; + +TEST(GlobalTimeSyncMaster, Basic) +{ + GlobalTimeSyncTestNetwork nwk; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + uavcan::GlobalTimeSyncSlave slave(nwk.slave.node); + uavcan::GlobalTimeSyncMaster master_low(nwk.master_low.node); + uavcan::GlobalTimeSyncMaster master_high(nwk.master_high.node); + + ASSERT_FALSE(master_low.isInitialized()); + + ASSERT_LE(0, slave.start()); + ASSERT_LE(0, master_low.init()); + ASSERT_LE(0, master_high.init()); + + ASSERT_TRUE(master_low.isInitialized()); + ASSERT_FALSE(slave.isActive()); + + /* + * Simple synchronization + */ + ASSERT_LE(0, master_low.publish()); // Update + nwk.spinAll(); + + usleep(400000); + ASSERT_LE(0, master_low.publish()); // Adjustment + nwk.spinAll(); + + // Synchronization complete. + ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc())); + ASSERT_TRUE(slave.isActive()); + ASSERT_EQ(nwk.master_low.node.getNodeID(), slave.getMasterNodeID()); + + /* + * Moving clocks forward and re-syncing with another master + */ + static const uavcan::UtcDuration OneDay = uavcan::UtcDuration::fromMSec(24 * 3600 * 1000); + nwk.master_high.clock.utc_adjustment = OneDay; + + usleep(400000); + ASSERT_LE(0, master_low.publish()); // Update from the old master + nwk.spinAll(); + + ASSERT_LE(0, master_high.publish()); // Update from the new master + nwk.spinAll(); + + usleep(400000); + ASSERT_LE(0, master_low.publish()); // Adjustment from the old master (ignored now) + ASSERT_LE(0, master_high.publish()); // Adjustment from the new master (accepted) + nwk.spinAll(); + + // Synchronization complete. + ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_high.clock.getUtc())); + ASSERT_FALSE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc())); + ASSERT_TRUE(slave.isActive()); + ASSERT_EQ(nwk.master_high.node.getNodeID(), slave.getMasterNodeID()); + + /* + * Frequent calls to publish() + */ + ASSERT_LE(0, master_low.publish()); // Dropped + ASSERT_LE(0, master_low.publish()); // Dropped + ASSERT_LE(0, master_low.publish()); // Dropped + + ASSERT_TRUE(nwk.slave.can.read_queue.empty()); + + usleep(400000); + ASSERT_LE(0, master_low.publish()); // Accepted + ASSERT_FALSE(nwk.slave.can.read_queue.empty()); + + nwk.spinAll(); + + // Synchronization did not change + ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_high.clock.getUtc())); + ASSERT_FALSE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc())); + ASSERT_TRUE(slave.isActive()); + ASSERT_EQ(nwk.master_high.node.getNodeID(), slave.getMasterNodeID()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/global_time_sync_slave.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/global_time_sync_slave.cpp new file mode 100644 index 000000000000..0ac4dda88309 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/global_time_sync_slave.cpp @@ -0,0 +1,314 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + + +TEST(GlobalTimeSyncSlave, Basic) +{ + InterlinkedTestNodesWithClockMock nodes(64, 65); + + SystemClockMock& slave_clock = nodes.clock_a; + SystemClockMock& master_clock = nodes.clock_b; + + slave_clock.advance(1000000); + master_clock.advance(1000000); + + master_clock.monotonic_auto_advance = slave_clock.monotonic_auto_advance = 1000; + master_clock.preserve_utc = slave_clock.preserve_utc = true; + slave_clock.utc = 0; // Not set yet + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + uavcan::GlobalTimeSyncSlave gtss(nodes.a); + uavcan::Publisher gts_pub(nodes.b); + + ASSERT_LE(0, gtss.start()); + ASSERT_FALSE(gtss.isActive()); + ASSERT_FALSE(gtss.getMasterNodeID().isValid()); + + /* + * Empty broadcast + * The slave must only register the timestamp and adjust nothing + */ + uavcan::protocol::GlobalTimeSync gts; + gts.previous_transmission_timestamp_usec = 0; + gts_pub.broadcast(gts); + gts.previous_transmission_timestamp_usec = master_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_EQ(0, slave_clock.utc); + ASSERT_EQ(1000000, master_clock.utc); + std::cout << "Master mono=" << master_clock.monotonic << " utc=" << master_clock.utc << std::endl; + std::cout << "Slave mono=" << slave_clock.monotonic << " utc=" << slave_clock.utc << std::endl; + + ASSERT_FALSE(gtss.isActive()); + ASSERT_FALSE(gtss.getMasterNodeID().isValid()); + + /* + * Follow-up broadcast with proper time + * Slave must adjust now + */ + gts_pub.broadcast(gts); + gts.previous_transmission_timestamp_usec = master_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(1000000, slave_clock.utc); + ASSERT_EQ(1000000, master_clock.utc); + std::cout << "Master mono=" << master_clock.monotonic << " utc=" << master_clock.utc << std::endl; + std::cout << "Slave mono=" << slave_clock.monotonic << " utc=" << slave_clock.utc << std::endl; + + master_clock.utc += 1000000; + slave_clock.utc += 1000000; + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID()); + + /* + * Next follow-up, slave is synchronized now + * Will update + */ + gts_pub.broadcast(gts); + gts.previous_transmission_timestamp_usec = master_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(2000000, slave_clock.utc); + ASSERT_EQ(2000000, master_clock.utc); + + master_clock.utc += 1000000; + slave_clock.utc += 1000000; + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID()); + + /* + * Next follow-up, slave is synchronized now + * Will adjust + */ + gts_pub.broadcast(gts); + gts.previous_transmission_timestamp_usec = master_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(3000000, slave_clock.utc); + ASSERT_EQ(3000000, master_clock.utc); + + master_clock.utc += 1000000; + slave_clock.utc += 1000000; + ASSERT_EQ(4000000, slave_clock.utc); + ASSERT_EQ(4000000, master_clock.utc); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID()); + + /* + * Another master + * This one has higher priority, so it will be preferred + */ + SystemClockMock master2_clock(100); + master2_clock.monotonic_auto_advance = 1000; + master2_clock.preserve_utc = true; + PairableCanDriver master2_can(master2_clock); + master2_can.others.insert(&nodes.can_a); + TestNode master2_node(master2_can, master2_clock, 8); + + uavcan::Publisher gts_pub2(master2_node); + + /* + * Update step, no adjustment yet + */ + gts.previous_transmission_timestamp_usec = 0; + gts_pub2.broadcast(gts); + gts.previous_transmission_timestamp_usec = master2_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(4000000, slave_clock.utc); + ASSERT_EQ(100, master2_clock.utc); + + master2_clock.utc += 1000000; + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID()); + + /* + * Adjustment + */ + gts_pub2.broadcast(gts); + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(100, slave_clock.utc); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID()); + + /* + * Another master will be ignored now + */ + gts.previous_transmission_timestamp_usec = 99999999; + // Update + gts_pub.broadcast(gts); + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(100, slave_clock.utc); + // Adjust + gts_pub.broadcast(gts); + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(100, slave_clock.utc); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID()); + + /* + * Timeout + */ + slave_clock.advance(100000000); + + ASSERT_FALSE(gtss.isActive()); + ASSERT_FALSE(gtss.getMasterNodeID().isValid()); +} + + +#if !defined(BYTE_ORDER) || !defined(LITTLE_ENDIAN) || (BYTE_ORDER != LITTLE_ENDIAN) +# error "This test cannot be executed on this platform" +#endif + +static uavcan::Frame makeSyncMsg(uavcan::uint64_t usec, uavcan::NodeID snid, uavcan::TransferID tid) +{ + uavcan::Frame frame(uavcan::protocol::GlobalTimeSync::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + snid, uavcan::NodeID::Broadcast, tid); + frame.setStartOfTransfer(true); + frame.setEndOfTransfer(true); + EXPECT_EQ(7, frame.setPayload(reinterpret_cast(&usec), 7)); // Assuming little endian!!! + return frame; +} + +static void broadcastSyncMsg(CanIfaceMock& iface, uavcan::uint64_t usec, uavcan::NodeID snid, uavcan::TransferID tid) +{ + const uavcan::Frame frame = makeSyncMsg(usec, snid, tid); + uavcan::CanFrame can_frame; + ASSERT_TRUE(frame.compile(can_frame)); + iface.pushRx(can_frame); +} + + +TEST(GlobalTimeSyncSlave, Validation) +{ + SystemClockMock slave_clock; + slave_clock.monotonic = 1000000; + slave_clock.preserve_utc = true; + + CanDriverMock slave_can(3, slave_clock); + for (uint8_t i = 0; i < slave_can.getNumIfaces(); i++) + { + slave_can.ifaces.at(i).enable_utc_timestamping = true; + } + + TestNode node(slave_can, slave_clock, 64); + + uavcan::GlobalTimeSyncSlave gtss(node); + uavcan::Publisher gts_pub(node); + + ASSERT_LE(0, gtss.start()); + ASSERT_FALSE(gtss.isActive()); + ASSERT_FALSE(gtss.getMasterNodeID().isValid()); + ASSERT_EQ(0, slave_clock.utc); + + /* + * Update/adjust/update + */ + broadcastSyncMsg(slave_can.ifaces.at(0), 0, 8, 0); // Locked on this + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 0); // Ignored + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + broadcastSyncMsg(slave_can.ifaces.at(0), 1000, 8, 1); // Adjust 1000 ahead + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 1); // Ignored + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(8, gtss.getMasterNodeID().get()); + ASSERT_EQ(1000, slave_clock.utc); + + broadcastSyncMsg(slave_can.ifaces.at(0), 2000, 8, 2); // Update + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_EQ(1000, slave_clock.utc); + std::cout << slave_clock.monotonic << std::endl; + + /* + * TID jump simulates a frame loss + */ + broadcastSyncMsg(slave_can.ifaces.at(0), 3000, 8, 4); // Adjustment skipped - expected TID 3, update instead + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(8, gtss.getMasterNodeID().get()); + ASSERT_EQ(1000, slave_clock.utc); + std::cout << slave_clock.monotonic << std::endl; + + /* + * Valid adjustment - continuing from TID 4 + */ + broadcastSyncMsg(slave_can.ifaces.at(0), 3000, 8, 5); // Slave UTC was 1000, master reports 3000 --> shift ahead + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 5); + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(8, gtss.getMasterNodeID().get()); + ASSERT_EQ(3000, slave_clock.utc); + std::cout << slave_clock.monotonic << std::endl; + + /* + * Update, then very long delay with correct TID + */ + broadcastSyncMsg(slave_can.ifaces.at(0), 2000, 8, 6); // Valid update, slave UTC is 3000 + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 6); + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + slave_clock.monotonic += 5000000; + + broadcastSyncMsg(slave_can.ifaces.at(0), 5000, 8, 7); // Adjustment skipped + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 7); + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + broadcastSyncMsg(slave_can.ifaces.at(0), 5000, 8, 8); // Valid adjustment now + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 8); + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(8, gtss.getMasterNodeID().get()); + ASSERT_EQ(5000, slave_clock.utc); + std::cout << slave_clock.monotonic << std::endl; +} + + +TEST(GlobalTimeSyncSlave, Suppression) +{ + SystemClockMock slave_clock; + slave_clock.monotonic = 1000000; + slave_clock.preserve_utc = true; + + CanDriverMock slave_can(3, slave_clock); + for (uint8_t i = 0; i < slave_can.getNumIfaces(); i++) + { + slave_can.ifaces.at(i).enable_utc_timestamping = true; + } + + TestNode node(slave_can, slave_clock, 64); + + uavcan::GlobalTimeSyncSlave gtss(node); + uavcan::Publisher gts_pub(node); + + ASSERT_LE(0, gtss.start()); + ASSERT_EQ(0, slave_clock.utc); + + gtss.suppress(true); + + broadcastSyncMsg(slave_can.ifaces.at(0), 0, 8, 0); // Locked on this + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 0); // Ignored + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + broadcastSyncMsg(slave_can.ifaces.at(0), 1000, 8, 1); // Adjust 1000 ahead + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 1); // Ignored + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(8, gtss.getMasterNodeID().get()); + ASSERT_EQ(0, slave_clock.utc); // The clock shall not be asjusted +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/helpers.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/helpers.hpp new file mode 100644 index 000000000000..e399b483b5d1 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/helpers.hpp @@ -0,0 +1,159 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include "../node/test_node.hpp" + + +template +class SubscriptionCollector : uavcan::Noncopyable +{ + void handler(const DataType& msg) + { + this->msg.reset(new DataType(msg)); + } + +public: + std::unique_ptr msg; + + typedef uavcan::MethodBinder Binder; + + Binder bind() { return Binder(this, &SubscriptionCollector::handler); } +}; + + +template +struct SubscriberWithCollector +{ + typedef SubscriptionCollector Collector; + typedef uavcan::Subscriber Subscriber; + + Collector collector; + Subscriber subscriber; + + SubscriberWithCollector(uavcan::INode& node) + : subscriber(node) + { } + + int start() { return subscriber.start(collector.bind()); } +}; + + +template +class ServiceCallResultCollector : uavcan::Noncopyable +{ + typedef uavcan::ServiceCallResult ServiceCallResult; + +public: + class Result + { + const typename ServiceCallResult::Status status_; + uavcan::ServiceCallID call_id_; + typename DataType::Response response_; + + public: + Result(typename ServiceCallResult::Status arg_status, + uavcan::ServiceCallID arg_call_id, + const typename DataType::Response& arg_response) + : status_(arg_status) + , call_id_(arg_call_id) + , response_(arg_response) + { } + + bool isSuccessful() const { return status_ == ServiceCallResult::Success; } + + typename ServiceCallResult::Status getStatus() const { return status_; } + + uavcan::ServiceCallID getCallID() const { return call_id_; } + + const typename DataType::Response& getResponse() const { return response_; } + typename DataType::Response& getResponse() { return response_; } + }; + +private: + void handler(const uavcan::ServiceCallResult& tmp_result) + { + std::cout << tmp_result << std::endl; + result.reset(new Result(tmp_result.getStatus(), tmp_result.getCallID(), tmp_result.getResponse())); + } + +public: + std::unique_ptr result; + + typedef uavcan::MethodBinder&)> + Binder; + + Binder bind() { return Binder(this, &ServiceCallResultCollector::handler); } +}; + + +template +struct ServiceClientWithCollector +{ + typedef ServiceCallResultCollector Collector; + typedef uavcan::ServiceClient ServiceClient; + + Collector collector; + ServiceClient client; + + ServiceClientWithCollector(uavcan::INode& node) + : client(node) + { } + + int call(uavcan::NodeID node_id, const typename DataType::Request& request) + { + client.setCallback(collector.bind()); + return client.call(node_id, request); + } +}; + + +struct BackgroundSpinner : uavcan::TimerBase +{ + uavcan::INode& spinning_node; + + BackgroundSpinner(uavcan::INode& spinning_node, uavcan::INode& running_node) + : uavcan::TimerBase(running_node) + , spinning_node(spinning_node) + { } + + virtual void handleTimerEvent(const uavcan::TimerEvent&) + { + spinning_node.spin(uavcan::MonotonicDuration::fromMSec(1)); + } +}; + +template +static inline void emulateSingleFrameBroadcastTransfer(CanDriver& can, uavcan::NodeID node_id, + const MessageType& message, uavcan::TransferID tid) +{ + uavcan::StaticTransferBuffer<100> buffer; + uavcan::BitStream bitstream(buffer); + uavcan::ScalarCodec codec(bitstream); + + // Manual message publication + ASSERT_LT(0, MessageType::encode(message, codec)); + ASSERT_GE(8, buffer.getMaxWritePos()); + + // DataTypeID data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame + uavcan::Frame frame(MessageType::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + node_id, uavcan::NodeID::Broadcast, tid); + frame.setStartOfTransfer(true); + frame.setEndOfTransfer(true); + + ASSERT_EQ(buffer.getMaxWritePos(), frame.setPayload(buffer.getRawPtr(), buffer.getMaxWritePos())); + + uavcan::CanFrame can_frame; + ASSERT_TRUE(frame.compile(can_frame)); + + can.pushRxToAllIfaces(can_frame); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/logger.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/logger.cpp new file mode 100644 index 000000000000..1d38cec64be8 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/logger.cpp @@ -0,0 +1,144 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +struct LogSink : public uavcan::ILogSink +{ + std::queue msgs; + LogLevel level; + + LogSink() + : level(uavcan::protocol::debug::LogLevel::ERROR) + { } + + LogLevel getLogLevel() const { return level; } + + void log(const uavcan::protocol::debug::LogMessage& message) + { + msgs.push(message); + std::cout << message << std::endl; + } + + uavcan::protocol::debug::LogMessage pop() + { + if (msgs.empty()) + { + std::cout << "LogSink is empty" << std::endl; + std::abort(); + } + const uavcan::protocol::debug::LogMessage m = msgs.front(); + msgs.pop(); + return m; + } + + bool popMatchByLevelAndText(int level, const std::string& source, const std::string& text) + { + if (msgs.empty()) + { + std::cout << "LogSink is empty" << std::endl; + return false; + } + const uavcan::protocol::debug::LogMessage m = pop(); + return + level == m.level.value && + source == m.source && + text == m.text; + } +}; + + +TEST(Logger, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::Logger logger(nodes.a); + + ASSERT_EQ(uavcan::protocol::debug::LogLevel::ERROR, logger.getLevel()); + + LogSink sink; + + // Will fail - types are not registered + uavcan::GlobalDataTypeRegistry::instance().reset(); + ASSERT_GT(0, logger.logError("foo", "Error (fail - type is not registered)")); + ASSERT_EQ(0, logger.logDebug("foo", "Debug (ignored - low logging level)")); + + ASSERT_FALSE(logger.getExternalSink()); + logger.setExternalSink(&sink); + ASSERT_EQ(&sink, logger.getExternalSink()); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + SubscriberWithCollector log_sub(nodes.b); + ASSERT_LE(0, log_sub.start()); + + // Sink test + ASSERT_EQ(0, logger.logDebug("foo", "Debug (ignored due to low logging level)")); + ASSERT_TRUE(sink.msgs.empty()); + + sink.level = uavcan::protocol::debug::LogLevel::DEBUG; + ASSERT_EQ(0, logger.logDebug("foo", "Debug (sink only)")); + ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::DEBUG, "foo", "Debug (sink only)")); + + ASSERT_LE(0, logger.logError("foo", "Error")); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_TRUE(log_sub.collector.msg.get()); + ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::ERROR); + ASSERT_EQ(log_sub.collector.msg->source, "foo"); + ASSERT_EQ(log_sub.collector.msg->text, "Error"); + + logger.setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + ASSERT_LE(0, logger.logWarning("foo", "Warning")); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::WARNING); + ASSERT_EQ(log_sub.collector.msg->source, "foo"); + ASSERT_EQ(log_sub.collector.msg->text, "Warning"); + + ASSERT_LE(0, logger.logInfo("foo", "Info")); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::INFO); + ASSERT_EQ(log_sub.collector.msg->source, "foo"); + ASSERT_EQ(log_sub.collector.msg->text, "Info"); + + ASSERT_LE(0, logger.logDebug("foo", "Debug")); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::DEBUG); + ASSERT_EQ(log_sub.collector.msg->source, "foo"); + ASSERT_EQ(log_sub.collector.msg->text, "Debug"); + + ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::ERROR, "foo", "Error")); + ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::WARNING, "foo", "Warning")); + ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::INFO, "foo", "Info")); + ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::DEBUG, "foo", "Debug")); +} + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +TEST(Logger, Cpp11Formatting) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::Logger logger(nodes.a); + logger.setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + + SubscriberWithCollector log_sub(nodes.b); + ASSERT_LE(0, log_sub.start()); + + ASSERT_LE(0, logger.logWarning("foo", "char='%*', %* is %*", '$', "double", 12.34)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_TRUE(log_sub.collector.msg.get()); + ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::WARNING); + ASSERT_EQ(log_sub.collector.msg->source, "foo"); + ASSERT_EQ(log_sub.collector.msg->text, "char='$', double is 12.34"); +} + +#endif diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_info_retriever.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_info_retriever.cpp new file mode 100644 index 000000000000..f0f1b5a9b456 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_info_retriever.cpp @@ -0,0 +1,291 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#include +#include +#include +#include +#include "helpers.hpp" + +static void publishNodeStatus(PairableCanDriver& can, uavcan::NodeID node_id, + uavcan::uint32_t uptime_sec, uavcan::TransferID tid) +{ + uavcan::protocol::NodeStatus msg; + msg.health = uavcan::protocol::NodeStatus::HEALTH_OK; + msg.mode = uavcan::protocol::NodeStatus::MODE_OPERATIONAL; + msg.uptime_sec = uptime_sec; + emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid); +} + + +struct NodeInfoListener : public uavcan::INodeInfoListener +{ + std::unique_ptr last_node_info; + uavcan::NodeID last_node_id; + unsigned status_message_cnt; + unsigned status_change_cnt; + unsigned info_unavailable_cnt; + + NodeInfoListener() + : status_message_cnt(0) + , status_change_cnt(0) + , info_unavailable_cnt(0) + { } + + virtual void handleNodeInfoRetrieved(uavcan::NodeID node_id, + const uavcan::protocol::GetNodeInfo::Response& node_info) + { + last_node_id = node_id; + std::cout << node_info << std::endl; + last_node_info.reset(new uavcan::protocol::GetNodeInfo::Response(node_info)); + } + + virtual void handleNodeInfoUnavailable(uavcan::NodeID node_id) + { + std::cout << "NODE INFO FOR " << int(node_id.get()) << " IS UNAVAILABLE" << std::endl; + last_node_id = node_id; + info_unavailable_cnt++; + } + + virtual void handleNodeStatusChange(const uavcan::NodeStatusMonitor::NodeStatusChangeEvent& event) + { + std::cout << "NODE " << int(event.node_id.get()) << " STATUS CHANGE: " + << event.old_status.toString() << " --> " << event.status.toString() << std::endl; + status_change_cnt++; + } + + virtual void handleNodeStatusMessage(const uavcan::ReceivedDataStructure& msg) + { + std::cout << msg << std::endl; + status_message_cnt++; + } +}; + + +TEST(NodeInfoRetriever, Basic) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + InterlinkedTestNodesWithSysClock nodes; + + uavcan::NodeInfoRetriever retr(nodes.a); + std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl; + std::cout << "sizeof(uavcan::ServiceClient): " + << sizeof(uavcan::ServiceClient) << std::endl; + + std::unique_ptr provider(new uavcan::NodeStatusProvider(nodes.b)); + + NodeInfoListener listener; + + /* + * Initialization + */ + ASSERT_LE(0, retr.start()); + + retr.removeListener(&listener); // Does nothing + retr.addListener(&listener); + retr.addListener(&listener); + retr.addListener(&listener); + ASSERT_EQ(1, retr.getNumListeners()); + + uavcan::protocol::HardwareVersion hwver; + hwver.unique_id[0] = 123; + hwver.unique_id[4] = 213; + hwver.unique_id[8] = 45; + + provider->setName("Ivan"); + provider->setHardwareVersion(hwver); + + ASSERT_LE(0, provider->startAndPublish()); + + ASSERT_FALSE(retr.isRetrievingInProgress()); + ASSERT_EQ(0, retr.getNumPendingRequests()); + + EXPECT_EQ(40, retr.getRequestInterval().toMSec()); // Default + + /* + * Waiting for discovery + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500)); + ASSERT_FALSE(retr.isRetrievingInProgress()); + + ASSERT_EQ(2, listener.status_message_cnt); + ASSERT_EQ(1, listener.status_change_cnt); + ASSERT_EQ(0, listener.info_unavailable_cnt); + ASSERT_TRUE(listener.last_node_info.get()); + ASSERT_EQ(uavcan::NodeID(2), listener.last_node_id); + ASSERT_EQ("Ivan", listener.last_node_info->name); + ASSERT_TRUE(hwver == listener.last_node_info->hardware_version); + + provider.reset(); // Moving the provider out of the way; its entry will timeout meanwhile + + /* + * Declaring a bunch of different nodes that don't support GetNodeInfo + */ + ASSERT_FALSE(retr.isRetrievingInProgress()); + + retr.setNumRequestAttempts(3); + + uavcan::TransferID tid; + + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 10, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 10, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 10, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(3, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 11, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 11, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 11, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(3, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 12, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 12, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 10, tid); // Reset + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(3, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + EXPECT_EQ(11, listener.status_message_cnt); + EXPECT_EQ(5, listener.status_change_cnt); // node 2 online/offline + 3 test nodes above + EXPECT_EQ(2, listener.info_unavailable_cnt); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 11, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(1, retr.getNumPendingRequests()); // Still one because two went offline + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1200)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 12, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1200)); + ASSERT_FALSE(retr.isRetrievingInProgress()); // Out of attempts, stopping + ASSERT_EQ(0, retr.getNumPendingRequests()); + + EXPECT_EQ(13, listener.status_message_cnt); + EXPECT_EQ(7, listener.status_change_cnt); // node 2 online/offline + 2 test nodes above online/offline + 1 + EXPECT_EQ(3, listener.info_unavailable_cnt); + + /* + * Forcing the class to forget everything + */ + std::cout << "Invalidation" << std::endl; + + retr.invalidateAll(); + + ASSERT_FALSE(retr.isRetrievingInProgress()); + ASSERT_EQ(0, retr.getNumPendingRequests()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 60, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 60, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 60, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200)); + + ASSERT_TRUE(retr.isRetrievingInProgress()); + ASSERT_EQ(3, retr.getNumPendingRequests()); +} + + +TEST(NodeInfoRetriever, MaxConcurrentRequests) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + InterlinkedTestNodesWithSysClock nodes; + + uavcan::NodeInfoRetriever retr(nodes.a); + std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl; + std::cout << "sizeof(uavcan::ServiceClient): " + << sizeof(uavcan::ServiceClient) << std::endl; + + NodeInfoListener listener; + + /* + * Initialization + */ + ASSERT_LE(0, retr.start()); + + retr.addListener(&listener); + ASSERT_EQ(1, retr.getNumListeners()); + + ASSERT_FALSE(retr.isRetrievingInProgress()); + ASSERT_EQ(0, retr.getNumPendingRequests()); + + ASSERT_EQ(40, retr.getRequestInterval().toMSec()); + + const unsigned MaxPendingRequests = 26; // See class docs + const unsigned MinPendingRequestsAtFullLoad = 12; + + /* + * Sending a lot of requests, making sure that the number of concurrent calls does not exceed the specified limit. + */ + for (uint8_t node_id = 1U; node_id <= 127U; node_id++) + { + publishNodeStatus(nodes.can_a, node_id, 0, uavcan::TransferID()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests()); + ASSERT_TRUE(retr.isRetrievingInProgress()); + } + + ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests()); + ASSERT_LE(MinPendingRequestsAtFullLoad, retr.getNumPendingRequests()); + + for (int i = 0; i < 8; i++) // Approximate + { + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(35)); + std::cout << "!!! SPIN " << i << " COMPLETE" << std::endl; + + ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests()); + ASSERT_LE(MinPendingRequestsAtFullLoad, retr.getNumPendingRequests()); + + ASSERT_TRUE(retr.isRetrievingInProgress()); + } + + ASSERT_LT(0, retr.getNumPendingRequests()); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000)); + + ASSERT_EQ(0, retr.getNumPendingRequests()); + ASSERT_FALSE(retr.isRetrievingInProgress()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_status_monitor.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_status_monitor.cpp new file mode 100644 index 000000000000..cbe9193cb317 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_status_monitor.cpp @@ -0,0 +1,176 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + +static void publishNodeStatus(CanDriverMock& can, uavcan::NodeID node_id, + uavcan::uint8_t health, uavcan::uint8_t mode, + uavcan::uint32_t uptime_sec, uavcan::TransferID tid) +{ + uavcan::protocol::NodeStatus msg; + msg.health = health; + msg.mode = mode; + msg.uptime_sec = uptime_sec; + emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid); +} + + +static void shortSpin(TestNode& node) +{ + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); +} + + +TEST(NodeStatusMonitor, Basic) +{ + using uavcan::protocol::NodeStatus; + using uavcan::NodeID; + + SystemClockMock clock_mock(100); + clock_mock.monotonic_auto_advance = 1000; + + CanDriverMock can(2, clock_mock); + + TestNode node(can, clock_mock, 64); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + uavcan::NodeStatusMonitor nsm(node); + ASSERT_LE(0, nsm.start()); + + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + /* + * Empty NSM, no nodes were registered yet + */ + ASSERT_FALSE(nsm.findNodeWithWorstHealth().isValid()); + + ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(123))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(123)).mode); + + /* + * Some new status messages + */ + publishNodeStatus(can, 10, NodeStatus::HEALTH_OK, NodeStatus::MODE_OPERATIONAL, 12, 0); + shortSpin(node); + ASSERT_EQ(NodeID(10), nsm.findNodeWithWorstHealth()); + + publishNodeStatus(can, 9, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_INITIALIZATION, 0, 0); + shortSpin(node); + ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstHealth()); + + publishNodeStatus(can, 11, NodeStatus::HEALTH_CRITICAL, NodeStatus::MODE_MAINTENANCE, 999, 0); + shortSpin(node); + ASSERT_EQ(NodeID(11), nsm.findNodeWithWorstHealth()); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(10)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_INITIALIZATION, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11))); + ASSERT_EQ(NodeStatus::MODE_MAINTENANCE, nsm.getNodeStatus(uavcan::NodeID(11)).mode); + ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health); + + /* + * Timeout + */ + std::cout << "Starting timeout test, current monotime is " << clock_mock.monotonic << std::endl; + + clock_mock.advance(500000); + shortSpin(node); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(10)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health); + + clock_mock.advance(500000); + shortSpin(node); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_INITIALIZATION, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health); + + clock_mock.advance(500000); + shortSpin(node); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11))); + ASSERT_EQ(NodeStatus::MODE_MAINTENANCE, nsm.getNodeStatus(uavcan::NodeID(11)).mode); + ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health); + + /* + * Will timeout now + */ + clock_mock.advance(4000000); + shortSpin(node); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(10)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(11)).mode); + ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health); + + /* + * Recovering one node, adding two extra + */ + publishNodeStatus(can, 11, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_OPERATIONAL, 999, 1); + shortSpin(node); + + publishNodeStatus(can, 127, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_OPERATIONAL, 9999, 1); + shortSpin(node); + + publishNodeStatus(can, 1, NodeStatus::HEALTH_OK, NodeStatus::MODE_OPERATIONAL, 1234, 1); + shortSpin(node); + + /* + * Making sure OFFLINE is still worst status + */ + ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstHealth()); + + /* + * Final validation + */ + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(10)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(11)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(11)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(127))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(127)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(127)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(1))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(1)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(1)).health); + + /* + * Forgetting + */ + nsm.forgetNode(127); + ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(127))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(127)).mode); + ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(127)).health); + + nsm.forgetNode(9); + ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(9)).health); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_status_provider.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_status_provider.cpp new file mode 100644 index 000000000000..2d57db3959b5 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_status_provider.cpp @@ -0,0 +1,150 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +struct AdHocNodeStatusUpdater : public uavcan::IAdHocNodeStatusUpdater +{ + uavcan::uint64_t invokations; + + AdHocNodeStatusUpdater() : invokations(0) { } + + virtual void updateNodeStatus() + { + invokations++; + } +}; + + +TEST(NodeStatusProvider, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::NodeStatusProvider nsp(nodes.a); + + /* + * Initialization + */ + uavcan::protocol::HardwareVersion hwver; + hwver.major = 3; + hwver.minor = 14; + + uavcan::protocol::SoftwareVersion swver; + swver.major = 2; + swver.minor = 18; + swver.vcs_commit = 0x600DF00D; + + nsp.setHardwareVersion(hwver); + nsp.setSoftwareVersion(swver); + + ASSERT_TRUE(nsp.getName().empty()); + nsp.setName("superluminal_communication_unit"); + ASSERT_STREQ("superluminal_communication_unit", nsp.getName().c_str()); + + ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_OK, nsp.getHealth()); + ASSERT_EQ(uavcan::protocol::NodeStatus::MODE_INITIALIZATION, nsp.getMode()); + nsp.setHealthError(); + nsp.setModeOperational(); + ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_ERROR, nsp.getHealth()); + ASSERT_EQ(uavcan::protocol::NodeStatus::MODE_OPERATIONAL, nsp.getMode()); + + // Will fail - types are not registered + uavcan::GlobalDataTypeRegistry::instance().reset(); + ASSERT_GT(0, nsp.startAndPublish()); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + ASSERT_LE(0, nsp.startAndPublish()); + + // Checking the publishing rate settings + ASSERT_EQ(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::NodeStatus::MAX_BROADCASTING_PERIOD_MS), + nsp.getStatusPublicationPeriod()); + + nsp.setStatusPublicationPeriod(uavcan::MonotonicDuration()); + ASSERT_EQ(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::NodeStatus::MIN_BROADCASTING_PERIOD_MS), + nsp.getStatusPublicationPeriod()); + + nsp.setStatusPublicationPeriod(uavcan::MonotonicDuration::fromMSec(3600 * 1000 * 24)); + ASSERT_EQ(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::NodeStatus::MAX_BROADCASTING_PERIOD_MS), + nsp.getStatusPublicationPeriod()); + + AdHocNodeStatusUpdater ad_hoc; + ASSERT_EQ(UAVCAN_NULLPTR, nsp.getAdHocNodeStatusUpdater()); + nsp.setAdHocNodeStatusUpdater(&ad_hoc); + ASSERT_EQ(&ad_hoc, nsp.getAdHocNodeStatusUpdater()); + + /* + * Initial status publication + */ + SubscriberWithCollector status_sub(nodes.b); + + ASSERT_LE(0, status_sub.start()); + ASSERT_FALSE(status_sub.collector.msg.get()); // No data yet + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(status_sub.collector.msg.get()); // Was published at startup + ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_ERROR, status_sub.collector.msg->health); + ASSERT_EQ(0, status_sub.collector.msg->vendor_specific_status_code); + ASSERT_GE(1, status_sub.collector.msg->uptime_sec); + + ASSERT_EQ(0, ad_hoc.invokations); // Not invoked from startAndPublish() + + /* + * Altering the vendor-specific status code, forcePublish()-ing it and checking the result + */ + ASSERT_EQ(0, nsp.getVendorSpecificStatusCode()); + nsp.setVendorSpecificStatusCode(1234); + ASSERT_EQ(1234, nsp.getVendorSpecificStatusCode()); + + ASSERT_LE(0, nsp.forcePublish()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_ERROR, status_sub.collector.msg->health); + ASSERT_EQ(1234, status_sub.collector.msg->vendor_specific_status_code); + ASSERT_GE(1, status_sub.collector.msg->uptime_sec); + + ASSERT_EQ(0, ad_hoc.invokations); // Not invoked from forcePublish() + + /* + * Explicit node info request + */ + ServiceClientWithCollector gni_cln(nodes.b); + + nsp.setHealthCritical(); + + ASSERT_FALSE(gni_cln.collector.result.get()); // No data yet + ASSERT_LE(0, gni_cln.call(1, uavcan::protocol::GetNodeInfo::Request())); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_TRUE(gni_cln.collector.result.get()); // Response must have been delivered + + ASSERT_TRUE(gni_cln.collector.result->isSuccessful()); + ASSERT_EQ(1, gni_cln.collector.result->getCallID().server_node_id.get()); + + ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_CRITICAL, + gni_cln.collector.result->getResponse().status.health); + + ASSERT_TRUE(hwver == gni_cln.collector.result->getResponse().hardware_version); + ASSERT_TRUE(swver == gni_cln.collector.result->getResponse().software_version); + + ASSERT_EQ("superluminal_communication_unit", gni_cln.collector.result->getResponse().name); + + ASSERT_EQ(0, ad_hoc.invokations); // No timer-triggered publications happened yet + + /* + * Timer triggered publication + */ + EXPECT_EQ(3, nodes.a.getDispatcher().getTransferPerfCounter().getTxTransferCount()); + + nodes.spinBoth(nsp.getStatusPublicationPeriod()); + + EXPECT_EQ(1, ad_hoc.invokations); // No timer-triggered publications happened yet + EXPECT_EQ(4, nodes.a.getDispatcher().getTransferPerfCounter().getTxTransferCount()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/panic_broadcaster.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/panic_broadcaster.cpp new file mode 100644 index 000000000000..edf9ba09e83d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/panic_broadcaster.cpp @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +TEST(PanicBroadcaster, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::PanicBroadcaster panicker(nodes.a); + + SubscriberWithCollector sub(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + ASSERT_LE(0, sub.start()); + + panicker.panic("I lost my towel!"); // Only the first 7 chars allowed + + ASSERT_STREQ("I lost ", panicker.getReason().c_str()); + ASSERT_TRUE(panicker.isPanicking()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_STREQ("I lost ", sub.collector.msg->reason_text.c_str()); + sub.collector.msg.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(300)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_STREQ("I lost ", sub.collector.msg->reason_text.c_str()); + sub.collector.msg.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(300)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_STREQ("I lost ", sub.collector.msg->reason_text.c_str()); + sub.collector.msg.reset(); + + panicker.dontPanic(); + ASSERT_FALSE(panicker.isPanicking()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(300)); + ASSERT_FALSE(sub.collector.msg.get()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/panic_listener.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/panic_listener.cpp new file mode 100644 index 000000000000..0f30bfb701b7 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/panic_listener.cpp @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + + +struct PanicHandler +{ + uavcan::protocol::Panic msg; + + PanicHandler() : msg() { } + + void handle(const uavcan::protocol::Panic& msg) + { + std::cout << msg << std::endl; + this->msg = msg; + } + + typedef uavcan::MethodBinder Binder; + + Binder bind() { return Binder(this, &PanicHandler::handle); } +}; + + +TEST(PanicListener, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + uavcan::PanicListener pl(nodes.a); + uavcan::PanicBroadcaster pbr(nodes.b); + PanicHandler handler; + ASSERT_LE(0, pl.start(handler.bind())); + + pbr.panic("PANIC!!!"); + ASSERT_TRUE(handler.msg == uavcan::protocol::Panic()); // One message published, panic is not registered yet + + pbr.dontPanic(); + ASSERT_FALSE(pbr.isPanicking()); + std::cout << "Not panicking" << std::endl; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); // Will reset + ASSERT_TRUE(handler.msg == uavcan::protocol::Panic()); + + pbr.panic("PANIC2!!!"); // Message text doesn't matter + ASSERT_TRUE(pbr.isPanicking()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_STREQ("PANIC2!", handler.msg.reason_text.c_str()); // Registered, only 7 chars preserved +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/param_server.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/param_server.cpp new file mode 100644 index 000000000000..94abbfafa214 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/param_server.cpp @@ -0,0 +1,177 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + +struct ParamServerTestManager : public uavcan::IParamManager +{ + typedef std::map KeyValue; + KeyValue kv; + + virtual void getParamNameByIndex(Index index, Name& out_name) const + { + Index current_idx = 0; + for (KeyValue::const_iterator it = kv.begin(); it != kv.end(); ++it, ++current_idx) + { + if (current_idx == index) + { + out_name = it->first.c_str(); + break; + } + } + } + + virtual void assignParamValue(const Name& name, const Value& value) + { + assert(!name.empty()); + std::cout << "ASSIGN [" << name.c_str() << "]\n" << value << "\n---" << std::endl; + KeyValue::iterator it = kv.find(name.c_str()); + if (it != kv.end()) + { + if (value.is(Value::Tag::boolean_value)) + { + assert(value.getTag() == Value::Tag::boolean_value); + it->second = double(value.boolean_value); + } + else if (value.is(Value::Tag::integer_value)) + { + assert(value.getTag() == Value::Tag::integer_value); + it->second = double(value.integer_value); + } + else if (value.is(Value::Tag::real_value)) + { + assert(value.getTag() == Value::Tag::real_value); + it->second = double(value.real_value); + } + else if (value.is(Value::Tag::string_value)) + { + assert(value.getTag() == Value::Tag::string_value); + it->second = std::atof(value.string_value.c_str()); + } + else + { + assert(0); + } + } + } + + virtual void readParamValue(const Name& name, Value& out_value) const + { + assert(!name.empty()); + KeyValue::const_iterator it = kv.find(name.c_str()); + if (it != kv.end()) + { + out_value.to() = float(it->second); + assert(out_value.getTag() == Value::Tag::real_value); + } + std::cout << "READ [" << name.c_str() << "]\n" << out_value << "\n---" << std::endl; + } + + virtual int saveAllParams() + { + std::cout << "SAVE" << std::endl; + return 0; + } + + virtual int eraseAllParams() + { + std::cout << "ERASE" << std::endl; + return 0; + } +}; + + +template +static void doCall(Client& client, const Message& request, InterlinkedTestNodesWithSysClock& nodes) +{ + ASSERT_LE(0, client.call(1, request)); + ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10))); + ASSERT_TRUE(client.collector.result.get()); + ASSERT_TRUE(client.collector.result->isSuccessful()); +} + + +TEST(ParamServer, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::ParamServer server(nodes.a); + + ParamServerTestManager mgr; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + ASSERT_LE(0, server.start(&mgr)); + + ServiceClientWithCollector get_set_cln(nodes.b); + ServiceClientWithCollector save_erase_cln(nodes.b); + + /* + * Save/erase + */ + uavcan::protocol::param::ExecuteOpcode::Request save_erase_rq; + save_erase_rq.opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE; + doCall(save_erase_cln, save_erase_rq, nodes); + ASSERT_TRUE(save_erase_cln.collector.result.get()); + ASSERT_TRUE(save_erase_cln.collector.result->getResponse().ok); + + save_erase_rq.opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE; + doCall(save_erase_cln, save_erase_rq, nodes); + ASSERT_TRUE(save_erase_cln.collector.result->getResponse().ok); + + // Invalid opcode + save_erase_rq.opcode = 0xFF; + doCall(save_erase_cln, save_erase_rq, nodes); + ASSERT_FALSE(save_erase_cln.collector.result->getResponse().ok); + + /* + * Get/set + */ + uavcan::protocol::param::GetSet::Request get_set_rq; + get_set_rq.name = "nonexistent_parameter"; + doCall(get_set_cln, get_set_rq, nodes); + ASSERT_TRUE(get_set_cln.collector.result.get()); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().name.empty()); + + // No such variable, shall return empty name/value + get_set_rq.index = 0; + get_set_rq.name.clear(); + get_set_rq.value.to() = 0xDEADBEEF; + doCall(get_set_cln, get_set_rq, nodes); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().name.empty()); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.is(uavcan::protocol::param::Value::Tag::empty)); + + mgr.kv["foobar"] = 123.456; // New param + + // Get by name + get_set_rq = uavcan::protocol::param::GetSet::Request(); + get_set_rq.name = "foobar"; + doCall(get_set_cln, get_set_rq, nodes); + ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str()); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.is(uavcan::protocol::param::Value::Tag::real_value)); + ASSERT_FLOAT_EQ(123.456F, get_set_cln.collector.result->getResponse().value. + to()); + + // Set by index + get_set_rq = uavcan::protocol::param::GetSet::Request(); + get_set_rq.index = 0; + get_set_rq.value.to() = "424242"; + doCall(get_set_cln, get_set_rq, nodes); + ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str()); + ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->getResponse().value. + to()); + + // Get by index + get_set_rq = uavcan::protocol::param::GetSet::Request(); + get_set_rq.index = 0; + doCall(get_set_cln, get_set_rq, nodes); + ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str()); + ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->getResponse().value. + to()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/restart_request_server.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/restart_request_server.cpp new file mode 100644 index 000000000000..8229ed8ce659 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/restart_request_server.cpp @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +struct RestartHandler : public uavcan::IRestartRequestHandler +{ + bool accept; + + bool handleRestartRequest(uavcan::NodeID request_source) + { + std::cout << "Restart request from " << int(request_source.get()) << " will be " + << (accept ? "accepted" : "rejected") << std::endl; + return accept; + } +}; + + +TEST(RestartRequestServer, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::RestartRequestServer rrs(nodes.a); + + ServiceClientWithCollector rrs_cln(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + ASSERT_LE(0, rrs.start()); + + uavcan::protocol::RestartNode::Request request; + request.magic_number = uavcan::protocol::RestartNode::Request::MAGIC_NUMBER; + + /* + * Rejected - handler was not set + */ + ASSERT_LE(0, rrs_cln.call(1, request)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(rrs_cln.collector.result.get()); + ASSERT_TRUE(rrs_cln.collector.result->isSuccessful()); + ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok); + + /* + * Accepted + */ + RestartHandler handler; + handler.accept = true; + rrs.setHandler(&handler); + + ASSERT_LE(0, rrs_cln.call(1, request)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(rrs_cln.collector.result->isSuccessful()); + ASSERT_TRUE(rrs_cln.collector.result->getResponse().ok); + + /* + * Rejected by handler + */ + handler.accept = false; + + ASSERT_LE(0, rrs_cln.call(1, request)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(rrs_cln.collector.result->isSuccessful()); + ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok); + + /* + * Rejected because of invalid magic number + */ + handler.accept = true; + + ASSERT_LE(0, rrs_cln.call(1, uavcan::protocol::RestartNode::Request())); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(rrs_cln.collector.result->isSuccessful()); + ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/transport_stats_provider.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/transport_stats_provider.cpp new file mode 100644 index 000000000000..dc107b350151 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/transport_stats_provider.cpp @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +TEST(TransportStatsProvider, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::TransportStatsProvider tsp(nodes.a); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + ASSERT_LE(0, tsp.start()); + + ServiceClientWithCollector tsp_cln(nodes.b); + + /* + * First request + */ + ASSERT_LE(0, tsp_cln.call(1, uavcan::protocol::GetTransportStats::Request())); + ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(tsp_cln.collector.result.get()); + ASSERT_TRUE(tsp_cln.collector.result->isSuccessful()); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfer_errors); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().transfers_rx); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfers_tx); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size()); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx); + + /* + * Second request + */ + ASSERT_LE(0, tsp_cln.call(1, uavcan::protocol::GetTransportStats::Request())); + ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(tsp_cln.collector.result.get()); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfer_errors); + ASSERT_EQ(2, tsp_cln.collector.result->getResponse().transfers_rx); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().transfers_tx); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size()); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors); + ASSERT_EQ(2, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx); + ASSERT_EQ(6, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx); + + /* + * Sending a malformed frame, it must be registered as tranfer error + */ + uavcan::Frame frame(uavcan::protocol::GetTransportStats::DefaultDataTypeID, uavcan::TransferTypeServiceRequest, + 2, 1, 1); + frame.setStartOfTransfer(true); + frame.setEndOfTransfer(true); + uavcan::CanFrame can_frame; + ASSERT_TRUE(frame.compile(can_frame)); + nodes.can_a.read_queue.push(can_frame); + + ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10))); + + /* + * Introducing a CAN driver error + */ + nodes.can_a.error_count = 72; + + /* + * Third request + */ + ASSERT_LE(0, tsp_cln.call(1, uavcan::protocol::GetTransportStats::Request())); + ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(tsp_cln.collector.result.get()); + EXPECT_EQ(1, tsp_cln.collector.result->getResponse().transfer_errors); // That broken frame + EXPECT_EQ(3, tsp_cln.collector.result->getResponse().transfers_rx); + EXPECT_EQ(2, tsp_cln.collector.result->getResponse().transfers_tx); + EXPECT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size()); + EXPECT_EQ(72, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors); + EXPECT_EQ(4, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx); // Same here + EXPECT_EQ(12, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/test_main.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/test_main.cpp new file mode 100644 index 000000000000..37afc97aaebf --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/test_main.cpp @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include +#include + +static void sigsegv_handler(int sig) +{ + const int BacktraceSize = 32; + void* array[BacktraceSize]; + const int size = backtrace(array, BacktraceSize); + + std::fprintf(stderr, "SIGNAL %d RECEIVED; STACKTRACE:\n", sig); + backtrace_symbols_fd(array, size, STDERR_FILENO); + std::exit(1); +} + +int main(int argc, char **argv) +{ + signal(SIGSEGV, sigsegv_handler); + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif +#if UAVCAN_CPP_VERSION == UAVCAN_CPP11 + std::cout << "C++11" << std::endl; +#elif UAVCAN_CPP_VERSION == UAVCAN_CPP03 + std::cout << "C++03" << std::endl; +#else +# error UAVCAN_CPP_VERSION +#endif + + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/time.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/time.cpp new file mode 100644 index 000000000000..18b3facd8068 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/time.cpp @@ -0,0 +1,107 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + + +TEST(Time, Monotonic) +{ + using uavcan::MonotonicDuration; + using uavcan::MonotonicTime; + + MonotonicTime m1; + MonotonicTime m2 = MonotonicTime::fromMSec(1); + MonotonicDuration md1 = m2 - m1; // 1000 + MonotonicDuration md2 = m1 - m2; // -1000 + + ASSERT_EQ(0, m1.toUSec()); + ASSERT_EQ(1000, m2.toUSec()); + ASSERT_EQ(1000, md1.toUSec()); + ASSERT_EQ(-1000, md2.toUSec()); + + ASSERT_LT(m1, m2); + ASSERT_LE(m1, m2); + ASSERT_NE(m1, m2); + ASSERT_TRUE(m1.isZero()); + ASSERT_FALSE(m2.isZero()); + + ASSERT_GT(md1, md2); + ASSERT_GE(md1, md2); + ASSERT_NE(md1, md2); + ASSERT_FALSE(md1.isZero()); + ASSERT_TRUE(md1.isPositive()); + ASSERT_TRUE(md2.isNegative()); + + ASSERT_EQ(0, (md1 + md2).toUSec()); + ASSERT_EQ(2000, (md1 - md2).toUSec()); + + md1 *= 2; // 2000 + ASSERT_EQ(2000, md1.toUSec()); + + md2 += md1; // md2 = -1000 + 2000 + ASSERT_EQ(1000, md2.toUSec()); + + ASSERT_EQ(-1000, (-md2).toUSec()); + + /* + * To string + */ + ASSERT_EQ("0.000000", m1.toString()); + ASSERT_EQ("0.001000", m2.toString()); + + ASSERT_EQ("0.002000", md1.toString()); + ASSERT_EQ("-0.001000", (-md2).toString()); + + ASSERT_EQ("1001.000001", MonotonicTime::fromUSec(1001000001).toString()); + ASSERT_EQ("-1001.000001", MonotonicDuration::fromUSec(-1001000001).toString()); +} + + +TEST(Time, Utc) +{ + using uavcan::UtcDuration; + using uavcan::UtcTime; + using uavcan::Timestamp; + + Timestamp ts; + ts.usec = 9000; + + UtcTime u1(ts); + ASSERT_EQ(9000, u1.toUSec()); + + ts.usec *= 2; + u1 = ts; + ASSERT_EQ(18000, u1.toUSec()); + + ts = UtcTime::fromUSec(12345678900); + ASSERT_EQ(12345678900, ts.usec); + + /* + * To string + */ + ASSERT_EQ("0.018000", u1.toString()); + ASSERT_EQ("12345.678900", UtcTime(ts).toString()); +} + + +TEST(Time, Overflow) +{ + using uavcan::MonotonicDuration; + using uavcan::MonotonicTime; + + MonotonicTime max_4 = MonotonicTime::fromUSec(MonotonicTime::getMax().toUSec() / 4); + MonotonicDuration max_4_duration = max_4 - MonotonicTime(); + + std::cout << max_4 << std::endl; + ASSERT_EQ(max_4_duration.toUSec(), max_4.toUSec()); + + MonotonicTime max = (((max_4 + max_4_duration) + max_4_duration) + max_4_duration) + max_4_duration; + ASSERT_EQ(max, MonotonicTime::getMax()); // Must not overflow + + MonotonicTime min; + min -= max_4_duration; + ASSERT_EQ(min, MonotonicTime()); // Must not underflow +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/can.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/can.hpp new file mode 100644 index 000000000000..41563a1ac846 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/can.hpp @@ -0,0 +1,282 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include "../../clock.hpp" + + +class CanIfaceMock : public uavcan::ICanIface +{ +public: + struct FrameWithTime + { + uavcan::CanFrame frame; + uavcan::MonotonicTime time; + uavcan::UtcTime time_utc; + uavcan::CanIOFlags flags; + + FrameWithTime(const uavcan::CanFrame& frame, uavcan::MonotonicTime time) + : frame(frame) + , time(time) + , flags(0) + { } + + FrameWithTime(const uavcan::CanFrame& frame, uavcan::MonotonicTime time, uavcan::UtcTime time_utc) + : frame(frame) + , time(time) + , time_utc(time_utc) + , flags(0) + { } + + FrameWithTime(const uavcan::CanFrame& frame, uint64_t time_usec) + : frame(frame) + , time(uavcan::MonotonicTime::fromUSec(time_usec)) + , flags(0) + { } + }; + + std::queue tx; ///< Queue of outgoing frames (bus <-- library) + std::queue rx; ///< Queue of incoming frames (bus --> library) + std::queue loopback; ///< Loopback + bool writeable; + bool tx_failure; + bool rx_failure; + uint64_t num_errors; + uavcan::ISystemClock& iclock; + bool enable_utc_timestamping; + uavcan::CanFrame pending_tx; + + CanIfaceMock(uavcan::ISystemClock& iclock) + : writeable(true) + , tx_failure(false) + , rx_failure(false) + , num_errors(0) + , iclock(iclock) + , enable_utc_timestamping(false) + { } + + void pushRx(const uavcan::CanFrame& frame) + { + rx.push(FrameWithTime(frame, iclock.getMonotonic())); + } + + void pushRx(const uavcan::RxFrame& frame) + { + uavcan::CanFrame can_frame; + EXPECT_TRUE(frame.compile(can_frame)); + rx.push(FrameWithTime(can_frame, frame.getMonotonicTimestamp(), frame.getUtcTimestamp())); + } + + bool matchAndPopTx(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline) + { + if (tx.empty()) + { + std::cout << "Tx buffer is empty" << std::endl; + return false; + } + const FrameWithTime frame_time = tx.front(); + tx.pop(); + return (frame_time.frame == frame) && (frame_time.time == tx_deadline); + } + + bool matchPendingTx(const uavcan::CanFrame& frame) const + { + if (pending_tx != frame) + { + std::cout << "Pending TX mismatch: \n" + << " Expected: " << frame.toString(uavcan::CanFrame::StrAligned) << "\n" + << " Actual: " << pending_tx.toString(uavcan::CanFrame::StrAligned) << std::endl; + } + return pending_tx == frame; + } + + bool matchAndPopTx(const uavcan::CanFrame& frame, uint64_t tx_deadline_usec) + { + return matchAndPopTx(frame, uavcan::MonotonicTime::fromUSec(tx_deadline_usec)); + } + + uavcan::CanFrame popTxFrame() + { + if (tx.empty()) + { + std::cout << "Tx buffer is empty" << std::endl; + std::abort(); + } + const FrameWithTime frame_time = tx.front(); + tx.pop(); + return frame_time.frame; + } + + virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, + uavcan::CanIOFlags flags) + { + assert(this); + EXPECT_TRUE(writeable); // Shall never be called when not writeable + if (tx_failure) + { + return -1; + } + if (!writeable) + { + return 0; + } + tx.push(FrameWithTime(frame, tx_deadline)); + tx.back().flags = flags; + if (flags & uavcan::CanIOFlagLoopback) + { + loopback.push(FrameWithTime(frame, iclock.getMonotonic())); + } + return 1; + } + + virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, + uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) + { + assert(this); + if (loopback.empty()) + { + EXPECT_TRUE(rx.size()); // Shall never be called when not readable + if (rx_failure) + { + return -1; + } + if (rx.empty()) + { + return 0; + } + const FrameWithTime frame = rx.front(); + rx.pop(); + out_frame = frame.frame; + out_ts_monotonic = frame.time; + out_ts_utc = frame.time_utc; + out_flags = frame.flags; + } + else + { + out_flags |= uavcan::CanIOFlagLoopback; + const FrameWithTime frame = loopback.front(); + loopback.pop(); + out_frame = frame.frame; + out_ts_monotonic = frame.time; + out_ts_utc = frame.time_utc; + } + + // Let's just all pretend that this code is autogenerated, instead of being carefully designed by a human. + if (out_ts_utc.isZero()) + { + out_ts_utc = enable_utc_timestamping ? iclock.getUtc() : uavcan::UtcTime(); + } + return 1; + } + + // cppcheck-suppress unusedFunction + // cppcheck-suppress functionConst + virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig*, uavcan::uint16_t) { return 0; } + // cppcheck-suppress unusedFunction + virtual uavcan::uint16_t getNumFilters() const { return 4; } // decrease number of HW_filters from 9 to 4 + virtual uavcan::uint64_t getErrorCount() const { return num_errors; } +}; + +class CanDriverMock : public uavcan::ICanDriver +{ +public: + std::vector ifaces; + uavcan::ISystemClock& iclock; + bool select_failure; + + CanDriverMock(unsigned num_ifaces, uavcan::ISystemClock& iclock) + : ifaces(num_ifaces, CanIfaceMock(iclock)) + , iclock(iclock) + , select_failure(false) + { } + + void pushRxToAllIfaces(const uavcan::CanFrame& can_frame) + { + for (uint8_t i = 0; i < getNumIfaces(); i++) + { + ifaces.at(i).pushRx(can_frame); + } + } + + virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, + const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces], + uavcan::MonotonicTime deadline) + { + assert(this); + //std::cout << "Write/read masks: " << inout_write_iface_mask << "/" << inout_read_iface_mask << std::endl; + + for (unsigned i = 0; i < ifaces.size(); i++) + { + ifaces.at(i).pending_tx = (pending_tx[i] == UAVCAN_NULLPTR) ? uavcan::CanFrame() : *pending_tx[i]; + } + + if (select_failure) + { + return -1; + } + + const uavcan::uint8_t valid_iface_mask = uavcan::uint8_t((1 << getNumIfaces()) - 1); + EXPECT_FALSE(inout_masks.write & ~valid_iface_mask); + EXPECT_FALSE(inout_masks.read & ~valid_iface_mask); + + uavcan::uint8_t out_write_mask = 0; + uavcan::uint8_t out_read_mask = 0; + for (unsigned i = 0; i < getNumIfaces(); i++) + { + const uavcan::uint8_t mask = uavcan::uint8_t(1 << i); + if ((inout_masks.write & mask) && ifaces.at(i).writeable) + { + out_write_mask |= mask; + } + if ((inout_masks.read & mask) && (ifaces.at(i).rx.size() || ifaces.at(i).loopback.size())) + { + out_read_mask |= mask; + } + } + inout_masks.write = out_write_mask; + inout_masks.read = out_read_mask; + if ((out_write_mask | out_read_mask) == 0) + { + const uavcan::MonotonicTime ts = iclock.getMonotonic(); + const uavcan::MonotonicDuration diff = deadline - ts; + SystemClockMock* const mock = dynamic_cast(&iclock); + if (mock) + { + if (diff.isPositive()) + { + mock->advance(uint64_t(diff.toUSec())); // Emulating timeout + } + } + else + { + if (diff.isPositive()) + { + usleep(unsigned(diff.toUSec())); + } + } + return 0; + } + return 1; // This value is not being checked anyway, it just has to be greater than zero + } + + virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) { return &ifaces.at(iface_index); } + virtual const uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) const { return &ifaces.at(iface_index); } + virtual uavcan::uint8_t getNumIfaces() const { return uavcan::uint8_t(ifaces.size()); } +}; + +enum FrameType { STD, EXT }; +inline uavcan::CanFrame makeCanFrame(uint32_t id, const std::string& str_data, FrameType type) +{ + id |= (type == EXT) ? uavcan::CanFrame::FlagEFF : 0; + return uavcan::CanFrame(id, reinterpret_cast(str_data.c_str()), uavcan::uint8_t(str_data.length())); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/can_driver.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/can_driver.cpp new file mode 100644 index 000000000000..c6fcfccfeb1b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/can_driver.cpp @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include "can.hpp" +#include + +TEST(CanFrame, FrameProperties) +{ + EXPECT_TRUE(makeCanFrame(0, "", EXT).isExtended()); + EXPECT_FALSE(makeCanFrame(0, "", STD).isExtended()); + EXPECT_FALSE(makeCanFrame(0, "", EXT).isRemoteTransmissionRequest()); + EXPECT_FALSE(makeCanFrame(0, "", STD).isRemoteTransmissionRequest()); + + uavcan::CanFrame frame = makeCanFrame(123, "", STD); + frame.id |= uavcan::CanFrame::FlagRTR; + EXPECT_TRUE(frame.isRemoteTransmissionRequest()); + EXPECT_FALSE(frame.isErrorFrame()); + frame.id |= uavcan::CanFrame::FlagERR; + EXPECT_TRUE(frame.isErrorFrame()); +} + +TEST(CanFrame, Arbitration) +{ + using uavcan::CanFrame; + + CanFrame a; + CanFrame b; + + /* + * Simple + */ + a.id = 123; + b.id = 122; + ASSERT_TRUE(a.priorityLowerThan(b)); + ASSERT_TRUE(b.priorityHigherThan(a)); + + a.id = 123 | CanFrame::FlagEFF; + b.id = 122 | CanFrame::FlagEFF; + ASSERT_TRUE(a.priorityLowerThan(b)); + ASSERT_TRUE(b.priorityHigherThan(a)); + + a.id = 8; + b.id = 8; + ASSERT_FALSE(a.priorityLowerThan(b)); + ASSERT_FALSE(b.priorityHigherThan(a)); + + /* + * EXT vs STD + */ + a.id = 1000; // 1000 + b.id = 2000 | CanFrame::FlagEFF; // 2000 >> 18, wins + ASSERT_TRUE(a.priorityLowerThan(b)); + ASSERT_TRUE(b.priorityHigherThan(a)); + + a.id = 0x400; + b.id = 0x10000000 | CanFrame::FlagEFF; // (0x400 << 18), 11 most significant bits are the same --> EFF loses + ASSERT_TRUE(a.priorityHigherThan(b)); + ASSERT_TRUE(b.priorityLowerThan(a)); + + /* + * RTR vs Data + */ + a.id = 123 | CanFrame::FlagRTR; // On the same ID, RTR loses + b.id = 123; + ASSERT_TRUE(a.priorityLowerThan(b)); + ASSERT_TRUE(b.priorityHigherThan(a)); + + a.id = CanFrame::MaskStdID | CanFrame::FlagRTR; // RTR is STD, so it wins + b.id = CanFrame::MaskExtID | CanFrame::FlagEFF; // Lowest possible priority for data frame + ASSERT_TRUE(a.priorityHigherThan(b)); + ASSERT_TRUE(b.priorityLowerThan(a)); + + a.id = 123 | CanFrame::FlagRTR; // Both RTR arbitrate as usually + b.id = 122 | CanFrame::FlagRTR; + ASSERT_TRUE(a.priorityLowerThan(b)); + ASSERT_TRUE(b.priorityHigherThan(a)); +} + +TEST(CanFrame, ToString) +{ + uavcan::CanFrame frame = makeCanFrame(123, "\x01\x02\x03\x04" "1234", EXT); + EXPECT_EQ("0x0000007b 01 02 03 04 31 32 33 34 '....1234'", frame.toString()); + EXPECT_TRUE(frame.toString() == frame.toString(uavcan::CanFrame::StrAligned)); + + frame = makeCanFrame(123, "z", EXT); + EXPECT_EQ("0x0000007b 7a 'z'", frame.toString(uavcan::CanFrame::StrAligned)); + EXPECT_EQ("0x0000007b 7a 'z'", frame.toString()); + + EXPECT_EQ(" 0x141 61 62 63 64 aa bb cc dd 'abcd....'", + makeCanFrame(321, "abcd" "\xaa\xbb\xcc\xdd", STD).toString(uavcan::CanFrame::StrAligned)); + + EXPECT_EQ(" 0x100 ''", + makeCanFrame(256, "", STD).toString(uavcan::CanFrame::StrAligned)); + + EXPECT_EQ("0x100 ''", + makeCanFrame(256, "", STD).toString()); + + EXPECT_EQ("0x141 61 62 63 64 aa bb cc dd 'abcd....'", + makeCanFrame(321, "abcd" "\xaa\xbb\xcc\xdd", STD).toString()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/iface_mock.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/iface_mock.cpp new file mode 100644 index 000000000000..b9b9cf4368da --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/iface_mock.cpp @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include "can.hpp" + +TEST(CanDriverMock, Basic) +{ + using uavcan::CanFrame; + using uavcan::CanSelectMasks; + + SystemClockMock clockmock; + CanDriverMock driver(3, clockmock); + + const uavcan::CanFrame* pending_tx[uavcan::MaxCanIfaces] = { }; + + ASSERT_EQ(3, driver.getNumIfaces()); + + // All WR, no RD + CanSelectMasks masks; + masks.write = 7; + masks.read = 7; + EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100))); + EXPECT_EQ(7, masks.write); + EXPECT_EQ(0, masks.read); + + for (unsigned i = 0; i < 3; i++) + { + driver.ifaces.at(i).writeable = false; + } + + // No WR, no RD + masks.write = 7; + masks.read = 7; + EXPECT_EQ(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100))); + EXPECT_EQ(0, masks.write); + EXPECT_EQ(0, masks.read); + EXPECT_EQ(100, clockmock.monotonic); + EXPECT_EQ(100, clockmock.utc); + + // No WR, #1 RD + const CanFrame fr1 = makeCanFrame(123, "foo", EXT); + driver.ifaces.at(1).pushRx(fr1); + masks.write = 7; + masks.read = 6; + EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100))); + EXPECT_EQ(0, masks.write); + EXPECT_EQ(2, masks.read); + CanFrame fr2; + uavcan::MonotonicTime ts_monotonic; + uavcan::UtcTime ts_utc; + uavcan::CanIOFlags flags = 0; + EXPECT_EQ(1, driver.getIface(1)->receive(fr2, ts_monotonic, ts_utc, flags)); + EXPECT_EQ(0, flags); + EXPECT_EQ(fr1, fr2); + EXPECT_EQ(100, ts_monotonic.toUSec()); + EXPECT_EQ(0, ts_utc.toUSec()); + + // #0 WR, #1 RD, Select failure + driver.ifaces.at(0).writeable = true; + driver.select_failure = true; + masks.write = 1; + masks.read = 7; + EXPECT_EQ(-1, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100))); + EXPECT_EQ(1, masks.write); // Leaving masks unchanged - the library must ignore them + EXPECT_EQ(7, masks.read); +} + +TEST(CanDriverMock, Loopback) +{ + using uavcan::CanFrame; + using uavcan::CanSelectMasks; + + SystemClockMock clockmock; + CanDriverMock driver(1, clockmock); + + const uavcan::CanFrame* pending_tx[uavcan::MaxCanIfaces] = { }; + + CanSelectMasks masks; + masks.write = 1; + masks.read = 1; + EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100))); + EXPECT_EQ(1, masks.write); + EXPECT_EQ(0, masks.read); + + clockmock.advance(200); + + CanFrame fr1; + fr1.id = 123 | CanFrame::FlagEFF; + EXPECT_EQ(1, driver.getIface(0)->send(fr1, uavcan::MonotonicTime::fromUSec(10000), uavcan::CanIOFlagLoopback)); + + masks.write = 0; + masks.read = 1; + EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100))); + EXPECT_EQ(0, masks.write); + EXPECT_EQ(1, masks.read); + + CanFrame fr2; + uavcan::MonotonicTime ts_monotonic; + uavcan::UtcTime ts_utc; + uavcan::CanIOFlags flags = 0; + EXPECT_EQ(1, driver.getIface(0)->receive(fr2, ts_monotonic, ts_utc, flags)); + EXPECT_EQ(uavcan::CanIOFlagLoopback, flags); + EXPECT_EQ(fr1, fr2); + EXPECT_EQ(200, ts_monotonic.toUSec()); + EXPECT_EQ(0, ts_utc.toUSec()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/io.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/io.cpp new file mode 100644 index 000000000000..5a046973c3d3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/io.cpp @@ -0,0 +1,386 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include "can.hpp" + +static bool rxFrameEquals(const uavcan::CanRxFrame& rxframe, const uavcan::CanFrame& frame, + uint64_t timestamp_usec, int iface_index) +{ + if (static_cast(rxframe) != frame) + { + std::cout << "Frame mismatch:\n" + << " " << rxframe.toString(uavcan::CanFrame::StrAligned) << "\n" + << " " << frame.toString(uavcan::CanFrame::StrAligned) << std::endl; + } + return (static_cast(rxframe) == frame) && + (rxframe.ts_mono == uavcan::MonotonicTime::fromUSec(timestamp_usec)) && + (rxframe.iface_index == iface_index); +} + +TEST(CanIOManager, Reception) +{ + // Memory + uavcan::PoolAllocator pool; + + // Platform interface + SystemClockMock clockmock; + CanDriverMock driver(2, clockmock); + + // IO Manager + uavcan::CanIOManager iomgr(driver, pool, clockmock); + ASSERT_EQ(2, iomgr.getNumIfaces()); + + /* + * Empty, will time out + */ + uavcan::CanRxFrame frame; + uavcan::CanIOFlags flags = uavcan::CanIOFlags(); + EXPECT_EQ(0, iomgr.receive(frame, tsMono(100), flags)); + EXPECT_EQ(0, flags); + EXPECT_EQ(100, clockmock.monotonic); + EXPECT_EQ(100, clockmock.utc); + + /* + * Non empty from multiple ifaces + */ + const uavcan::CanFrame frames[2][3] = { + { makeCanFrame(1, "a0", EXT), makeCanFrame(99, "a1", EXT), makeCanFrame(803, "a2", STD) }, + { makeCanFrame(6341, "b0", EXT), makeCanFrame(196, "b1", STD), makeCanFrame(73, "b2", EXT) }, + }; + + clockmock.advance(10); + driver.ifaces.at(0).pushRx(frames[0][0]); // Timestamp 110 + driver.ifaces.at(1).pushRx(frames[1][0]); + clockmock.advance(10); + driver.ifaces.at(0).pushRx(frames[0][1]); // Timestamp 120 + driver.ifaces.at(1).pushRx(frames[1][1]); + clockmock.advance(10); + driver.ifaces.at(0).pushRx(frames[0][2]); // Timestamp 130 + driver.ifaces.at(1).pushRx(frames[1][2]); + clockmock.advance(10); + + EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + EXPECT_TRUE(rxFrameEquals(frame, frames[0][0], 110, 0)); + EXPECT_EQ(0, flags); + + EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + EXPECT_TRUE(rxFrameEquals(frame, frames[0][1], 120, 0)); + EXPECT_EQ(0, flags); + + EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + EXPECT_TRUE(rxFrameEquals(frame, frames[0][2], 130, 0)); + EXPECT_EQ(0, flags); + + EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + EXPECT_TRUE(rxFrameEquals(frame, frames[1][0], 110, 1)); + EXPECT_EQ(0, flags); + + EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + EXPECT_TRUE(rxFrameEquals(frame, frames[1][1], 120, 1)); + EXPECT_EQ(0, flags); + + EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + EXPECT_TRUE(rxFrameEquals(frame, frames[1][2], 130, 1)); + EXPECT_EQ(0, flags); + + EXPECT_EQ(0, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); // Will time out + EXPECT_EQ(0, flags); + + /* + * Perf counters + */ + driver.select_failure = true; + EXPECT_EQ(-uavcan::ErrDriver, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + + driver.select_failure = false; + driver.ifaces.at(1).pushRx(frames[0][0]); + driver.ifaces.at(1).rx_failure = true; + EXPECT_EQ(-uavcan::ErrDriver, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + + driver.ifaces.at(0).num_errors = 9000; + driver.ifaces.at(1).num_errors = 100500; + EXPECT_EQ(9000, iomgr.getIfacePerfCounters(0).errors); + EXPECT_EQ(100500, iomgr.getIfacePerfCounters(1).errors); + + EXPECT_EQ(3, iomgr.getIfacePerfCounters(0).frames_rx); + EXPECT_EQ(3, iomgr.getIfacePerfCounters(1).frames_rx); + + EXPECT_EQ(0, iomgr.getIfacePerfCounters(0).frames_tx); + EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).frames_tx); +} + +TEST(CanIOManager, Transmission) +{ + using uavcan::CanIOManager; + using uavcan::CanTxQueue; + + // Memory + uavcan::PoolAllocator pool; + + // Platform interface + SystemClockMock clockmock; + CanDriverMock driver(2, clockmock); + + // IO Manager + CanIOManager iomgr(driver, pool, clockmock, 9999); + ASSERT_EQ(2, iomgr.getNumIfaces()); + + const int ALL_IFACES_MASK = 3; + + const uavcan::CanFrame frames[] = { + makeCanFrame(1, "a0", EXT), makeCanFrame(99, "a1", EXT), makeCanFrame(803, "a2", STD) + }; + + uavcan::CanIOFlags flags = uavcan::CanIOFlags(); + + /* + * Simple transmission + */ + EXPECT_EQ(2, iomgr.send(frames[0], tsMono(100), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Volatile, flags)); + EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 100)); + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 100)); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0])); + + EXPECT_EQ(1, iomgr.send(frames[1], tsMono(200), tsMono(100), 2, CanTxQueue::Persistent, flags)); // To #1 only + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 200)); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(uavcan::CanFrame())); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1])); + + EXPECT_EQ(0, clockmock.monotonic); + EXPECT_EQ(0, clockmock.utc); + EXPECT_TRUE(driver.ifaces.at(0).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(1).tx.empty()); + EXPECT_EQ(0, iomgr.getIfacePerfCounters(0).errors); + EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).errors); + + /* + * TX Queue basics + */ + EXPECT_EQ(0, pool.getNumUsedBlocks()); + + // Sending to both, #0 blocked + driver.ifaces.at(0).writeable = false; + EXPECT_LT(0, iomgr.send(frames[0], tsMono(201), tsMono(200), ALL_IFACES_MASK, CanTxQueue::Persistent, flags)); + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 201)); + EXPECT_EQ(200, clockmock.monotonic); + EXPECT_EQ(200, clockmock.utc); + EXPECT_TRUE(driver.ifaces.at(0).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(1).tx.empty()); + EXPECT_EQ(1, pool.getNumUsedBlocks()); // One frame went into TX queue, and will expire soon + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); // This one will persist + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(uavcan::CanFrame())); // This will drop off on the second select() + + // Sending to both, both blocked + driver.ifaces.at(1).writeable = false; + EXPECT_EQ(0, iomgr.send(frames[1], tsMono(777), tsMono(300), ALL_IFACES_MASK, CanTxQueue::Volatile, flags)); + EXPECT_EQ(3, pool.getNumUsedBlocks()); // Total 3 frames in TX queue now + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); // Still 0 + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1])); // 1!! + + // Sending to #0, both blocked + EXPECT_EQ(0, iomgr.send(frames[2], tsMono(888), tsMono(400), 1, CanTxQueue::Persistent, flags)); + EXPECT_EQ(400, clockmock.monotonic); + EXPECT_EQ(400, clockmock.utc); + EXPECT_TRUE(driver.ifaces.at(0).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(1).tx.empty()); + EXPECT_EQ(4, pool.getNumUsedBlocks()); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1])); + + // At this time TX queues are containing the following data: + // iface 0: frames[0] (EXPIRED), frames[1], frames[2] + // iface 1: frames[1] + + // Sending to #1, both writeable + driver.ifaces.at(0).writeable = true; + driver.ifaces.at(1).writeable = true; + // One frame per each iface will be sent: + EXPECT_LT(0, iomgr.send(frames[0], tsMono(999), tsMono(500), 2, CanTxQueue::Persistent, flags)); + EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 777)); // Note that frame[0] on iface #0 has expired + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 999)); // In different order due to prioritization + EXPECT_TRUE(driver.ifaces.at(0).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(1).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); // Expired but still will be reported + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0])); + + // Calling receive() to flush the rest two frames + uavcan::CanRxFrame dummy_rx_frame; + EXPECT_EQ(0, iomgr.receive(dummy_rx_frame, tsMono(0), flags)); + EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 888)); + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 777)); + ASSERT_EQ(0, flags); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[2])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1])); + + // Final checks + EXPECT_TRUE(driver.ifaces.at(0).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(1).tx.empty()); + EXPECT_EQ(0, pool.getNumUsedBlocks()); // Make sure the memory was properly released + EXPECT_EQ(1, iomgr.getIfacePerfCounters(0).errors); // This is because of expired frame[0] + EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).errors); + + /* + * TX Queue updates from receive() call + */ + driver.ifaces.at(0).writeable = false; + driver.ifaces.at(1).writeable = false; + + // Sending 5 frames, one will be rejected + EXPECT_EQ(0, iomgr.send(frames[2], tsMono(2222), tsMono(1000), ALL_IFACES_MASK, CanTxQueue::Persistent, flags)); + EXPECT_EQ(0, iomgr.send(frames[0], tsMono(3333), tsMono(1100), 2, CanTxQueue::Persistent, flags)); + // One frame kicked here: + EXPECT_EQ(0, iomgr.send(frames[1], tsMono(4444), tsMono(1200), ALL_IFACES_MASK, CanTxQueue::Volatile, flags)); + + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[1])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0])); + + // State checks + EXPECT_EQ(4, pool.getNumUsedBlocks()); // TX queue is full + EXPECT_EQ(1200, clockmock.monotonic); + EXPECT_EQ(1200, clockmock.utc); + EXPECT_TRUE(driver.ifaces.at(0).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(1).tx.empty()); + + // Preparing the driver mock for receive() call + driver.ifaces.at(0).writeable = true; + driver.ifaces.at(1).writeable = true; + const uavcan::CanFrame rx_frames[] = { makeCanFrame(123, "rx0", STD), makeCanFrame(321, "rx1", EXT) }; + driver.ifaces.at(0).pushRx(rx_frames[0]); + driver.ifaces.at(1).pushRx(rx_frames[1]); + + // This shall transmit _some_ frames now, at least one per iface (exact number can be changed - it will be OK) + uavcan::CanRxFrame rx_frame; + EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0), flags)); // Non-blocking + EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[0], 1200, 0)); + EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 4444)); + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 3333)); + ASSERT_EQ(0, flags); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[1])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0])); + + EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0), flags)); + EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[1], 1200, 1)); + EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 2222)); + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[2], 2222)); // Iface #1, frame[1] was rejected (VOLATILE) + ASSERT_EQ(0, flags); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[2])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[2])); + + // State checks + EXPECT_EQ(0, pool.getNumUsedBlocks()); // TX queue is empty + EXPECT_EQ(1200, clockmock.monotonic); + EXPECT_EQ(1200, clockmock.utc); + EXPECT_TRUE(driver.ifaces.at(0).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(1).tx.empty()); + EXPECT_EQ(1, iomgr.getIfacePerfCounters(0).errors); + EXPECT_EQ(1, iomgr.getIfacePerfCounters(1).errors); // This is because of rejected frame[1] + + /* + * Error handling + */ + // Select failure + driver.select_failure = true; + EXPECT_EQ(-uavcan::ErrDriver, iomgr.receive(rx_frame, tsMono(2000), flags)); + EXPECT_EQ(-uavcan::ErrDriver, + iomgr.send(frames[0], tsMono(2100), tsMono(2000), ALL_IFACES_MASK, CanTxQueue::Volatile, flags)); + EXPECT_EQ(1200, clockmock.monotonic); + EXPECT_EQ(1200, clockmock.utc); + ASSERT_EQ(0, flags); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0])); + + // Transmission failure + driver.select_failure = false; + driver.ifaces.at(0).writeable = true; + driver.ifaces.at(1).writeable = true; + driver.ifaces.at(0).tx_failure = true; + driver.ifaces.at(1).tx_failure = true; + // Non-blocking - return < 0 + EXPECT_GE(0, iomgr.send(frames[0], tsMono(2200), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Persistent, flags)); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0])); + + ASSERT_EQ(2, pool.getNumUsedBlocks()); // Untransmitted frames will be buffered + + // Failure removed - transmission shall proceed + driver.ifaces.at(0).tx_failure = false; + driver.ifaces.at(1).tx_failure = false; + EXPECT_EQ(0, iomgr.receive(rx_frame, tsMono(2500), flags)); + EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 2200)); + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 2200)); + EXPECT_EQ(0, pool.getNumUsedBlocks()); // All transmitted + ASSERT_EQ(0, flags); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(uavcan::CanFrame())); // Last call will be receive-only, + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(uavcan::CanFrame())); // hence empty TX + + /* + * Perf counters + */ + EXPECT_EQ(1, iomgr.getIfacePerfCounters(0).frames_rx); + EXPECT_EQ(1, iomgr.getIfacePerfCounters(1).frames_rx); + + EXPECT_EQ(6, iomgr.getIfacePerfCounters(0).frames_tx); + EXPECT_EQ(8, iomgr.getIfacePerfCounters(1).frames_tx); +} + +TEST(CanIOManager, Loopback) +{ + using uavcan::CanIOManager; + using uavcan::CanTxQueue; + using uavcan::CanFrame; + using uavcan::CanRxFrame; + + // Memory + uavcan::PoolAllocator pool; + + // Platform interface + SystemClockMock clockmock; + CanDriverMock driver(2, clockmock); + + // IO Manager + CanIOManager iomgr(driver, pool, clockmock); + ASSERT_EQ(2, iomgr.getNumIfaces()); + + CanFrame fr1; + fr1.id = 123 | CanFrame::FlagEFF; + + CanFrame fr2; + fr2.id = 456 | CanFrame::FlagEFF; + + CanRxFrame rfr1; + CanRxFrame rfr2; + + uavcan::CanIOFlags flags = 0; + ASSERT_EQ(1, iomgr.send(fr1, tsMono(1000), tsMono(0), 1, CanTxQueue::Volatile, uavcan::CanIOFlagLoopback)); + ASSERT_LE(0, iomgr.receive(rfr1, tsMono(100), flags)); + ASSERT_EQ(uavcan::CanIOFlagLoopback, flags); + ASSERT_TRUE(rfr1 == fr1); + + flags = 0; + ASSERT_EQ(1, iomgr.send(fr1, tsMono(1000), tsMono(0), 1, CanTxQueue::Volatile, uavcan::CanIOFlagLoopback)); + ASSERT_EQ(1, iomgr.send(fr2, tsMono(1000), tsMono(0), 1, CanTxQueue::Persistent, uavcan::CanIOFlagLoopback)); + ASSERT_LE(0, iomgr.receive(rfr1, tsMono(100), flags)); + ASSERT_EQ(uavcan::CanIOFlagLoopback, flags); + ASSERT_LE(0, iomgr.receive(rfr2, tsMono(100), flags)); + ASSERT_EQ(uavcan::CanIOFlagLoopback, flags); + ASSERT_TRUE(rfr1 == fr1); + ASSERT_TRUE(rfr2 == fr2); + + /* + * Perf counters + * Loopback frames are not registered as RX + */ + EXPECT_EQ(0, iomgr.getIfacePerfCounters(0).frames_rx); + EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).frames_rx); + + EXPECT_EQ(3, iomgr.getIfacePerfCounters(0).frames_tx); + EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).frames_tx); +} + +TEST(CanIOManager, Size) +{ + std::cout << sizeof(uavcan::CanIOManager) << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/tx_queue.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/tx_queue.cpp new file mode 100644 index 000000000000..da5aa58bb293 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/tx_queue.cpp @@ -0,0 +1,216 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "can.hpp" + + +static int getQueueLength(uavcan::CanTxQueue& queue) +{ + const uavcan::CanTxQueue::Entry* p = queue.peek(); + int length = 0; + while (p) + { + length++; + p = p->getNextListNode(); + } + return length; +} + +static bool isInQueue(uavcan::CanTxQueue& queue, const uavcan::CanFrame& frame) +{ + const uavcan::CanTxQueue::Entry* p = queue.peek(); + while (p) + { + if (frame == p->frame) + { + return true; + } + p = p->getNextListNode(); + } + return false; +} + +TEST(CanTxQueue, Qos) +{ + const uavcan::CanIOFlags flags = 0; + uavcan::CanTxQueue::Entry e1(makeCanFrame(100, "", EXT), tsMono(1000), uavcan::CanTxQueue::Volatile, flags); + uavcan::CanTxQueue::Entry e2(makeCanFrame(100, "", EXT), tsMono(1000), uavcan::CanTxQueue::Volatile, flags); + + EXPECT_FALSE(e1.qosHigherThan(e2)); + EXPECT_FALSE(e2.qosHigherThan(e1)); + EXPECT_FALSE(e1.qosLowerThan(e2)); + EXPECT_FALSE(e2.qosLowerThan(e1)); + + e2.qos = uavcan::CanTxQueue::Persistent; + + EXPECT_FALSE(e1.qosHigherThan(e2)); + EXPECT_TRUE(e2.qosHigherThan(e1)); + EXPECT_TRUE(e1.qosLowerThan(e2)); + EXPECT_FALSE(e2.qosLowerThan(e1)); + + e1.qos = uavcan::CanTxQueue::Persistent; + e1.frame.id -= 1; + + EXPECT_TRUE(e1.qosHigherThan(e2)); + EXPECT_FALSE(e2.qosHigherThan(e1)); + EXPECT_FALSE(e1.qosLowerThan(e2)); + EXPECT_TRUE(e2.qosLowerThan(e1)); +} + +TEST(CanTxQueue, TxQueue) +{ + using uavcan::CanTxQueue; + using uavcan::CanFrame; + + ASSERT_GE(40, sizeof(CanTxQueue::Entry)); // should be true for any platforms, though not required + + uavcan::PoolAllocator<40 * 4, 40> pool; + + SystemClockMock clockmock; + + CanTxQueue queue(pool, clockmock, 99999); + EXPECT_TRUE(queue.isEmpty()); + + const uavcan::CanIOFlags flags = 0; + + // Descending priority + const CanFrame f0 = makeCanFrame(0, "f0", EXT); + const CanFrame f1 = makeCanFrame(10, "f1", EXT); + const CanFrame f2 = makeCanFrame(20, "f2", EXT); + const CanFrame f3 = makeCanFrame(100, "f3", EXT); + const CanFrame f4 = makeCanFrame(10000, "f4", EXT); + const CanFrame f5 = makeCanFrame(99999, "f5", EXT); + const CanFrame f5a = makeCanFrame(99999, "f5a", EXT); + const CanFrame f6 = makeCanFrame(999999, "f6", EXT); + + /* + * Priority insertion + */ + queue.push(f4, tsMono(100), CanTxQueue::Persistent, flags); + EXPECT_FALSE(queue.isEmpty()); + EXPECT_EQ(1, pool.getNumUsedBlocks()); + EXPECT_EQ(f4, queue.peek()->frame); + EXPECT_TRUE(queue.topPriorityHigherOrEqual(f5)); + EXPECT_TRUE(queue.topPriorityHigherOrEqual(f4)); // Equal + EXPECT_FALSE(queue.topPriorityHigherOrEqual(f3)); + + queue.push(f3, tsMono(200), CanTxQueue::Persistent, flags); + EXPECT_EQ(f3, queue.peek()->frame); + + queue.push(f0, tsMono(300), CanTxQueue::Volatile, flags); + EXPECT_EQ(f0, queue.peek()->frame); + + queue.push(f1, tsMono(400), CanTxQueue::Volatile, flags); + EXPECT_EQ(f0, queue.peek()->frame); // Still f0, since it is highest + EXPECT_TRUE(queue.topPriorityHigherOrEqual(f0)); // Equal + EXPECT_TRUE(queue.topPriorityHigherOrEqual(f1)); + + // Out of free memory now + + EXPECT_EQ(0, queue.getRejectedFrameCount()); + EXPECT_EQ(4, getQueueLength(queue)); + EXPECT_TRUE(isInQueue(queue, f0)); + EXPECT_TRUE(isInQueue(queue, f1)); + EXPECT_TRUE(isInQueue(queue, f3)); + EXPECT_TRUE(isInQueue(queue, f4)); + + const CanTxQueue::Entry* p = queue.peek(); + while (p) + { + std::cout << p->toString() << std::endl; + p = p->getNextListNode(); + } + + /* + * QoS + */ + EXPECT_FALSE(isInQueue(queue, f2)); + queue.push(f2, tsMono(100), CanTxQueue::Volatile, flags); // Non preempting, will be rejected + EXPECT_FALSE(isInQueue(queue, f2)); + + queue.push(f2, tsMono(500), CanTxQueue::Persistent, flags); // Will override f1 (f3 and f4 are presistent) + EXPECT_TRUE(isInQueue(queue, f2)); + EXPECT_FALSE(isInQueue(queue, f1)); + EXPECT_EQ(4, getQueueLength(queue)); + EXPECT_EQ(2, queue.getRejectedFrameCount()); + EXPECT_EQ(f0, queue.peek()->frame); // Check the priority + + queue.push(f5, tsMono(600), CanTxQueue::Persistent, flags); // Will override f0 (rest are presistent) + EXPECT_TRUE(isInQueue(queue, f5)); + EXPECT_FALSE(isInQueue(queue, f0)); + EXPECT_EQ(f2, queue.peek()->frame); // Check the priority + + // No volatile frames left now + + queue.push(f5a, tsMono(700), CanTxQueue::Persistent, flags); // Will override f5 (same frame, same QoS) + EXPECT_TRUE(isInQueue(queue, f5a)); + EXPECT_FALSE(isInQueue(queue, f5)); + + queue.push(f6, tsMono(700), CanTxQueue::Persistent, flags); // Will be rejected (lowest QoS) + EXPECT_FALSE(isInQueue(queue, f6)); + + EXPECT_FALSE(queue.topPriorityHigherOrEqual(f0)); + EXPECT_TRUE(queue.topPriorityHigherOrEqual(f2)); // Equal + EXPECT_TRUE(queue.topPriorityHigherOrEqual(f5a)); + EXPECT_EQ(4, getQueueLength(queue)); + EXPECT_EQ(4, pool.getNumUsedBlocks()); + EXPECT_EQ(5, queue.getRejectedFrameCount()); + EXPECT_TRUE(isInQueue(queue, f2)); + EXPECT_TRUE(isInQueue(queue, f3)); + EXPECT_TRUE(isInQueue(queue, f4)); + EXPECT_TRUE(isInQueue(queue, f5a)); + EXPECT_EQ(f2, queue.peek()->frame); // Check the priority + + /* + * Expiration + */ + clockmock.monotonic = 101; + queue.push(f0, tsMono(800), CanTxQueue::Volatile, flags); // Will replace f4 which is expired now + EXPECT_TRUE(isInQueue(queue, f0)); + EXPECT_FALSE(isInQueue(queue, f4)); + EXPECT_EQ(6, queue.getRejectedFrameCount()); + + clockmock.monotonic = 1001; + queue.push(f5, tsMono(2000), CanTxQueue::Volatile, flags); // Entire queue is expired + EXPECT_TRUE(isInQueue(queue, f5)); + EXPECT_EQ(1, getQueueLength(queue)); // Just one entry left - f5 + EXPECT_EQ(1, pool.getNumUsedBlocks()); // Make sure there is no leaks + EXPECT_EQ(10, queue.getRejectedFrameCount()); + + queue.push(f0, tsMono(1000), CanTxQueue::Persistent, flags); // This entry is already expired + EXPECT_EQ(1, getQueueLength(queue)); + EXPECT_EQ(1, pool.getNumUsedBlocks()); + EXPECT_EQ(11, queue.getRejectedFrameCount()); + + /* + * Removing + */ + queue.push(f4, tsMono(5000), CanTxQueue::Volatile, flags); + EXPECT_EQ(2, getQueueLength(queue)); + EXPECT_TRUE(isInQueue(queue, f4)); + EXPECT_EQ(f4, queue.peek()->frame); + + CanTxQueue::Entry* entry = queue.peek(); + EXPECT_TRUE(entry); + queue.remove(entry); + EXPECT_FALSE(entry); + + EXPECT_FALSE(isInQueue(queue, f4)); + EXPECT_TRUE(isInQueue(queue, f5)); + + entry = queue.peek(); + EXPECT_TRUE(entry); + queue.remove(entry); + EXPECT_FALSE(entry); + + EXPECT_FALSE(isInQueue(queue, f5)); + + EXPECT_EQ(0, getQueueLength(queue)); // Final state checks + EXPECT_EQ(0, pool.getNumUsedBlocks()); + EXPECT_EQ(11, queue.getRejectedFrameCount()); + EXPECT_FALSE(queue.peek()); + EXPECT_FALSE(queue.topPriorityHigherOrEqual(f0)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can_acceptance_filter_configurator.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can_acceptance_filter_configurator.cpp new file mode 100644 index 000000000000..1d3f3fc6a7f0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can_acceptance_filter_configurator.cpp @@ -0,0 +1,219 @@ +/* + * Copyright (C) 2014 Pavel Kirienko , + * Ilia Sheremet + */ + +#include +#include + +#include +#include "../node/test_node.hpp" +#include "uavcan/node/subscriber.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +template +struct SubscriptionListener +{ + typedef uavcan::ReceivedDataStructure ReceivedDataStructure; + + struct ReceivedDataStructureCopy + { + uavcan::MonotonicTime ts_monotonic; + uavcan::UtcTime ts_utc; + uavcan::TransferType transfer_type; + uavcan::TransferID transfer_id; + uavcan::NodeID src_node_id; + uavcan::uint8_t iface_index; + DataType msg; + + ReceivedDataStructureCopy(const ReceivedDataStructure& s) : + ts_monotonic(s.getMonotonicTimestamp()), + ts_utc(s.getUtcTimestamp()), + transfer_type(s.getTransferType()), + transfer_id(s.getTransferID()), + src_node_id(s.getSrcNodeID()), + iface_index(s.getIfaceIndex()), + msg(s) + { } + }; + + std::vector simple; + std::vector extended; + + void receiveExtended(ReceivedDataStructure& msg) + { + extended.push_back(msg); + } + + void receiveSimple(DataType& msg) + { + simple.push_back(msg); + } + + typedef SubscriptionListener SelfType; + typedef uavcan::MethodBinder ExtendedBinder; + typedef uavcan::MethodBinder SimpleBinder; + + ExtendedBinder bindExtended() { return ExtendedBinder(this, &SelfType::receiveExtended); } + SimpleBinder bindSimple() { return SimpleBinder(this, &SelfType::receiveSimple); } +}; + +static void writeServiceServerCallback( + const uavcan::ReceivedDataStructure& req, + uavcan::protocol::file::BeginFirmwareUpdate::Response& rsp) +{ + std::cout << req << std::endl; + rsp.error = rsp.ERROR_UNKNOWN; +} + +TEST(CanAcceptanceFilter, Basic_test) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + uavcan::DefaultDataTypeRegistrator _reg6; + uavcan::DefaultDataTypeRegistrator _reg7; + + SystemClockDriver clock_driver; + CanDriverMock can_driver(1, clock_driver); + TestNode node(can_driver, clock_driver, 24); + + uavcan::Subscriber::ExtendedBinder> sub_1(node); + uavcan::Subscriber::ExtendedBinder> sub_2(node); + uavcan::Subscriber::ExtendedBinder> sub_3(node); + uavcan::Subscriber::ExtendedBinder> sub_4(node); + uavcan::Subscriber::ExtendedBinder> sub_5(node); + uavcan::Subscriber::ExtendedBinder> sub_6(node); + uavcan::Subscriber::ExtendedBinder> sub_6_1(node); + uavcan::ServiceServer server(node); + + SubscriptionListener listener_1; + SubscriptionListener listener_2; + SubscriptionListener listener_3; + SubscriptionListener listener_4; + SubscriptionListener listener_5; + SubscriptionListener listener_6; + + sub_1.start(listener_1.bindExtended()); + sub_2.start(listener_2.bindExtended()); + sub_3.start(listener_3.bindExtended()); + sub_4.start(listener_4.bindExtended()); + sub_5.start(listener_5.bindExtended()); + sub_6.start(listener_6.bindExtended()); + sub_6_1.start(listener_6.bindExtended()); + server.start(writeServiceServerCallback); + std::cout << "Subscribers are initialized." << std::endl; + + uavcan::CanAcceptanceFilterConfigurator anon_test_configuration(node, 10); + + int configure_filters_assert = anon_test_configuration.computeConfiguration(); + ASSERT_EQ(configure_filters_assert, 0); + std::cout << "Filters are calculated with anonymous configuration." << std::endl; + + const auto& configure_array = anon_test_configuration.getConfiguration(); + uint32_t configure_array_size = configure_array.getSize(); + std::cout << "Number of configurations after first time computeConfiguration() invoked: " + << configure_array_size << std::endl; + ASSERT_EQ(configure_array_size, 9); + + std::cout << "Adding two additional configurations ... " << std::endl; + uavcan::CanFilterConfig aux_config_1, aux_config_2; + aux_config_1.id = 911; + aux_config_1.mask = 1488; + aux_config_2.id = 999999; + aux_config_2.mask = 849128412; + configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_1); + ASSERT_EQ(configure_filters_assert, 0); + configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_2); + ASSERT_EQ(configure_filters_assert, 0); + configure_array_size = configure_array.getSize(); + std::cout << "New configuration anon_container size: " << configure_array_size << std::endl; + ASSERT_EQ(configure_array_size, 11); + + std::cout << "Applying configuration ... " << std::endl; + configure_filters_assert = anon_test_configuration.applyConfiguration(); + ASSERT_EQ(configure_filters_assert, 0); + std::cout << "Filters are configured." << std::endl; + configure_array_size = configure_array.getSize(); + std::cout << "Final configuration anon_container size: " << configure_array_size << std::endl; + ASSERT_EQ(configure_array_size, 10); + + for (uint16_t i = 0; i + */ + +#include +#include +#include +#include "transfer_test_helpers.hpp" +#include "can/can.hpp" +#include + + +class DispatcherTransferEmulator : public IncomingTransferEmulatorBase +{ + CanDriverMock& target_; + +public: + DispatcherTransferEmulator(CanDriverMock& target, uavcan::NodeID dst_node_id = 127) + : IncomingTransferEmulatorBase(dst_node_id) + , target_(target) + { } + + void sendOneFrame(const uavcan::RxFrame& frame) + { + CanIfaceMock* const iface = static_cast(target_.getIface(frame.getIfaceIndex())); + EXPECT_TRUE(iface); + if (iface) + { + iface->pushRx(frame); + } + } +}; + + +struct RxFrameListener : public uavcan::IRxFrameListener +{ + std::vector rx_frames; + + virtual void handleRxFrame(const uavcan::CanRxFrame& frame, uavcan::CanIOFlags flags) + { + std::cout << "RX frame [flags=" << flags << "]: " << frame.toString() << std::endl; + if ((flags & uavcan::CanIOFlagLoopback) == 0) + { + rx_frames.push_back(frame); + } + } +}; + + +static const uavcan::NodeID SELF_NODE_ID(64); + + +TEST(Dispatcher, Reception) +{ + uavcan::PoolAllocator pool; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + uavcan::Dispatcher dispatcher(driver, pool, clockmock); + ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID)); // Can be set only once + ASSERT_FALSE(dispatcher.setNodeID(SELF_NODE_ID)); + ASSERT_EQ(SELF_NODE_ID, dispatcher.getNodeID()); + + DispatcherTransferEmulator emulator(driver, SELF_NODE_ID); + + /* + * RX listener + */ + RxFrameListener rx_listener; + ASSERT_FALSE(dispatcher.getRxFrameListener()); + dispatcher.installRxFrameListener(&rx_listener); + ASSERT_TRUE(dispatcher.getRxFrameListener()); + ASSERT_TRUE(rx_listener.rx_frames.empty()); + + /* + * Test environment + */ + static const uavcan::DataTypeDescriptor TYPES[4] = + { + makeDataType(uavcan::DataTypeKindMessage, 1), + makeDataType(uavcan::DataTypeKindMessage, 2), + makeDataType(uavcan::DataTypeKindService, 1), + makeDataType(uavcan::DataTypeKindService, 1) + }; + + typedef std::unique_ptr TestListenerPtr; + static const int MaxBufSize = 512; + static const int NumSubscribers = 6; + TestListenerPtr subscribers[NumSubscribers] = + { + TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[0], MaxBufSize, pool)), // msg + TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[0], MaxBufSize, pool)), // msg // Two similar + TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[1], MaxBufSize, pool)), // msg + TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[2], MaxBufSize, pool)), // srv + TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[3], MaxBufSize, pool)), // srv + TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[3], MaxBufSize, pool)) // srv // Repeat again + }; + + static const std::string DATA[6] = + { + "Yes, man is mortal, but that would be only half the trouble. " + "The worst of it is that he's sometimes unexpectedly mortal - there's the trick!", + + "In fact, I'm beginning to fear that this confusion will go on for a long time. " + "And all because he writes down what I said incorrectly.", + + "I had the pleasure of meeting that young man at the Patriarch's Ponds. " + "He almost drove me mad myself, proving to me that I don't exist. But you do believe that it is really I?", + + "He was a dreamer, a thinker, a speculative philosopher... or, as his wife would have it, an idiot.", + + "The only way to get ideas for stories is to drink way too much coffee and buy a desk that doesn't " + "collapse when you beat your head against it", + + "" + }; + + for (unsigned i = 0; i < sizeof(DATA) / sizeof(DATA[0]); i++) + { + std::cout << "Size of test data chunk " << i << ": " << DATA[i].length() << std::endl; + } + + const Transfer transfers[8] = + { + emulator.makeTransfer(0, uavcan::TransferTypeMessageBroadcast, 10, DATA[0], TYPES[0]), + emulator.makeTransfer(5, uavcan::TransferTypeMessageBroadcast, 11, DATA[1], TYPES[1]), + emulator.makeTransfer(10, uavcan::TransferTypeServiceRequest, 12, DATA[2], TYPES[2]), + emulator.makeTransfer(15, uavcan::TransferTypeServiceResponse, 13, DATA[4], TYPES[3]), + emulator.makeTransfer(20, uavcan::TransferTypeMessageBroadcast, 14, DATA[3], TYPES[0]), + emulator.makeTransfer(25, uavcan::TransferTypeMessageBroadcast, 15, DATA[5], TYPES[1]), + // Wrongly addressed: + emulator.makeTransfer(29, uavcan::TransferTypeServiceResponse, 10, DATA[0], TYPES[3], 100), + emulator.makeTransfer(31, uavcan::TransferTypeServiceRequest, 11, DATA[4], TYPES[2], 101) + }; + + /* + * Registration + */ + for (int i = 0; i < NumSubscribers; i++) + { + ASSERT_FALSE(dispatcher.hasSubscriber(subscribers[i]->getDataTypeDescriptor().getID())); + ASSERT_FALSE(dispatcher.hasPublisher(subscribers[i]->getDataTypeDescriptor().getID())); + ASSERT_FALSE(dispatcher.hasServer(subscribers[i]->getDataTypeDescriptor().getID())); + } + + ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[0].get())); + ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[1].get())); + ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[2].get())); + ASSERT_TRUE(dispatcher.registerServiceRequestListener(subscribers[3].get())); + ASSERT_TRUE(dispatcher.registerServiceResponseListener(subscribers[4].get())); + ASSERT_TRUE(dispatcher.registerServiceResponseListener(subscribers[5].get())); + + for (int i = 0; i < NumSubscribers; i++) + { + ASSERT_FALSE(dispatcher.hasPublisher(subscribers[i]->getDataTypeDescriptor().getID())); + } + + // Subscribers + ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[0]->getDataTypeDescriptor().getID())); + ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[1]->getDataTypeDescriptor().getID())); + ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[2]->getDataTypeDescriptor().getID())); + + // Servers + ASSERT_TRUE(dispatcher.hasServer(subscribers[3]->getDataTypeDescriptor().getID())); + + /* + * Sending the transfers + */ + // Multiple service request listeners are not allowed + ASSERT_FALSE(dispatcher.registerServiceRequestListener(subscribers[3].get())); + + // Item count validation + ASSERT_EQ(3, dispatcher.getNumMessageListeners()); + ASSERT_EQ(1, dispatcher.getNumServiceRequestListeners()); + ASSERT_EQ(2, dispatcher.getNumServiceResponseListeners()); + + for (int i = 0; i < NumSubscribers; i++) + { + ASSERT_TRUE(subscribers[i]->isEmpty()); + } + + emulator.send(transfers); + emulator.send(transfers); // Just for fun, they will be ignored anyway. + + while (true) + { + const int res = dispatcher.spinOnce(); + ASSERT_LE(0, res); + clockmock.advance(100); + if (res == 0) + { + break; + } + } + + /* + * Matching. + */ + ASSERT_TRUE(subscribers[0]->matchAndPop(transfers[4])); + ASSERT_TRUE(subscribers[0]->matchAndPop(transfers[0])); + + ASSERT_TRUE(subscribers[1]->matchAndPop(transfers[4])); + ASSERT_TRUE(subscribers[1]->matchAndPop(transfers[0])); + + ASSERT_TRUE(subscribers[2]->matchAndPop(transfers[5])); + ASSERT_TRUE(subscribers[2]->matchAndPop(transfers[1])); + + ASSERT_TRUE(subscribers[3]->matchAndPop(transfers[2])); + + ASSERT_TRUE(subscribers[4]->matchAndPop(transfers[3])); + + ASSERT_TRUE(subscribers[5]->matchAndPop(transfers[3])); + + for (int i = 0; i < NumSubscribers; i++) + { + ASSERT_TRUE(subscribers[i]->isEmpty()); + } + + /* + * Unregistering all transfers + */ + dispatcher.unregisterMessageListener(subscribers[0].get()); + dispatcher.unregisterMessageListener(subscribers[1].get()); + dispatcher.unregisterMessageListener(subscribers[2].get()); + dispatcher.unregisterServiceRequestListener(subscribers[3].get()); + dispatcher.unregisterServiceResponseListener(subscribers[4].get()); + dispatcher.unregisterServiceResponseListener(subscribers[5].get()); + + ASSERT_EQ(0, dispatcher.getNumMessageListeners()); + ASSERT_EQ(0, dispatcher.getNumServiceRequestListeners()); + ASSERT_EQ(0, dispatcher.getNumServiceResponseListeners()); + + /* + * Perf counters + */ + EXPECT_LT(0, dispatcher.getTransferPerfCounter().getErrorCount()); // Repeated transfers + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(9, dispatcher.getTransferPerfCounter().getRxTransferCount()); + + /* + * RX listener + */ + std::cout << "Num received frames: " << rx_listener.rx_frames.size() << std::endl; + ASSERT_EQ(292, rx_listener.rx_frames.size()); +} + + +TEST(Dispatcher, Transmission) +{ + uavcan::PoolAllocator pool; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + uavcan::Dispatcher dispatcher(driver, pool, clockmock); + ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID)); // Can be set only once + ASSERT_FALSE(dispatcher.setNodeID(SELF_NODE_ID)); + + /* + * RX listener + */ + RxFrameListener rx_listener; + dispatcher.installRxFrameListener(&rx_listener); + + /* + * Transmission + */ + static const uavcan::MonotonicTime TX_DEADLINE = tsMono(123456); + + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false + uavcan::Frame frame(123, uavcan::TransferTypeServiceRequest, SELF_NODE_ID, 2, 0); + frame.setPayload(reinterpret_cast("123"), 3); + + ASSERT_FALSE(dispatcher.hasPublisher(123)); + ASSERT_FALSE(dispatcher.hasPublisher(456)); + const uavcan::OutgoingTransferRegistryKey otr_key(123, uavcan::TransferTypeMessageBroadcast, 0); + ASSERT_TRUE(dispatcher.getOutgoingTransferRegistry().accessOrCreate(otr_key, + uavcan::MonotonicTime::fromMSec(1000000))); + ASSERT_TRUE(dispatcher.hasPublisher(123)); + ASSERT_FALSE(dispatcher.hasPublisher(456)); + + ASSERT_EQ(2, dispatcher.send(frame, TX_DEADLINE, tsMono(0), uavcan::CanTxQueue::Volatile, 0, 0xFF)); + + /* + * Validation + */ + uavcan::CanFrame expected_can_frame; + ASSERT_TRUE(frame.compile(expected_can_frame)); + + ASSERT_TRUE(driver.ifaces.at(0).matchAndPopTx(expected_can_frame, TX_DEADLINE)); + ASSERT_TRUE(driver.ifaces.at(1).matchAndPopTx(expected_can_frame, TX_DEADLINE)); + + ASSERT_TRUE(driver.ifaces.at(0).tx.empty()); + ASSERT_TRUE(driver.ifaces.at(1).tx.empty()); + + /* + * Perf counters - all empty because dispatcher itself does not count TX transfers + */ + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount()); + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount()); + + /* + * RX listener + */ + ASSERT_TRUE(rx_listener.rx_frames.empty()); +} + + +TEST(Dispatcher, Spin) +{ + NullAllocator poolmgr; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock); + ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID)); // Can be set only once + ASSERT_FALSE(dispatcher.setNodeID(SELF_NODE_ID)); + + clockmock.monotonic_auto_advance = 100; + + ASSERT_EQ(100, clockmock.monotonic); + ASSERT_EQ(0, dispatcher.spin(tsMono(1000))); + ASSERT_LE(1000, clockmock.monotonic); + ASSERT_EQ(0, dispatcher.spinOnce()); + ASSERT_LE(1000, clockmock.monotonic); + ASSERT_EQ(0, dispatcher.spin(tsMono(1100))); + ASSERT_LE(1100, clockmock.monotonic); +} + + +struct DispatcherTestLoopbackFrameListener : public uavcan::LoopbackFrameListenerBase +{ + uavcan::RxFrame last_frame; + unsigned count; + + DispatcherTestLoopbackFrameListener(uavcan::Dispatcher& dispatcher) + : uavcan::LoopbackFrameListenerBase(dispatcher) + , count(0) + { } + + using uavcan::LoopbackFrameListenerBase::startListening; + using uavcan::LoopbackFrameListenerBase::isListening; + + void handleLoopbackFrame(const uavcan::RxFrame& frame) + { + std::cout << "DispatcherTestLoopbackFrameListener: " << frame.toString() << std::endl; + last_frame = frame; + count++; + } +}; + +TEST(Dispatcher, Loopback) +{ + NullAllocator poolmgr; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock); + ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID)); + + { + DispatcherTestLoopbackFrameListener listener(dispatcher); + ASSERT_FALSE(listener.isListening()); + listener.startListening(); + ASSERT_TRUE(listener.isListening()); + + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false + uavcan::Frame frame(123, uavcan::TransferTypeServiceResponse, SELF_NODE_ID, 2, 0); + frame.setPayload(reinterpret_cast("123"), 3); + + ASSERT_TRUE(listener.last_frame == uavcan::RxFrame()); + + ASSERT_LE(0, dispatcher.send(frame, tsMono(1000), tsMono(0), uavcan::CanTxQueue::Persistent, + uavcan::CanIOFlagLoopback, 0xFF)); + + ASSERT_EQ(0, dispatcher.spin(tsMono(1000))); + + ASSERT_TRUE(listener.last_frame != uavcan::RxFrame()); + ASSERT_TRUE(listener.last_frame == frame); + ASSERT_EQ(1, listener.last_frame.getIfaceIndex()); // Last iface + ASSERT_EQ(2, listener.count); + + ASSERT_EQ(1, dispatcher.getLoopbackFrameListenerRegistry().getNumListeners()); + } + ASSERT_EQ(0, dispatcher.getLoopbackFrameListenerRegistry().getNumListeners()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/frame.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/frame.cpp new file mode 100644 index 000000000000..522ad42f9d17 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/frame.cpp @@ -0,0 +1,389 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include "../clock.hpp" +#include "can/can.hpp" + + +TEST(Frame, MessageParseCompile) +{ + using uavcan::Frame; + using uavcan::CanFrame; + using uavcan::TransferID; + using uavcan::TransferType; + + Frame frame; + + /* + * Priority + * Message Type ID + * Service Not Message + * Source Node ID + */ + const uint32_t can_id = + (16 << 24) | // Priority + (20000 << 8) | // Message Type ID + (0 << 7) | // Service Not Message + (42 << 0); // Source Node ID + + const std::string payload_string = "hello\xD4"; // SET = 110, TID = 20 + + /* + * Parse + */ + // Invalid CAN frames + ASSERT_FALSE(frame.parse(CanFrame(can_id | CanFrame::FlagRTR, reinterpret_cast(""), 0))); + ASSERT_FALSE(frame.parse(makeCanFrame(can_id, payload_string, STD))); + + // Valid + ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); + + EXPECT_EQ(TransferID(20), frame.getTransferID()); + EXPECT_TRUE(frame.isStartOfTransfer()); + EXPECT_TRUE(frame.isEndOfTransfer()); + EXPECT_FALSE(frame.getToggle()); + EXPECT_EQ(uavcan::NodeID(42), frame.getSrcNodeID()); + EXPECT_TRUE(frame.getDstNodeID().isBroadcast()); + EXPECT_EQ(uavcan::TransferTypeMessageBroadcast, frame.getTransferType()); + EXPECT_EQ(20000, frame.getDataTypeID().get()); + EXPECT_EQ(16, frame.getPriority().get()); + + EXPECT_EQ(payload_string.length() - 1, frame.getPayloadLen()); + EXPECT_TRUE(std::equal(frame.getPayloadPtr(), frame.getPayloadPtr() + frame.getPayloadLen(), + payload_string.begin())); + + std::cout << frame.toString() << std::endl; + + /* + * Compile + */ + CanFrame can_frame; + ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); + + ASSERT_TRUE(frame.compile(can_frame)); + ASSERT_EQ(can_frame, makeCanFrame(can_id, payload_string, EXT)); + + EXPECT_EQ(payload_string.length(), can_frame.dlc); + std::cout << can_frame.toString() << std::endl; + /* + * FUN FACT: comparison of uint8_t with char may fail on the character 0xD4 (depending on the locale), + * because it will be considered a Unicode character. Hence, we do reinterpret_cast<>. + */ + EXPECT_TRUE(std::equal(can_frame.data, can_frame.data + can_frame.dlc, + reinterpret_cast(&payload_string[0]))); + + /* + * Comparison + */ + ASSERT_FALSE(Frame() == frame); + ASSERT_TRUE(Frame() != frame); + frame = Frame(); + ASSERT_TRUE(Frame() == frame); + ASSERT_FALSE(Frame() != frame); +} + + +TEST(Frame, ServiceParseCompile) +{ + using uavcan::Frame; + using uavcan::CanFrame; + using uavcan::TransferID; + using uavcan::TransferType; + + Frame frame; + + /* + * Priority + * Service Type ID + * Request Not Response + * Destination Node ID + * Service Not Message + * Source Node ID + */ + const uint32_t can_id = + (31 << 24) | // Priority + (200 << 16) | // Service Type ID + (1 << 15) | // Request Not Response + (0x42 << 8) | // Destination Node ID + (1 << 7) | // Service Not Message + (42 << 0); // Source Node ID + + const std::string payload_string = "hello\x6a"; // SET = 011, TID = 10 + + /* + * Parse + */ + // Invalid CAN frames + ASSERT_FALSE(frame.parse(CanFrame(can_id | CanFrame::FlagRTR, reinterpret_cast(""), 0))); + ASSERT_FALSE(frame.parse(makeCanFrame(can_id, payload_string, STD))); + + // Valid + ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); + + EXPECT_EQ(TransferID(10), frame.getTransferID()); + EXPECT_FALSE(frame.isStartOfTransfer()); + EXPECT_TRUE(frame.isEndOfTransfer()); + EXPECT_TRUE(frame.getToggle()); + EXPECT_EQ(uavcan::NodeID(42), frame.getSrcNodeID()); + EXPECT_EQ(uavcan::NodeID(0x42), frame.getDstNodeID()); + EXPECT_EQ(uavcan::TransferTypeServiceRequest, frame.getTransferType()); + EXPECT_EQ(200, frame.getDataTypeID().get()); + EXPECT_EQ(31, frame.getPriority().get()); + + EXPECT_EQ(payload_string.length(), frame.getPayloadLen() + 1); + EXPECT_TRUE(std::equal(frame.getPayloadPtr(), frame.getPayloadPtr() + frame.getPayloadLen(), + reinterpret_cast(&payload_string[0]))); + + std::cout << frame.toString() << std::endl; + + /* + * Compile + */ + CanFrame can_frame; + ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); + + ASSERT_TRUE(frame.compile(can_frame)); + ASSERT_EQ(can_frame, makeCanFrame(can_id, payload_string, EXT)); + + EXPECT_EQ(payload_string.length(), can_frame.dlc); + EXPECT_TRUE(std::equal(can_frame.data, can_frame.data + can_frame.dlc, + reinterpret_cast(&payload_string[0]))); + + /* + * Comparison + */ + ASSERT_FALSE(Frame() == frame); + ASSERT_TRUE(Frame() != frame); + frame = Frame(); + ASSERT_TRUE(Frame() == frame); + ASSERT_FALSE(Frame() != frame); +} + + +TEST(Frame, AnonymousParseCompile) +{ + using uavcan::Frame; + using uavcan::CanFrame; + using uavcan::TransferID; + using uavcan::TransferType; + + Frame frame; + + /* + * Priority + * Discriminator + * Message Type ID + * Service Not Message + * Source Node ID + */ + const uint32_t can_id = + (16383 << 10) | // Discriminator + (1 << 8); // Message Type ID + + const std::string payload_string = "hello\xd4"; // SET = 110, TID = 20 + + uavcan::TransferCRC payload_crc; + payload_crc.add(reinterpret_cast(payload_string.c_str()), unsigned(payload_string.length())); + + /* + * Parse + */ + ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); + + EXPECT_EQ(TransferID(20), frame.getTransferID()); + EXPECT_TRUE(frame.isStartOfTransfer()); + EXPECT_TRUE(frame.isEndOfTransfer()); + EXPECT_FALSE(frame.getToggle()); + EXPECT_TRUE(frame.getSrcNodeID().isBroadcast()); + EXPECT_TRUE(frame.getDstNodeID().isBroadcast()); + EXPECT_EQ(uavcan::TransferTypeMessageBroadcast, frame.getTransferType()); + EXPECT_EQ(1, frame.getDataTypeID().get()); + EXPECT_EQ(0, frame.getPriority().get()); + + EXPECT_EQ(payload_string.length() - 1, frame.getPayloadLen()); + EXPECT_TRUE(std::equal(frame.getPayloadPtr(), frame.getPayloadPtr() + frame.getPayloadLen(), + reinterpret_cast(&payload_string[0]))); + + std::cout << frame.toString() << std::endl; + + /* + * Compile + */ + const uint32_t DiscriminatorMask = 0x00FFFC00; + const uint32_t NoDiscriminatorMask = 0xFF0003FF; + + CanFrame can_frame; + ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); + + ASSERT_TRUE(frame.compile(can_frame)); + ASSERT_EQ(can_id & NoDiscriminatorMask & uavcan::CanFrame::MaskExtID, + can_frame.id & NoDiscriminatorMask & uavcan::CanFrame::MaskExtID); + + EXPECT_EQ(payload_string.length(), can_frame.dlc); + EXPECT_TRUE(std::equal(can_frame.data, can_frame.data + can_frame.dlc, + reinterpret_cast(&payload_string[0]))); + + EXPECT_EQ((can_frame.id & DiscriminatorMask & uavcan::CanFrame::MaskExtID) >> 10, payload_crc.get() & 16383); + + /* + * Comparison + */ + ASSERT_FALSE(Frame() == frame); + ASSERT_TRUE(Frame() != frame); + frame = Frame(); + ASSERT_TRUE(Frame() == frame); + ASSERT_FALSE(Frame() != frame); +} + + +TEST(Frame, FrameParsing) +{ + using uavcan::Frame; + using uavcan::CanFrame; + using uavcan::NodeID; + using uavcan::TransferID; + + CanFrame can; + Frame frame; + ASSERT_FALSE(frame.parse(can)); + + for (unsigned i = 0; i < sizeof(CanFrame::data); i++) + { + can.data[i] = uint8_t(i | (i << 4)); + } + + /* + * Message CAN ID fields and offsets: + * 24 Priority + * 8 Message Type ID + * 7 Service Not Message (0) + * 0 Source Node ID + * + * Service CAN ID fields and offsets: + * 24 Priority + * 16 Service Type ID + * 15 Request Not Response + * 8 Destination Node ID + * 7 Service Not Message (1) + * 0 Source Node ID + */ + + /* + * SFT message broadcast + */ + can.id = CanFrame::FlagEFF | + (2 << 24) | + (456 << 8) | + (0 << 7) | + (42 << 0); + + can.data[7] = 0xcf; // SET=110, TID=0 + + ASSERT_FALSE(frame.parse(can)); + can.dlc = 8; + + ASSERT_TRUE(frame.parse(can)); + EXPECT_TRUE(frame.isStartOfTransfer()); + EXPECT_TRUE(frame.isEndOfTransfer()); + EXPECT_FALSE(frame.getToggle()); + ASSERT_EQ(2, frame.getPriority().get()); + ASSERT_EQ(NodeID(42), frame.getSrcNodeID()); + ASSERT_EQ(NodeID::Broadcast, frame.getDstNodeID()); + ASSERT_EQ(456, frame.getDataTypeID().get()); + ASSERT_EQ(TransferID(15), frame.getTransferID()); + ASSERT_EQ(uavcan::TransferTypeMessageBroadcast, frame.getTransferType()); + + // TODO: test service frames + // TODO: test malformed frames +} + + +TEST(Frame, RxFrameParse) +{ + using uavcan::Frame; + using uavcan::RxFrame; + using uavcan::CanFrame; + using uavcan::CanRxFrame; + + CanRxFrame can_rx_frame; + RxFrame rx_frame; + + // Failure + ASSERT_FALSE(rx_frame.parse(can_rx_frame)); + + // Valid + can_rx_frame.ts_mono = uavcan::MonotonicTime::fromUSec(1); // Zero is not allowed + can_rx_frame.id = CanFrame::FlagEFF | + (2 << 24) | + (456 << 8) | + (0 << 7) | + (42 << 0); + + ASSERT_FALSE(rx_frame.parse(can_rx_frame)); + + can_rx_frame.data[0] = 0xc0; // SOT, EOT + can_rx_frame.dlc = 1; + + ASSERT_TRUE(rx_frame.parse(can_rx_frame)); + ASSERT_EQ(1, rx_frame.getMonotonicTimestamp().toUSec()); + ASSERT_EQ(0, rx_frame.getIfaceIndex()); + + can_rx_frame.ts_mono = tsMono(123); + can_rx_frame.iface_index = 2; + + Frame frame(456, uavcan::TransferTypeMessageBroadcast, 1, uavcan::NodeID::Broadcast, 0); + ASSERT_TRUE(frame.compile(can_rx_frame)); + + ASSERT_TRUE(rx_frame.parse(can_rx_frame)); + ASSERT_EQ(123, rx_frame.getMonotonicTimestamp().toUSec()); + ASSERT_EQ(2, rx_frame.getIfaceIndex()); + ASSERT_EQ(456, rx_frame.getDataTypeID().get()); + ASSERT_EQ(uavcan::TransferTypeMessageBroadcast, rx_frame.getTransferType()); +} + + +TEST(Frame, FrameToString) +{ + using uavcan::Frame; + using uavcan::RxFrame; + + // RX frame default + RxFrame rx_frame; + EXPECT_EQ("prio=255 dtid=65535 tt=3 snid=255 dnid=255 sot=0 eot=0 togl=0 tid=0 payload=[] ts_m=0.000000 ts_utc=0.000000 iface=0", + rx_frame.toString()); + + // RX frame max len + rx_frame = RxFrame(Frame(uavcan::DataTypeID::MaxPossibleDataTypeIDValue, uavcan::TransferTypeMessageBroadcast, + uavcan::NodeID::Max, 0, uavcan::TransferID::Max), + uavcan::MonotonicTime::getMax(), uavcan::UtcTime::getMax(), 3); + + uint8_t data[8]; + for (unsigned i = 0; i < sizeof(data); i++) + { + data[i] = uint8_t(i); + } + rx_frame.setPayload(data, sizeof(data)); + + rx_frame.setStartOfTransfer(true); + rx_frame.setEndOfTransfer(true); + rx_frame.flipToggle(); + rx_frame.setPriority(uavcan::TransferPriority::NumericallyMax); + + EXPECT_EQ("prio=31 dtid=65535 tt=2 snid=127 dnid=0 sot=1 eot=1 togl=1 tid=31 payload=[00 01 02 03 04 05 06] " + "ts_m=18446744073709.551615 ts_utc=18446744073709.551615 iface=3", + rx_frame.toString()); + + // Plain frame default + Frame frame; + EXPECT_EQ("prio=255 dtid=65535 tt=3 snid=255 dnid=255 sot=0 eot=0 togl=0 tid=0 payload=[]", frame.toString()); + + // Plain frame max len + frame = rx_frame; + EXPECT_EQ("prio=31 dtid=65535 tt=2 snid=127 dnid=0 sot=1 eot=1 togl=1 tid=31 payload=[00 01 02 03 04 05 06]", + frame.toString()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/incoming_transfer.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/incoming_transfer.cpp new file mode 100644 index 000000000000..3e1f55bc58d3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/incoming_transfer.cpp @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "../clock.hpp" +#include "transfer_test_helpers.hpp" + + +static uavcan::RxFrame makeFrame() +{ + uavcan::RxFrame frame(uavcan::Frame(123, uavcan::TransferTypeMessageBroadcast, 1, uavcan::NodeID::Broadcast, 0), + tsMono(123), tsUtc(456), 0); + uint8_t data[8]; + for (uint8_t i = 0; i < sizeof(data); i++) + { + data[i] = i; + } + frame.setPayload(data, sizeof(data)); + frame.setEndOfTransfer(true); + return frame; +} + + +static bool match(const uavcan::IncomingTransfer& it, const uavcan::RxFrame& frame, + const uint8_t* payload, unsigned payload_len) +{ + // Fields extracted from the frame struct + EXPECT_EQ(it.getMonotonicTimestamp(), frame.getMonotonicTimestamp()); + EXPECT_EQ(it.getUtcTimestamp(), frame.getUtcTimestamp()); + EXPECT_EQ(it.getSrcNodeID(), frame.getSrcNodeID()); + EXPECT_EQ(it.getTransferID(), frame.getTransferID()); + EXPECT_EQ(it.getTransferType(), frame.getTransferType()); + + // Payload comparison + static const unsigned BUFLEN = 1024; + uint8_t buf_reference[BUFLEN], buf_actual[BUFLEN]; + + if (payload_len > BUFLEN) + { + std::cout << "match(): Payload is too long" << std::endl; + exit(1); + } + + std::fill(buf_reference, buf_reference + BUFLEN, 0); + std::fill(buf_actual, buf_actual + BUFLEN, 0); + std::copy(payload, payload + payload_len, buf_reference); + + EXPECT_EQ(payload_len, it.read(0, buf_actual, payload_len * 3)); + EXPECT_EQ(0, it.read(payload_len, buf_actual, payload_len * 3)); + + return std::equal(buf_reference, buf_reference + BUFLEN, buf_actual); +} + + +TEST(SingleFrameIncomingTransfer, Basic) +{ + using uavcan::RxFrame; + using uavcan::SingleFrameIncomingTransfer; + + const RxFrame frame = makeFrame(); + SingleFrameIncomingTransfer it(frame); + + ASSERT_TRUE(match(it, frame, frame.getPayloadPtr(), frame.getPayloadLen())); +} + + +TEST(MultiFrameIncomingTransfer, Basic) +{ + using uavcan::RxFrame; + using uavcan::MultiFrameIncomingTransfer; + + uavcan::PoolAllocator poolmgr; + uavcan::TransferBufferManager bufmgr(256, poolmgr); + + const RxFrame frame = makeFrame(); + uavcan::TransferBufferManagerKey bufmgr_key(frame.getSrcNodeID(), frame.getTransferType()); + uavcan::TransferBufferAccessor tba(bufmgr, bufmgr_key); + + MultiFrameIncomingTransfer it(frame.getMonotonicTimestamp(), frame.getUtcTimestamp(), frame, tba); + + /* + * Empty read must fail + */ + uint8_t data_byte = 0; + ASSERT_GT(0, it.read(0, &data_byte, 1)); // Error - no such buffer + + /* + * Filling the test data + */ + const std::string data = "123Hello world"; + const uint8_t* const data_ptr = reinterpret_cast(data.c_str()); + ASSERT_FALSE(bufmgr.access(bufmgr_key)); + ASSERT_TRUE(bufmgr.create(bufmgr_key)); + ASSERT_EQ(data.length(), bufmgr.access(bufmgr_key)->write(0, data_ptr, unsigned(data.length()))); + + /* + * Check + */ + ASSERT_TRUE(match(it, frame, data_ptr, unsigned(data.length()))); + + /* + * Buffer release + */ + ASSERT_TRUE(bufmgr.access(bufmgr_key)); + it.release(); + ASSERT_FALSE(bufmgr.access(bufmgr_key)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/outgoing_transfer_registry.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/outgoing_transfer_registry.cpp new file mode 100644 index 000000000000..393a0abb7f0d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/outgoing_transfer_registry.cpp @@ -0,0 +1,90 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "../clock.hpp" +#include "transfer_test_helpers.hpp" + + +TEST(OutgoingTransferRegistry, Basic) +{ + using uavcan::OutgoingTransferRegistryKey; + uavcan::PoolAllocator poolmgr; + uavcan::OutgoingTransferRegistry otr(poolmgr); + + otr.cleanup(tsMono(1000)); + + static const int NUM_KEYS = 5; + const OutgoingTransferRegistryKey keys[NUM_KEYS] = + { + OutgoingTransferRegistryKey(123, uavcan::TransferTypeServiceRequest, 42), + OutgoingTransferRegistryKey(321, uavcan::TransferTypeMessageBroadcast, 0), + OutgoingTransferRegistryKey(213, uavcan::TransferTypeServiceRequest, 2), + OutgoingTransferRegistryKey(312, uavcan::TransferTypeServiceRequest, 4), + OutgoingTransferRegistryKey(456, uavcan::TransferTypeServiceRequest, 2) + }; + + ASSERT_EQ(0, otr.accessOrCreate(keys[0], tsMono(1000000))->get()); + ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(1000000))->get()); + ASSERT_EQ(0, otr.accessOrCreate(keys[2], tsMono(1000000))->get()); + ASSERT_EQ(0, otr.accessOrCreate(keys[3], tsMono(1000000))->get()); + ASSERT_FALSE(otr.accessOrCreate(keys[4], tsMono(1000000))); // OOM + + /* + * Incrementing a little + */ + otr.accessOrCreate(keys[0], tsMono(2000000))->increment(); + otr.accessOrCreate(keys[0], tsMono(4000000))->increment(); + otr.accessOrCreate(keys[0], tsMono(3000000))->increment(); + ASSERT_EQ(3, otr.accessOrCreate(keys[0], tsMono(5000000))->get()); + + otr.accessOrCreate(keys[2], tsMono(2000000))->increment(); + otr.accessOrCreate(keys[2], tsMono(3000000))->increment(); + ASSERT_EQ(2, otr.accessOrCreate(keys[2], tsMono(6000000))->get()); + + otr.accessOrCreate(keys[3], tsMono(9000000))->increment(); + ASSERT_EQ(1, otr.accessOrCreate(keys[3], tsMono(4000000))->get()); + + ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(4000000))->get()); + + ASSERT_FALSE(otr.accessOrCreate(keys[4], tsMono(1000000))); // Still OOM + + /* + * Checking existence + * Exist: 0, 1, 2, 3 + * Does not exist: 4 + */ + ASSERT_TRUE(otr.exists(keys[1].getDataTypeID(), keys[1].getTransferType())); + ASSERT_TRUE(otr.exists(keys[0].getDataTypeID(), keys[0].getTransferType())); + ASSERT_TRUE(otr.exists(keys[3].getDataTypeID(), keys[3].getTransferType())); + ASSERT_TRUE(otr.exists(keys[2].getDataTypeID(), keys[2].getTransferType())); + + ASSERT_FALSE(otr.exists(keys[1].getDataTypeID(), keys[2].getTransferType())); // Invalid combination + ASSERT_FALSE(otr.exists(keys[0].getDataTypeID(), keys[1].getTransferType())); // Invalid combination + ASSERT_FALSE(otr.exists(keys[4].getDataTypeID(), keys[4].getTransferType())); // Plain missing + + /* + * Cleaning up + */ + otr.cleanup(tsMono(4000001)); // Kills 1, 3 + ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(1000000))->get()); + ASSERT_EQ(0, otr.accessOrCreate(keys[3], tsMono(1000000))->get()); + otr.accessOrCreate(keys[1], tsMono(5000000))->increment(); + otr.accessOrCreate(keys[3], tsMono(5000000))->increment(); + + ASSERT_EQ(3, otr.accessOrCreate(keys[0], tsMono(5000000))->get()); + ASSERT_EQ(2, otr.accessOrCreate(keys[2], tsMono(6000000))->get()); + + otr.cleanup(tsMono(5000001)); // Kills 1, 3 (He needs a bath, Jud. He stinks of the ground you buried him in.), 0 + ASSERT_EQ(0, otr.accessOrCreate(keys[0], tsMono(1000000))->get()); + ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(1000000))->get()); + ASSERT_EQ(0, otr.accessOrCreate(keys[3], tsMono(1000000))->get()); + + ASSERT_EQ(2, otr.accessOrCreate(keys[2], tsMono(1000000))->get()); + + otr.cleanup(tsMono(5000001)); // Frees some memory for 4 + ASSERT_EQ(0, otr.accessOrCreate(keys[0], tsMono(1000000))->get()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer.cpp new file mode 100644 index 000000000000..db630b7770f9 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer.cpp @@ -0,0 +1,93 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + + +TEST(Transfer, TransferID) +{ + using uavcan::TransferID; + + // Tests below are based on this assumption + ASSERT_EQ(32, 1 << TransferID::BitLen); + + /* + * forwardDistance() + */ + EXPECT_EQ(0, TransferID(0).computeForwardDistance(0)); + EXPECT_EQ(1, TransferID(0).computeForwardDistance(1)); + EXPECT_EQ(7, TransferID(0).computeForwardDistance(7)); + + EXPECT_EQ(0, TransferID(7).computeForwardDistance(7)); + EXPECT_EQ(31,TransferID(31).computeForwardDistance(30)); + EXPECT_EQ(1, TransferID(31).computeForwardDistance(0)); + + EXPECT_EQ(30,TransferID(7).computeForwardDistance(5)); + EXPECT_EQ(5, TransferID(0).computeForwardDistance(5)); + + /* + * Misc + */ + EXPECT_TRUE(TransferID(2) == TransferID(2)); + EXPECT_FALSE(TransferID(2) != TransferID(2)); + EXPECT_FALSE(TransferID(2) == TransferID(0)); + EXPECT_TRUE(TransferID(2) != TransferID(0)); + + TransferID tid; + for (int i = 0; i < 999; i++) + { + ASSERT_EQ(i & ((1 << TransferID::BitLen) - 1), tid.get()); + const TransferID copy = tid; + tid.increment(); + ASSERT_EQ(1, copy.computeForwardDistance(tid)); + ASSERT_EQ(31, tid.computeForwardDistance(copy)); + ASSERT_EQ(0, tid.computeForwardDistance(tid)); + } +} + + +TEST(Transfer, NodeID) +{ + uavcan::NodeID nid1(1); + uavcan::NodeID nid127(127); + uavcan::NodeID nid0(0); + uavcan::NodeID nidx; + + ASSERT_TRUE(nid1.isUnicast()); + ASSERT_FALSE(nid1.isBroadcast()); + ASSERT_TRUE(nid1.isValid()); + + ASSERT_TRUE(nid127.isUnicast()); + ASSERT_FALSE(nid127.isBroadcast()); + ASSERT_TRUE(nid127.isValid()); + + ASSERT_FALSE(nid0.isUnicast()); + ASSERT_TRUE(nid0.isBroadcast()); + ASSERT_TRUE(nid0.isValid()); + + ASSERT_FALSE(nidx.isUnicast()); + ASSERT_FALSE(nidx.isBroadcast()); + ASSERT_FALSE(nidx.isValid()); + + /* + * Comparison operators + */ + ASSERT_TRUE(nid1 < nid127); + ASSERT_TRUE(nid1 <= nid127); + ASSERT_TRUE(nid0 < nid1); + ASSERT_TRUE(nid0 <= nid1); + + ASSERT_FALSE(nid1 > nid127); + ASSERT_FALSE(nid1 >= nid127); + ASSERT_FALSE(nid0 > nid1); + ASSERT_FALSE(nid0 >= nid1); + + ASSERT_FALSE(nid1 > uavcan::NodeID(1)); + ASSERT_TRUE(nid1 >= uavcan::NodeID(1)); + + ASSERT_FALSE(nid1 == nid127); + ASSERT_TRUE(nid127 == uavcan::NodeID(127)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_buffer.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_buffer.cpp new file mode 100644 index 000000000000..1a4ac031fb77 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_buffer.cpp @@ -0,0 +1,318 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#include +#include +#include +#include + +static const std::string TEST_DATA = + "It was like this: I asked myself one day this question - what if Napoleon, for instance, had happened to be in my " + "place, and if he had not had Toulon nor Egypt nor the passage of Mont Blanc to begin his career with, but " + "instead of all those picturesque and monumental things, there had simply been some ridiculous old hag, a " + "pawnbroker, who had to be murdered too to get money from her trunk (for his career, you understand). " + "Well, would he have brought himself to that if there had been no other means?"; + +template +static bool allEqual(const T (&a)[Size]) +{ + unsigned n = Size; + while ((--n > 0) && (a[n] == a[0])) { } + return n == 0; +} + +template +static void fill(T (&a)[Size], R value) +{ + for (unsigned i = 0; i < Size; i++) + { + a[i] = T(value); + } +} + +static bool matchAgainst(const std::string& data, const uavcan::ITransferBuffer& tbb, + unsigned offset = 0, int len = -1) +{ + uint8_t local_buffer[1024]; + fill(local_buffer, 0); + assert((len < 0) || (sizeof(local_buffer) >= static_cast(len))); + + if (len < 0) + { + const int res = tbb.read(offset, local_buffer, sizeof(local_buffer)); + if (res < 0) + { + std::cout << "matchAgainst(): res " << res << std::endl; + return false; + } + len = res; + } + else + { + const int res = tbb.read(offset, local_buffer, unsigned(len)); + if (res != len) + { + std::cout << "matchAgainst(): res " << res << " expected " << len << std::endl; + return false; + } + } + const bool equals = std::equal(local_buffer, local_buffer + len, data.begin() + offset); + if (!equals) + { + std::cout << "local_buffer:\n\t" << local_buffer << std::endl; + std::cout << "test_data:\n\t" << std::string(data.begin() + offset, data.begin() + offset + len) << std::endl; + } + return equals; +} + +static bool matchAgainstTestData(const uavcan::ITransferBuffer& tbb, unsigned offset, int len = -1) +{ + return matchAgainst(TEST_DATA, tbb, offset, len); +} + +TEST(TransferBuffer, TestDataValidation) +{ + ASSERT_LE(4, TEST_DATA.length() / uavcan::MemPoolBlockSize); + uint8_t local_buffer[50]; + std::copy(TEST_DATA.begin(), TEST_DATA.begin() + sizeof(local_buffer), local_buffer); + ASSERT_FALSE(allEqual(local_buffer)); +} + +static const int TEST_BUFFER_SIZE = 200; + +TEST(StaticTransferBuffer, Basic) +{ + using uavcan::StaticTransferBuffer; + StaticTransferBuffer buf; + + uint8_t local_buffer[TEST_BUFFER_SIZE * 2]; + const uint8_t* const test_data_ptr = reinterpret_cast(TEST_DATA.c_str()); + + // Empty reads + fill(local_buffer, 0xA5); + ASSERT_EQ(0, buf.read(0, local_buffer, 999)); + ASSERT_EQ(0, buf.read(0, local_buffer, 0)); + ASSERT_EQ(0, buf.read(999, local_buffer, 0)); + ASSERT_TRUE(allEqual(local_buffer)); + + // Bulk write + ASSERT_EQ(TEST_BUFFER_SIZE, buf.write(0, test_data_ptr, unsigned(TEST_DATA.length()))); + ASSERT_TRUE(matchAgainstTestData(buf, 0)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2, TEST_BUFFER_SIZE / 4)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 4, TEST_BUFFER_SIZE / 2)); + ASSERT_TRUE(matchAgainstTestData(buf, 0, TEST_BUFFER_SIZE / 4)); + + // Reset + fill(local_buffer, 0xA5); + buf.reset(); + ASSERT_EQ(0, buf.read(0, local_buffer, 0)); + ASSERT_EQ(0, buf.read(0, local_buffer, 999)); + ASSERT_TRUE(allEqual(local_buffer)); + + // Random write + ASSERT_EQ(21, buf.write(12, test_data_ptr + 12, 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 12, 21)); + + ASSERT_EQ(12, buf.write(0, test_data_ptr, 12)); + ASSERT_TRUE(matchAgainstTestData(buf, 0)); + + ASSERT_EQ(0, buf.write(21, test_data_ptr + 21, 0)); + ASSERT_EQ(TEST_BUFFER_SIZE - 21, buf.write(21, test_data_ptr + 21, 999)); + ASSERT_TRUE(matchAgainstTestData(buf, 21, TEST_BUFFER_SIZE - 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 0)); +} + + +TEST(TransferBufferManagerEntry, Basic) +{ + using uavcan::TransferBufferManagerEntry; + + static const int MAX_SIZE = TEST_BUFFER_SIZE; + static const int POOL_BLOCKS = 8; + uavcan::PoolAllocator pool; + + TransferBufferManagerEntry buf(pool, MAX_SIZE); + + uint8_t local_buffer[TEST_BUFFER_SIZE * 2]; + const uint8_t* const test_data_ptr = reinterpret_cast(TEST_DATA.c_str()); + + // Empty reads + fill(local_buffer, 0xA5); + ASSERT_EQ(0, buf.read(0, local_buffer, 999)); + ASSERT_EQ(0, buf.read(0, local_buffer, 0)); + ASSERT_EQ(0, buf.read(999, local_buffer, 0)); + ASSERT_TRUE(allEqual(local_buffer)); + + // Bulk write + ASSERT_EQ(MAX_SIZE, buf.write(0, test_data_ptr, unsigned(TEST_DATA.length()))); + + ASSERT_LT(0, pool.getNumUsedBlocks()); // Making sure some memory was used + + ASSERT_TRUE(matchAgainstTestData(buf, 0)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2, TEST_BUFFER_SIZE / 4)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 4, TEST_BUFFER_SIZE / 2)); + ASSERT_TRUE(matchAgainstTestData(buf, 0, TEST_BUFFER_SIZE / 4)); + + // Reset + fill(local_buffer, 0xA5); + buf.reset(); + ASSERT_EQ(0, buf.read(0, local_buffer, 0)); + ASSERT_EQ(0, buf.read(0, local_buffer, 999)); + ASSERT_TRUE(allEqual(local_buffer)); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + + // Random write + ASSERT_EQ(21, buf.write(12, test_data_ptr + 12, 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 12, 21)); + + ASSERT_EQ(60, buf.write(TEST_BUFFER_SIZE - 60, test_data_ptr + TEST_BUFFER_SIZE - 60, 60)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE - 60)); + + // Now we have two empty regions: empty-data-empty-data + + ASSERT_EQ(0, buf.write(0, test_data_ptr, 0)); + ASSERT_EQ(TEST_BUFFER_SIZE - 21, buf.write(21, test_data_ptr + 21, TEST_BUFFER_SIZE - 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 21, TEST_BUFFER_SIZE - 21)); + + // Now: empty-data-data-data + + ASSERT_EQ(21, buf.write(0, test_data_ptr, 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 0)); + + // Destroying the object; memory should be released + ASSERT_LT(0, pool.getNumUsedBlocks()); + buf.~TransferBufferManagerEntry(); + ASSERT_EQ(0, pool.getNumUsedBlocks()); +} + + +static const std::string MGR_TEST_DATA[4] = +{ + "I thought you would cry out again \'don\'t speak of it, leave off.\'\" Raskolnikov gave a laugh, but rather a " + "forced one. \"What, silence again?\" he asked a minute later. \"We must talk about something, you know. ", + + "It would be interesting for me to know how you would decide a certain \'problem\' as Lebeziatnikov would say.\" " + "(He was beginning to lose the thread.) \"No, really, I am serious. Imagine, Sonia, that you had known all ", + + "Luzhin\'s intentions beforehand. Known, that is, for a fact, that they would be the ruin of Katerina Ivanovna " + "and the children and yourself thrown in--since you don\'t count yourself for anything--Polenka too... for ", + + "she\'ll go the same way. Well, if suddenly it all depended on your decision whether he or they should go on " + "living, that is whether Luzhin should go on living and doing wicked things, or Katerina Ivanovna should die? " + "How would you decide which of them was to die? I ask you?" +}; + +static const int MGR_MAX_BUFFER_SIZE = 100; + +TEST(TransferBufferManager, TestDataValidation) +{ + for (unsigned i = 0; i < sizeof(MGR_TEST_DATA) / sizeof(MGR_TEST_DATA[0]); i++) + { + ASSERT_LT(MGR_MAX_BUFFER_SIZE, MGR_TEST_DATA[i].length()); + } +} + + +static int fillTestData(const std::string& data, uavcan::ITransferBuffer* tbb) +{ + return tbb->write(0, reinterpret_cast(data.c_str()), unsigned(data.length())); +} + +TEST(TransferBufferManager, Basic) +{ + using uavcan::TransferBufferManager; + using uavcan::TransferBufferManagerKey; + using uavcan::ITransferBuffer; + + static const int POOL_BLOCKS = 100; + uavcan::PoolAllocator pool; + + std::unique_ptr mgr(new TransferBufferManager(MGR_MAX_BUFFER_SIZE, pool)); + + // Empty + ASSERT_FALSE(mgr->access(TransferBufferManagerKey(0, uavcan::TransferTypeMessageBroadcast))); + ASSERT_FALSE(mgr->access(TransferBufferManagerKey(127, uavcan::TransferTypeServiceRequest))); + + ITransferBuffer* tbb = UAVCAN_NULLPTR; + + const TransferBufferManagerKey keys[5] = + { + TransferBufferManagerKey(0, uavcan::TransferTypeServiceRequest), + TransferBufferManagerKey(1, uavcan::TransferTypeMessageBroadcast), + TransferBufferManagerKey(2, uavcan::TransferTypeServiceRequest), + TransferBufferManagerKey(127, uavcan::TransferTypeServiceResponse), + TransferBufferManagerKey(64, uavcan::TransferTypeMessageBroadcast) + }; + + ASSERT_TRUE((tbb = mgr->create(keys[0]))); + ASSERT_EQ(MGR_MAX_BUFFER_SIZE, fillTestData(MGR_TEST_DATA[0], tbb)); + ASSERT_EQ(1, mgr->getNumBuffers()); + + ASSERT_TRUE((tbb = mgr->create(keys[1]))); + ASSERT_EQ(MGR_MAX_BUFFER_SIZE, fillTestData(MGR_TEST_DATA[1], tbb)); + ASSERT_EQ(2, mgr->getNumBuffers()); + ASSERT_LT(2, pool.getNumUsedBlocks()); + + ASSERT_TRUE((tbb = mgr->create(keys[2]))); + ASSERT_EQ(MGR_MAX_BUFFER_SIZE, fillTestData(MGR_TEST_DATA[2], tbb)); + ASSERT_EQ(3, mgr->getNumBuffers()); + + std::cout << "TransferBufferManager - Basic: Pool usage: " << pool.getNumUsedBlocks() << std::endl; + + ASSERT_TRUE((tbb = mgr->create(keys[3]))); + + ASSERT_LT(0, fillTestData(MGR_TEST_DATA[3], tbb)); + ASSERT_EQ(4, mgr->getNumBuffers()); + + // Making sure all buffers contain proper data + ASSERT_TRUE((tbb = mgr->access(keys[0]))); + ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[0], *tbb)); + + ASSERT_TRUE((tbb = mgr->access(keys[1]))); + ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[1], *tbb)); + + ASSERT_TRUE((tbb = mgr->access(keys[2]))); + ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[2], *tbb)); + + ASSERT_TRUE((tbb = mgr->access(keys[3]))); + ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[3], *tbb)); + + mgr->remove(keys[1]); + ASSERT_FALSE(mgr->access(keys[1])); + ASSERT_EQ(3, mgr->getNumBuffers()); + ASSERT_LT(0, pool.getNumFreeBlocks()); + + mgr->remove(keys[0]); + ASSERT_FALSE(mgr->access(keys[0])); + ASSERT_EQ(2, mgr->getNumBuffers()); + + // At this time we have the following NodeID: 2, 127 + ASSERT_TRUE((tbb = mgr->access(keys[2]))); + ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[2], *tbb)); + + ASSERT_TRUE((tbb = mgr->access(keys[3]))); + ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[3], *tbb)); + + // These were deleted: 0, 1; 3 is still there + ASSERT_FALSE(mgr->access(keys[1])); + ASSERT_FALSE(mgr->access(keys[0])); + ASSERT_TRUE(mgr->access(keys[3])); + + // Filling the memory again in order to check the destruction below + ASSERT_TRUE((tbb = mgr->create(keys[1]))); + ASSERT_LT(0, fillTestData(MGR_TEST_DATA[1], tbb)); + + // Deleting the object; all memory must be freed + ASSERT_NE(0, pool.getNumUsedBlocks()); + mgr.reset(); + ASSERT_EQ(0, pool.getNumUsedBlocks()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_listener.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_listener.cpp new file mode 100644 index 000000000000..cf3806c7f0b4 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_listener.cpp @@ -0,0 +1,268 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include "transfer_test_helpers.hpp" +#include "../clock.hpp" + + +class TransferListenerEmulator : public IncomingTransferEmulatorBase +{ + uavcan::TransferListener& target_; + const uavcan::DataTypeDescriptor data_type_; + +public: + TransferListenerEmulator(uavcan::TransferListener& target, const uavcan::DataTypeDescriptor& type, + uavcan::NodeID dst_node_id = 127) + : IncomingTransferEmulatorBase(dst_node_id) + , target_(target) + , data_type_(type) + { } + + void sendOneFrame(const uavcan::RxFrame& frame) { target_.handleFrame(frame); } + + Transfer makeTransfer(uavcan::TransferPriority priority, uavcan::TransferType transfer_type, + uint8_t source_node_id, const std::string& payload) + { + return IncomingTransferEmulatorBase::makeTransfer(priority, transfer_type, source_node_id, payload, data_type_); + } +}; + + +TEST(TransferListener, BasicMFT) +{ + const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); + + static const int NUM_POOL_BLOCKS = 100; + uavcan::PoolAllocator pool; + + uavcan::TransferPerfCounter perf; + TestListener subscriber(perf, type, 256, pool); + + /* + * Test data + */ + static const std::string DATA[] = + { + "Build a man a fire, and he'll be warm for a day. " + "Set a man on fire, and he'll be warm for the rest of his life.", + + "123456789", + + "In the beginning there was nothing, which exploded.", + + "The USSR, which they'd begun to renovate and improve at about the time when Tatarsky decided to " + "change his profession, improved so much that it ceased to exist", + + "BEWARE JET BLAST" + }; + + for (unsigned i = 0; i < sizeof(DATA) / sizeof(DATA[0]); i++) + { + std::cout << "Size of test data chunk " << i << ": " << DATA[i].length() << std::endl; + } + + TransferListenerEmulator emulator(subscriber, type); + const Transfer transfers[] = + { + emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 1, DATA[0]), + emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 2, DATA[1]), // Same NID + emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 3, DATA[2]), + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 4, DATA[3]), + emulator.makeTransfer(16, uavcan::TransferTypeServiceResponse, 5, DATA[4]), + }; + + /* + * Sending concurrently + * Expected reception order: 1, 4, 2, 0, 3 + */ + emulator.send(transfers); + + ASSERT_TRUE(subscriber.matchAndPop(transfers[1])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[4])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[2])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[0])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[3])); + + ASSERT_TRUE(subscriber.isEmpty()); +} + + +TEST(TransferListener, CrcFailure) +{ + const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); + + static const int NUM_POOL_BLOCKS = 100; + uavcan::PoolAllocator poolmgr; + uavcan::TransferPerfCounter perf; + TestListener subscriber(perf, type, 256, poolmgr); // Static buffer only, 2 entries + + /* + * Generating transfers with damaged payload (CRC is not valid) + */ + TransferListenerEmulator emulator(subscriber, type); + const Transfer tr_mft = emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 42, "123456789abcdefghik"); + const Transfer tr_sft = emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 11, "abcd"); + + std::vector ser_mft = serializeTransfer(tr_mft); + std::vector ser_sft = serializeTransfer(tr_sft); + + ASSERT_TRUE(ser_mft.size() > 1); + ASSERT_TRUE(ser_sft.size() == 1); + + const_cast(ser_mft[1].getPayloadPtr())[1] = uint8_t(~ser_mft[1].getPayloadPtr()[1]); // CRC invalid now + const_cast(ser_sft[0].getPayloadPtr())[2] = uint8_t(~ser_sft[0].getPayloadPtr()[2]); // no CRC here + + /* + * Sending and making sure that MFT was not received, but SFT was. + */ + std::vector > sers; + sers.push_back(ser_mft); + sers.push_back(ser_sft); + sers.push_back(ser_mft); // Ignored + sers.push_back(ser_sft); // Ignored + + emulator.send(sers); + + Transfer tr_sft_damaged = tr_sft; + tr_sft_damaged.payload[2] = char(~tr_sft.payload[2]); // Damaging the data similarly, so that it can be matched + + ASSERT_TRUE(subscriber.matchAndPop(tr_sft_damaged)); + ASSERT_TRUE(subscriber.isEmpty()); +} + + +TEST(TransferListener, BasicSFT) +{ + const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); + + static const int NUM_POOL_BLOCKS = 100; + uavcan::PoolAllocator poolmgr; + uavcan::TransferPerfCounter perf; + TestListener subscriber(perf, type, 0, poolmgr); // Max buf size is 0, i.e. SFT-only + + TransferListenerEmulator emulator(subscriber, type); + const Transfer transfers[] = + { + emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 1, "123"), + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 1, "456"), // Same NID + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 2, ""), + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 3, "abc"), + emulator.makeTransfer(16, uavcan::TransferTypeServiceResponse, 4, ""), + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 2, "foo"), // Same as 2, not ignored + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 2, "123456789abc"), // Same as 2, not SFT - ignore + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 2, "bar"), // Same as 2, not ignored + }; + + emulator.send(transfers); + + ASSERT_TRUE(subscriber.matchAndPop(transfers[0])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[1])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[2])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[3])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[4])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[5])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[7])); + + ASSERT_TRUE(subscriber.isEmpty()); +} + + +TEST(TransferListener, Cleanup) +{ + const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); + + static const int NUM_POOL_BLOCKS = 100; + uavcan::PoolAllocator poolmgr; + uavcan::TransferPerfCounter perf; + TestListener subscriber(perf, type, 256, poolmgr); + + /* + * Generating transfers + */ + TransferListenerEmulator emulator(subscriber, type); + const Transfer tr_mft = emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 42, "123456789abcdefghik"); + const Transfer tr_sft = emulator.makeTransfer(16, uavcan::TransferTypeServiceResponse, 11, "abcd"); + + const std::vector ser_mft = serializeTransfer(tr_mft); + const std::vector ser_sft = serializeTransfer(tr_sft); + + ASSERT_TRUE(ser_mft.size() > 1); + ASSERT_TRUE(ser_sft.size() == 1); + + const std::vector ser_mft_begin(ser_mft.begin(), ser_mft.begin() + 1); + + /* + * Sending the first part and SFT + */ + std::vector > sers; + sers.push_back(ser_mft_begin); // Only the first part + sers.push_back(ser_sft); + + emulator.send(sers); + + ASSERT_TRUE(subscriber.matchAndPop(tr_sft)); + ASSERT_TRUE(subscriber.isEmpty()); + + /* + * Cleanup with huge timestamp value will remove all entries + */ + static_cast(subscriber).cleanup(tsMono(100000000)); + + /* + * Sending the same transfers again - they will be accepted since registres were cleared + */ + sers.clear(); + sers.push_back(ser_mft); // Complete transfer + sers.push_back(ser_sft); + + emulator.send(sers); + + ASSERT_TRUE(subscriber.matchAndPop(tr_sft)); + ASSERT_TRUE(subscriber.matchAndPop(tr_mft)); + ASSERT_TRUE(subscriber.isEmpty()); +} + + +TEST(TransferListener, AnonymousTransfers) +{ + const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); + + static const int NUM_POOL_BLOCKS = 100; + uavcan::PoolAllocator poolmgr; + uavcan::TransferPerfCounter perf; + TestListener subscriber(perf, type, 0, poolmgr); + + TransferListenerEmulator emulator(subscriber, type); + const Transfer transfers[] = + { + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 0, "1234567"), // Invalid - not broadcast + emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 0, "1234567"), // Valid + emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 0, "12345678"), // Invalid - not SFT + emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 0, "") // Valid + }; + + emulator.send(transfers); + + // Nothing will be received, because anonymous transfers are disabled by default + ASSERT_TRUE(subscriber.isEmpty()); + + subscriber.allowAnonymousTransfers(); + + // Re-send everything again + emulator.send(transfers); + + // Now the anonymous transfers are enabled + ASSERT_TRUE(subscriber.matchAndPop(transfers[1])); // Only SFT broadcast will be accepted + ASSERT_TRUE(subscriber.matchAndPop(transfers[3])); + + ASSERT_TRUE(subscriber.isEmpty()); +} + +TEST(TransferListener, Sizes) +{ + using namespace uavcan; + + std::cout << "sizeof(TransferListener): " << sizeof(TransferListener) << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_receiver.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_receiver.cpp new file mode 100644 index 000000000000..dd7e85d1d2b9 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_receiver.cpp @@ -0,0 +1,561 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "../clock.hpp" +#include "transfer_test_helpers.hpp" + +/* + * Beware! + * The code you're about to look at desperately needs some cleaning. + */ + +enum SotEotToggle +{ + SET000 = 0, + SET001 = 1, + SET010 = 2, + SET011 = 3, + SET100 = 4, + SET101 = 5, // Illegal + SET110 = 6, + SET111 = 7 // Illegal +}; + +struct RxFrameGenerator +{ + static const uavcan::TransferBufferManagerKey DEFAULT_KEY; + enum { TARGET_NODE_ID = 126 }; + + uint16_t data_type_id; + uavcan::TransferBufferManagerKey bufmgr_key; + + RxFrameGenerator(uint16_t data_type_id, const uavcan::TransferBufferManagerKey& bufmgr_key = DEFAULT_KEY) + : data_type_id(data_type_id) + , bufmgr_key(bufmgr_key) + { } + + /// iface_index, data, set, transfer_id, ts_monotonic [, ts_utc] + uavcan::RxFrame operator()(uint8_t iface_index, const std::string& data, SotEotToggle set, + uint8_t transfer_id, uint64_t ts_monotonic, uint64_t ts_utc = 0) + { + const uavcan::NodeID dst_nid = + (bufmgr_key.getTransferType() == uavcan::TransferTypeMessageBroadcast) ? + uavcan::NodeID::Broadcast : TARGET_NODE_ID; + + uavcan::Frame frame(data_type_id, bufmgr_key.getTransferType(), bufmgr_key.getNodeID(), + dst_nid, transfer_id); + + frame.setStartOfTransfer((set & (1 << 2)) != 0); + frame.setEndOfTransfer((set & (1 << 1)) != 0); + + if ((set & (1 << 0)) != 0) + { + frame.flipToggle(); + } + + EXPECT_EQ(data.length(), + frame.setPayload(reinterpret_cast(data.c_str()), unsigned(data.length()))); + + uavcan::RxFrame output(frame, uavcan::MonotonicTime::fromUSec(ts_monotonic), + uavcan::UtcTime::fromUSec(ts_utc), iface_index); + //std::cout << "Generated frame: " << output.toString() << std::endl; + + return output; + } +}; + +const uavcan::TransferBufferManagerKey RxFrameGenerator::DEFAULT_KEY(42, uavcan::TransferTypeMessageBroadcast); + + +template +struct Context +{ + uavcan::PoolAllocator pool; + uavcan::TransferReceiver receiver; // Must be default constructible and copyable + uavcan::TransferBufferManager bufmgr; + + Context() : + bufmgr(BufSize, pool) + { } + + ~Context() + { + // We need to destroy the receiver before its buffer manager + receiver = uavcan::TransferReceiver(); + } +}; + + +static bool matchBufferContent(const uavcan::ITransferBuffer* tbb, const std::string& content) +{ + uint8_t data[1024]; + std::fill(data, data + sizeof(data), 0); + if (content.length() > sizeof(data)) + { + std::cerr << "matchBufferContent(): Content is too long" << std::endl; + std::exit(1); + } + tbb->read(0, data, unsigned(content.length())); + if (std::equal(content.begin(), content.end(), data)) + { + return true; + } + std::cout << "Buffer content mismatch:" + << "\n\tExpected: " << content + << "\n\tActually: " << reinterpret_cast(data) + << std::endl; + return false; +} + + +#define CHECK_NOT_COMPLETE(x) ASSERT_EQ(uavcan::TransferReceiver::ResultNotComplete, (x)) +#define CHECK_COMPLETE(x) ASSERT_EQ(uavcan::TransferReceiver::ResultComplete, (x)) +#define CHECK_SINGLE_FRAME(x) ASSERT_EQ(uavcan::TransferReceiver::ResultSingleFrame, (x)) + +TEST(TransferReceiver, Basic) +{ + using uavcan::TransferReceiver; + Context<32> context; + RxFrameGenerator gen(789); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY); + + std::cout << "sizeof(TransferReceiver): " << sizeof(TransferReceiver) << std::endl; + + /* + * Empty + */ + ASSERT_EQ(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); + ASSERT_EQ(0, rcv.getLastTransferTimestampMonotonic().toUSec()); + + /* + * Single frame transfer with zero ts, must be ignored + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "Foo", SET110, 0, 0), bk)); + ASSERT_EQ(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); + ASSERT_EQ(0, rcv.getLastTransferTimestampMonotonic().toUSec()); + + /* + * Valid compound transfer + * Args: iface_index, data, set, transfer_id, ts_monotonic + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "\x34\x12" "34567", SET100, 0, 100), bk)); + CHECK_COMPLETE( rcv.addFrame(gen(0, "foo", SET011, 0, 200), bk)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567foo")); + ASSERT_EQ(0x1234, rcv.getLastTransferCrc()); + ASSERT_EQ(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); // Not initialized yet + ASSERT_EQ(100, rcv.getLastTransferTimestampMonotonic().toUSec()); + + /* + * Compound transfer mixed with invalid frames; buffer was not released explicitly + * Args: iface_index, data, set, transfer_id, ts_monotonic + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", SET100, 0, 300), bk)); // Previous TID, rejected + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "rty", SET100, 0, 300), bk)); // Previous TID, wrong iface + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "\x9a\x78" "34567", SET100, 1, 1000), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwertyu", SET100, 1, 1100), bk)); // Old toggle + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwertyu", SET000, 1, 1100), bk)); // Old toggle + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "abcdefg", SET001, 1, 1200), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "4567891", SET001, 2, 1300), bk)); // Next TID, but not SOT + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", SET010, 1, 1300), bk)); // Wrong iface + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "", SET001, 1, 1300), bk)); // Unexpected toggle + CHECK_COMPLETE( rcv.addFrame(gen(0, "", SET010, 1, 1300), bk)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567abcdefg")); + ASSERT_EQ(0x789A, rcv.getLastTransferCrc()); + ASSERT_GE(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); + ASSERT_LE(TransferReceiver::getMinTransferInterval(), rcv.getInterval()); + ASSERT_EQ(1000, rcv.getLastTransferTimestampMonotonic().toUSec()); + ASSERT_FALSE(rcv.isTimedOut(tsMono(1000))); + ASSERT_FALSE(rcv.isTimedOut(tsMono(5000))); + ASSERT_TRUE(rcv.isTimedOut(tsMono(60000000))); + + /* + * Single-frame transfers + * Args: iface_index, data, set, transfer_id, ts_monotonic + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", SET110, 1, 2000), bk)); // Previous TID + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", SET110, 2, 2100), bk)); // Wrong iface + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", SET110, 2, 2200), bk)); + + ASSERT_FALSE(bufmgr.access(gen.bufmgr_key)); // Buffer must be removed + ASSERT_GT(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); + ASSERT_EQ(2200, rcv.getLastTransferTimestampMonotonic().toUSec()); + + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 3, 2500), bk)); + ASSERT_EQ(2500, rcv.getLastTransferTimestampMonotonic().toUSec()); + + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 0, 3000), bk)); + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 1, 3100), bk)); + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 3, 3200), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", SET110, 0, 3300), bk)); // Old TID, wrong iface + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", SET110, 2, 3400), bk)); // Old TID, wrong iface + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", SET110, 3, 3500), bk)); // Old TID, wrong iface + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 4, 3600), bk)); + ASSERT_EQ(3600, rcv.getLastTransferTimestampMonotonic().toUSec()); + + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + + /* + * Timeouts + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", SET110, 1, 5000), bk)); // Wrong iface - ignored + CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", SET110, 6, 1500000), bk)); // Accepted due to iface timeout + ASSERT_EQ(1500000, rcv.getLastTransferTimestampMonotonic().toUSec()); + + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", SET110, 7, 1500100), bk)); // Ignored - old iface 0 + CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", SET110, 7, 1500100), bk)); + ASSERT_EQ(1500100, rcv.getLastTransferTimestampMonotonic().toUSec()); + + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", SET110, 7, 1500100), bk)); // Old TID + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", SET110, 7, 100000000), bk)); // Accepted - global timeout + ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec()); + + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", SET110, 8, 100000100), bk)); + ASSERT_EQ(100000100, rcv.getLastTransferTimestampMonotonic().toUSec()); + + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + + ASSERT_TRUE(rcv.isTimedOut(tsMono(900000000))); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "\x78\x56" "34567", SET100, 0, 900000000), bk)); // Global timeout + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, 0, 900000100), bk)); // Wrong iface + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", SET011, 0, 900000200), bk)); // Wrong iface + CHECK_COMPLETE( rcv.addFrame(gen(1, "qwe", SET011, 0, 900000200), bk)); + + ASSERT_EQ(900000000, rcv.getLastTransferTimestampMonotonic().toUSec()); + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + + ASSERT_FALSE(rcv.isTimedOut(tsMono(1000))); + ASSERT_FALSE(rcv.isTimedOut(tsMono(900000300))); + ASSERT_TRUE(rcv.isTimedOut(tsMono(9990000000))); + + ASSERT_LT(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); + ASSERT_LE(TransferReceiver::getMinTransferInterval(), rcv.getInterval()); + ASSERT_GE(TransferReceiver::getMaxTransferInterval(), rcv.getInterval()); + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwe")); + ASSERT_EQ(0x5678, rcv.getLastTransferCrc()); + + /* + * Destruction + */ + ASSERT_TRUE(bufmgr.access(gen.bufmgr_key)); + context.receiver.~TransferReceiver(); // TransferReceiver does not own the buffer, it must not be released! + ASSERT_TRUE(bufmgr.access(gen.bufmgr_key)); // Making sure that the buffer is still there +} + + +TEST(TransferReceiver, OutOfBufferSpace_32bytes) +{ + Context<32> context; + RxFrameGenerator gen(789); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY); + + /* + * Simple transfer, maximum buffer length + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 1, 100000000), bk)); // 5 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 1, 100000100), bk)); // 12 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET000, 1, 100000200), bk)); // 19 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 1, 100000300), bk)); // 26 + CHECK_COMPLETE( rcv.addFrame(gen(1, "123456", SET010, 1, 100000400), bk)); // 32 + + ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec()); + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567123456712345671234567123456")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); // CRC from "12", which is 0x3231 in little endian + + /* + * Transfer longer than available buffer space + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 2, 100001000), bk)); // 5 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 2, 100001100), bk)); // 12 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET000, 2, 100001200), bk)); // 19 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 2, 100001200), bk)); // 26 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET010, 2, 100001300), bk)); // 33 // EOT, ignored - lost sync + + ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec()); // Timestamp will not be overriden + ASSERT_FALSE(bufmgr.access(gen.bufmgr_key)); // Buffer should be removed + + ASSERT_EQ(1, rcv.yieldErrorCount()); + ASSERT_EQ(0, rcv.yieldErrorCount()); +} + + +TEST(TransferReceiver, OutOfOrderFrames) +{ + Context<32> context; + RxFrameGenerator gen(789); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY); + + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 7, 100000000), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET000, 7, 100000100), bk)); // Out of order + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET010, 7, 100000200), bk)); // Out of order + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwertyu", SET001, 7, 100000300), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET011, 7, 100000200), bk)); // Out of order + CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", SET010, 7, 100000400), bk)); + + ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec()); + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); + + ASSERT_EQ(3, rcv.yieldErrorCount()); + ASSERT_EQ(0, rcv.yieldErrorCount()); +} + + +TEST(TransferReceiver, IntervalMeasurement) +{ + Context<32> context; + RxFrameGenerator gen(789); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY); + + static const int INTERVAL = 1000; + uavcan::TransferID tid; + uint64_t timestamp = 100000000; + + for (int i = 0; i < 1000; i++) + { + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, tid.get(), timestamp), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwertyu", SET001, tid.get(), timestamp), bk)); + CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", SET010, tid.get(), timestamp), bk)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); + ASSERT_EQ(timestamp, rcv.getLastTransferTimestampMonotonic().toUSec()); + + timestamp += uint64_t(INTERVAL); + tid.increment(); + } + + ASSERT_EQ(INTERVAL, rcv.getInterval().toUSec()); +} + + +TEST(TransferReceiver, Restart) +{ + Context<32> context; + RxFrameGenerator gen(789); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY); + + /* + * This transfer looks complete, but must be ignored because of large delay after the first frame + * Args: iface_index, data, set, transfer_id, ts_monotonic [, ts_utc] + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET100, 0, 100), bk)); // Begin + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET001, 0, 100000100), bk)); // Continue 100 sec later, expired + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET010, 0, 100000200), bk)); // Ignored + + /* + * Begins immediately after, encounters a delay 0.9 sec but completes OK + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 0, 100000300), bk)); // Begin + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 0, 100900300), bk)); // 0.9 sec later + CHECK_COMPLETE( rcv.addFrame(gen(1, "1234567", SET010, 0, 100900400), bk)); // OK nevertheless + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "3456712345671234567")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); + + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + + /* + * Begins OK, gets a timeout, switches to another iface + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET100, 1, 103000500), bk)); // Begin + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET001, 1, 105000500), bk)); // 2 sec later, timeout + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET001, 1, 105000600), bk)); // Same TID, another iface - ignore + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET001, 2, 105000700), bk)); // Not first frame - ignore + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, 2, 105000800), bk)); // First, another iface - restart + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET010, 1, 105000600), bk)); // Old iface - ignore + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET001, 2, 105000900), bk)); // Continuing + CHECK_COMPLETE( rcv.addFrame(gen(0, "1234567", SET010, 2, 105000910), bk)); // Done + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "3456712345671234567")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); + + ASSERT_EQ(4, rcv.yieldErrorCount()); + ASSERT_EQ(0, rcv.yieldErrorCount()); +} + + +TEST(TransferReceiver, UtcTransferTimestamping) +{ + Context<32> context; + RxFrameGenerator gen(789); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY); + + /* + * Zero UTC timestamp must be preserved + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 0, 1, 0), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwertyu", SET001, 0, 2, 0), bk)); + CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", SET010, 0, 3, 0), bk)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd")); + ASSERT_EQ(1, rcv.getLastTransferTimestampMonotonic().toUSec()); + ASSERT_EQ(0, rcv.getLastTransferTimestampUtc().toUSec()); + + /* + * Non-zero UTC timestamp + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 1, 4, 123), bk)); // This UTC is going to be preserved + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwertyu", SET001, 1, 5, 0), bk)); // Following are ignored + CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", SET010, 1, 6, 42), bk)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd")); + ASSERT_EQ(4, rcv.getLastTransferTimestampMonotonic().toUSec()); + ASSERT_EQ(123, rcv.getLastTransferTimestampUtc().toUSec()); + + /* + * Single-frame transfers + * iface_index, data, set, transfer_id, ts_monotonic + */ + CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "abc", SET110, 2, 10, 100000000), bk)); // Exact value is irrelevant + ASSERT_EQ(10, rcv.getLastTransferTimestampMonotonic().toUSec()); + ASSERT_EQ(100000000, rcv.getLastTransferTimestampUtc().toUSec()); + + /* + * Restart recovery + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, 1, 100000000, 800000000), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwertyu", SET001, 1, 100000001, 300000000), bk)); + CHECK_COMPLETE( rcv.addFrame(gen(0, "abcd", SET010, 1, 100000002, 900000000), bk)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd")); + ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec()); + ASSERT_EQ(800000000, rcv.getLastTransferTimestampUtc().toUSec()); +} + + +TEST(TransferReceiver, HeaderParsing) +{ + static const std::string SFT_PAYLOAD = "1234567"; + + uavcan::TransferID tid; + + /* + * Broadcast + */ + { + Context<32> context; + RxFrameGenerator gen(123); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + + /* + * MFT, message broadcasting + */ + { + gen.bufmgr_key = + uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), uavcan::TransferTypeMessageBroadcast); + uavcan::TransferBufferAccessor bk1(context.bufmgr, gen.bufmgr_key); + + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, tid.get(), 1), bk1)); + CHECK_COMPLETE( rcv.addFrame(gen(0, "abcd", SET011, tid.get(), 2), bk1)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567abcd")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); + + tid.increment(); + bk1.remove(); + } + + /* + * SFT, message broadcasting + */ + { + gen.bufmgr_key = + uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), uavcan::TransferTypeMessageBroadcast); + uavcan::TransferBufferAccessor bk(context.bufmgr, gen.bufmgr_key); + + const uavcan::RxFrame frame = gen(0, SFT_PAYLOAD, SET110, tid.get(), 1000); + + CHECK_SINGLE_FRAME(rcv.addFrame(frame, bk)); + ASSERT_EQ(0x0000, rcv.getLastTransferCrc()); // Default value - zero + + // All bytes are payload, zero overhead + ASSERT_TRUE(std::equal(SFT_PAYLOAD.begin(), SFT_PAYLOAD.end(), frame.getPayloadPtr())); + + tid.increment(); + } + } + + /* + * Unicast + */ + { + Context<32> context; + RxFrameGenerator gen(123); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + + static const uavcan::TransferType ADDRESSED_TRANSFER_TYPES[2] = + { + uavcan::TransferTypeServiceRequest, + uavcan::TransferTypeServiceResponse + }; + + /* + * MFT, service request/response + */ + for (unsigned i = 0; i < (sizeof(ADDRESSED_TRANSFER_TYPES) / sizeof(ADDRESSED_TRANSFER_TYPES[0])); i++) + { + gen.bufmgr_key = + uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), ADDRESSED_TRANSFER_TYPES[i]); + uavcan::TransferBufferAccessor bk2(context.bufmgr, gen.bufmgr_key); + + const uint64_t ts_monotonic = i + 10; + + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, tid.get(), ts_monotonic), bk2)); + CHECK_COMPLETE( rcv.addFrame(gen(0, "abcd", SET011, tid.get(), ts_monotonic), bk2)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567abcd")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); + + tid.increment(); + bk2.remove(); + } + + /* + * SFT, message unicast, service request/response + */ + for (unsigned i = 0; i < int(sizeof(ADDRESSED_TRANSFER_TYPES) / sizeof(ADDRESSED_TRANSFER_TYPES[0])); i++) + { + gen.bufmgr_key = + uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), ADDRESSED_TRANSFER_TYPES[i]); + uavcan::TransferBufferAccessor bk(context.bufmgr, gen.bufmgr_key); + + const uavcan::RxFrame frame = gen(0, SFT_PAYLOAD, SET110, tid.get(), i + 10000U); + + CHECK_SINGLE_FRAME(rcv.addFrame(frame, bk)); + ASSERT_EQ(0x0000, rcv.getLastTransferCrc()); // Default value - zero + + // First byte must be ignored + ASSERT_TRUE(std::equal(SFT_PAYLOAD.begin(), SFT_PAYLOAD.end(), frame.getPayloadPtr())); + + tid.increment(); + } + } + + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_sender.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_sender.cpp new file mode 100644 index 000000000000..ff27b6fe98d5 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_sender.cpp @@ -0,0 +1,266 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "transfer_test_helpers.hpp" +#include "can/can.hpp" +#include + +static int sendOne(uavcan::TransferSender& sender, const std::string& data, + uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline, + uavcan::TransferType transfer_type, uavcan::NodeID dst_node_id) +{ + return sender.send(reinterpret_cast(data.c_str()), unsigned(data.length()), + uavcan::MonotonicTime::fromUSec(monotonic_tx_deadline), + uavcan::MonotonicTime::fromUSec(monotonic_blocking_deadline), transfer_type, dst_node_id); +} + +static int sendOne(uavcan::TransferSender& sender, const std::string& data, + uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline, + uavcan::TransferType transfer_type, uavcan::NodeID dst_node_id, uavcan::TransferID tid) +{ + return sender.send(reinterpret_cast(data.c_str()), unsigned(data.length()), + uavcan::MonotonicTime::fromUSec(monotonic_tx_deadline), + uavcan::MonotonicTime::fromUSec(monotonic_blocking_deadline), transfer_type, dst_node_id, tid); +} + + +TEST(TransferSender, Basic) +{ + uavcan::PoolAllocator poolmgr; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + static const uavcan::NodeID TX_NODE_ID(64); + static const uavcan::NodeID RX_NODE_ID(65); + uavcan::Dispatcher dispatcher_tx(driver, poolmgr, clockmock); + uavcan::Dispatcher dispatcher_rx(driver, poolmgr, clockmock); + ASSERT_TRUE(dispatcher_tx.setNodeID(TX_NODE_ID)); + ASSERT_TRUE(dispatcher_rx.setNodeID(RX_NODE_ID)); + + /* + * Test environment + */ + static const uavcan::DataTypeDescriptor TYPES[2] = + { + makeDataType(uavcan::DataTypeKindMessage, 1), + makeDataType(uavcan::DataTypeKindService, 1) + }; + + uavcan::TransferSender senders[2] = + { + uavcan::TransferSender(dispatcher_tx, TYPES[0], uavcan::CanTxQueue::Volatile), + uavcan::TransferSender(dispatcher_tx, TYPES[1], uavcan::CanTxQueue::Persistent) + }; + + static const std::string DATA[4] = + { + "Don't panic.", + + "The ships hung in the sky in much the same way that bricks don't.", + + "Would it save you a lot of time if I just gave up and went mad now?", + + "If there's anything more important than my ego around, I want it caught and shot now." + }; + + /* + * Transmission + */ + static const uint64_t TX_DEADLINE = 1000000; + + // Low priority + senders[0].setPriority(20); + sendOne(senders[0], DATA[0], TX_DEADLINE, 0, uavcan::TransferTypeMessageBroadcast, 0); + sendOne(senders[0], DATA[1], TX_DEADLINE, 0, uavcan::TransferTypeMessageBroadcast, 0); + // High priority + senders[0].setPriority(10); + sendOne(senders[0], "123", TX_DEADLINE, 0, uavcan::TransferTypeMessageBroadcast, 0); + sendOne(senders[0], "456", TX_DEADLINE, 0, uavcan::TransferTypeMessageBroadcast, 0); + + senders[1].setPriority(15); + sendOne(senders[1], DATA[2], TX_DEADLINE, 0, uavcan::TransferTypeServiceRequest, RX_NODE_ID); + sendOne(senders[1], DATA[3], TX_DEADLINE, 0, uavcan::TransferTypeServiceResponse, RX_NODE_ID, 1); + sendOne(senders[1], "", TX_DEADLINE, 0, uavcan::TransferTypeServiceRequest, RX_NODE_ID); + sendOne(senders[1], "", TX_DEADLINE, 0, uavcan::TransferTypeServiceResponse, RX_NODE_ID, 2); + + using namespace uavcan; + static const Transfer TRANSFERS[8] = + { + Transfer(TX_DEADLINE, 0, 20, TransferTypeMessageBroadcast, 0, TX_NODE_ID, 0, DATA[0], TYPES[0]), + Transfer(TX_DEADLINE, 0, 20, TransferTypeMessageBroadcast, 1, TX_NODE_ID, 0, DATA[1], TYPES[0]), + Transfer(TX_DEADLINE, 0, 10, TransferTypeMessageBroadcast, 2, TX_NODE_ID, 0, "123", TYPES[0]), + Transfer(TX_DEADLINE, 0, 10, TransferTypeMessageBroadcast, 3, TX_NODE_ID, 0, "456", TYPES[0]), + + Transfer(TX_DEADLINE, 0, 15, TransferTypeServiceRequest, 0, TX_NODE_ID, RX_NODE_ID, DATA[2], TYPES[1]), + Transfer(TX_DEADLINE, 0, 15, TransferTypeServiceResponse, 1, TX_NODE_ID, RX_NODE_ID, DATA[3], TYPES[1]), + Transfer(TX_DEADLINE, 0, 15, TransferTypeServiceRequest, 1, TX_NODE_ID, RX_NODE_ID, "", TYPES[1]), + Transfer(TX_DEADLINE, 0, 15, TransferTypeServiceResponse, 2, TX_NODE_ID, RX_NODE_ID, "", TYPES[1]) + }; + + /* + * Making sure that the abort flag is not used. + */ + ASSERT_EQ(0, driver.ifaces.at(0).tx.front().flags); + + /* + * Receiving on the other side. + */ + for (uint8_t i = 0; i < driver.getNumIfaces(); i++) // Moving the frames from TX to RX side + { + CanIfaceMock& iface = driver.ifaces.at(i); + std::cout << "Num frames: " << iface.tx.size() << std::endl; + while (!iface.tx.empty()) + { + CanIfaceMock::FrameWithTime ft = iface.tx.front(); + iface.tx.pop(); + iface.rx.push(ft); + } + } + + TestListener sub_msg(dispatcher_rx.getTransferPerfCounter(), TYPES[0], 512, poolmgr); + TestListener sub_srv_req(dispatcher_rx.getTransferPerfCounter(), TYPES[1], 512, poolmgr); + TestListener sub_srv_resp(dispatcher_rx.getTransferPerfCounter(), TYPES[1], 512, poolmgr); + + dispatcher_rx.registerMessageListener(&sub_msg); + dispatcher_rx.registerServiceRequestListener(&sub_srv_req); + dispatcher_rx.registerServiceResponseListener(&sub_srv_resp); + + while (true) + { + const int res = dispatcher_rx.spin(tsMono(0)); + ASSERT_LE(0, res); + clockmock.advance(100); + if (res == 0) + { + break; + } + } + + /* + * Validation + */ + ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[0])); + ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[1])); + ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[2])); + ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[3])); + + ASSERT_TRUE(sub_srv_req.matchAndPop(TRANSFERS[4])); + ASSERT_TRUE(sub_srv_req.matchAndPop(TRANSFERS[6])); + + ASSERT_TRUE(sub_srv_resp.matchAndPop(TRANSFERS[5])); + ASSERT_TRUE(sub_srv_resp.matchAndPop(TRANSFERS[7])); + + /* + * Perf counters + */ + EXPECT_EQ(0, dispatcher_tx.getTransferPerfCounter().getErrorCount()); + EXPECT_EQ(8, dispatcher_tx.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(0, dispatcher_tx.getTransferPerfCounter().getRxTransferCount()); + + EXPECT_EQ(0, dispatcher_rx.getTransferPerfCounter().getErrorCount()); + EXPECT_EQ(0, dispatcher_rx.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(8, dispatcher_rx.getTransferPerfCounter().getRxTransferCount()); +} + + +struct TransferSenderTestLoopbackFrameListener : public uavcan::LoopbackFrameListenerBase +{ + uavcan::RxFrame last_frame; + unsigned count; + + TransferSenderTestLoopbackFrameListener(uavcan::Dispatcher& dispatcher) + : uavcan::LoopbackFrameListenerBase(dispatcher) + , count(0) + { + startListening(); + } + + void handleLoopbackFrame(const uavcan::RxFrame& frame) + { + last_frame = frame; + count++; + } +}; + +TEST(TransferSender, Loopback) +{ + uavcan::PoolAllocator poolmgr; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + static const uavcan::NodeID TX_NODE_ID(64); + uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock); + ASSERT_TRUE(dispatcher.setNodeID(TX_NODE_ID)); + + uavcan::DataTypeDescriptor desc = makeDataType(uavcan::DataTypeKindMessage, 1, "Foobar"); + + uavcan::TransferSender sender(dispatcher, desc, uavcan::CanTxQueue::Volatile); + + sender.setCanIOFlags(uavcan::CanIOFlagLoopback); + ASSERT_EQ(uavcan::CanIOFlagLoopback, sender.getCanIOFlags()); + + sender.setIfaceMask(2); + ASSERT_EQ(2, sender.getIfaceMask()); + + TransferSenderTestLoopbackFrameListener listener(dispatcher); + + ASSERT_LE(0, sender.send(reinterpret_cast("123"), 3, tsMono(1000), tsMono(0), + uavcan::TransferTypeMessageBroadcast, 0)); + + ASSERT_EQ(0, listener.count); + ASSERT_EQ(0, dispatcher.spin(tsMono(1000))); + ASSERT_EQ(1, listener.count); + ASSERT_EQ(1, listener.last_frame.getIfaceIndex()); + ASSERT_EQ(3, listener.last_frame.getPayloadLen()); + ASSERT_TRUE(TX_NODE_ID == listener.last_frame.getSrcNodeID()); + ASSERT_TRUE(listener.last_frame.isEndOfTransfer()); + + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount()); + EXPECT_EQ(1, dispatcher.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount()); +} + +TEST(TransferSender, PassiveMode) +{ + uavcan::PoolAllocator poolmgr; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock); + + uavcan::TransferSender sender(dispatcher, makeDataType(uavcan::DataTypeKindMessage, 123), + uavcan::CanTxQueue::Volatile); + + static const uint8_t Payload[] = {1, 2, 3, 4, 5}; + + // By default, sending in passive mode is not enabled + ASSERT_EQ(-uavcan::ErrPassiveMode, + sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), + uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast)); + + // Overriding the default + sender.allowAnonymousTransfers(); + + // OK, now we can broadcast in any mode + ASSERT_LE(0, sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), + uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast)); + + // ...but not unicast or anything else + ASSERT_EQ(-uavcan::ErrPassiveMode, + sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), + uavcan::TransferTypeServiceRequest, uavcan::NodeID(42))); + + // Making sure the abort flag is set + ASSERT_FALSE(driver.ifaces.at(0).tx.empty()); + ASSERT_EQ(uavcan::CanIOFlagAbortOnError, driver.ifaces.at(0).tx.front().flags); + + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount()); + EXPECT_EQ(1, dispatcher.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_test_helpers.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_test_helpers.cpp new file mode 100644 index 000000000000..e3da98260294 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_test_helpers.cpp @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include "transfer_test_helpers.hpp" +#include "../clock.hpp" + + +TEST(TransferTestHelpers, Transfer) +{ + uavcan::PoolAllocator pool; + + uavcan::TransferBufferManager mgr(128, pool); + uavcan::TransferBufferAccessor tba(mgr, uavcan::TransferBufferManagerKey(0, uavcan::TransferTypeMessageBroadcast)); + + uavcan::RxFrame frame(uavcan::Frame(123, uavcan::TransferTypeMessageBroadcast, 1, 0, 0), + uavcan::MonotonicTime(), uavcan::UtcTime(), 0); + frame.setEndOfTransfer(true); + uavcan::MultiFrameIncomingTransfer mfit(tsMono(10), tsUtc(1000), frame, tba); + + // Filling the buffer with data + static const std::string TEST_DATA = "Kaneda! What do you see? Kaneda! What do you see? Kaneda! Kaneda!!!"; + ASSERT_TRUE(tba.create()); + ASSERT_EQ(TEST_DATA.length(), tba.access()->write(0, reinterpret_cast(TEST_DATA.c_str()), + unsigned(TEST_DATA.length()))); + + // Reading back + const Transfer transfer(mfit, uavcan::DataTypeDescriptor()); + ASSERT_EQ(TEST_DATA, transfer.payload); +} + + +TEST(TransferTestHelpers, MFTSerialization) +{ + uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "Foo"); + + static const std::string DATA = "To go wrong in one's own way is better than to go right in someone else's."; + const Transfer transfer(1, 100000, 10, + uavcan::TransferTypeServiceRequest, 2, 42, 127, DATA, type); + + const std::vector ser = serializeTransfer(transfer); + + std::cout << "Serialized transfer:\n"; + for (std::vector::const_iterator it = ser.begin(); it != ser.end(); ++it) + { + std::cout << "\t" << it->toString() << "\n"; + } + + for (std::vector::const_iterator it = ser.begin(); it != ser.end(); ++it) + { + std::cout << "\t'"; + for (unsigned i = 0; i < it->getPayloadLen(); i++) + { + uint8_t ch = it->getPayloadPtr()[i]; + if (ch < 0x20 || ch > 0x7E) + { + ch = '.'; + } + std::cout << static_cast(ch); + } + std::cout << "'\n"; + } + std::cout << std::flush; +} + + +TEST(TransferTestHelpers, SFTSerialization) +{ + uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "Foo"); + + { + const Transfer transfer(1, 100000, 10, + uavcan::TransferTypeMessageBroadcast, 7, 42, 0, "Nvrfrgt", type); + const std::vector ser = serializeTransfer(transfer); + ASSERT_EQ(1, ser.size()); + std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; + } + { + const Transfer transfer(1, 100000, 11, + uavcan::TransferTypeServiceRequest, 7, 42, 127, "7-chars", type); + const std::vector ser = serializeTransfer(transfer); + ASSERT_EQ(1, ser.size()); + std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; + } + { + const Transfer transfer(1, 100000, 12, + uavcan::TransferTypeMessageBroadcast, 7, 42, 0, "", type); + const std::vector ser = serializeTransfer(transfer); + ASSERT_EQ(1, ser.size()); + std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; + } + { + const Transfer transfer(1, 100000, 13, + uavcan::TransferTypeServiceResponse, 7, 42, 127, "", type); + const std::vector ser = serializeTransfer(transfer); + ASSERT_EQ(1, ser.size()); + std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; + } +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_test_helpers.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_test_helpers.hpp new file mode 100644 index 000000000000..14d6ccc8b1b3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_test_helpers.hpp @@ -0,0 +1,320 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include + +/** + * UAVCAN transfer representation used in various tests. + */ +struct Transfer +{ + uavcan::MonotonicTime ts_monotonic; + uavcan::UtcTime ts_utc; + uavcan::TransferPriority priority; + uavcan::TransferType transfer_type; + uavcan::TransferID transfer_id; + uavcan::NodeID src_node_id; + uavcan::NodeID dst_node_id; + uavcan::DataTypeDescriptor data_type; + std::string payload; + + Transfer(const uavcan::IncomingTransfer& tr, const uavcan::DataTypeDescriptor& data_type) + : ts_monotonic(tr.getMonotonicTimestamp()) + , ts_utc(tr.getUtcTimestamp()) + , priority(tr.getPriority()) + , transfer_type(tr.getTransferType()) + , transfer_id(tr.getTransferID()) + , src_node_id(tr.getSrcNodeID()) + , dst_node_id() // default is invalid + , data_type(data_type) + { + unsigned offset = 0; + while (true) + { + uint8_t buf[256]; + int res = tr.read(offset, buf, sizeof(buf)); + if (res < 0) + { + std::cout << "IncomingTransferContainer: read failure " << res << std::endl; + exit(1); + } + if (res == 0) + { + break; + } + payload += std::string(reinterpret_cast(buf), unsigned(res)); + offset += unsigned(res); + } + } + + Transfer(uavcan::MonotonicTime ts_monotonic, uavcan::UtcTime ts_utc, uavcan::TransferPriority priority, + uavcan::TransferType transfer_type, uavcan::TransferID transfer_id, uavcan::NodeID src_node_id, + uavcan::NodeID dst_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& data_type) + : ts_monotonic(ts_monotonic) + , ts_utc(ts_utc) + , priority(priority) + , transfer_type(transfer_type) + , transfer_id(transfer_id) + , src_node_id(src_node_id) + , dst_node_id(dst_node_id) + , data_type(data_type) + , payload(payload) + { } + + Transfer(uint64_t ts_monotonic, uint64_t ts_utc, uavcan::TransferPriority priority, + uavcan::TransferType transfer_type, uavcan::TransferID transfer_id, uavcan::NodeID src_node_id, + uavcan::NodeID dst_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& data_type) + : ts_monotonic(uavcan::MonotonicTime::fromUSec(ts_monotonic)) + , ts_utc(uavcan::UtcTime::fromUSec(ts_utc)) + , priority(priority) + , transfer_type(transfer_type) + , transfer_id(transfer_id) + , src_node_id(src_node_id) + , dst_node_id(dst_node_id) + , data_type(data_type) + , payload(payload) + { } + + bool operator==(const Transfer& rhs) const + { + return + (ts_monotonic == rhs.ts_monotonic) && + ((!ts_utc.isZero() && !rhs.ts_utc.isZero()) ? (ts_utc == rhs.ts_utc) : true) && + (priority == rhs.priority) && + (transfer_type == rhs.transfer_type) && + (transfer_id == rhs.transfer_id) && + (src_node_id == rhs.src_node_id) && + ((dst_node_id.isValid() && rhs.dst_node_id.isValid()) ? (dst_node_id == rhs.dst_node_id) : true) && + (data_type == rhs.data_type) && + (payload == rhs.payload); + } + + std::string toString() const + { + std::ostringstream os; + os << "ts_m=" << ts_monotonic + << " ts_utc=" << ts_utc + << " prio=" << int(priority.get()) + << " tt=" << int(transfer_type) + << " tid=" << int(transfer_id.get()) + << " snid=" << int(src_node_id.get()) + << " dnid=" << int(dst_node_id.get()) + << " dtid=" << int(data_type.getID().get()) + << "\n\t'" << payload << "'"; + return os.str(); + } +}; + +/** + * This subscriber accepts any types of transfers - this makes testing easier. + * In reality, uavcan::TransferListener should accept only specific transfer types + * which are dispatched/filtered by uavcan::Dispatcher. + */ +class TestListener : public uavcan::TransferListener +{ + typedef uavcan::TransferListener Base; + + std::queue transfers_; + +public: + TestListener(uavcan::TransferPerfCounter& perf, const uavcan::DataTypeDescriptor& data_type, + uavcan::uint16_t max_buffer_size, uavcan::IPoolAllocator& allocator) + : Base(perf, data_type, max_buffer_size, allocator) + { } + + void handleIncomingTransfer(uavcan::IncomingTransfer& transfer) + { + const Transfer rx(transfer, Base::getDataTypeDescriptor()); + transfers_.push(rx); + std::cout << "Received transfer: " << rx.toString() << std::endl; + + const bool single_frame = dynamic_cast(&transfer) != UAVCAN_NULLPTR; + + const bool anonymous = single_frame && + transfer.getSrcNodeID().isBroadcast() && + (transfer.getTransferType() == uavcan::TransferTypeMessageBroadcast); + + ASSERT_EQ(anonymous, transfer.isAnonymousTransfer()); + } + + bool matchAndPop(const Transfer& reference) + { + if (transfers_.empty()) + { + std::cout << "No received transfers" << std::endl; + return false; + } + + const Transfer tr = transfers_.front(); + transfers_.pop(); + + const bool res = (tr == reference); + if (!res) + { + std::cout << "TestSubscriber: Transfer mismatch:\n" + << "Expected: " << reference.toString() << "\n" + << "Received: " << tr.toString() << std::endl; + } + return res; + } + + unsigned getNumReceivedTransfers() const { return static_cast(transfers_.size()); } + bool isEmpty() const { return transfers_.empty(); } +}; + + +namespace +{ + +std::vector serializeTransfer(const Transfer& transfer) +{ + const bool need_crc = transfer.payload.length() > (sizeof(uavcan::CanFrame::data) - 1); + + std::vector raw_payload; + if (need_crc) + { + uavcan::TransferCRC payload_crc = transfer.data_type.getSignature().toTransferCRC(); + payload_crc.add(reinterpret_cast(transfer.payload.c_str()), uint16_t(transfer.payload.length())); + // Little endian + raw_payload.push_back(uint8_t(payload_crc.get() & 0xFF)); + raw_payload.push_back(uint8_t((payload_crc.get() >> 8) & 0xFF)); + } + raw_payload.insert(raw_payload.end(), transfer.payload.begin(), transfer.payload.end()); + + std::vector output; + unsigned offset = 0; + uavcan::MonotonicTime ts_monotonic = transfer.ts_monotonic; + uavcan::UtcTime ts_utc = transfer.ts_utc; + + uavcan::Frame frm(transfer.data_type.getID(), transfer.transfer_type, transfer.src_node_id, + transfer.dst_node_id, transfer.transfer_id); + frm.setStartOfTransfer(true); + frm.setPriority(transfer.priority); + + while (true) + { + const int bytes_left = int(raw_payload.size()) - int(offset); + EXPECT_TRUE(bytes_left >= 0); + + const int spres = frm.setPayload(&*(raw_payload.begin() + offset), unsigned(bytes_left)); + if (spres < 0) + { + std::cerr << ">_<" << std::endl; + std::exit(1); + } + if (spres == bytes_left) + { + frm.setEndOfTransfer(true); + } + + offset += unsigned(spres); + + const uavcan::RxFrame rxfrm(frm, ts_monotonic, ts_utc, 0); + ts_monotonic += uavcan::MonotonicDuration::fromUSec(1); + ts_utc += uavcan::UtcDuration::fromUSec(1); + + output.push_back(rxfrm); + if (frm.isEndOfTransfer()) + { + break; + } + + frm.setStartOfTransfer(false); + frm.flipToggle(); + } + return output; +} + +inline uavcan::DataTypeDescriptor makeDataType(uavcan::DataTypeKind kind, uint16_t id, const char* name = "") +{ + const uavcan::DataTypeSignature signature((uint64_t(kind) << 16) | uint16_t(id << 8) | uint16_t(id & 0xFF)); + return uavcan::DataTypeDescriptor(kind, id, signature, name); +} + +} + + +class IncomingTransferEmulatorBase +{ + uavcan::MonotonicTime ts_; + uavcan::TransferID tid_; + uavcan::NodeID dst_node_id_; + +public: + IncomingTransferEmulatorBase(uavcan::NodeID dst_node_id) + : dst_node_id_(dst_node_id) + { } + + virtual ~IncomingTransferEmulatorBase() { } + + Transfer makeTransfer(uavcan::TransferPriority priority, uavcan::TransferType transfer_type, + uint8_t source_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& type, + uavcan::NodeID dst_node_id_override = uavcan::NodeID()) + { + ts_ += uavcan::MonotonicDuration::fromUSec(100); + const uavcan::UtcTime utc = uavcan::UtcTime::fromUSec(ts_.toUSec() + 1000000000ul); + const uavcan::NodeID dst_node_id = (transfer_type == uavcan::TransferTypeMessageBroadcast) ? + uavcan::NodeID::Broadcast : + (dst_node_id_override.isValid() ? dst_node_id_override : dst_node_id_); + const Transfer tr(ts_, utc, priority, transfer_type, tid_, source_node_id, dst_node_id, payload, type); + tid_.increment(); + return tr; + } + + virtual void sendOneFrame(const uavcan::RxFrame& frame) = 0; + + void send(const std::vector >& sers) + { + unsigned index = 0; + while (true) + { + // Sending all transfers concurrently + bool all_empty = true; + for (std::vector >::const_iterator it = sers.begin(); it != sers.end(); ++it) + { + if (it->size() <= index) + { + continue; + } + all_empty = false; + std::cout << "Incoming Transfer Emulator: Sending: " << it->at(index).toString() << std::endl; + sendOneFrame(it->at(index)); + } + index++; + if (all_empty) + { + break; + } + } + } + + void send(const Transfer* transfers, unsigned num_transfers) + { + std::vector > sers; + while (num_transfers--) + { + sers.push_back(serializeTransfer(*transfers++)); + } + send(sers); + } + + template void send(const Transfer (&transfers)[SIZE]) { send(transfers, SIZE); } +}; + +/** + * Zero allocator - always fails + */ +class NullAllocator : public uavcan::IPoolAllocator +{ +public: + virtual void* allocate(std::size_t) { return UAVCAN_NULLPTR; } + virtual void deallocate(const void*) { } + virtual uint16_t getBlockCapacity() const { return 0; } +}; diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/util/comparison.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/util/comparison.cpp new file mode 100644 index 000000000000..10ef7042d6bb --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/util/comparison.cpp @@ -0,0 +1,163 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +TEST(Comparison, Basic) +{ + // Basic same type floats + ASSERT_TRUE(uavcan::areClose(0.1, 0.1)); + ASSERT_TRUE(uavcan::areClose(0.1F, 0.1F)); + ASSERT_TRUE(uavcan::areClose(0.1L, 0.1L)); + + // Basic mixed type floats + ASSERT_TRUE(uavcan::areClose(0.1F, 0.1)); + ASSERT_TRUE(uavcan::areClose(0.1, 0.1F)); + ASSERT_TRUE(uavcan::areClose(0.1F, 0.1L)); + ASSERT_TRUE(uavcan::areClose(0.1L, 0.1F)); + ASSERT_TRUE(uavcan::areClose(0.1, 0.1L)); + ASSERT_TRUE(uavcan::areClose(0.1L, 0.1)); + + // Basic floats + ASSERT_TRUE(uavcan::areClose(0x07, '\x07')); + ASSERT_TRUE(uavcan::areClose(123456789LL, 123456789)); + ASSERT_TRUE(uavcan::areClose("123", std::string("123"))); + + // Non-equality + ASSERT_FALSE(uavcan::areClose(-0.1, 0.1)); + ASSERT_FALSE(uavcan::areClose(-0.1F, 0.0L)); + ASSERT_FALSE(uavcan::areClose("123", std::string("12"))); + ASSERT_FALSE(uavcan::areClose(0x07L, '\0')); +} + +TEST(Comparison, FloatSpecialCase) +{ + ASSERT_FALSE(uavcan::areClose(0.1, std::numeric_limits::infinity())); + + ASSERT_TRUE(uavcan::areClose(std::numeric_limits::infinity(), + std::numeric_limits::infinity())); + + ASSERT_FALSE(uavcan::areClose(std::numeric_limits::infinity(), -std::numeric_limits::infinity())); + + ASSERT_FALSE(uavcan::areClose(std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN())); +} + +TEST(Comparison, FloatULP) +{ + ASSERT_FALSE(uavcan::areClose(0.100000000000000001L, 0.1L)); + ASSERT_TRUE( uavcan::areClose(0.100000000000000001, 0.1L)); + ASSERT_TRUE( uavcan::areClose(0.100000000000000001F, 0.1L)); + + // Near zero + ASSERT_TRUE( uavcan::areClose(0.0F, std::numeric_limits::epsilon())); + ASSERT_TRUE( uavcan::areClose(0.0F, -std::numeric_limits::epsilon())); + ASSERT_FALSE(uavcan::areClose(0.0F, std::numeric_limits::epsilon() * 2)); +} + +TEST(Comparison, BruteforceValidation) +{ + const std::streamsize default_precision = std::cout.precision(); + std::cout.precision(20); + + float x = -uavcan::NumericTraits::max(); + + while (x < uavcan::NumericTraits::max()) + { + const float y1 = x + std::abs(x) * (uavcan::NumericTraits::epsilon() * 2); // Still equal + const float y2 = x + uavcan::max(std::abs(x), 1.F) * (uavcan::NumericTraits::epsilon() * 20); // Nope + + if (!uavcan::areClose(y1, x)) + { + std::cout << "y1=" << y1 << " y2=" << y2 << " x=" << x << std::endl; + ASSERT_TRUE(false); + } + if (uavcan::areClose(y2, x)) + { + std::cout << "y1=" << y1 << " y2=" << y2 << " x=" << x << std::endl; + ASSERT_TRUE(false); + } + + x = y2; + } + + std::cout.precision(default_precision); +} + + +struct B +{ + long double b; + B(long double val = 0.0L) : b(val) { } +}; + +struct A +{ + float a; + explicit A(float val = 0.0F) : a(val) { } + + bool isClose(A rhs) const + { + std::cout << "bool A::isClose(A) --> " << rhs.a << std::endl; + return uavcan::areClose(a, rhs.a); + } + + bool isClose(const B& rhs) const + { + std::cout << "bool A::isClose(const B&) --> " << rhs.b << std::endl; + return uavcan::areClose(a, rhs.b); + } +}; + +struct C +{ + long long c; + explicit C(long long val = 0.0L) : c(val) { } + + bool operator==(B rhs) const + { + std::cout << "bool C::operator==(B) --> " << rhs.b << std::endl; + return c == static_cast(rhs.b); + } +}; + +TEST(Comparison, IsCloseMethod) +{ + B b; + A a; + C c; + + std::cout << 1 << std::endl; + ASSERT_TRUE(uavcan::areClose(a, b)); // Fuzzy + ASSERT_TRUE(uavcan::areClose(a, A())); // Fuzzy + ASSERT_TRUE(uavcan::areClose(b, a)); // Fuzzy, reverse application + ASSERT_TRUE(uavcan::areClose(c, b)); // Exact + + std::cout << 2 << std::endl; + + a.a = uavcan::NumericTraits::epsilon(); + + ASSERT_TRUE(uavcan::areClose(a, b)); + ASSERT_TRUE(uavcan::areClose(b, a)); + ASSERT_TRUE(a.isClose(b)); + ASSERT_TRUE(a.isClose(A())); + ASSERT_TRUE(uavcan::areClose(A(), a)); + + std::cout << 3 << std::endl; + + a.a = 1e-5F; + + ASSERT_FALSE(uavcan::areClose(a, b)); + ASSERT_FALSE(uavcan::areClose(b, a)); + ASSERT_FALSE(uavcan::areClose(A(), a)); + + std::cout << 4 << std::endl; + + b.b = 1.1L; + c.c = 1; + + ASSERT_TRUE(uavcan::areClose(c, b)); // Round to integer + ASSERT_TRUE(uavcan::areClose(c, 1.0L)); // Implicit cast to B + ASSERT_FALSE(uavcan::areClose(c, 0.0L)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/util/lazy_constructor.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/util/lazy_constructor.cpp new file mode 100644 index 000000000000..778a5c0674c2 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/util/lazy_constructor.cpp @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + + +TEST(LazyConstructor, Basic) +{ + using ::uavcan::LazyConstructor; + + LazyConstructor a; + LazyConstructor b; + + ASSERT_FALSE(a); + ASSERT_FALSE(b.isConstructed()); + + /* + * Construction + */ + a.destroy(); // no-op + a.construct(); + b.construct("Hello world"); + + ASSERT_TRUE(a); + ASSERT_TRUE(b.isConstructed()); + + ASSERT_NE(*a, *b); + ASSERT_STRNE(a->c_str(), b->c_str()); + + ASSERT_EQ(*a, ""); + ASSERT_EQ(*b, "Hello world"); + + /* + * Copying + */ + a = b; // Assignment operator performs destruction and immediate copy construction + ASSERT_EQ(*a, *b); + ASSERT_EQ(*a, "Hello world"); + + LazyConstructor c(a); // Copy constructor call is forwarded to std::string + + ASSERT_EQ(*c, *a); + + *a = "123"; + ASSERT_NE(*c, *a); + ASSERT_EQ(*c, *b); + + *c = "456"; + ASSERT_NE(*a, *c); + ASSERT_NE(*b, *a); + ASSERT_NE(*c, *b); + + /* + * Destruction + */ + ASSERT_TRUE(c); + c.destroy(); + ASSERT_FALSE(c); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/util/linked_list.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/util/linked_list.cpp new file mode 100644 index 000000000000..8910bb8684c0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/util/linked_list.cpp @@ -0,0 +1,145 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +struct ListItem : uavcan::LinkedListNode +{ + int value; + + ListItem(int value = 0) + : value(value) + { } + + struct GreaterThanComparator + { + const int compare_with; + + GreaterThanComparator(int compare_with) + : compare_with(compare_with) + { } + + bool operator()(const ListItem* item) const + { + return item->value > compare_with; + } + }; + + void insort(uavcan::LinkedListRoot& root) + { + root.insertBefore(this, GreaterThanComparator(value)); + } +}; + +TEST(LinkedList, Basic) +{ + uavcan::LinkedListRoot root; + + /* + * Insert/remove + */ + EXPECT_EQ(0, root.getLength()); + + ListItem item1; + root.insert(&item1); + root.insert(&item1); // Insert twice - second will be ignored + EXPECT_EQ(1, root.getLength()); + + root.remove(&item1); + root.remove(&item1); + EXPECT_EQ(0, root.getLength()); + + ListItem items[3]; + root.insert(&item1); + root.insert(items + 0); + root.insert(items + 1); + root.insert(items + 2); + EXPECT_EQ(4, root.getLength()); + + /* + * Order persistence + */ + items[0].value = 10; + items[1].value = 11; + items[2].value = 12; + const int expected_values[] = {12, 11, 10, 0}; + ListItem* node = root.get(); + for (int i = 0; i < 4; i++) + { + EXPECT_EQ(expected_values[i], node->value); + node = node->getNextListNode(); + } + + root.remove(items + 0); + root.remove(items + 2); + root.remove(items + 2); + EXPECT_EQ(2, root.getLength()); + + const int expected_values2[] = {11, 0}; + node = root.get(); + for (int i = 0; i < 2; i++) + { + EXPECT_EQ(expected_values2[i], node->value); + node = node->getNextListNode(); + } + + root.insert(items + 2); + EXPECT_EQ(3, root.getLength()); + EXPECT_EQ(12, root.get()->value); + + /* + * Emptying + */ + root.remove(&item1); + root.remove(items + 0); + root.remove(items + 1); + root.remove(items + 2); + EXPECT_EQ(0, root.getLength()); +} + +TEST(LinkedList, Sorting) +{ + uavcan::LinkedListRoot root; + ListItem items[6]; + for (int i = 0; i < 6; i++) + { + items[i].value = i; + } + + EXPECT_EQ(0, root.getLength()); + + items[2].insort(root); + EXPECT_EQ(1, root.getLength()); + + items[2].insort(root); // Making sure the duplicate will be removed + EXPECT_EQ(1, root.getLength()); + + items[3].insort(root); + + items[0].insort(root); + + items[4].insort(root); + EXPECT_EQ(4, root.getLength()); + + items[1].insort(root); + EXPECT_EQ(5, root.getLength()); + + items[1].insort(root); // Another duplicate + EXPECT_EQ(5, root.getLength()); + + items[5].insort(root); + + EXPECT_EQ(6, root.getLength()); + + int prev_val = -100500; + const ListItem* item = root.get(); + while (item) + { + //std::cout << item->value << std::endl; + EXPECT_LT(prev_val, item->value); + prev_val = item->value; + item = item->getNextListNode(); + } +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/util/map.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/util/map.cpp new file mode 100644 index 000000000000..89c2faf8163a --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/util/map.cpp @@ -0,0 +1,224 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#include +#include +#include +#include +#include + + +/* + * TODO: This one test has been temporarily disabled because it is not compatible with newer versions of libstdc++ + * that ship with newer versions of GCC. The problem is that std::string has become too large to fit into a 64-byte + * large memory block. This should be fixed in the future. + */ +#if 0 +static std::string toString(long x) +{ + char buf[80]; + std::snprintf(buf, sizeof(buf), "%li", x); + return std::string(buf); +} + +static bool oddValuePredicate(const std::string& key, const std::string& value) +{ + EXPECT_FALSE(key.empty()); + EXPECT_FALSE(value.empty()); + const int num = atoi(value.c_str()); + return num & 1; +} + +struct KeyFindPredicate +{ + const std::string target; + KeyFindPredicate(std::string target) : target(target) { } + bool operator()(const std::string& key, const std::string&) const { return key == target; } +}; + +struct ValueFindPredicate +{ + const std::string target; + ValueFindPredicate(std::string target) : target(target) { } + bool operator()(const std::string&, const std::string& value) const { return value == target; } +}; + + +TEST(Map, Basic) +{ + using uavcan::Map; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + + typedef Map MapType; + std::unique_ptr map(new MapType(pool)); + + // Empty + ASSERT_FALSE(map->access("hi")); + map->remove("foo"); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_FALSE(map->getByIndex(0)); + ASSERT_FALSE(map->getByIndex(1)); + ASSERT_FALSE(map->getByIndex(10000)); + + // Insertion + ASSERT_EQ("a", *map->insert("1", "a")); + ASSERT_EQ("b", *map->insert("2", "b")); + ASSERT_EQ(1, pool.getNumUsedBlocks()); + ASSERT_EQ(2, map->getSize()); + + // Ordering + ASSERT_TRUE(map->getByIndex(0)->match("1")); + ASSERT_TRUE(map->getByIndex(1)->match("2")); + + // Insertion + ASSERT_EQ("c", *map->insert("3", "c")); + ASSERT_EQ(1, pool.getNumUsedBlocks()); + + ASSERT_EQ("d", *map->insert("4", "d")); + ASSERT_EQ(2, pool.getNumUsedBlocks()); // Assuming that at least 2 items fit one block + ASSERT_EQ(4, map->getSize()); + + // Making sure everything is here + ASSERT_EQ("a", *map->access("1")); + ASSERT_EQ("b", *map->access("2")); + ASSERT_EQ("c", *map->access("3")); + ASSERT_EQ("d", *map->access("4")); + ASSERT_FALSE(map->access("hi")); + + // Modifying existing entries + *map->access("1") = "A"; + *map->access("2") = "B"; + *map->access("3") = "C"; + *map->access("4") = "D"; + ASSERT_EQ("A", *map->access("1")); + ASSERT_EQ("B", *map->access("2")); + ASSERT_EQ("C", *map->access("3")); + ASSERT_EQ("D", *map->access("4")); + + // Finding some keys + ASSERT_EQ("1", *map->find(KeyFindPredicate("1"))); + ASSERT_EQ("2", *map->find(KeyFindPredicate("2"))); + ASSERT_EQ("3", *map->find(KeyFindPredicate("3"))); + ASSERT_EQ("4", *map->find(KeyFindPredicate("4"))); + ASSERT_FALSE(map->find(KeyFindPredicate("nonexistent_key"))); + + // Finding some values + ASSERT_EQ("1", *map->find(ValueFindPredicate("A"))); + ASSERT_EQ("2", *map->find(ValueFindPredicate("B"))); + ASSERT_EQ("3", *map->find(ValueFindPredicate("C"))); + ASSERT_EQ("4", *map->find(ValueFindPredicate("D"))); + ASSERT_FALSE(map->find(KeyFindPredicate("nonexistent_value"))); + + // Removing one + map->remove("1"); // One of dynamics now migrates to the static storage + map->remove("foo"); // There's no such thing anyway + ASSERT_EQ(2, pool.getNumUsedBlocks()); + ASSERT_EQ(3, map->getSize()); + + ASSERT_FALSE(map->access("1")); + ASSERT_EQ("B", *map->access("2")); + ASSERT_EQ("C", *map->access("3")); + ASSERT_EQ("D", *map->access("4")); + + // Removing another + map->remove("2"); + ASSERT_EQ(2, map->getSize()); + ASSERT_EQ(2, pool.getNumUsedBlocks()); + + ASSERT_FALSE(map->access("1")); + ASSERT_FALSE(map->access("2")); + ASSERT_EQ("C", *map->access("3")); + ASSERT_EQ("D", *map->access("4")); + + // Adding some new + unsigned max_key_integer = 0; + for (int i = 0; i < 100; i++) + { + const std::string key = toString(i); + const std::string value = toString(i); + std::string* res = map->insert(key, value); // Will override some from the above + if (res == UAVCAN_NULLPTR) + { + ASSERT_LT(2, i); + break; + } + else + { + ASSERT_EQ(value, *res); + } + max_key_integer = unsigned(i); + } + std::cout << "Max key/value: " << max_key_integer << std::endl; + ASSERT_LT(4, max_key_integer); + + // Making sure there is true OOM + ASSERT_EQ(0, pool.getNumFreeBlocks()); + ASSERT_FALSE(map->insert("nonexistent", "value")); + ASSERT_FALSE(map->access("nonexistent")); + ASSERT_FALSE(map->access("value")); + + // Removing odd values - nearly half of them + map->removeAllWhere(oddValuePredicate); + + // Making sure there's no odd values left + for (unsigned kv_int = 0; kv_int <= max_key_integer; kv_int++) + { + const std::string* val = map->access(toString(kv_int)); + if (val) + { + ASSERT_FALSE(kv_int & 1); + } + else + { + ASSERT_TRUE(kv_int & 1); + } + } + + // Making sure the memory will be released + map.reset(); + ASSERT_EQ(0, pool.getNumUsedBlocks()); +} +#endif + + +TEST(Map, PrimitiveKey) +{ + using uavcan::Map; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + + typedef Map MapType; + std::unique_ptr map(new MapType(pool)); + + // Empty + ASSERT_FALSE(map->access(1)); + map->remove(8); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_EQ(0, map->getSize()); + ASSERT_FALSE(map->getByIndex(0)); + + // Insertion + ASSERT_EQ(1, *map->insert(1, 1)); + ASSERT_EQ(1, map->getSize()); + ASSERT_EQ(2, *map->insert(2, 2)); + ASSERT_EQ(2, map->getSize()); + ASSERT_EQ(3, *map->insert(3, 3)); + ASSERT_EQ(4, *map->insert(4, 4)); + ASSERT_EQ(4, map->getSize()); + + // Ordering + ASSERT_TRUE(map->getByIndex(0)->match(1)); + ASSERT_TRUE(map->getByIndex(1)->match(2)); + ASSERT_TRUE(map->getByIndex(2)->match(3)); + ASSERT_TRUE(map->getByIndex(3)->match(4)); + ASSERT_FALSE(map->getByIndex(5)); + ASSERT_FALSE(map->getByIndex(1000)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/util/multiset.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/util/multiset.cpp new file mode 100644 index 000000000000..c3f227f86565 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/util/multiset.cpp @@ -0,0 +1,267 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#include +#include +#include +#include +#include + + +static std::string toString(long x) +{ + char buf[80]; + std::snprintf(buf, sizeof(buf), "%li", x); + return std::string(buf); +} + +static bool oddValuePredicate(const std::string& value) +{ + EXPECT_FALSE(value.empty()); + const int num = atoi(value.c_str()); + return num & 1; +} + +struct FindPredicate +{ + const std::string target; + FindPredicate(const std::string& target) : target(target) { } + bool operator()(const std::string& value) const { return value == target; } +}; + +struct NoncopyableWithCounter : uavcan::Noncopyable +{ + static int num_objects; + long long value; + + NoncopyableWithCounter() : value(0) { num_objects++; } + NoncopyableWithCounter(long long x) : value(x) { num_objects++; } + ~NoncopyableWithCounter() { num_objects--; } + + static bool isNegative(const NoncopyableWithCounter& val) + { + return val.value < 0; + } + + bool operator==(const NoncopyableWithCounter& ref) const { return ref.value == value; } +}; + +int NoncopyableWithCounter::num_objects = 0; + +template +struct SummationOperator : uavcan::Noncopyable +{ + T accumulator; + SummationOperator() : accumulator() { } + void operator()(const T& x) { accumulator += x; } +}; + +struct ClearingOperator +{ + template + void operator()(T& x) const { x = T(); } +}; + + +TEST(Multiset, Basic) +{ + using uavcan::Multiset; + + static const int POOL_BLOCKS = 4; + uavcan::PoolAllocator pool; + + typedef Multiset MultisetType; + std::unique_ptr mset(new MultisetType(pool)); + + typedef SummationOperator StringConcatenationOperator; + + // Empty + mset->removeFirst("foo"); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_FALSE(mset->getByIndex(0)); + ASSERT_FALSE(mset->getByIndex(1)); + ASSERT_FALSE(mset->getByIndex(10000)); + + // Static addion + ASSERT_EQ("1", *mset->emplace("1")); + ASSERT_EQ("2", *mset->emplace("2")); + ASSERT_LE(1, pool.getNumUsedBlocks()); // One or more + ASSERT_EQ(2, mset->getSize()); + + { + StringConcatenationOperator op; + mset->forEach(op); + ASSERT_EQ(2, op.accumulator.size()); + } + + // Dynamic addition + ASSERT_EQ("3", *mset->emplace("3")); + ASSERT_LE(1, pool.getNumUsedBlocks()); // One or more + + ASSERT_EQ("4", *mset->emplace("4")); + ASSERT_LE(1, pool.getNumUsedBlocks()); // One or more + ASSERT_EQ(4, mset->getSize()); + + ASSERT_FALSE(mset->getByIndex(100)); + ASSERT_FALSE(mset->getByIndex(4)); + + // Finding some items + ASSERT_EQ("1", *mset->find(FindPredicate("1"))); + ASSERT_EQ("2", *mset->find(FindPredicate("2"))); + ASSERT_EQ("3", *mset->find(FindPredicate("3"))); + ASSERT_EQ("4", *mset->find(FindPredicate("4"))); + ASSERT_FALSE(mset->find(FindPredicate("nonexistent"))); + + { + StringConcatenationOperator op; + mset->forEach(op); + std::cout << "Accumulator: " << op.accumulator << std::endl; + ASSERT_EQ(4, op.accumulator.size()); + } + + // Removing some + mset->removeFirst("1"); + mset->removeFirst("foo"); // There's no such thing anyway + mset->removeFirst("2"); + + // Adding some new items + unsigned max_value_integer = 0; + for (int i = 0; i < 100; i++) + { + const std::string value = toString(i); + std::string* res = mset->emplace(value); // Will NOT override above + if (res == UAVCAN_NULLPTR) + { + ASSERT_LT(1, i); + break; + } + else + { + ASSERT_EQ(value, *res); + } + max_value_integer = unsigned(i); + } + std::cout << "Max value: " << max_value_integer << std::endl; + + // Making sure there is true OOM + ASSERT_EQ(0, pool.getNumFreeBlocks()); + ASSERT_FALSE(mset->emplace("nonexistent")); + + // Removing odd values - nearly half of them + mset->removeAllWhere(oddValuePredicate); + + // Making sure there's no odd values left + for (unsigned kv_int = 0; kv_int <= max_value_integer; kv_int++) + { + const std::string* val = mset->find(FindPredicate(toString(kv_int))); + if (val) + { + ASSERT_FALSE(kv_int & 1); + } + else + { + ASSERT_TRUE(kv_int & 1); + } + } + + // Clearing all strings + { + StringConcatenationOperator op; + mset->forEach(op); + std::cout << "Accumulator before clearing: " << op.accumulator << std::endl; + } + mset->forEach(ClearingOperator()); + { + StringConcatenationOperator op; + mset->forEach(op); + std::cout << "Accumulator after clearing: " << op.accumulator << std::endl; + ASSERT_TRUE(op.accumulator.empty()); + } + + // Making sure the memory will be released + mset.reset(); + ASSERT_EQ(0, pool.getNumUsedBlocks()); +} + + +TEST(Multiset, PrimitiveKey) +{ + using uavcan::Multiset; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + + typedef Multiset MultisetType; + std::unique_ptr mset(new MultisetType(pool)); + + // Empty + mset->removeFirst(8); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_EQ(0, mset->getSize()); + ASSERT_FALSE(mset->getByIndex(0)); + + // Insertion + ASSERT_EQ(1, *mset->emplace(1)); + ASSERT_EQ(1, mset->getSize()); + ASSERT_EQ(2, *mset->emplace(2)); + ASSERT_EQ(2, mset->getSize()); + ASSERT_EQ(3, *mset->emplace(3)); + ASSERT_EQ(4, *mset->emplace(4)); + ASSERT_EQ(4, mset->getSize()); + + // Summation and clearing + { + SummationOperator summation_operator; + mset->forEach&>(summation_operator); + ASSERT_EQ(1 + 2 + 3 + 4, summation_operator.accumulator); + } + mset->forEach(ClearingOperator()); + { + SummationOperator summation_operator; + mset->forEach&>(summation_operator); + ASSERT_EQ(0, summation_operator.accumulator); + } +} + + +TEST(Multiset, NoncopyableWithCounter) +{ + using uavcan::Multiset; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + + typedef Multiset MultisetType; + std::unique_ptr mset(new MultisetType(pool)); + + ASSERT_EQ(0, NoncopyableWithCounter::num_objects); + ASSERT_EQ(0, mset->emplace()->value); + ASSERT_EQ(1, NoncopyableWithCounter::num_objects); + ASSERT_EQ(123, mset->emplace(123)->value); + ASSERT_EQ(2, NoncopyableWithCounter::num_objects); + ASSERT_EQ(-456, mset->emplace(-456)->value); + ASSERT_EQ(3, NoncopyableWithCounter::num_objects); + ASSERT_EQ(456, mset->emplace(456)->value); + ASSERT_EQ(4, NoncopyableWithCounter::num_objects); + ASSERT_EQ(-789, mset->emplace(-789)->value); + ASSERT_EQ(5, NoncopyableWithCounter::num_objects); + + mset->removeFirst(NoncopyableWithCounter(0)); + ASSERT_EQ(4, NoncopyableWithCounter::num_objects); + + mset->removeFirstWhere(&NoncopyableWithCounter::isNegative); + ASSERT_EQ(3, NoncopyableWithCounter::num_objects); + + mset->removeAllWhere(&NoncopyableWithCounter::isNegative); + ASSERT_EQ(2, NoncopyableWithCounter::num_objects); // Only 1 and 2 are left + + mset.reset(); + + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_EQ(0, NoncopyableWithCounter::num_objects); // All destroyed +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/util/templates.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/util/templates.cpp new file mode 100644 index 000000000000..dc234dd7f703 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/util/templates.cpp @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +struct NonConvertible { }; + +struct ConvertibleToBool +{ + const bool value; + ConvertibleToBool(bool value) : value(value) { } + operator bool() const { return value; } + bool operator!() const { return !value; } +}; + +struct NonDefaultConstructible +{ + NonDefaultConstructible(int) { } +}; + +TEST(Util, CoerceOrFallback) +{ + using uavcan::coerceOrFallback; + + ASSERT_FALSE(coerceOrFallback(NonConvertible())); + ASSERT_TRUE(coerceOrFallback(NonConvertible(), true)); + + ASSERT_EQ(0, coerceOrFallback(NonConvertible())); + ASSERT_EQ(9000, coerceOrFallback(NonConvertible(), 9000)); + + ASSERT_TRUE(coerceOrFallback(ConvertibleToBool(true))); + ASSERT_TRUE(coerceOrFallback(ConvertibleToBool(true), false)); + ASSERT_FALSE(coerceOrFallback(ConvertibleToBool(false))); + ASSERT_FALSE(coerceOrFallback(ConvertibleToBool(false), true)); + ASSERT_EQ(1, coerceOrFallback(ConvertibleToBool(true))); + ASSERT_EQ(0, coerceOrFallback(ConvertibleToBool(false), -100)); + + //coerceOrFallback(ConvertibleToBool(true)); // Will fail to compile + coerceOrFallback(NonConvertible(), NonDefaultConstructible(64)); +} + +TEST(Util, FloatClassification) +{ + // NAN + ASSERT_TRUE(uavcan::isNaN(std::numeric_limits::quiet_NaN())); + ASSERT_FALSE(uavcan::isNaN(std::numeric_limits::infinity())); + ASSERT_FALSE(uavcan::isNaN(std::numeric_limits::infinity())); + ASSERT_FALSE(uavcan::isNaN(123.456)); + + // INF + ASSERT_TRUE(uavcan::isInfinity(std::numeric_limits::infinity())); + ASSERT_TRUE(uavcan::isInfinity(-std::numeric_limits::infinity())); + ASSERT_FALSE(uavcan::isInfinity(std::numeric_limits::quiet_NaN())); + ASSERT_FALSE(uavcan::isInfinity(-0.1L)); + + // Signbit + ASSERT_FALSE(uavcan::getSignBit(12)); + ASSERT_TRUE(uavcan::getSignBit(-std::numeric_limits::infinity())); + ASSERT_FALSE(uavcan::getSignBit(std::numeric_limits::infinity())); + ASSERT_TRUE(uavcan::getSignBit(-0.0)); // Negative zero +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/tools/coverity_scan_model.cpp b/src/drivers/uavcan/libdronecan/libuavcan/tools/coverity_scan_model.cpp new file mode 100644 index 000000000000..e66bb785fa76 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/tools/coverity_scan_model.cpp @@ -0,0 +1,45 @@ +/* + * Coverity Scan model. + * + * - A model file can't import any header files. + * - Therefore only some built-in primitives like int, char and void are + * available but not wchar_t, NULL etc. + * - Modeling doesn't need full structs and typedefs. Rudimentary structs + * and similar types are sufficient. + * - An uninitialized local pointer is not an error. It signifies that the + * variable could be either NULL or have some data. + * + * Coverity Scan doesn't pick up modifications automatically. The model file + * must be uploaded by an admin in the analysis settings of + * https://scan.coverity.com/projects/1513 + */ + +namespace std +{ + typedef unsigned long size_t; +} + +namespace uavcan +{ + +void handleFatalError(const char* msg) +{ + __coverity_panic__(); +} + +template +class PoolAllocator +{ +public: + void* allocate(std::size_t size) + { + return __coverity_alloc__(size); + } + + void deallocate(const void* ptr) + { + __coverity_free__(ptr); + } +}; + +} diff --git a/src/drivers/uavcan/libuavcan b/src/drivers/uavcan/libuavcan deleted file mode 160000 index dce2d4e7d8f4..000000000000 --- a/src/drivers/uavcan/libuavcan +++ /dev/null @@ -1 +0,0 @@ -Subproject commit dce2d4e7d8f41348e571481cd2e4788ac8af900d diff --git a/src/drivers/uavcan/uavcan_drivers/posix/CMakeLists.txt b/src/drivers/uavcan/uavcan_drivers/posix/CMakeLists.txt new file mode 100644 index 000000000000..c7335e837b56 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/posix/CMakeLists.txt @@ -0,0 +1,12 @@ +# +# Copyright (C) 2015 Pavel Kirienko +# + +cmake_minimum_required(VERSION 3.9) + +project(libuavcan_posix) + +# +# Library (header only) +# +install(DIRECTORY include/uavcan_posix DESTINATION include) diff --git a/src/drivers/uavcan/uavcan_drivers/posix/include.mk b/src/drivers/uavcan/uavcan_drivers/posix/include.mk new file mode 100644 index 000000000000..84ad8e174484 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/posix/include.mk @@ -0,0 +1,7 @@ +# +# Copyright (C) 2015 David Sidrane +# + +LIBUAVCAN_POSIX_DIR := $(dir $(lastword $(MAKEFILE_LIST))) + +LIBUAVCAN_POSIX_INC := $(LIBUAVCAN_POSIX_DIR)include/ diff --git a/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp new file mode 100644 index 000000000000..b58a40f34e91 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -0,0 +1,473 @@ +/**************************************************************************** +* +* Copyright (c) 2015, 2021 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_BASIC_FILE_SERVER_BACKEND_HPP_INCLUDED +#define UAVCAN_POSIX_BASIC_FILE_SERVER_BACKEND_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan_posix +{ +/** + * This interface implements a POSIX compliant IFileServerBackend interface + */ +class BasicFileServerBackend : public uavcan::IFileServerBackend +{ + enum { FilePermissions = 438 }; ///< 0o666 + +protected: + + class FDCacheBase + { + public: + FDCacheBase() { } + virtual ~FDCacheBase() { } + + virtual int open(const char *path, int oflags) + { + using namespace std; + + return ::open(path, oflags); + } + + virtual int close(int fd, bool done = true) + { + (void)done; + using namespace std; + + return ::close(fd); + } + + virtual void init() { } + }; + + FDCacheBase fallback_; + + class FDCache : public FDCacheBase, protected uavcan::TimerBase + { + /// Age in Seconds an entry will stay in the cache if not accessed. + enum { MaxAgeSeconds = 7 }; + + /// Rate in Seconds that the cache will be flushed of stale entries. + enum { GarbageCollectionSeconds = 60 }; + + IFileServerBackend::Path &alt_root_path_; + IFileServerBackend::Path &root_path_; + + class FDCacheItem : uavcan::Noncopyable + { + friend FDCache; + + FDCacheItem *next_; + std::time_t last_access_; + const int fd_; + const int oflags_; + const char *const path_; + + public: + enum { InvalidFD = -1 }; + + FDCacheItem() : + next_(UAVCAN_NULLPTR), + last_access_(0), + fd_(InvalidFD), + oflags_(0), + path_(UAVCAN_NULLPTR) + { } + + FDCacheItem(int fd, const char *path, int oflags) : + next_(UAVCAN_NULLPTR), + last_access_(0), + fd_(fd), + oflags_(oflags), + path_(::strndup(path, uavcan::protocol::file::Path::FieldTypes::path::MaxSize)) + { } + + ~FDCacheItem() + { + using namespace std; + + if (valid()) { + ::free(const_cast(path_)); + } + } + + bool valid() const + { + return path_ != UAVCAN_NULLPTR; + } + + int getFD() const + { + return fd_; + } + + std::time_t getAccess() const + { + return last_access_; + } + + std::time_t acessed() + { + using namespace std; + last_access_ = time(UAVCAN_NULLPTR); + return getAccess(); + } + + void expire() + { + last_access_ = 0; + } + + bool expired() const + { + using namespace std; + return 0 == last_access_ || (time(UAVCAN_NULLPTR) - last_access_) > MaxAgeSeconds; + } + + bool equals(const char *path, int oflags) const + { + using namespace std; + return oflags_ == oflags && 0 == ::strcmp(path, path_); + } + + bool equals(int fd) const + { + return fd_ == fd; + } + }; + + FDCacheItem *head_; + + FDCacheItem *find(const char *path, int oflags) + { + for (FDCacheItem *pi = head_; pi; pi = pi->next_) { + if (pi->equals(path, oflags)) { + return pi; + } + } + + return UAVCAN_NULLPTR; + } + + FDCacheItem *find(int fd) + { + for (FDCacheItem *pi = head_; pi; pi = pi->next_) { + if (pi->equals(fd)) { + return pi; + } + } + + return UAVCAN_NULLPTR; + } + + FDCacheItem *add(FDCacheItem *pi) + { + pi->next_ = head_; + head_ = pi; + pi->acessed(); + return pi; + } + + void removeExpired(FDCacheItem **pi) + { + while (*pi) { + if ((*pi)->expired()) { + FDCacheItem *next = (*pi)->next_; + (void)FDCacheBase::close((*pi)->fd_); + delete (*pi); + *pi = next; + continue; + } + + pi = &(*pi)->next_; + } + } + + void remove(FDCacheItem *pi, bool done) + { + if (done) { + pi->expire(); + } + + removeExpired(&head_); + } + + void clear() + { + FDCacheItem *tmp; + + for (FDCacheItem *pi = head_; pi; pi = tmp) { + tmp = pi->next_; + (void)FDCacheBase::close(pi->fd_); + delete pi; + } + } + + /* Removed stale entries. In the normal case a node will read the + * complete contents of a file and the read of the last block will + * cause the method remove() to be invoked with done true. Thereby + * flushing the entry from the cache. But if the node does not + * stay the course of the read, it may leave a dangling entry. + * This call back handles the garbage collection. + */ + virtual void handleTimerEvent(const uavcan::TimerEvent &) + { + removeExpired(&head_); + } + + public: + FDCache(uavcan::INode &node, IFileServerBackend::Path &root_path, IFileServerBackend::Path &alt_root_path) : + TimerBase(node), + alt_root_path_(alt_root_path), + root_path_(root_path), + head_(UAVCAN_NULLPTR) + { } + + virtual ~FDCache() + { + stop(); + clear(); + } + + virtual void init() + { + startPeriodic(uavcan::MonotonicDuration::fromMSec(GarbageCollectionSeconds * 1000)); + } + + virtual int open(const char *path, int oflags) + { + int fd = FDCacheItem::InvalidFD; + + FDCacheItem *pi = find(path, oflags); + + if (pi != UAVCAN_NULLPTR) { + pi->acessed(); + + } else { + Path vpath = root_path_.c_str(); + vpath += path; + + fd = FDCacheBase::open(vpath.c_str(), oflags); + + if (fd < 0) { + vpath = alt_root_path_.c_str(); + vpath += path; + fd = FDCacheBase::open(vpath.c_str(), oflags); + } + + if (fd < 0) { + return fd; + } + + /* Allocate and clone path */ + + pi = new FDCacheItem(fd, path, oflags); + + /* Allocation worked but check clone */ + + if (pi && !pi->valid()) { + /* Allocation worked but clone or path failed */ + delete pi; + pi = UAVCAN_NULLPTR; + } + + if (pi == UAVCAN_NULLPTR) { + /* + * If allocation fails no harm just can not cache it + * return open fd + */ + return fd; + } + + /* add new */ + add(pi); + } + + return pi->getFD(); + } + + virtual int close(int fd, bool done) + { + FDCacheItem *pi = find(fd); + + if (pi == UAVCAN_NULLPTR) { + /* + * If not found just close it + */ + return FDCacheBase::close(fd); + } + + remove(pi, done); + return 0; + } + }; + + FDCacheBase *fdcache_; + uavcan::INode &node_; + + FDCacheBase &getFDCache() + { + if (fdcache_ == UAVCAN_NULLPTR) { + fdcache_ = new FDCache(node_, getRootPath(), getAltRootPath()); + + if (fdcache_ == UAVCAN_NULLPTR) { + fdcache_ = &fallback_; + } + + fdcache_->init(); + } + + return *fdcache_; + } + + /** + * Back-end for uavcan.protocol.file.GetInfo. + * Implementation of this method is required. + * On success the method must return zero. + */ + virtual uavcan::int16_t getInfo(const Path &path, uavcan::uint64_t &out_size, EntryType &out_type) + { + int rv = uavcan::protocol::file::Error::INVALID_VALUE; + + if (path.size() > 0) { + + using namespace std; + + struct stat sb; + + Path vpath = getRootPath().c_str(); + vpath += path; + + rv = stat(vpath.c_str(), &sb); + + if (rv < 0) { + vpath = getAltRootPath().c_str(); + vpath += path; + rv = stat(vpath.c_str(), &sb); + } + + if (rv < 0) { + rv = errno; + + } else { + rv = 0; + out_size = sb.st_size; + out_type.flags = uavcan::protocol::file::EntryType::FLAG_READABLE; + + if (S_ISDIR(sb.st_mode)) { + out_type.flags |= uavcan::protocol::file::EntryType::FLAG_DIRECTORY; + + } else if (S_ISREG(sb.st_mode)) { + out_type.flags |= uavcan::protocol::file::EntryType::FLAG_FILE; + } + + // TODO Using fixed flag FLAG_READABLE until we add file permission checks to return actual value. + } + } + + return rv; + } + + /** + * Back-end for uavcan.protocol.file.Read. + * Implementation of this method is required. + * @ref inout_size is set to @ref ReadSize; read operation is required to return exactly this amount, except + * if the end of file is reached. + * On success the method must return zero. + */ + virtual uavcan::int16_t read(const Path &path, const uavcan::uint64_t offset, uavcan::uint8_t *out_buffer, + uavcan::uint16_t &inout_size) + { + int rv = uavcan::protocol::file::Error::INVALID_VALUE; + + if (path.size() > 0 && inout_size != 0) { + using namespace std; + + FDCacheBase &cache = getFDCache(); + + int fd = cache.open(path.c_str(), O_RDONLY); + + if (fd < 0) { + rv = -errno; + + } else { + ssize_t total_read = 0; + + rv = ::lseek(fd, offset, SEEK_SET); + + if (rv < 0) { + rv = -errno; + + } else { + rv = 0; + ssize_t remaining = inout_size; + ssize_t nread = 0; + + do { + nread = ::read(fd, &out_buffer[total_read], remaining); + + if (nread < 0) { + rv = errno; + + } else { + remaining -= nread, + total_read += nread; + } + } while (nread > 0 && remaining > 0); + } + + (void)cache.close(fd, rv != 0 || total_read != inout_size); + inout_size = total_read; + } + } + + return rv; + } + +public: + BasicFileServerBackend(uavcan::INode &node) : + fdcache_(UAVCAN_NULLPTR), + node_(node) + { } + + ~BasicFileServerBackend() + { + if (fdcache_ != &fallback_) { + delete fdcache_; + fdcache_ = UAVCAN_NULLPTR; + } + } +}; + +#if __GNUC__ +/// Typo fix in a backwards-compatible way (only for GCC projects). Will be removed someday. +typedef BasicFileServerBackend +BasicFileSeverBackend // Missing 'r' +__attribute__((deprecated)); +#endif + +} + +#endif // Include guard diff --git a/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp new file mode 100644 index 000000000000..fe6129318dcc --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp @@ -0,0 +1,100 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_EVENT_TRACER_HPP_INCLUDED +#define UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_EVENT_TRACER_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan_posix +{ +namespace dynamic_node_id_server +{ +/** + * This interface implements a POSIX compliant file based IEventTracer interface + */ +class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer +{ + /** + * Maximum length of full path to log file + */ + enum { MaxPathLength = 128 }; + + enum { FilePermissions = 438 }; ///< 0o666 + + /** + * This type is used for the path + */ + typedef uavcan::MakeString::Type PathString; + + PathString path_; + +protected: + virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) + { + using namespace std; + + timespec ts = timespec(); // If clock_gettime() fails, zero time will be used + (void)clock_gettime(CLOCK_REALTIME, &ts); + + int fd = open(path_.c_str(), O_WRONLY | O_CREAT | O_APPEND, FilePermissions); + + if (fd >= 0) { + const int FormatBufferLength = 63; + char buffer[FormatBufferLength + 1]; + ssize_t remaining = snprintf(buffer, FormatBufferLength, "%ld.%06ld\t%d\t%lld\n", + static_cast(ts.tv_sec), static_cast(ts.tv_nsec / 1000L), + static_cast(code), static_cast(argument)); + + ssize_t total_written = 0; + ssize_t written = 0; + + do { + written = write(fd, &buffer[total_written], remaining); + + if (written > 0) { + total_written += written; + remaining -= written; + } + } while (written > 0 && remaining > 0); + + (void)close(fd); + } + } + +public: + /** + * Initializes the file based event tracer. + */ + int init(const PathString &path) + { + using namespace std; + + int rv = -uavcan::ErrInvalidParam; + + if (path.size() > 0) { + rv = 0; + path_ = path.c_str(); + int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC, FilePermissions); + + if (fd >= 0) { + (void)close(fd); + } + } + + return rv; + } +}; +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp new file mode 100644 index 000000000000..1989cc98dadc --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -0,0 +1,158 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_STORAGE_BACKEND_HPP_INCLUDED +#define UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_STORAGE_BACKEND_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace uavcan_posix +{ +namespace dynamic_node_id_server +{ +/** + * This interface implements a POSIX compliant IStorageBackend interface + */ +class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBackend +{ + /** + * Maximum length of full path including / and key max + */ + enum { MaxPathLength = 128 }; + + enum { FilePermissions = 438 }; ///< 0o666 + + /** + * This type is used for the path + */ + typedef uavcan::MakeString::Type PathString; + + PathString base_path; + +protected: + virtual String get(const String &key) const + { + using namespace std; + PathString path = base_path.c_str(); + path += key; + String value; + int fd = open(path.c_str(), O_RDONLY); + + if (fd >= 0) { + char buffer[MaxStringLength + 1]; + (void)memset(buffer, 0, sizeof(buffer)); + ssize_t remaining = MaxStringLength; + ssize_t total_read = 0; + ssize_t nread = 0; + + do { + nread = ::read(fd, &buffer[total_read], remaining); + + if (nread > 0) { + remaining -= nread, + total_read += nread; + } + } while (nread > 0 && remaining > 0); + + (void)close(fd); + + if (total_read > 0) { + for (int i = 0; i < total_read; i++) { + if (buffer[i] == ' ' || buffer[i] == '\n' || buffer[i] == '\r') { + buffer[i] = '\0'; + break; + } + } + + value = buffer; + } + } + + return value; + } + + virtual void set(const String &key, const String &value) + { + using namespace std; + PathString path = base_path.c_str(); + path += key; + int fd = open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC, FilePermissions); + + if (fd >= 0) { + ssize_t remaining = value.size(); + ssize_t total_written = 0; + ssize_t written = 0; + + do { + written = write(fd, &value.c_str()[total_written], remaining); + + if (written > 0) { + total_written += written; + remaining -= written; + } + } while (written > 0 && remaining > 0); + + (void)fsync(fd); + (void)close(fd); + } + } + +public: + /** + * Initializes the file based backend storage by passing a path to + * the directory where the key named files will be stored. + * The return value should be 0 on success. + * If it is -ErrInvalidConfiguration then the the path name is too long to + * accommodate the trailing slash and max key length. + */ + int init(const PathString &path) + { + using namespace std; + + int rv = -uavcan::ErrInvalidParam; + + if (path.size() > 0) { + base_path = path.c_str(); + + if (base_path.back() == '/') { + base_path.pop_back(); + } + + rv = 0; + struct stat sb; + + if (stat(base_path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode)) { + // coverity[toctou] + rv = mkdir(base_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); + } + + if (rv >= 0) { + base_path.push_back('/'); + + if ((base_path.size() + MaxStringLength) > MaxPathLength) { + rv = -uavcan::ErrInvalidConfiguration; + } + } + } + + return rv; + } +}; +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp new file mode 100644 index 000000000000..ddea2bbef5d6 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -0,0 +1,319 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_FIRMWARE_VERSION_CHECKER_HPP_INCLUDED +#define UAVCAN_POSIX_FIRMWARE_VERSION_CHECKER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +#include + +// TODO Get rid of the macro +#if !defined(DIRENT_ISFILE) && defined(DT_REG) +# define DIRENT_ISFILE(dtype) ((dtype) == DT_REG) +#endif + +#define ALT_APD_SIGNATURE 0x40, 0xa2, 0xe4, 0xf1, 0x64, 0x68, 0x91, 0x06 + +namespace uavcan_posix +{ +/** + * Firmware version checking logic. + * Refer to @ref FirmwareUpdateTrigger for details. + */ +class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker +{ + enum { FilePermissions = 438 }; ///< 0o666 + + enum { MaxBasePathLength = 128 }; + + /** + * This type is used for the base path + */ + typedef uavcan::MakeString::Type BasePathString; + + /** + * Maximum length of full path including / the file name + */ + enum { MaxPathLength = uavcan::protocol::file::Path::FieldTypes::path::MaxSize + MaxBasePathLength }; + + /** + * This type is used internally for the full path to file + */ + typedef uavcan::MakeString::Type PathString; + + BasePathString base_path_; + BasePathString alt_base_path_; + + static void addSlash(BasePathString &path) + { + if (path.back() != getPathSeparator()) { + path.push_back(getPathSeparator()); + } + } + + static void removeSlash(BasePathString &path) + { + if (path.back() == getPathSeparator()) { + path.pop_back(); + } + } + + void setFirmwareBasePath(const char *path) + { + base_path_ = path; + } + + void setFirmwareAltBasePath(const char *path) + { + alt_base_path_ = path; + } + +protected: + /** + * This method will be invoked when the class obtains a response to GetNodeInfo request. + * + * @param node_id Node ID that this GetNodeInfo response was received from. + * + * @param node_info Actual node info structure; refer to uavcan.protocol.GetNodeInfo for details. + * + * @param out_firmware_file_path The implementation should return the firmware image path via this argument. + * Note that this path must be reachable via uavcan.protocol.file.Read service. + * Refer to @ref FileServer and @ref BasicFileServer for details. + * + * @return True - the class will begin sending update requests. + * False - the node will be ignored, no request will be sent. + */ + virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID, + const uavcan::protocol::GetNodeInfo::Response &node_info, + FirmwareFilePath &out_firmware_file_path) + { + using namespace std; + + /* This is a work around for two issues. + * 1) FirmwareFilePath is 40 + * 2) OK using is using 32 for max file names. + * + * So for the file: + * org.pixhawk.px4cannode-v1-0.1.59efc137.uavcan.bin + * +---ufw + * +- board_id.bin + */ + bool rv = false; + uint16_t board_id = (node_info.hardware_version.major << 8) + node_info.hardware_version.minor; + char bin_file_name[MaxBasePathLength + 1]; + int n = snprintf(bin_file_name, sizeof(bin_file_name), "%u.bin", board_id); + + if (n > 0 && n < (int)sizeof(bin_file_name) - 2) { + char bin_file_path[MaxBasePathLength + 1]; + + // Look on Primary location + + snprintf(bin_file_path, sizeof(bin_file_path), "%s%s", + getFirmwareBasePath().c_str(), bin_file_name); + + // We have a file path, is it a valid image + + AppDescriptor descriptor{0}; + + bool found = getFileInfo(bin_file_path, descriptor) == 0; + + if (!found && !getFirmwareAltBasePath().empty()) { + snprintf(bin_file_name, sizeof(bin_file_name), "%u.bin", board_id); + snprintf(bin_file_path, sizeof(bin_file_path), "%s/%s", + getFirmwareAltBasePath().c_str(), bin_file_name); + + found = getFileInfo(bin_file_path, descriptor) == 0; + } + + if (found && (node_info.software_version.image_crc == 0 || + (node_info.software_version.major == 0 && node_info.software_version.minor == 0) || + descriptor.image_crc != node_info.software_version.image_crc)) { + rv = true; + out_firmware_file_path = bin_file_name; + } + } + + return rv; + } + + /** + * This method will be invoked when a node responds to the update request with an error. If the request simply + * times out, this method will not be invoked. + * Note that if by the time of arrival of the response the node is already removed, this method will not be called. + * + * SPECIAL CASE: If the node responds with ERROR_IN_PROGRESS, the class will assume that further requesting + * is not needed anymore. This method will not be invoked. + * + * @param node_id Node ID that returned this error. + * + * @param error_response Contents of the error response. It contains error code and text. + * + * @param out_firmware_file_path New firmware path if a retry is needed. Note that this argument will be + * initialized with old path, so if the same path needs to be reused, this + * argument should be left unchanged. + * + * @return True - the class will continue sending update requests with new firmware path. + * False - the node will be forgotten, new requests will not be sent. + */ + virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID, + const uavcan::protocol::file::BeginFirmwareUpdate::Response &, + FirmwareFilePath &) + { + // TODO: Limit the number of attempts per node + return true; + } + +public: + struct AppDescriptor { + uavcan::uint8_t signature[sizeof(uavcan::uint64_t)]; + union { + uavcan::uint64_t image_crc; + uavcan::uint32_t crc32_block1; + uavcan::uint32_t crc32_block2; + }; + uavcan::uint32_t image_size; + uavcan::uint32_t vcs_commit; + uavcan::uint8_t major_version; + uavcan::uint8_t minor_version; + uavcan::uint16_t board_id; + uavcan::uint8_t reserved[ 3 + 3 + 2]; + }; + + static int getFileInfo(const char *path, AppDescriptor &descriptor, int limit = 0) + { + using namespace std; + + const unsigned MaxChunk = 512 / sizeof(uint64_t); + + // Make sure this does not present as a valid descriptor + struct { + union { + uavcan::uint64_t l; + uavcan::uint8_t b[sizeof(uint64_t)] {ALT_APD_SIGNATURE}; + } signature; + uavcan::uint8_t zeropad[sizeof(AppDescriptor) - sizeof(uint64_t)] {0}; + } s; + + int rv = -ENOENT; + uint64_t chunk[MaxChunk]; + int fd = open(path, O_RDONLY); + + if (fd >= 0) { + AppDescriptor *pdescriptor = UAVCAN_NULLPTR; + + while (pdescriptor == UAVCAN_NULLPTR && limit >= 0) { + int len = read(fd, chunk, sizeof(chunk)); + + if (len == 0) { + break; + } + + if (len < 0) { + rv = -errno; + goto out_close; + } + + uint64_t *p = &chunk[0]; + + if (limit > 0) { + limit -= sizeof(chunk); + } + + do { + if (*p == s.signature.l) { + pdescriptor = reinterpret_cast(p); // FIXME TODO This breaks strict aliasing + descriptor = *pdescriptor; + rv = 0; + break; + } + } while (p++ <= &chunk[MaxChunk - (sizeof(AppDescriptor) / sizeof(chunk[0]))]); + } + +out_close: + (void)close(fd); + } + + return rv; + } + + const BasePathString &getFirmwareBasePath() const { return base_path_; } + + const BasePathString &getFirmwareAltBasePath() const { return alt_base_path_; } + + static char getPathSeparator() + { + return static_cast(uavcan::protocol::file::Path::SEPARATOR); + } + + /** + * Creates the Directories were the files will be stored + * + * This is directory structure is in support of a workaround + * for the issues that FirmwareFilePath is 40 + * + * It creates a path structure: + * +---(base_path) <----------- Files are here. + */ + + int createFwPaths(const char *base_path, const char *alt_base_path = nullptr) + { + using namespace std; + int rv = -uavcan::ErrInvalidParam; + + if (alt_base_path) { + const int len = strlen(alt_base_path); + + if (len > 0 && len < base_path_.MaxSize) { + setFirmwareAltBasePath(alt_base_path); + + } else { + return rv; + } + } + + if (base_path) { + const int len = strlen(base_path); + + if (len > 0 && len < base_path_.MaxSize) { + setFirmwareBasePath(base_path); + removeSlash(base_path_); + const char *path = getFirmwareBasePath().c_str(); + + rv = 0; + struct stat sb; + + if (stat(path, &sb) != 0 || !S_ISDIR(sb.st_mode)) { + rv = mkdir(path, S_IRWXU | S_IRWXG | S_IRWXO); + } + + addSlash(base_path_); + } + } + + return rv; + } + + const char *getFirmwarePath() const + { + return getFirmwareBasePath().c_str(); + } + + const char *getAltFirmwarePath() const + { + return getFirmwareAltBasePath().c_str(); + } +}; +} + +#endif // Include guard diff --git a/src/drivers/uavcannode/CMakeLists.txt b/src/drivers/uavcannode/CMakeLists.txt index 21366ad0646c..d170bde437ed 100644 --- a/src/drivers/uavcannode/CMakeLists.txt +++ b/src/drivers/uavcannode/CMakeLists.txt @@ -31,10 +31,13 @@ # ############################################################################ -set(LIBUAVCAN_DIR ${PX4_SOURCE_DIR}/src/drivers/uavcan/libuavcan) -set(LIBUAVCAN_DIR_DRIVERS ${PX4_SOURCE_DIR}/src/drivers/uavcan/uavcan_drivers) +set(LIBDRONECAN_DIR ${PX4_SOURCE_DIR}/src/drivers/uavcan/libdronecan) +set(LIBDRONECAN_DIR_DRIVERS ${PX4_SOURCE_DIR}/src/drivers/uavcan/uavcan_drivers) -px4_add_git_submodule(TARGET git_uavcan PATH ${LIBUAVCAN_DIR}) +set(DSDLC_DIR "${PX4_SOURCE_DIR}/src/drivers/uavcan/libdronecan/dsdl") + +px4_add_git_submodule(TARGET git_uavcan_dsdl PATH ${DSDLC_DIR}) +px4_add_git_submodule(TARGET git_uavcan_pydronecan PATH ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/pydronecan) set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03") set(UAVCAN_PLATFORM "generic") @@ -89,25 +92,24 @@ add_compile_options( -Wno-address-of-packed-member ) set(CMAKE_WARN_DEPRECATED OFF CACHE BOOL "" FORCE) # silence libuavcan deprecation warning for now (TODO: fix and remove) -add_subdirectory(${LIBUAVCAN_DIR} libuavcan EXCLUDE_FROM_ALL) +add_subdirectory(${LIBDRONECAN_DIR} libdronecan EXCLUDE_FROM_ALL) add_dependencies(uavcan prebuild_targets) # driver -add_subdirectory(${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver libuavcan_drivers EXCLUDE_FROM_ALL) +add_subdirectory(${LIBDRONECAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver libuavcan_drivers EXCLUDE_FROM_ALL) target_include_directories(uavcan_${UAVCAN_DRIVER}_driver PUBLIC - ${LIBUAVCAN_DIR}/libuavcan/include - ${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated + ${LIBDRONECAN_DIR}/libuavcan/include + ${LIBDRONECAN_DIR}/libuavcan/include/dsdlc_generated ) # generated DSDL -set(DSDLC_DIR "${PX4_SOURCE_DIR}/src/drivers/uavcan/dsdl") set(DSDLC_INPUTS - "${LIBUAVCAN_DIR}/dsdl/ardupilot" - "${LIBUAVCAN_DIR}/dsdl/com" - "${LIBUAVCAN_DIR}/dsdl/cuav" - "${LIBUAVCAN_DIR}/dsdl/dronecan" - "${LIBUAVCAN_DIR}/dsdl/uavcan" + "${DSDLC_DIR}/ardupilot" + "${DSDLC_DIR}/com" + "${DSDLC_DIR}/cuav" + "${DSDLC_DIR}/dronecan" + "${DSDLC_DIR}/uavcan" ) set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated") @@ -118,7 +120,7 @@ foreach(DSDLC_INPUT ${DSDLC_INPUTS}) endforeach(DSDLC_INPUT) add_custom_command(OUTPUT px4_uavcan_dsdlc_run.stamp COMMAND - ${PYTHON_EXECUTABLE} ${LIBUAVCAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc + ${PYTHON_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc --outdir ${DSDLC_OUTPUT} ${DSDLC_INPUTS} #--verbose COMMAND ${CMAKE_COMMAND} -E touch px4_uavcan_dsdlc_run.stamp @@ -135,10 +137,10 @@ px4_add_module( #-DDEBUG_BUILD INCLUDES ${DSDLC_OUTPUT} - ${LIBUAVCAN_DIR}/libuavcan/include - ${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated - ${LIBUAVCAN_DIR}/libuavcan_drivers/posix/include - ${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver/include + ${LIBDRONECAN_DIR}/libuavcan/include + ${LIBDRONECAN_DIR}/libuavcan/include/dsdlc_generated + ${LIBDRONECAN_DIR}/libuavcan/posix/include + ${LIBDRONECAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver/include SRCS allocator.hpp uavcan_driver.hpp @@ -153,7 +155,9 @@ px4_add_module( drivers_bootloaders version conversion - git_uavcan + + git_uavcan_dsdl + git_uavcan_pydronecan uavcan_${UAVCAN_DRIVER}_driver # within libuavcan