mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-22 06:04:18 +08:00
drivers/uavcan: subtree merge last working libuavcan (preserving history) (#23819)
- fully absorb the libuavcan submodule (renamed libdronecan to deconflict) up to our last good working commit and preserve all history (upstream libuavcan was broken and then marked deprecated) - fixes https://github.com/PX4/PX4-Autopilot/issues/23727 - this puts us in a much better position to be able to evolve the library going forward ow that we have full control in tree
This commit is contained in:
Daniel Agar
GitHub
+6
-4
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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 \
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -0,0 +1,113 @@
|
||||
#
|
||||
# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
#
|
||||
|
||||
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 :
|
||||
@@ -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 ""
|
||||
)
|
||||
@@ -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.
|
||||
@@ -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.
|
||||
Submodule
+1
Submodule src/drivers/uavcan/libdronecan/dsdl added at 993be80a62
@@ -0,0 +1,146 @@
|
||||
#
|
||||
# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
#
|
||||
|
||||
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 :
|
||||
+18
@@ -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 $@
|
||||
+313
@@ -0,0 +1,313 @@
|
||||
#!/usr/bin/env python
|
||||
#
|
||||
# UAVCAN DSDL compiler for libuavcan
|
||||
#
|
||||
# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
#
|
||||
|
||||
'''
|
||||
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: ==> <!--(foo)-->
|
||||
template_text = re.sub(r'(?m)^(\ *)\%\ *(.+?):{0,1}$', r'\1<!--(\2)-->', template_text)
|
||||
|
||||
# Block termination transformation: <!--(endfoo)--> ==> <!--(end)-->
|
||||
template_text = re.sub(r'\<\!--\(end[a-z]+\)--\>', r'<!--(end)-->', 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
|
||||
+558
File diff suppressed because it is too large
Load Diff
Executable
+1225
File diff suppressed because it is too large
Load Diff
@@ -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 <pavel.kirienko@gmail.com>
|
||||
#
|
||||
|
||||
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))
|
||||
Submodule src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan added at 19fdf2e5b3
@@ -0,0 +1,276 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#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. <functional>), 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 <string>
|
||||
#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 <new>.
|
||||
*/
|
||||
#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 <typename T>
|
||||
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
|
||||
@@ -0,0 +1,176 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_DATA_TYPE_HPP_INCLUDED
|
||||
#define UAVCAN_DATA_TYPE_HPP_INCLUDED
|
||||
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
#include <uavcan/std.hpp>
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/transport/transfer.hpp>
|
||||
|
||||
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<uint16_t>(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
|
||||
@@ -0,0 +1,35 @@
|
||||
/*
|
||||
* Debug stuff, should only be used for library development.
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_DEBUG_HPP_INCLUDED
|
||||
#define UAVCAN_DEBUG_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/build_config.hpp>
|
||||
|
||||
#if UAVCAN_DEBUG
|
||||
|
||||
# include <cstdio>
|
||||
# include <cstdarg>
|
||||
|
||||
# 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
|
||||
@@ -0,0 +1,249 @@
|
||||
/*
|
||||
* CAN bus driver interface.
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_DRIVER_CAN_HPP_INCLUDED
|
||||
#define UAVCAN_DRIVER_CAN_HPP_INCLUDED
|
||||
|
||||
#include <cassert>
|
||||
#include <uavcan/std.hpp>
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/driver/system_clock.hpp>
|
||||
|
||||
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<ICanDriver*>(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
|
||||
@@ -0,0 +1,60 @@
|
||||
/*
|
||||
* System clock driver interface.
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_DRIVER_SYSTEM_CLOCK_HPP_INCLUDED
|
||||
#define UAVCAN_DRIVER_SYSTEM_CLOCK_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/std.hpp>
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/time.hpp>
|
||||
|
||||
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
|
||||
@@ -0,0 +1,210 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_DYNAMIC_MEMORY_HPP_INCLUDED
|
||||
#define UAVCAN_DYNAMIC_MEMORY_HPP_INCLUDED
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <uavcan/std.hpp>
|
||||
#include <uavcan/util/templates.hpp>
|
||||
#include <uavcan/util/placement_new.hpp>
|
||||
#include <uavcan/build_config.hpp>
|
||||
|
||||
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 <std::size_t PoolSize,
|
||||
uint8_t BlockSize,
|
||||
typename RaiiSynchronizer = char>
|
||||
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<uint16_t>(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<uint16_t>(min<std::size_t>(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 <std::size_t PoolSize, uint8_t BlockSize, typename RaiiSynchronizer>
|
||||
const uint16_t PoolAllocator<PoolSize, BlockSize, RaiiSynchronizer>::NumBlocks;
|
||||
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wcast-align"
|
||||
|
||||
template <std::size_t PoolSize, uint8_t BlockSize, typename RaiiSynchronizer>
|
||||
PoolAllocator<PoolSize, BlockSize, RaiiSynchronizer>::PoolAllocator() :
|
||||
free_list_(reinterpret_cast<Node*>(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 <std::size_t PoolSize, uint8_t BlockSize, typename RaiiSynchronizer>
|
||||
void* PoolAllocator<PoolSize, BlockSize, RaiiSynchronizer>::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 <std::size_t PoolSize, uint8_t BlockSize, typename RaiiSynchronizer>
|
||||
void PoolAllocator<PoolSize, BlockSize, RaiiSynchronizer>::deallocate(const void* ptr)
|
||||
{
|
||||
if (ptr == UAVCAN_NULLPTR)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
RaiiSynchronizer lock;
|
||||
(void)lock;
|
||||
|
||||
Node* p = static_cast<Node*>(const_cast<void*>(ptr));
|
||||
p->next = free_list_;
|
||||
free_list_ = p;
|
||||
|
||||
// Statistics
|
||||
UAVCAN_ASSERT(used_ > 0);
|
||||
used_--;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif // UAVCAN_DYNAMIC_MEMORY_HPP_INCLUDED
|
||||
@@ -0,0 +1,60 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_ERROR_HPP_INCLUDED
|
||||
#define UAVCAN_ERROR_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/std.hpp>
|
||||
|
||||
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
|
||||
+220
@@ -0,0 +1,220 @@
|
||||
/*
|
||||
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_HELPERS_HEAP_BASED_POOL_ALLOCATOR_HPP_INCLUDED
|
||||
#define UAVCAN_HELPERS_HEAP_BASED_POOL_ALLOCATOR_HPP_INCLUDED
|
||||
|
||||
#include <cstdlib>
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/debug.hpp>
|
||||
#include <uavcan/dynamic_memory.hpp>
|
||||
|
||||
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 <std::size_t BlockSize,
|
||||
typename RaiiSynchronizer = char>
|
||||
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<uint16_t>(min(static_cast<uint32_t>(block_capacity_soft_limit) * 2U,
|
||||
static_cast<uint32_t>(NumericTraits<uint16_t>::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<Node*>(const_cast<void*>(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
|
||||
@@ -0,0 +1,63 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_HELPERS_OSTREAM_HPP_INCLUDED
|
||||
#define UAVCAN_HELPERS_OSTREAM_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/util/templates.hpp>
|
||||
#include <cstdio>
|
||||
|
||||
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<int>(x)); }
|
||||
inline OStream& operator<<(OStream& s, unsigned short x) { return operator<<(s, static_cast<unsigned>(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<double>(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
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,93 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_MARSHAL_BIT_STREAM_HPP_INCLUDED
|
||||
#define UAVCAN_MARSHAL_BIT_STREAM_HPP_INCLUDED
|
||||
|
||||
#include <cassert>
|
||||
#include <uavcan/std.hpp>
|
||||
#include <uavcan/util/templates.hpp>
|
||||
#include <uavcan/transport/abstract_transfer_buffer.hpp>
|
||||
#include <uavcan/build_config.hpp>
|
||||
|
||||
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<const unsigned char*>(src_org), 0, src_len,
|
||||
reinterpret_cast<unsigned char*>(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<const unsigned char*>(src_org), src_offset, src_len,
|
||||
reinterpret_cast<unsigned char*>(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<sizeof(uint8_t) == 1>::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
|
||||
+142
@@ -0,0 +1,142 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED
|
||||
#define UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/marshal/array.hpp>
|
||||
|
||||
#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
|
||||
# error UAVCAN_CPP_VERSION
|
||||
#endif
|
||||
|
||||
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
|
||||
# include <type_traits>
|
||||
#endif
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
|
||||
|
||||
template <typename ArrayType_>
|
||||
class UAVCAN_EXPORT CharArrayFormatter
|
||||
{
|
||||
ArrayType_& array_;
|
||||
|
||||
template <typename T>
|
||||
typename std::enable_if<std::is_floating_point<T>::value>::type
|
||||
writeValue(T value)
|
||||
{
|
||||
array_.template appendFormatted<double>("%g", double(value));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
typename std::enable_if<std::is_integral<T>::value>::type
|
||||
writeValue(T value)
|
||||
{
|
||||
if (std::is_same<T, char>())
|
||||
{
|
||||
if (array_.size() != array_.capacity())
|
||||
{
|
||||
array_.push_back(typename ArrayType_::ValueType(value));
|
||||
}
|
||||
}
|
||||
else if (std::is_signed<T>())
|
||||
{
|
||||
array_.template appendFormatted<long long>("%lli", static_cast<long long>(value));
|
||||
}
|
||||
else
|
||||
{
|
||||
array_.template appendFormatted<unsigned long long>("%llu", static_cast<unsigned long long>(value));
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
typename std::enable_if<std::is_pointer<T>::value && !std::is_same<T, const char*>::value>::type
|
||||
writeValue(T value)
|
||||
{
|
||||
array_.template appendFormatted<const void*>("%p", static_cast<const void*>(value));
|
||||
}
|
||||
|
||||
void writeValue(const char* value)
|
||||
{
|
||||
array_.template appendFormatted<const char*>("%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 <typename T, typename... Args>
|
||||
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 <typename ArrayType_>
|
||||
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<const char*>("%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 <typename A>
|
||||
void write(const char* const format, const A value)
|
||||
{
|
||||
array_.template appendFormatted<A>(format, value);
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
#endif // UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED
|
||||
@@ -0,0 +1,238 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_MARSHAL_FLOAT_SPEC_HPP_INCLUDED
|
||||
#define UAVCAN_MARSHAL_FLOAT_SPEC_HPP_INCLUDED
|
||||
|
||||
#include <cmath>
|
||||
#include <uavcan/std.hpp>
|
||||
#include <uavcan/data_type.hpp>
|
||||
#include <uavcan/util/templates.hpp>
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/marshal/type_util.hpp>
|
||||
#include <uavcan/marshal/integer_spec.hpp>
|
||||
|
||||
#ifndef UAVCAN_CPP_VERSION
|
||||
# error UAVCAN_CPP_VERSION
|
||||
#endif
|
||||
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
|
||||
# include <limits> // Assuming that in C++11 mode all standard headers are available
|
||||
#endif
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
template <unsigned BitLen>
|
||||
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 <unsigned BitLen>
|
||||
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<std::numeric_limits<typename NativeFloatSelector<BitLen>::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 <unsigned BitLen>
|
||||
static typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType
|
||||
toIeee(typename NativeFloatSelector<BitLen>::Type value)
|
||||
{
|
||||
enforceIeee<BitLen>();
|
||||
union
|
||||
{
|
||||
typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType i;
|
||||
typename NativeFloatSelector<BitLen>::Type f;
|
||||
} u;
|
||||
StaticAssert<sizeof(u.f) * 8 == BitLen>::check();
|
||||
u.f = value;
|
||||
return u.i;
|
||||
}
|
||||
|
||||
template <unsigned BitLen>
|
||||
static typename NativeFloatSelector<BitLen>::Type
|
||||
toNative(typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType value)
|
||||
{
|
||||
enforceIeee<BitLen>();
|
||||
union
|
||||
{
|
||||
typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType i;
|
||||
typename NativeFloatSelector<BitLen>::Type f;
|
||||
} u;
|
||||
StaticAssert<sizeof(u.f) * 8 == BitLen>::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 <unsigned BitLen> struct IEEE754Limits;
|
||||
template <> struct IEEE754Limits<16>
|
||||
{
|
||||
typedef typename NativeFloatSelector<16>::Type NativeType;
|
||||
static NativeType max() { return static_cast<NativeType>(65504.0); }
|
||||
static NativeType epsilon() { return static_cast<NativeType>(9.77e-04); }
|
||||
};
|
||||
template <> struct IEEE754Limits<32>
|
||||
{
|
||||
typedef typename NativeFloatSelector<32>::Type NativeType;
|
||||
static NativeType max() { return static_cast<NativeType>(3.40282346638528859812e+38); }
|
||||
static NativeType epsilon() { return static_cast<NativeType>(1.19209289550781250000e-7); }
|
||||
};
|
||||
template <> struct IEEE754Limits<64>
|
||||
{
|
||||
typedef typename NativeFloatSelector<64>::Type NativeType;
|
||||
static NativeType max() { return static_cast<NativeType>(1.79769313486231570815e+308L); }
|
||||
static NativeType epsilon() { return static_cast<NativeType>(2.22044604925031308085e-16L); }
|
||||
};
|
||||
|
||||
|
||||
template <unsigned BitLen_, CastMode CastMode>
|
||||
class UAVCAN_EXPORT FloatSpec : public IEEE754Limits<BitLen_>
|
||||
{
|
||||
FloatSpec();
|
||||
|
||||
public:
|
||||
enum { BitLen = BitLen_ };
|
||||
enum { MinBitLen = BitLen };
|
||||
enum { MaxBitLen = BitLen };
|
||||
enum { IsPrimitive = 1 };
|
||||
|
||||
typedef typename NativeFloatSelector<BitLen>::Type StorageType;
|
||||
|
||||
#if UAVCAN_CPP_VERSION < UAVCAN_CPP11
|
||||
enum { IsExactRepresentation = (sizeof(StorageType) * 8 == BitLen) };
|
||||
#else
|
||||
enum { IsExactRepresentation = (sizeof(StorageType) * 8 == BitLen) && std::numeric_limits<StorageType>::is_iec559 };
|
||||
#endif
|
||||
|
||||
using IEEE754Limits<BitLen>::max;
|
||||
using IEEE754Limits<BitLen>::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<BitLen>(IEEE754Converter::toIeee<BitLen>(value));
|
||||
}
|
||||
|
||||
static int decode(StorageType& out_value, ScalarCodec& codec, TailArrayOptimizationMode)
|
||||
{
|
||||
typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType ieee = 0;
|
||||
const int res = codec.decode<BitLen>(ieee);
|
||||
if (res <= 0)
|
||||
{
|
||||
return res;
|
||||
}
|
||||
out_value = IEEE754Converter::toNative<BitLen>(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<StorageType>::infinity();
|
||||
}
|
||||
else if (value < -max())
|
||||
{
|
||||
value = -NumericTraits<StorageType>::infinity();
|
||||
}
|
||||
else
|
||||
{
|
||||
; // Valid range
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
template <unsigned BitLen, CastMode CastMode>
|
||||
class UAVCAN_EXPORT YamlStreamer<FloatSpec<BitLen, CastMode> >
|
||||
{
|
||||
typedef typename FloatSpec<BitLen, CastMode>::StorageType StorageType;
|
||||
|
||||
public:
|
||||
template <typename Stream> // cppcheck-suppress passedByValue
|
||||
static void stream(Stream& s, const StorageType value, int)
|
||||
{
|
||||
s << value;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // UAVCAN_MARSHAL_FLOAT_SPEC_HPP_INCLUDED
|
||||
@@ -0,0 +1,210 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_MARSHAL_INTEGER_SPEC_HPP_INCLUDED
|
||||
#define UAVCAN_MARSHAL_INTEGER_SPEC_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/std.hpp>
|
||||
#include <uavcan/data_type.hpp>
|
||||
#include <uavcan/util/templates.hpp>
|
||||
#include <uavcan/marshal/scalar_codec.hpp>
|
||||
#include <uavcan/marshal/type_util.hpp>
|
||||
|
||||
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 <unsigned BitLen_, Signedness Signedness, CastMode CastMode>
|
||||
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<IsSigned, int8_t, uint8_t>::Result,
|
||||
typename Select<(BitLen <= 16), typename Select<IsSigned, int16_t, uint16_t>::Result,
|
||||
typename Select<(BitLen <= 32), typename Select<IsSigned, int32_t, uint32_t>::Result,
|
||||
typename Select<(BitLen <= 64), typename Select<IsSigned, int64_t, uint64_t>::Result,
|
||||
ErrorNoSuchInteger>::Result>::Result>::Result>::Result StorageType;
|
||||
|
||||
typedef typename IntegerSpec<BitLen, SignednessUnsigned, CastMode>::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<unsigned>(BitLen)) - 1U);
|
||||
}
|
||||
else
|
||||
{
|
||||
return StorageType((uintmax_t(1) << (static_cast<unsigned>(BitLen) - 1U)) - 1);
|
||||
}
|
||||
}
|
||||
static UnsignedStorageType mask()
|
||||
{
|
||||
StaticAssert<(sizeof(uintmax_t) >= 8U)>::check();
|
||||
return UnsignedStorageType((uintmax_t(1) << static_cast<unsigned>(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<StorageType>::max());
|
||||
// coverity[result_independent_of_operands : FALSE]
|
||||
UAVCAN_ASSERT(min() >= NumericTraits<StorageType>::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<BitLen>(value);
|
||||
}
|
||||
|
||||
static int decode(StorageType& out_value, ScalarCodec& codec, TailArrayOptimizationMode)
|
||||
{
|
||||
validate();
|
||||
return codec.decode<BitLen>(out_value);
|
||||
}
|
||||
|
||||
static void extendDataTypeSignature(DataTypeSignature&) { }
|
||||
};
|
||||
|
||||
/**
|
||||
* Boolean specialization
|
||||
*/
|
||||
template <CastMode CastMode>
|
||||
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<BitLen>(value);
|
||||
}
|
||||
|
||||
static int decode(StorageType& out_value, ScalarCodec& codec, TailArrayOptimizationMode)
|
||||
{
|
||||
return codec.decode<BitLen>(out_value);
|
||||
}
|
||||
|
||||
static void extendDataTypeSignature(DataTypeSignature&) { }
|
||||
};
|
||||
|
||||
template <CastMode CastMode>
|
||||
class IntegerSpec<1, SignednessSigned, CastMode>; // Invalid instantiation
|
||||
|
||||
template <Signedness Signedness, CastMode CastMode>
|
||||
class IntegerSpec<0, Signedness, CastMode>; // Invalid instantiation
|
||||
|
||||
|
||||
template <typename T>
|
||||
struct IsIntegerSpec
|
||||
{
|
||||
enum { Result = 0 };
|
||||
};
|
||||
|
||||
template <unsigned BitLen, Signedness Signedness, CastMode CastMode>
|
||||
struct IsIntegerSpec<IntegerSpec<BitLen, Signedness, CastMode> >
|
||||
{
|
||||
enum { Result = 1 };
|
||||
};
|
||||
|
||||
|
||||
template <unsigned BitLen, Signedness Signedness, CastMode CastMode>
|
||||
class UAVCAN_EXPORT YamlStreamer<IntegerSpec<BitLen, Signedness, CastMode> >
|
||||
{
|
||||
typedef IntegerSpec<BitLen, Signedness, CastMode> RawType;
|
||||
typedef typename RawType::StorageType StorageType;
|
||||
|
||||
public:
|
||||
template <typename Stream> // 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<RawType::IsSigned, int, unsigned>::Result >::Result TempType;
|
||||
s << TempType(value);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // UAVCAN_MARSHAL_INTEGER_SPEC_HPP_INCLUDED
|
||||
@@ -0,0 +1,140 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_MARSHAL_SCALAR_CODEC_HPP_INCLUDED
|
||||
#define UAVCAN_MARSHAL_SCALAR_CODEC_HPP_INCLUDED
|
||||
|
||||
#include <cassert>
|
||||
#include <uavcan/std.hpp>
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/util/templates.hpp>
|
||||
#include <uavcan/marshal/bit_stream.hpp>
|
||||
|
||||
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 <unsigned BitLen, unsigned Size>
|
||||
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 <unsigned BitLen, unsigned Size>
|
||||
static typename EnableIf<(BitLen <= 8)>::Type
|
||||
convertByteOrder(uint8_t (&)[Size]) { }
|
||||
|
||||
template <unsigned BitLen, typename T>
|
||||
static typename EnableIf<static_cast<bool>(NumericTraits<T>::IsSigned) && ((sizeof(T) * 8) > BitLen)>::Type
|
||||
fixTwosComplement(T& value)
|
||||
{
|
||||
StaticAssert<NumericTraits<T>::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 <unsigned BitLen, typename T>
|
||||
static typename EnableIf<!static_cast<bool>(NumericTraits<T>::IsSigned) || ((sizeof(T) * 8) == BitLen)>::Type
|
||||
fixTwosComplement(T&) { }
|
||||
|
||||
template <unsigned BitLen, typename T>
|
||||
static typename EnableIf<((sizeof(T) * 8) > BitLen)>::Type
|
||||
clearExtraBits(T& value)
|
||||
{
|
||||
value &= (T(1) << BitLen) - 1; // Signedness doesn't matter
|
||||
}
|
||||
|
||||
template <unsigned BitLen, typename T>
|
||||
static typename EnableIf<((sizeof(T) * 8) == BitLen)>::Type
|
||||
clearExtraBits(T&) { }
|
||||
|
||||
template <unsigned BitLen, typename T>
|
||||
void validate()
|
||||
{
|
||||
StaticAssert<((sizeof(T) * 8) >= BitLen)>::check();
|
||||
StaticAssert<(BitLen <= BitStream::MaxBitsPerRW)>::check();
|
||||
StaticAssert<static_cast<bool>(NumericTraits<T>::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 <unsigned BitLen, typename T>
|
||||
int encode(const T value);
|
||||
|
||||
template <unsigned BitLen, typename T>
|
||||
int decode(T& value);
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
|
||||
template <unsigned BitLen, typename T>
|
||||
int ScalarCodec::encode(const T value)
|
||||
{
|
||||
validate<BitLen, T>();
|
||||
union ByteUnion
|
||||
{
|
||||
T value;
|
||||
uint8_t bytes[sizeof(T)];
|
||||
} byte_union;
|
||||
byte_union.value = value;
|
||||
clearExtraBits<BitLen, T>(byte_union.value);
|
||||
convertByteOrder<BitLen>(byte_union.bytes);
|
||||
return encodeBytesImpl(byte_union.bytes, BitLen);
|
||||
}
|
||||
|
||||
template <unsigned BitLen, typename T>
|
||||
int ScalarCodec::decode(T& value)
|
||||
{
|
||||
validate<BitLen, T>();
|
||||
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<BitLen>(byte_union.bytes);
|
||||
fixTwosComplement<BitLen, T>(byte_union.value);
|
||||
value = byte_union.value;
|
||||
}
|
||||
return read_res;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif // UAVCAN_MARSHAL_SCALAR_CODEC_HPP_INCLUDED
|
||||
@@ -0,0 +1,88 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED
|
||||
#define UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/util/templates.hpp>
|
||||
#include <uavcan/util/comparison.hpp>
|
||||
|
||||
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 <unsigned long Num>
|
||||
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 <unsigned long BitLen>
|
||||
struct BitLenToByteLen
|
||||
{
|
||||
enum { Result = (BitLen + 7) / 8 };
|
||||
};
|
||||
|
||||
/**
|
||||
* Compile-time: Helper for integer and float specialization classes. Returns the platform-specific storage type.
|
||||
*/
|
||||
template <typename T, typename Enable = void>
|
||||
struct StorageType
|
||||
{
|
||||
typedef T Type;
|
||||
};
|
||||
template <typename T>
|
||||
struct StorageType<T, typename EnableIfType<typename T::StorageType>::Type>
|
||||
{
|
||||
typedef typename T::StorageType Type;
|
||||
};
|
||||
|
||||
/**
|
||||
* Compile-time: Whether T is a primitive type on this platform.
|
||||
*/
|
||||
template <typename T>
|
||||
class IsPrimitiveType
|
||||
{
|
||||
typedef char Yes;
|
||||
struct No { Yes dummy[8]; };
|
||||
|
||||
template <typename U>
|
||||
static typename EnableIf<U::IsPrimitive, Yes>::Type test(int);
|
||||
|
||||
template <typename>
|
||||
static No test(...);
|
||||
|
||||
public:
|
||||
enum { Result = sizeof(test<T>(0)) == sizeof(Yes) };
|
||||
};
|
||||
|
||||
/**
|
||||
* Streams a given value into YAML string. Please see the specializations.
|
||||
*/
|
||||
template <typename T>
|
||||
class UAVCAN_EXPORT YamlStreamer;
|
||||
|
||||
}
|
||||
|
||||
#endif // UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED
|
||||
@@ -0,0 +1,13 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_MARSHAL_TYPES_HPP_INCLUDED
|
||||
#define UAVCAN_MARSHAL_TYPES_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/marshal/integer_spec.hpp>
|
||||
#include <uavcan/marshal/float_spec.hpp>
|
||||
#include <uavcan/marshal/array.hpp>
|
||||
#include <uavcan/marshal/type_util.hpp>
|
||||
|
||||
#endif // UAVCAN_MARSHAL_TYPES_HPP_INCLUDED
|
||||
@@ -0,0 +1,130 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_NODE_ABSTRACT_NODE_HPP_INCLUDED
|
||||
#define UAVCAN_NODE_ABSTRACT_NODE_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/dynamic_memory.hpp>
|
||||
#include <uavcan/node/scheduler.hpp>
|
||||
|
||||
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
|
||||
@@ -0,0 +1,201 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_NODE_GENERIC_PUBLISHER_HPP_INCLUDED
|
||||
#define UAVCAN_NODE_GENERIC_PUBLISHER_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/error.hpp>
|
||||
#include <uavcan/node/abstract_node.hpp>
|
||||
#include <uavcan/data_type.hpp>
|
||||
#include <uavcan/node/global_data_type_registry.hpp>
|
||||
#include <uavcan/debug.hpp>
|
||||
#include <uavcan/transport/transfer_buffer.hpp>
|
||||
#include <uavcan/transport/transfer_sender.hpp>
|
||||
#include <uavcan/marshal/scalar_codec.hpp>
|
||||
#include <uavcan/marshal/types.hpp>
|
||||
|
||||
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 <typename DataSpec, typename DataStruct>
|
||||
class UAVCAN_EXPORT GenericPublisher : public GenericPublisherBase
|
||||
{
|
||||
struct ZeroTransferBuffer : public StaticTransferBufferImpl
|
||||
{
|
||||
ZeroTransferBuffer() : StaticTransferBufferImpl(UAVCAN_NULLPTR, 0) { }
|
||||
};
|
||||
|
||||
typedef typename Select<DataStruct::MaxBitLen == 0,
|
||||
ZeroTransferBuffer,
|
||||
StaticTransferBuffer<BitLenToByteLen<DataStruct::MaxBitLen>::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 <typename DataSpec, typename DataStruct>
|
||||
int GenericPublisher<DataSpec, DataStruct>::checkInit()
|
||||
{
|
||||
if (isInited())
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
return doInit(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName(), CanTxQueue::Qos(Qos));
|
||||
}
|
||||
|
||||
template <typename DataSpec, typename DataStruct>
|
||||
int GenericPublisher<DataSpec, DataStruct>::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 <typename DataSpec, typename DataStruct>
|
||||
int GenericPublisher<DataSpec, DataStruct>::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
|
||||
@@ -0,0 +1,299 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_NODE_GENERIC_SUBSCRIBER_HPP_INCLUDED
|
||||
#define UAVCAN_NODE_GENERIC_SUBSCRIBER_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/error.hpp>
|
||||
#include <uavcan/node/abstract_node.hpp>
|
||||
#include <uavcan/data_type.hpp>
|
||||
#include <uavcan/node/global_data_type_registry.hpp>
|
||||
#include <uavcan/util/templates.hpp>
|
||||
#include <uavcan/util/lazy_constructor.hpp>
|
||||
#include <uavcan/debug.hpp>
|
||||
#include <uavcan/transport/transfer_listener.hpp>
|
||||
#include <uavcan/marshal/scalar_codec.hpp>
|
||||
#include <uavcan/marshal/types.hpp>
|
||||
|
||||
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<Foo>& 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<DataType>
|
||||
* // dt is now copyable
|
||||
*/
|
||||
template <typename DataType_>
|
||||
class UAVCAN_EXPORT ReceivedDataStructure : public DataType_, Noncopyable
|
||||
{
|
||||
const IncomingTransfer* const _transfer_; ///< Such weird name is necessary to avoid clashing with DataType fields
|
||||
|
||||
template <typename Ret, Ret(IncomingTransfer::*Fun) () const>
|
||||
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<MonotonicTime, &IncomingTransfer::getMonotonicTimestamp>();
|
||||
}
|
||||
UtcTime getUtcTimestamp() const { return safeget<UtcTime, &IncomingTransfer::getUtcTimestamp>(); }
|
||||
TransferPriority getPriority() const { return safeget<TransferPriority, &IncomingTransfer::getPriority>(); }
|
||||
TransferType getTransferType() const { return safeget<TransferType, &IncomingTransfer::getTransferType>(); }
|
||||
TransferID getTransferID() const { return safeget<TransferID, &IncomingTransfer::getTransferID>(); }
|
||||
NodeID getSrcNodeID() const { return safeget<NodeID, &IncomingTransfer::getSrcNodeID>(); }
|
||||
uint8_t getIfaceIndex() const { return safeget<uint8_t, &IncomingTransfer::getIfaceIndex>(); }
|
||||
bool isAnonymousTransfer() const { return safeget<bool, &IncomingTransfer::isAnonymousTransfer>(); }
|
||||
};
|
||||
|
||||
/**
|
||||
* 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 <typename Stream, typename DataType>
|
||||
static Stream& operator<<(Stream& s, const ReceivedDataStructure<DataType>& rds)
|
||||
{
|
||||
s << "# Received struct ts_m=" << rds.getMonotonicTimestamp()
|
||||
<< " ts_utc=" << rds.getUtcTimestamp()
|
||||
<< " snid=" << int(rds.getSrcNodeID().get()) << "\n";
|
||||
s << static_cast<const DataType&>(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 <typename DataSpec, typename DataStruct, typename TransferListenerType>
|
||||
class UAVCAN_EXPORT GenericSubscriber : public GenericSubscriberBase
|
||||
{
|
||||
typedef GenericSubscriber<DataSpec, DataStruct, TransferListenerType> 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<TransferForwarder> forwarder_;
|
||||
|
||||
int checkInit();
|
||||
|
||||
void handleIncomingTransfer(IncomingTransfer& transfer);
|
||||
|
||||
int genericStart(bool (Dispatcher::*registration_method)(TransferListener*));
|
||||
|
||||
protected:
|
||||
struct ReceivedDataStructureSpec : public ReceivedDataStructure<DataStruct>
|
||||
{
|
||||
ReceivedDataStructureSpec() { }
|
||||
|
||||
ReceivedDataStructureSpec(const IncomingTransfer* arg_transfer) :
|
||||
ReceivedDataStructure<DataStruct>(arg_transfer)
|
||||
{ }
|
||||
};
|
||||
|
||||
explicit GenericSubscriber(INode& node) : GenericSubscriberBase(node)
|
||||
{ }
|
||||
|
||||
virtual ~GenericSubscriber() { stop(); }
|
||||
|
||||
virtual void handleReceivedDataStruct(ReceivedDataStructure<DataStruct>&) = 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 <typename DataSpec, typename DataStruct, typename TransferListenerType>
|
||||
int GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::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<DataStruct::MaxBitLen>::Result;
|
||||
|
||||
forwarder_.template construct<SelfType&, const DataTypeDescriptor&, uint16_t, IPoolAllocator&>
|
||||
(*this, *descr, MaxBufferSize, node_.getAllocator());
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
template <typename DataSpec, typename DataStruct, typename TransferListenerType>
|
||||
void GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::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 <typename DataSpec, typename DataStruct, typename TransferListenerType>
|
||||
int GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::
|
||||
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
|
||||
+241
@@ -0,0 +1,241 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_NODE_GLOBAL_DATA_TYPE_REGISTRY_HPP_INCLUDED
|
||||
#define UAVCAN_NODE_GLOBAL_DATA_TYPE_REGISTRY_HPP_INCLUDED
|
||||
|
||||
#include <cassert>
|
||||
#include <uavcan/error.hpp>
|
||||
#include <uavcan/std.hpp>
|
||||
#include <uavcan/data_type.hpp>
|
||||
#include <uavcan/util/templates.hpp>
|
||||
#include <uavcan/util/linked_list.hpp>
|
||||
#if UAVCAN_DEBUG
|
||||
# include <uavcan/debug.hpp>
|
||||
#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<Entry>
|
||||
{
|
||||
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<Entry> 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 <typename Type>
|
||||
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 <typename Type>
|
||||
struct UAVCAN_EXPORT DefaultDataTypeRegistrator
|
||||
{
|
||||
DefaultDataTypeRegistrator()
|
||||
{
|
||||
#if !UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY
|
||||
const GlobalDataTypeRegistry::RegistrationResult res =
|
||||
GlobalDataTypeRegistry::instance().registerDataType<Type>(Type::DefaultDataTypeID);
|
||||
|
||||
if (res != GlobalDataTypeRegistry::RegistrationResultOk)
|
||||
{
|
||||
handleFatalError("Type reg failed");
|
||||
}
|
||||
#endif
|
||||
}
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
|
||||
/*
|
||||
* GlobalDataTypeRegistry
|
||||
*/
|
||||
template <typename Type>
|
||||
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
|
||||
@@ -0,0 +1,319 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_NODE_NODE_HPP_INCLUDED
|
||||
#define UAVCAN_NODE_NODE_HPP_INCLUDED
|
||||
|
||||
#include <cassert>
|
||||
#include <uavcan/error.hpp>
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/node/abstract_node.hpp>
|
||||
|
||||
// High-level functionality available by default
|
||||
#include <uavcan/protocol/node_status_provider.hpp>
|
||||
|
||||
#if !UAVCAN_TINY
|
||||
# include <uavcan/protocol/data_type_info_provider.hpp>
|
||||
# include <uavcan/protocol/logger.hpp>
|
||||
# include <uavcan/protocol/restart_request_server.hpp>
|
||||
# include <uavcan/protocol/transport_stats_provider.hpp>
|
||||
#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 <std::size_t MemPoolSize = 0>
|
||||
class UAVCAN_EXPORT Node : public INode
|
||||
{
|
||||
typedef typename
|
||||
Select<(MemPoolSize > 0),
|
||||
PoolAllocator<MemPoolSize, MemPoolBlockSize>, // 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<Allocator>::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 <typename... Args>
|
||||
inline void logDebug(const char* source, const char* format, Args... args)
|
||||
{
|
||||
(void)proto_logger_.logDebug(source, format, args...);
|
||||
}
|
||||
|
||||
template <typename... Args>
|
||||
inline void logInfo(const char* source, const char* format, Args... args)
|
||||
{
|
||||
(void)proto_logger_.logInfo(source, format, args...);
|
||||
}
|
||||
|
||||
template <typename... Args>
|
||||
inline void logWarning(const char* source, const char* format, Args... args)
|
||||
{
|
||||
(void)proto_logger_.logWarning(source, format, args...);
|
||||
}
|
||||
|
||||
template <typename... Args>
|
||||
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 <std::size_t MemPoolSize_>
|
||||
int Node<MemPoolSize_>::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
|
||||
@@ -0,0 +1,89 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_NODE_PUBLISHER_HPP_INCLUDED
|
||||
#define UAVCAN_NODE_PUBLISHER_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/node/generic_publisher.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
/**
|
||||
* Use this class to publish messages to the bus (broadcast, unicast, or both).
|
||||
*
|
||||
* @tparam DataType_ Message data type
|
||||
*/
|
||||
template <typename DataType_>
|
||||
class UAVCAN_EXPORT Publisher : protected GenericPublisher<DataType_, DataType_>
|
||||
{
|
||||
typedef GenericPublisher<DataType_, DataType_> 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<DataTypeKind(DataType::DataTypeKind) == DataTypeKindMessage>::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
|
||||
@@ -0,0 +1,155 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_NODE_SCHEDULER_HPP_INCLUDED
|
||||
#define UAVCAN_NODE_SCHEDULER_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/error.hpp>
|
||||
#include <uavcan/util/linked_list.hpp>
|
||||
#include <uavcan/transport/dispatcher.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
class UAVCAN_EXPORT Scheduler;
|
||||
|
||||
class UAVCAN_EXPORT DeadlineHandler : public LinkedListNode<DeadlineHandler>
|
||||
{
|
||||
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<DeadlineHandler> 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
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,201 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_NODE_SERVICE_SERVER_HPP_INCLUDED
|
||||
#define UAVCAN_NODE_SERVICE_SERVER_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/node/generic_publisher.hpp>
|
||||
#include <uavcan/node/generic_subscriber.hpp>
|
||||
|
||||
#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
|
||||
# error UAVCAN_CPP_VERSION
|
||||
#endif
|
||||
|
||||
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
|
||||
# include <functional>
|
||||
#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<Foo::Request>& request,
|
||||
* ServiceResponseDataStructure<Foo::Response>& 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 <typename ResponseDataType_>
|
||||
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 <typename DataType_,
|
||||
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
|
||||
typename Callback_ = std::function<void (const ReceivedDataStructure<typename DataType_::Request>&,
|
||||
ServiceResponseDataStructure<typename DataType_::Response>&)>
|
||||
#else
|
||||
typename Callback_ = void (*)(const ReceivedDataStructure<typename DataType_::Request>&,
|
||||
ServiceResponseDataStructure<typename DataType_::Response>&)
|
||||
#endif
|
||||
>
|
||||
class UAVCAN_EXPORT ServiceServer
|
||||
: public GenericSubscriber<DataType_, typename DataType_::Request, TransferListener>
|
||||
{
|
||||
public:
|
||||
typedef DataType_ DataType;
|
||||
typedef typename DataType::Request RequestType;
|
||||
typedef typename DataType::Response ResponseType;
|
||||
typedef Callback_ Callback;
|
||||
|
||||
private:
|
||||
typedef GenericSubscriber<DataType, RequestType, TransferListener> SubscriberType;
|
||||
typedef GenericPublisher<DataType, ResponseType> PublisherType;
|
||||
|
||||
PublisherType publisher_;
|
||||
Callback callback_;
|
||||
uint32_t response_failure_count_;
|
||||
|
||||
virtual void handleReceivedDataStruct(ReceivedDataStructure<RequestType>& request) override
|
||||
{
|
||||
UAVCAN_ASSERT(request.getTransferType() == TransferTypeServiceRequest);
|
||||
|
||||
ServiceResponseDataStructure<ResponseType> response;
|
||||
|
||||
if (coerceOrFallback<bool>(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<DataTypeKind(DataType::DataTypeKind) == DataTypeKindService>::check();
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the server.
|
||||
* Incoming service requests will be passed to the application via the callback.
|
||||
*/
|
||||
int start(const Callback& callback)
|
||||
{
|
||||
stop();
|
||||
|
||||
if (!coerceOrFallback<bool>(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
|
||||
@@ -0,0 +1,76 @@
|
||||
/*
|
||||
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_SUB_NODE_NODE_HPP_INCLUDED
|
||||
#define UAVCAN_SUB_NODE_NODE_HPP_INCLUDED
|
||||
|
||||
#include <cassert>
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/node/abstract_node.hpp>
|
||||
|
||||
#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 <std::size_t MemPoolSize = 0>
|
||||
class UAVCAN_EXPORT SubNode : public INode
|
||||
{
|
||||
typedef typename
|
||||
Select<(MemPoolSize > 0),
|
||||
PoolAllocator<MemPoolSize, MemPoolBlockSize>, // 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<Allocator>::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
|
||||
@@ -0,0 +1,104 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_NODE_SUBSCRIBER_HPP_INCLUDED
|
||||
#define UAVCAN_NODE_SUBSCRIBER_HPP_INCLUDED
|
||||
|
||||
#include <cassert>
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/node/generic_subscriber.hpp>
|
||||
|
||||
#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
|
||||
# error UAVCAN_CPP_VERSION
|
||||
#endif
|
||||
|
||||
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
|
||||
# include <functional>
|
||||
#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<DataType_>&
|
||||
* - const ReceivedDataStructure<DataType_>&
|
||||
* 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 <typename DataType_,
|
||||
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
|
||||
typename Callback_ = std::function<void (const ReceivedDataStructure<DataType_>&)>
|
||||
#else
|
||||
typename Callback_ = void (*)(const ReceivedDataStructure<DataType_>&)
|
||||
#endif
|
||||
>
|
||||
class UAVCAN_EXPORT Subscriber
|
||||
: public GenericSubscriber<DataType_, DataType_, TransferListener>
|
||||
{
|
||||
public:
|
||||
typedef Callback_ Callback;
|
||||
|
||||
private:
|
||||
typedef GenericSubscriber<DataType_, DataType_, TransferListener> BaseType;
|
||||
|
||||
Callback callback_;
|
||||
|
||||
virtual void handleReceivedDataStruct(ReceivedDataStructure<DataType_>& msg) override
|
||||
{
|
||||
if (coerceOrFallback<bool>(callback_, true))
|
||||
{
|
||||
callback_(msg);
|
||||
}
|
||||
else
|
||||
{
|
||||
handleFatalError("Sub clbk");
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
typedef DataType_ DataType;
|
||||
|
||||
explicit Subscriber(INode& node)
|
||||
: BaseType(node)
|
||||
, callback_()
|
||||
{
|
||||
StaticAssert<DataTypeKind(DataType::DataTypeKind) == DataTypeKindMessage>::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<bool>(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
|
||||
@@ -0,0 +1,148 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_NODE_TIMER_HPP_INCLUDED
|
||||
#define UAVCAN_NODE_TIMER_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/std.hpp>
|
||||
#include <uavcan/error.hpp>
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/util/linked_list.hpp>
|
||||
#include <uavcan/node/scheduler.hpp>
|
||||
#include <uavcan/node/abstract_node.hpp>
|
||||
#include <uavcan/util/templates.hpp>
|
||||
|
||||
#if !defined(UAVCAN_CPP11) || !defined(UAVCAN_CPP_VERSION)
|
||||
# error UAVCAN_CPP_VERSION
|
||||
#endif
|
||||
|
||||
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
|
||||
# include <functional>
|
||||
#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 <typename Callback_>
|
||||
class UAVCAN_EXPORT TimerEventForwarder : public TimerBase
|
||||
{
|
||||
public:
|
||||
typedef Callback_ Callback;
|
||||
|
||||
private:
|
||||
Callback callback_;
|
||||
|
||||
virtual void handleTimerEvent(const TimerEvent& event)
|
||||
{
|
||||
if (coerceOrFallback<bool>(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<std::function<void (const TimerEvent& event)> > Timer;
|
||||
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
#endif // UAVCAN_NODE_TIMER_HPP_INCLUDED
|
||||
@@ -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.
|
||||
+137
@@ -0,0 +1,137 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_PROTOCOL_DATA_TYPE_INFO_PROVIDER_HPP_INCLUDED
|
||||
#define UAVCAN_PROTOCOL_DATA_TYPE_INFO_PROVIDER_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/node/service_server.hpp>
|
||||
#include <uavcan/util/method_binder.hpp>
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/protocol/GetDataTypeInfo.hpp>
|
||||
#include <uavcan/debug.hpp>
|
||||
|
||||
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<DataTypeInfoProvider*,
|
||||
void (DataTypeInfoProvider::*)(const protocol::GetDataTypeInfo::Request&,
|
||||
protocol::GetDataTypeInfo::Response&)> GetDataTypeInfoCallback;
|
||||
|
||||
ServiceServer<protocol::GetDataTypeInfo, GetDataTypeInfoCallback> 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<int>(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<int>(request.id), static_cast<int>(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
|
||||
+112
@@ -0,0 +1,112 @@
|
||||
/*
|
||||
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED
|
||||
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/node/subscriber.hpp>
|
||||
#include <uavcan/node/publisher.hpp>
|
||||
#include <uavcan/node/timer.hpp>
|
||||
#include <uavcan/util/method_binder.hpp>
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/protocol/dynamic_node_id/Allocation.hpp>
|
||||
#include <uavcan/protocol/HardwareVersion.hpp>
|
||||
|
||||
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<DynamicNodeIDClient*,
|
||||
void (DynamicNodeIDClient::*)
|
||||
(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>&)>
|
||||
AllocationCallback;
|
||||
|
||||
enum Mode
|
||||
{
|
||||
ModeWaitingForTimeSlot,
|
||||
ModeDelayBeforeFollowup,
|
||||
NumModes
|
||||
};
|
||||
|
||||
Publisher<protocol::dynamic_node_id::Allocation> dnida_pub_;
|
||||
Subscriber<protocol::dynamic_node_id::Allocation, AllocationCallback> 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<protocol::dynamic_node_id::Allocation>& 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
|
||||
+114
@@ -0,0 +1,114 @@
|
||||
/*
|
||||
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_SERVER_HPP_INCLUDED
|
||||
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_SERVER_HPP_INCLUDED
|
||||
|
||||
#include <uavcan/build_config.hpp>
|
||||
#include <uavcan/debug.hpp>
|
||||
#include <uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp>
|
||||
#include <uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp>
|
||||
#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
|
||||
|
||||
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
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user