diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..358e344 --- /dev/null +++ b/.clang-format @@ -0,0 +1,51 @@ +--- +Language: Cpp +BasedOnStyle: LLVM +AccessModifierOffset: -4 +AlignAfterOpenBracket: BlockIndent +AlignConsecutiveBitFields: + Enabled: true + PadOperators: true +AlignConsecutiveDeclarations: + Enabled: true + PadOperators: true +AlignConsecutiveMacros: + PadOperators: true +AlignEscapedNewlines: Left +AllowShortBlocksOnASingleLine: Empty +AllowShortEnumsOnASingleLine: false +AllowShortFunctionsOnASingleLine: Inline +AlwaysBreakTemplateDeclarations: Yes +BreakBeforeTernaryOperators: false +BreakConstructorInitializers: AfterColon +BreakInheritanceList: AfterColon +ColumnLimit: 120 +EmptyLineBeforeAccessModifier: Always +IncludeCategories: + - Regex: '^<.*>' + Priority: 1 + SortPriority: 1 + - Regex: '^".*"' + SortPriority: 2 +# No include is treated as the main header, so that sorting preserves the existing +# grouping instead of promoting the header matching the source file name +IncludeIsMainRegex: 'NO_MAIN_INCLUDE' +IndentCaseLabels: true +IndentExternBlock: NoIndent +IndentPPDirectives: BeforeHash +IndentRequiresClause: false +IndentWidth: 4 +IndentWrappedFunctionNames: true +InsertNewlineAtEOF: true +KeepEmptyLinesAtTheStartOfBlocks: false +LineEnding: LF +PackConstructorInitializers: NextLine +PointerAlignment: Left +SeparateDefinitionBlocks: Always +ShortNamespaceLines: 0 +SortIncludes: CaseSensitive +SpaceInEmptyBlock: true +SpacesBeforeTrailingComments: 2 +Standard: Latest +TabWidth: 4 +... diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..33139c4 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,79 @@ +--- +# llvm-include-order enforces LLVM's own ordering, which contradicts the project's +# .clang-format rules; clang-format is the single authority on include order here. +# +# The trailing group disables checks for constructs this project cannot contain. They match +# nothing, so they only cost time scanning the standard library and the Cube headers: +# string_view, coroutines, Objective-C, GoogleTest, the LLVM internal APIs, std::bind, +# iostreams, std::auto_ptr and std::random_shuffle (both removed in C++17), and printf. +# It also drops clang-analyzer-core.FixedAddressDereference, which every memory mapped +# peripheral access looks like by construction. +Checks: + -*, + bugprone-*, + -bugprone-easily-swappable-parameters, + -bugprone-dynamic-static-initializers, + -bugprone-bitwise-pointer-cast, + clang-*, + cppcoreguidelines-*, + -cppcoreguidelines-avoid-magic-numbers, + -cppcoreguidelines-avoid-do-while, + -cppcoreguidelines-narrowing-conversions, + -cppcoreguidelines-pro-type-union-access, + google-*, + llvm-*, + -llvm-header-guard, + -llvm-include-order, + misc-*, + -misc-use-anonymous-namespace, + modernize-*, + -modernize-use-nodiscard, + -modernize-use-trailing-return-type, + performance-*, + readability-*, + -readability-duplicate-include, + -readability-magic-numbers, + -readability-math-missing-parentheses, + -bugprone-stringview-nullptr, + -cppcoreguidelines-no-suspend-with-lock, + -google-objc-*, + -google-upgrade-googletest-case, + -llvm-prefer-isa-or-dyn-cast-in-conditionals, + -llvm-prefer-register-over-unsigned, + -llvm-twine-local, + -modernize-avoid-bind, + -modernize-deprecated-ios-base-aliases, + -modernize-replace-auto-ptr, + -modernize-replace-random-shuffle, + -modernize-use-std-print, + -clang-analyzer-core.FixedAddressDereference, +HeaderFileExtensions: + - '' + - h + - hh + - hpp + - hxx +ImplementationFileExtensions: + - c + - cc + - cpp + - cxx +HeaderFilterRegex: '(include|config)/.*\.(hpp|tpp)$' +ExcludeHeaderFilterRegex: '(_deps|external)/' +FormatStyle: file +CheckOptions: + # The vendor HAL module headers are not standalone; they require stm32xx_hal.h to have + # pulled in stm32xx_hal_def.h first, so include-cleaner must not suggest or remove them. + # This is why micras_hal includes the generated main.h, which reaches the right one for the + # family it was generated for, instead of naming a module header of its own. main.h is on the + # list for the same reason: every symbol behind it resolves to a header already listed here, so + # nothing is credited to main.h and it would otherwise be reported as unused. + misc-include-cleaner.IgnoreHeaders: 'stm32.*;core_cm.*;cmsis.*;system_stm32.*;lsm6dsv.*;main\.h' + readability-identifier-length.MinimumVariableNameLength: 2 + readability-identifier-length.IgnoredVariableNames: ^[nxyz]$ + readability-identifier-length.MinimumParameterNameLength: 2 + readability-identifier-length.IgnoredParameterNames: ^[nxyz]$ + misc-non-private-member-variables-in-classes.IgnoreClassesWithAllMemberVariablesBeingPublic: true + bugprone-narrowing-conversions.WarnOnIntegerToFloatingPointNarrowingConversion: false +SystemHeaders: false +... diff --git a/.docker/Dockerfile b/.docker/Dockerfile new file mode 100644 index 0000000..8726d41 --- /dev/null +++ b/.docker/Dockerfile @@ -0,0 +1,103 @@ +# syntax=docker/dockerfile:1.27 + +# >>> host stage >>> +############################################################################### +# Host tools: shared byte for byte by micras-lib, micras-simulation and # +# MicrasFirmware, whose CI compares the lines between the two markers # +############################################################################### +FROM ubuntu:26.04 AS host + +# The compilers and the clang tooling are pinned to a major version, so that a distribution update +# can move neither the generated code nor the formatting rules and the enabled check set +ARG HOST_GCC_VERSION=15 +ARG HOST_CLANG_VERSION=22 + +# The apt caches are kept in BuildKit cache mounts instead of the image layers, so the hook that +# deletes the downloaded packages after every install has to go +RUN rm -f /etc/apt/apt.conf.d/docker-clean + +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + apt-get update && \ + apt-get install -y --no-install-recommends \ + ca-certificates \ + cmake \ + doxygen \ + git \ + graphviz \ + ninja-build \ + python3 \ + python3-matplotlib \ + python3-numpy \ + "gcc-${HOST_GCC_VERSION}" \ + "g++-${HOST_GCC_VERSION}" \ + "clang-${HOST_CLANG_VERSION}" \ + "clang-format-${HOST_CLANG_VERSION}" \ + "clang-tidy-${HOST_CLANG_VERSION}" + +WORKDIR /project +# <<< host stage <<< + +############################################################# +# Reference projects' Cube files, generated by STM32CubeMX # +############################################################# +FROM thunderatz/stm32cubemx:6.18.1-h7 AS generator-h7 + +WORKDIR /project +COPY reference/h7/reference.ioc reference/h7/ + +# Without network, CubeMX skips its online update check, which can hang the project generation. The +# CMakeLists.txt and CMakePresets.json CubeMX writes next to the .ioc are its own suggestion of a +# project, which the reference replaces, so they are left out of the generated tree. +RUN --network=none < /tmp/cube_script + xvfb-run --auto-servernum "${CUBE_PATH}/STM32CubeMX" -q /tmp/cube_script + rm /tmp/cube_script + test -f reference/h7/cmake/stm32cubemx/CMakeLists.txt || { echo "STM32CubeMX failed to generate the project" >&2; exit 1; } + rm -f reference/h7/CMakeLists.txt reference/h7/CMakePresets.json reference/h7/reference.ioc +EOT + +# Only the generated files, can be replaced by already generated ones with --build-context generated-h7= +FROM scratch AS generated-h7 + +COPY --from=generator-h7 /project/reference/h7 / + +FROM thunderatz/stm32cubemx:6.18.1-g4 AS generator-g4 + +WORKDIR /project +COPY reference/g4/reference.ioc reference/g4/ + +# The same generation as the H7 reference's, with the STM32CubeMX image that carries the G4 package +RUN --network=none < /tmp/cube_script + xvfb-run --auto-servernum "${CUBE_PATH}/STM32CubeMX" -q /tmp/cube_script + rm /tmp/cube_script + test -f reference/g4/cmake/stm32cubemx/CMakeLists.txt || { echo "STM32CubeMX failed to generate the project" >&2; exit 1; } + rm -f reference/g4/CMakeLists.txt reference/g4/CMakePresets.json reference/g4/reference.ioc +EOT + +# Only the generated files, can be replaced by already generated ones with --build-context generated-g4= +FROM scratch AS generated-g4 + +COPY --from=generator-g4 /project/reference/g4 / + +########################################################## +# ARM toolchain for the reference projects and benches # +########################################################## +FROM host AS stm32 + +# The ARM libraries come prebuilt for every ARM core, only the Cortex-M4F and Cortex-M7 double +# precision ones are kept +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + apt-get update && \ + apt-get install -y --no-install-recommends \ + gcc-arm-none-eabi \ + libnewlib-arm-none-eabi \ + libstdc++-arm-none-eabi-newlib \ + make && \ + find /usr/lib/arm-none-eabi/lib/thumb /usr/lib/gcc/arm-none-eabi/*/thumb -mindepth 1 -maxdepth 1 \ + -not -name "v7e-m+fp" -not -name "v7e-m+dp" -exec rm -rf {} + && \ + rm -rf /usr/lib/arm-none-eabi/lib/arm /usr/lib/gcc/arm-none-eabi/*/arm diff --git a/.docker/Dockerfile.dockerignore b/.docker/Dockerfile.dockerignore new file mode 100644 index 0000000..fd65b4d --- /dev/null +++ b/.docker/Dockerfile.dockerignore @@ -0,0 +1,6 @@ +# The images need no source: the CI mounts the checkout into the containers it runs +.git/ +build/ +docs/ +reference/*/* +!reference/*/reference.ioc diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..19901b0 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,17 @@ +# Every text file is stored with LF in the repository and checked out with LF, whatever platform +# wrote it. +# +# The files this exists for are reference/*/reference.ioc. STM32CubeMX writes its project file with +# the line endings of the platform it runs on, CRLF from a Windows install and LF from the Linux +# container the CI uses, so without normalization the same regeneration shows up as a whole file +# rewrite depending on who ran it, burying the handful of settings that actually changed. +* text=auto eol=lf + +# Binary types, so that git never rewrites them +*.png binary +*.jpg binary +*.pdf binary +*.ico binary +*.bin binary +*.elf binary +*.hex binary diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml new file mode 100644 index 0000000..e9a4002 --- /dev/null +++ b/.github/workflows/ci.yaml @@ -0,0 +1,128 @@ +name: Build test lint and check formatting + +on: + push: + pull_request: + workflow_dispatch: + +permissions: + contents: read + +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: true + +# A push to a branch of this repository is checked once by its push run, so a pull request only runs +# here when it comes from a fork +jobs: + host: + name: ๐Ÿงช Host build, tests, format, lint and docs + runs-on: ubuntu-latest + if: github.event_name != 'pull_request' || github.event.pull_request.head.repo.full_name != github.repository + timeout-minutes: 45 + steps: + - name: ๐Ÿ”€ Checkout + uses: actions/checkout@v7 + + - name: ๐Ÿ‹ Build the host image + run: docker build --file .docker/Dockerfile --target host --tag micras-lib:host . + + - name: ๐Ÿ—ƒ๏ธ Restore the fetched dependencies + uses: actions/cache@v6 + with: + path: | + build/host/_deps + build/host/CMakeFiles/fc-stamp + key: deps-host-${{ hashFiles('micras_proxy/CMakeLists.txt', 'tests/CMakeLists.txt', 'cmake/micras_docs.cmake') }} + + # One long lived container runs every step below, as the user that owns the checkout + - name: ๐Ÿ“ฆ Start the host container + run: > + docker run --detach --name host --user "$(id -u):$(id -g)" --env HOME=/tmp + --volume "$PWD:/project" --workdir /project micras-lib:host sleep infinity + + - name: โš™๏ธ Configure + run: docker exec host cmake --preset host -DMICRAS_LIB_WERROR=ON + + - name: ๐Ÿ”จ Build + run: docker exec host cmake --build --preset host + + - name: ๐Ÿงช Run the tests + run: docker exec host ctest --preset host + + - name: ๐ŸŽจ Check the formatting + run: docker exec host cmake --build --preset host --target format_check + + - name: ๐Ÿšจ Lint + run: docker exec host cmake --build --preset host --target lint + + - name: ๐Ÿ“ Build the documentation + run: docker exec host cmake --build --preset host --target docs + + - name: ๐Ÿ” Check the shared code for chip macros + run: docker exec host scripts/check_no_chip_macros.sh + + - name: ๐Ÿงน Stop the host container + if: always() + run: docker rm --force host + + stm32: + name: ๐Ÿ”ฉ ${{ matrix.name }} reference and bench + runs-on: ubuntu-latest + if: github.event_name != 'pull_request' || github.event.pull_request.head.repo.full_name != github.repository + timeout-minutes: 45 + strategy: + fail-fast: false + matrix: + include: + - family: h7 + name: STM32H7 + - family: g4 + name: STM32G4 + steps: + - name: ๐Ÿ”€ Checkout + uses: actions/checkout@v7 + + - name: ๐Ÿ—ƒ๏ธ Restore the generated Cube files + id: cube + uses: actions/cache/restore@v6 + with: + path: build/generated-${{ matrix.family }} + key: cube-${{ matrix.family }}-${{ hashFiles(format('reference/{0}/reference.ioc', matrix.family), '.docker/Dockerfile') }} + + - name: โš™๏ธ Generate the Cube files + if: steps.cube.outputs.cache-hit != 'true' + run: > + docker build --file .docker/Dockerfile --target generated-${{ matrix.family }} + --output type=local,dest=build/generated-${{ matrix.family }} . + + - name: ๐Ÿ—ƒ๏ธ Save the generated Cube files + if: steps.cube.outputs.cache-hit != 'true' + uses: actions/cache/save@v6 + with: + path: build/generated-${{ matrix.family }} + key: ${{ steps.cube.outputs.cache-primary-key }} + + - name: ๐Ÿ“‚ Place the Cube files in the reference project + run: cp -r build/generated-${{ matrix.family }}/. reference/${{ matrix.family }}/ + + - name: ๐Ÿ‹ Build the ARM image + run: docker build --file .docker/Dockerfile --target stm32 --tag micras-lib:stm32 . + + - name: ๐Ÿ“ฆ Start the ARM container + run: > + docker run --detach --name stm32 --user "$(id -u):$(id -g)" --env HOME=/tmp + --volume "$PWD:/project" --workdir /project micras-lib:stm32 sleep infinity + + - name: โš™๏ธ Configure the reference project + run: docker exec stm32 cmake --preset reference-${{ matrix.family }} + + - name: ๐Ÿ”จ Build the reference and its bench + run: docker exec stm32 cmake --build --preset reference-${{ matrix.family }} --parallel + + - name: ๐Ÿšจ Lint the stm32 backend and the bench + run: docker exec stm32 cmake --build --preset reference-${{ matrix.family }} --target lint + + - name: ๐Ÿงน Stop the ARM container + if: always() + run: docker rm --force stm32 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..58b2223 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +build/ +docs/ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..3d313be --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,143 @@ +cmake_minimum_required(VERSION 3.28...4.4) + +############################################################################### +## Reference projects +############################################################################### + +# The reference presets build one of the projects under reference/, which adds micras-lib back the +# way any consumer does. They are selected here because a preset always configures this directory. +if(MICRAS_LIB_REFERENCE AND NOT MICRAS_LIB_REFERENCE_ADDED) + set(MICRAS_LIB_REFERENCE_ADDED TRUE) + add_subdirectory(reference/${MICRAS_LIB_REFERENCE}) + return() +endif() + +############################################################################### +## Project +############################################################################### + +# A project that reaches micras-lib through two submodule paths uses the one its top level adds +# first; every later add_subdirectory is a no-op +get_property(MICRAS_LIB_ALREADY_ADDED GLOBAL PROPERTY MICRAS_LIB_ADDED) + +if(MICRAS_LIB_ALREADY_ADDED) + return() +endif() + +set_property(GLOBAL PROPERTY MICRAS_LIB_ADDED TRUE) + +project(micras_lib LANGUAGES C CXX) + +# As a subproject, the consumer's language settings and flags reach the libraries by inheritance +if(micras_lib_IS_TOP_LEVEL) + set(CMAKE_C_STANDARD 17) + set(CMAKE_C_STANDARD_REQUIRED ON) + set(CMAKE_C_EXTENSIONS ON) + + set(CMAKE_CXX_STANDARD 23) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) + + set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + + # No target declares a module, and the scan puts -fmodules-ts and a module mapper into the + # compilation database, which makes clang-tidy reject every file + set(CMAKE_CXX_SCAN_FOR_MODULES OFF) + + if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + set(CMAKE_BUILD_TYPE Debug CACHE STRING "Build type" FORCE) + endif() +endif() + +############################################################################### +## Options +############################################################################### + +include(CMakeDependentOption) + +option(MICRAS_LIB_NAV "Add micras_nav" ON) +option(MICRAS_LIB_COMM "Add micras_comm" ON) +option(MICRAS_LIB_HAL "Add micras_hal" ON) +cmake_dependent_option(MICRAS_LIB_PROXY "Add micras_proxy, which fetches lsm6dsv-pid" ON "MICRAS_LIB_HAL" OFF) + +set(MICRAS_LIB_HAL_BACKEND "stm32" CACHE STRING "Backend of micras_hal: stm32 or host") +set_property(CACHE MICRAS_LIB_HAL_BACKEND PROPERTY STRINGS stm32 host) + +set(MICRAS_LIB_STM32_FAMILY "" CACHE STRING "STM32 family of the stm32 backend, detected from the board target when empty") +set_property(CACHE MICRAS_LIB_STM32_FAMILY PROPERTY STRINGS "" h7 g4) + +set(MICRAS_LIB_CUBE_TARGET "stm32cubemx" CACHE STRING "Board target micras_hal links, on every backend") + +option(MICRAS_LIB_WARNINGS "Apply the project warnings to every library target" ON) +option(MICRAS_LIB_WERROR "Treat the warnings of the library targets as errors" OFF) + +option(MICRAS_LIB_TESTS "Build the tests, on the host backend; fetches doctest" ${micras_lib_IS_TOP_LEVEL}) +option(MICRAS_LIB_DOCS "Add the docs target" ${micras_lib_IS_TOP_LEVEL}) + +if(NOT MICRAS_LIB_HAL_BACKEND MATCHES "^(stm32|host)$") + message(FATAL_ERROR "micras-lib: MICRAS_LIB_HAL_BACKEND is '${MICRAS_LIB_HAL_BACKEND}'; it must be stm32 or host.") +endif() + +if(MICRAS_LIB_TESTS AND MICRAS_LIB_HAL AND + NOT (MICRAS_LIB_HAL_BACKEND STREQUAL "host" AND MICRAS_LIB_CUBE_TARGET STREQUAL "micras_lib_host_board")) + message(FATAL_ERROR + "micras-lib: the tests run on the host backend, over the fake Cube layer of tests/support. " + "Configure with the host preset (MICRAS_LIB_HAL_BACKEND=host, " + "MICRAS_LIB_CUBE_TARGET=micras_lib_host_board), or set MICRAS_LIB_TESTS=OFF.") +endif() + +include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/micras_warnings.cmake) + +############################################################################### +## Libraries +############################################################################### + +# Every library requires C++23 without extensions, whatever the consumer builds with +function(micras_lib_setup_library TARGET_NAME) + target_compile_features(${TARGET_NAME} PUBLIC cxx_std_23) + set_target_properties(${TARGET_NAME} PROPERTIES CXX_EXTENSIONS OFF) + + if(MICRAS_LIB_WARNINGS) + micras_apply_warnings(${TARGET_NAME} WERROR ${MICRAS_LIB_WERROR}) + endif() +endfunction() + +add_subdirectory(micras_core) + +# The tests' board target, defined before the HAL as any consumer's +if(MICRAS_LIB_TESTS AND MICRAS_LIB_HAL) + add_subdirectory(tests/support/host_board) +endif() + +if(MICRAS_LIB_HAL) + add_subdirectory(micras_hal) +endif() + +if(MICRAS_LIB_PROXY) + add_subdirectory(micras_proxy) +endif() + +if(MICRAS_LIB_NAV) + add_subdirectory(micras_nav) +endif() + +if(MICRAS_LIB_COMM) + add_subdirectory(micras_comm) +endif() + +############################################################################### +## Tests, style and documentation +############################################################################### + +if(MICRAS_LIB_TESTS) + enable_testing() + add_subdirectory(tests) +endif() + +if(micras_lib_IS_TOP_LEVEL) + include(cmake/micras_lint.cmake) +endif() + +if(MICRAS_LIB_DOCS) + include(cmake/micras_docs.cmake) +endif() diff --git a/CMakePresets.json b/CMakePresets.json new file mode 100644 index 0000000..a3722d3 --- /dev/null +++ b/CMakePresets.json @@ -0,0 +1,95 @@ +{ + "version": 6, + "cmakeMinimumRequired": { + "major": 3, + "minor": 28, + "patch": 0 + }, + "configurePresets": [ + { + "name": "host-base", + "hidden": true, + "generator": "Ninja", + "binaryDir": "${sourceDir}/build/${presetName}", + "cacheVariables": { + "CMAKE_C_COMPILER": "gcc-15", + "CMAKE_CXX_COMPILER": "g++-15", + "MICRAS_LIB_HAL_BACKEND": "host", + "MICRAS_LIB_CUBE_TARGET": "micras_lib_host_board" + } + }, + { + "name": "host", + "displayName": "Host, Debug with the address and undefined behavior sanitizers", + "inherits": "host-base", + "cacheVariables": { + "CMAKE_BUILD_TYPE": "Debug", + "CMAKE_C_FLAGS": "-fsanitize=address,undefined -fno-sanitize-recover=all -fno-omit-frame-pointer", + "CMAKE_CXX_FLAGS": "-fsanitize=address,undefined -fno-sanitize-recover=all -fno-omit-frame-pointer", + "CMAKE_EXE_LINKER_FLAGS": "-fsanitize=address,undefined" + } + }, + { + "name": "host-release", + "displayName": "Host, Release", + "inherits": "host-base", + "cacheVariables": { + "CMAKE_BUILD_TYPE": "Release" + } + }, + { + "name": "reference-h7", + "displayName": "Reference STM32H725 project and its bench program", + "generator": "Unix Makefiles", + "binaryDir": "${sourceDir}/build/${presetName}", + "cacheVariables": { + "MICRAS_LIB_REFERENCE": "h7", + "CMAKE_BUILD_TYPE": "Release" + } + }, + { + "name": "reference-g4", + "displayName": "Reference STM32G474 project and its bench program", + "generator": "Unix Makefiles", + "binaryDir": "${sourceDir}/build/${presetName}", + "cacheVariables": { + "MICRAS_LIB_REFERENCE": "g4", + "CMAKE_BUILD_TYPE": "Release" + } + } + ], + "buildPresets": [ + { + "name": "host", + "configurePreset": "host" + }, + { + "name": "host-release", + "configurePreset": "host-release" + }, + { + "name": "reference-h7", + "configurePreset": "reference-h7" + }, + { + "name": "reference-g4", + "configurePreset": "reference-g4" + } + ], + "testPresets": [ + { + "name": "host", + "configurePreset": "host", + "output": { + "outputOnFailure": true + } + }, + { + "name": "host-release", + "configurePreset": "host-release", + "output": { + "outputOnFailure": true + } + } + ] +} diff --git a/Doxyfile b/Doxyfile new file mode 100644 index 0000000..c4e11a7 --- /dev/null +++ b/Doxyfile @@ -0,0 +1,45 @@ +# Doxyfile for micras-lib; built by the docs target, which fills in the $(...) variables + +PROJECT_NAME = "micras-lib" +PROJECT_NUMBER = $(MICRAS_LIB_VERSION) +PROJECT_BRIEF = "Core, navigation, communication, HAL and proxies of the Micras micromouse, for STM32 and the host" +OUTPUT_DIRECTORY = $(MICRAS_LIB_DOCS_DIR) +DISTRIBUTE_GROUP_DOC = YES +NUM_PROC_THREADS = 0 +DOT_NUM_THREADS = 0 +WARN_IF_UNDOC_ENUM_VAL = YES +EXTRACT_PRIVATE = YES +EXTRACT_PRIV_VIRTUAL = YES +EXTRACT_STATIC = YES +CASE_SENSE_NAMES = YES +SHOW_INCLUDE_FILES = NO +SHOW_HEADERFILE = NO +INPUT = README.md \ + micras_core/ \ + micras_nav/ \ + micras_comm/ \ + micras_hal/ \ + micras_proxy/ +RECURSIVE = YES +FILE_PATTERNS = *.c *.cpp *.h *.hpp *.tpp *.md +EXTENSION_MAPPING = tpp=C++ +USE_MDFILE_AS_MAINPAGE = README.md +VERBATIM_HEADERS = NO +GENERATE_HTML = YES +GENERATE_LATEX = NO +GENERATE_TREEVIEW = YES +DISABLE_INDEX = NO +FULL_SIDEBAR = NO +HTML_COLORSTYLE = AUTO_DARK +HTML_EXTRA_STYLESHEET = $(DOXYGEN_AWESOME_DIR)/doxygen-awesome.css \ + $(DOXYGEN_AWESOME_DIR)/doxygen-awesome-sidebar-only.css +UML_LOOK = YES +UML_LIMIT_NUM_FIELDS = 50 +DOT_WRAP_THRESHOLD = 100 +TEMPLATE_RELATIONS = YES +INCLUDE_GRAPH = NO +INCLUDED_BY_GRAPH = NO +CALL_GRAPH = YES +CALLER_GRAPH = YES +DOT_GRAPH_MAX_NODES = 100 +MAX_DOT_GRAPH_DEPTH = 1 diff --git a/README.md b/README.md new file mode 100644 index 0000000..c6927d2 --- /dev/null +++ b/README.md @@ -0,0 +1,281 @@ +# micras-lib + +The libraries of the [Micras](https://github.com/Team-Micras/MicrasFirmware) micromouse, for any +robot that wants them: the core utilities, the navigation, the communication protocol, the hardware +abstraction layer and the proxies of the devices behind it. The HAL has one set of headers and two +backends, selected by CMake: **stm32**, for the robot, built against the files STM32CubeMX generates +for an STM32H7 or an STM32G4, and **host**, which runs the same code on a PC for tests and simulations. + +| Library | Target | What it holds | Needs | +|---|---|---|---| +| `micras_core` | `micras::core` | math, filters, PID, COBS, the variable pool, the serializable interface | nothing | +| `micras_nav` | `micras::nav` | maze, planner, turn tables, route compiler, velocity planner, localizer, controller, mission | core | +| `micras_comm` | `micras::comm` | the framing and the session layer of the [communication protocol](micras_comm/README.md) | core | +| `micras_hal` | `micras::hal` | GPIO, PWM, DMA-fed PWM, encoder, ADC over DMA, SPI, UART over DMA, CRC, FMAC, flash, timer, MCU | the board target | +| `micras_proxy` | `micras::proxy` | IMU, rotary sensors, wall and torque sensors, motors, storage, watchdog and the rest | hal, core, ST's `lsm6dsv-pid` | +| `micras_proxy_models` | `micras::proxy_models` | LSM6DSV and AS5047U chip models (host backend only) | hal | + +## Using it + +A project adds micras-lib as a git submodule, under `external/` by convention, and adds it to its +build after defining its board target: + +```bash +git submodule add -b refactor/restructure https://github.com/Team-Micras/micras-lib.git external/micras-lib +``` + +```cmake +include(cmake/cube.cmake) # defines stm32cubemx, the board target +add_subdirectory(external/micras-lib EXCLUDE_FROM_ALL) +include(external/micras-lib/cmake/micras_warnings.cmake) + +add_executable(robot src/main.cpp) +target_link_libraries(robot PRIVATE micras::nav micras::proxy cube_app STM32_Drivers) +micras_apply_warnings(robot WERROR ${ROBOT_WERROR}) +``` + +- `EXCLUDE_FROM_ALL` builds only the libraries something links. What is fetched at configure time is + chosen by the options below: a project that needs only the navigation sets `MICRAS_LIB_HAL=OFF`, + links `micras::nav`, and builds core and nav with nothing downloaded. +- Adding micras-lib a second time, as a project that reaches it through two submodules does, is a + no-op: the first add wins. +- As a subproject micras-lib sets no language standard and no flag: the consumer's reach its + libraries by inheritance, and each library requires C++23 without extensions on its own. +- `cmake/micras_warnings.cmake` is the warning set of every Micras repository. + `micras_apply_warnings(... [WERROR ])` applies it to the consumer's own targets; the + libraries get it from `MICRAS_LIB_WARNINGS`. + +| Option | Default | Effect | +|---|---|---| +| `MICRAS_LIB_NAV`, `MICRAS_LIB_COMM` | `ON` | Add the library; core is always added | +| `MICRAS_LIB_HAL` | `ON` | Add `micras_hal` | +| `MICRAS_LIB_PROXY` | `ON` | Add `micras_proxy`, which fetches `lsm6dsv-pid`; forced off without the HAL | +| `MICRAS_LIB_HAL_BACKEND` | `stm32` | `stm32` or `host` | +| `MICRAS_LIB_STM32_FAMILY` | empty | `h7` or `g4`; read from the board target's device macro when empty | +| `MICRAS_LIB_CUBE_TARGET` | `stm32cubemx` | The board target the HAL links, on every backend | +| `MICRAS_LIB_WARNINGS` | `ON` | Apply the project warnings to every library target | +| `MICRAS_LIB_WERROR` | `OFF` | Treat them as errors | +| `MICRAS_LIB_TESTS`, `MICRAS_LIB_DOCS` | on when top level | The tests (host backend, fetches doctest) and the `docs` target | + +Third-party code is fetched with FetchContent and pinned by commit: `lsm6dsv-pid` (v5.1.1) by +`micras_proxy`, doctest (v2.5.3) by the tests and doxygen-awesome-css (v2.5.0) by the docs. + +## The stm32 backend + +A consumer of the stm32 backend: + +1. Defines the board target (`stm32cubemx` by default) **before** adding micras-lib. It carries + `main.h`, the HAL and CMSIS include directories and the device macro that selects the family + (`STM32H725xx` or `STM32G474xx`, for instance). The vendor driver and application objects (`cube_app` and + `STM32_Drivers` in the reference project) are linked into every executable: they hold the vector + table, the interrupt handlers and the `MX_*_Init` functions. +2. Enables in `stm32xx_hal_conf.h` every module the headers name: ADC, CRC, FMAC, SPI, TIM, UART, + FLASH, DMA, RCC and GPIO. +3. Keeps `USE_HAL_ADC_REGISTER_CALLBACKS` and `USE_HAL_SPI_REGISTER_CALLBACKS` at 0. micras-lib + defines `HAL_ADC_ConvCpltCallback`, `HAL_ADC_ErrorCallback`, `HAL_SPI_TxRxCpltCallback` and + `HAL_SPI_ErrorCallback`, and the consumer must not. They win over the vendor's weak defaults because + each lives in the source of a constructor the program references (`AdcDma`, `Spi`). +4. Places every object that holds a DMA buffer in memory the DMA reaches, with the data cache off or + the buffers non-cacheable. +5. Configures the CRC unit for the AS5047U (polynomial 0x1D, 8 bits, initial value 0xC4) when it uses + `RotarySensor`. +6. Keeps code out of the flash `Storage` uses, which the family names (see the table below): the upper + half of bank 1 on the STM32H7, bank 2 on the STM32G4. A sector of `Storage::Config` is the unit + the family erases, so a G4 storage spans as many 2 KB pages as its pool needs. + +### Families + +| | `h7` | `g4` | +|---|---|---| +| Parts | STM32H7, the STM32H725 of Micras v1 and the upcoming projects | STM32G4, the STM32G474RET6 of the old board, Micras v0 | +| Selected by | `STM32H7` among the board target's definitions | `STM32G4` among the board target's definitions | +| Flash | 128 KB sectors programmed in 256-bit flash words | 2 KB pages programmed in 64-bit double words | +| `Storage` region | the upper half of bank 1: sectors 4 to 7, from 0x08080000, on a 1 MB STM32H725xG | bank 2 of a 512 KB part in its factory dual bank mode: 128 pages from 0x08040000; on any other layout (single bank, a 128 or 256 KB part) every flash operation fails | +| Erasing | around 2 s per sector, up to 4 s | around 20 ms per page | +| Caches | `Mcu::init` enables the instruction cache | nothing to enable, `HAL_Init` sets up the ART accelerator | +| Watchdog | `IWDG1`, frozen under the debugger | `IWDG`, frozen under the debugger | +| `cpu_frequency_boost` | reads the `CPUFREQ_BOOST` option byte of the STM32H72x/73x | always supported, the family has no such option byte | +| ADC calibration | offset, single ended | single ended | +| Timer clock | twice the APB clock when its prescaler (`RCC_APBx_DIV1`) is not 1 | twice the APB clock when its prescaler (`RCC_HCLK_DIV1`) is not 1 | +| Cycle counter | `DWT` unlocked through its lock access register, then started | `DWT` started, the Cortex-M4 has no lock | + +The chip-independent code of the backend is in `micras_hal/stm32/src/`, and it names no chip. What +differs between families is in `micras_hal/stm32//`, which CMake compiles for the selected +family alone: the flash driver (`src/flash.cpp`), the flash geometry the shared `flash.hpp` reads +(`include/micras/hal/family/flash.hpp`: `flash_word_bits`, `sector_size`, `storage_first_sector`, +`storage_sectors`), and the steps of the shared classes that differ, the functions of the internal +header `micras/hal/family.hpp` (`calibrate_adc`, `is_dma_enabled`, `has_dma_finished`, +`enable_caches`, `was_reset_by_watchdog`, `freeze_watchdog_in_debug`, `is_cpu_frequency_supported`, +`watchdog`, `timer_clock`, `enable_cycle_counter`) in `src/family.cpp`. `scripts/check_no_chip_macros.sh` fails when a +conditional directive in the shared code (`micras_hal/include`, `micras_hal/stm32/src`, the proxies, +core, nav and comm) names a chip, and the CI runs it. + +### Reference projects and benches + +`reference/h7/` and `reference/g4/` are the smallest STM32CubeMX projects that exercise everything the +HAL and the proxies use, and nothing of Micras: an ADC scanning the internal reference and the battery +channel into a circular DMA buffer, the CRC unit set up for the AS5047U, the FMAC, a full duplex SPI +master with DMA and a GPIO chip select, a PWM channel, a DMA-fed PWM channel, an encoder timer, a +free-running timer, a UART with DMA both ways, the independent watchdog, one input and one output pin, +and the internal oscillator through the PLL. CubeMX generates no `main()` and no callback +registration. Each `CMakeLists.txt` glues the generated tree in as a consumer does +(`reference/common/toolchain.cmake` and `reference/common/reference.cmake`), adds micras-lib with +`MICRAS_LIB_WERROR=ON`, and builds and links `micras_lib_bench_`, which proves the link-time +contract above. + +| | `reference/h7` | `reference/g4` | +|---|---|---| +| Part | STM32H725RGVx | STM32G474RETx | +| Core clock | 400 MHz: HSI 64 MHz / 4 x 50 / 2, VOS1 | 170 MHz: HSI 16 MHz / 4 x 85 / 2, range 1 boost | +| UART | UART4 | USART3 | +| Supply | the direct SMPS supply of Micras v1 | the internal regulator | + +The Cube files are generated, never committed: + +```bash +docker build --file .docker/Dockerfile --target generated-g4 --output type=local,dest=reference/g4 . +cmake --preset reference-g4 +cmake --build --preset reference-g4 +``` + +and the same with `h7`. The bench program is `reference/common/bench.cpp`; each family's `bench.cpp` +names the UART and converts the internal reference with its family's low-layer ADC header. It drives +no pin, so it can be flashed on any board with the part (for the H7, one whose supply is the direct +SMPS one the project configures, as on Micras v1; for the G4, the old Micras board). It writes each +result into a `volatile` global for the debugger or STM32CubeMonitor: + +| Global | Check | +|---|---| +| `storage_passed` | a `Storage` saved before a watchdog reset is restored after it | +| `watchdog_passed` | a `Watchdog` left alone resets the chip, and the reset cause reads back | +| `timer_passed`, `timer_clock_error` | the free-running timer against the cycle counter, for the timer clock the family reports | +| `crc_passed`, `crc_value` | the CRC unit against the same CRC computed in software | +| `fmac_passed`, `fmac_error` | `FmacFilter` against the same filter computed in software | +| `adc_passed`, `vdda_mv` | the internal reference converted over DMA, against its factory calibration | +| `bench_done` | every check ran | + +The pin-driving classes (`Gpio`, `Pwm`, `PwmDma`, `Encoder`, `Spi`, `UartDma`) are compiled and linked, +but started only when the debugger sets `start_pin_drivers` before the checks end. + +## The host backend + +A consumer of the host backend: + +1. Defines its board target, a static library standing in for the Cube layer, before adding + micras-lib, and sets `MICRAS_LIB_CUBE_TARGET` to it. It provides `main.h` (which includes + `stm32_host.h`), the per-peripheral headers its configuration includes, `SystemCoreClock`, the + flash geometry macros the host's `micras/hal/family/flash.hpp` reads + (`FLASH_NB_32BITWORD_IN_FLASHWORD`, `FLASH_SECTOR_SIZE`, `FLASH_SECTOR_TOTAL`, with the values of + the chip it stands for), and the handles and `MX_*_Init` functions that fill the registers the host + `Pwm`, `Timer` and `Spi` read. It links `micras::hal`, and `micras_hal` links it back. +2. Keeps exceptions on if a thread of its own runs the program the HAL serves. +3. Resets `micras::hal::host::Board` and `micras::hal::host::Clock` between independent runs in one + process. + +`tests/support/host_board/` is the board of micras-lib's own tests: the peripherals of the reference +project under the same handle and pin names, with the values its CubeMX project generates. + +### Design + +The host backend implements every `micras_hal` class on a PC, private and static data members +included, and includes nothing but the HAL's headers and its own. It also implements the family +functions of `micras/hal/family.hpp` (`host/src/family.cpp`: nothing to calibrate, cache or unlock, a +timer's clock is its kernel clock). A change to a shared header lands with every implementation in one +commit. + +- **Ports.** `Board` is a registry of ports (`GpioPort`, `PwmPort`, `PwmDmaPort`, `AdcPort`, + `UartPort`, `EncoderPort`, `SpiPort`, `FmacPort`, `FlashPort`, `McuPort`) **keyed by the address of + the Cube handle or GPIO port** that names them, so the consumer's own configuration is the key: + `.handle = &htim15` there and `Board::pwm(&htim15, TIM_CHANNEL_1)` in a binding reach the same port. + Nothing in the backend knows a physics engine; a consumer's bindings connect ports to whatever + simulates the board. A port the program touched that nothing is bound to is listed by + `Board::unbound()`, and `McuPort` counts watchdog expiries and emergency stops instead of resetting a + process. +- **Time.** `Clock` is the only place time exists. **Every read of the timer costs one microsecond + of host time**, since a read on the robot takes time too and a loop that polls the timer must see it + advance. When a read crosses the end of a step, the clock calls its handover: whatever drives the + world advances it by one step and the read returns after it. A busy wait therefore runs the world + exactly as long as it waits, and the time inside one iteration of a loop is the number of timer reads + times the quantum: deterministic, not a measurement of anything. +- **Registers.** `stm32_host.h` holds the handle types and HAL constants, with only the fields the + backend reads or writes. The host `Pwm` and `Timer` compute frequencies and duty cycles from the + prescaler, autoreload and counter mode the board's init functions write, with the timer's kernel + clock in its `Instance`. +- **FMAC.** `Fmac` keeps the configured IIR filter in the accelerator's port and computes it in q1.15, + with exact products and the output clipped to the q1.15 range. +- **Flash.** `FlashPort` holds the reserved region as bytes, erased to 0xFF and written once per + erase, and its `write_budget` cuts writes off after a number of flash words, as a power loss would. + +### The SPI device slot + +A chip on an SPI bus is a `SpiDevice` (`micras/hal/host/spi_device.hpp`): `select()`, +`exchange(tx, rx)`, `deselect()` and the SPI mode it answers in. A binding attaches it with +`Board::spi_device(handle, cs_port, cs_pin, device)`, **keyed by the bus and the chip select**, since +one bus carries several chips. The host `Spi` selects the device in `select_device`, routes every +`transmit`, `receive` and `transmit_receive` to it, and deselects it in `unselect_device`, so a register +read the driver makes of two HAL calls is one transaction for the chip, as on the bus. + +- **The mode is checked on every transfer.** A device whose mode differs from the one `select_device` + wrote into the handle is not reached and the program reads all ones, as from a chip clocked on the + wrong edge. So is a chip select with no device, which is also an unbound port. +- **`start_transfer` completes on the host clock.** The bytes are exchanged at the start, and the + transfer ends when the host clock passes the time its bytes take at the bus's bit rate: the kernel + clock the board writes into the handle's `Instance`, divided by the baud rate prescaler. + `get_transfer`, and a `select_device` that finds the bus busy, end it through `on_transfer_end`, + which raises the chip select and sets `COMPLETE`, as the DMA interrupt does. +- **Blocking transfers take no time.** Only timer reads cost time on the host. +- **Devices outlive the drivers that talk to them.** A driver ends its last transfer when it is + destroyed; forgetting the ports with `Board::reset()` detaches the devices first. + +### The chip models + +`micras_proxy/models/` builds `micras_proxy_models` (namespace `micras::models`): SPI devices that +know the slot and nothing else, so the real `Imu` and `RotarySensor` proxies run on the host. + +- **`Lsm6dsvModel`**: the 128 registers of the main page with the datasheet defaults (WHO_AM_I 0x70), + `SW_POR` and `SW_RESET`, auto-increment under `IF_INC`, SPI mode 3. `push_sample(angular_velocity, + linear_acceleration)`, in rad/s and m/s^2 at the configured rate, encodes a sample with the full + scale in CTRL6/CTRL8, the one the driver wrote, and sets `GDA`/`XLDA`; reading a sensor's output + high byte clears its bit. A sensor whose output data rate is off ignores samples. +- **`As5047uModel`**: 24-bit frames with the CRC-8 (polynomial 0x1D, initial value 0xC4, final XOR + 0xFF) checked on every frame, answers pipelined by one frame, the volatile registers (DISABLE, + ZPOSM, ZPOSL, SETTINGS1 to 3, ECC), ERRFL with the CRC and framing error bits, SPI mode 1. The + position is not modeled: it reaches the program through the timer encoder, as on the robot. + +## Developing + +| Preset | What | +|---|---| +| `host` | Debug, the host backend and the tests under the address and undefined behavior sanitizers | +| `host-release` | Release, the host backend and the tests | +| `reference-h7` | The reference STM32H725 project and its bench, with the ARM toolchain | +| `reference-g4` | The reference STM32G474 project and its bench, with the ARM toolchain | + +```bash +cmake --preset host +cmake --build --preset host +ctest --preset host +cmake --build --preset host --target format_check lint docs +``` + +The host presets use GCC 15 (`gcc-15`, `g++-15`) and Ninja. The tests are doctest suites, one +executable per folder of `tests/` (`core`, `comm`, `hal`, `proxy`, `models`, `nav`), each case +registered in CTest. The nav suite plans on the ten contest mazes of `tests/data/mazes/` with golden +route times and step counts, which an intended change of the planner regenerates. + +`format`, `format_check`, `lint` and `lint_fix` use clang 22, found as `clang-format-22`, +`clang-tidy-22`, `run-clang-tidy-22` and `clang-apply-replacements-22`; configuring the top level +fails without them. `lint` covers the host build; `cmake --build --preset reference- --target +lint` covers the stm32 backend of that family and the bench against its ARM headers. +`scripts/check_no_chip_macros.sh` checks the shared code for chip macros. `docs` builds the Doxygen pages into +`build//docs/`. + +`.docker/Dockerfile` has the image every CI job runs in (the `host` stage, shared byte for byte with +micras-simulation and MicrasFirmware), the CubeMX generation of the reference projects +(`generated-h7`, `generated-g4`), and the ARM toolchain on top of the host stage (`stm32`). +`.github/workflows/ci.yaml` builds, tests, checks the format, lints, builds the docs and checks the +shared code for chip macros on the host, and builds and lints each reference and its bench. + +The code style is the one of every Micras repository: `.clang-format`, `.clang-tidy` (with +`tests/.clang-tidy`), Doxygen on every declaration of a header, no comments in sources, American +English, and gitmoji commit messages with one imperative sentence. diff --git a/cmake/micras_docs.cmake b/cmake/micras_docs.cmake new file mode 100644 index 0000000..1ee8c7d --- /dev/null +++ b/cmake/micras_docs.cmake @@ -0,0 +1,31 @@ +############################################################################### +## Documentation +############################################################################### + +find_program(MICRAS_DOXYGEN doxygen) + +if(NOT MICRAS_DOXYGEN) + message(FATAL_ERROR "micras-lib: doxygen was not found. Install it, or set MICRAS_LIB_DOCS=OFF.") +endif() + +include(FetchContent) + +# The theme the Doxyfile names through $(DOXYGEN_AWESOME_DIR), pinned by the commit of v2.5.0 +FetchContent_Declare( + doxygen-awesome-css + GIT_REPOSITORY https://github.com/jothepro/doxygen-awesome-css.git + GIT_TAG 46483f1e5a70ffb9ecd3b82d0a1cd1b24edf13da +) + +FetchContent_MakeAvailable(doxygen-awesome-css) + +# The Doxyfile reads the version, the theme and the output directory from the environment +add_custom_target(docs + COMMAND ${CMAKE_COMMAND} -E env + DOXYGEN_AWESOME_DIR=${doxygen-awesome-css_SOURCE_DIR} + MICRAS_LIB_DOCS_DIR=${CMAKE_BINARY_DIR}/docs + sh -c "MICRAS_LIB_VERSION=\"$(git describe --always --dirty --tags 2>/dev/null || echo unknown)\" exec ${MICRAS_DOXYGEN} Doxyfile" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + COMMENT "Generating the HTML documentation in ${CMAKE_BINARY_DIR}/docs" + VERBATIM +) diff --git a/cmake/micras_lint.cmake b/cmake/micras_lint.cmake new file mode 100644 index 0000000..6d2323b --- /dev/null +++ b/cmake/micras_lint.cmake @@ -0,0 +1,91 @@ +############################################################################### +## Formatting and linting +############################################################################### + +# The clang tools are found by their versioned names, so that a distribution update or another +# version first on the PATH can move neither the formatting rules nor the enabled check set +set(MICRAS_CLANG_VERSION 22) + +foreach(TOOL clang-format clang-tidy run-clang-tidy clang-apply-replacements) + string(TOUPPER ${TOOL} TOOL_VARIABLE) + string(REPLACE "-" "_" TOOL_VARIABLE ${TOOL_VARIABLE}) + + find_program(MICRAS_${TOOL_VARIABLE} ${TOOL}-${MICRAS_CLANG_VERSION}) + + if(NOT MICRAS_${TOOL_VARIABLE}) + message(FATAL_ERROR + "micras-lib: ${TOOL}-${MICRAS_CLANG_VERSION} was not found. The format and lint targets need " + "clang ${MICRAS_CLANG_VERSION} (apt install clang-format-${MICRAS_CLANG_VERSION} " + "clang-tidy-${MICRAS_CLANG_VERSION}), found by that name.") + endif() +endforeach() + +# micras_add_format_targets(...) +# +# format rewrites the files, format_check fails on the first one that is not formatted +function(micras_add_format_targets) + add_custom_target(format + COMMAND ${MICRAS_CLANG_FORMAT} -style=file -i ${ARGN} --verbose + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + + add_custom_target(format_check + COMMAND ${MICRAS_CLANG_FORMAT} -style=file --dry-run --Werror ${ARGN} + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) +endfunction() + +# micras_add_lint_targets(SOURCES ... [EXTRA_ARGS ...]) +# +# lint runs clang-tidy on the sources of the compilation database, with the extra arguments the +# compiler needs, and lint_fix applies the fixes it can +function(micras_add_lint_targets) + cmake_parse_arguments(PARSE_ARGV 0 LINT "" "" "SOURCES;EXTRA_ARGS") + + list(JOIN LINT_EXTRA_ARGS "\n" MICRAS_TIDY_EXTRA_ARGS) + set(MICRAS_CLANG_TIDY ${MICRAS_CLANG_TIDY}) + set(MICRAS_RUN_CLANG_TIDY ${MICRAS_RUN_CLANG_TIDY}) + set(MICRAS_CLANG_APPLY_REPLACEMENTS ${MICRAS_CLANG_APPLY_REPLACEMENTS}) + + set(SCRIPT_PATH ${CMAKE_BINARY_DIR}/run_clang_tidy.sh) + configure_file(${CMAKE_CURRENT_FUNCTION_LIST_DIR}/templates/run_clang_tidy.sh.in ${SCRIPT_PATH} @ONLY) + + add_custom_target(lint + COMMAND ${SCRIPT_PATH} ${LINT_SOURCES} + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + + add_custom_target(lint_fix + COMMAND ${SCRIPT_PATH} --fix ${LINT_SOURCES} + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) +endfunction() + +# The sources of the libraries, their tests and the bench programs, never the generated or fetched ones +get_filename_component(MICRAS_LIB_ROOT ${CMAKE_CURRENT_LIST_DIR}/.. ABSOLUTE) + +file(GLOB_RECURSE MICRAS_LIB_FORMAT_FILES CONFIGURE_DEPENDS + ${MICRAS_LIB_ROOT}/micras_*/*.cpp + ${MICRAS_LIB_ROOT}/micras_*/*.hpp + ${MICRAS_LIB_ROOT}/micras_*/*.tpp + ${MICRAS_LIB_ROOT}/micras_*/*.h + ${MICRAS_LIB_ROOT}/tests/*.cpp + ${MICRAS_LIB_ROOT}/tests/*.hpp + ${MICRAS_LIB_ROOT}/tests/*.h +) + +file(GLOB MICRAS_LIB_BENCH_FILES CONFIGURE_DEPENDS + ${MICRAS_LIB_ROOT}/reference/*/bench.cpp + ${MICRAS_LIB_ROOT}/reference/common/*.hpp +) +list(APPEND MICRAS_LIB_FORMAT_FILES ${MICRAS_LIB_BENCH_FILES}) + +if(micras_lib_IS_TOP_LEVEL) + file(GLOB_RECURSE MICRAS_LIB_LINT_SOURCES CONFIGURE_DEPENDS + ${MICRAS_LIB_ROOT}/micras_*/*.cpp + ${MICRAS_LIB_ROOT}/tests/*.cpp + ) + + micras_add_format_targets(${MICRAS_LIB_FORMAT_FILES}) + micras_add_lint_targets(SOURCES ${MICRAS_LIB_LINT_SOURCES}) +endif() diff --git a/cmake/micras_warnings.cmake b/cmake/micras_warnings.cmake new file mode 100644 index 0000000..eb201b2 --- /dev/null +++ b/cmake/micras_warnings.cmake @@ -0,0 +1,57 @@ +############################################################################### +## Project Warning Flags +############################################################################### + +# The canonical copy lives in micras-lib; micras-simulation and MicrasFirmware carry the same file +# byte for byte, and the firmware's CI compares them. The -Werror switch belongs to each repository +# (MICRAS_LIB_WERROR, MICRAS_SIM_WERROR, MICRAS_WERROR) and is passed to micras_apply_warnings. + +# The -Wduplicated-* and -Wuseless-cast flags are GCC only. -fno-common is a code generation flag: +# GCC's default for C and a no-op for C++, kept so that the robot's image stays the same. +set(MICRAS_WARNING_OPTIONS + -Wall + -Wextra + -Wpedantic + -Wcast-align + -Wcast-qual + -Wdouble-promotion + $<$:-Wduplicated-branches> + $<$:-Wduplicated-cond> + -Wfloat-conversion + -Wformat=2 + -Wimplicit-fallthrough + -Wmisleading-indentation + -Wnull-dereference + -Wsign-conversion + -Wundef + -Wunused-parameter + -fno-common + + # C++ only, they make the C compiler warn about the command line itself + $<$:-Wnon-virtual-dtor> + $<$:-Wold-style-cast> + $<$:-Woverloaded-virtual> + $<$,$>:-Wuseless-cast> +) + +# Kept globally as well, so that the function works in any directory, whichever of the three +# repositories included this file first +set_property(GLOBAL PROPERTY MICRAS_WARNING_OPTIONS ${MICRAS_WARNING_OPTIONS}) + +# micras_apply_warnings(... [WERROR ]) +# +# Applied per target, so that the generated sources are never compiled with them, and as options +# rather than through a target, which a static library would carry in its interface. The targets +# are real names, not aliases. +function(micras_apply_warnings) + cmake_parse_arguments(PARSE_ARGV 0 MICRAS_WARNINGS "" "WERROR" "") + get_property(WARNING_OPTIONS GLOBAL PROPERTY MICRAS_WARNING_OPTIONS) + + if(MICRAS_WARNINGS_WERROR) + list(APPEND WARNING_OPTIONS -Werror) + endif() + + foreach(TARGET_NAME ${MICRAS_WARNINGS_UNPARSED_ARGUMENTS}) + target_compile_options(${TARGET_NAME} PRIVATE ${WARNING_OPTIONS}) + endforeach() +endfunction() diff --git a/cmake/templates/run_clang_tidy.sh.in b/cmake/templates/run_clang_tidy.sh.in new file mode 100755 index 0000000..8de0cd4 --- /dev/null +++ b/cmake/templates/run_clang_tidy.sh.in @@ -0,0 +1,59 @@ +#!/bin/bash + +# Positional arguments are the source files to check; they are passed to run-clang-tidy as +# path regexes matched against the entries of the compilation database. +# +# The canonical copy lives in micras-lib; micras-simulation and MicrasFirmware carry the same file +# byte for byte. Each repository configures it with the clang tools it found by their versioned +# names and with the extra arguments its compiler needs (the ARM sysroot on the robot, none on the +# host), one argument per line in MICRAS_TIDY_EXTRA_ARGS. + +CLANG_TIDY="@MICRAS_CLANG_TIDY@" +RUN_CLANG_TIDY="@MICRAS_RUN_CLANG_TIDY@" +CLANG_APPLY_REPLACEMENTS="@MICRAS_CLANG_APPLY_REPLACEMENTS@" + +mapfile -t EXTRA_ARGS <<'EXTRA_ARGS_END' +@MICRAS_TIDY_EXTRA_ARGS@ +EXTRA_ARGS_END + +RUN_FLAGS=(-clang-tidy-binary "${CLANG_TIDY}") + +for ARG in "${EXTRA_ARGS[@]}"; do + if [[ -n "${ARG}" ]]; then + RUN_FLAGS+=("-extra-arg-before=${ARG}") + fi +done + +if [[ "${1:-}" == "--fix" ]]; then + RUN_FLAGS+=(-fix -format -clang-apply-replacements-binary "${CLANG_APPLY_REPLACEMENTS}") + shift +fi + +if [[ -t 1 ]]; then + RUN_FLAGS+=(-use-color) +fi + +# Unknown check names are silently ignored during a normal run, so the configuration is +# validated explicitly to keep it from rotting across clang-tidy versions. +"${CLANG_TIDY}" --verify-config || exit 1 + +echo "Running clang-tidy on all source files..." + +# run-clang-tidy bounds the number of concurrent clang-tidy processes, unlike a plain +# background job per file, and serializes the fixes it applies to shared headers. +# The filtered lines are the echoed clang-tidy invocations and the per-file diagnostic +# counters, which include the warnings suppressed in the vendor headers and are always noise. +# Real diagnostics name their own file, so nothing useful is hidden. +# The turn tables are designed as a constant expression wherever a program compiles them, which +# takes more evaluation steps than clang allows by default. +"${RUN_CLANG_TIDY}" \ + "${RUN_FLAGS[@]}" \ + -quiet \ + -j "$(nproc)" \ + -warnings-as-errors="*" \ + -p "@CMAKE_BINARY_DIR@" \ + -extra-arg-before=-Wno-unknown-warning-option \ + -extra-arg=-fconstexpr-steps=1000000000 \ + "$@" 2>&1 | grep -Ev '^(\[ *[0-9]+/[0-9]+\]|[0-9]+ (warning|error)s? generated\.|clang-tidy(-[0-9.]+)? )' + +exit "${PIPESTATUS[0]}" diff --git a/micras_comm/CMakeLists.txt b/micras_comm/CMakeLists.txt new file mode 100644 index 0000000..4ad6c92 --- /dev/null +++ b/micras_comm/CMakeLists.txt @@ -0,0 +1,19 @@ +file(GLOB_RECURSE MICRAS_COMM_SOURCES CONFIGURE_DEPENDS "src/*.c*") + +add_library(micras_comm STATIC + ${MICRAS_COMM_SOURCES} +) + +add_library(micras::comm ALIAS micras_comm) + +target_include_directories(micras_comm PUBLIC + include +) + +# Only the reflection and the framing, so that the whole session layer can be built and tested +# without a single peripheral behind it +target_link_libraries(micras_comm PUBLIC + micras::core +) + +micras_lib_setup_library(micras_comm) diff --git a/micras_comm/README.md b/micras_comm/README.md new file mode 100644 index 0000000..8455463 --- /dev/null +++ b/micras_comm/README.md @@ -0,0 +1,53 @@ +# Communication protocol + +The session layer the robot runs over a byte stream to expose its variables to an application such +as [micras-monitor](https://github.com/Team-Micras/micras-monitor): a schema, streamed groups of +samples, writes, reads and commands. It knows nothing of the transport below the stream; the project +that uses it documents the radio and the wiring it runs over. + +The protocol assumes a transport with no flow control, which drops bytes silently when its buffer +fills, as a serial Bluetooth module without `RTS`/`CTS` does. The credit window of the session layer +exists to keep it from filling. + +## Framing + +Frames are [COBS](https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing) encoded and +delimited by a zero byte: + +```text +COBS( type | payload | fletcher16 ) 0x00 +``` + +The frame check is Fletcher-16 over the type and the payload, before encoding, little endian. It is +not protecting against the radio, which has a CRC-24 and retransmits until acknowledged. It covers +the two hops BLE never sees: the 8N1 serial port with no parity between the microcontroller and the +module, and the module's buffer, which drops runs of bytes with no indication that it did. A +dropped run can join the head of one frame to the tail of the next into something with a plausible +shape, and the two running sums make that about as unlikely as a sixteen bit check can: one over +sixty five thousand, against one in two hundred and fifty five for a single sum of bytes. It is +weaker than a CRC on long bursts, which is what a CRC is uniquely good at, and it is four lines +that live beside the codec they belong to. + +Message types, their payloads and the meaning of every field are in +`micras_comm/include/micras/comm/protocol.hpp`. Everything on the wire is little endian, which is +what both the microcontroller and `DataView` in the browser already are. + +## The parts worth knowing before reading the code + +- **The schema is fetched once per firmware build, not once per connection.** `HELLO_ACK` carries a + `schema_hash` over every name, type and access flag in order. Identifiers are registration order, + so adding one variable shifts every later one; comparing the hash against the one a cached schema + was fetched with is what stops the application from plotting the wrong signal. +- **Samples come in groups, not one variable at a time.** A `SAMPLE` carries several variables + captured in the same control loop iteration under one timestamp. A response plotted against a + setpoint captured two iterations later is not a plot of a control loop. +- **The application has to return credit.** The robot may have at most `initial_credit` bytes + outstanding. `CREDIT` says how many more bytes the application has taken. When the window is + closed, samples are dropped and the sequence number shows the gap; replies to requests are not + charged to the window, because they are already bounded by the rate of the requests themselves. +- **Writes are levels and commands are edges.** `WRITE` sets a gain or a flag and is acknowledged + with a result; the `idle` flag on a variable refuses the dangerous ones while the robot is moving. + `COMMAND` happens once, when it arrives. +- **The link cannot carry the control loop.** It is between twenty and a hundred times too slow for + 8 kHz, so a group is defined with a period in loop iterations and only every period-th iteration + is sent. The rate the application asks for is a rate it can actually receive. diff --git a/micras_comm/include/micras/comm/frame.hpp b/micras_comm/include/micras/comm/frame.hpp new file mode 100644 index 0000000..8120388 --- /dev/null +++ b/micras_comm/include/micras/comm/frame.hpp @@ -0,0 +1,192 @@ +/** + * @file + */ + +#ifndef MICRAS_COMM_FRAME_HPP +#define MICRAS_COMM_FRAME_HPP + +#include +#include +#include +#include +#include + +#include "micras/comm/protocol.hpp" + +namespace micras::comm { +/** + * @brief Append little endian values to a payload buffer. + * + * @note Every write is checked against the end of the buffer and the first one that does not fit + * poisons the writer, so a caller only has to check once, at the end. + */ +class Writer { +public: + /** + * @brief Construct a new Writer object. + * + * @param into Buffer to write into. + */ + explicit Writer(std::span into); + + /** + * @brief Append a value of one, two or four bytes. + */ + ///@{ + void u8(uint8_t value); + void u16(uint16_t value); + void u32(uint32_t value); + ///@} + + /** + * @brief Append raw bytes. + * + * @param bytes Bytes to append. + */ + void raw(std::span bytes); + + /** + * @brief Append a string with no length prefix. + * + * @param value Text to append. + */ + void text(std::string_view value); + + /** + * @brief Get what has been written so far. + * + * @return View over the written bytes, empty if anything did not fit. + */ + std::span done() const; + + /** + * @brief Get how many bytes can still be appended. + * + * @return Number of bytes left in the buffer. + */ + std::size_t left() const; + +private: + std::span buffer; + std::size_t size{}; + bool overflowed{}; +}; + +/** + * @brief Take little endian values from a payload buffer. + * + * @note Reading past the end yields zero and marks the reader as bad, so a caller only has to + * check once, after taking everything it expects. + */ +class Reader { +public: + /** + * @brief Construct a new Reader object. + * + * @param from Buffer to read from. + */ + explicit Reader(std::span from); + + /** + * @brief Take a value of one, two or four bytes. + */ + ///@{ + uint8_t u8(); + uint16_t u16(); + uint32_t u32(); + ///@} + + /** + * @brief Take everything that is left. + * + * @return View over the remaining bytes. + */ + std::span rest() const; + + /** + * @brief Check whether every value taken so far was inside the buffer. + * + * @return True if nothing was read past the end, false otherwise. + */ + bool valid() const; + +private: + std::span buffer; + std::size_t index{}; + bool underflowed{}; +}; + +/** + * @brief Build a complete frame around a payload. + * + * @param type Type of the message. + * @param payload Payload of the message. + * @param into Buffer to build the frame in, which should hold max_frame_size bytes. + * @return Number of bytes written, or zero if the frame does not fit. + */ +std::size_t encode_frame(MessageType type, std::span payload, std::span into); + +/** + * @brief Recover frames from a stream of bytes. + * + * @note Every encoded frame is free of the delimiter by construction, so a corrupt or truncated + * frame costs exactly the bytes up to the next delimiter and never desynchronizes the reader. + */ +class FrameReader { +public: + /** + * @brief Feed one byte. + * + * @param byte Byte taken from the stream. + * @return True if a whole valid frame is now available, false otherwise. + */ + bool push(uint8_t byte); + + /** + * @brief Get the type of the frame that was just completed. + * + * @return Type of the message. + */ + MessageType type() const; + + /** + * @brief Get the payload of the frame that was just completed. + * + * @return View over the payload. + */ + std::span payload() const; + + /** + * @brief Get how many frames were thrown away for failing the frame check or for being + * malformed. + * + * @return Number of frames discarded since the reader was created. + */ + uint32_t discarded() const; + +private: + /** + * @brief Decode and check the frame currently in the buffer. + * + * @return True if the frame is whole and passes the frame check, false otherwise. + */ + bool finish(); + + /** + * @brief Bytes of the current frame, still encoded. + */ + std::array encoded{}; + + /** + * @brief Decoded type, payload and frame check of the current frame. + */ + std::array decoded{}; + + std::size_t encoded_size{}; + std::size_t decoded_size{}; + uint32_t discarded_frames{}; + bool overrun{}; +}; +} // namespace micras::comm + +#endif // MICRAS_COMM_FRAME_HPP diff --git a/micras_comm/include/micras/comm/link.hpp b/micras_comm/include/micras/comm/link.hpp new file mode 100644 index 0000000..f4bb788 --- /dev/null +++ b/micras_comm/include/micras/comm/link.hpp @@ -0,0 +1,275 @@ +/** + * @file + */ + +#ifndef MICRAS_COMM_LINK_HPP +#define MICRAS_COMM_LINK_HPP + +#include +#include +#include +#include +#include + +#include "micras/comm/frame.hpp" +#include "micras/comm/protocol.hpp" +#include "micras/core/byte_stream.hpp" +#include "micras/core/variable_pool.hpp" + +namespace micras::comm { +/** + * @brief Result of a command sent over the link. + */ +enum class CommandResult : uint8_t { + OK = 0, + UNKNOWN = 1, + REFUSED = 2 +}; + +/** + * @brief Interface for whatever turns a command into an action on the robot. + * + * @note Commands are edges and writes are levels, which is the distinction that keeps a command + * from being re-applied on every iteration. The link never holds a command flag of its own. + */ +class ICommandHandler { +public: + /** + * @brief Virtual destructor for the ICommandHandler class. + */ + virtual ~ICommandHandler() = default; + + /** + * @brief Act on a command. + * + * @param code Command to run. + * @param argument Argument of the command. + * @return Whether the command ran. + */ + virtual CommandResult handle_command(uint8_t code, uint32_t argument) = 0; + +protected: + /** + * @brief Special member functions declared as default. + */ + ///@{ + ICommandHandler() = default; + ICommandHandler(const ICommandHandler&) = default; + ICommandHandler(ICommandHandler&&) = default; + ICommandHandler& operator=(const ICommandHandler&) = default; + ICommandHandler& operator=(ICommandHandler&&) = default; + ///@} +}; + +/** + * @brief Session over a byte stream, exposing the variable pool to the outside world. + */ +class Link { +public: + /** + * @brief Configuration struct for the link. + */ + struct Config { + /** + * @brief Period of the control loop in microseconds, which is the unit of a group period. + */ + uint32_t loop_time_us; + }; + + /** + * @brief Construct a new Link object. + * + * @param stream Transport the session runs over. + * @param pool Variables the session exposes. + * @param commands Handler the commands are given to. + * @param config Configuration for the link. + */ + Link(core::IByteStream& stream, core::VariablePool& pool, ICommandHandler& commands, const Config& config); + + /** + * @brief Take whatever arrived and act on at most one message. + * + * @note Limiting this to one message per iteration is what keeps a burst of requests from + * spending the loop budget, and one message per 125 us is far faster than the link can deliver + * them anyway. + * + * @param robot_is_idle Whether the robot is stopped, which gates the guarded writes. + */ + void poll(bool robot_is_idle); + + /** + * @brief Send whatever is due. + * + * @note Called every iteration. It costs a few compares when no group is due. + * + * @param timestamp_us Current time in microseconds, stamped on the samples. + */ + void pump(uint32_t timestamp_us); + + /** + * @brief Send a message to the application. + * + * @note Dropped when the transport is full rather than queued, and the number of dropped + * messages is itself registered in the pool, so a gap is visible instead of silent. + * + * @param severity Severity of the message. + * @param text Message to send. + */ + void log(Severity severity, std::string_view text); + + /** + * @brief Register the counters of the link itself. + * + * @param pool Pool to register into. + * @param prefix Prefix of the names. + */ + void register_variables(core::VariablePool& pool, std::string_view prefix); + +private: + /** + * @brief A set of variables sampled in the same loop iteration and sent under one timestamp. + * + * @note Sampling several variables under one header and one timestamp is not only cheaper on a + * link this slow, it is the only way the samples mean anything together. A response plotted + * against a setpoint captured two iterations later is a plot of the loop plus an unknown delay. + */ + struct Group { + /** + * @brief Variables of the group, in the order their values are packed. + */ + std::array ids{}; + + /** + * @brief Number of variables in the group. + */ + uint8_t count{}; + + /** + * @brief Number of loop iterations between two samples. + */ + uint16_t period{1}; + + /** + * @brief Number of bytes of the values of one sample. + */ + uint16_t sample_size{}; + + /** + * @brief Iterations left until the next sample. + */ + uint16_t counter{}; + + /** + * @brief Number of samples taken, which lets the application see the ones that were dropped. + */ + uint16_t sequence{}; + + /** + * @brief Whether the group is being sent. + */ + bool enabled{}; + }; + + /** + * @brief Act on one decoded message. + * + * @param robot_is_idle Whether the robot is stopped. + */ + void execute(bool robot_is_idle); + + /** + * @brief Handlers for each kind of request. + */ + ///@{ + void on_hello(); + void on_schema_request(Reader& reader); + void on_group_define(Reader& reader); + void on_group_enable(Reader& reader); + void on_credit(Reader& reader); + void on_write(Reader& reader, bool robot_is_idle); + void on_read(Reader& reader); + void on_command(Reader& reader); + ///@} + + /** + * @brief Send one page of the schema, if one was asked for. + */ + void send_schema_page(); + + /** + * @brief Send an error for a message that could not be acted on. + * + * @param code Reason the message was refused. + * @param context Whatever identifies the offending message. + */ + void send_error(ErrorCode code, uint16_t context); + + /** + * @brief Send a frame the application asked for, which is bounded by its own request rate and + * so is not charged to the credit window. + * + * @param type Type of the message. + * @param payload Payload of the message. + * @return True if the transport took the frame, false otherwise. + */ + bool send(MessageType type, std::span payload); + + /** + * @brief Send a frame the robot produced on its own, which the credit window has to allow. + * + * @param type Type of the message. + * @param payload Payload of the message. + * @return True if the transport took the frame, false otherwise. + */ + bool send_metered(MessageType type, std::span payload); + + // NOLINTBEGIN(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + core::IByteStream& stream; + core::VariablePool& pool; + ICommandHandler& commands; + // NOLINTEND(*-avoid-const-or-ref-data-members) + + Config config; + + FrameReader reader; + + /** + * @brief Bytes taken from the transport but not yet fed to the frame reader. + */ + ///@{ + std::array staging{}; + std::size_t staged{}; + std::size_t consumed{}; + ///@} + + /** + * @brief Scratch buffers for the message being built and for the frame around it. + */ + ///@{ + std::array payload{}; + std::array frame{}; + ///@} + + std::array groups{}; + + /** + * @brief Bytes the application still allows to be sent on the robot's own initiative. + */ + int32_t credit{initial_credit}; + + /** + * @brief Index of the next schema entry to send, past the last one when no page is due. + */ + uint16_t schema_index{}; + + /** + * @brief Counters worth watching from the application. + */ + ///@{ + uint32_t dropped_samples{}; + uint32_t dropped_logs{}; + ///@} +}; +} // namespace micras::comm + +#endif // MICRAS_COMM_LINK_HPP diff --git a/micras_comm/include/micras/comm/protocol.hpp b/micras_comm/include/micras/comm/protocol.hpp new file mode 100644 index 0000000..58cdfd8 --- /dev/null +++ b/micras_comm/include/micras/comm/protocol.hpp @@ -0,0 +1,102 @@ +/** + * @file + */ + +#ifndef MICRAS_COMM_PROTOCOL_HPP +#define MICRAS_COMM_PROTOCOL_HPP + +#include +#include + +#include "micras/core/cobs.hpp" + +namespace micras::comm { +/** + * @brief Version of the wire format, refused by the application when it does not match. + */ +constexpr uint8_t protocol_version{1}; + +/** + * @brief Largest payload of a message, not counting the type and the frame check. + */ +constexpr std::size_t max_payload_size{200}; + +/** + * @brief Largest size of an encoded frame, including its delimiter. + */ +constexpr std::size_t max_frame_size{core::cobs_encoded_size(max_payload_size + 3) + 1}; + +/** + * @brief Number of groups that can be defined at the same time. + */ +constexpr uint8_t max_groups{4}; + +/** + * @brief Number of variables a single group can hold. + */ +constexpr uint8_t max_group_variables{16}; + +/** + * @brief Number of bytes the robot may send before the application returns any credit. + * + * @note The radio module has no flow control and drops silently when its buffer fills, so this is + * the only thing keeping the firmware from overrunning it. + */ +constexpr uint16_t initial_credit{256}; + +/** + * @brief Type of a message, which is the first byte of every frame. + * + * @note Requests have the top bit clear and responses have it set, so that the direction of a + * frame can be told from its type alone. + * + * @note PING exists because a radio link can stall without either end being told, and because the + * only other round trip that proves the robot is alive is HELLO, which resets the session. + */ +enum class MessageType : uint8_t { + HELLO = 0x01, + SCHEMA_REQUEST = 0x02, + GROUP_DEFINE = 0x03, + GROUP_ENABLE = 0x04, + CREDIT = 0x05, + WRITE = 0x06, + READ = 0x07, + COMMAND = 0x08, + PING = 0x09, + + HELLO_ACK = 0x81, + SCHEMA_PAGE = 0x82, + GROUP_ACK = 0x83, + SAMPLE = 0x85, + WRITE_ACK = 0x86, + VALUE = 0x87, + COMMAND_ACK = 0x88, + PONG = 0x89, + LOG = 0x8A, + ERROR = 0x8F +}; + +/** + * @brief Reason a frame could not be acted on. + */ +enum class ErrorCode : uint8_t { + UNKNOWN_TYPE = 0, + MALFORMED = 1, + NO_SUCH_GROUP = 2, + GROUP_TOO_LARGE = 3, + NOT_STREAMABLE = 4, + NO_SUCH_VARIABLE = 5 +}; + +/** + * @brief Severity of a log message. + */ +enum class Severity : uint8_t { + DEBUG = 0, + INFO = 1, + WARNING = 2, + ERROR = 3 +}; +} // namespace micras::comm + +#endif // MICRAS_COMM_PROTOCOL_HPP diff --git a/micras_comm/src/frame.cpp b/micras_comm/src/frame.cpp new file mode 100644 index 0000000..ccaff5e --- /dev/null +++ b/micras_comm/src/frame.cpp @@ -0,0 +1,190 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/comm/frame.hpp" +#include "micras/comm/protocol.hpp" +#include "micras/core/cobs.hpp" + +namespace micras::comm { +// The buffers here are all fixed size arrays and spans the caller owns, indexed right after the +// check that bounds the index, so the checked accessors would only repeat the comparison. +// NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access,cppcoreguidelines-pro-bounds-constant-array-index) +Writer::Writer(std::span into) : buffer{into} { } + +void Writer::u8(uint8_t value) { + if (this->size >= this->buffer.size()) { + this->overflowed = true; + return; + } + + this->buffer[this->size++] = value; +} + +void Writer::u16(uint16_t value) { + this->u8(value); + this->u8(value >> 8); +} + +void Writer::u32(uint32_t value) { + this->u16(value); + this->u16(value >> 16); +} + +void Writer::raw(std::span bytes) { + if (bytes.size() > this->buffer.size() - this->size) { + this->overflowed = true; + return; + } + + if (not bytes.empty()) { + std::memcpy(&this->buffer[this->size], bytes.data(), bytes.size()); + this->size += bytes.size(); + } +} + +void Writer::text(std::string_view value) { + this->raw({std::bit_cast(value.data()), value.size()}); +} + +std::span Writer::done() const { + return this->overflowed ? std::span{} : this->buffer.first(this->size); +} + +std::size_t Writer::left() const { + return this->overflowed ? 0 : this->buffer.size() - this->size; +} + +Reader::Reader(std::span from) : buffer{from} { } + +uint8_t Reader::u8() { + if (this->index >= this->buffer.size()) { + this->underflowed = true; + return 0; + } + + return this->buffer[this->index++]; +} + +uint16_t Reader::u16() { + const uint16_t low = this->u8(); + return static_cast(low | this->u8() << 8); +} + +uint32_t Reader::u32() { + const uint32_t low = this->u16(); + return low | static_cast(this->u16()) << 16; +} + +std::span Reader::rest() const { + return this->underflowed ? std::span{} : this->buffer.subspan(this->index); +} + +bool Reader::valid() const { + return not this->underflowed; +} + +static uint16_t fletcher16(std::span data) { + uint16_t low = 0; + uint16_t high = 0; + + for (const uint8_t byte : data) { + low = static_cast((low + byte) % 255); + high = static_cast((high + low) % 255); + } + + return static_cast(high << 8U | low); +} + +std::size_t encode_frame(MessageType type, std::span payload, std::span into) { + if (payload.size() > max_payload_size or into.empty()) { + return 0; + } + + std::array plain{}; + plain[0] = std::to_underlying(type); + + if (not payload.empty()) { + std::memcpy(&plain[1], payload.data(), payload.size()); + } + + const std::size_t checked_size = payload.size() + 1; + const uint16_t check = fletcher16(std::span{plain}.first(checked_size)); + + plain[checked_size] = check; + plain[checked_size + 1] = check >> 8; + + const std::size_t size = core::cobs_encode(std::span{plain}.first(checked_size + 2), into.first(into.size() - 1)); + + if (size == 0) { + return 0; + } + + into[size] = core::cobs_delimiter; + return size + 1; +} + +bool FrameReader::push(uint8_t byte) { + if (byte != core::cobs_delimiter) { + if (this->encoded_size >= this->encoded.size()) { + this->overrun = true; + } else { + this->encoded[this->encoded_size++] = byte; + } + + return false; + } + + const bool complete = not this->overrun and this->encoded_size > 0 and this->finish(); + + if (not complete and (this->overrun or this->encoded_size > 0)) { + this->discarded_frames++; + } + + this->encoded_size = 0; + this->overrun = false; + + return complete; +} + +bool FrameReader::finish() { + const std::size_t size = core::cobs_decode(std::span{this->encoded}.first(this->encoded_size), this->decoded); + + if (size < 3) { + return false; + } + + const std::span checked = std::span{this->decoded}.first(size - 2); + const auto expected = static_cast(this->decoded[size - 2] | this->decoded[size - 1] << 8); + + if (fletcher16(checked) != expected) { + return false; + } + + this->decoded_size = size - 2; + return true; +} + +MessageType FrameReader::type() const { + return static_cast(this->decoded[0]); +} + +std::span FrameReader::payload() const { + return std::span{this->decoded}.subspan(1, this->decoded_size - 1); +} + +uint32_t FrameReader::discarded() const { + return this->discarded_frames; +} + +// NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access,cppcoreguidelines-pro-bounds-constant-array-index) +} // namespace micras::comm diff --git a/micras_comm/src/link.cpp b/micras_comm/src/link.cpp new file mode 100644 index 0000000..7969e2e --- /dev/null +++ b/micras_comm/src/link.cpp @@ -0,0 +1,388 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "micras/comm/frame.hpp" +#include "micras/comm/link.hpp" +#include "micras/comm/protocol.hpp" +#include "micras/core/byte_stream.hpp" +#include "micras/core/serializable.hpp" +#include "micras/core/variable_pool.hpp" + +namespace micras::comm { +Link::Link(core::IByteStream& stream, core::VariablePool& pool, ICommandHandler& commands, const Config& config) : + stream{stream}, pool{pool}, commands{commands}, config{config} { + this->schema_index = pool.all().size(); +} + +void Link::register_variables(core::VariablePool& pool, std::string_view prefix) { + pool.add(prefix, "dropped_samples", this->dropped_samples, {.stream = true}); + pool.add(prefix, "dropped_logs", this->dropped_logs, {.stream = true}); + pool.add(prefix, "credit", this->credit, {.stream = true}); +} + +void Link::poll(bool robot_is_idle) { + if (this->consumed == this->staged) { + this->staged = this->stream.read(this->staging); + this->consumed = 0; + } + + while (this->consumed < this->staged) { + if (this->reader.push(this->staging.at(this->consumed++))) { + this->execute(robot_is_idle); + return; + } + } +} + +void Link::execute(bool robot_is_idle) { + Reader payload_reader{this->reader.payload()}; + + switch (this->reader.type()) { + case MessageType::HELLO: + this->on_hello(); + return; + + case MessageType::SCHEMA_REQUEST: + this->on_schema_request(payload_reader); + return; + + case MessageType::GROUP_DEFINE: + this->on_group_define(payload_reader); + return; + + case MessageType::GROUP_ENABLE: + this->on_group_enable(payload_reader); + return; + + case MessageType::CREDIT: + this->on_credit(payload_reader); + return; + + case MessageType::WRITE: + this->on_write(payload_reader, robot_is_idle); + return; + + case MessageType::READ: + this->on_read(payload_reader); + return; + + case MessageType::COMMAND: + this->on_command(payload_reader); + return; + + case MessageType::PING: + this->send(MessageType::PONG, {}); + return; + + default: + this->send_error(ErrorCode::UNKNOWN_TYPE, std::to_underlying(this->reader.type())); + return; + } +} + +void Link::on_hello() { + for (Group& group : this->groups) { + group = {}; + } + + this->schema_index = this->pool.all().size(); + this->credit = initial_credit; + + Writer writer{this->payload}; + writer.u8(protocol_version); + writer.u32(this->pool.schema_hash()); + writer.u16(this->pool.all().size()); + writer.u32(this->config.loop_time_us); + writer.u16(initial_credit); + + this->send(MessageType::HELLO_ACK, writer.done()); +} + +void Link::on_schema_request(Reader& reader) { + const uint16_t first = reader.u16(); + + if (not reader.valid() or first > this->pool.all().size()) { + this->send_error(ErrorCode::MALFORMED, first); + return; + } + + this->schema_index = first; +} + +void Link::on_group_define(Reader& reader) { + const uint8_t index = reader.u8(); + const uint16_t period = reader.u16(); + const uint8_t count = reader.u8(); + + if (not reader.valid() or index >= max_groups) { + this->send_error(ErrorCode::NO_SUCH_GROUP, index); + return; + } + + if (count > max_group_variables) { + this->send_error(ErrorCode::GROUP_TOO_LARGE, count); + return; + } + + Group group{}; + group.count = count; + group.period = std::max(period, 1); + + for (uint8_t position = 0; position < count; position++) { + const core::VariableId id = reader.u16(); + + if (id >= this->pool.all().size()) { + this->send_error(ErrorCode::NO_SUCH_VARIABLE, id); + return; + } + + const core::Variable& variable = this->pool.at(id); + + if (not variable.access.stream) { + this->send_error(ErrorCode::NOT_STREAMABLE, id); + return; + } + + group.ids.at(position) = id; + group.sample_size += variable.size; + } + + if (not reader.valid() or group.sample_size + 7U > max_payload_size) { + this->send_error(ErrorCode::GROUP_TOO_LARGE, group.sample_size); + return; + } + + this->groups.at(index) = group; + + Writer writer{this->payload}; + writer.u8(index); + writer.u16(group.period); + writer.u16(group.sample_size); + + this->send(MessageType::GROUP_ACK, writer.done()); +} + +void Link::on_group_enable(Reader& reader) { + const uint8_t index = reader.u8(); + const bool enable = reader.u8() != 0; + + if (not reader.valid() or index >= max_groups or this->groups.at(index).count == 0) { + this->send_error(ErrorCode::NO_SUCH_GROUP, index); + return; + } + + Group& group = this->groups.at(index); + group.enabled = enable; + group.counter = 0; + + Writer writer{this->payload}; + writer.u8(index); + writer.u16(group.period); + writer.u16(group.sample_size); + + this->send(MessageType::GROUP_ACK, writer.done()); +} + +void Link::on_credit(Reader& reader) { + const uint16_t bytes = reader.u16(); + + if (reader.valid()) { + this->credit = std::min(this->credit + bytes, initial_credit); + } +} + +void Link::on_write(Reader& reader, bool robot_is_idle) { + const core::VariableId id = reader.u16(); + + if (not reader.valid()) { + this->send_error(ErrorCode::MALFORMED, id); + return; + } + + const auto status = this->pool.write(id, reader.rest(), robot_is_idle); + + Writer writer{this->payload}; + writer.u16(id); + writer.u8(std::to_underlying(status)); + + this->send(MessageType::WRITE_ACK, writer.done()); +} + +void Link::on_read(Reader& reader) { + const core::VariableId id = reader.u16(); + + if (not reader.valid() or id >= this->pool.all().size()) { + this->send_error(ErrorCode::NO_SUCH_VARIABLE, id); + return; + } + + Writer writer{this->payload}; + writer.u16(id); + + const core::Variable& variable = this->pool.at(id); + + if (variable.type == core::TypeCode::BLOB) { + const auto data = static_cast(variable.address)->serialize(); + + if (data.size() > writer.left()) { + this->send_error(ErrorCode::GROUP_TOO_LARGE, id); + return; + } + + writer.raw(data); + } else { + writer.raw({std::bit_cast(variable.address), variable.size}); + } + + this->send(MessageType::VALUE, writer.done()); +} + +void Link::on_command(Reader& reader) { + const uint8_t code = reader.u8(); + const uint32_t argument = reader.u32(); + + if (not reader.valid()) { + this->send_error(ErrorCode::MALFORMED, code); + return; + } + + const CommandResult result = this->commands.handle_command(code, argument); + + Writer writer{this->payload}; + writer.u8(code); + writer.u8(std::to_underlying(result)); + + this->send(MessageType::COMMAND_ACK, writer.done()); +} + +void Link::send_error(ErrorCode code, uint16_t context) { + Writer writer{this->payload}; + writer.u8(std::to_underlying(code)); + writer.u16(context); + + this->send(MessageType::ERROR, writer.done()); +} + +void Link::log(Severity severity, std::string_view text) { + Writer writer{this->payload}; + writer.u8(std::to_underlying(severity)); + writer.text(text.substr(0, std::min(text.size(), max_payload_size - 1))); + + if (not this->send(MessageType::LOG, writer.done())) { + this->dropped_logs++; + } +} + +bool Link::send(MessageType type, std::span payload) { + const std::size_t size = encode_frame(type, payload, this->frame); + + return size > 0 and this->stream.write(std::span{this->frame}.first(size)) == size; +} + +bool Link::send_metered(MessageType type, std::span payload) { + const std::size_t size = encode_frame(type, payload, this->frame); + + if (size == 0 or std::cmp_less(this->credit, size)) { + return false; + } + + if (this->stream.write(std::span{this->frame}.first(size)) != size) { + return false; + } + + this->credit -= static_cast(size); + return true; +} + +void Link::send_schema_page() { + const std::span variables = this->pool.all(); + + if (this->schema_index >= variables.size()) { + return; + } + + constexpr std::size_t count_offset{8}; + + Writer writer{this->payload}; + writer.u32(this->pool.schema_hash()); + writer.u16(this->schema_index); + writer.u16(variables.size()); + writer.u8(0); + + uint16_t index = this->schema_index; + uint8_t count = 0; + + while (index < variables.size() and count < UINT8_MAX) { + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) bounded by the loop + const core::Variable& variable = variables[index]; + const std::size_t name_size = variable.prefix.size() + variable.name.size(); + + if (name_size + 3 > writer.left()) { + break; + } + + writer.u8(std::to_underlying(variable.type)); + writer.u8(variable.access.to_byte()); + writer.u8(name_size); + writer.text(variable.prefix); + writer.text(variable.name); + + index++; + count++; + } + + if (count == 0) { + this->schema_index = variables.size(); + return; + } + + this->payload.at(count_offset) = count; + + if (this->send_metered(MessageType::SCHEMA_PAGE, writer.done())) { + this->schema_index = index; + } +} + +void Link::pump(uint32_t timestamp_us) { + for (uint8_t index = 0; index < max_groups; index++) { + Group& group = this->groups.at(index); + + if (not group.enabled) { + continue; + } + + if (group.counter > 0) { + group.counter--; + continue; + } + + group.counter = group.period - 1; + + Writer writer{this->payload}; + writer.u8(index); + writer.u16(group.sequence++); + writer.u32(timestamp_us); + + std::size_t offset = writer.done().size(); + + for (uint8_t position = 0; position < group.count; position++) { + offset += this->pool.read(group.ids.at(position), std::span{this->payload}.subspan(offset)); + } + + if (not this->send_metered(MessageType::SAMPLE, std::span{this->payload}.first(offset))) { + this->dropped_samples++; + } + } + + this->send_schema_page(); +} +} // namespace micras::comm diff --git a/micras_core/CMakeLists.txt b/micras_core/CMakeLists.txt new file mode 100644 index 0000000..09741c9 --- /dev/null +++ b/micras_core/CMakeLists.txt @@ -0,0 +1,13 @@ +file(GLOB_RECURSE MICRAS_CORE_SOURCES CONFIGURE_DEPENDS "src/*.c*") + +add_library(micras_core STATIC + ${MICRAS_CORE_SOURCES} +) + +add_library(micras::core ALIAS micras_core) + +target_include_directories(micras_core PUBLIC + include +) + +micras_lib_setup_library(micras_core) diff --git a/micras_core/include/micras/core/butterworth_filter.hpp b/micras_core/include/micras/core/butterworth_filter.hpp new file mode 100644 index 0000000..2df5dbd --- /dev/null +++ b/micras_core/include/micras/core/butterworth_filter.hpp @@ -0,0 +1,146 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_BUTTERWORTH_FILTER_HPP +#define MICRAS_CORE_BUTTERWORTH_FILTER_HPP + +#include +#include +#include + +namespace micras::core { +/** + * @brief Implementation of Butterworth second order low-pass filter. + * + * @details + * A generic digital filter follows the relation: + * a0 * y[k] = sum(bi * x[k - i]) - sum(aj * y[k - j]) + * Where: + * x[k] - measurement at instant k + * y[k] - filtered signal at instant k + * The Butterworth filter have the special property of being a + * maximally flat magnitude filter, in other words, is the best + * filter that doesn't present distortions around the cutoff + * frequency. + * + * @see + * The formula for the continuos coefficients of the Butterworth + * filter is available here: + * https://en.wikipedia.org/wiki/Butterworth_filter + * The discrete version were computed with the Tustin method: + * https://en.wikipedia.org/wiki/Bilinear_transform + */ +class ButterworthFilter { +public: + /** + * @brief Order of the filter. + */ + static constexpr uint8_t filter_order{2}; + + /** + * @brief Configuration struct for the filter. + * + * @note Both frequencies are in hertz, and the cutoff is the -3 dB point of the resulting + * discrete filter, exactly, at any ratio up to Nyquist. + * + * @note The sampling frequency has no default on purpose: the discrete coefficients depend on + * the ratio of the cutoff to the sampling frequency, so a filter that assumes the wrong rate is + * silently a filter with the wrong cutoff. + */ + struct Config { + float cutoff_frequency; + float sampling_frequency; + }; + + /** + * @brief Coefficients of the discrete transfer function. + * + * @note Written in the natural order, most recent sample first, for the relation + * y[k] = sum(feed_forward[i] * x[k - i]) - sum(feedback[j] * y[k - 1 - j]). + */ + struct Coefficients { + std::array feed_forward; + std::array feedback; + }; + + /** + * @brief Get the delay the filter adds to a signal that changes slowly compared with the cutoff. + * + * @note This is the group delay at low frequency, `sqrt(2) / (2 * pi * cutoff)`. Whoever turns a + * filtered reading into a position has to account for the distance traveled during it. + * + * @param cutoff_frequency Cutoff frequency in Hz. + * @return The delay in seconds. + */ + static constexpr float get_delay(float cutoff_frequency) { + return std::numbers::sqrt2_v / (2.0F * std::numbers::pi_v * cutoff_frequency); + } + + /** + * @brief Compute the discrete coefficients of the filter. + * + * @param config Cutoff and sampling frequencies in Hz. + * @return Coefficients of the discrete transfer function. + */ + static Coefficients compute_coefficients(const Config& config); + + /** + * @brief Construct a new Butterworth Second Order filter object. + * + * @param config Cutoff and sampling frequencies in Hz. + */ + explicit ButterworthFilter(const Config& config); + + /** + * @brief Produce a new value from measured data. + * + * @details The recursion is written for the change of the output instead of the output, + * `y[k] = y[k-1] + r[k]` with `r[k] = a2 * r[k-1] + b0 * (x[k] + 2 * x[k-1] + x[k-2] - 4 * y[k-1])`, + * which is the same transfer function. The direct form computes the output as the small + * difference of two large terms, and gets its unity gain from three rounded coefficients + * adding up exactly, which single precision stops doing when the cutoff is a thousand times + * below the sampling rate. In this form the gain is one by construction. + * + * @param x0 Last measure. + * @return Filtered value. + */ + float update(float x0); + + /** + * @brief Get the last filtered value. + * + * @return Last filtered value. + */ + float get_last() const; + +private: + /** + * @brief Last two input values of the filter, most recent first. + */ + std::array inputs{}; + + /** + * @brief Last output value of the filter. + */ + float output{}; + + /** + * @brief Change of the output in the last update. + */ + float rate{}; + + /** + * @brief Share of the rate that is kept from one update to the next, which is the feedback + * coefficient of the oldest output. + */ + float damping; + + /** + * @brief Gain from the inputs to the rate, which is the first feed forward coefficient. + */ + float gain; +}; +} // namespace micras::core + +#endif // MICRAS_CORE_BUTTERWORTH_FILTER_HPP diff --git a/micras_core/include/micras/core/byte_stream.hpp b/micras_core/include/micras/core/byte_stream.hpp new file mode 100644 index 0000000..035ffc7 --- /dev/null +++ b/micras_core/include/micras/core/byte_stream.hpp @@ -0,0 +1,60 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_BYTE_STREAM_HPP +#define MICRAS_CORE_BYTE_STREAM_HPP + +#include +#include +#include + +namespace micras::core { +/** + * @brief Interface for a bidirectional stream of bytes. + * + * @note The session layer only ever sees this, so the same protocol runs over the radio, over a + * socket in the simulators and over a pipe in a host test. An interface is used instead of a + * callable because it allocates nothing and costs one virtual call per batch rather than per byte. + */ +class IByteStream { +public: + /** + * @brief Virtual destructor for the IByteStream class. + */ + virtual ~IByteStream() = default; + + /** + * @brief Take whatever has already arrived. + * + * @param into Buffer to read into. + * @return Number of bytes read, which is zero when nothing has arrived. + */ + virtual std::size_t read(std::span into) = 0; + + /** + * @brief Queue data to be sent. + * + * @note All of the data is taken or none of it is, so a frame is never cut in half and asking + * beforehand whether it would fit would only be the same question twice. + * + * @param from Data to send. + * @return Number of bytes accepted, which is either all of them or none. + */ + virtual std::size_t write(std::span from) = 0; + +protected: + /** + * @brief Special member functions declared as default. + */ + ///@{ + IByteStream() = default; + IByteStream(const IByteStream&) = default; + IByteStream(IByteStream&&) = default; + IByteStream& operator=(const IByteStream&) = default; + IByteStream& operator=(IByteStream&&) = default; + ///@} +}; +} // namespace micras::core + +#endif // MICRAS_CORE_BYTE_STREAM_HPP diff --git a/micras_core/include/micras/core/cobs.hpp b/micras_core/include/micras/core/cobs.hpp new file mode 100644 index 0000000..f5ea32b --- /dev/null +++ b/micras_core/include/micras/core/cobs.hpp @@ -0,0 +1,52 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_COBS_HPP +#define MICRAS_CORE_COBS_HPP + +#include +#include +#include + +namespace micras::core { +/** + * @brief Byte reserved to delimit frames, which encoding removes from the data. + */ +constexpr uint8_t cobs_delimiter{0x00}; + +/** + * @brief Compute the largest size the encoding of a buffer can have. + * + * @param size Number of bytes of the data. + * @return Number of bytes the encoded data can take at most. + */ +constexpr std::size_t cobs_encoded_size(std::size_t size) { + return size + size / 254 + 1; +} + +/** + * @brief Encode a buffer so that it contains no delimiter byte. + * + * @note Consistent Overhead Byte Stuffing costs one byte every 254, against up to one byte per byte + * for escaping, and leaves the delimiter impossible to produce by construction. Resynchronizing + * after the module silently drops a run of bytes is then just a matter of scanning to the next + * delimiter. + * + * @param from Data to encode. + * @param into Buffer to encode into, which should hold cobs_encoded_size bytes. + * @return Number of bytes written, or zero if the buffer is too small. + */ +std::size_t cobs_encode(std::span from, std::span into); + +/** + * @brief Decode a frame that was encoded by cobs_encode. + * + * @param from Encoded frame, without any delimiter. + * @param into Buffer to decode into. + * @return Number of bytes written, or zero if the frame is malformed or the buffer is too small. + */ +std::size_t cobs_decode(std::span from, std::span into); +} // namespace micras::core + +#endif // MICRAS_CORE_COBS_HPP diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp new file mode 100644 index 0000000..cf68ea1 --- /dev/null +++ b/micras_core/include/micras/core/fsm.hpp @@ -0,0 +1,131 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_FSM_HPP +#define MICRAS_CORE_FSM_HPP + +#include +#include + +namespace micras::core { +/** + * @brief A single state of a finite state machine. + */ +class FsmState { +public: + /** + * @brief Destroy the FsmState object. + */ + virtual ~FsmState() = default; + + /** + * @brief Execute the entry function of this state. + */ + virtual void on_entry() = 0; + + /** + * @brief Execute this state. + * + * @return The id of the next state. + */ + virtual uint8_t execute() = 0; + + /** + * @brief Get the id object of the state. + * + * @return The id of the state. + */ + uint8_t get_id() const { return this->id; } + + /** + * @brief The id of the state that is not valid. + */ + static constexpr uint8_t invalid_id{0xFF}; + +protected: + /** + * @brief Special member functions declared as default. + */ + ///@{ + explicit FsmState(uint8_t id) : id{id} { } + + FsmState(const FsmState&) = default; + FsmState(FsmState&&) = default; + FsmState& operator=(const FsmState&) = default; + FsmState& operator=(FsmState&&) = default; + ///@} + +private: + /** + * @brief Fixed id of the state. + */ + uint8_t id; +}; + +/** + * @brief Finite state machine over a dense set of state ids. + * + * @note The states are stored in an array indexed by their own id, so running the machine costs an + * array access rather than a hash lookup. That only works because the ids are a dense enumeration + * from zero. + * + * @tparam num_of_states Number of states, and therefore one past the largest valid id. + */ +template +class TFsm { +public: + /** + * @brief Construct a new TFsm object. + * + * @param initial_state_id The id of the initial state. + */ + explicit TFsm(uint8_t initial_state_id); + + /** + * @brief Add a state to the FSM. + * + * @note The state is borrowed, not owned: whoever owns the machine holds its states by value, + * next to it, so that nothing is allocated and they live exactly as long as the machine does. + * + * @param state The state to be added. + */ + void add_state(FsmState& state); + + /** + * @brief Run the FSM current state to compute the next state. + * + * @note Aborts when the current state id has no state behind it, which can only be a + * programming error, and running the abort handler beats dispatching through a null pointer + * with the motors turning. + */ + void update(); + + /** + * @brief Get the id of the state currently running. + * + * @return The id of the current state. + */ + uint8_t get_current_state_id() const; + +private: + /** + * @brief States of the machine, indexed by their id. + */ + std::array states{}; + + /** + * @brief Id of the state currently running. + */ + uint8_t current_state_id{0}; + + /** + * @brief Id of the last executed state. + */ + uint8_t previous_state_id{FsmState::invalid_id}; +}; +} // namespace micras::core + +#include "micras/core/impl/fsm.tpp" // IWYU pragma: export + +#endif // MICRAS_CORE_FSM_HPP diff --git a/micras_core/include/micras/core/impl/fsm.tpp b/micras_core/include/micras/core/impl/fsm.tpp new file mode 100644 index 0000000..b42b615 --- /dev/null +++ b/micras_core/include/micras/core/impl/fsm.tpp @@ -0,0 +1,44 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_FSM_TPP +#define MICRAS_CORE_FSM_TPP + +#include +#include + +namespace micras::core { +template +TFsm::TFsm(uint8_t initial_state_id) : current_state_id{initial_state_id} { } + +template +void TFsm::add_state(FsmState& state) { + this->states.at(state.get_id()) = &state; +} + +template +uint8_t TFsm::get_current_state_id() const { + return this->current_state_id; +} + +template +void TFsm::update() { + if (this->current_state_id >= num_of_states or this->states.at(this->current_state_id) == nullptr) { + std::abort(); + } + + FsmState& state = *this->states.at(this->current_state_id); + + if (this->current_state_id != this->previous_state_id) { + state.on_entry(); + } + + const uint8_t next_state_id = state.execute(); + + this->previous_state_id = this->current_state_id; + this->current_state_id = next_state_id; +} +} // namespace micras::core + +#endif // MICRAS_CORE_FSM_TPP diff --git a/micras_core/include/micras/core/impl/variable_pool.tpp b/micras_core/include/micras/core/impl/variable_pool.tpp new file mode 100644 index 0000000..442ceaa --- /dev/null +++ b/micras_core/include/micras/core/impl/variable_pool.tpp @@ -0,0 +1,57 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_VARIABLE_POOL_TPP +#define MICRAS_CORE_VARIABLE_POOL_TPP + +#include +#include +#include + +namespace micras::core { +template +VariableId VariablePool::add(std::string_view prefix, std::string_view name, T& value, Access access) { + Variable* variable = this->next(); + + if (variable == nullptr) { + return invalid_id; + } + + *variable = { + .prefix = prefix, + .name = name, + .address = static_cast(&value), + .size = sizeof(T), + .type = TypeCodeOf>::value, + .access = access, + }; + + return this->count++; +} + +template +VariableId VariablePool::add(std::string_view prefix, std::string_view name, const T& value, Access access) { + access.write = false; + access.idle = false; + + Variable* variable = this->next(); + + if (variable == nullptr) { + return invalid_id; + } + + *variable = { + .prefix = prefix, + .name = name, + .address = const_cast(&value), // NOLINT(cppcoreguidelines-pro-type-const-cast) + .size = sizeof(T), + .type = TypeCodeOf>::value, + .access = access, + }; + + return this->count++; +} +} // namespace micras::core + +#endif // MICRAS_CORE_VARIABLE_POOL_TPP diff --git a/micras_core/include/micras/core/math.hpp b/micras_core/include/micras/core/math.hpp new file mode 100644 index 0000000..652e406 --- /dev/null +++ b/micras_core/include/micras/core/math.hpp @@ -0,0 +1,201 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_MATH_HPP +#define MICRAS_CORE_MATH_HPP + +#include +#include +#include +#include + +/** + * @brief Elementary functions that are constant expressions on every compiler. + * + * @note The functions of the standard library only become constant expressions in C++26, and only + * GCC accepts them earlier as an extension. Everything computed when the firmware is compiled, such + * as the shape of each turn, goes through these instead, so that the same sources build with clang + * and with the compilers of the simulators. They are also usable at run time, where the series are + * short enough to be cheaper than a table. + */ +namespace micras::core::math { +/** + * @brief Absolute value. + * + * @tparam T Floating point type. + * @param value Value. + * @return The absolute value. + */ +template +constexpr T abs(T value) { + return value < T{0} ? -value : value; +} + +/** + * @brief Square root by Newton iterations. + * + * @details Newton's method only converges quickly once the estimate is close to the root, so the + * value is first brought to [1/4, 4) by powers of four, whose square roots are exact powers of two + * that scale the result back. From an estimate above the root, which the mean of the value and one + * always is, every iteration lowers the estimate until rounding stops it, which is where the + * iterations end: within an ulp of the root, in about six iterations for a double. + * + * @tparam T Floating point type. + * @param value Non negative value. + * @return The square root of the value, zero for a non positive value, and the value itself for an + * infinity or a NaN. + */ +template +constexpr T sqrt(T value) { + if (value <= T{0}) { + return T{0}; + } + + if (not(value <= std::numeric_limits::max())) { + return value; + } + + T scale{1}; + + while (value >= T{0x1p64}) { + value *= T{0x1p-64}; + scale *= T{0x1p32}; + } + + while (value < T{0x1p-64}) { + value *= T{0x1p64}; + scale *= T{0x1p-32}; + } + + while (value >= T{4}) { + value /= T{4}; + scale *= T{2}; + } + + while (value < T{0.25}) { + value *= T{4}; + scale /= T{2}; + } + + T estimate = (value + T{1}) / T{2}; + + for (uint8_t i = 0; i < 16; i++) { + const T next = (estimate + value / estimate) / T{2}; + + if (next >= estimate) { + break; + } + + estimate = next; + } + + return estimate * scale; +} + +/** + * @brief Wrap an angle to the range [-pi, pi]. + * + * @tparam T Floating point type. + * @param angle Angle in radians. + * @return The equivalent angle in [-pi, pi]. + */ +template +constexpr T wrap_angle(T angle) { + constexpr T two_pi = T{2} * std::numbers::pi_v; + + const auto turns = static_cast(angle / two_pi + (angle < T{0} ? T{-0.5} : T{0.5})); + + return angle - static_cast(turns) * two_pi; +} + +/** + * @brief Sine by its Taylor series, after folding the angle into [-pi/2, pi/2]. + * + * @tparam T Floating point type. + * @param angle Angle in radians. + * @return The sine of the angle. + */ +template +constexpr T sin(T angle) { + angle = wrap_angle(angle); + + if (angle > std::numbers::pi_v / T{2}) { + angle = std::numbers::pi_v - angle; + } else if (angle < -std::numbers::pi_v / T{2}) { + angle = -std::numbers::pi_v - angle; + } + + const T squared = angle * angle; + T term = angle; + T sum = angle; + + for (uint8_t order = 1; order < 12; order++) { + term *= -squared / static_cast((2 * order) * (2 * order + 1)); + sum += term; + } + + return sum; +} + +/** + * @brief Cosine, as the sine of the complementary angle. + * + * @tparam T Floating point type. + * @param angle Angle in radians. + * @return The cosine of the angle. + */ +template +constexpr T cos(T angle) { + return sin(angle + std::numbers::pi_v / T{2}); +} + +/** + * @brief Point reached by a clothoid that starts straight along the x axis. + * + * @tparam T Floating point type. + */ +template +struct ClothoidPoint { + T x; + T y; +}; + +/** + * @brief Evaluate a clothoid, the curve whose curvature grows linearly with the distance traveled. + * + * @details The path of a robot that moves at a constant speed while its angular speed ramps up is a + * clothoid. With a heading of `sharpness * s^2 / 2` after a distance `s`, its coordinates are the + * Fresnel integrals `x = int cos(sharpness * t^2 / 2) dt` and `y = int sin(sharpness * t^2 / 2) dt`. + * They are evaluated here by their power series in the heading, which converges for every argument + * and needs about ten terms for the quarter turn that is the most a ramp of a slalom turn covers. + * + * @tparam T Floating point type. + * @param distance Distance traveled along the curve, in meters. + * @param sharpness Rate of change of the curvature with the distance, in 1/m^2. + * @param terms Number of terms of the series. + * @return The coordinates of the point, in the frame of the start of the curve. + */ +template +constexpr ClothoidPoint clothoid(T distance, T sharpness, uint8_t terms = 14) { + const T heading = sharpness * distance * distance / T{2}; + const T squared = heading * heading; + + T even_term = T{1}; + T odd_term = heading; + T x_sum = T{0}; + T y_sum = T{0}; + + for (uint8_t order = 0; order < terms; order++) { + x_sum += even_term / static_cast(4 * order + 1); + y_sum += odd_term / static_cast(4 * order + 3); + + even_term *= -squared / static_cast((2 * order + 1) * (2 * order + 2)); + odd_term *= -squared / static_cast((2 * order + 2) * (2 * order + 3)); + } + + return {.x = distance * x_sum, .y = distance * y_sum}; +} +} // namespace micras::core::math + +#endif // MICRAS_CORE_MATH_HPP diff --git a/micras_core/include/micras/core/pid_controller.hpp b/micras_core/include/micras/core/pid_controller.hpp new file mode 100644 index 0000000..dde7a4b --- /dev/null +++ b/micras_core/include/micras/core/pid_controller.hpp @@ -0,0 +1,126 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_PID_CONTROLLER_HPP +#define MICRAS_CORE_PID_CONTROLLER_HPP + +namespace micras::core { +/** + * @brief Implementation of simple PID controller. + * + * @details The response is `kp * (error + ki * integral(error) - kd * d/dt(state))`. The derivative + * acts on the controlled variable and not on the error, so a step of the setpoint does not kick it. + */ +class PidController { +public: + /** + * @brief Configuration struct for the PID controller. + * + * @note A negative saturation or maximum integral disables the respective limit. The monitored + * flag makes the controller publish its terms to the file scope variables of its source file, + * where a variable monitor can read them. There is one set of those for every instance, so only + * one controller at a time should have it set. + */ + struct Config { + float kp{}; + float ki{}; + float kd{}; + float setpoint{}; + float saturation{-1.0F}; + float max_integral{-1.0F}; + bool monitored{false}; + }; + + /** + * @brief Construct a new Pid Controller object. + * + * @param config Controller parameters. + */ + explicit PidController(Config config); + + /** + * @brief Set the desired setpoint. + * + * @param setpoint Desired state. + */ + void set_setpoint(float setpoint); + + /** + * @brief Reset prev_error and error_acc objects. + */ + void reset(); + + /** + * @brief Update PID with new state and return response. + * + * @param state Current value of the controlled variable. + * @param elapsed_time Time since the last update. + * @return Response of the controller. + */ + float compute_response(float state, float elapsed_time); + + /** + * @brief Update PID with new state and return response. + * + * @param state Current value of the controlled variable. + * @param elapsed_time Time since the last update. + * @param state_change Derivative of the controlled variable. + * @return Response of the controller. + */ + float compute_response(float state, float elapsed_time, float state_change); + +private: + /** + * @brief Proportional constant. + */ + float kp; + + /** + * @brief Integrative constant. + */ + float ki; + + /** + * @brief Derivative constant. + */ + float kd; + + /** + * @brief Desired state. + */ + float setpoint; + + /** + * @brief Maximum response returned by the controller. + */ + float saturation; + + /** + * @brief Maximum integrative response. + */ + float max_integral; + + /** + * @brief Whether the terms of the controller are published for a variable monitor. + */ + bool monitored; + + /** + * @brief Flag indicating whether this is the first run of the controller. + */ + bool first_run = true; + + /** + * @brief Accumulated error for integrative term. + */ + float error_acc = 0; + + /** + * @brief Previous state for derivative term. + */ + float prev_state = 0; +}; +} // namespace micras::core + +#endif // MICRAS_CORE_PID_CONTROLLER_HPP diff --git a/micras_core/include/micras/core/serializable.hpp b/micras_core/include/micras/core/serializable.hpp new file mode 100644 index 0000000..e49c612 --- /dev/null +++ b/micras_core/include/micras/core/serializable.hpp @@ -0,0 +1,51 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_SERIALIZABLE_HPP +#define MICRAS_CORE_SERIALIZABLE_HPP + +#include +#include + +namespace micras::core { +/** + * @brief Interface class for serializable classes. + */ +class ISerializable { +public: + /** + * @brief Virtual destructor for the ISerializable class. + */ + virtual ~ISerializable() = default; + + /** + * @brief Serialize the class instance. + * + * @return Serialized data. + */ + virtual std::vector serialize() const = 0; + + /** + * @brief Deserialize the class instance. + * + * @param serial_data Serialized data. + * @param size Size of the serialized data. + */ + virtual void deserialize(const uint8_t* serial_data, uint16_t size) = 0; + +protected: + /** + * @brief Special member functions declared as default. + */ + ///@{ + ISerializable() = default; + ISerializable(const ISerializable&) = default; + ISerializable(ISerializable&&) = default; + ISerializable& operator=(const ISerializable&) = default; + ISerializable& operator=(ISerializable&&) = default; + ///@} +}; +} // namespace micras::core + +#endif // MICRAS_CORE_SERIALIZABLE_HPP diff --git a/micras_core/include/micras/core/types.hpp b/micras_core/include/micras/core/types.hpp new file mode 100644 index 0000000..3d5ef17 --- /dev/null +++ b/micras_core/include/micras/core/types.hpp @@ -0,0 +1,21 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_TYPES_HPP +#define MICRAS_CORE_TYPES_HPP + +#include + +namespace micras::core { +/** + * @brief Possible objectives of the robot. + */ +enum class Objective : uint8_t { + EXPLORE = 0, + RETURN = 1, + SOLVE = 2 +}; +} // namespace micras::core + +#endif // MICRAS_CORE_TYPES_HPP diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp new file mode 100644 index 0000000..60c3f60 --- /dev/null +++ b/micras_core/include/micras/core/utils.hpp @@ -0,0 +1,140 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_UTILS_HPP +#define MICRAS_CORE_UTILS_HPP + +#include +#include +#include +#include + +namespace micras::core { +/** + * @brief Remap a value from one range to another. + * + * @tparam T Type of the value. + * @param value Value to be remapped. + * @param in_min Minimum value of the input range. + * @param in_max Maximum value of the input range. + * @param out_min Minimum value of the output range. + * @param out_max Maximum value of the output range. + * @return Remapped value. + */ +template +constexpr T remap(T value, T in_min, T in_max, T out_min, T out_max) { + return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +/** + * @brief Move a value towards a target limiting by the step size. + * + * @tparam T Type of the value. + * @param value Current value. + * @param target Target value. + * @param step Step size. + * @return New value. + */ +template +constexpr T move_towards(T value, T target, T step) { + return std::clamp(target, value - step, value + step); +} + +/** + * @brief Vary a value from start to end smoothly. + * + * @tparam T Type of the value. + * @param value Current value. + * @param start Start value. + * @param end End value. + * @param resistance Value resistance to variation. + * @return New value. + */ +template +constexpr T transition(T value, T start, T end, T resistance) { + return end - (end - start) * resistance / (resistance + value * value); +} + +/** + * @brief Create an array of objects calling their constructors from an array of parameters. + * + * @tparam T Type of the array. + * @tparam N Size of the array. + * @tparam C Type of the parameters. + * @param parameters List of parameters. + * @return Array with the objects created from the parameters. + */ +template +constexpr std::array make_array(const std::array& parameters) { + return [&](std::index_sequence) -> std::array { + return {T{std::get(parameters)}...}; + }(std::make_index_sequence()); +} + +/** + * @brief Create an array of objects calling their constructors from a single parameter. + * + * @tparam T Type of the array. + * @tparam N Size of the array. + * @tparam C Type of the parameter. + * @param value Parameter. + * @return Array with the objects created from the parameter. + */ +template +constexpr std::array make_array(C value) { + return [&](std::index_sequence) -> std::array { + return {(static_cast(I), T{value})...}; + }(std::make_index_sequence()); +} + +/** + * @brief Assert an angle to be in the range [-pi, pi]. + * + * @param angle Angle to be asserted. + * @return Asserted angle. + */ +constexpr float assert_angle(float angle) { + angle = std::fmod(angle, 2 * std::numbers::pi_v); + + if (angle > std::numbers::pi_v) { + angle -= 2 * std::numbers::pi_v; + } else if (angle < -std::numbers::pi_v) { + angle += 2 * std::numbers::pi_v; + } + + return angle; +} + +/** + * @brief Assert an angle to be in the range [-pi/2, pi/2]. + * + * @param angle Angle to be asserted. + * @return Asserted angle. + */ +constexpr float assert_half_angle(float angle) { + angle = std::fmod(angle, std::numbers::pi_v); + + if (angle > std::numbers::pi_v / 2.0F) { + angle -= std::numbers::pi_v; + } else if (angle < -std::numbers::pi_v / 2.0F) { + angle += std::numbers::pi_v; + } + + return angle; +} + +/** + * @brief Check if two floating point numbers are near each other. + * + * @param x First number. + * @param y Second number. + * @param tolerance Tolerance. + * @return True if the numbers are near each other, false otherwise. + */ +constexpr bool is_near(float x, float y, float tolerance = 0.001F) { + return std::abs(x - y) <= tolerance; +} +} // namespace micras::core + +#endif // MICRAS_CORE_UTILS_HPP diff --git a/micras_core/include/micras/core/variable_pool.hpp b/micras_core/include/micras/core/variable_pool.hpp new file mode 100644 index 0000000..3513c78 --- /dev/null +++ b/micras_core/include/micras/core/variable_pool.hpp @@ -0,0 +1,380 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_VARIABLE_POOL_HPP +#define MICRAS_CORE_VARIABLE_POOL_HPP + +#include +#include +#include +#include +#include +#include + +#include "micras/core/serializable.hpp" + +namespace micras::core { +/** + * @brief Type of a registered variable, as it is written to the flash and to the wire. + */ +enum class TypeCode : uint8_t { + BOOL = 0, + U8 = 1, + I8 = 2, + U16 = 3, + I16 = 4, + U32 = 5, + I32 = 6, + U64 = 7, + I64 = 8, + F32 = 9, + F64 = 10, + BLOB = 11 +}; + +/** + * @brief Map from a C++ type to the type code that describes it. + * + * @note Left undefined for every type without a specialization, so that registering an unsupported + * type fails to compile instead of being written out as raw bytes nothing can read back. + * + * @tparam T Type to map. + */ +template +struct TypeCodeOf { }; + +/** + * @brief Specializations of the type code map. + */ +///@{ +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::BOOL}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::U8}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::I8}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::U16}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::I16}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::U32}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::I32}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::U64}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::I64}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::F32}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::F64}; +}; + +template +requires std::is_enum_v +struct TypeCodeOf : TypeCodeOf> { }; + +///@} + +/** + * @brief Types that can be registered by value, as opposed to through ISerializable. + * + * @tparam T Type to check. + */ +template +concept Registrable = std::is_trivially_copyable_v and requires { TypeCodeOf>::value; }; + +/** + * @brief What each consumer of the pool is allowed to do with a variable. + */ +struct Access { + /** + * @brief Whether the variable can be part of a stream. + */ + bool stream : 1 {}; + + /** + * @brief Whether the variable can be written from outside the robot. + */ + bool write : 1 {}; + + /** + * @brief Whether writes are restricted to a stopped robot, ignored when write is not set. + */ + bool idle : 1 {}; + + /** + * @brief Whether the variable is part of the flash image. + */ + bool persist : 1 {}; + + /** + * @brief Get the flags as one byte, the first one in the lowest bit. + * + * @note The byte of the struct itself also holds four bits no field owns, whose value is not + * defined, so it is never copied as it is. + * + * @return The flags. + */ + constexpr uint8_t to_byte() const { + return static_cast( + static_cast(this->stream) | static_cast(this->write) << 1U | + static_cast(this->idle) << 2U | static_cast(this->persist) << 3U + ); + } +}; + +/** + * @brief A variable registered in the pool. + * + * @note The name is split in two views so that an owner can be given a prefix at registration + * without concatenating anything at run time. Both are expected to be string literals, and must + * outlive the pool. + */ +struct Variable { + std::string_view prefix; + std::string_view name; + + /** + * @brief Where the value lives, or the ISerializable that owns it when the type is BLOB. + */ + void* address{}; + + /** + * @brief Number of bytes of the value, zero for a BLOB, whose size is only known once it is + * serialized. + */ + uint16_t size{}; + + TypeCode type{}; + Access access{}; +}; + +/** + * @brief Index of a variable in the pool, and the identifier used on the wire. + */ +using VariableId = uint16_t; + +/** + * @brief Reflection over the variables the rest of the firmware is willing to expose. + * + * @note This class knows nothing about flash, packets or schedules. It is a flat array of plain + * descriptors, and every consumer walks it and decides for itself what the bytes are for. + * + * @note The pool does not own the storage of the values, only their addresses, so a registered + * object must outlive the pool. + */ +class VariablePool { +public: + /** + * @brief Result of an attempt to write a variable from outside the robot. + */ + enum class WriteStatus : uint8_t { + OK = 0, + NO_SUCH_ID = 1, + READ_ONLY = 2, + NEEDS_IDLE = 3, + WRONG_SIZE = 4 + }; + + /** + * @brief Identifier returned when a variable could not be registered. + */ + static constexpr VariableId invalid_id{0xFFFF}; + + /** + * @brief Construct a new VariablePool object over storage owned by the caller. + * + * @param storage Array the descriptors are written into, which bounds the number of variables. + */ + explicit VariablePool(std::span storage); + + /** + * @brief Register a writable variable. + * + * @tparam T Type of the variable. + * @param prefix Prefix of the name, usually identifying the owner. + * @param name Name of the variable inside its owner. + * @param value Reference to the variable, which must outlive the pool. + * @param access What the consumers of the pool may do with the variable. + * @return Identifier of the variable, or invalid_id if the pool is full. + */ + template + VariableId add(std::string_view prefix, std::string_view name, T& value, Access access); + + /** + * @brief Register a read only variable. + * + * @note The write flag is cleared whatever the caller asked for, so that constness is enough to + * state the intent at the registration site. The pool holds one mutable address for every kind + * of variable, so that no consumer has to branch on constness, and casting the constness away is + * safe because the flag through which anything can write is cleared. + * + * @tparam T Type of the variable. + * @param prefix Prefix of the name, usually identifying the owner. + * @param name Name of the variable inside its owner. + * @param value Reference to the variable, which must outlive the pool. + * @param access What the consumers of the pool may do with the variable. + * @return Identifier of the variable, or invalid_id if the pool is full. + */ + template + VariableId add(std::string_view prefix, std::string_view name, const T& value, Access access); + + /** + * @brief Register an object with its own encoding. + * + * @note Serializing costs a heap allocation, so a blob is meant for composite state that is + * saved and loaded, never for something sampled at loop rate. + * + * @param prefix Prefix of the name, usually identifying the owner. + * @param name Name of the variable inside its owner. + * @param object Reference to the object, which must outlive the pool. + * @param access What the consumers of the pool may do with the variable. + * @return Identifier of the variable, or invalid_id if the pool is full. + */ + VariableId add(std::string_view prefix, std::string_view name, ISerializable& object, Access access); + + /** + * @brief Find a variable by its full name. + * + * @param full_name Prefix and name of the variable, concatenated. + * @return Identifier of the variable, or no value if it is not registered. + */ + std::optional find(std::string_view full_name) const; + + /** + * @brief Get a registered variable. + * + * @param id Identifier of the variable, which must be valid. + * @return The descriptor of the variable. + */ + const Variable& at(VariableId id) const; + + /** + * @brief Get every registered variable, in registration order. + * + * @return View over the descriptors. + */ + std::span all() const; + + /** + * @brief Copy the current value of a variable. + * + * @note Only defined for variables that are not blobs, which are read through their own + * interface instead. + * + * @param id Identifier of the variable, which must be valid. + * @param into Buffer to copy into, which must hold at least the size of the variable. + * @return Number of bytes copied, zero if the variable is a blob or the buffer is too small. + */ + uint16_t read(VariableId id, std::span into) const; + + /** + * @brief Write a variable from outside the robot. + * + * @param id Identifier of the variable. + * @param bytes New value of the variable. + * @param robot_is_idle Whether the robot is currently stopped. + * @return Whether the write was performed, and why not otherwise. + */ + WriteStatus write(VariableId id, std::span bytes, bool robot_is_idle); + + /** + * @brief Compute a hash over the layout of the pool. + * + * @note Identifiers are registration order, so adding one variable shifts every later one. An + * application holding a cached schema would then plot the wrong signal with nothing to warn it. + * Comparing this hash against the one the schema was fetched with is that warning. + * + * @note Only has to change whenever the schema does, so it is an FNV-1a hash rather than + * anything with error detection properties. Nothing is being corrected here, and the schema it + * stands for arrives over a link that checks its own frames. + * + * @return Hash over the name, type and access flags of every variable, in order. + */ + uint32_t schema_hash() const; + +private: + /** + * @brief Reserve the next descriptor. + * + * @return Pointer to the descriptor, or nullptr if the pool is full. + */ + Variable* next(); + + /** + * @brief Descriptors of the registered variables, owned by the caller. + */ + std::span storage; + + /** + * @brief Number of variables registered so far. + */ + VariableId count{}; +}; + +namespace detail { +/** + * @brief Storage for a variable pool, held in a base class so that it is already constructed when + * the pool base is given a view over it. + * + * @tparam N Maximum number of variables. + */ +template +struct VariableStorage { + std::array variables{}; +}; +} // namespace detail + +/** + * @brief A variable pool owning storage for a fixed number of variables. + * + * @tparam N Maximum number of variables. + */ +template +// NOLINTNEXTLINE(misc-multiple-inheritance) the first base only exists to hold the storage +class TVariablePool : private detail::VariableStorage, public VariablePool { +public: + /** + * @brief Construct a new TVariablePool object. + */ + TVariablePool() : VariablePool{this->variables} { } +}; +} // namespace micras::core + +#include "micras/core/impl/variable_pool.tpp" // IWYU pragma: export + +#endif // MICRAS_CORE_VARIABLE_POOL_HPP diff --git a/micras_core/include/micras/core/vector.hpp b/micras_core/include/micras/core/vector.hpp new file mode 100644 index 0000000..3f8d706 --- /dev/null +++ b/micras_core/include/micras/core/vector.hpp @@ -0,0 +1,89 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_VECTOR_HPP +#define MICRAS_CORE_VECTOR_HPP + +namespace micras::core { +/** + * @brief Type to store a point in 2D space. + */ +struct Vector { + /** + * @brief Calculate the distance to another point. + * + * @param other The other point. + * @return The distance to the other point. + */ + float distance(const Vector& other) const; + + /** + * @brief Calculate the distance of the point to the origin. + * + * @return The distance of the point to the origin. + */ + float magnitude() const; + + /** + * @brief Calculate the angle between two points. + * + * @param other The other point. + * @return The angle between the two points. + */ + float angle_between(const Vector& other) const; + + /** + * @brief Move the point towards another point by a given distance. + * + * @param other The point to move towards. + * @param distance The distance to move. + * @return The point after moving towards the other point. + */ + Vector move_towards(const Vector& other, float distance) const; + + /** + * @brief Add a point to another. + * + * @param other The point to add. + * @return The result of the addition. + */ + Vector operator+(const Vector& other) const; + + /** + * @brief Subtract a point from another. + * + * @param other The point to subtract. + * @return The result of the subtraction. + */ + Vector operator-(const Vector& other) const; + + /** + * @brief Compare two points for equality. + * + * @param other The other point to compare. + * @return True if the points are equal, false otherwise. + */ + bool operator==(const Vector& other) const; + + /** + * @brief Calculate the remainder of the division of the point by a value. + * + * @param value The value to divide the point by. + * @return The remainder of the division. + */ + Vector operator%(float value) const; + + /** + * @brief The x coordinate of the point in 2D space. + */ + float x; + + /** + * @brief The y coordinate of the point in 2D space. + */ + float y; +}; +} // namespace micras::core + +#endif // MICRAS_CORE_VECTOR_HPP diff --git a/micras_core/src/butterworth_filter.cpp b/micras_core/src/butterworth_filter.cpp new file mode 100644 index 0000000..ba6b03d --- /dev/null +++ b/micras_core/src/butterworth_filter.cpp @@ -0,0 +1,51 @@ +/** + * @file + */ + +#include +#include + +#include "micras/core/butterworth_filter.hpp" + +namespace micras::core { +ButterworthFilter::Coefficients ButterworthFilter::compute_coefficients(const Config& config) { + const float warp = 1.0F / std::tan(std::numbers::pi_v * config.cutoff_frequency / config.sampling_frequency); + const float warp_2 = warp * warp; + + const float b0 = 1; + const float b1 = 2; + const float b2 = 1; + + const float a0 = warp_2 + std::numbers::sqrt2_v * warp + 1; + const float a1 = 2 - 2 * warp_2; + const float a2 = warp_2 - std::numbers::sqrt2_v * warp + 1; + + return { + .feed_forward = {b0 / a0, b1 / a0, b2 / a0}, + .feedback = {a1 / a0, a2 / a0}, + }; +} + +ButterworthFilter::ButterworthFilter(const Config& config) { + const Coefficients coefficients = compute_coefficients(config); + + this->damping = std::get<1>(coefficients.feedback); + this->gain = std::get<0>(coefficients.feed_forward); +} + +float ButterworthFilter::update(float x0) { + const float input_sum = x0 + 2.0F * std::get<0>(this->inputs) + std::get<1>(this->inputs); + + this->rate = this->damping * this->rate + this->gain * (input_sum - 4.0F * this->output); + this->output += this->rate; + + std::get<1>(this->inputs) = std::get<0>(this->inputs); + std::get<0>(this->inputs) = x0; + + return this->output; +} + +float ButterworthFilter::get_last() const { + return this->output; +} +} // namespace micras::core diff --git a/micras_core/src/cobs.cpp b/micras_core/src/cobs.cpp new file mode 100644 index 0000000..30d2af3 --- /dev/null +++ b/micras_core/src/cobs.cpp @@ -0,0 +1,80 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/core/cobs.hpp" + +namespace micras::core { +// Both functions index buffers the caller sized, and every access is bounded by a check just +// above it, which is the whole content of the algorithm. +// NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) +std::size_t cobs_encode(std::span from, std::span into) { + if (into.size() < cobs_encoded_size(from.size())) { + return 0; + } + + std::size_t code_index = 0; + std::size_t write_index = 1; + uint8_t code = 1; + + for (const uint8_t byte : from) { + if (byte != cobs_delimiter) { + into[write_index++] = byte; + + if (++code != 0xFF) { + continue; + } + } + + into[code_index] = code; + code_index = write_index++; + code = 1; + } + + if (not from.empty() and from.back() != cobs_delimiter and code == 1) { + return write_index - 1; + } + + into[code_index] = code; + return write_index; +} + +std::size_t cobs_decode(std::span from, std::span into) { + std::size_t read_index = 0; + std::size_t write_index = 0; + + while (read_index < from.size()) { + const uint8_t code = from[read_index]; + + if (code == cobs_delimiter or read_index + code > from.size()) { + return 0; + } + + read_index++; + + for (uint8_t i = 1; i < code; i++) { + if (write_index >= into.size()) { + return 0; + } + + into[write_index++] = from[read_index++]; + } + + if (code != 0xFF and read_index < from.size()) { + if (write_index >= into.size()) { + return 0; + } + + into[write_index++] = cobs_delimiter; + } + } + + return write_index; +} + +// NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) +} // namespace micras::core diff --git a/micras_core/src/pid_controller.cpp b/micras_core/src/pid_controller.cpp new file mode 100644 index 0000000..dd08569 --- /dev/null +++ b/micras_core/src/pid_controller.cpp @@ -0,0 +1,82 @@ +/** + * @file + */ + +#include +#include + +#include "micras/core/pid_controller.hpp" + +// NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) +static volatile float test_error; +static volatile float test_proportional; +static volatile float test_integrative; +static volatile float test_derivative; +static volatile float test_response; + +// NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) + +namespace micras::core { +PidController::PidController(Config config) : + kp{config.kp}, + ki{config.ki}, + kd{config.kd}, + setpoint{config.setpoint}, + saturation{config.saturation}, + max_integral{config.max_integral}, + monitored{config.monitored} { } + +void PidController::set_setpoint(float setpoint) { + this->setpoint = setpoint; +} + +void PidController::reset() { + this->error_acc = 0; + this->prev_state = 0; + this->first_run = true; +} + +float PidController::compute_response(float state, float elapsed_time) { + if (this->first_run) { + this->prev_state = state; + } + + const float state_change = (state - this->prev_state) / elapsed_time; + return this->compute_response(state, elapsed_time, state_change); +} + +float PidController::compute_response(float state, float elapsed_time, float state_change) { + const float error = this->setpoint - state; + this->prev_state = state; + this->first_run = false; + + float response = this->kp * (error + this->ki * this->error_acc - this->kd * state_change); + + if (this->saturation < 0 or std::abs(response) < this->saturation or + (this->error_acc != 0 and std::signbit(this->error_acc) != std::signbit(error))) { + this->error_acc += error * elapsed_time; + } + + if (this->max_integral >= 0 and this->ki > 0) { + this->error_acc = std::clamp( + this->error_acc, -this->max_integral / (this->kp * this->ki), this->max_integral / (this->kp * this->ki) + ); + } + + response = this->kp * (error + this->ki * this->error_acc - this->kd * state_change); + + if (this->saturation >= 0 and std::abs(response) >= this->saturation) { + response = std::clamp(response, -this->saturation, this->saturation); + } + + if (this->monitored) { + test_error = error; + test_proportional = this->kp * error; + test_integrative = this->kp * this->ki * this->error_acc; + test_derivative = -this->kp * this->kd * state_change; + test_response = response; + } + + return response; +} +} // namespace micras::core diff --git a/micras_core/src/variable_pool.cpp b/micras_core/src/variable_pool.cpp new file mode 100644 index 0000000..beb0a29 --- /dev/null +++ b/micras_core/src/variable_pool.cpp @@ -0,0 +1,138 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include "micras/core/serializable.hpp" +#include "micras/core/variable_pool.hpp" + +namespace micras::core { +static uint32_t mix(uint32_t hash, uint8_t byte) { + return (hash ^ byte) * 16777619U; +} + +static uint32_t mix(uint32_t hash, std::string_view text) { + for (const char character : text) { + hash = mix(hash, static_cast(character)); + } + + return hash; +} + +VariablePool::VariablePool(std::span storage) : storage{storage} { } + +Variable* VariablePool::next() { + if (this->count >= this->storage.size()) { + return nullptr; + } + + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + return &this->storage[this->count]; +} + +VariableId VariablePool::add(std::string_view prefix, std::string_view name, ISerializable& object, Access access) { + Variable* variable = this->next(); + + if (variable == nullptr) { + return invalid_id; + } + + access.stream = false; + + *variable = { + .prefix = prefix, + .name = name, + .address = static_cast(&object), + .size = 0, + .type = TypeCode::BLOB, + .access = access, + }; + + return this->count++; +} + +std::optional VariablePool::find(std::string_view full_name) const { + for (VariableId id = 0; id < this->count; id++) { + const Variable& variable = this->at(id); + + if (full_name.size() == variable.prefix.size() + variable.name.size() and + full_name.starts_with(variable.prefix) and full_name.ends_with(variable.name)) { + return id; + } + } + + return std::nullopt; +} + +const Variable& VariablePool::at(VariableId id) const { + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + return this->storage[id]; +} + +std::span VariablePool::all() const { + return this->storage.first(this->count); +} + +uint16_t VariablePool::read(VariableId id, std::span into) const { + if (id >= this->count) { + return 0; + } + + const Variable& variable = this->at(id); + + if (variable.type == TypeCode::BLOB or into.size() < variable.size) { + return 0; + } + + std::memcpy(into.data(), variable.address, variable.size); + return variable.size; +} + +// NOLINTNEXTLINE(readability-make-member-function-const) it writes through the stored addresses +VariablePool::WriteStatus VariablePool::write(VariableId id, std::span bytes, bool robot_is_idle) { + if (id >= this->count) { + return WriteStatus::NO_SUCH_ID; + } + + const Variable& variable = this->at(id); + + if (not variable.access.write) { + return WriteStatus::READ_ONLY; + } + + if (variable.access.idle and not robot_is_idle) { + return WriteStatus::NEEDS_IDLE; + } + + if (variable.type == TypeCode::BLOB) { + static_cast(variable.address)->deserialize(bytes.data(), bytes.size()); + return WriteStatus::OK; + } + + if (bytes.size() != variable.size) { + return WriteStatus::WRONG_SIZE; + } + + std::memcpy(variable.address, bytes.data(), variable.size); + return WriteStatus::OK; +} + +uint32_t VariablePool::schema_hash() const { + uint32_t hash = 2166136261U; + + for (const Variable& variable : this->all()) { + hash = mix(hash, variable.prefix); + hash = mix(hash, variable.name); + hash = mix(hash, std::to_underlying(variable.type)); + hash = mix(hash, variable.access.to_byte()); + } + + return hash; +} +} // namespace micras::core diff --git a/micras_core/src/vector.cpp b/micras_core/src/vector.cpp new file mode 100644 index 0000000..b1a551b --- /dev/null +++ b/micras_core/src/vector.cpp @@ -0,0 +1,42 @@ +/** + * @file + */ + +#include + +#include "micras/core/vector.hpp" + +namespace micras::core { +float Vector::distance(const Vector& other) const { + return std::hypot(other.x - this->x, other.y - this->y); +} + +float Vector::magnitude() const { + return std::hypot(this->x, this->y); +} + +float Vector::angle_between(const Vector& other) const { + return std::atan2(other.y - this->y, other.x - this->x); +} + +Vector Vector::move_towards(const Vector& other, float distance) const { + const float angle = this->angle_between(other); + return {.x = this->x + distance * std::cos(angle), .y = this->y + distance * std::sin(angle)}; +} + +Vector Vector::operator+(const Vector& other) const { + return {.x = this->x + other.x, .y = this->y + other.y}; +} + +Vector Vector::operator-(const Vector& other) const { + return {.x = this->x - other.x, .y = this->y - other.y}; +} + +bool Vector::operator==(const Vector& other) const { + return this->x == other.x and this->y == other.y; +} + +Vector Vector::operator%(float value) const { + return {.x = std::fmod(this->x, value), .y = std::fmod(this->y, value)}; +} +} // namespace micras::core diff --git a/micras_hal/CMakeLists.txt b/micras_hal/CMakeLists.txt new file mode 100644 index 0000000..ccbd7db --- /dev/null +++ b/micras_hal/CMakeLists.txt @@ -0,0 +1,72 @@ +# The board target carries main.h and the vendor or fake Cube layer behind it, which the headers +# include, so it has to exist before the HAL is added +if(NOT TARGET ${MICRAS_LIB_CUBE_TARGET}) + message(FATAL_ERROR + "micras-lib: the ${MICRAS_LIB_HAL_BACKEND} HAL links '${MICRAS_LIB_CUBE_TARGET}', which is not " + "defined yet. Define it (CubeMX's stm32cubemx, or a fake Cube layer for the host) before adding " + "micras-lib, or set MICRAS_LIB_CUBE_TARGET.") +endif() + +add_library(micras_hal STATIC) +add_library(micras::hal ALIAS micras_hal) + +target_include_directories(micras_hal PUBLIC + include +) + +# stm32cubemx, or the consumer's fake Cube layer on the host, carries the vendor include directories +# and the device macros the public headers name, so it propagates +target_link_libraries(micras_hal PUBLIC + ${MICRAS_LIB_CUBE_TARGET} +) + +# The HAL uses nothing of the core. The link only keeps the core's archive after the HAL's on a +# consumer's link line, the order the robot's image was verified with. +target_link_libraries(micras_hal PRIVATE + micras::core +) + +if(MICRAS_LIB_HAL_BACKEND STREQUAL "stm32") + # The family is read from the device macro CubeMX puts on its interface target + if(MICRAS_LIB_STM32_FAMILY) + set(MICRAS_HAL_FAMILY ${MICRAS_LIB_STM32_FAMILY}) + else() + get_target_property(MICRAS_CUBE_DEFINITIONS ${MICRAS_LIB_CUBE_TARGET} INTERFACE_COMPILE_DEFINITIONS) + + if(MICRAS_CUBE_DEFINITIONS MATCHES "STM32H7") + set(MICRAS_HAL_FAMILY h7) + elseif(MICRAS_CUBE_DEFINITIONS MATCHES "STM32G4") + set(MICRAS_HAL_FAMILY g4) + else() + message(FATAL_ERROR "micras-lib: cannot tell the STM32 family from ${MICRAS_LIB_CUBE_TARGET}; " + "set MICRAS_LIB_STM32_FAMILY to h7 or g4.") + endif() + endif() + + if(NOT IS_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/stm32/${MICRAS_HAL_FAMILY}) + message(FATAL_ERROR "micras-lib: the stm32 backend has no code for the '${MICRAS_HAL_FAMILY}' family.") + endif() + + file(GLOB MICRAS_HAL_SOURCES CONFIGURE_DEPENDS + stm32/src/*.cpp + stm32/${MICRAS_HAL_FAMILY}/src/*.cpp + ) + + target_include_directories(micras_hal PUBLIC + stm32/${MICRAS_HAL_FAMILY}/include + ) +else() + file(GLOB MICRAS_HAL_SOURCES CONFIGURE_DEPENDS + host/src/*.cpp + ) + + target_include_directories(micras_hal PUBLIC + host/include + ) +endif() + +target_sources(micras_hal PRIVATE + ${MICRAS_HAL_SOURCES} +) + +micras_lib_setup_library(micras_hal) diff --git a/micras_hal/host/include/micras/hal/family/flash.hpp b/micras_hal/host/include/micras/hal/family/flash.hpp new file mode 100644 index 0000000..26d0068 --- /dev/null +++ b/micras_hal/host/include/micras/hal/family/flash.hpp @@ -0,0 +1,39 @@ +/** + * @file + * + * @brief Flash memory geometry of the host backend, read from the board's macros. + * + * @note The board's main.h defines FLASH_NB_32BITWORD_IN_FLASHWORD, FLASH_SECTOR_SIZE and + * FLASH_SECTOR_TOTAL with the values of the chip it stands for, and the storage region is the upper + * half of those sectors, as on the stm32 backend. + */ + +#ifndef MICRAS_HAL_FAMILY_FLASH_HPP +#define MICRAS_HAL_FAMILY_FLASH_HPP + +#include +#include + +namespace micras::hal::family { +/** + * @brief Number of bits of the smallest unit the flash memory programs. + */ +inline constexpr uint32_t flash_word_bits{FLASH_NB_32BITWORD_IN_FLASHWORD * 32U}; + +/** + * @brief Number of bytes of an erasable sector. + */ +inline constexpr uint32_t sector_size{FLASH_SECTOR_SIZE}; + +/** + * @brief Index of the first sector reserved for data storage. + */ +inline constexpr uint16_t storage_first_sector{FLASH_SECTOR_TOTAL / 2}; + +/** + * @brief Number of sectors reserved for data storage. + */ +inline constexpr uint16_t storage_sectors{FLASH_SECTOR_TOTAL / 2}; +} // namespace micras::hal::family + +#endif // MICRAS_HAL_FAMILY_FLASH_HPP diff --git a/micras_hal/host/include/micras/hal/host/board.hpp b/micras_hal/host/include/micras/hal/host/board.hpp new file mode 100644 index 0000000..24d7479 --- /dev/null +++ b/micras_hal/host/include/micras/hal/host/board.hpp @@ -0,0 +1,172 @@ +/** + * @file + * + * @brief Every host peripheral port, found by the handle or pin that names it. + */ + +#ifndef MICRAS_HAL_HOST_BOARD_HPP +#define MICRAS_HAL_HOST_BOARD_HPP + +#include +#include +#include +#include + +#include "micras/hal/host/ports.hpp" +#include "micras/hal/host/spi_device.hpp" + +struct GPIO_TypeDef; +struct SPI_HandleTypeDef; + +namespace micras::hal::host { +/** + * @brief Registry of the ports the host backend and the outside world share. + * + * @note A port is created the first time either side asks for it, keyed by the + * address of the Cube handle or GPIO port that names it, so the firmware's + * own configuration is the key: `.port = Status_LED_GPIO_Port, .pin = + * Status_LED_Pin` in a board configuration and the same two names in a + * robot's bindings reach the same port. Nothing here knows a physics engine; bindings + * connect ports to whatever simulates the board. + * + * @note Not thread safe, and it does not need to be: the firmware and whatever + * drives the ports take strict turns. + */ +class Board { +public: + Board() = delete; + + /** + * @brief Get the port of a GPIO pin. + * + * @param port GPIO port of the pin. + * @param pin Pin mask. + * @return The port. + */ + static GpioPort& gpio(const void* port, uint16_t pin); + + /** + * @brief Get the port of a PWM channel. + * + * @param timer Timer handle. + * @param channel Timer channel. + * @return The port. + */ + static PwmPort& pwm(const void* timer, uint32_t channel); + + /** + * @brief Get the port of a DMA-fed timer channel. + * + * @param timer Timer handle. + * @param channel Timer channel. + * @return The port. + */ + static PwmDmaPort& pwm_dma(const void* timer, uint32_t channel); + + /** + * @brief Get the port of an ADC. + * + * @param adc ADC handle. + * @return The port. + */ + static AdcPort& adc(const void* adc); + + /** + * @brief Get the port of a UART. + * + * @param uart UART handle. + * @return The port. + */ + static UartPort& uart(const void* uart); + + /** + * @brief Get the port of a timer in encoder mode. + * + * @param timer Timer handle. + * @return The port. + */ + static EncoderPort& encoder(const void* timer); + + /** + * @brief Get the port of one chip select of an SPI bus. + * + * @param spi SPI handle. + * @param cs_port GPIO port of the chip select. + * @param cs_pin Pin mask of the chip select. + * @return The port. + */ + static SpiPort& spi(const void* spi, const void* cs_port, uint16_t cs_pin); + + /** + * @brief Attach a device to one chip select of an SPI bus. + * + * @note Keyed by the bus and the chip select, since several devices share + * a bus. Binds the SPI port and the chip select's GPIO port. The device + * is held by reference and must outlive the run. + * + * @param spi SPI handle. + * @param cs_port GPIO port of the chip select. + * @param cs_pin Pin mask of the chip select. + * @param device The device. + */ + static void + spi_device(const SPI_HandleTypeDef* spi, const GPIO_TypeDef* cs_port, uint16_t cs_pin, SpiDevice& device); + + /** + * @brief Get the port of the filter math accelerator. + * + * @param fmac FMAC handle. + * @return The port. + */ + static FmacPort& fmac(const void* fmac); + + /** + * @brief Get the flash. + * + * @return The port. + */ + static FlashPort& flash(); + + /** + * @brief Get the microcontroller core. + * + * @return The port. + */ + static McuPort& mcu(); + + /** + * @brief Name a GPIO pin, as the Cube layer labels it. + * + * @param port GPIO port of the pin. + * @param pin Pin mask. + * @param name Label. + */ + static void name_gpio(const void* port, uint16_t pin, std::string_view name); + + /** + * @brief Name a peripheral handle, as the Cube layer calls it. + * + * @note Ports created for the handle later are named after it. + * + * @param handle Peripheral handle. + * @param name Name. + */ + static void name_handle(const void* handle, std::string_view name); + + /** + * @brief List the ports the firmware used that nothing is bound to. + * + * @return Their names, one per port. + */ + static std::vector unbound(); + + /** + * @brief Forget every port and name. + * + * @note Called at the start of every run. References to ports obtained before are dangling after. + */ + static void reset(); +}; +} // namespace micras::hal::host + +#endif // MICRAS_HAL_HOST_BOARD_HPP diff --git a/micras_hal/host/include/micras/hal/host/clock.hpp b/micras_hal/host/include/micras/hal/host/clock.hpp new file mode 100644 index 0000000..4abe3fe --- /dev/null +++ b/micras_hal/host/include/micras/hal/host/clock.hpp @@ -0,0 +1,145 @@ +/** + * @file + * + * @brief The only place time exists for a firmware running on a host. + */ + +#ifndef MICRAS_HAL_HOST_CLOCK_HPP +#define MICRAS_HAL_HOST_CLOCK_HPP + +#include +#include + +namespace micras::hal::host { +/** + * @brief Counts the core cycles the firmware sees, and hands time over to the world. + * + * @note Every read of the timer costs a quantum of simulated time, one + * microsecond, since a read on the robot also takes time and a loop that + * polls the timer must see it advance. When a + * read crosses the end of a step, the handover callback runs: whatever + * drives the world advances it by one step and returns, and the read + * returns after it. So a busy wait runs the world exactly as long as it + * waits, and time inside an iteration is the number of timer reads times + * the quantum: deterministic, but not a measurement of anything. + * + * @note The counter wraps at 32 bits exactly as the cycle counter does. + */ +class Clock { +public: + /** + * @brief Called at every step boundary a read crosses. + */ + using Handover = std::function; + + /** + * @brief Get the process-wide clock. + * + * @return The clock. + */ + static Clock& instance(); + + /** + * @brief Set the core frequency, which makes a read cost one microsecond of cycles. + * + * @param cycles_per_microsecond Core clock, in cycles per microsecond. + */ + void configure(uint32_t cycles_per_microsecond); + + /** + * @brief Hand time over to the world at every step. + * + * @param step_us Length of a step, in microseconds. + * @param handover Called when a read crosses a step boundary. + */ + void set_handover(uint32_t step_us, Handover handover); + + /** + * @brief Stop handing time over; reads keep counting. + */ + void clear_handover(); + + /** + * @brief Read the cycle counter, as the firmware does. + * + * @return Cycles since the start, modulo 2^32. + */ + uint32_t read_cycles(); + + /** + * @brief Read the millisecond counter, as the firmware does. + * + * @return Milliseconds since the start, modulo 2^32. + */ + uint32_t read_ms(); + + /** + * @brief Get the time without charging for it. + * + * @return Cycles since the start, not wrapped. + */ + uint64_t now() const { return this->cycles; } + + /** + * @brief Get the core frequency. + * + * @return Cycles per microsecond. + */ + uint32_t cycles_per_microsecond() const { return this->cycles_per_us; } + + /** + * @brief Get how many step boundaries were handed over. + * + * @return Number of handovers. + */ + uint64_t steps() const { return this->handed_over; } + + /** + * @brief Rewind to the start, with no handover. + */ + void reset(); + +private: + /** + * @brief Charge one read and hand over at every boundary it crosses. + */ + void charge(); + + /** + * @brief Core cycles per microsecond. + */ + uint32_t cycles_per_us{1}; + + /** + * @brief Cycles one read costs. + */ + uint64_t quantum{1}; + + /** + * @brief Cycles since the start. + */ + uint64_t cycles{0}; + + /** + * @brief Cycles per step, zero when nothing is handed over. + */ + uint64_t step{0}; + + /** + * @brief Cycle count of the next step boundary. + */ + uint64_t next_boundary{0}; + + /** + * @brief Number of boundaries handed over. + */ + uint64_t handed_over{0}; + + /** + * @brief What runs at a boundary. + */ + Handover handover; +}; +} // namespace micras::hal::host + +#endif // MICRAS_HAL_HOST_CLOCK_HPP diff --git a/micras_hal/host/include/micras/hal/host/ports.hpp b/micras_hal/host/include/micras/hal/host/ports.hpp new file mode 100644 index 0000000..65cc03e --- /dev/null +++ b/micras_hal/host/include/micras/hal/host/ports.hpp @@ -0,0 +1,308 @@ +/** + * @file + * + * @brief What each host peripheral holds: the state the firmware and the outside world share. + */ + +#ifndef MICRAS_HAL_HOST_PORTS_HPP +#define MICRAS_HAL_HOST_PORTS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/hal/host/spi_device.hpp" + +namespace micras::hal::host { +/** + * @brief What every port has: a name and who has used it. + * + * @note touched is set by the firmware side, bound by whatever connects the port + * to the outside world. A port the firmware touched that nothing is bound + * to is a wiring mistake, and is reported. + */ +struct Port { + /** + * @brief Label from the Cube layer, or a description of the handle. + */ + std::string name; + + /** + * @brief Whether the firmware used the port. + */ + bool touched{false}; + + /** + * @brief Whether something outside the firmware is connected to it. + */ + bool bound{false}; +}; + +/** + * @brief One GPIO pin. + */ +struct GpioPort : Port { + /** + * @brief Level the firmware drives, as its output data register holds it. + */ + bool output{false}; + + /** + * @brief Level driven from outside, which is what a read returns when set. + */ + std::optional input; + + /** + * @brief Get the level the pin reads. + * + * @return The external level when one is driven, the output level otherwise. + */ + bool read() const { return this->input.value_or(this->output); } +}; + +/** + * @brief One PWM output channel. + */ +struct PwmPort : Port { + /** + * @brief Fraction of the period the output is active, in percent, polarity applied. + */ + float duty_cycle{0.0F}; + + /** + * @brief Frequency the channel runs at, in hertz. + */ + float frequency{0.0F}; +}; + +/** + * @brief One timer channel whose compare register a DMA stream feeds. + */ +struct PwmDmaPort : Port { + /** + * @brief Compare values of the last transfer the firmware started. + */ + std::vector compares; + + /** + * @brief Counts in one period, the autoreload plus one. + */ + uint32_t period{0}; + + /** + * @brief Whether a transfer is still going out. + */ + bool busy{false}; + + /** + * @brief Host clock cycle the current transfer ends at, one timer period per value. + */ + uint64_t transfer_end{0}; +}; + +/** + * @brief One ADC with its DMA stream. + * + * @note The firmware hands over the buffer the DMA writes; a device fills it + * with a whole sequence and then calls finish_sequence(), which is what + * the transfer complete interrupt does on the robot. + */ +struct AdcPort : Port { + /** + * @brief Buffer the firmware handed to the DMA, in one of its two widths. + */ + ///@{ + std::span buffer16; + std::span buffer32; + ///@} + + /** + * @brief Called at the end of a sequence, as the conversion complete interrupt would. + */ + std::function complete; + + /** + * @brief Get how many conversions the buffer holds. + * + * @return Size of the buffer, zero before the DMA was started. + */ + std::size_t size() const { return this->buffer16.empty() ? this->buffer32.size() : this->buffer16.size(); } + + /** + * @brief Write one conversion result. + * + * @param index Index in the buffer; ignored when out of range. + * @param counts Result, in ADC counts. + * + * @note Const because the buffer belongs to the firmware and is only viewed through a span: the port itself + * does not change. + */ + void write(std::size_t index, uint32_t counts) const; + + /** + * @brief End the sequence, as the DMA interrupt does. + */ + void finish_sequence() const; +}; + +/** + * @brief One UART with its DMA streams. + */ +struct UartPort : Port { + /** + * @brief Circular buffer the firmware handed to the receive DMA. + */ + std::span rx_buffer; + + /** + * @brief Index the next received byte goes to, as the DMA counter implies it. + */ + std::size_t rx_head{0}; + + /** + * @brief Bytes the firmware handed to the transmit DMA and not yet sent. + */ + std::deque tx; + + /** + * @brief Put one received byte where the DMA would. + * + * @param byte Byte on the wire. + */ + void receive(uint8_t byte); + + /** + * @brief Check whether a transmission is in progress. + * + * @return True while bytes are waiting to leave. + */ + bool transmitting() const { return not this->tx.empty(); } +}; + +/** + * @brief The part of the flash the firmware may use, as a byte array. + */ +struct FlashPort : Port { + /** + * @brief Contents, erased to 0xFF. + */ + std::vector bytes; + + /** + * @brief Flash words the writes may still program before a simulated power loss. + * + * @note Each flash word a write programs uses one; a write that finds none left fails there, + * with the words before it programmed, as a write the power cut off leaves the flash. + * Unlimited unless a test sets it. + */ + uint32_t write_budget{std::numeric_limits::max()}; +}; + +/** + * @brief The filter math accelerator, running the filter the firmware configured. + * + * @note The accelerator computes in q1.15 and keeps the history of its filter + * in its own memory, which is what this port holds on the host. + */ +struct FmacPort : Port { + /** + * @brief Coefficients applied to the inputs, most recent first, in q1.15. + */ + std::vector feed_forward; + + /** + * @brief Coefficients applied to the previous outputs, most recent first, in q1.15, added. + */ + std::vector feedback; + + /** + * @brief Last inputs, most recent first, as many as there are feed forward coefficients. + */ + std::deque inputs; + + /** + * @brief Last outputs, most recent first, as many as there are feedback coefficients. + */ + std::deque outputs; + + /** + * @brief Start a filter with an empty history. + * + * @param feed_forward_coefficients Coefficients applied to the inputs. + * @param feedback_coefficients Coefficients applied to the previous outputs. + */ + void configure(std::span feed_forward_coefficients, std::span feedback_coefficients); + + /** + * @brief Push one input through the filter, as a write to the accelerator's input register does. + * + * @note The products are exact and their sum is kept whole, where the accelerator's accumulator + * has 26 bits; the output is the sum shifted back to q1.15 and clipped to its range. + * + * @param sample Input, in q1.15. + * @return Output, in q1.15. + */ + int16_t filter(int16_t sample); +}; + +/** + * @brief What the microcontroller core reports. + */ +struct McuPort : Port { + /** + * @brief Independent watchdog timeout the firmware set, zero before it set one. + */ + uint32_t watchdog_timeout_ms{0}; + + /** + * @brief Host time of the last refresh, in timer cycles. + */ + uint64_t last_refresh{0}; + + /** + * @brief Number of times the watchdog would have reset the chip. + */ + uint32_t watchdog_expiries{0}; + + /** + * @brief Number of emergency stops the firmware performed. + */ + uint32_t emergency_stops{0}; +}; + +/** + * @brief A quadrature count, as a timer in encoder mode holds it. + */ +struct EncoderPort : Port { + /** + * @brief Count, positive forward. + */ + int32_t count{0}; +}; + +/** + * @brief One chip select of an SPI bus, and the device behind it. + * + * @note The firmware touches the port when it selects the device; a binding + * binds it by attaching a device with Board::spi_device(). + */ +struct SpiPort : Port { + /** + * @brief Device attached to the chip select, none before a binding attaches one. + */ + SpiDevice* device{nullptr}; + + /** + * @brief Whether the chip select is low, between the firmware's select and unselect. + */ + bool selected{false}; +}; +} // namespace micras::hal::host + +#endif // MICRAS_HAL_HOST_PORTS_HPP diff --git a/micras_hal/host/include/micras/hal/host/spi_device.hpp b/micras_hal/host/include/micras/hal/host/spi_device.hpp new file mode 100644 index 0000000..cc52f2c --- /dev/null +++ b/micras_hal/host/include/micras/hal/host/spi_device.hpp @@ -0,0 +1,107 @@ +/** + * @file + * + * @brief A chip on an SPI bus, as the host backend sees it: a chip select and a stream of bytes. + */ + +#ifndef MICRAS_HAL_HOST_SPI_DEVICE_HPP +#define MICRAS_HAL_HOST_SPI_DEVICE_HPP + +#include +#include + +namespace micras::hal::host { +/** + * @brief A device behind one chip select of an SPI bus. + * + * @note The host Spi calls select() when the firmware pulls the device's chip + * select low, exchange() for every transfer while it is low, however many + * HAL calls the firmware splits the transaction into, and deselect() when + * the chip select goes high again. A transaction is therefore everything + * between select() and deselect(), which is what a chip sees on the bus. + * + * @note A device only answers in the SPI mode it expects. When the mode the bus + * runs in differs, the host Spi does not call exchange() and the firmware + * reads all ones, as a chip clocked on the wrong edge answers on the robot. + */ +class SpiDevice { +public: + /** + * @brief SPI mode: the clock polarity in the high bit, the clock phase in the low one. + */ + enum class Mode : uint8_t { + /** + * @brief The clock idles low and the data is sampled on its first edge, the rising one. + */ + MODE_0 = 0, + + /** + * @brief The clock idles low and the data is sampled on its second edge, the falling one. + */ + MODE_1 = 1, + + /** + * @brief The clock idles high and the data is sampled on its first edge, the falling one. + */ + MODE_2 = 2, + + /** + * @brief The clock idles high and the data is sampled on its second edge, the rising one. + */ + MODE_3 = 3, + }; + + /** + * @brief Special member functions: a device is attached by reference, so it cannot be copied or moved. + */ + ///@{ + virtual ~SpiDevice() = default; + SpiDevice(const SpiDevice&) = delete; + SpiDevice(SpiDevice&&) = delete; + SpiDevice& operator=(const SpiDevice&) = delete; + SpiDevice& operator=(SpiDevice&&) = delete; + + ///@} + + /** + * @brief Get the SPI mode the device answers in. + * + * @return The mode. + */ + Mode mode() const { return this->spi_mode; } + + /** + * @brief Start a transaction: the chip select went low. + */ + virtual void select() = 0; + + /** + * @brief Clock bytes through the device while it is selected. + * + * @param transmitted Bytes the controller sends, one per received byte. + * @param received Bytes the device answers with, as many as were transmitted. + */ + virtual void exchange(std::span transmitted, std::span received) = 0; + + /** + * @brief End the transaction: the chip select went high. + */ + virtual void deselect() = 0; + +protected: + /** + * @brief Construct a device that answers in one SPI mode. + * + * @param mode The mode. + */ + explicit SpiDevice(Mode mode) : spi_mode{mode} { } + +private: + /** + * @brief SPI mode the device answers in. + */ + Mode spi_mode; +}; +} // namespace micras::hal::host + +#endif // MICRAS_HAL_HOST_SPI_DEVICE_HPP diff --git a/micras_hal/host/include/stm32_host.h b/micras_hal/host/include/stm32_host.h new file mode 100644 index 0000000..0559b5b --- /dev/null +++ b/micras_hal/host/include/stm32_host.h @@ -0,0 +1,373 @@ +/** + * @file + * + * @brief The STM32 handle types and HAL constants a fake Cube layer builds on. + * + * @note The micras_hal headers name STM32 handle types in their configurations, + * and a robot's target.hpp names handles, channels and SPI modes. On a PC + * none of the vendor HAL exists, so a robot's fake Cube layer includes this + * file from its main.h and adds only what is its own: its handles, pins, + * init functions and clock tree. The types carry just the fields the host + * backend reads or writes; the constants keep the vendor's values where the + * firmware could compare them. + */ + +#ifndef STM32_HOST_H +#define STM32_HOST_H + +#include + +/***************************************** + * GPIO + *****************************************/ + +/** + * @brief A GPIO port, identified by its address alone. + */ +struct GPIO_TypeDef { + /** + * @brief Letter of the port, for messages. + */ + char name; +}; + +///@{ +inline constexpr uint16_t GPIO_PIN_0{0x0001U}; +inline constexpr uint16_t GPIO_PIN_1{0x0002U}; +inline constexpr uint16_t GPIO_PIN_2{0x0004U}; +inline constexpr uint16_t GPIO_PIN_3{0x0008U}; +inline constexpr uint16_t GPIO_PIN_4{0x0010U}; +inline constexpr uint16_t GPIO_PIN_5{0x0020U}; +inline constexpr uint16_t GPIO_PIN_6{0x0040U}; +inline constexpr uint16_t GPIO_PIN_7{0x0080U}; +inline constexpr uint16_t GPIO_PIN_8{0x0100U}; +inline constexpr uint16_t GPIO_PIN_9{0x0200U}; +inline constexpr uint16_t GPIO_PIN_10{0x0400U}; +inline constexpr uint16_t GPIO_PIN_11{0x0800U}; +inline constexpr uint16_t GPIO_PIN_12{0x1000U}; +inline constexpr uint16_t GPIO_PIN_13{0x2000U}; +inline constexpr uint16_t GPIO_PIN_14{0x4000U}; +inline constexpr uint16_t GPIO_PIN_15{0x8000U}; + +///@} + +/***************************************** + * Timers + *****************************************/ + +/** + * @brief The registers of a timer the host backend models. + * + * @note kernel_clock is not a register: it is the frequency the timer counts + * at, which on the chip comes from the clock tree the init functions + * configure. The fake init function writes it. + */ +struct TIM_TypeDef { + uint32_t CR1; + uint32_t CCER; + uint32_t PSC; + uint32_t ARR; + uint32_t kernel_clock; +}; + +/** + * @brief Base configuration of a timer, as the init function fills it. + */ +struct TIM_Base_InitTypeDef { + uint32_t Prescaler; + uint32_t CounterMode; + uint32_t Period; +}; + +/** + * @brief State of a timer handle. + */ +enum HAL_TIM_StateTypeDef : uint8_t { + HAL_TIM_STATE_RESET = 0, + HAL_TIM_STATE_READY = 1, +}; + +/** + * @brief A timer handle. + */ +struct TIM_HandleTypeDef { + TIM_TypeDef* Instance; + TIM_Base_InitTypeDef Init; + HAL_TIM_StateTypeDef State; +}; + +///@{ +inline constexpr uint32_t TIM_CHANNEL_1{0x00000000U}; +inline constexpr uint32_t TIM_CHANNEL_2{0x00000004U}; +inline constexpr uint32_t TIM_CHANNEL_3{0x00000008U}; +inline constexpr uint32_t TIM_CHANNEL_4{0x0000000CU}; +inline constexpr uint32_t TIM_CHANNEL_ALL{0x0000003CU}; +///@} + +///@{ +inline constexpr uint32_t TIM_CR1_CMS{0x3U << 5U}; +inline constexpr uint32_t TIM_CCER_CC1P{0x1U << 1U}; +inline constexpr uint32_t TIM_COUNTERMODE_UP{0x00000000U}; +inline constexpr uint32_t TIM_COUNTERMODE_CENTERALIGNED1{0x1U << 5U}; + +///@} + +/***************************************** + * DMA + *****************************************/ + +/** + * @brief A DMA handle, whose transfers the host backend never lets stop. + */ +struct DMA_HandleTypeDef { }; + +/***************************************** + * ADC + *****************************************/ + +/** + * @brief Configuration of an ADC, as the init function fills it. + */ +struct ADC_InitTypeDef { + uint32_t Resolution; + uint32_t NbrOfConversion; +}; + +///@{ +inline constexpr uint32_t HAL_ADC_STATE_RESET{0x00000000U}; +inline constexpr uint32_t HAL_ADC_STATE_READY{0x00000001U}; + +///@} + +/** + * @brief An ADC handle. + */ +struct ADC_HandleTypeDef { + ADC_InitTypeDef Init; + uint32_t State; +}; + +/***************************************** + * SPI + *****************************************/ + +///@{ +inline constexpr uint32_t SPI_POLARITY_LOW{0x00000000U}; +inline constexpr uint32_t SPI_POLARITY_HIGH{0x1U << 25U}; +inline constexpr uint32_t SPI_PHASE_1EDGE{0x00000000U}; +inline constexpr uint32_t SPI_PHASE_2EDGE{0x1U << 24U}; + +///@} + +/** + * @brief Dividers of the SPI kernel clock, from 2 to 256, in the bits the vendor puts them. + */ +///@{ +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_2{0x00000000U}; +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_4{0x10000000U}; +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_8{0x20000000U}; +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_16{0x30000000U}; +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_32{0x40000000U}; +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_64{0x50000000U}; +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_128{0x60000000U}; +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_256{0x70000000U}; + +///@} + +/** + * @brief The registers of an SPI bus the host backend models. + * + * @note kernel_clock is not a register: it is the frequency the bus's kernel + * clock runs at, which on the chip comes from the clock tree the init + * functions configure. The fake init function writes it, and the bit + * rate is that divided by the prescaler. + */ +struct SPI_TypeDef { + /** + * @brief Frequency of the bus's kernel clock, in hertz. + */ + uint32_t kernel_clock; +}; + +/** + * @brief Configuration of an SPI bus, as the init function fills it. + */ +struct SPI_InitTypeDef { + /** + * @brief Level the clock idles at, SPI_POLARITY_LOW or SPI_POLARITY_HIGH. + */ + uint32_t CLKPolarity; + + /** + * @brief Edge the data is sampled on, SPI_PHASE_1EDGE or SPI_PHASE_2EDGE. + */ + uint32_t CLKPhase; + + /** + * @brief Divider of the kernel clock that gives the bit rate, SPI_BAUDRATEPRESCALER_2 to SPI_BAUDRATEPRESCALER_256. + */ + uint32_t BaudRatePrescaler; +}; + +/** + * @brief State of an SPI handle. + */ +enum HAL_SPI_StateTypeDef : uint8_t { + HAL_SPI_STATE_RESET = 0, + HAL_SPI_STATE_READY = 1, +}; + +/** + * @brief An SPI handle. + */ +struct SPI_HandleTypeDef { + /** + * @brief Registers of the bus. + */ + SPI_TypeDef* Instance; + + /** + * @brief Configuration of the bus. + */ + SPI_InitTypeDef Init; + + /** + * @brief State of the handle. + */ + HAL_SPI_StateTypeDef State; +}; + +/***************************************** + * UART + *****************************************/ + +/** + * @brief State of a UART handle. + */ +enum HAL_UART_StateTypeDef : uint8_t { + HAL_UART_STATE_RESET = 0x00, + HAL_UART_STATE_READY = 0x20, + HAL_UART_STATE_BUSY_RX = 0x22, +}; + +/** + * @brief Configuration of a UART, as the init function fills it. + */ +struct UART_InitTypeDef { + uint32_t BaudRate; +}; + +/** + * @brief A UART handle. + */ +struct UART_HandleTypeDef { + UART_InitTypeDef Init; + HAL_UART_StateTypeDef gState; + HAL_UART_StateTypeDef RxState; +}; + +/***************************************** + * CRC + *****************************************/ + +///@{ +inline constexpr uint8_t DEFAULT_POLYNOMIAL_ENABLE{0x00U}; +inline constexpr uint8_t DEFAULT_POLYNOMIAL_DISABLE{0x01U}; +inline constexpr uint8_t DEFAULT_INIT_VALUE_ENABLE{0x00U}; +inline constexpr uint8_t DEFAULT_INIT_VALUE_DISABLE{0x01U}; +inline constexpr uint32_t CRC_POLYLENGTH_32B{0x00000000U}; +inline constexpr uint32_t CRC_POLYLENGTH_16B{0x1U << 3U}; +inline constexpr uint32_t CRC_POLYLENGTH_8B{0x2U << 3U}; +inline constexpr uint32_t CRC_POLYLENGTH_7B{0x3U << 3U}; +inline constexpr uint32_t CRC_INPUTDATA_INVERSION_NONE{0x00000000U}; +inline constexpr uint32_t CRC_INPUTDATA_INVERSION_BYTE{0x1U << 5U}; +inline constexpr uint32_t CRC_INPUTDATA_INVERSION_HALFWORD{0x2U << 5U}; +inline constexpr uint32_t CRC_INPUTDATA_INVERSION_WORD{0x3U << 5U}; +inline constexpr uint32_t CRC_OUTPUTDATA_INVERSION_DISABLE{0x00000000U}; +inline constexpr uint32_t CRC_OUTPUTDATA_INVERSION_ENABLE{0x1U << 7U}; +inline constexpr uint32_t CRC_INPUTDATA_FORMAT_BYTES{0x00000001U}; +inline constexpr uint32_t CRC_INPUTDATA_FORMAT_HALFWORDS{0x00000002U}; +inline constexpr uint32_t CRC_INPUTDATA_FORMAT_WORDS{0x00000003U}; +inline constexpr uint32_t DEFAULT_CRC32_POLY{0x04C11DB7U}; +inline constexpr uint32_t DEFAULT_CRC_INITVALUE{0xFFFFFFFFU}; + +///@} + +/** + * @brief Configuration of the CRC unit, as the init function fills it. + */ +struct CRC_InitTypeDef { + uint8_t DefaultPolynomialUse; + uint8_t DefaultInitValueUse; + uint32_t GeneratingPolynomial; + uint32_t CRCLength; + uint32_t InitValue; + uint32_t InputDataInversionMode; + uint32_t OutputDataInversionMode; +}; + +/** + * @brief The CRC unit's handle. + */ +struct CRC_HandleTypeDef { + CRC_InitTypeDef Init; + uint32_t InputDataFormat; +}; + +/***************************************** + * Independent watchdog + *****************************************/ + +/** + * @brief The registers of the independent watchdog. + * + * @note The host backend times the watchdog in the MCU port and never reads + * these registers; they exist so that the family functions have the + * same signatures on every backend. + */ +struct IWDG_TypeDef { + /** + * @brief Key register: starts, reloads and unlocks the watchdog. + */ + uint32_t KR; + + /** + * @brief Prescaler register. + */ + uint32_t PR; + + /** + * @brief Reload register. + */ + uint32_t RLR; + + /** + * @brief Status register, with the prescaler and reload update flags. + */ + uint32_t SR; +}; + +/***************************************** + * FMAC + *****************************************/ + +/** + * @brief States of the filter accelerator's handle. + */ +///@{ +inline constexpr uint32_t HAL_FMAC_STATE_RESET{0x00U}; +inline constexpr uint32_t HAL_FMAC_STATE_READY{0x20U}; + +///@} + +/** + * @brief The filter accelerator's handle. + * + * @note The host backend keeps the filter itself in the accelerator's port, so + * the handle carries only the state the init function sets. + */ +struct FMAC_HandleTypeDef { + uint32_t State; +}; + +#endif // STM32_HOST_H diff --git a/micras_hal/host/src/adc_dma.cpp b/micras_hal/host/src/adc_dma.cpp new file mode 100644 index 0000000..96f4164 --- /dev/null +++ b/micras_hal/host/src/adc_dma.cpp @@ -0,0 +1,142 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/hal/adc_dma.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" + +namespace micras::hal { +std::array AdcDma::instances{}; +uint32_t AdcDma::restarts{}; + +AdcDma::AdcDma(const Config& config) : + max_reading{config.max_reading}, reference_voltage{config.reference_voltage}, handle{config.handle} { + if (this->handle->State == HAL_ADC_STATE_RESET) { + config.init_function(); + } + + auto* const slot = std::ranges::find(instances, nullptr); + + if (slot == instances.end()) { + return; + } + + *slot = this; + this->initialized = this->handle->State == HAL_ADC_STATE_READY; +} + +AdcDma::~AdcDma() { + std::ranges::replace(instances, this, static_cast(nullptr)); +} + +bool AdcDma::start_dma(std::span buffer) { + host::AdcPort& port = host::Board::adc(this->handle); + port.touched = true; + port.buffer32 = buffer; + port.buffer16 = {}; + port.complete = [adc = this->handle] { on_sequence_complete(adc); }; + return true; +} + +bool AdcDma::start_dma(std::span buffer) { + host::AdcPort& port = host::Board::adc(this->handle); + port.touched = true; + port.buffer16 = buffer; + port.buffer32 = {}; + port.complete = [adc = this->handle] { on_sequence_complete(adc); }; + return true; +} + +bool AdcDma::start_dma(std::span buffer, std::span snapshot) { + if (snapshot.size() != buffer.size()) { + this->initialized = false; + return false; + } + + this->buffer = buffer; + this->snapshot = snapshot; + + return this->start_dma(buffer); +} + +uint32_t AdcDma::read_snapshot(std::span destination) const { + uint32_t before = this->sequence; + + while (true) { + std::ranges::copy(this->snapshot, destination.begin()); + + const uint32_t after = this->sequence; + + if (after == before) { + return after; + } + + before = after; + } +} + +void AdcDma::on_sequence_complete(const ADC_HandleTypeDef* handle) { + AdcDma* const instance = find(handle); + + if (instance == nullptr or instance->stopped) { + return; + } + + std::ranges::copy(instance->buffer, instance->snapshot.begin()); + instance->sequence = instance->sequence + 1; +} + +void AdcDma::on_error(const ADC_HandleTypeDef* handle) { + AdcDma* const instance = find(handle); + + if (instance != nullptr) { + instance->stopped = true; + } +} + +AdcDma* AdcDma::find(const ADC_HandleTypeDef* handle) { + const auto* const found = std::ranges::find_if(instances, [handle](const AdcDma* instance) { + return instance != nullptr and instance->handle == handle; + }); + + return found == instances.end() ? nullptr : *found; +} + +void AdcDma::stop_dma() { + host::AdcPort& port = host::Board::adc(this->handle); + port.buffer16 = {}; + port.buffer32 = {}; + port.complete = nullptr; +} + +void AdcDma::recover() { + if (not this->stopped) { + return; + } + + this->stopped = false; + restarts++; +} + +uint32_t AdcDma::get_restarts() { + return restarts; +} + +uint16_t AdcDma::get_max_reading() const { + return this->max_reading; +} + +float AdcDma::get_reference_voltage() const { + return this->reference_voltage; +} + +bool AdcDma::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/board.cpp b/micras_hal/host/src/board.cpp new file mode 100644 index 0000000..4d6c9e4 --- /dev/null +++ b/micras_hal/host/src/board.cpp @@ -0,0 +1,199 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" +#include "micras/hal/host/spi_device.hpp" + +namespace micras::hal::host { +namespace { +/** + * @brief Key of a port: the handle or GPIO port, and the channel or pin. + */ +using Key = std::pair; + +/** + * @brief Key of an SPI port: the handle, and the GPIO port and pin of the chip select. + */ +using SpiKey = std::tuple; + +/** + * @brief Every port and name, created on first use. + */ +struct Registry { + std::map gpios; + std::map pwms; + std::map pwm_dmas; + std::map adcs; + std::map uarts; + std::map encoders; + std::map spis; + std::map fmacs; + std::map gpio_names; + std::map handle_names; + FlashPort flash{{.name = "flash", .touched = false, .bound = false}, {}, std::numeric_limits::max()}; + McuPort mcu{{.name = "mcu", .touched = false, .bound = false}, 0, 0, 0, 0}; +}; +} // namespace + +/** + * @brief Get the registry. + * + * @return The process-wide registry. + */ +static Registry& registry() { + static Registry instance; + return instance; +} + +/** + * @brief Name a handle, falling back to its address. + * + * @param handle Peripheral handle. + * @return Its name. + */ +static std::string handle_name(const void* handle) { + const auto found = registry().handle_names.find(handle); + return found != registry().handle_names.end() ? found->second : + std::format("handle@{:#x}", std::bit_cast(handle)); +} + +/** + * @brief Name a GPIO pin, falling back to its port and mask. + * + * @param key GPIO port and pin mask. + * @return Its name. + */ +static std::string gpio_name(const Key& key) { + const auto found = registry().gpio_names.find(key); + return found != registry().gpio_names.end() ? found->second : + std::format("{} pin {:#06x}", handle_name(key.first), key.second); +} + +/** + * @brief Find a port, creating it with a name on first use. + * + * @param ports Ports of one kind. + * @param key Key of the port. + * @param name Makes the name of a new port. + * @return The port. + */ +template +static Map::mapped_type& find_or_create(Map& ports, const typename Map::key_type& key, const Namer& name) { + const auto [found, created] = ports.try_emplace(key); + + if (created) { + found->second.name = name(); + } + + return found->second; +} + +GpioPort& Board::gpio(const void* port, uint16_t pin) { + const Key key{port, pin}; + + return find_or_create(registry().gpios, key, [&key] { return gpio_name(key); }); +} + +PwmPort& Board::pwm(const void* timer, uint32_t channel) { + return find_or_create(registry().pwms, Key{timer, channel}, [timer, channel] { + return std::format("{} channel {}", handle_name(timer), channel / 4 + 1); + }); +} + +PwmDmaPort& Board::pwm_dma(const void* timer, uint32_t channel) { + return find_or_create(registry().pwm_dmas, Key{timer, channel}, [timer, channel] { + return std::format("{} channel {} DMA", handle_name(timer), channel / 4 + 1); + }); +} + +AdcPort& Board::adc(const void* adc) { + return find_or_create(registry().adcs, adc, [adc] { return handle_name(adc); }); +} + +UartPort& Board::uart(const void* uart) { + return find_or_create(registry().uarts, uart, [uart] { return handle_name(uart); }); +} + +EncoderPort& Board::encoder(const void* timer) { + return find_or_create(registry().encoders, timer, [timer] { return handle_name(timer) + " encoder"; }); +} + +SpiPort& Board::spi(const void* spi, const void* cs_port, uint16_t cs_pin) { + return find_or_create(registry().spis, SpiKey{spi, cs_port, cs_pin}, [spi, cs_port, cs_pin] { + return std::format("{} {}", handle_name(spi), gpio_name(Key{cs_port, cs_pin})); + }); +} + +void Board::spi_device(const SPI_HandleTypeDef* spi, const GPIO_TypeDef* cs_port, uint16_t cs_pin, SpiDevice& device) { + SpiPort& port = Board::spi(spi, cs_port, cs_pin); + port.device = &device; + port.bound = true; + Board::gpio(cs_port, cs_pin).bound = true; +} + +FmacPort& Board::fmac(const void* fmac) { + return find_or_create(registry().fmacs, fmac, [fmac] { return handle_name(fmac); }); +} + +FlashPort& Board::flash() { + return registry().flash; +} + +McuPort& Board::mcu() { + return registry().mcu; +} + +void Board::name_gpio(const void* port, uint16_t pin, std::string_view name) { + const Key key{port, pin}; + registry().gpio_names[key] = std::string{name}; + + const auto found = registry().gpios.find(key); + + if (found != registry().gpios.end()) { + found->second.name = std::string{name}; + } +} + +void Board::name_handle(const void* handle, std::string_view name) { + registry().handle_names[handle] = std::string{name}; +} + +std::vector Board::unbound() { + std::vector names; + + const auto collect = [&names](const auto& ports) { + for (const auto& [key, port] : ports) { + if (port.touched and not port.bound) { + names.push_back(port.name); + } + } + }; + + collect(registry().gpios); + collect(registry().pwms); + collect(registry().pwm_dmas); + collect(registry().adcs); + collect(registry().uarts); + collect(registry().encoders); + collect(registry().spis); + + return names; +} + +void Board::reset() { + registry() = Registry{}; +} +} // namespace micras::hal::host diff --git a/micras_hal/host/src/clock.cpp b/micras_hal/host/src/clock.cpp new file mode 100644 index 0000000..b92d9b9 --- /dev/null +++ b/micras_hal/host/src/clock.cpp @@ -0,0 +1,58 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/host/clock.hpp" + +namespace micras::hal::host { +Clock& Clock::instance() { + static Clock clock; + return clock; +} + +void Clock::configure(uint32_t cycles_per_microsecond) { + this->cycles_per_us = cycles_per_microsecond; + this->quantum = cycles_per_microsecond; +} + +void Clock::set_handover(uint32_t step_us, Handover handover) { + this->step = static_cast(step_us) * this->cycles_per_us; + this->next_boundary = (this->cycles / this->step + 1) * this->step; + this->handover = std::move(handover); +} + +void Clock::clear_handover() { + this->step = 0; + this->handover = nullptr; +} + +void Clock::charge() { + this->cycles += this->quantum; + + while (this->step != 0 and this->cycles >= this->next_boundary) { + this->next_boundary += this->step; + this->handed_over++; + + if (this->handover) { + this->handover(); + } + } +} + +uint32_t Clock::read_cycles() { + this->charge(); + return static_cast(this->cycles); +} + +uint32_t Clock::read_ms() { + this->charge(); + return static_cast(this->cycles / (1000ULL * this->cycles_per_us)); +} + +void Clock::reset() { + *this = Clock{}; +} +} // namespace micras::hal::host diff --git a/micras_hal/host/src/crc.cpp b/micras_hal/host/src/crc.cpp new file mode 100644 index 0000000..cb033e0 --- /dev/null +++ b/micras_hal/host/src/crc.cpp @@ -0,0 +1,77 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/crc.hpp" + +namespace micras::hal { +/** + * @brief Reverse the order of the lowest bits of a value. + * + * @param value Value to reflect. + * @param width Number of bits to reflect. + * @return The reflected bits. + */ +static uint32_t reflect(uint32_t value, uint32_t width) { + uint32_t reflected = 0; + + for (uint32_t bit = 0; bit < width; bit++) { + reflected |= ((value >> bit) & 1U) << (width - 1 - bit); + } + + return reflected; +} + +/** + * @brief Get the width of the CRC a configuration computes. + * + * @param length CRCLength field. + * @return Width in bits. + */ +static uint32_t width_of(uint32_t length) { + if (length == CRC_POLYLENGTH_7B) { + return 7; + } + + if (length == CRC_POLYLENGTH_8B) { + return 8; + } + + if (length == CRC_POLYLENGTH_16B) { + return 16; + } + + return 32; +} + +Crc::Crc(const Config& config) : handle{config.handle} { } + +uint32_t Crc::calculate(std::span data) { + const CRC_InitTypeDef& init = this->handle->Init; + const uint32_t width = width_of(init.CRCLength); + const uint32_t mask = width == 32 ? 0xFFFFFFFFU : (1U << width) - 1; + const uint32_t polynomial = + (init.DefaultPolynomialUse == DEFAULT_POLYNOMIAL_ENABLE ? DEFAULT_CRC32_POLY : init.GeneratingPolynomial) & + mask; + uint32_t crc = + (init.DefaultInitValueUse == DEFAULT_INIT_VALUE_ENABLE ? DEFAULT_CRC_INITVALUE : init.InitValue) & mask; + + for (const uint8_t byte : data) { + const uint32_t input = init.InputDataInversionMode == CRC_INPUTDATA_INVERSION_NONE ? byte : reflect(byte, 8); + + for (int bit = 7; bit >= 0; bit--) { + const uint32_t top = (crc >> (width - 1)) & 1U; + crc = (crc << 1U) & mask; + + if ((top ^ ((input >> static_cast(bit)) & 1U)) != 0) { + crc ^= polynomial; + } + } + } + + return init.OutputDataInversionMode == CRC_OUTPUTDATA_INVERSION_ENABLE ? reflect(crc, width) : crc; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/encoder.cpp b/micras_hal/host/src/encoder.cpp new file mode 100644 index 0000000..7d9b939 --- /dev/null +++ b/micras_hal/host/src/encoder.cpp @@ -0,0 +1,30 @@ +/** + * @file + */ + +#include + +#include "micras/hal/encoder.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" + +namespace micras::hal { +Encoder::Encoder(const Config& config) : handle{config.handle} { + if (this->handle->State == HAL_TIM_STATE_RESET) { + config.init_function(); + } + + host::EncoderPort& port = host::Board::encoder(this->handle); + port.touched = true; + this->start_count = static_cast(port.count); + this->initialized = this->handle->State == HAL_TIM_STATE_READY; +} + +int32_t Encoder::get_counter() const { + return static_cast(static_cast(host::Board::encoder(this->handle).count) - this->start_count); +} + +bool Encoder::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/family.cpp b/micras_hal/host/src/family.cpp new file mode 100644 index 0000000..a303573 --- /dev/null +++ b/micras_hal/host/src/family.cpp @@ -0,0 +1,45 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/family.hpp" + +namespace micras::hal::family { +bool calibrate_adc(ADC_HandleTypeDef* /*handle*/) { + return true; +} + +bool is_dma_enabled(const DMA_HandleTypeDef* /*handle*/) { + return true; +} + +bool has_dma_finished(const DMA_HandleTypeDef* /*handle*/) { + return false; +} + +void enable_caches() { } + +bool was_reset_by_watchdog() { + return false; +} + +void freeze_watchdog_in_debug() { } + +bool is_cpu_frequency_supported(bool /*boost*/) { + return true; +} + +IWDG_TypeDef* watchdog() { + static IWDG_TypeDef registers{}; + return ®isters; +} + +uint32_t timer_clock(const TIM_TypeDef* instance) { + return instance->kernel_clock; +} + +void enable_cycle_counter() { } +} // namespace micras::hal::family diff --git a/micras_hal/host/src/flash.cpp b/micras_hal/host/src/flash.cpp new file mode 100644 index 0000000..2e2388d --- /dev/null +++ b/micras_hal/host/src/flash.cpp @@ -0,0 +1,133 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/hal/flash.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" + +namespace micras::hal { +/** + * @brief Round a size up to whole flash words. + * + * @param size Size in bytes. + * @return The rounded size. + */ +static constexpr uint32_t align_size(uint32_t size) { + return (size + FlashWord::size - 1) / FlashWord::size * FlashWord::size; +} + +/** + * @brief Get the flash contents, erased on first use. + * + * @return The port. + */ +static host::FlashPort& storage() { + host::FlashPort& port = host::Board::flash(); + + if (port.bytes.size() != Flash::total_size) { + port.bytes.assign(Flash::total_size, FlashWord::erased_value); + } + + port.touched = true; + return port; +} + +FlashWord::FlashWord(std::span data) { + const auto data_address = std::bit_cast(data.data()); + + if (data.size() >= size and data_address % alignof(uint32_t) == 0) { + this->source = std::bit_cast(data.data()); + return; + } + + const std::span bytes{std::bit_cast(this->buffer.data()), size}; + + std::ranges::fill(bytes, erased_value); + std::ranges::copy(data.first(std::min(data.size(), size)), bytes.begin()); + + this->source = this->buffer.data(); +} + +const uint32_t* FlashWord::data() const { + return this->source; +} + +bool FlashWord::is_padded() const { + return this->source == this->buffer.data(); +} + +std::span Flash::read(uint32_t address, uint32_t size) { + if (address > total_size or size > total_size - address) { + return {}; + } + + return std::span{storage().bytes}.subspan(address, size); +} + +std::span Flash::read(uint16_t sector, uint32_t sector_address, uint32_t size) { + if (sector >= total_sectors or sector_address > sector_size) { + return {}; + } + + return read(sector * sector_size + sector_address, size); +} + +Flash::Status Flash::write(uint32_t address, std::span data) { + if (address % FlashWord::size != 0) { + return Status::MISALIGNED; + } + + if (address > total_size or align_size(data.size()) > total_size - address) { + return Status::OUT_OF_BOUNDS; + } + + host::FlashPort& port = storage(); + const std::span memory{port.bytes}; + + for (uint32_t offset = 0; offset < data.size(); offset += FlashWord::size) { + const FlashWord word{data.subspan(offset)}; + const std::span target = memory.subspan(address + offset, FlashWord::size); + + if (port.write_budget == 0 or + not std::ranges::all_of(target, [](uint8_t byte) { return byte == FlashWord::erased_value; })) { + return Status::ERROR; + } + + port.write_budget--; + + const std::span bytes{std::bit_cast(word.data()), FlashWord::size}; + std::ranges::copy(bytes, target.begin()); + } + + return Status::OK; +} + +Flash::Status Flash::write(uint16_t sector, uint32_t sector_address, std::span data) { + if (sector >= total_sectors or sector_address > sector_size) { + return Status::OUT_OF_BOUNDS; + } + + return write(sector * sector_size + sector_address, data); +} + +Flash::Status Flash::erase_sectors(uint16_t start_sector, uint16_t number_of_sectors) { + if (start_sector >= total_sectors or number_of_sectors > total_sectors - start_sector) { + return Status::OUT_OF_BOUNDS; + } + + host::FlashPort& port = storage(); + const auto first = static_cast(start_sector) * sector_size; + const auto count = static_cast(number_of_sectors) * sector_size; + + std::fill(port.bytes.begin() + first, port.bytes.begin() + first + count, FlashWord::erased_value); + + return Status::OK; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/fmac.cpp b/micras_hal/host/src/fmac.cpp new file mode 100644 index 0000000..5f71b66 --- /dev/null +++ b/micras_hal/host/src/fmac.cpp @@ -0,0 +1,44 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/fmac.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" + +namespace micras::hal { +Fmac::Fmac(const Config& config) : handle{config.handle} { + if (this->handle->State == HAL_FMAC_STATE_RESET) { + config.init_function(); + } +} + +bool Fmac::configure_iir(std::span feed_forward, std::span feedback) { + this->initialized = false; + + if (feed_forward.empty() or feedback.empty() or feed_forward.size() + feedback.size() > buffer_size or + this->handle->State != HAL_FMAC_STATE_READY) { + return false; + } + + host::FmacPort& port = host::Board::fmac(this->handle); + port.touched = true; + port.configure(feed_forward, feedback); + this->output_buffer_size = static_cast(this->output_buffer.size()); + + this->initialized = true; + return true; +} + +int16_t Fmac::update(int16_t sample) { + this->output_buffer.front() = host::Board::fmac(this->handle).filter(sample); + return this->output_buffer.front(); +} + +bool Fmac::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/gpio.cpp b/micras_hal/host/src/gpio.cpp new file mode 100644 index 0000000..9c868c5 --- /dev/null +++ b/micras_hal/host/src/gpio.cpp @@ -0,0 +1,27 @@ +/** + * @file + */ + +#include "micras/hal/gpio.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" + +namespace micras::hal { +Gpio::Gpio(const Config& config) : port{config.port}, pin{config.pin} { } + +bool Gpio::read() const { + host::GpioPort& gpio = host::Board::gpio(this->port, this->pin); + gpio.touched = true; + return gpio.read(); +} + +void Gpio::write(bool state) { + host::GpioPort& gpio = host::Board::gpio(this->port, this->pin); + gpio.touched = true; + gpio.output = state; +} + +void Gpio::toggle() { + this->write(not host::Board::gpio(this->port, this->pin).output); +} +} // namespace micras::hal diff --git a/micras_hal/host/src/mcu.cpp b/micras_hal/host/src/mcu.cpp new file mode 100644 index 0000000..d896680 --- /dev/null +++ b/micras_hal/host/src/mcu.cpp @@ -0,0 +1,88 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "micras/hal/gpio.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/clock.hpp" +#include "micras/hal/mcu.hpp" +#include "micras/hal/pwm.hpp" +#include "micras/hal/timer.hpp" + +namespace micras::hal { +void Mcu::init(const Config& config) { + config.clock_init(); + + if (config.peripheral_clock_init != nullptr) { + config.peripheral_clock_init(); + } + + Timer::init(); + + for (const InitFunction init_function : config.peripheral_inits) { + init_function(); + } + + host::Board::mcu().touched = true; +} + +void Mcu::emergency_stop(std::span pwm_outputs, std::span enable_gpios) { + for (const auto& pwm_output : pwm_outputs) { + host::Board::pwm(pwm_output.handle, pwm_output.timer_channel).duty_cycle = 0.0F; + } + + for (const auto& enable_gpio : enable_gpios) { + host::Board::gpio(enable_gpio.port, enable_gpio.pin).output = false; + } + + host::McuPort& mcu = host::Board::mcu(); + mcu.emergency_stops++; + + const host::Clock& clock = host::Clock::instance(); + std::array message{}; + const auto result = std::format_to_n( + message.data(), static_cast(message.size()), "firmware emergency stop at step {} ({:.6f} s)\n", + clock.steps(), static_cast(clock.now()) / (1e6 * clock.cycles_per_microsecond()) + ); + const auto length = std::min(static_cast(result.size), message.size()); + const ssize_t written = ::write(STDERR_FILENO, message.data(), length); + static_cast(written); +} + +void Mcu::set_watchdog_timeout(uint32_t timeout_ms) { + host::McuPort& mcu = host::Board::mcu(); + mcu.touched = true; + mcu.watchdog_timeout_ms = timeout_ms; + mcu.last_refresh = host::Clock::instance().now(); +} + +void Mcu::refresh_watchdog() { + host::McuPort& mcu = host::Board::mcu(); + const host::Clock& clock = host::Clock::instance(); + const uint64_t timeout = static_cast(mcu.watchdog_timeout_ms) * 1000 * clock.cycles_per_microsecond(); + + if (mcu.watchdog_timeout_ms != 0 and clock.now() - mcu.last_refresh > timeout) { + mcu.watchdog_expiries++; + } + + mcu.last_refresh = clock.now(); +} + +bool Mcu::was_reset_by_watchdog() { + return false; +} + +bool Mcu::is_cpu_frequency_supported() { + return true; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/ports.cpp b/micras_hal/host/src/ports.cpp new file mode 100644 index 0000000..71c4453 --- /dev/null +++ b/micras_hal/host/src/ports.cpp @@ -0,0 +1,79 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/hal/host/ports.hpp" + +namespace micras::hal::host { +void AdcPort::write(std::size_t index, uint32_t counts) const { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) bounded just above + if (index < this->buffer16.size()) { + this->buffer16[index] = static_cast(counts); + } else if (index < this->buffer32.size()) { + this->buffer32[index] = counts; + } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) +} + +void AdcPort::finish_sequence() const { + if (this->complete) { + this->complete(); + } +} + +void UartPort::receive(uint8_t byte) { + if (this->rx_buffer.empty()) { + return; + } + + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) the head wraps below the size + this->rx_buffer[this->rx_head] = byte; + this->rx_head = (this->rx_head + 1) % this->rx_buffer.size(); +} + +void FmacPort::configure( + std::span feed_forward_coefficients, std::span feedback_coefficients +) { + this->feed_forward.assign(feed_forward_coefficients.begin(), feed_forward_coefficients.end()); + this->feedback.assign(feedback_coefficients.begin(), feedback_coefficients.end()); + this->inputs.assign(this->feed_forward.size(), 0); + this->outputs.assign(this->feedback.size(), 0); +} + +int16_t FmacPort::filter(int16_t sample) { + if (this->feed_forward.empty()) { + return 0; + } + + this->inputs.pop_back(); + this->inputs.push_front(sample); + + int64_t accumulator = 0; + + for (std::size_t tap = 0; tap < this->feed_forward.size(); tap++) { + accumulator += static_cast(this->feed_forward.at(tap)) * this->inputs.at(tap); + } + + for (std::size_t tap = 0; tap < this->feedback.size(); tap++) { + accumulator += static_cast(this->feedback.at(tap)) * this->outputs.at(tap); + } + + constexpr int64_t fractional_bits{15}; + const auto output = static_cast(std::clamp( + accumulator >> fractional_bits, std::numeric_limits::min(), std::numeric_limits::max() + )); + + if (not this->outputs.empty()) { + this->outputs.pop_back(); + this->outputs.push_front(output); + } + + return output; +} +} // namespace micras::hal::host diff --git a/micras_hal/host/src/pwm.cpp b/micras_hal/host/src/pwm.cpp new file mode 100644 index 0000000..b3fb92b --- /dev/null +++ b/micras_hal/host/src/pwm.cpp @@ -0,0 +1,88 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" +#include "micras/hal/pwm.hpp" + +namespace micras::hal { +namespace { +/** + * @brief Mask of the channel offset, which the vendor HAL uses as a bit shift. + */ +constexpr uint32_t channel_shift_mask{0x1F}; +} // namespace + +/** + * @brief Check whether a timer counts up and down. + * + * @param handle Timer handle. + * @return True in a center-aligned mode. + */ +static bool is_center_aligned(const TIM_HandleTypeDef* handle) { + return (handle->Instance->CR1 & TIM_CR1_CMS) != 0; +} + +Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_channel}, inverted{config.inverted} { + if (this->handle->State == HAL_TIM_STATE_RESET) { + config.init_function(); + } + + if (this->inverted) { + this->handle->Instance->CCER |= TIM_CCER_CC1P << (this->channel & channel_shift_mask); + } + + this->set_duty_cycle(0.0F); + this->initialized = this->handle->State == HAL_TIM_STATE_READY; + + host::PwmPort& port = host::Board::pwm(this->handle, this->channel); + port.frequency = this->get_frequency(); +} + +void Pwm::set_duty_cycle(float duty_cycle) { + duty_cycle = std::clamp(duty_cycle, 0.0F, 100.0F); + + if (this->inverted) { + duty_cycle = 100.0F - duty_cycle; + } + + const uint32_t autoreload = this->handle->Instance->ARR; + const float scaled = duty_cycle * static_cast(autoreload + 1) * 0.01F; + + // NOLINTNEXTLINE(bugprone-incorrect-roundings): the rounding the firmware's own driver does. + const auto compare = static_cast(scaled + 0.5F); + const float counts = + is_center_aligned(this->handle) ? static_cast(autoreload) : static_cast(autoreload + 1); + const float active = std::min(100.0F, 100.0F * static_cast(compare) / counts); + + host::PwmPort& port = host::Board::pwm(this->handle, this->channel); + port.touched = true; + port.duty_cycle = this->inverted ? 100.0F - active : active; +} + +void Pwm::set_frequency(uint32_t frequency) { + const uint32_t prescaler = this->handle->Instance->PSC; + + this->handle->Instance->ARR = this->handle->Instance->kernel_clock / ((prescaler + 1) * frequency) - 1; + + host::PwmPort& port = host::Board::pwm(this->handle, this->channel); + port.touched = true; + port.frequency = this->get_frequency(); +} + +float Pwm::get_frequency() const { + const uint32_t autoreload = this->handle->Instance->ARR; + const uint32_t period = is_center_aligned(this->handle) ? 2 * autoreload : autoreload + 1; + const auto ticks = static_cast(this->handle->Instance->PSC + 1) * static_cast(period); + + return static_cast(this->handle->Instance->kernel_clock) / ticks; +} + +bool Pwm::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/pwm_dma.cpp b/micras_hal/host/src/pwm_dma.cpp new file mode 100644 index 0000000..d60b547 --- /dev/null +++ b/micras_hal/host/src/pwm_dma.cpp @@ -0,0 +1,84 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/clock.hpp" +#include "micras/hal/host/ports.hpp" +#include "micras/hal/pwm_dma.hpp" + +namespace micras::hal { +/** + * @brief Start a transfer of compare values. + * + * @param handle Timer handle. + * @param channel Timer channel. + * @param compares Values the DMA feeds the compare register with. + */ +template +static void start(TIM_HandleTypeDef* handle, uint32_t channel, std::span compares) { + host::PwmDmaPort& port = host::Board::pwm_dma(handle, channel); + port.touched = true; + port.compares.assign(compares.begin(), compares.end()); + port.period = handle->Instance->ARR + 1; + port.busy = true; + + const uint64_t cycles_per_count = static_cast(handle->Instance->PSC + 1) * + host::Clock::instance().cycles_per_microsecond() * 1000000ULL / + handle->Instance->kernel_clock; + + port.transfer_end = host::Clock::instance().now() + compares.size() * port.period * cycles_per_count; +} + +PwmDma::PwmDma(const Config& config) : handle{config.handle}, channel{config.timer_channel} { + if (this->handle->State == HAL_TIM_STATE_RESET) { + config.init_function(); + } + + this->initialized = this->handle->State == HAL_TIM_STATE_READY; +} + +void PwmDma::start_dma(std::span buffer) { + if (this->is_busy()) { + return; + } + + start(this->handle, this->channel, buffer); +} + +void PwmDma::start_dma(std::span buffer) { + if (this->is_busy()) { + return; + } + + start(this->handle, this->channel, buffer); +} + +void PwmDma::stop_dma() { + host::Board::pwm_dma(this->handle, this->channel).busy = false; +} + +uint32_t PwmDma::get_compare(float duty_cycle) const { + const float scaled = duty_cycle * static_cast(this->handle->Instance->ARR + 1) * 0.01F; + + // NOLINTNEXTLINE(bugprone-incorrect-roundings): the rounding the firmware's own driver does. + return static_cast(scaled + 0.5F); +} + +bool PwmDma::is_busy() { + host::PwmDmaPort& port = host::Board::pwm_dma(this->handle, this->channel); + + if (port.busy and host::Clock::instance().now() >= port.transfer_end) { + port.busy = false; + } + + return port.busy; +} + +bool PwmDma::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/spi.cpp b/micras_hal/host/src/spi.cpp new file mode 100644 index 0000000..9c5d1fb --- /dev/null +++ b/micras_hal/host/src/spi.cpp @@ -0,0 +1,258 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "micras/hal/gpio.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/clock.hpp" +#include "micras/hal/host/ports.hpp" +#include "micras/hal/host/spi_device.hpp" +#include "micras/hal/spi.hpp" +#include "micras/hal/timer.hpp" + +namespace micras::hal { +namespace { +/** + * @brief What the host keeps for each Spi that the firmware's class has no member for. + */ +struct HostState { + /** + * @brief Chip select of the device, which keys its port together with the handle. + */ + Gpio::Config cs{}; + + /** + * @brief Host clock cycle the running transfer ends at. + */ + uint64_t transfer_end{0}; +}; +} // namespace + +/** + * @brief Get the host state of every Spi alive. + * + * @return The states, keyed by the object. + */ +static std::map& host_states() { + static std::map states; + return states; +} + +/** + * @brief Get the port of the chip select an Spi drives. + * + * @param spi The Spi. + * @param handle Its handle. + * @return The port. + */ +static host::SpiPort& port_of(const Spi* spi, const SPI_HandleTypeDef* handle) { + const Gpio::Config& cs = host_states().at(spi).cs; + return host::Board::spi(handle, cs.port, cs.pin); +} + +/** + * @brief Get the SPI mode the bus runs in, from the polarity and phase its handle holds. + * + * @param handle Handle of the bus. + * @return The mode. + */ +static host::SpiDevice::Mode mode_of(const SPI_HandleTypeDef& handle) { + const bool polarity = handle.Init.CLKPolarity == SPI_POLARITY_HIGH; + const bool phase = handle.Init.CLKPhase == SPI_PHASE_2EDGE; + return static_cast((polarity ? 2 : 0) + (phase ? 1 : 0)); +} + +/** + * @brief Clock bytes through the device a port selects. + * + * @param handle Handle of the bus. + * @param port Port of the chip select. + * @param transmitted Bytes sent. + * @param received Bytes answered, as many as were sent; all ones when no device drives the bus in its mode. + */ +static void exchange( + const SPI_HandleTypeDef& handle, host::SpiPort& port, std::span transmitted, + std::span received +) { + if (not port.selected or port.device == nullptr or port.device->mode() != mode_of(handle)) { + std::ranges::fill(received, 0xFF); + return; + } + + port.device->exchange(transmitted, received); +} + +/** + * @brief Get how long a transfer takes on a bus. + * + * @param handle Handle of the bus. + * @param bytes Bytes transferred. + * @return Host clock cycles, zero when the bus has no clock to time it by. + */ +static uint64_t transfer_cycles(const SPI_HandleTypeDef& handle, std::size_t bytes) { + if (handle.Instance == nullptr or handle.Instance->kernel_clock == 0) { + return 0; + } + + const uint64_t kernel_clock = handle.Instance->kernel_clock; + const uint64_t divider = 2ULL << (handle.Init.BaudRatePrescaler >> 28U); + const uint64_t cycles_per_second = host::Clock::instance().cycles_per_microsecond() * 1000000ULL; + + return ((bytes * 8 * divider * cycles_per_second) + kernel_clock - 1) / kernel_clock; +} + +std::array Spi::transferring{}; + +Spi::Spi(const Config& config) : + handle{config.handle}, + cs_gpio{config.cs_gpio}, + timeout{config.timeout}, + clock_polarity{config.clock_polarity}, + clock_phase{config.clock_phase} { + host_states()[this] = HostState{.cs = config.cs_gpio}; + this->unselect_device(); + + if (this->handle->State == HAL_SPI_STATE_RESET) { + config.init_function(); + } + + this->initialized = this->handle->State == HAL_SPI_STATE_READY; +} + +Spi::~Spi() { + if (this->transfer == Transfer::RUNNING) { + this->unselect_device(); + } + + std::ranges::replace(transferring, this, static_cast(nullptr)); + host_states().erase(this); +} + +bool Spi::select_device() { + const auto bus_busy = [this] { + return std::ranges::any_of(transferring, [this](const Spi* device) { + return device != nullptr and device->handle == this->handle and device->get_transfer() == Transfer::RUNNING; + }); + }; + + if (bus_busy()) { + const uint32_t start = Timer::get_counter(); + const uint32_t limit = Timer::to_cycles(1000 * this->timeout); + + while (bus_busy()) { + if (Timer::get_counter() - start > limit) { + return false; + } + } + } + + this->handle->Init.CLKPolarity = this->clock_polarity; + this->handle->Init.CLKPhase = this->clock_phase; + this->cs_gpio.write(false); + + host::SpiPort& port = port_of(this, this->handle); + port.touched = true; + + if (not port.selected) { + port.selected = true; + + if (port.device != nullptr) { + port.device->select(); + } + } + + return true; +} + +void Spi::unselect_device() { + this->cs_gpio.write(true); + + host::SpiPort& port = port_of(this, this->handle); + + if (port.selected) { + port.selected = false; + + if (port.device != nullptr) { + port.device->deselect(); + } + } +} + +bool Spi::transmit(std::span data) { + if (this->handle->State != HAL_SPI_STATE_READY) { + return false; + } + + std::vector ignored(data.size()); + exchange(*this->handle, port_of(this, this->handle), data, ignored); + return true; +} + +bool Spi::receive(std::span data) { + if (this->handle->State != HAL_SPI_STATE_READY) { + return false; + } + + const std::vector zeros(data.size()); + exchange(*this->handle, port_of(this, this->handle), zeros, data); + return true; +} + +bool Spi::transmit_receive(std::span transmitted, std::span received) { + if (received.size() < transmitted.size() or this->handle->State != HAL_SPI_STATE_READY) { + return false; + } + + exchange(*this->handle, port_of(this, this->handle), transmitted, received.first(transmitted.size())); + return true; +} + +bool Spi::start_transfer(std::span transmitted, std::span received) { + auto* const slot = std::ranges::find(transferring, nullptr); + + if (received.size() < transmitted.size() or slot == transferring.end() or + this->handle->State != HAL_SPI_STATE_READY or not this->select_device()) { + this->transfer = Transfer::FAILED; + return false; + } + + *slot = this; + this->transfer = Transfer::RUNNING; + exchange(*this->handle, port_of(this, this->handle), transmitted, received.first(transmitted.size())); + host_states().at(this).transfer_end = + host::Clock::instance().now() + transfer_cycles(*this->handle, transmitted.size()); + + return true; +} + +Spi::Transfer Spi::get_transfer() const { + if (this->transfer == Transfer::RUNNING and host::Clock::instance().now() >= host_states().at(this).transfer_end) { + on_transfer_end(this->handle, true); + } + + return this->transfer; +} + +void Spi::on_transfer_end(const SPI_HandleTypeDef* handle, bool succeeded) { + for (Spi*& device : transferring) { + if (device != nullptr and device->handle == handle) { + device->unselect_device(); + device->transfer = succeeded ? Transfer::COMPLETE : Transfer::FAILED; + device = nullptr; + return; + } + } +} + +bool Spi::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/timer.cpp b/micras_hal/host/src/timer.cpp new file mode 100644 index 0000000..27187a9 --- /dev/null +++ b/micras_hal/host/src/timer.cpp @@ -0,0 +1,34 @@ +/** + * @file + */ + +#include + +#include + +#include "micras/hal/host/clock.hpp" +#include "micras/hal/timer.hpp" + +namespace micras::hal { +uint32_t Timer::cycles_per_microsecond{1}; + +void Timer::init() { + cycles_per_microsecond = SystemCoreClock / 1000000; +} + +uint32_t Timer::get_counter() { + return host::Clock::instance().read_cycles(); +} + +uint32_t Timer::get_counter_ms() { + return host::Clock::instance().read_ms(); +} + +uint32_t Timer::to_microseconds(uint32_t cycles) { + return cycles / cycles_per_microsecond; +} + +uint32_t Timer::to_cycles(uint32_t microseconds) { + return microseconds * cycles_per_microsecond; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/uart_dma.cpp b/micras_hal/host/src/uart_dma.cpp new file mode 100644 index 0000000..d29a883 --- /dev/null +++ b/micras_hal/host/src/uart_dma.cpp @@ -0,0 +1,81 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" +#include "micras/hal/uart_dma.hpp" + +namespace micras::hal { +UartDma::UartDma(const Config& config) : handle{config.handle} { + if (this->handle->gState == HAL_UART_STATE_RESET) { + config.init_function(); + } +} + +bool UartDma::start_rx(std::span buffer) { + host::UartPort& port = host::Board::uart(this->handle); + port.touched = true; + port.rx_buffer = buffer; + port.rx_head = 0; + + this->rx_buffer = buffer; + this->rx_tail = 0; + this->handle->RxState = HAL_UART_STATE_BUSY_RX; + this->initialized = not buffer.empty(); + + return this->initialized; +} + +std::size_t UartDma::rx_head() const { + return host::Board::uart(this->handle).rx_head; +} + +std::size_t UartDma::read(std::span into) { + if (this->rx_buffer.empty()) { + return 0; + } + + const std::size_t size = this->rx_buffer.size(); + const std::size_t available = (this->rx_head() + size - this->rx_tail) % size; + const std::size_t taken = std::min(available, into.size()); + const std::size_t first = std::min(taken, size - this->rx_tail); + + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access): bounded just above. + std::memcpy(into.data(), &this->rx_buffer[this->rx_tail], first); + + if (first < taken) { + std::memcpy(&into[first], this->rx_buffer.data(), taken - first); + } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + + this->rx_tail = (this->rx_tail + taken) % size; + + return taken; +} + +bool UartDma::start_tx(std::span from) { + if (from.empty() or this->is_transmitting()) { + return false; + } + + host::UartPort& port = host::Board::uart(this->handle); + port.touched = true; + port.tx.insert(port.tx.end(), from.begin(), from.end()); + return true; +} + +bool UartDma::is_transmitting() const { + return host::Board::uart(this->handle).transmitting(); +} + +bool UartDma::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp new file mode 100644 index 0000000..c73b82a --- /dev/null +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -0,0 +1,254 @@ +/** + * @file + */ + +#ifndef MICRAS_HAL_ADC_DMA_HPP +#define MICRAS_HAL_ADC_DMA_HPP + +#include +#include +#include +#include + +namespace micras::hal { +/** + * @brief Class to handle ADC peripheral on STM32 microcontrollers using DMA. + * + * @note The stm32 backend defines HAL_ADC_ConvCpltCallback and HAL_ADC_ErrorCallback, so a consumer + * must not define them, and must keep USE_HAL_ADC_REGISTER_CALLBACKS at 0. They win over the vendor's + * weak defaults because they sit in the source of this class's constructor, which every program that + * uses the class references. + */ +class AdcDma { +public: + /** + * @brief Configuration struct for ADC DMA. + * + * @note The reference voltage is a property of the board, not of the silicon, so it is + * configured per instance next to the resolution instead of being fixed by this class. + */ + struct Config { + void (*init_function)(); + ADC_HandleTypeDef* handle; + uint16_t max_reading; + float reference_voltage; + }; + + /** + * @brief Construct a new AdcDma object. + * + * @note The object registers itself for the interrupts to find it, and a converter that finds + * no free place is not initialized. + * + * @param config ADC DMA configuration struct. + */ + explicit AdcDma(const Config& config); + + /** + * @brief Special member functions, of which only the destructor exists. + * + * @note The interrupt finds a converter by its address, so an object cannot be copied or moved. + */ + ///@{ + ~AdcDma(); + AdcDma(const AdcDma&) = delete; + AdcDma(AdcDma&&) = delete; + AdcDma& operator=(const AdcDma&) = delete; + AdcDma& operator=(AdcDma&&) = delete; + ///@} + + /** + * @brief Enable ADC, start conversion of regular group and transfer result through DMA. + * + * @param buffer 32 bit destination buffer address. + * @return True if the conversion was started, false otherwise. + */ + bool start_dma(std::span buffer); + + /** + * @brief Enable ADC, start conversion of regular group and transfer result through DMA. + * + * @param buffer 16 bit destination buffer address. + * @return True if the conversion was started, false otherwise. + */ + bool start_dma(std::span buffer); + + /** + * @brief Start the conversions as start_dma does, keeping a copy of every complete sequence. + * + * @details The DMA rewrites the buffer continuously, so reading it while it is being written + * can mix values of two different sequences. Every time the DMA reaches the end of the buffer, + * the transfer complete interrupt copies it to the snapshot, which is therefore always one whole + * sequence. The copy is a few words long, so the interrupt costs a few tens of cycles. + * + * @note Both buffers are borrowed and have to outlive the conversions. + * + * @param buffer 16 bit destination buffer of the DMA. + * @param snapshot Buffer of the same size that receives the copies. + * @return True if the conversion was started, false otherwise. + */ + bool start_dma(std::span buffer, std::span snapshot); + + /** + * @brief Read the last complete sequence. + * + * @note The snapshot may be replaced while it is being read, which is detected with the + * sequence counter and answered by reading it again. The counter is volatile but the snapshot + * is not, so the copy is fenced on both sides to keep the compiler from moving it past either + * read of the counter. + * + * @param destination Buffer of the size of the snapshot that receives it, of which a smaller + * one receives only what fits. + * @return The number of sequences completed so far, which tells whether this one is new. + */ + uint32_t read_snapshot(std::span destination) const; + + /** + * @brief Stop ADC conversion of regular group (and injected group in case of auto_injection mode). + */ + void stop_dma(); + + /** + * @brief Restart the conversions if an error stopped them. + * + * @details After an overrun the converter stops requesting transfers until its overrun flag is + * cleared, and clearing only the flag would leave one conversion missing and every following + * value in the place of another channel. A restart begins the buffer again at its first + * channel. A converter is restarted when the error callback or the snapshot marked it, and also + * when its overrun flag is set or its DMA stopped while the vendor HAL still holds the transfer + * as running, which is how an overrun or a transfer error shows when the interrupts of the + * converter and of its DMA are disabled. A transfer error leaves the vendor HAL in an error + * state that only a new initialization clears and that turns every complete transfer into + * another error, so that state is cleared before the start. + * + * @note To be called once per iteration by the owner of the converter. No sequence completes + * until then, so read_snapshot reports none as new. + */ + void recover(); + + /** + * @brief Get the maximum reading of the ADC. + * + * @return Maximum reading of the ADC. + */ + uint16_t get_max_reading() const; + + /** + * @brief Get the reference voltage of the ADC measurement. + * + * @return Reference voltage in volts. + */ + float get_reference_voltage() const; + + /** + * @brief Check if the ADC was calibrated and started successfully. + * + * @note A failed calibration or start leaves the DMA buffer at zero, which reads exactly like + * a real measurement, so this is the only way to tell one from the other. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + + /** + * @brief Get the number of times any converter was restarted. + * + * @note Anything but zero means that a converter stopped, and how often. + * + * @return Number of restarts since power on. + */ + static uint32_t get_restarts(); + + /** + * @brief Count a complete sequence of a converter and copy its buffer to its snapshot. + * + * @note To be called by the conversion complete callback only. An overrun drops a conversion, + * and the transfers go on out of line with the sequence until the converter is restarted, so + * nothing is counted once one happened: the converter is marked stopped here when the overrun + * flag is set, without waiting for the overrun interrupt or for recover to notice it. + * + * @param handle Handle of the converter that completed a sequence. + */ + static void on_sequence_complete(const ADC_HandleTypeDef* handle); + + /** + * @brief Mark a converter that an error stopped, for recover to restart it. + * + * @note To be called by the error callback only. + * + * @param handle Handle of the converter that stopped. + */ + static void on_error(const ADC_HandleTypeDef* handle); + +private: + /** + * @brief Find the object of a converter. + * + * @param handle Handle of the converter. + * @return Object of the converter, or nullptr if none was constructed for it. + */ + static AdcDma* find(const ADC_HandleTypeDef* handle); + + /** + * @brief Largest number of converters. + */ + static constexpr uint8_t max_instances{4}; + + /** + * @brief Every converter, for the interrupts to find the one that completed or stopped. + */ + static std::array instances; + + /** + * @brief Number of times any converter was restarted. + */ + static uint32_t restarts; + + /** + * @brief Maximum ADC reading. + */ + uint16_t max_reading; + + /** + * @brief Reference voltage for the ADC measurement. + */ + float reference_voltage; + + /** + * @brief ADC handle. + */ + ADC_HandleTypeDef* handle; + + /** + * @brief Destination of the DMA as the vendor HAL takes it, whose size is the number of transfers. + */ + std::span transfer; + + /** + * @brief Destination buffer of the DMA, of a converter that keeps a snapshot. + */ + std::span buffer; + + /** + * @brief Copy of the last complete sequence. + */ + std::span snapshot; + + /** + * @brief Number of sequences completed so far. + */ + volatile uint32_t sequence{}; + + /** + * @brief Flag set by the error callback when an error stopped the converter. + */ + volatile bool stopped{}; + + /** + * @brief Flag to check if the ADC was calibrated and started. + */ + bool initialized{}; +}; +} // namespace micras::hal + +#endif // MICRAS_HAL_ADC_DMA_HPP diff --git a/micras_hal/include/micras/hal/crc.hpp b/micras_hal/include/micras/hal/crc.hpp new file mode 100644 index 0000000..c7e2dcb --- /dev/null +++ b/micras_hal/include/micras/hal/crc.hpp @@ -0,0 +1,51 @@ +/** + * @file + */ + +#ifndef MICRAS_HAL_CRC_HPP +#define MICRAS_HAL_CRC_HPP + +#include +#include +#include + +namespace micras::hal { +/** + * @brief Class to handle the cyclic redundancy check peripheral on STM32 microcontrollers. + */ +class Crc { +public: + /** + * @brief CRC configuration struct. + */ + struct Config { + CRC_HandleTypeDef* handle; + }; + + /** + * @brief Construct a new Crc object. + * + * @param config Configuration for the CRC. + */ + explicit Crc(const Config& config); + + /** + * @brief Calculate the CRC value over a buffer of bytes. + * + * @note The polynomial, width and initial value come from the peripheral configuration, and the + * calculation unit is reset to that initial value on every call. + * + * @param data Data to calculate the CRC over. + * @return CRC value. + */ + uint32_t calculate(std::span data); + +private: + /** + * @brief CRC handle. + */ + CRC_HandleTypeDef* handle; +}; +} // namespace micras::hal + +#endif // MICRAS_HAL_CRC_HPP diff --git a/micras_hal/include/micras/hal/encoder.hpp b/micras_hal/include/micras/hal/encoder.hpp new file mode 100644 index 0000000..892e096 --- /dev/null +++ b/micras_hal/include/micras/hal/encoder.hpp @@ -0,0 +1,64 @@ +/** + * @file + */ + +#ifndef MICRAS_HAL_ENCODER_HPP +#define MICRAS_HAL_ENCODER_HPP + +#include + +namespace micras::hal { +/** + * @brief Class to handle encoder peripheral on STM32 microcontrollers. + */ +class Encoder { +public: + /** + * @brief Encoder configuration struct. + */ + struct Config { + void (*init_function)(); + TIM_HandleTypeDef* handle; + uint32_t timer_channel; + }; + + /** + * @brief Construct a new Encoder object. + * + * @param config Configuration for the encoder. + */ + explicit Encoder(const Config& config); + + /** + * @brief Get the counter value. + * + * @return Current value of the counter. + */ + int32_t get_counter() const; + + /** + * @brief Check if the encoder was successfully started. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief Timer handle. + */ + TIM_HandleTypeDef* handle; + + /** + * @brief Start value of the timer counter. + */ + uint32_t start_count; + + /** + * @brief Flag to check if the encoder was started. + */ + bool initialized{}; +}; +} // namespace micras::hal + +#endif // MICRAS_HAL_ENCODER_HPP diff --git a/micras_hal/include/micras/hal/family.hpp b/micras_hal/include/micras/hal/family.hpp new file mode 100644 index 0000000..6563654 --- /dev/null +++ b/micras_hal/include/micras/hal/family.hpp @@ -0,0 +1,98 @@ +/** + * @file + * + * @brief The steps of the shared HAL classes that differ between chip families. + * + * @note Internal to micras_hal: each backend implements these functions once, the stm32 backend in the + * sources of the family CMake selects and the host backend in its own. Nothing here names a chip, so + * the shared classes call them without any conditional compilation. + */ + +#ifndef MICRAS_HAL_FAMILY_HPP +#define MICRAS_HAL_FAMILY_HPP + +#include +#include + +namespace micras::hal::family { +/** + * @brief Calibrate a converter for single-ended conversions. + * + * @param handle Handle of the converter, initialized and not enabled. + * @return True if the calibration succeeded, false otherwise. + */ +bool calibrate_adc(ADC_HandleTypeDef* handle); + +/** + * @brief Check whether the stream or channel of a DMA is enabled. + * + * @note A transfer error disables the stream or channel in hardware, so a transfer the vendor HAL + * still holds as busy and whose stream or channel is disabled has stopped, whether or not its + * interrupt is enabled to report it. + * + * @param handle Handle of the DMA, initialized. + * @return True if the stream or channel is enabled, false otherwise. + */ +bool is_dma_enabled(const DMA_HandleTypeDef* handle); + +/** + * @brief Check whether a transfer of a DMA in normal mode is over. + * + * @note A stream of the DMA controllers of the STM32H7 clears its enable bit once it moved its last + * item, while a channel of its BDMA or of the STM32G4 keeps it set and only its counter reaches zero. + * A transfer error disables either, which ends the transfer as well. + * + * @param handle Handle of the DMA, running a transfer started in normal mode. + * @return True if the DMA moved its last item or an error stopped it, false while items are left. + */ +bool has_dma_finished(const DMA_HandleTypeDef* handle); + +/** + * @brief Enable the caches the family runs its code from, before the vendor HAL is initialized. + */ +void enable_caches(); + +/** + * @brief Read from the reset flags whether the last reset came from the independent watchdog. + * + * @note The flags are read before they are cleared, so this is called once at startup. + * + * @return True if the independent watchdog reset the microcontroller, false otherwise. + */ +bool was_reset_by_watchdog(); + +/** + * @brief Stop the independent watchdog while the core is halted by a debugger. + */ +void freeze_watchdog_in_debug(); + +/** + * @brief Check whether the part can run at the core frequency the board asks for. + * + * @param boost Whether the board runs the core above the frequency the part allows by default. + * @return True if the part supports the requested frequency, false otherwise. + */ +bool is_cpu_frequency_supported(bool boost); + +/** + * @brief Get the registers of the independent watchdog. + * + * @return Registers of the independent watchdog. + */ +IWDG_TypeDef* watchdog(); + +/** + * @brief Get the frequency of the clock that feeds a timer. + * + * @param instance Timer peripheral. + * @return Timer clock frequency in Hz. + */ +uint32_t timer_clock(const TIM_TypeDef* instance); + +/** + * @brief Start the cycle counter of the core from zero. + */ +void enable_cycle_counter(); +} // namespace micras::hal::family + +#endif // MICRAS_HAL_FAMILY_HPP diff --git a/micras_hal/include/micras/hal/flash.hpp b/micras_hal/include/micras/hal/flash.hpp new file mode 100644 index 0000000..736ac4c --- /dev/null +++ b/micras_hal/include/micras/hal/flash.hpp @@ -0,0 +1,171 @@ +/** + * @file + */ + +#ifndef MICRAS_HAL_FLASH_HPP +#define MICRAS_HAL_FLASH_HPP + +#include +#include +#include + +#include "micras/hal/family/flash.hpp" + +namespace micras::hal { +/** + * @brief Class to handle the smallest unit of data that can be programmed into the flash memory. + * + * @note The buffer of the caller is used directly whenever it already covers a whole flash word, so only a + * trailing incomplete flash word is copied to the internal buffer and padded. + */ +class FlashWord { +public: + /** + * @brief Number of 32 bit words inside a flash word. + */ + static constexpr uint32_t words{family::flash_word_bits / 32U}; + + /** + * @brief Number of bytes inside a flash word. + */ + static constexpr uint32_t size{words * sizeof(uint32_t)}; + + /** + * @brief Value of every byte of an erased flash memory. + */ + static constexpr uint8_t erased_value{0xFF}; + + /** + * @brief Construct a new Flash Word object from the beginning of a buffer. + * + * @param data Buffer to take the data from, padded with the erased value if smaller than a flash word. + */ + explicit FlashWord(std::span data); + + /** + * @brief Get the data ready to be programmed into the flash memory. + * + * @return Pointer to size bytes of data aligned to 32 bits. + */ + const uint32_t* data() const; + + /** + * @brief Check if the data had to be copied to the internal buffer. + * + * @return True if the data was padded or realigned, false otherwise. + */ + bool is_padded() const; + +private: + /** + * @brief Buffer used only when the data needs to be padded or realigned. + */ + std::array buffer{}; + + /** + * @brief Data to be programmed, pointing either to the buffer of the caller or to the internal one. + */ + const uint32_t* source{}; +}; + +/** + * @brief Class to handle flash memory on STM32 microcontrollers. + * + * @note Only the region of the flash memory reserved for data storage is accessible, and every address is + * relative to the beginning of that region. The family header micras/hal/family/flash.hpp gives the + * geometry: a sector is the unit the family erases, a 128 KB sector on the STM32H7 and a 2 KB page on + * the STM32G4. + */ +class Flash { +public: + /** + * @brief Enum for the status of a flash memory operation. + */ + enum class Status : uint8_t { + OK = 0, + MISALIGNED = 1, + OUT_OF_BOUNDS = 2, + ERROR = 3 + }; + + /** + * @brief Deleted constructor for static class. + */ + Flash() = delete; + + /** + * @brief Number of bytes of an erasable sector. + */ + static constexpr uint32_t sector_size{family::sector_size}; + + /** + * @brief Number of sectors reserved for data storage. + */ + static constexpr uint16_t total_sectors{family::storage_sectors}; + + /** + * @brief Number of bytes reserved for data storage. + */ + static constexpr uint32_t total_size{total_sectors * sector_size}; + + /** + * @brief Read data from the flash memory at an absolute address. + * + * @param address Address to read from in bytes, relative to the beginning of the reserved region. + * @param size Number of bytes to read. + * @return View over the flash memory, empty if the data is outside of the reserved region. + */ + static std::span read(uint32_t address, uint32_t size); + + /** + * @brief Read data from the flash memory at an address relative to a sector. + * + * @param sector Sector to read from, counting from the first reserved sector. + * @param sector_address Address inside the sector to read from in bytes. + * @param size Number of bytes to read. + * @return View over the flash memory, empty if the data is outside of the reserved region. + */ + static std::span read(uint16_t sector, uint32_t sector_address, uint32_t size); + + /** + * @brief Write data to the flash memory at an absolute address. + * + * @note The data is padded with the erased value up to the next flash word boundary, so the number of + * bytes actually written is the size of the data rounded up to a multiple of FlashWord::size. + * + * @param address Address to write to in bytes, must be a multiple of FlashWord::size. + * @param data Data to write. + * @return Status of the operation. + */ + static Status write(uint32_t address, std::span data); + + /** + * @brief Write data to the flash memory at an address relative to a sector. + * + * @note The data is padded with the erased value up to the next flash word boundary, so the number of + * bytes actually written is the size of the data rounded up to a multiple of FlashWord::size. + * + * @param sector Sector to write to, counting from the first reserved sector. + * @param sector_address Address inside the sector to write to in bytes, must be a multiple of + * FlashWord::size. + * @param data Data to write. + * @return Status of the operation. + */ + static Status write(uint16_t sector, uint32_t sector_address, std::span data); + + /** + * @brief Erase sectors of the flash memory. + * + * @note This operation blocks the processor, since the flash memory cannot be read while it is being + * erased: around 2 s per sector and up to 4 s in the worst case on the STM32H7, around 20 ms per page + * on the STM32G4. + * + * @param start_sector First sector to erase, counting from the first reserved sector. + * @param number_of_sectors Number of sectors to erase. + * @return Status of the operation. + */ + static Status erase_sectors(uint16_t start_sector, uint16_t number_of_sectors = 1); +}; +} // namespace micras::hal + +#endif // MICRAS_HAL_FLASH_HPP diff --git a/micras_hal/include/micras/hal/fmac.hpp b/micras_hal/include/micras/hal/fmac.hpp new file mode 100644 index 0000000..2e42757 --- /dev/null +++ b/micras_hal/include/micras/hal/fmac.hpp @@ -0,0 +1,113 @@ +/** + * @file + */ + +#ifndef MICRAS_HAL_FMAC_HPP +#define MICRAS_HAL_FMAC_HPP + +#include +#include +#include + +#include + +namespace micras::hal { +/** + * @brief Class to handle the filter math accelerator on STM32 microcontrollers. + * + * @note The accelerator works in q1.15 fixed point and runs one function at a time. Its local + * memory of 256 words can hold the coefficients of several filters, chosen by the base address of + * the coefficient buffer, and N signals that share a cutoff can be interleaved through one filter + * whose coefficients are spaced N taps apart. What it does not keep is the history of a filter + * across a change of configuration: stopping it resets the pointers into its buffers, so the last + * inputs and outputs have to be loaded again, which for one sample at a time costs more than the + * filter does. This wrapper uses none of that. It configures one filter and runs it sample by + * sample. + */ +class Fmac { +public: + /** + * @brief Configuration struct for the filter math accelerator. + */ + struct Config { + void (*init_function)(); + FMAC_HandleTypeDef* handle; + }; + + /** + * @brief Construct a new Fmac object. + * + * @param config Configuration for the filter math accelerator. + */ + explicit Fmac(const Config& config); + + /** + * @brief Configure the accelerator as an infinite impulse response filter in direct form 1. + * + * @param feed_forward Coefficients applied to the input samples, most recent first, in q1.15. + * @param feedback Coefficients applied to the previous outputs, most recent first, in q1.15, + * with the sign the accelerator expects, since it adds their contribution where the filter + * relation subtracts it. + * @return True if the filter was configured and started, false otherwise. + */ + bool configure_iir(std::span feed_forward, std::span feedback); + + /** + * @brief Push one sample through the configured filter and read the matching output. + * + * @note The data registers are accessed directly rather than through the streaming API of the + * vendor HAL, whose per call bookkeeping costs more than the filter it is driving. + * + * @param sample Input sample in q1.15. + * @return Filtered value in q1.15. + */ + int16_t update(int16_t sample); + + /** + * @brief Check if the accelerator holds a running filter. + * + * @return True if a filter was configured and started, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief Number of 16 bit words of the internal memory allocated to each buffer. + */ + static constexpr uint8_t buffer_size{8}; + + /** + * @brief Base addresses of the three buffers inside the internal memory. + */ + ///@{ + static constexpr uint8_t input_base_address{0}; + static constexpr uint8_t coefficient_base_address{buffer_size}; + static constexpr uint8_t output_base_address{2 * buffer_size}; + ///@} + + /** + * @brief Filter math accelerator handle. + */ + FMAC_HandleTypeDef* handle; + + /** + * @brief Output buffer registered with the vendor HAL when the filter is started. + * + * @note Never read, since update accesses the data register directly, but the HAL refuses to + * start a filter without one. + */ + std::array output_buffer{}; + + /** + * @brief Size of the output buffer, as the vendor HAL expects it by reference. + */ + uint16_t output_buffer_size{1}; + + /** + * @brief Flag to check if a filter is running. + */ + bool initialized{}; +}; +} // namespace micras::hal + +#endif // MICRAS_HAL_FMAC_HPP diff --git a/micras_hal/include/micras/hal/gpio.hpp b/micras_hal/include/micras/hal/gpio.hpp new file mode 100644 index 0000000..6ce5ba0 --- /dev/null +++ b/micras_hal/include/micras/hal/gpio.hpp @@ -0,0 +1,65 @@ +/** + * @file + */ + +#ifndef MICRAS_HAL_GPIO_HPP +#define MICRAS_HAL_GPIO_HPP + +#include + +#include + +namespace micras::hal { +/** + * @brief Class to handle GPIO pins on STM32 microcontrollers. + */ +class Gpio { +public: + /** + * @brief Configuration struct for GPIO pin. + */ + struct Config { + GPIO_TypeDef* port; + uint16_t pin; + }; + + /** + * @brief Construct a new Gpio object. + * + * @param config Configuration for the GPIO pin. + */ + explicit Gpio(const Config& config); + + /** + * @brief Read the current state of the GPIO pin. + * + * @return True if the current state of the GPIO pin is high, false otherwise. + */ + bool read() const; + + /** + * @brief Write a new state to the GPIO pin. + * + * @param pin_state The state to be written (true for high, false for low). + */ + void write(bool state); + + /** + * @brief Toggle the state of the GPIO pin. + */ + void toggle(); + +private: + /** + * @brief The port of the GPIO. + */ + GPIO_TypeDef* port; + + /** + * @brief The pin number of the GPIO. + */ + uint16_t pin; +}; +} // namespace micras::hal + +#endif // MICRAS_HAL_GPIO_HPP diff --git a/micras_hal/include/micras/hal/mcu.hpp b/micras_hal/include/micras/hal/mcu.hpp new file mode 100644 index 0000000..a48b33b --- /dev/null +++ b/micras_hal/include/micras/hal/mcu.hpp @@ -0,0 +1,130 @@ +/** + * @file + */ + +#ifndef MICRAS_HAL_MCU_HPP +#define MICRAS_HAL_MCU_HPP + +#include +#include + +#include "micras/hal/gpio.hpp" +#include "micras/hal/pwm.hpp" + +namespace micras::hal { +/** + * @brief Microcontroller unit class. + */ +class Mcu { +public: + /** + * @brief Signature of the initialization function of a peripheral. + */ + using InitFunction = void (*)(); + + /** + * @brief Configuration struct for the microcontroller. + * + * @note The initialization functions are taken rather than named, so that this package refers + * to no symbol the application generates. The two clock functions are separate fields because + * the order matters: the clock tree has to be running before the timebase is resolved from the + * core clock frequency, and before any peripheral is initialized. + * + * @note The core clock of some parts is only within their datasheet above a threshold, 520 MHz + * on the STM32H72x and STM32H73x, when an option byte says so, and that option byte comes + * cleared from the factory. A board whose clock tree needs it sets cpu_frequency_boost, the + * option byte is programmed once per chip with STM32CubeProgrammer, and a chip without it is + * reported by is_cpu_frequency_supported. + */ + struct Config { + InitFunction clock_init; + InitFunction peripheral_clock_init; + std::span peripheral_inits; + bool cpu_frequency_boost; + }; + + /** + * @brief Deleted constructor for static class. + */ + Mcu() = delete; + + /** + * @brief Initialize the microcontroller, the timebase and the peripherals that no wrapper owns. + * + * @note Every peripheral held by a wrapper is initialized by that wrapper instead, through the + * initialization function of its own configuration, so peripheral_inits carries only what is + * left over: the pin configuration, the DMA controllers and anything else without an owner. + * + * @note The watchdog is frozen while the core is halted by a debugger, since otherwise any + * breakpoint would reset the microcontroller as soon as the timeout ran out. Without a debugger + * the setting has no effect. + * + * @param config Initialization functions of the board, which may leave peripheral_clock_init + * null on a part where every peripheral runs from a bus clock. + */ + static void init(const Config& config); + + /** + * @brief Silence every actuator as directly as possible. + * + * @note Written to be callable from a fault or abort handler, so it touches only the timer and + * GPIO registers and relies on no object state. + * + * @param pwm_outputs Every PWM output to bring to a null duty cycle. + * @param enable_gpios Every driver enable pin to deassert. + */ + static void emergency_stop(std::span pwm_outputs, std::span enable_gpios); + + /** + * @brief Start the independent watchdog, or change the timeout of a running one. + * + * @note Once started the watchdog cannot be stopped by software, so anything that hangs for + * longer than the timeout resets the microcontroller, which brings every driver enable pin and + * every PWM output back to its reset state. The timeout can still be widened around an + * operation that legitimately stalls the core for longer than the control loop budget, such as + * erasing a flash sector. + * + * @param timeout_ms Time without a refresh that triggers a reset, in milliseconds. + */ + static void set_watchdog_timeout(uint32_t timeout_ms); + + /** + * @brief Refresh the independent watchdog. + */ + static void refresh_watchdog(); + + /** + * @brief Check whether the last reset was caused by the independent watchdog. + * + * @note The reset flags are read and cleared by init, so this describes the last reset only, + * and a watchdog reset followed by a reset from the button reads as a normal start. + * + * @return True if the watchdog reset the microcontroller, false otherwise. + */ + static bool was_reset_by_watchdog(); + + /** + * @brief Check whether the core runs within its datasheet. + * + * @note The option byte is only read, never programmed, so that nothing but a deliberate step on + * the bench changes it. + * + * @return False if the clock tree of the board needs the CPU frequency boost and the option byte + * that allows it is not set, true otherwise. + */ + static bool is_cpu_frequency_supported(); + +private: + /** + * @brief Whether the last reset was caused by the independent watchdog. + */ + static bool watchdog_reset; + + /** + * @brief Whether the core runs within its datasheet. + */ + static bool cpu_frequency_supported; +}; +} // namespace micras::hal + +#endif // MICRAS_HAL_MCU_HPP diff --git a/micras_hal/include/micras/hal/pwm.hpp b/micras_hal/include/micras/hal/pwm.hpp new file mode 100644 index 0000000..87c00ea --- /dev/null +++ b/micras_hal/include/micras/hal/pwm.hpp @@ -0,0 +1,104 @@ +/** + * @file + */ + +#ifndef MICRAS_HAL_PWM_HPP +#define MICRAS_HAL_PWM_HPP + +#include + +#include + +namespace micras::hal { +/** + * @brief Class to handle PWM peripheral on STM32 microcontrollers. + */ +class Pwm { +public: + /** + * @brief PWM configuration struct. + * + * @note An inverted output is active for the last part of the period instead of the first. On a + * center aligned timer that centers its pulse on the overflow instead of the underflow, which + * is how two groups of channels of one timer are made to take turns. The duty cycle keeps + * meaning the fraction of the period the output is active for, so zero is always off. + */ + struct Config { + void (*init_function)(); + TIM_HandleTypeDef* handle; + uint32_t timer_channel; + bool inverted; + }; + + /** + * @brief Construct a new Pwm object, with its output at a duty cycle of zero. + * + * @note The compare register is preloaded, so a value written to it waits for the next update + * of the timer. The first one is written around the preload: an inverted output would otherwise + * start fully on, since zero is what the register holds until then. + * + * @param config Configuration for the PWM. + */ + explicit Pwm(const Config& config); + + /** + * @brief Set the PWM duty cycle. + * + * @param duty_cycle Duty cycle value in percent, which is clamped to the range from 0 to 100. + */ + void set_duty_cycle(float duty_cycle); + + /** + * @brief Set the PWM frequency. + * + * @note Changing the pwm frequency will modify the autoreload and reset the counter, + * but the compare value will stay the same, so the duty cycle will be different from the previously set value. + * There is a minimum and maximum frequency that can be set by changing the autoreload register. + * The minimum frequency is base_freq / (2^timer_resolution * (prescaler + 1)). + * The maximum frequency is base_freq / (prescaler + 1). + * + * @param frequency Frequency value in Hz. + */ + void set_frequency(uint32_t frequency); + + /** + * @brief Get the frequency of the PWM signal from the registers of the timer. + * + * @note A timer that counts up and down takes twice its autoreload value to complete a period, + * where one that counts in a single direction takes the autoreload value plus one. + * + * @return Frequency value in Hz. + */ + float get_frequency() const; + + /** + * @brief Check if the PWM was successfully started. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief Timer handle. + */ + TIM_HandleTypeDef* handle; + + /** + * @brief Channel number of the timer. + */ + uint32_t channel; + + /** + * @brief Whether the output is active for the last part of the period. + */ + bool inverted; + + /** + * @brief Flag to check if the PWM was started. + */ + bool initialized{}; +}; +} // namespace micras::hal + +#endif // MICRAS_HAL_PWM_HPP diff --git a/micras_hal/include/micras/hal/pwm_dma.hpp b/micras_hal/include/micras/hal/pwm_dma.hpp new file mode 100644 index 0000000..5890fef --- /dev/null +++ b/micras_hal/include/micras/hal/pwm_dma.hpp @@ -0,0 +1,94 @@ +/** + * @file + */ + +#ifndef MICRAS_HAL_PWM_DMA_HPP +#define MICRAS_HAL_PWM_DMA_HPP + +#include +#include + +#include + +namespace micras::hal { +/** + * @brief Class to handle PWM peripheral on STM32 microcontrollers using DMA. + */ +class PwmDma { +public: + /** + * @brief PWM configuration struct. + */ + struct Config { + void (*init_function)(); + TIM_HandleTypeDef* handle; + uint32_t timer_channel; + }; + + /** + * @brief Construct a new PwmDma object. + * + * @param config Configuration for the PWM. + */ + explicit PwmDma(const Config& config); + + /** + * @brief Start PWM and DMA transfer. + * + * @param buffer Buffer to transfer. + */ + void start_dma(std::span buffer); + + /** + * @brief Start PWM and DMA transfer. + * + * @param buffer Buffer to transfer. + */ + void start_dma(std::span buffer); + + /** + * @brief Stop PWM and DMA transfer. + */ + void stop_dma(); + + /** + * @brief Get the compare value for ad duty cycle. + * + * @param duty_cycle Duty cycle to get the compare value for. + * @return Compare value for the duty cycle. + */ + uint32_t get_compare(float duty_cycle) const; + + /** + * @brief Check if the PWM is busy. + * + * @return True If the PWM is busy, false otherwise. + */ + bool is_busy(); + + /** + * @brief Check if the PWM was successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief Timer handle. + */ + TIM_HandleTypeDef* handle; + + /** + * @brief Channel number of the timer. + */ + uint32_t channel; + + /** + * @brief Flag to check if the PWM was initialized. + */ + bool initialized{}; +}; +} // namespace micras::hal + +#endif // MICRAS_HAL_PWM_DMA_HPP diff --git a/micras_hal/include/micras/hal/spi.hpp b/micras_hal/include/micras/hal/spi.hpp new file mode 100644 index 0000000..ce357ad --- /dev/null +++ b/micras_hal/include/micras/hal/spi.hpp @@ -0,0 +1,204 @@ +/** + * @file + */ + +#ifndef MICRAS_HAL_SPI_HPP +#define MICRAS_HAL_SPI_HPP + +#include +#include +#include + +#include +#include "micras/hal/gpio.hpp" + +namespace micras::hal { +/** + * @brief Class to handle SPI peripheral on STM32 microcontrollers. + * + * @note The stm32 backend defines HAL_SPI_TxRxCpltCallback and HAL_SPI_ErrorCallback, so a consumer + * must not define them, and must keep USE_HAL_SPI_REGISTER_CALLBACKS at 0. They win over the vendor's + * weak defaults because they sit in the source of this class's constructor, which every program that + * uses the class references. + */ +class Spi { +public: + /** + * @brief SPI configuration struct. + * + * @note The clock polarity and phase belong to the device, not to the bus: several devices with + * different SPI modes can share one peripheral, and select_device reconfigures it when the mode + * of the device being selected differs from the one currently programmed. + */ + struct Config { + void (*init_function)(); + SPI_HandleTypeDef* handle; + hal::Gpio::Config cs_gpio; + uint32_t timeout; + uint32_t clock_polarity; + uint32_t clock_phase; + }; + + /** + * @brief State of the transfer started by start_transfer. + * + * @note A transfer that could not be started and one that ended in a bus error both read as + * failed, and the received data is valid only when complete. + */ + enum class Transfer : uint8_t { + NONE = 0, + RUNNING = 1, + COMPLETE = 2, + FAILED = 3, + }; + + /** + * @brief Construct a new Spi object. + * + * @param config Configuration for the SPI. + */ + explicit Spi(const Config& config); + + /** + * @brief Special member functions, of which only the destructor exists. + * + * @note The interrupt that ends a transfer finds its device by address, so an object cannot be + * copied or moved. + */ + ///@{ + ~Spi(); + Spi(const Spi&) = delete; + Spi(Spi&&) = delete; + Spi& operator=(const Spi&) = delete; + Spi& operator=(Spi&&) = delete; + ///@} + + /** + * @brief Activate the chip select, reconfiguring the bus for this device if needed. + * + * @note Blocks for up to the configured timeout waiting for the peripheral to become ready, so + * that an SPI stuck in an error state is reported instead of hanging the caller. + * + * @return True if the device was successfully selected, false otherwise. + */ + bool select_device(); + + /** + * @brief Deactivate the chip select. + */ + void unselect_device(); + + /** + * @brief Transmit data over SPI. + * + * @param data Data to transmit. + * @return True if the transfer completed, false otherwise. + */ + bool transmit(std::span data); + + /** + * @brief Receive data over SPI. + * + * @param data Buffer to receive data into. + * @return True if the transfer completed, false otherwise. + */ + bool receive(std::span data); + + /** + * @brief Transmit and receive data over SPI in the same transfer. + * + * @param transmitted Data to transmit. + * @param received Buffer to receive data into, at least as large as the transmitted one. + * @return True if the transfer completed, false otherwise. + */ + bool transmit_receive(std::span transmitted, std::span received); + + /** + * @brief Start a transfer in both directions that the DMA carries out, without waiting for it. + * + * @details The device is selected here and unselected by the interrupt that ends the transfer, + * so the bus is free for the other devices on it as soon as the last bit is out, and the caller + * has nothing to do but look at the state of the transfer later. + * + * @note Both buffers are borrowed until the transfer ends, and have to be in memory that the DMA + * reaches, which on the parts with tightly coupled memory excludes it. + * + * @param transmitted Data to transmit. + * @param received Buffer to receive data into, at least as large as the transmitted one. + * @return True if the transfer was started, false otherwise. + */ + bool start_transfer(std::span transmitted, std::span received); + + /** + * @brief Get the state of the last transfer started by start_transfer. + * + * @return State of the transfer. + */ + Transfer get_transfer() const; + + /** + * @brief Check if the SPI was successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + + /** + * @brief Unselect the device whose transfer ended and record how it ended. + * + * @note To be called by the transfer callbacks only. + * + * @param handle Handle of the bus whose transfer ended. + * @param succeeded Whether the transfer ended without a bus error. + */ + static void on_transfer_end(const SPI_HandleTypeDef* handle, bool succeeded); + +private: + /** + * @brief Largest number of transfers that can be running at once, which is one per bus. + */ + static constexpr uint8_t max_transfers{6}; + + /** + * @brief Devices with a transfer running, for the interrupt to find the one that ended. + */ + static std::array transferring; + + /** + * @brief Handle for the SPI. + */ + SPI_HandleTypeDef* handle; + + /** + * @brief GPIO for the chip select pin. + */ + hal::Gpio cs_gpio; + + /** + * @brief Timeout for the SPI operations in ms. + */ + uint32_t timeout; + + /** + * @brief Clock polarity required by the device behind this chip select. + */ + uint32_t clock_polarity; + + /** + * @brief Clock phase required by the device behind this chip select. + */ + uint32_t clock_phase; + + /** + * @brief State of the last transfer started by start_transfer. + */ + volatile Transfer transfer{Transfer::NONE}; + + /** + * @brief Flag to check if the SPI was initialized. + */ + bool initialized{}; +}; +} // namespace micras::hal + +#endif // MICRAS_HAL_SPI_HPP diff --git a/micras_hal/include/micras/hal/timer.hpp b/micras_hal/include/micras/hal/timer.hpp new file mode 100644 index 0000000..7c119e1 --- /dev/null +++ b/micras_hal/include/micras/hal/timer.hpp @@ -0,0 +1,76 @@ +/** + * @file + */ + +#ifndef MICRAS_HAL_TIMER_HPP +#define MICRAS_HAL_TIMER_HPP + +#include + +namespace micras::hal { +/** + * @brief Class providing the timebases of the microcontroller. + * + * @note The microsecond timebase comes from the Cortex-M cycle counter instead of a hardware timer: + * it costs no peripheral, resolves single cycles and is present on every Cortex-M3, M4 and M7, so + * the same implementation ports unchanged. Cortex-M0 and M0+ have no cycle counter and would need a + * timer here. + */ +class Timer { +public: + /** + * @brief Deleted constructor for static class. + */ + Timer() = delete; + + /** + * @brief Start the cycle counter and compute the conversion to microseconds. + * + * @note Must be called after the system clock is configured, since the conversion depends on + * the core clock frequency. + */ + static void init(); + + /** + * @brief Get the current value of the free running cycle counter. + * + * @note Wraps every 2^32 core clock cycles, which bounds the longest measurable interval to + * about 7.8 s at 550 MHz. Differences taken with unsigned arithmetic are correct across the + * wrap, so no special case is needed below that bound. + * + * @return Current value of the cycle counter. + */ + static uint32_t get_counter(); + + /** + * @brief Get the current timer counter. + * + * @return Current timer counter in milliseconds. + */ + static uint32_t get_counter_ms(); + + /** + * @brief Convert a number of core clock cycles to microseconds. + * + * @param cycles Number of core clock cycles. + * @return Equivalent time in microseconds. + */ + static uint32_t to_microseconds(uint32_t cycles); + + /** + * @brief Convert a time in microseconds to core clock cycles. + * + * @param microseconds Time in microseconds. + * @return Equivalent number of core clock cycles. + */ + static uint32_t to_cycles(uint32_t microseconds); + +private: + /** + * @brief Core clock cycles in one microsecond, resolved by init. + */ + static uint32_t cycles_per_microsecond; +}; +} // namespace micras::hal + +#endif // MICRAS_HAL_TIMER_HPP diff --git a/micras_hal/include/micras/hal/uart_dma.hpp b/micras_hal/include/micras/hal/uart_dma.hpp new file mode 100644 index 0000000..1112368 --- /dev/null +++ b/micras_hal/include/micras/hal/uart_dma.hpp @@ -0,0 +1,147 @@ +/** + * @file + */ + +#ifndef MICRAS_HAL_UART_DMA_HPP +#define MICRAS_HAL_UART_DMA_HPP + +#include +#include +#include +#include + +namespace micras::hal { +/** + * @brief Class to handle a UART peripheral on STM32 microcontrollers using DMA. + * + * @note Reception is a circular DMA that only an error stops, and is polled by comparing the transfer + * counter against what was already taken. Nothing is done in an interrupt, there is no window + * between stopping and restarting in which bytes are lost, and the latency is one control loop + * iteration, which is far below anything the radio adds. A transfer error disables the receive + * DMA stream, which read finds whether or not the stream's interrupt is enabled, so that interrupt + * is not needed. + * + * Transmission is polled the same way, and needs no interrupt either. The HAL only returns the + * peripheral to ready from the transmission complete interrupt of the UART, which is left disabled + * so that a reception error never aborts the circular transfer, and its DMA to ready from the + * interrupt of the transmit stream, which may be disabled too. A transfer is over once its DMA + * moved the last byte, which the stream or channel tells, and the next one closes what the HAL left + * open: aborting the transmission returns the DMA to ready and unlocks it, which starting a + * transfer needs, and leaves the byte the peripheral is still sending alone. Aborting also empties + * the transmit FIFO of the peripheral when it is enabled, so a transfer is only over once that + * FIFO is empty. + */ +class UartDma { +public: + /** + * @brief Configuration struct for the UART. + */ + struct Config { + void (*init_function)(); + UART_HandleTypeDef* handle; + }; + + /** + * @brief Construct a new UartDma object. + * + * @param config UART configuration struct. + */ + explicit UartDma(const Config& config); + + /** + * @brief Special member functions deleted. + * + * @note The object owns a transfer in flight that points into its own buffers, so it cannot be + * copied or moved. + */ + ///@{ + UartDma(const UartDma&) = delete; + UartDma(UartDma&&) = delete; + UartDma& operator=(const UartDma&) = delete; + UartDma& operator=(UartDma&&) = delete; + ~UartDma() = default; + ///@} + + /** + * @brief Start receiving into a circular buffer. + * + * @note The buffer is borrowed and has to outlive the reception. It should live in a memory + * the DMA controller can reach without going through the bus matrix of another domain. + * + * @param buffer Buffer the DMA writes into, wrapping around. + * @return True if the reception was started, false otherwise. + */ + bool start_rx(std::span buffer); + + /** + * @brief Take the bytes that arrived since the last call. + * + * @note Bytes are lost silently if this is called more rarely than the buffer takes to fill, + * which the framing recovers from by resynchronizing on the next delimiter. A reception that the + * vendor HAL ended or whose DMA stream was disabled by a transfer error is aborted and started + * again from the beginning of the buffer, and the bytes it held are dropped. + * + * @param into Buffer to copy the received bytes into. + * @return Number of bytes copied. + */ + std::size_t read(std::span into); + + /** + * @brief Start sending a buffer. + * + * @note The buffer is borrowed and has to stay valid until the transfer completes. + * + * @param from Data to send. + * @return True if the transfer was started, false if one is still running or it failed. + */ + bool start_tx(std::span from); + + /** + * @brief Check if a transfer is still running. + * + * @note The last byte may still be leaving the shift register, which does not stop the next + * transfer from starting. + * + * @return True if the DMA is still feeding the peripheral or its transmit FIFO holds bytes, + * false otherwise. + */ + bool is_transmitting() const; + + /** + * @brief Check if the peripheral was initialized and is receiving. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief Get how many bytes the DMA has written into the buffer so far. + * + * @return Index the DMA will write the next byte at. + */ + std::size_t rx_head() const; + + /** + * @brief UART handle. + */ + UART_HandleTypeDef* handle; + + /** + * @brief Destination buffer of the reception, written around by the DMA. + */ + std::span rx_buffer; + + /** + * @brief Index of the first byte that has not been taken yet. + */ + std::size_t rx_tail{}; + + /** + * @brief Whether the reception is running. + */ + bool initialized{}; +}; +} // namespace micras::hal + +#endif // MICRAS_HAL_UART_DMA_HPP diff --git a/micras_hal/stm32/g4/README.md b/micras_hal/stm32/g4/README.md new file mode 100644 index 0000000..2015179 --- /dev/null +++ b/micras_hal/stm32/g4/README.md @@ -0,0 +1,19 @@ +# STM32G4 family + +Code of the stm32 backend that only the STM32G4 family runs, which CMake compiles when the board +target defines an STM32G4 device (or `MICRAS_LIB_STM32_FAMILY` is `g4`): + +- `src/family.cpp`: the functions of `micras/hal/family.hpp`. The ADC calibrates for single-ended + conversions, there is no cache to enable (`HAL_Init` sets up the ART accelerator), the watchdog is + `IWDG`, every core frequency the clock tree reaches is supported (the family has no boost option + byte), a timer runs at twice its APB clock when the APB prescaler (`RCC_HCLK_DIV1`) is not 1, and + the cycle counter starts without an unlock, since the Cortex-M4 has no lock access register. +- `src/flash.cpp`: the flash driver, pages of 2 KB programmed in 64-bit double words. A sector of the + shared `Flash` class is a page. +- `include/micras/hal/family/flash.hpp`: the flash geometry the shared `flash.hpp` reads. The storage + region is bank 2 of a 512 KB part, such as the STM32G474RET6 of the old Micras board, in the dual + bank mode it leaves the factory in: 128 pages from 0x08040000. Every flash operation checks the + `DBANK` option bit and the size of the part, and fails on any other layout rather than erase what + may be code. + +The code in `../src` is the same on every family and names no chip. diff --git a/micras_hal/stm32/g4/include/micras/hal/family/flash.hpp b/micras_hal/stm32/g4/include/micras/hal/family/flash.hpp new file mode 100644 index 0000000..93d3a16 --- /dev/null +++ b/micras_hal/stm32/g4/include/micras/hal/family/flash.hpp @@ -0,0 +1,67 @@ +/** + * @file + * + * @brief Flash memory geometry of the STM32G4 family. + * + * @note The flash memory is erased in pages and programmed in double words of 64 bits; a sector of the + * shared Flash class is a page. The geometry is the one the 512 KB parts, such as the STM32G474xE, have + * in the dual bank mode they leave the factory in: two banks of 128 pages of 2 KB. The storage region + * is bank 2, the upper half of the flash memory. Every operation checks the part and the mode at run + * time and fails on any other layout. + */ + +#ifndef MICRAS_HAL_FAMILY_FLASH_HPP +#define MICRAS_HAL_FAMILY_FLASH_HPP + +#include +#include + +namespace micras::hal::family { +/** + * @brief Number of pages of a bank of the 512 KB parts in dual bank mode. + */ +inline constexpr uint16_t pages_per_bank{128}; + +/** + * @brief Number of bits of the smallest unit the flash memory programs. + */ +inline constexpr uint32_t flash_word_bits{64}; + +/** + * @brief Number of bytes of an erasable page. + */ +inline constexpr uint32_t sector_size{FLASH_PAGE_SIZE}; + +/** + * @brief Index of the first page reserved for data storage, counting from the start of bank 1. + */ +inline constexpr uint16_t storage_first_sector{pages_per_bank}; + +/** + * @brief Number of pages reserved for data storage, the whole of bank 2. + */ +inline constexpr uint16_t storage_sectors{pages_per_bank}; + +/** + * @brief Round a number of bytes up to a whole number of the double words the flash memory programs. + * + * @param size Number of bytes to round up. + * @return Number of bytes the data occupies in the flash memory. + */ +constexpr uint32_t align_size(uint32_t size) { + constexpr uint32_t word_size{flash_word_bits / 8U}; + return (size + word_size - 1) / word_size * word_size; +} + +/** + * @brief Check that the part has the layout this header describes: dual bank mode, with banks of + * pages_per_bank pages. + * + * @return Whether the storage region exists on this part. + */ +inline bool is_storage_available() { + return (FLASH->OPTR & FLASH_OPTR_DBANK) != 0 and FLASH_PAGE_NB == pages_per_bank; +} +} // namespace micras::hal::family + +#endif // MICRAS_HAL_FAMILY_FLASH_HPP diff --git a/micras_hal/stm32/g4/src/family.cpp b/micras_hal/stm32/g4/src/family.cpp new file mode 100644 index 0000000..66f8df1 --- /dev/null +++ b/micras_hal/stm32/g4/src/family.cpp @@ -0,0 +1,62 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/hal/family.hpp" + +namespace micras::hal::family { +bool calibrate_adc(ADC_HandleTypeDef* handle) { + return HAL_ADCEx_Calibration_Start(handle, ADC_SINGLE_ENDED) == HAL_OK; +} + +bool is_dma_enabled(const DMA_HandleTypeDef* handle) { + return (handle->Instance->CCR & DMA_CCR_EN) != 0; +} + +bool has_dma_finished(const DMA_HandleTypeDef* handle) { + return (handle->Instance->CCR & DMA_CCR_EN) == 0 or handle->Instance->CNDTR == 0; +} + +void enable_caches() { } + +bool was_reset_by_watchdog() { + return __HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST) != 0; +} + +void freeze_watchdog_in_debug() { + __HAL_DBGMCU_FREEZE_IWDG(); +} + +bool is_cpu_frequency_supported(bool /*boost*/) { + return true; +} + +IWDG_TypeDef* watchdog() { + return IWDG; +} + +uint32_t timer_clock(const TIM_TypeDef* instance) { + RCC_ClkInitTypeDef clock_config{}; + uint32_t flash_latency{}; + HAL_RCC_GetClockConfig(&clock_config, &flash_latency); + + if (std::bit_cast(instance) >= APB2PERIPH_BASE) { + const uint32_t pclk2 = HAL_RCC_GetPCLK2Freq(); + return clock_config.APB2CLKDivider == RCC_HCLK_DIV1 ? pclk2 : 2 * pclk2; + } + + const uint32_t pclk1 = HAL_RCC_GetPCLK1Freq(); + return clock_config.APB1CLKDivider == RCC_HCLK_DIV1 ? pclk1 : 2 * pclk1; +} + +void enable_cycle_counter() { + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + + DWT->CYCCNT = 0; + DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; +} +} // namespace micras::hal::family diff --git a/micras_hal/stm32/g4/src/flash.cpp b/micras_hal/stm32/g4/src/flash.cpp new file mode 100644 index 0000000..741e748 --- /dev/null +++ b/micras_hal/stm32/g4/src/flash.cpp @@ -0,0 +1,138 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include "micras/hal/family/flash.hpp" +#include "micras/hal/flash.hpp" + +namespace micras::hal { +static_assert(FlashWord::size == sizeof(uint64_t)); + +namespace { +constexpr uint32_t base_address{FLASH_BASE + (family::storage_first_sector * family::sector_size)}; + +constexpr uint32_t erase_complete{0xFFFFFFFFU}; +} // namespace + +FlashWord::FlashWord(std::span data) { + const auto data_address = std::bit_cast(data.data()); + + if (data.size() >= size and data_address % alignof(uint32_t) == 0) { + this->source = std::bit_cast(data.data()); + return; + } + + const std::span bytes{std::bit_cast(this->buffer.data()), size}; + + std::ranges::fill(bytes, erased_value); + std::ranges::copy(data.first(std::min(data.size(), size)), bytes.begin()); + + this->source = this->buffer.data(); +} + +const uint32_t* FlashWord::data() const { + return this->source; +} + +bool FlashWord::is_padded() const { + return this->source == this->buffer.data(); +} + +std::span Flash::read(uint32_t address, uint32_t size) { + if (address > total_size or size > total_size - address or not family::is_storage_available()) { + return {}; + } + + return {std::bit_cast(base_address + address), size}; +} + +std::span Flash::read(uint16_t sector, uint32_t sector_address, uint32_t size) { + if (sector >= total_sectors or sector_address > sector_size) { + return {}; + } + + return read(sector * sector_size + sector_address, size); +} + +Flash::Status Flash::write(uint32_t address, std::span data) { + if (address % FlashWord::size != 0) { + return Status::MISALIGNED; + } + + if (address > total_size or family::align_size(data.size()) > total_size - address) { + return Status::OUT_OF_BOUNDS; + } + + if (not family::is_storage_available() or HAL_FLASH_Unlock() != HAL_OK) { + return Status::ERROR; + } + + __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS); + + Status status = Status::OK; + + for (uint32_t offset = 0; offset < data.size(); offset += FlashWord::size) { + const FlashWord word{data.subspan(offset)}; + uint64_t double_word{}; + std::memcpy(&double_word, word.data(), sizeof(double_word)); + + if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, base_address + address + offset, double_word) != HAL_OK) { + status = Status::ERROR; + break; + } + } + + HAL_FLASH_Lock(); + + return status; +} + +Flash::Status Flash::write(uint16_t sector, uint32_t sector_address, std::span data) { + if (sector >= total_sectors or sector_address > sector_size) { + return Status::OUT_OF_BOUNDS; + } + + return write(sector * sector_size + sector_address, data); +} + +Flash::Status Flash::erase_sectors(uint16_t start_sector, uint16_t number_of_sectors) { + if (start_sector >= total_sectors or number_of_sectors > total_sectors - start_sector) { + return Status::OUT_OF_BOUNDS; + } + + FLASH_EraseInitTypeDef erase_struct = { + .TypeErase = FLASH_TYPEERASE_PAGES, + .Banks = FLASH_BANK_2, + .Page = start_sector, + .NbPages = number_of_sectors, + }; + + if (not family::is_storage_available() or HAL_FLASH_Unlock() != HAL_OK) { + return Status::ERROR; + } + + __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS); + pFlash.ErrorCode = HAL_FLASH_ERROR_NONE; + + uint32_t page_error{}; + + const HAL_StatusTypeDef hal_status = HAL_FLASHEx_Erase(&erase_struct, &page_error); + + HAL_FLASH_Lock(); + + if (hal_status != HAL_OK or page_error != erase_complete) { + return Status::ERROR; + } + + return Status::OK; +} +} // namespace micras::hal diff --git a/micras_hal/stm32/h7/README.md b/micras_hal/stm32/h7/README.md new file mode 100644 index 0000000..575d71c --- /dev/null +++ b/micras_hal/stm32/h7/README.md @@ -0,0 +1,16 @@ +# STM32H7 family + +Code of the stm32 backend that only the STM32H7 family runs, which CMake compiles when the board +target defines an STM32H7 device (or `MICRAS_LIB_STM32_FAMILY` is `h7`): + +- `src/family.cpp`: the functions of `micras/hal/family.hpp`. The ADC calibrates its offset for + single-ended conversions, `Mcu::init` enables the instruction cache, the watchdog is `IWDG1`, the + `CPUFREQ_BOOST` option byte of the STM32H72x/73x tells whether the core may run above its default + maximum, a timer runs at twice its APB clock when the APB prescaler (`RCC_APBx_DIV1`) is not 1, and + the cycle counter is unlocked through the lock access register of the Cortex-M7 before it starts. +- `src/flash.cpp`: the flash driver, sectors of 128 KB programmed in 256-bit flash words. +- `include/micras/hal/family/flash.hpp`: the flash geometry the shared `flash.hpp` reads. The storage + region is the upper half of bank 1: sectors 4 to 7 of an STM32H725xG, from 0x08080000. The flash + driver derives the region's address from these constants, as every family does. + +The code in `../src` is the same on every family and names no chip. diff --git a/micras_hal/stm32/h7/include/micras/hal/family/flash.hpp b/micras_hal/stm32/h7/include/micras/hal/family/flash.hpp new file mode 100644 index 0000000..ff95c66 --- /dev/null +++ b/micras_hal/stm32/h7/include/micras/hal/family/flash.hpp @@ -0,0 +1,38 @@ +/** + * @file + * + * @brief Flash memory geometry of the STM32H7 family. + * + * @note The flash memory is erased in sectors and programmed in flash words of 256 bits. The storage + * region is the upper half of bank 1: on the STM32H725xG, sectors 4 to 7 of 128 KB. + */ + +#ifndef MICRAS_HAL_FAMILY_FLASH_HPP +#define MICRAS_HAL_FAMILY_FLASH_HPP + +#include +#include + +namespace micras::hal::family { +/** + * @brief Number of bits of the smallest unit the flash memory programs. + */ +inline constexpr uint32_t flash_word_bits{FLASH_NB_32BITWORD_IN_FLASHWORD * 32U}; + +/** + * @brief Number of bytes of an erasable sector. + */ +inline constexpr uint32_t sector_size{FLASH_SECTOR_SIZE}; + +/** + * @brief Index of the first sector reserved for data storage. + */ +inline constexpr uint16_t storage_first_sector{FLASH_SECTOR_TOTAL / 2}; + +/** + * @brief Number of sectors reserved for data storage. + */ +inline constexpr uint16_t storage_sectors{FLASH_SECTOR_TOTAL / 2}; +} // namespace micras::hal::family + +#endif // MICRAS_HAL_FAMILY_FLASH_HPP diff --git a/micras_hal/stm32/h7/src/family.cpp b/micras_hal/stm32/h7/src/family.cpp new file mode 100644 index 0000000..0e11b0b --- /dev/null +++ b/micras_hal/stm32/h7/src/family.cpp @@ -0,0 +1,85 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/hal/family.hpp" + +namespace micras::hal::family { +namespace { +constexpr uint32_t software_lock_key{0xC5ACCE55}; +} // namespace + +bool calibrate_adc(ADC_HandleTypeDef* handle) { + return HAL_ADCEx_Calibration_Start(handle, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED) == HAL_OK; +} + +bool is_dma_enabled(const DMA_HandleTypeDef* handle) { + if (IS_DMA_STREAM_INSTANCE(handle->Instance)) { + return (static_cast(handle->Instance)->CR & DMA_SxCR_EN) != 0; + } + + return (static_cast(handle->Instance)->CCR & BDMA_CCR_EN) != 0; +} + +bool has_dma_finished(const DMA_HandleTypeDef* handle) { + if (IS_DMA_STREAM_INSTANCE(handle->Instance)) { + return (static_cast(handle->Instance)->CR & DMA_SxCR_EN) == 0; + } + + const auto* const channel = static_cast(handle->Instance); + return (channel->CCR & BDMA_CCR_EN) == 0 or channel->CNDTR == 0; +} + +void enable_caches() { + SCB_EnableICache(); +} + +bool was_reset_by_watchdog() { + return __HAL_RCC_GET_FLAG(RCC_FLAG_IWDG1RST) != 0; +} + +void freeze_watchdog_in_debug() { + __HAL_DBGMCU_FREEZE_IWDG1(); +} + +bool is_cpu_frequency_supported(bool boost) { +#ifdef FLASH_OPTSR2_CPUFREQ_BOOST + return not boost or (FLASH->OPTSR2_CUR & FLASH_OPTSR2_CPUFREQ_BOOST) != 0; +#else + return not boost; +#endif +} + +IWDG_TypeDef* watchdog() { + return IWDG1; +} + +uint32_t timer_clock(const TIM_TypeDef* instance) { + RCC_ClkInitTypeDef clock_config{}; + uint32_t flash_latency{}; + HAL_RCC_GetClockConfig(&clock_config, &flash_latency); + + if (std::bit_cast(instance) >= APB2PERIPH_BASE) { + const uint32_t pclk2 = HAL_RCC_GetPCLK2Freq(); + return clock_config.APB2CLKDivider == RCC_APB2_DIV1 ? pclk2 : 2 * pclk2; + } + + const uint32_t pclk1 = HAL_RCC_GetPCLK1Freq(); + return clock_config.APB1CLKDivider == RCC_APB1_DIV1 ? pclk1 : 2 * pclk1; +} + +void enable_cycle_counter() { + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + + if ((DWT->LSR & ITM_LSR_Present_Msk) != 0 and (DWT->LSR & ITM_LSR_Access_Msk) != 0) { + DWT->LAR = software_lock_key; + } + + DWT->CYCCNT = 0; + DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; +} +} // namespace micras::hal::family diff --git a/micras_hal/stm32/h7/src/flash.cpp b/micras_hal/stm32/h7/src/flash.cpp new file mode 100644 index 0000000..ca37d29 --- /dev/null +++ b/micras_hal/stm32/h7/src/flash.cpp @@ -0,0 +1,147 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include + +#include "micras/hal/family/flash.hpp" +#include "micras/hal/flash.hpp" + +namespace micras::hal { +/** + * @brief First address of the flash memory region reserved for data storage. + */ +static constexpr uint32_t base_address{FLASH_BASE + (family::storage_first_sector * family::sector_size)}; + +/** + * @brief Index of the first sector of the flash memory region reserved for data storage. + */ +static constexpr uint16_t base_sector{family::storage_first_sector}; + +/** + * @brief Round a number of bytes up to a whole number of flash words. + * + * @param size Number of bytes to round up. + * @return Number of bytes occupied in the flash memory. + */ +static constexpr uint32_t align_size(uint32_t size) { + return (size + FlashWord::size - 1) / FlashWord::size * FlashWord::size; +} + +FlashWord::FlashWord(std::span data) { + const auto data_address = std::bit_cast(data.data()); + + if (data.size() >= size and data_address % alignof(uint32_t) == 0) { + this->source = std::bit_cast(data.data()); + return; + } + + const std::span bytes{std::bit_cast(this->buffer.data()), size}; + + std::ranges::fill(bytes, erased_value); + std::ranges::copy(data.first(std::min(data.size(), size)), bytes.begin()); + + this->source = this->buffer.data(); +} + +const uint32_t* FlashWord::data() const { + return this->source; +} + +bool FlashWord::is_padded() const { + return this->source == this->buffer.data(); +} + +std::span Flash::read(uint32_t address, uint32_t size) { + if (address > total_size or size > total_size - address) { + return {}; + } + + return {std::bit_cast(base_address + address), size}; +} + +std::span Flash::read(uint16_t sector, uint32_t sector_address, uint32_t size) { + if (sector >= total_sectors or sector_address > sector_size) { + return {}; + } + + return read(sector * sector_size + sector_address, size); +} + +Flash::Status Flash::write(uint32_t address, std::span data) { + if (address % FlashWord::size != 0) { + return Status::MISALIGNED; + } + + if (address > total_size or align_size(data.size()) > total_size - address) { + return Status::OUT_OF_BOUNDS; + } + + if (HAL_FLASH_Unlock() != HAL_OK) { + return Status::ERROR; + } + + Status status = Status::OK; + + for (uint32_t offset = 0; offset < data.size(); offset += FlashWord::size) { + const FlashWord word{data.subspan(offset)}; + + if (HAL_FLASH_Program( + FLASH_TYPEPROGRAM_FLASHWORD, base_address + address + offset, std::bit_cast(word.data()) + ) != HAL_OK) { + status = Status::ERROR; + break; + } + } + + HAL_FLASH_Lock(); + + return status; +} + +Flash::Status Flash::write(uint16_t sector, uint32_t sector_address, std::span data) { + if (sector >= total_sectors or sector_address > sector_size) { + return Status::OUT_OF_BOUNDS; + } + + return write(sector * sector_size + sector_address, data); +} + +Flash::Status Flash::erase_sectors(uint16_t start_sector, uint16_t number_of_sectors) { + if (start_sector >= total_sectors or number_of_sectors > total_sectors - start_sector) { + return Status::OUT_OF_BOUNDS; + } + + FLASH_EraseInitTypeDef erase_struct = { + .TypeErase = FLASH_TYPEERASE_SECTORS, + .Banks = FLASH_BANK_1, + .Sector = static_cast(base_sector + start_sector), + .NbSectors = number_of_sectors, + .VoltageRange = FLASH_VOLTAGE_RANGE_4 + }; + + if (HAL_FLASH_Unlock() != HAL_OK) { + return Status::ERROR; + } + + pFlash.ErrorCode = HAL_FLASH_ERROR_NONE; + + uint32_t sector_error{}; + + const HAL_StatusTypeDef hal_status = HAL_FLASHEx_Erase(&erase_struct, §or_error); + + HAL_FLASH_Lock(); + + if (hal_status != HAL_OK or sector_error != 0xFFFFFFFFU) { + return Status::ERROR; + } + + return Status::OK; +} +} // namespace micras::hal diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp new file mode 100644 index 0000000..b34e46d --- /dev/null +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -0,0 +1,180 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include "micras/hal/adc_dma.hpp" +#include "micras/hal/family.hpp" + +extern "C" { +/** + * @brief Callback of the vendor HAL for the end of a DMA transfer of a converter. + * + * @param hadc Handle of the converter. + */ +// NOLINTNEXTLINE(readability-identifier-naming) the name is fixed by the vendor HAL +void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc) { + micras::hal::AdcDma::on_sequence_complete(hadc); +} + +/** + * @brief Callback of the vendor HAL for an overrun of a converter or a transfer error of its DMA. + * + * @param hadc Handle of the converter. + */ +// NOLINTNEXTLINE(readability-identifier-naming) the name is fixed by the vendor HAL +void HAL_ADC_ErrorCallback(ADC_HandleTypeDef* hadc) { + micras::hal::AdcDma::on_error(hadc); +} +} + +namespace micras::hal { +std::array AdcDma::instances{}; +uint32_t AdcDma::restarts{}; + +AdcDma::AdcDma(const Config& config) : + max_reading{config.max_reading}, reference_voltage{config.reference_voltage}, handle{config.handle} { + if (this->handle->State == HAL_ADC_STATE_RESET) { + config.init_function(); + } + + const bool calibrated = family::calibrate_adc(this->handle); + + auto* const slot = std::ranges::find(instances, nullptr); + + if (slot == instances.end()) { + return; + } + + *slot = this; + this->initialized = calibrated; +} + +AdcDma::~AdcDma() { + std::ranges::replace(instances, this, static_cast(nullptr)); +} + +bool AdcDma::start_dma(std::span buffer) { + this->transfer = buffer; + + if (HAL_ADC_Start_DMA(this->handle, buffer.data(), buffer.size()) != HAL_OK) { + this->initialized = false; + return false; + } + + return true; +} + +bool AdcDma::start_dma(std::span buffer) { + return this->start_dma({std::bit_cast(buffer.data()), buffer.size()}); +} + +bool AdcDma::start_dma(std::span buffer, std::span snapshot) { + if (snapshot.size() != buffer.size()) { + this->initialized = false; + return false; + } + + this->buffer = buffer; + this->snapshot = snapshot; + + return this->start_dma(buffer); +} + +uint32_t AdcDma::read_snapshot(std::span destination) const { + const std::span source = this->snapshot.first(std::min(this->snapshot.size(), destination.size())); + uint32_t before = this->sequence; + + while (true) { + std::atomic_signal_fence(std::memory_order_seq_cst); + std::ranges::copy(source, destination.begin()); + std::atomic_signal_fence(std::memory_order_seq_cst); + + const uint32_t after = this->sequence; + + if (after == before) { + return after; + } + + before = after; + } +} + +void AdcDma::on_sequence_complete(const ADC_HandleTypeDef* handle) { + AdcDma* const instance = find(handle); + + if (instance == nullptr or instance->stopped) { + return; + } + + if (__HAL_ADC_GET_FLAG(handle, ADC_FLAG_OVR)) { + instance->stopped = true; + return; + } + + std::ranges::copy(instance->buffer, instance->snapshot.begin()); + instance->sequence = instance->sequence + 1; +} + +void AdcDma::on_error(const ADC_HandleTypeDef* handle) { + AdcDma* const instance = find(handle); + + if (instance != nullptr) { + instance->stopped = true; + } +} + +AdcDma* AdcDma::find(const ADC_HandleTypeDef* handle) { + const auto* const found = std::ranges::find_if(instances, [handle](const AdcDma* instance) { + return instance != nullptr and instance->handle == handle; + }); + + return found == instances.end() ? nullptr : *found; +} + +void AdcDma::stop_dma() { + HAL_ADC_Stop_DMA(this->handle); +} + +void AdcDma::recover() { + const DMA_HandleTypeDef* const dma = this->handle->DMA_Handle; + const bool frozen = dma->State == HAL_DMA_STATE_BUSY and + (__HAL_ADC_GET_FLAG(this->handle, ADC_FLAG_OVR) or not family::is_dma_enabled(dma)); + + if (not this->stopped and not frozen) { + return; + } + + HAL_ADC_Stop_DMA(this->handle); + this->handle->State = this->handle->State & ~(HAL_ADC_STATE_ERROR_DMA | HAL_ADC_STATE_ERROR_INTERNAL); + this->stopped = false; + + if (HAL_ADC_Start_DMA(this->handle, this->transfer.data(), this->transfer.size()) != HAL_OK) { + this->stopped = true; + } + + restarts++; +} + +uint32_t AdcDma::get_restarts() { + return restarts; +} + +uint16_t AdcDma::get_max_reading() const { + return this->max_reading; +} + +float AdcDma::get_reference_voltage() const { + return this->reference_voltage; +} + +bool AdcDma::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/stm32/src/crc.cpp b/micras_hal/stm32/src/crc.cpp new file mode 100644 index 0000000..669402f --- /dev/null +++ b/micras_hal/stm32/src/crc.cpp @@ -0,0 +1,17 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/hal/crc.hpp" + +namespace micras::hal { +Crc::Crc(const Config& config) : handle{config.handle} { } + +uint32_t Crc::calculate(std::span data) { + return HAL_CRC_Calculate(this->handle, std::bit_cast(data.data()), data.size()); +} +} // namespace micras::hal diff --git a/micras_hal/stm32/src/encoder.cpp b/micras_hal/stm32/src/encoder.cpp new file mode 100644 index 0000000..b31da06 --- /dev/null +++ b/micras_hal/stm32/src/encoder.cpp @@ -0,0 +1,30 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/encoder.hpp" + +namespace micras::hal { +Encoder::Encoder(const Config& config) : handle{config.handle} { + if (this->handle->State == HAL_TIM_STATE_RESET) { + config.init_function(); + } + + this->initialized = HAL_TIM_Encoder_Start(this->handle, config.timer_channel) == HAL_OK; + + // NOLINTNEXTLINE(cppcoreguidelines-prefer-member-initializer) + this->start_count = __HAL_TIM_GET_AUTORELOAD(this->handle) / 2; + __HAL_TIM_SET_COUNTER(this->handle, this->start_count); +} + +int32_t Encoder::get_counter() const { + return std::bit_cast(__HAL_TIM_GET_COUNTER(this->handle) - this->start_count); +} + +bool Encoder::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/stm32/src/fmac.cpp b/micras_hal/stm32/src/fmac.cpp new file mode 100644 index 0000000..edee9bd --- /dev/null +++ b/micras_hal/stm32/src/fmac.cpp @@ -0,0 +1,71 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/fmac.hpp" + +namespace micras::hal { +Fmac::Fmac(const Config& config) : handle{config.handle} { + if (this->handle->State == HAL_FMAC_STATE_RESET) { + config.init_function(); + } +} + +bool Fmac::configure_iir(std::span feed_forward, std::span feedback) { + this->initialized = false; + + if (feed_forward.empty() or feedback.empty() or feed_forward.size() + feedback.size() > buffer_size) { + return false; + } + + FMAC_FilterConfigTypeDef filter_config{ + .InputBaseAddress = input_base_address, + .InputBufferSize = buffer_size, + .InputThreshold = FMAC_THRESHOLD_1, + .CoeffBaseAddress = coefficient_base_address, + .CoeffBufferSize = buffer_size, + .OutputBaseAddress = output_base_address, + .OutputBufferSize = buffer_size, + .OutputThreshold = FMAC_THRESHOLD_1, + // NOLINTBEGIN(cppcoreguidelines-pro-type-const-cast) the HAL only reads the coefficients + .pCoeffA = const_cast(feedback.data()), + .CoeffASize = static_cast(feedback.size()), + .pCoeffB = const_cast(feed_forward.data()), + .CoeffBSize = static_cast(feed_forward.size()), + // NOLINTEND(cppcoreguidelines-pro-type-const-cast) + .InputAccess = FMAC_BUFFER_ACCESS_POLLING, + .OutputAccess = FMAC_BUFFER_ACCESS_POLLING, + .Clip = FMAC_CLIP_ENABLED, + .Filter = FMAC_FUNC_IIR_DIRECT_FORM_1, + .P = static_cast(feed_forward.size()), + .Q = static_cast(feedback.size()), + .R = 0, + }; + + if (HAL_FMAC_FilterConfig(this->handle, &filter_config) != HAL_OK) { + return false; + } + + if (HAL_FMAC_FilterStart(this->handle, this->output_buffer.data(), &this->output_buffer_size) != HAL_OK) { + return false; + } + + this->initialized = true; + return true; +} + +int16_t Fmac::update(int16_t sample) { + this->handle->Instance->WDATA = static_cast(sample); + + while ((this->handle->Instance->SR & FMAC_SR_YEMPTY) != 0) { } + + return static_cast(this->handle->Instance->RDATA & FMAC_WDATA_WDATA); +} + +bool Fmac::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/stm32/src/gpio.cpp b/micras_hal/stm32/src/gpio.cpp new file mode 100644 index 0000000..3486259 --- /dev/null +++ b/micras_hal/stm32/src/gpio.cpp @@ -0,0 +1,28 @@ +/** + * @file + */ + +#include + +#include "micras/hal/gpio.hpp" + +namespace micras::hal { +/** + * @brief Offset of the reset half of the bit set reset register. + */ +static constexpr uint32_t reset_offset{16}; + +Gpio::Gpio(const Config& config) : port{config.port}, pin{config.pin} { } + +bool Gpio::read() const { + return (this->port->IDR & this->pin) != 0; +} + +void Gpio::write(bool state) { + this->port->BSRR = state ? this->pin : static_cast(this->pin) << reset_offset; +} + +void Gpio::toggle() { + this->write((this->port->ODR & this->pin) == 0); +} +} // namespace micras::hal diff --git a/micras_hal/stm32/src/mcu.cpp b/micras_hal/stm32/src/mcu.cpp new file mode 100644 index 0000000..663b035 --- /dev/null +++ b/micras_hal/stm32/src/mcu.cpp @@ -0,0 +1,133 @@ +/** + * @file + */ + +#include +#include +#include + +#include +#include "micras/hal/family.hpp" +#include "micras/hal/gpio.hpp" +#include "micras/hal/mcu.hpp" +#include "micras/hal/pwm.hpp" +#include "micras/hal/timer.hpp" + +namespace micras::hal { +/** + * @brief Key values of the independent watchdog key register. + */ +///@{ +static constexpr uint32_t watchdog_key_reload{0xAAAA}; +static constexpr uint32_t watchdog_key_write{0x5555}; +static constexpr uint32_t watchdog_key_start{0xCCCC}; +///@} + +/** + * @brief Smallest divider the watchdog prescaler can apply to the low speed oscillator. + */ +static constexpr uint32_t watchdog_min_divider{4}; + +/** + * @brief Largest value the watchdog prescaler register can hold. + */ +static constexpr uint32_t watchdog_max_prescaler{6}; + +/** + * @brief Largest value the 12 bit watchdog reload register can hold. + */ +static constexpr uint32_t watchdog_max_reload{0xFFF}; + +/** + * @brief Time to wait for the watchdog registers to take effect before giving up. + * + * @note Bounded so that a low speed oscillator that never starts cannot hang the boot. + */ +static constexpr uint32_t watchdog_timeout_us{1000}; + +/** + * @brief Compare value that keeps an inverted PWM output inactive, above any period. + */ +static constexpr uint32_t inactive_compare{0xFFFFFFFF}; + +bool Mcu::watchdog_reset{}; +bool Mcu::cpu_frequency_supported{}; + +void Mcu::init(const Config& config) { + watchdog_reset = family::was_reset_by_watchdog(); + __HAL_RCC_CLEAR_RESET_FLAGS(); + + family::enable_caches(); + + HAL_Init(); + + family::freeze_watchdog_in_debug(); + + cpu_frequency_supported = family::is_cpu_frequency_supported(config.cpu_frequency_boost); + + config.clock_init(); + + if (config.peripheral_clock_init != nullptr) { + config.peripheral_clock_init(); + } + + Timer::init(); + + for (const InitFunction init_function : config.peripheral_inits) { + init_function(); + } +} + +void Mcu::emergency_stop(std::span pwm_outputs, std::span enable_gpios) { + for (const auto& pwm_output : pwm_outputs) { + const uint32_t compare = pwm_output.inverted ? inactive_compare : 0; + __HAL_TIM_SET_COMPARE(pwm_output.handle, pwm_output.timer_channel, compare); + } + + for (const auto& enable_gpio : enable_gpios) { + HAL_GPIO_WritePin(enable_gpio.port, enable_gpio.pin, GPIO_PIN_RESET); + } +} + +void Mcu::set_watchdog_timeout(uint32_t timeout_ms) { + uint32_t prescaler = 0; + uint32_t ticks = timeout_ms * LSI_VALUE / (watchdog_min_divider * 1000); + + while (ticks > watchdog_max_reload + 1 and prescaler < watchdog_max_prescaler) { + prescaler++; + ticks /= 2; + } + + const uint32_t reload = std::clamp(ticks, 1, watchdog_max_reload + 1) - 1; + + IWDG_TypeDef* const watchdog = family::watchdog(); + + watchdog->KR = watchdog_key_start; + watchdog->KR = watchdog_key_write; + watchdog->PR = prescaler; + watchdog->RLR = reload; + + const uint32_t start = Timer::get_counter(); + const uint32_t limit = Timer::to_cycles(watchdog_timeout_us); + + while ((watchdog->SR & (IWDG_SR_PVU | IWDG_SR_RVU)) != 0) { + if (Timer::get_counter() - start > limit) { + break; + } + } + + refresh_watchdog(); +} + +void Mcu::refresh_watchdog() { + family::watchdog()->KR = watchdog_key_reload; +} + +bool Mcu::was_reset_by_watchdog() { + return watchdog_reset; +} + +bool Mcu::is_cpu_frequency_supported() { + return cpu_frequency_supported; +} +} // namespace micras::hal diff --git a/micras_hal/stm32/src/pwm.cpp b/micras_hal/stm32/src/pwm.cpp new file mode 100644 index 0000000..d06a66e --- /dev/null +++ b/micras_hal/stm32/src/pwm.cpp @@ -0,0 +1,90 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/family.hpp" +#include "micras/hal/pwm.hpp" + +namespace micras::hal { +/** + * @brief Mask that turns the identifier of one of the first four channels into the position of its + * bits in the capture and compare enable register. + */ +static constexpr uint32_t channel_shift_mask{0x1F}; + +/** + * @brief Make a value written to the compare register of a channel take effect at once. + * + * @param handle Timer handle. + * @param channel Timer channel. + */ +static void disable_compare_preload(TIM_HandleTypeDef* handle, uint32_t channel) { + __HAL_TIM_DISABLE_OCxPRELOAD(handle, channel); +} + +/** + * @brief Make a value written to the compare register of a channel wait for the next update. + * + * @param handle Timer handle. + * @param channel Timer channel. + */ +static void enable_compare_preload(TIM_HandleTypeDef* handle, uint32_t channel) { + __HAL_TIM_ENABLE_OCxPRELOAD(handle, channel); +} + +Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_channel}, inverted{config.inverted} { + if (this->handle->State == HAL_TIM_STATE_RESET) { + config.init_function(); + } + + if (this->inverted) { + this->handle->Instance->CCER |= TIM_CCER_CC1P << (this->channel & channel_shift_mask); + } + + disable_compare_preload(this->handle, this->channel); + this->set_duty_cycle(0.0F); + enable_compare_preload(this->handle, this->channel); + + this->initialized = HAL_TIM_PWM_Start(this->handle, this->channel) == HAL_OK; +} + +void Pwm::set_duty_cycle(float duty_cycle) { + duty_cycle = std::clamp(duty_cycle, 0.0F, 100.0F); + + if (this->inverted) { + duty_cycle = 100.0F - duty_cycle; + } + + const float scaled = duty_cycle * static_cast(__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) * 0.01F; + + // NOLINTNEXTLINE(bugprone-incorrect-roundings) + const auto compare = static_cast(scaled + 0.5F); + + __HAL_TIM_SET_COMPARE(this->handle, this->channel, compare); +} + +void Pwm::set_frequency(uint32_t frequency) { + const uint32_t base_freq = family::timer_clock(this->handle->Instance); + const uint32_t prescaler = this->handle->Instance->PSC; + + const uint32_t autoreload = base_freq / ((prescaler + 1) * frequency) - 1; + __HAL_TIM_SET_AUTORELOAD(this->handle, autoreload); + __HAL_TIM_SET_COUNTER(this->handle, 0); +} + +float Pwm::get_frequency() const { + const uint32_t autoreload = __HAL_TIM_GET_AUTORELOAD(this->handle); + const bool center_aligned = (this->handle->Instance->CR1 & TIM_CR1_CMS) != 0; + const uint32_t period = center_aligned ? 2 * autoreload : autoreload + 1; + const auto ticks = static_cast(this->handle->Instance->PSC + 1) * static_cast(period); + + return static_cast(family::timer_clock(this->handle->Instance)) / ticks; +} + +bool Pwm::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/stm32/src/pwm_dma.cpp b/micras_hal/stm32/src/pwm_dma.cpp new file mode 100644 index 0000000..2441df6 --- /dev/null +++ b/micras_hal/stm32/src/pwm_dma.cpp @@ -0,0 +1,50 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/hal/pwm_dma.hpp" + +namespace micras::hal { +PwmDma::PwmDma(const Config& config) : handle{config.handle}, channel{config.timer_channel} { + if (this->handle->State == HAL_TIM_STATE_RESET) { + config.init_function(); + } + + this->initialized = this->handle->State == HAL_TIM_STATE_READY; +} + +void PwmDma::start_dma(std::span buffer) { + if (this->is_busy()) { + return; + } + + HAL_TIM_PWM_Start_DMA(this->handle, this->channel, buffer.data(), buffer.size()); +} + +void PwmDma::start_dma(std::span buffer) { + start_dma({std::bit_cast(buffer.data()), buffer.size()}); +} + +void PwmDma::stop_dma() { + HAL_TIM_PWM_Stop_DMA(this->handle, this->channel); +} + +uint32_t PwmDma::get_compare(float duty_cycle) const { + const float scaled = duty_cycle * static_cast(__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) * 0.01F; + + // NOLINTNEXTLINE(bugprone-incorrect-roundings) + return static_cast(scaled + 0.5F); +} + +bool PwmDma::is_busy() { + return TIM_CHANNEL_STATE_GET(this->handle, this->channel) == HAL_TIM_CHANNEL_STATE_BUSY; +} + +bool PwmDma::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/stm32/src/spi.cpp b/micras_hal/stm32/src/spi.cpp new file mode 100644 index 0000000..661bca4 --- /dev/null +++ b/micras_hal/stm32/src/spi.cpp @@ -0,0 +1,153 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/hal/spi.hpp" +#include "micras/hal/timer.hpp" + +extern "C" { +/** + * @brief Callback of the vendor HAL for the end of a transfer in both directions. + * + * @param hspi Handle of the bus. + */ +// NOLINTNEXTLINE(readability-identifier-naming) the name is fixed by the vendor HAL +void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef* hspi) { + micras::hal::Spi::on_transfer_end(hspi, true); +} + +/** + * @brief Callback of the vendor HAL for a transfer that ended in a bus or DMA error. + * + * @param hspi Handle of the bus. + */ +// NOLINTNEXTLINE(readability-identifier-naming) the name is fixed by the vendor HAL +void HAL_SPI_ErrorCallback(SPI_HandleTypeDef* hspi) { + micras::hal::Spi::on_transfer_end(hspi, false); +} +} + +namespace micras::hal { +std::array Spi::transferring{}; + +Spi::Spi(const Config& config) : + handle{config.handle}, + cs_gpio{config.cs_gpio}, + timeout{config.timeout}, + clock_polarity{config.clock_polarity}, + clock_phase{config.clock_phase} { + this->unselect_device(); + + if (this->handle->State == HAL_SPI_STATE_RESET) { + config.init_function(); + } + + this->initialized = this->handle->State == HAL_SPI_STATE_READY; +} + +Spi::~Spi() { + if (this->transfer == Transfer::RUNNING) { + HAL_SPI_Abort(this->handle); + this->unselect_device(); + } + + std::ranges::replace(transferring, this, static_cast(nullptr)); +} + +bool Spi::select_device() { + const uint32_t start = Timer::get_counter(); + const uint32_t limit = Timer::to_cycles(1000 * this->timeout); + + while (HAL_SPI_GetState(this->handle) != HAL_SPI_STATE_READY) { + if (Timer::get_counter() - start > limit) { + return false; + } + } + + if (this->handle->Init.CLKPolarity != this->clock_polarity or this->handle->Init.CLKPhase != this->clock_phase) { + this->handle->Init.CLKPolarity = this->clock_polarity; + this->handle->Init.CLKPhase = this->clock_phase; + + if (HAL_SPI_Init(this->handle) != HAL_OK) { + return false; + } + } + + this->cs_gpio.write(false); + return true; +} + +void Spi::unselect_device() { + this->cs_gpio.write(true); +} + +bool Spi::transmit(std::span data) { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-const-cast) the HAL takes a mutable pointer it only reads + auto* buffer = const_cast(data.data()); + + return HAL_SPI_Transmit(this->handle, buffer, data.size(), this->timeout) == HAL_OK; +} + +bool Spi::receive(std::span data) { + return HAL_SPI_Receive(this->handle, data.data(), data.size(), this->timeout) == HAL_OK; +} + +bool Spi::transmit_receive(std::span transmitted, std::span received) { + if (received.size() < transmitted.size()) { + return false; + } + + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-const-cast) the HAL takes a mutable pointer it only reads + auto* buffer = const_cast(transmitted.data()); + + return HAL_SPI_TransmitReceive(this->handle, buffer, received.data(), transmitted.size(), this->timeout) == HAL_OK; +} + +bool Spi::start_transfer(std::span transmitted, std::span received) { + auto* const slot = std::ranges::find(transferring, nullptr); + + if (received.size() < transmitted.size() or slot == transferring.end() or not this->select_device()) { + this->transfer = Transfer::FAILED; + return false; + } + + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-const-cast) the HAL takes a mutable pointer it only reads + auto* buffer = const_cast(transmitted.data()); + + *slot = this; + this->transfer = Transfer::RUNNING; + + if (HAL_SPI_TransmitReceive_DMA(this->handle, buffer, received.data(), transmitted.size()) != HAL_OK) { + *slot = nullptr; + this->transfer = Transfer::FAILED; + this->unselect_device(); + return false; + } + + return true; +} + +Spi::Transfer Spi::get_transfer() const { + return this->transfer; +} + +void Spi::on_transfer_end(const SPI_HandleTypeDef* handle, bool succeeded) { + for (Spi*& device : transferring) { + if (device != nullptr and device->handle == handle) { + device->unselect_device(); + device->transfer = succeeded ? Transfer::COMPLETE : Transfer::FAILED; + device = nullptr; + return; + } + } +} + +bool Spi::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/stm32/src/timer.cpp b/micras_hal/stm32/src/timer.cpp new file mode 100644 index 0000000..6291004 --- /dev/null +++ b/micras_hal/stm32/src/timer.cpp @@ -0,0 +1,35 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/family.hpp" +#include "micras/hal/timer.hpp" + +namespace micras::hal { +uint32_t Timer::cycles_per_microsecond{1}; + +void Timer::init() { + family::enable_cycle_counter(); + + cycles_per_microsecond = SystemCoreClock / 1000000; +} + +uint32_t Timer::get_counter() { + return DWT->CYCCNT; +} + +uint32_t Timer::get_counter_ms() { + return HAL_GetTick(); +} + +uint32_t Timer::to_microseconds(uint32_t cycles) { + return cycles / cycles_per_microsecond; +} + +uint32_t Timer::to_cycles(uint32_t microseconds) { + return microseconds * cycles_per_microsecond; +} +} // namespace micras::hal diff --git a/micras_hal/stm32/src/uart_dma.cpp b/micras_hal/stm32/src/uart_dma.cpp new file mode 100644 index 0000000..941b6bb --- /dev/null +++ b/micras_hal/stm32/src/uart_dma.cpp @@ -0,0 +1,92 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include "micras/hal/family.hpp" +#include "micras/hal/uart_dma.hpp" + +namespace micras::hal { +UartDma::UartDma(const Config& config) : handle{config.handle} { + if (this->handle->gState == HAL_UART_STATE_RESET) { + config.init_function(); + } +} + +bool UartDma::start_rx(std::span buffer) { + this->rx_buffer = buffer; + this->rx_tail = 0; + this->initialized = HAL_UART_Receive_DMA(this->handle, buffer.data(), buffer.size()) == HAL_OK; + + return this->initialized; +} + +std::size_t UartDma::rx_head() const { + return this->rx_buffer.size() - __HAL_DMA_GET_COUNTER(this->handle->hdmarx); +} + +std::size_t UartDma::read(std::span into) { + if (this->rx_buffer.empty()) { + return 0; + } + + if (this->handle->RxState != HAL_UART_STATE_BUSY_RX or not family::is_dma_enabled(this->handle->hdmarx)) { + HAL_UART_AbortReceive(this->handle); + this->start_rx(this->rx_buffer); + return 0; + } + + const std::size_t size = this->rx_buffer.size(); + const std::size_t available = (this->rx_head() + size - this->rx_tail) % size; + const std::size_t taken = std::min(available, into.size()); + const std::size_t first = std::min(taken, size - this->rx_tail); + + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) bounded just above + std::memcpy(into.data(), &this->rx_buffer[this->rx_tail], first); + + if (first < taken) { + std::memcpy(&into[first], this->rx_buffer.data(), taken - first); + } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + + this->rx_tail = (this->rx_tail + taken) % size; + + return taken; +} + +bool UartDma::start_tx(std::span from) { + if (from.empty() or this->is_transmitting()) { + return false; + } + + if (this->handle->gState == HAL_UART_STATE_BUSY_TX) { + HAL_UART_AbortTransmit(this->handle); + } + + return HAL_UART_Transmit_DMA(this->handle, std::bit_cast(from.data()), from.size()) == HAL_OK; +} + +bool UartDma::is_transmitting() const { + if (this->handle->gState != HAL_UART_STATE_BUSY_TX) { + return false; + } + + const DMA_HandleTypeDef* const dma = this->handle->hdmatx; + + if (dma->State == HAL_DMA_STATE_BUSY and not family::has_dma_finished(dma)) { + return true; + } + + return this->handle->FifoMode == UART_FIFOMODE_ENABLE and not __HAL_UART_GET_FLAG(this->handle, UART_FLAG_TXFE); +} + +bool UartDma::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_nav/CMakeLists.txt b/micras_nav/CMakeLists.txt new file mode 100644 index 0000000..b6e6f5f --- /dev/null +++ b/micras_nav/CMakeLists.txt @@ -0,0 +1,25 @@ +file(GLOB_RECURSE MICRAS_NAV_SOURCES CONFIGURE_DEPENDS "src/*.c*") + +add_library(micras_nav STATIC + ${MICRAS_NAV_SOURCES} +) + +add_library(micras::nav ALIAS micras_nav) + +target_include_directories(micras_nav PUBLIC + include +) + +# The navigation takes plain data and gives plain data back, so it links nothing that can reach the +# hardware and builds anywhere micras::core does +target_link_libraries(micras_nav PUBLIC + micras::core +) + +# A program that designs the turn tables as a constant expression needs more evaluation steps than +# clang allows by default +target_compile_options(micras_nav INTERFACE + $<$:-fconstexpr-steps=1000000000> +) + +micras_lib_setup_library(micras_nav) diff --git a/micras_nav/include/micras/nav/controller.hpp b/micras_nav/include/micras/nav/controller.hpp new file mode 100644 index 0000000..efc4ef6 --- /dev/null +++ b/micras_nav/include/micras/nav/controller.hpp @@ -0,0 +1,251 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_CONTROLLER_HPP +#define MICRAS_NAV_CONTROLLER_HPP + +#include + +#include "micras/nav/robot_model.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Computation of the motor commands that make the robot follow a reference. + * + * @details The command has two axes, forward and rotation. On each of them a feed forward, which is + * the model of the drive train fed with the speed and the acceleration of the reference, supplies + * almost all of the voltage, and a feedback closes the loop on the integrated quantities, the + * position along the path and the orientation, with a derivative term on the speed: + * + * forward = feed_forward(v, a) + K_s * e_s + D_s * (v - v_measured) + * rotation = feed_forward(omega, alpha) + K_o * (e_o + steering(e_y)) + D_o * (omega - omega_measured) + * + * The errors are those of the estimated pose as seen from the pose of the reference: e_s along the + * path, e_y across it and e_o in orientation. There is no integrator, so there is nothing to wind up + * or to reset between segments. The steering is a small and bounded offset of the orientation, + * proportional to the error across the path, which turns the robot back towards it at a rate set by + * the distance traveled rather than by time. It works wherever the pose is known, walls or not. + * + * In a curve the tires slide to the outside at a speed the lateral compliance of the model gives, + * so the robot has to point into the curve by that speed over its own, which is the compliance + * times the angular speed of the reference, to move along the path. That angle is added to the + * orientation the feedback aims at, and its rate to the angular speed. + * + * @note The gains are not tuned: they follow from the model of the drive train and from the natural + * frequency and damping asked of each axis. + */ +class Controller { +public: + /** + * @brief Closed loop behavior asked of one axis. + * + * @note The natural frequency is in rad/s. The error is clamped before it is used, which keeps a + * large error from swamping the rest of the command. It does not keep the motors from + * saturating when the robot is held, since the proportional term alone can reach the supply + * within the clamp; the robot stops on the saturation timeout for that. + */ + struct Axis { + float natural_frequency; + float damping; + float max_error; + }; + + /** + * @brief Configuration struct for the controller. + * + * @note The steering gain is in radians of offset per meter of error and the offset is limited + * to the largest steering. Below the blend speed the steering fades out, since turning in place + * cannot reduce an error across the path. The friction speed is the wheel speed over which the + * static friction compensation goes from nothing to all of it. A change of the time scale is + * itself an acceleration of the reference, of its rate times the speed, so the rate of the + * scale times the speed of the faster wheel is kept within the largest time scale acceleration. + */ + struct Config { + RobotModel model; + Axis linear; + Axis angular; + float steering_gain; + float max_steering; + float steering_blend_speed; + float friction_speed; + float voltage_reserve; + float max_time_scale_acceleration; + }; + + /** + * @brief Command for the locomotion, in percent of the motor supply. + */ + struct Command { + float forward; + float rotation; + }; + + /** + * @brief Errors and terms of the last update, for a monitor. + */ + struct Status { + float along_error; + float across_error; + float orientation_error; + float forward_feed_forward; + float rotation_feed_forward; + float forward_feedback; + float rotation_feedback; + }; + + /** + * @brief Construct a new Controller object. + * + * @param config The configuration for the controller. + */ + explicit Controller(const Config& config); + + /** + * @brief Forget the time scale and the saturation of the last run, before the next one starts. + */ + void reset(); + + /** + * @brief Compute the command for this instant. + * + * @note The reference is played at the time scale found for it, which scales its speeds by the + * scale and its accelerations by the square of the scale, plus the rate of the scale times the + * speed. + * + * @param unscaled What the robot should be doing, in the maze frame, at the full pace. + * @param estimate What the robot is doing, as far as it is known. + * @param elapsed_time Time since the previous update, in seconds. + * @return The command for the locomotion. + */ + Command update(const Reference& unscaled, const State& estimate, float elapsed_time); + + /** + * @brief Get how much slower the reference has to be played for the motors to follow it. + * + * @note The feed forward alone may ask for more voltage than there is. Slowing the clock of + * the reference scales its linear and angular speeds together, so the robot stays on the same + * path and only takes longer, instead of cutting the turn it is in. While the grip limits the + * planned motions this stays at one; where the motors limit them, a straight uses the whole of + * the voltage on the forward axis, and any rotation on top needs it. The scale is the largest + * one at which the feed forward of both wheels fits in the voltage available, found from the + * reference at the full pace, so it does not depend on the scale of the previous iteration. + * + * @return The factor to apply to the elapsed time of the reference, in (0, 1]. + */ + float get_time_scale() const; + + /** + * @brief Get the errors and terms of the last update. + * + * @return The status. + */ + const Status& get_status() const; + +private: + /** + * @brief Feedback gains of one axis. + */ + struct Gains { + float proportional; + float derivative; + }; + + /** + * @brief Compute the feedback gains of an axis from the model of the drive train. + * + * @param axis The behavior asked of the axis. + * @param speed_constant The voltage per unit of speed of the axis. + * @param acceleration_constant The voltage per unit of acceleration of the axis. + * @return The gains, in volts per unit of position and of speed. + */ + static Gains compute_gains(const Axis& axis, float speed_constant, float acceleration_constant); + + /** + * @brief Find the time scale of this iteration, moving towards the one the reference fits at. + * + * @note The scale falls as fast as the time scale acceleration allows. It rises no faster than + * that either, and only as fast as the voltage left over at the new scale lets through the + * acceleration the rise adds, so that rising never saturates the motors by itself. It does not + * rise at all while the last command saturated the motors, since the feedback is then + * catching up with the reference and needs the voltage the rise would take. + * + * @param reference What the robot should be doing, at the full pace. + * @param elapsed_time Time since the last iteration. + * @return The time scale, in (0, 1]. + */ + float find_next_time_scale(const Reference& reference, float elapsed_time) const; + + /** + * @brief Find the largest time scale at which the feed forward fits in the voltage available. + * + * @note The feed forward of each wheel is a quadratic in the scale: the friction does not scale, + * the speed term scales with it and the acceleration term with its square. So the scale is the + * largest root in (0, 1] of the wheels reaching the limit, or one if they never do. + * + * @param reference What the robot should be doing, at the full pace. + * @return The time scale, in (0, 1]. + */ + float find_time_scale(const Reference& reference) const; + + /** + * @brief Get the terms of the feed forward of each wheel, left then right. + * + * @note Each wheel has four terms, in volts: the static friction, which does not scale, the + * speed term, which scales with the time scale, the acceleration term, which scales with its + * square, and the term of the rate of the time scale, which is the acceleration a change of the + * scale adds, per unit of that rate. + * + * @param reference What the robot should be doing, at the full pace. + * @return The terms of the feed forward of each wheel. + */ + std::array, 2> get_wheel_terms(const Reference& reference) const; + + /** + * @brief Get the voltage the feed forward may use, with the reserve for the feedback left out. + * + * @return The voltage available. + */ + float get_available_voltage() const; + + /** + * @brief Parameters of the controller, with the physical description of the robot. + */ + Config config; + + /** + * @brief Feedback gains of the forward axis. + */ + Gains linear_gains; + + /** + * @brief Feedback gains of the rotation axis. + */ + Gains angular_gains; + + /** + * @brief Factor to apply to the elapsed time of the reference. + */ + float time_scale{1.0F}; + + /** + * @brief Smallest time scale, which is only reached if the friction alone takes more than the + * voltage available. + */ + static constexpr float min_time_scale{0.1F}; + + /** + * @brief Whether the last command asked more of a wheel than the supply gives. + */ + bool saturated{}; + + /** + * @brief Errors and terms of the last update. + */ + Status status{}; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_CONTROLLER_HPP diff --git a/micras_nav/include/micras/nav/curve_speed.hpp b/micras_nav/include/micras/nav/curve_speed.hpp new file mode 100644 index 0000000..522a9f3 --- /dev/null +++ b/micras_nav/include/micras/nav/curve_speed.hpp @@ -0,0 +1,184 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_CURVE_SPEED_HPP +#define MICRAS_NAV_CURVE_SPEED_HPP + +#include +#include +#include + +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::nav { +/** + * @brief Fastest motion along a curve between two speeds, as a function of time. + * + * @details The curve is sampled at equal steps of distance. Each sample gets the largest speed its + * bending allows, a pass from the start then lowers every sample to what can be reached from the one + * before, with the grip the curve leaves there, and a pass from the end lowers it to what can be + * braked from before the one after. Between two samples the acceleration is constant. A turn is + * therefore braked into as long as its curvature leaves grip to spare and accelerated out of as + * soon as it does. + * + * The passes are written over any curve, given as a function of the index of a sample that returns + * the curvature at that sample and the sharpness between it and the next one, and over storage that + * the caller owns, which is how the racing line uses them over thousands of samples. An object of + * this class holds the samples of one turn, which is all the executor needs to play it back. + */ +class CurveSpeed { +public: + /** + * @brief Largest number of steps a turn is sampled with. + */ + static constexpr uint8_t max_intervals{128}; + + /** + * @brief Distance between two samples of a turn, in meters, which the longest turns stretch. + */ + static constexpr float turn_spacing{0.005F}; + + /** + * @brief Construct an empty motion, of zero duration. + */ + CurveSpeed() = default; + + /** + * @brief Construct the motion along a turn. + * + * @note Speeds the turn does not allow are lowered, like the velocity planner would lower them, + * so the motion always covers the turn. + * + * @param shape The shape of the turn. + * @param start_speed The speed at the start of the curve. + * @param end_speed The speed wanted at the end of the curve. + * @param limits The limits of the run. + */ + CurveSpeed(const TurnShape& shape, float start_speed, float end_speed, const CurveLimits& limits); + + /** + * @brief Get the time the motion takes. + * + * @return The duration in seconds. + */ + float duration() const; + + /** + * @brief Evaluate the motion at an instant. + * + * @param time Time since the start of the motion, clamped to its duration. + * @return The distance covered, the speed and the acceleration at that instant. + */ + SpeedProfile::Sample sample(float time) const; + + /** + * @brief Get the largest speed a turn can be entered at. + * + * @param shape The shape of the turn. + * @param end_speed The speed wanted at the end of the curve. + * @param limits The limits of the run. + * @return The speed at the start when braking all the way to the end speed. + */ + static float get_entry_speed(const TurnShape& shape, float end_speed, const CurveLimits& limits); + + /** + * @brief Get the largest speed a turn can be left at. + * + * @param shape The shape of the turn. + * @param start_speed The speed at the start of the curve. + * @param limits The limits of the run. + * @return The speed at the end when accelerating all the way from the start speed. + */ + static float get_exit_speed(const TurnShape& shape, float start_speed, const CurveLimits& limits); + + /** + * @brief Fill in the speed of every sample of a curve. + * + * @tparam F Type of the function giving the bending at a sample, as described for the class. + * @param bending The bending of the curve, called for every sample. + * @param speeds The speed of each sample, one more than there are steps. + * @param spacing The distance between two samples. + * @param start_speed The speed at the first sample. + * @param end_speed The speed wanted at the last sample. + * @param limits The limits of the run. + */ + template + static void plan( + F&& bending, std::span speeds, float spacing, float start_speed, float end_speed, + const CurveLimits& limits + ); + + /** + * @brief Get the time at which every sample of a curve is reached. + * + * @param speeds The speed of each sample. + * @param spacing The distance between two samples. + * @param times The time of each sample, the first being zero. + */ + static void integrate(std::span speeds, float spacing, std::span times); + + /** + * @brief Evaluate the motion along a curve at an instant. + * + * @param speeds The speed of each sample. + * @param times The time of each sample. + * @param spacing The distance between two samples. + * @param time Time since the start of the motion, clamped to its duration. + * @return The distance covered, the speed and the acceleration at that instant. + */ + static SpeedProfile::Sample + sample(std::span speeds, std::span times, float spacing, float time); + +private: + /** + * @brief Smallest sum of the speeds of two samples a step is timed with, in m/s, which only + * matters for a curve that starts or ends at rest. + */ + static constexpr float min_speed_sum{1.0e-3F}; + + /** + * @brief Get the number of steps a turn is sampled with. + * + * @param shape The shape of the turn. + * @return The number of steps. + */ + static uint8_t get_intervals(const TurnShape& shape); + + /** + * @brief Get the largest speed at a sample. + * + * @param before The bending between the sample before and this one. + * @param at The bending at this sample and between it and the next one. + * @param limits The limits of the run. + * @return The speed limit, which takes the sharper side of the sample. + */ + static float get_limit(const Bending& before, const Bending& at, const CurveLimits& limits); + + /** + * @brief Speed at every sample. + */ + std::array speeds{}; + + /** + * @brief Time at which every sample is reached. + */ + std::array times{}; + + /** + * @brief Number of steps between the samples. + */ + uint8_t intervals{}; + + /** + * @brief Distance between two samples. + */ + float spacing{}; +}; +} // namespace micras::nav + +#include "micras/nav/impl/curve_speed.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_CURVE_SPEED_HPP diff --git a/micras_nav/include/micras/nav/drive_identification.hpp b/micras_nav/include/micras/nav/drive_identification.hpp new file mode 100644 index 0000000..6384196 --- /dev/null +++ b/micras_nav/include/micras/nav/drive_identification.hpp @@ -0,0 +1,258 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_DRIVE_IDENTIFICATION_HPP +#define MICRAS_NAV_DRIVE_IDENTIFICATION_HPP + +#include +#include + +#include "micras/nav/controller.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/robot_model.hpp" + +namespace micras::nav { +/** + * @brief Procedure that measures the constants of the drive train on the robot itself. + * + * @details The robot is driven open loop through a fixed sequence: a slow ramp until the wheels + * start to turn, a step forward and the same step backward, then a step of rotation to each side. + * Every step has two levels, the full command and then a fraction of it, because the response to a + * single level cannot tell the friction from the speed constant. + * Each axis is modeled as `u = k_s * sign(v) + k_v * v + k_a * dv/dt`, and the three constants are + * fitted to the whole response by least squares. The model is fitted in its integrated form, + * `integral(u) = k_s * integral(sign(v)) + k_v * x + k_a * (v - v0)`, which uses the position and + * the speed as they are measured and never differentiates a noisy signal. + * + * The constants come out in volts, and get_model() turns them into the physical constants of the + * robot model, so the result of a run is a set of numbers to type into the configuration. + * + * @note The robot stays between where it started and the distance limit ahead of it, as far as the + * model it starts from is right: a step ends early enough for the robot to coast to a stop inside + * that stretch. It needs that much free floor ahead of it, and some margin on both sides for a + * model that is off. It should run with the fan as it is used in a fast run. + */ +class DriveIdentification { +public: + /** + * @brief Configuration struct for the identification. + * + * @note The commands are in percent of the motor supply. The ramp rate is in percent per + * second, and the wheels count as turning once they are faster than the breakaway speed. Each + * level of a step lasts for the step time, a step forward also ends once the robot would coast + * past the distance limit and a step backward once it would coast past where it started, and the + * robot rests for the rest time between two steps. + */ + struct Config { + RobotModel model; + float ramp_rate; + float breakaway_speed; + float linear_command; + float angular_command; + float step_time; + float rest_time; + float max_distance; + }; + + /** + * @brief Constants of one axis, in volts per unit of speed and of acceleration. + */ + struct Axis { + float static_friction; + float speed_constant; + float acceleration_constant; + }; + + /** + * @brief Construct a new Drive Identification object. + * + * @param config The configuration for the identification. + */ + explicit DriveIdentification(const Config& config); + + /** + * @brief Start the procedure. + * + * @param measurements The current measurements. + */ + void start(const Measurements& measurements); + + /** + * @brief Advance the procedure by one iteration. + * + * @param measurements The current measurements. + * @param elapsed_time Time since the last iteration, in seconds. + * @return The command to apply to the locomotion. + */ + Controller::Command update(const Measurements& measurements, float elapsed_time); + + /** + * @brief Check if the procedure has ended. + * + * @return True once every step was driven and the constants were fitted. + */ + bool is_finished() const; + + /** + * @brief Check if the fit of both axes was well conditioned. + * + * @return True if the result can be trusted. + */ + bool is_valid() const; + + /** + * @brief Get the voltage at which the wheels started to turn. + * + * @return The breakaway voltage, per motor. + */ + float get_breakaway_voltage() const; + + /** + * @brief Get the constants fitted for the forward axis. + * + * @return The constants, in V, V*s/m and V*s^2/m. + */ + const Axis& get_linear() const; + + /** + * @brief Get the constants fitted for the rotation axis. + * + * @return The constants, in V, V*s/rad and V*s^2/rad. + */ + const Axis& get_angular() const; + + /** + * @brief Get the robot model with the drive constants replaced by the ones that were measured. + * + * @note The torque constant follows from the speed constant, the resistance from the + * acceleration constant and the mass, and the yaw inertia from the angular acceleration + * constant. The mass, the wheel radius, the track width and the gear ratio are taken as given. + * + * @return The model. + */ + RobotModel get_model() const; + +private: + /** + * @brief Command of the second level of a step, as a fraction of the first. + */ + static constexpr float second_level{0.5F}; + + /** + * @brief Steps of the procedure. + */ + enum class Phase : uint8_t { + RAMP = 0, + FORWARD = 1, + BACKWARD = 2, + LEFT = 3, + RIGHT = 4, + FINISHED = 5, + }; + + /** + * @brief Accumulator of a least squares fit with three unknowns. + */ + struct Fit { + /** + * @brief Add one sample of the integrated model. + * + * @param regressors The integral of the sign of the speed, the position and the speed. + * @param response The integral of the voltage. + */ + void add(const std::array& regressors, float response); + + /** + * @brief Solve for the constants. + * + * @param axis The constants, which are only written if the problem is well conditioned. + * @return True if the constants were found. + */ + bool solve(Axis& axis) const; + + std::array, 3> normal{}; + std::array moment{}; + }; + + /** + * @brief Move on to the next phase, after a rest. + */ + void advance(); + + /** + * @brief Parameters of the procedure, with the physical description of the robot. + */ + Config config; + + /** + * @brief Current step of the procedure. + */ + Phase phase{Phase::FINISHED}; + + /** + * @brief Time since the current step started. + */ + float phase_time{}; + + /** + * @brief Time left to rest before the current step drives the motors. + */ + float rest_left{}; + + /** + * @brief Wheel angles at the last iteration. + */ + ///@{ + float last_left_angle{}; + float last_right_angle{}; + ///@} + + /** + * @brief Integrals of the current step: voltage, sign of the speed and position. + */ + ///@{ + float voltage_integral{}; + float sign_integral{}; + float position{}; + ///@} + + /** + * @brief Speed at the start of the current step. + */ + float start_speed{}; + + /** + * @brief Distance traveled forward since the procedure started. + */ + float distance{}; + + /** + * @brief Voltage at which the wheels started to turn. + */ + float breakaway_voltage{}; + + /** + * @brief Fit of each axis. + */ + ///@{ + Fit linear_fit{}; + Fit angular_fit{}; + ///@} + + /** + * @brief Constants fitted for each axis. + */ + ///@{ + Axis linear{}; + Axis angular{}; + ///@} + + /** + * @brief Whether both fits were well conditioned. + */ + bool valid{}; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_DRIVE_IDENTIFICATION_HPP diff --git a/micras_nav/include/micras/nav/executor.hpp b/micras_nav/include/micras/nav/executor.hpp new file mode 100644 index 0000000..09d03c1 --- /dev/null +++ b/micras_nav/include/micras/nav/executor.hpp @@ -0,0 +1,226 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_EXECUTOR_HPP +#define MICRAS_NAV_EXECUTOR_HPP + +#include +#include +#include + +#include "micras/nav/curve_speed.hpp" +#include "micras/nav/line.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Playback of a route as a reference in time. + * + * @details The executor owns the clock of the segment in progress and turns it into a pose, a twist + * and an acceleration in the maze frame, by composing the nominal start of the segment with the + * motion inside it. The reference depends on time alone, never on where the robot is, so a + * correction of the pose estimate cannot make it jump, and the time a route takes is exactly the + * time the planner computed for it. + */ +class Executor { +public: + /** + * @brief Configuration struct for the executor. + * + * @note The tolerances tell when the robot has settled against a wall: the pose error and the + * speeds have to stay below them for the settle time. + */ + struct Config { + std::size_t capacity; + float settle_distance; + float settle_angle; + float settle_linear_speed; + float settle_angular_speed; + float settle_time; + }; + + /** + * @brief Construct a new Executor object. + * + * @param dynamics The physical limits of the robot, borrowed for the lifetime of the executor. + * @param line The racing line a segment of that kind plays back, borrowed for the same time. + * @param config The configuration for the executor. + */ + Executor(const Dynamics& dynamics, const Line& line, const Config& config); + + /** + * @brief Drop every segment and stand still at a pose. + * + * @param pose The pose to hold. + * @param profile The profile of the run about to start. + */ + void reset(const Pose& pose, const RunProfile& profile); + + /** + * @brief Queue segments after the ones already queued. + * + * @note The segments already done are dropped first, so nothing is allocated as long as the + * number of segments waiting stays within the capacity, which a route planned with the robot + * stopped may exceed and the few segments queued per cell while exploring never do. + * + * @param segments The segments, with their speeds already planned. + */ + void push(std::span segments); + + /** + * @brief Advance the clock and get the reference for this instant. + * + * @note A time scale below one plays the reference in slow motion: the clock advances by less + * than the time that passed. The reference comes out at the full pace, and the controller + * scales its speeds to match, so the path stays the same. + * + * @param elapsed_time Time since the last update, in seconds. + * @param time_scale Factor applied to the elapsed time, in (0, 1]. + * @param estimate The estimated state of the robot, which only tells when it has settled. + * @return The reference in the maze frame, at the full pace. + */ + Reference update(float elapsed_time, float time_scale, const State& estimate); + + /** + * @brief Check if there is nothing left to execute. + * + * @return True if every segment has ended. + */ + bool is_finished() const; + + /** + * @brief Check if the last segment queued ends within a time from now. + * + * @note Whoever feeds the executor cell by cell asks this every iteration, so as to decide the + * next move as late as possible and still hand it over before the reference runs out. + * + * @param horizon The time ahead to look, in seconds. + * @return True if everything that is queued ends within the horizon. + */ + bool is_ending(float horizon) const; + + /** + * @brief Replace everything after a point of the segment in progress by other segments. + * + * @note Used to stop short of a wall that was seen late. The segment in progress must be a + * straight, which is cut where the robot is, and the new segments start from there. + * + * @param segments The segments to execute instead, the first starting at the current speed. + */ + void divert(std::span segments); + + /** + * @brief Get the segment in progress. + * + * @return The segment, or a null pointer if every segment has ended. + */ + const Segment* get_current() const; + + /** + * @brief Get the last reference produced. + * + * @return The reference. + */ + const Reference& get_reference() const; + +private: + /** + * @brief Start the next segment, carrying over the time the previous one did not use. + */ + void start_next(); + + /** + * @brief Evaluate the segment in progress at the current clock. + * + * @return The reference in the maze frame. + */ + Reference evaluate() const; + + /** + * @brief Check if the robot has settled at the reference. + * + * @param estimate The estimated state of the robot. + * @param elapsed_time Time since the last update, in seconds. + * @return True if the errors and the speeds have been small for long enough. + */ + bool has_settled(const State& estimate, float elapsed_time); + + /** + * @brief Physical limits of the robot. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + const Dynamics& dynamics; + + /** + * @brief Racing line, for the segment that drives it. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + const Line& line; + + /** + * @brief Tolerances to consider the robot settled. + */ + Config config; + + /** + * @brief Profile of the run in progress. + */ + RunProfile run_profile{}; + + /** + * @brief Segments queued, the ones before the index being already done. + */ + std::vector segments; + + /** + * @brief Time each of the segments queued takes. + */ + std::vector durations; + + /** + * @brief Index of the segment in progress. + */ + std::size_t index{}; + + /** + * @brief Time the segments queued after the one in progress take. + */ + float queued_time{}; + + /** + * @brief Time since the start of the segment in progress. + */ + float clock{}; + + /** + * @brief Duration of the segment in progress. + */ + float duration{}; + + /** + * @brief Motion along the segment in progress, for the straights and the rotations in place. + */ + SpeedProfile speed_profile; + + /** + * @brief Motion along the segment in progress, for the turns. + */ + CurveSpeed curve_speed; + + /** + * @brief Time the robot has been within the settling tolerances. + */ + float settled_time{}; + + /** + * @brief Last reference produced, which is held when there is nothing to execute. + */ + Reference reference{}; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_EXECUTOR_HPP diff --git a/micras_nav/include/micras/nav/explorer.hpp b/micras_nav/include/micras/nav/explorer.hpp new file mode 100644 index 0000000..4ab04e9 --- /dev/null +++ b/micras_nav/include/micras/nav/explorer.hpp @@ -0,0 +1,176 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_EXPLORER_HPP +#define MICRAS_NAV_EXPLORER_HPP + +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" + +namespace micras::nav { +/** + * @brief Choice of what is still worth visiting, and of when the search is over. + * + * @details The fastest route that could exist is the one the planner finds when every wall that was + * never seen is taken as absent. If that route crosses no unknown wall, the map already proves that + * nothing faster exists and the search is over. Otherwise the cells next to the unknown walls it + * crosses are what is worth visiting, and the question is asked again once the map has changed. The + * test uses the cost of the planner, whose candidates the fast run is chosen among, so a route + * faster by more than the error of that cost cannot stay hidden, and only cells on some best + * conceivable route are ever targeted. A route priced within that error of a better one may be + * dropped by the planner even when it would drive faster. + * + * The fastest route depends on the profile of the run, so the test is repeated for every profile the + * map has to be good for and the targets are put together. + * + * @note The planner is advanced a bounded number of edges per call, which lets all of this happen + * while the robot is moving. A round of every profile takes seconds, and a wall is decided about + * once per cell, so a round is not restarted when the map changes: it runs to its end and the next + * one starts from the new map, which keeps the targets at most one round old. The planner reads the + * map as it goes, so a round the map changed during may name targets but never proves the search + * over; only a round on an unchanged map does. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + */ +template +class TExplorer { +public: + /** + * @brief How much slower than the best a candidate route can be and still be looked at. + * + * @note The search prices a route to within a few hundredths of a second of what the velocity + * planner will make of it, so a candidate this close to the best may turn out to be the best. + */ + static constexpr float candidate_window{0.05F}; + + /** + * @brief Largest number of cells that can be targeted at once. + */ + static constexpr uint8_t max_targets{64}; + + /** + * @brief Construct a new TExplorer object. + * + * @param planner The planner, borrowed for the lifetime of the explorer and shared with whoever + * plans the fast run, which never happens at the same time. + * @param profiles The profiles the map has to be good for, borrowed for the same time. + */ + TExplorer(TPlanner& planner, std::span profiles); + + /** + * @brief Forget the targets and start asking again. + */ + void reset(); + + /** + * @brief Advance the planning. + * + * @param maze The map of the walls. + * @param max_edges The largest number of edges the planner may try in this call. + * @return True if the targets changed. + */ + bool update(const TMaze& maze, uint32_t max_edges); + + /** + * @brief Check if an answer for the current map exists yet. + * + * @return True once every profile was planned at least once since the last reset. + */ + bool has_targets() const; + + /** + * @brief Check if the map proves that the fastest route of every profile is known. + * + * @return True if nothing is left that is worth visiting. + */ + bool is_complete() const; + + /** + * @brief Get the cells worth visiting. + * + * @return The cells next to the unknown walls of the fastest conceivable routes. + */ + std::span get_targets() const; + +private: + /** + * @brief Add the cells on both sides of a wall to the targets being collected. + * + * @param wall The cell and the side of it where the wall is. + */ + void add_target(const GridPose& wall); + + /** + * @brief Planner used to find the fastest conceivable routes. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + TPlanner& planner; + + /** + * @brief Profiles the map has to be good for. + */ + std::span profiles; + + /** + * @brief Route being inspected, kept to reuse its storage. + */ + Route route; + + /** + * @brief Cells worth visiting, as of the last complete round. + */ + std::array targets{}; + + /** + * @brief Number of cells worth visiting. + */ + uint8_t number_of_targets{}; + + /** + * @brief Cells collected by the round in progress. + */ + std::array collected{}; + + /** + * @brief Number of cells collected by the round in progress. + */ + uint8_t number_collected{}; + + /** + * @brief Index of the profile being planned. + */ + uint8_t profile_index{}; + + /** + * @brief Whether the planner is in the middle of a search of this explorer. + */ + bool planning{}; + + /** + * @brief Whether a round was completed since the last reset. + */ + bool answered{}; + + /** + * @brief Revision of the map the round in progress started from. + */ + uint32_t round_revision{}; + + /** + * @brief Revision of the map the current targets were computed for. + */ + uint32_t answered_revision{}; +}; +} // namespace micras::nav + +#include "micras/nav/impl/explorer.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_EXPLORER_HPP diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp new file mode 100644 index 0000000..4dd2355 --- /dev/null +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -0,0 +1,185 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_GRID_POSE_HPP +#define MICRAS_NAV_GRID_POSE_HPP + +#include +#include +#include +#include + +#include "micras/core/vector.hpp" + +namespace micras::nav { +/** + * @brief Possible sides in the grid. + */ +enum class Side : uint8_t { + RIGHT = 0, + UP = 1, + LEFT = 2, + DOWN = 3 +}; + +/** + * @brief All the sides in the grid, in order, for iteration. + */ +constexpr std::array all_sides{Side::RIGHT, Side::UP, Side::LEFT, Side::DOWN}; + +/** + * @brief Convert an angle in radians to a Grid side. + * + * @param angle The angle in radians. + * @return The corresponding Grid side. + */ +Side angle_to_grid(float angle); + +/** + * @brief Type to store a point in the grid. + */ +struct GridPoint { + /** + * @brief Return the direction from this point to the next one. + * + * @param next The next point. + * @return The direction from this point to the next one. + */ + Side direction(const GridPoint& next) const; + + /** + * @brief Convert the point to a grid point. + * + * @note A point outside of the first quadrant is clamped to it, since a negative coordinate + * cannot be represented and converting one to an unsigned type is undefined. + * + * @param point The point to convert. + * @param cell_size The size of the grid cells. + * @return The grid point corresponding to the point. + */ + static GridPoint from_vector(const core::Vector& point, float cell_size); + + /** + * @brief Convert a grid point to a point. + * + * @param cell_size The size of the grid cells. + * @return The point corresponding to the grid point. + */ + core::Vector to_vector(float cell_size) const; + + /** + * @brief Move in the grid in the direction of the side. + * + * @param side The side to move to. + * @return The new point after moving. + */ + GridPoint operator+(const Side& side) const; + + /** + * @brief Compare two points for equality. + * + * @param other The other point to compare. + * @return True if the points are equal, false otherwise. + */ + bool operator==(const GridPoint& other) const; + + /** + * @brief The x coordinate of the point on the grid. + */ + uint8_t x; + + /** + * @brief The y coordinate of the point on the grid. + */ + uint8_t y; +}; + +struct GridPose { + /** + * @brief Return the pose after moving forward. + * + * @return The pose after moving forward. + */ + GridPose front() const; + + /** + * @brief Return the pose after turning back. + * + * @return The pose after turning back. + */ + GridPose turned_back() const; + + /** + * @brief Return the pose after turning left. + * + * @return The pose after turning left. + */ + GridPose turned_left() const; + + /** + * @brief Return the pose after turning right. + * + * @return The pose after turning right. + */ + GridPose turned_right() const; + + /** + * @brief Gets the relative side of a grid point compared to current pose. + * + * @param other The grid point to check. + * @return LEFT/RIGHT for left/right of pose, UP for in front, DOWN for behind. + */ + Side get_relative_side(const GridPoint& other) const; + + /** + * @brief Compare two poses for equality. + * + * @param other The other pose to compare. + * @return True if the poses are equal, false otherwise. + */ + bool operator==(const GridPose& other) const; + + /** + * @brief The position of the pose on the grid. + */ + GridPoint position; + + /** + * @brief The orientation of the pose on the grid. + */ + Side orientation; +}; +} // namespace micras::nav + +namespace std { +/** + * @brief Hash specialization for the GridPoint type. + * + * @tparam T GridPoint type. + */ +template <> +struct hash { + size_t operator()(const micras::nav::GridPoint& point) const noexcept { + const size_t h1 = hash{}(point.x); + const size_t h2 = hash{}(point.y); + return h1 ^ (h2 << 1); + } +}; + +/** + * @brief Hash specialization for the GridPose type. + * + * @tparam T GridPose type. + */ +template <> +struct hash { + size_t operator()(const micras::nav::GridPose& pose) const noexcept { + const size_t h1 = hash{}(pose.position); + const size_t h2 = hash{}(std::to_underlying(pose.orientation)); + return h1 ^ (h2 << 1); + } +}; +} // namespace std + +#endif // MICRAS_NAV_GRID_POSE_HPP diff --git a/micras_nav/include/micras/nav/gyroscope_calibration.hpp b/micras_nav/include/micras/nav/gyroscope_calibration.hpp new file mode 100644 index 0000000..aee7081 --- /dev/null +++ b/micras_nav/include/micras/nav/gyroscope_calibration.hpp @@ -0,0 +1,176 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_GYROSCOPE_CALIBRATION_HPP +#define MICRAS_NAV_GYROSCOPE_CALIBRATION_HPP + +#include + +#include "micras/nav/measurements.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/robot_model.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Procedure that measures the scale factor of the gyroscope. + * + * @details The robot is placed facing a wall and turns a whole number of times in place. Its + * orientation relative to the wall is measured before and after with the two sensors that look + * forward, from the line through the two points they see, so the angle it really turned is known + * to a fraction of a degree however many turns it made. Dividing it by what the gyroscope + * integrated over the same time gives the factor that corrects the sensitivity of the gyroscope, + * which is the constant to type into the robot model. + * + * @note More turns divide the error of the two wall measurements by a larger angle, so the result + * gets better with the number of turns, as long as the robot stays in front of the wall. + */ +class GyroscopeCalibration { +public: + /** + * @brief Configuration struct for the calibration. + * + * @note The sensors are the two that look forward, to the left and to the right of the center + * line. The robot stands still for the settle time before and after turning, and the wall is + * measured over the second half of it. + */ + struct Config { + RobotModel model; + uint8_t left_sensor; + uint8_t right_sensor; + float turns; + float settle_time; + }; + + /** + * @brief Construct a new Gyroscope Calibration object. + * + * @param config The configuration for the calibration. + */ + explicit GyroscopeCalibration(const Config& config); + + /** + * @brief Start the procedure. + * + * @param pose The pose of the robot, which is held while it turns. + * @param limits The limits of the rotation in place. + */ + void start(const Pose& pose, const MotionLimits& limits); + + /** + * @brief Advance the procedure by one iteration. + * + * @param measurements The current measurements. + * @param bias The bias of the gyroscope, in rad/s after the scale correction. + * @param elapsed_time Time since the last iteration, in seconds. + * @return What the robot should be doing at this instant. + */ + Reference update(const Measurements& measurements, float bias, float elapsed_time); + + /** + * @brief Check if the procedure has ended. + * + * @return True once the wall was measured for the second time. + */ + bool is_finished() const; + + /** + * @brief Check if the wall was seen by both sensors during both measurements. + * + * @return True if the result can be trusted. + */ + bool is_valid() const; + + /** + * @brief Get the scale factor that was measured. + * + * @return The value for the gyroscope scale of the robot model. + */ + float get_scale() const; + +private: + /** + * @brief Steps of the procedure. + */ + enum class Phase : uint8_t { + BEFORE = 0, + TURNING = 1, + AFTER = 2, + FINISHED = 3, + }; + + /** + * @brief Get the orientation of the robot relative to the wall ahead of it. + * + * @param measurements The current measurements. + * @return The angle between the heading of the robot and the perpendicular of the wall. + */ + float get_wall_angle(const Measurements& measurements) const; + + /** + * @brief Parameters of the procedure, with the physical description of the robot. + */ + Config config; + + /** + * @brief Current step of the procedure. + */ + Phase phase{Phase::FINISHED}; + + /** + * @brief Time since the current step started. + */ + float phase_time{}; + + /** + * @brief Pose held by the robot, whose orientation is the one before turning. + */ + Pose pose{}; + + /** + * @brief Rotation in place. + */ + SpeedProfile spin; + + /** + * @brief Sum and number of the measurements of the wall being averaged. + */ + ///@{ + float angle_sum{}; + uint32_t angle_count{}; + ///@} + + /** + * @brief Orientation relative to the wall before turning. + */ + float angle_before{}; + + /** + * @brief Rotation reported by the raw gyroscope while turning, and the time it took. + */ + ///@{ + float raw_rotation{}; + float turning_time{}; + ///@} + + /** + * @brief Bias of the gyroscope when the robot started to turn. + */ + float bias{}; + + /** + * @brief Scale factor that was measured. + */ + float scale{1.0F}; + + /** + * @brief Whether both measurements of the wall were good. + */ + bool valid{}; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_GYROSCOPE_CALIBRATION_HPP diff --git a/micras_nav/include/micras/nav/impl/curve_speed.tpp b/micras_nav/include/micras/nav/impl/curve_speed.tpp new file mode 100644 index 0000000..c45f658 --- /dev/null +++ b/micras_nav/include/micras/nav/impl/curve_speed.tpp @@ -0,0 +1,56 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_CURVE_SPEED_TPP +#define MICRAS_NAV_CURVE_SPEED_TPP + +#include +#include +#include +#include + +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::nav { +template +void CurveSpeed::plan( + // NOLINTNEXTLINE(cppcoreguidelines-missing-std-forward) called once per sample, so never forwarded + F&& bending, std::span speeds, float spacing, float start_speed, float end_speed, const CurveLimits& limits +) { + if (speeds.empty()) { + return; + } + + const std::size_t last = speeds.size() - 1; + + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) every index is at most last + + Bending before = bending(0); + + for (std::size_t i = 0; i <= last; i++) { + const Bending at = bending(i); + speeds[i] = get_limit(before, at, limits); + before = at; + } + + speeds[0] = std::min(speeds[0], start_speed); + + for (std::size_t i = 0; i < last; i++) { + const float acceleration = limits.get_acceleration(speeds[i], bending(i)); + speeds[i + 1] = std::min(speeds[i + 1], std::sqrt(speeds[i] * speeds[i] + 2.0F * spacing * acceleration)); + } + + speeds[last] = std::min(speeds[last], end_speed); + + for (std::size_t i = last; i > 0; i--) { + const Bending at{.curvature = bending(i).curvature, .sharpness = bending(i - 1).sharpness}; + const float deceleration = limits.get_deceleration(speeds[i], at); + speeds[i - 1] = std::min(speeds[i - 1], std::sqrt(speeds[i] * speeds[i] + 2.0F * spacing * deceleration)); + } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) +} +} // namespace micras::nav + +#endif // MICRAS_NAV_CURVE_SPEED_TPP diff --git a/micras_nav/include/micras/nav/impl/explorer.tpp b/micras_nav/include/micras/nav/impl/explorer.tpp new file mode 100644 index 0000000..944db45 --- /dev/null +++ b/micras_nav/include/micras/nav/impl/explorer.tpp @@ -0,0 +1,134 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_EXPLORER_TPP +#define MICRAS_NAV_EXPLORER_TPP + +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/planner.hpp" + +namespace micras::nav { +template +TExplorer::TExplorer(TPlanner& planner, std::span profiles) : + planner{planner}, profiles{profiles} { + this->route.steps.reserve(static_cast(width) * height); +} + +template +void TExplorer::reset() { + this->number_of_targets = 0; + this->number_collected = 0; + this->profile_index = 0; + this->planning = false; + this->answered = false; +} + +template +bool TExplorer::update(const TMaze& maze, uint32_t max_edges) { + if (this->profiles.empty()) { + this->answered = true; + return false; + } + + if (not this->planning) { + if (this->answered and maze.get_revision() == this->answered_revision) { + return false; + } + + if (this->profile_index == 0) { + this->number_collected = 0; + this->round_revision = maze.get_revision(); + } + + this->planner.begin(maze, WallAssumption::OPTIMISTIC, this->profiles[this->profile_index]); + this->planning = true; + } + + if (not this->planner.step(max_edges)) { + return false; + } + + this->planning = false; + + float best_time = 0.0F; + + for (uint8_t i = 0; i < this->planner.get_number_of_routes(); i++) { + this->planner.get_route(i, this->route); + + if (i == 0) { + best_time = this->route.time; + } else if (this->route.time > best_time + candidate_window) { + break; + } + + TPlanner::for_each_wall(this->route, [this, &maze](const GridPose& wall) { + if (maze.get_wall(wall) == WallState::UNKNOWN) { + this->add_target(wall); + } + }); + } + + this->profile_index++; + + if (this->profile_index < this->profiles.size()) { + return false; + } + + this->profile_index = 0; + + if (this->number_collected == 0 and maze.get_revision() != this->round_revision) { + return false; + } + + const bool changed = + not this->answered or this->number_collected != this->number_of_targets or + not std::equal( + this->collected.begin(), this->collected.begin() + this->number_collected, this->targets.begin() + ); + + this->targets = this->collected; + this->number_of_targets = this->number_collected; + this->answered = true; + this->answered_revision = this->round_revision; + + return changed; +} + +template +bool TExplorer::has_targets() const { + return this->answered; +} + +template +bool TExplorer::is_complete() const { + return this->answered and this->number_of_targets == 0; +} + +template +std::span TExplorer::get_targets() const { + return std::span{this->targets}.first(this->number_of_targets); +} + +template +void TExplorer::add_target(const GridPose& wall) { + for (const GridPoint& cell : {wall.position, wall.front().position}) { + const auto end = this->collected.begin() + this->number_collected; + + if (not TMaze::contains(cell) or this->number_collected == max_targets or + std::find(this->collected.begin(), end, cell) != end) { + continue; + } + + this->collected.at(this->number_collected) = cell; + this->number_collected++; + } +} +} // namespace micras::nav + +#endif // MICRAS_NAV_EXPLORER_TPP diff --git a/micras_nav/include/micras/nav/impl/localizer.tpp b/micras_nav/include/micras/nav/impl/localizer.tpp new file mode 100644 index 0000000..f05e2ae --- /dev/null +++ b/micras_nav/include/micras/nav/impl/localizer.tpp @@ -0,0 +1,173 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_LOCALIZER_TPP +#define MICRAS_NAV_LOCALIZER_TPP + +#include +#include +#include +#include + +#include "micras/nav/maze.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/robot_model.hpp" +#include "micras/nav/state.hpp" +#include "micras/nav/wall_model.hpp" + +namespace micras::nav { +template +void Localizer::correct( + const Measurements& measurements, const WallModel& wall_model, const TMaze& maze +) { + if (std::abs(this->state.velocity.angular) > this->config.max_angular_speed) { + for (EdgeTracker& tracker : this->edge_trackers) { + tracker.locked = false; + } + + return; + } + + const bool stationary = this->is_stationary(); + const float gate = stationary ? this->config.stationary_gate : this->config.gate; + + for (uint8_t sensor = 0; sensor < number_of_wall_sensors; sensor++) { + const WallReading& reading = measurements.walls.at(sensor); + + if (not reading.is_new) { + continue; + } + + const Pose sampled = this->state.pose.compose({ + .position = {.x = -this->state.velocity.linear * this->config.range_delay, .y = 0.0F}, + .orientation = -this->state.velocity.angular * this->config.range_delay, + }); + + const RayHit hit = wall_model.cast(sampled, sensor, maze); + + if (wall_model.is_side_looking(sensor)) { + this->track_edge(sensor, reading, sampled, hit, wall_model, maze); + } + + if (not reading.valid or not hit.valid or hit.state != WallState::WALL) { + continue; + } + + const float range = wall_model.get_range(reading.distance, sensor, hit); + + if (std::min(range, hit.range) > this->config.max_range or + not wall_model.is_footprint_clear( + hit, sensor, this->get_position_deviation(), this->get_orientation_deviation() + )) { + continue; + } + + const float deviation = wall_model.get_range_deviation(hit.range); + + const bool accepted = this->update( + range - hit.range, {hit.jacobian.at(0), hit.jacobian.at(1), hit.jacobian.at(2), 0.0F}, + deviation * deviation * this->config.range_correlation, gate, this->config.max_position_correction + ); + + this->count_range(accepted, stationary); + } +} + +template +void Localizer::track_edge( + uint8_t sensor, const WallReading& reading, const Pose& sampled, const RayHit& hit, const WallModel& wall_model, + const TMaze& maze +) { + EdgeTracker& tracker = this->edge_trackers.at(sensor); + + const auto tolerance = [this](float range) { + return this->config.range_tolerance + this->config.relative_range_tolerance * range; + }; + + if (this->state.velocity.linear <= this->config.edge_speed) { + tracker.locked = false; + return; + } + + if (tracker.locked) { + const PlaneCrossing crossing = wall_model.cross(sampled, sensor, tracker.hit); + const bool reading_on = reading.valid and reading.distance < crossing.range + tolerance(crossing.range); + + if (crossing.range > this->config.edge_range) { + tracker.locked = false; + } else if (reading_on != tracker.reading_on) { + tracker.reading_on = reading_on; + + const float heading = this->state.pose.orientation; + const float travel = + this->state.velocity.linear * (tracker.hit.vertical ? std::sin(heading) : std::cos(heading)); + const float face = this->config.model.maze.wall_thickness * std::abs(crossing.slope); + const std::optional edge = this->find_edge(maze, tracker.hit.wall, travel > 0.0F, reading_on, face); + + if (edge.has_value() and std::abs(*edge - crossing.offset) <= this->config.edge_window and + this->update( + *edge - crossing.offset, + {crossing.jacobian.at(0), crossing.jacobian.at(1), crossing.jacobian.at(2), 0.0F}, + this->config.edge_deviation * this->config.edge_deviation, this->config.gate, + this->config.max_edge_correction + )) { + this->status.edges++; + } + } + } + + const bool on_wall = hit.valid and hit.state == WallState::WALL and hit.range < this->config.edge_range and + std::abs(reading.distance - hit.range) < tolerance(hit.range) and reading.valid; + + if (on_wall) { + tracker.locked = true; + tracker.reading_on = true; + tracker.hit = hit; + } +} + +template +std::optional Localizer::find_edge( + const TMaze& maze, const GridPose& wall, bool upwards, bool start, float face +) const { + const bool vertical = wall.orientation == Side::LEFT or wall.orientation == Side::RIGHT; + const float cell_size = this->config.model.maze.cell_size; + const float half_wall = this->config.model.maze.wall_thickness / 2.0F; + const float sign = upwards ? 1.0F : -1.0F; + + Side step = upwards ? Side::UP : Side::DOWN; + + if (not vertical) { + step = upwards ? Side::RIGHT : Side::LEFT; + } + + GridPose segment = wall; + bool in_gap = false; + + for (uint8_t shift = 1; shift <= edge_search_cells; shift++) { + segment.position = segment.position + step; + + const WallState state = + TMaze::contains(segment.position) ? maze.get_wall(segment) : WallState::UNKNOWN; + + if (state == WallState::UNKNOWN) { + return std::nullopt; + } + + const bool open = state == WallState::NO_WALL; + in_gap = in_gap or open; + + if (start ? (in_gap and not open) : open) { + const auto cells = static_cast(shift); + const float boundary = (upwards ? cells : 1.0F - cells) * cell_size; + + return start ? boundary - sign * (half_wall + face) : boundary + sign * half_wall; + } + } + + return std::nullopt; +} +} // namespace micras::nav + +#endif // MICRAS_NAV_LOCALIZER_TPP diff --git a/micras_nav/include/micras/nav/impl/maze.tpp b/micras_nav/include/micras/nav/impl/maze.tpp new file mode 100644 index 0000000..3972e4d --- /dev/null +++ b/micras_nav/include/micras/nav/impl/maze.tpp @@ -0,0 +1,270 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_MAZE_TPP +#define MICRAS_NAV_MAZE_TPP + +#include +#include +#include +#include +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" + +namespace micras::nav { +template +TMaze::TMaze(const Config& config) : start{config.start}, goal{config.goal} { + this->reset(); +} + +template +void TMaze::reset() { + for (auto& row : this->walls) { + for (auto& cell : row) { + cell.fill(WallState::UNKNOWN); + } + } + + for (uint8_t row = 0; row < height; row++) { + this->write_wall({.position = {.x = 0, .y = row}, .orientation = Side::LEFT}, WallState::WALL); + this->write_wall( + {.position = {.x = static_cast(width - 1), .y = row}, .orientation = Side::RIGHT}, WallState::WALL + ); + } + + for (uint8_t column = 0; column < width; column++) { + this->write_wall({.position = {.x = column, .y = 0}, .orientation = Side::DOWN}, WallState::WALL); + this->write_wall( + {.position = {.x = column, .y = static_cast(height - 1)}, .orientation = Side::UP}, WallState::WALL + ); + } + + for (const Side side : all_sides) { + const GridPose wall{.position = this->start.position, .orientation = side}; + this->write_wall(wall, side == this->start.orientation ? WallState::NO_WALL : WallState::WALL); + } + + for (const GridPoint& cell : this->goal) { + for (const Side side : all_sides) { + if (this->is_goal(cell + side)) { + this->write_wall({.position = cell, .orientation = side}, WallState::NO_WALL); + } + } + } + + this->revision = 0; + this->flood(this->goal); +} + +template +WallState TMaze::get_wall(const GridPose& pose) const { + if (not contains(pose.position)) { + return WallState::WALL; + } + + return this->walls.at(pose.position.y).at(pose.position.x).at(std::to_underlying(pose.orientation)); +} + +template +bool TMaze::set_wall(const GridPose& pose, bool present) { + if (this->get_wall(pose) != WallState::UNKNOWN) { + return false; + } + + this->write_wall(pose, present ? WallState::WALL : WallState::NO_WALL); + this->revision++; + + return true; +} + +template +bool TMaze::is_blocked(const GridPose& pose) const { + return this->get_wall(pose) == WallState::WALL; +} + +template +bool TMaze::is_possibly_open(const GridPose& pose) const { + return this->get_wall(pose) != WallState::WALL; +} + +template +bool TMaze::is_known_open(const GridPose& pose) const { + return this->get_wall(pose) == WallState::NO_WALL; +} + +template +bool TMaze::is_goal(const GridPoint& position) const { + // NOLINTNEXTLINE(readability-use-anyofallof) the loop the robot's image was verified with + for (const GridPoint& cell : this->goal) { + if (cell == position) { + return true; + } + } + + return false; +} + +template +const GridPose& TMaze::get_start() const { + return this->start; +} + +template +std::span TMaze::get_goal() const { + return this->goal; +} + +template +uint32_t TMaze::get_revision() const { + return this->revision; +} + +template +void TMaze::flood(std::span targets) { + for (auto& row : this->costs) { + row.fill(unreachable); + } + + std::size_t head = 0; + std::size_t tail = 0; + + for (const GridPoint& target : targets) { + if (contains(target) and this->costs.at(target.y).at(target.x) != 0) { + this->costs.at(target.y).at(target.x) = 0; + this->queue.at(tail++) = target; + } + } + + while (head < tail) { + const GridPoint cell = this->queue.at(head++); + const uint16_t next_cost = this->costs.at(cell.y).at(cell.x) + 1; + + for (const Side side : all_sides) { + if (this->is_blocked({.position = cell, .orientation = side})) { + continue; + } + + const GridPoint neighbor = cell + side; + + if (this->costs.at(neighbor.y).at(neighbor.x) == unreachable) { + this->costs.at(neighbor.y).at(neighbor.x) = next_cost; + this->queue.at(tail++) = neighbor; + } + } + } +} + +template +uint16_t TMaze::get_cost(const GridPoint& position) const { + return contains(position) ? this->costs.at(position.y).at(position.x) : unreachable; +} + +template +std::optional TMaze::get_next(const GridPose& pose) const { + const std::array preference{ + pose.orientation, + pose.turned_left().orientation, + pose.turned_right().orientation, + pose.turned_back().orientation, + }; + + std::optional best{}; + uint16_t best_cost = unreachable; + + for (const Side side : preference) { + if (this->is_blocked({.position = pose.position, .orientation = side})) { + continue; + } + + const GridPoint neighbor = pose.position + side; + const uint16_t cost = this->get_cost(neighbor); + + if (cost < best_cost) { + best_cost = cost; + best = GridPose{.position = neighbor, .orientation = side}; + } + } + + return best; +} + +template +std::vector TMaze::serialize() const { + std::vector buffer(header_size + walls_size, 0); + + buffer.at(0) = format_version; + buffer.at(1) = width; + buffer.at(2) = height; + + for (uint8_t row = 0; row < height; row++) { + for (uint8_t column = 0; column < width; column++) { + const auto& cell = this->walls.at(row).at(column); + const uint16_t index = row * width + column; + + const auto packed = static_cast( + std::to_underlying(cell.at(std::to_underlying(Side::RIGHT))) | + (std::to_underlying(cell.at(std::to_underlying(Side::UP))) << 2U) + ); + + buffer.at(header_size + index / 2U) |= static_cast(packed << (4U * (index % 2U))); + } + } + + return buffer; +} + +template +void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { + const std::span data{buffer, size}; + + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) the size is checked first + + if (size != header_size + walls_size or data[0] != format_version or data[1] != width or data[2] != height) { + return; + } + + this->reset(); + + for (uint8_t row = 0; row < height; row++) { + for (uint8_t column = 0; column < width; column++) { + const uint16_t index = row * width + column; + const uint8_t packed = (data[header_size + index / 2U] >> (4U * (index % 2U))) & 0x0F; + + const std::array, 2> sides{ + {{Side::RIGHT, packed & 0x03U}, {Side::UP, packed >> 2U}} + }; + + for (const auto& [side, state] : sides) { + if (state == std::to_underlying(WallState::WALL) or state == std::to_underlying(WallState::NO_WALL)) { + this->set_wall( + {.position = {.x = column, .y = row}, .orientation = side}, + state == std::to_underlying(WallState::WALL) + ); + } + } + } + } + + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + + this->flood(this->goal); +} + +template +void TMaze::write_wall(const GridPose& pose, WallState state) { + this->walls.at(pose.position.y).at(pose.position.x).at(std::to_underlying(pose.orientation)) = state; + + const GridPose opposite = pose.front().turned_back(); + + if (contains(opposite.position)) { + this->walls.at(opposite.position.y).at(opposite.position.x).at(std::to_underlying(opposite.orientation)) = + state; + } +} +} // namespace micras::nav + +#endif // MICRAS_NAV_MAZE_TPP diff --git a/micras_nav/include/micras/nav/impl/mission.tpp b/micras_nav/include/micras/nav/impl/mission.tpp new file mode 100644 index 0000000..4d3e6dc --- /dev/null +++ b/micras_nav/include/micras/nav/impl/mission.tpp @@ -0,0 +1,637 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_MISSION_TPP +#define MICRAS_NAV_MISSION_TPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/core/types.hpp" +#include "micras/nav/executor.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/line.hpp" +#include "micras/nav/localizer.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/route_compiler.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/velocity_planner.hpp" + +namespace micras::nav { +template +TMission::TMission(const Dynamics& dynamics, const WallModel& wall_model, const Config& config) : + dynamics{dynamics}, + wall_model{wall_model}, + config{config}, + maze{config.maze}, + observer{config.observer}, + planner{dynamics, config.planner}, + explorer{planner, config.map_profiles}, + racing_line{dynamics, config.racing_line}, + executor{dynamics, racing_line.get_line(), config.executor}, + search_speed{std::min( + dynamics.get_linear_limits(config.search_profile).max_speed, + dynamics.get_turn_speed(config.search_profile, TurnId::SS90S) + )} { + this->solve_segments.reserve(config.executor.capacity); + this->candidate_segments.reserve(config.executor.capacity); + this->candidate_route.steps.reserve(config.executor.capacity); + this->solve_route.steps.reserve(config.executor.capacity); + this->careful_segments.reserve(config.executor.capacity); + this->careful_route.steps.reserve(config.executor.capacity); + this->observer.reset(); +} + +template +TMaze& TMission::get_maze() { + return this->maze; +} + +template +const TMaze& TMission::get_maze() const { + return this->maze; +} + +template +Pose TMission::get_start_pose() const { + const float cell_size = this->dynamics.get_model().maze.cell_size; + + return this->get_center_pose(this->maze.get_start()) + .compose({.position = {.x = this->config.start_offset - cell_size / 2.0F, .y = 0.0F}, .orientation = 0.0F}); +} + +template +void TMission::begin_plan(const RunProfile& profile) { + this->solve_profile = profile; + this->solve_segments.clear(); + this->route_time = std::numeric_limits::infinity(); + this->planner.begin(this->maze, WallAssumption::PESSIMISTIC, profile); + this->racing_line.reset(); + this->stage = PlanStage::SEARCH; + this->careful_search = false; +} + +template +bool TMission::update_plan(uint32_t max_edges) { + switch (this->stage) { + case PlanStage::IDLE: + return true; + + case PlanStage::SEARCH: + if (not this->planner.step(max_edges)) { + return false; + } + + if (not this->careful_search) { + this->route_time = this->choose_route(this->solve_profile, this->solve_route, this->solve_segments); + + if (not this->solve_profile.racing_line or this->solve_segments.empty()) { + this->stage = PlanStage::IDLE; + return true; + } + + if (this->solve_profile.risky) { + RunProfile careful = this->solve_profile; + careful.risky = false; + + this->careful_search = true; + this->planner.begin(this->maze, WallAssumption::PESSIMISTIC, careful); + return false; + } + + this->racing_line.begin(this->solve_route, this->solve_segments, this->maze, this->solve_profile); + } else { + RunProfile careful = this->solve_profile; + careful.risky = false; + this->choose_route(careful, this->careful_route, this->careful_segments); + + if (this->careful_segments.empty()) { + this->stage = PlanStage::IDLE; + return true; + } + + this->racing_line.begin(this->careful_route, this->careful_segments, this->maze, careful); + } + + this->stage = PlanStage::LINE; + return false; + + case PlanStage::LINE: + break; + } + + if (not this->racing_line.step()) { + return false; + } + + const Line& line = this->racing_line.get_line(); + + this->stage = PlanStage::IDLE; + + if (line.is_ready() and line.get_finish_time() < this->route_time) { + this->route_time = line.get_finish_time(); + this->solve_segments.clear(); + this->solve_segments.push_back(make_segment(SegmentKind::LINE, line.length(), line.get_start())); + } + + return true; +} + +template +float TMission::choose_route(const RunProfile& profile, Route& route, std::vector& segments) { + const MotionLimits limits = this->dynamics.get_linear_limits(profile); + + float best = std::numeric_limits::infinity(); + segments.clear(); + + for (uint8_t i = 0; i < this->planner.get_number_of_routes(); i++) { + this->planner.get_route(i, this->candidate_route); + + RouteCompiler::compile( + this->candidate_route, this->dynamics, profile, this->config.planner.start_distance, + this->candidate_segments + ); + + const float total_time = VelocityPlanner::plan(this->candidate_segments, this->dynamics, profile, 0.0F, 0.0F); + + const Segment& last = this->candidate_segments.back(); + const SpeedProfile braking{last.length, last.start_speed, 0.0F, limits}; + + float after_line = braking.duration(); + + if (this->candidate_route.finish_distance <= last.length) { + after_line -= braking.time_at(last.length - this->candidate_route.finish_distance); + } else { + after_line += (this->candidate_route.finish_distance - last.length) / last.start_speed; + } + + if (total_time - after_line < best) { + best = total_time - after_line; + std::swap(segments, this->candidate_segments); + std::swap(route, this->candidate_route); + } + } + + return best; +} + +template +bool TMission::has_route() const { + return this->stage == PlanStage::IDLE and not this->solve_segments.empty(); +} + +template +float TMission::get_route_time() const { + return this->route_time; +} + +template +void TMission::start(core::Objective objective) { + const float cell_size = this->dynamics.get_model().maze.cell_size; + + this->objective = objective; + this->status = Status::RUNNING; + this->finishing = false; + this->watching_front = false; + this->looks = 0; + + switch (objective) { + case core::Objective::EXPLORE: { + if (not this->map_cleared) { + this->maze.reset(); + this->observer.reset(); + this->map_cleared = true; + } + + this->executor.reset(this->get_start_pose(), this->config.search_profile); + this->cell = this->maze.get_start().front(); + this->at_center = false; + this->flood(); + + Move move{}; + move.add( + make_segment(SegmentKind::STRAIGHT, cell_size - this->config.start_offset, this->get_start_pose()) + ); + this->execute(move, 0.0F, this->search_speed); + break; + } + + case core::Objective::RETURN: + this->executor.reset(this->get_center_pose(this->cell), this->config.search_profile); + this->at_center = true; + this->explorer.reset(); + this->flood(); + break; + + case core::Objective::SOLVE: + this->executor.reset(this->get_start_pose(), this->solve_profile); + this->executor.push(this->solve_segments); + this->finishing = true; + break; + } + + this->reference = this->executor.get_reference(); +} + +template +TMission::Status TMission::update( + const Measurements& measurements, Localizer& localizer, float elapsed_time, float time_scale +) { + if (this->status != Status::RUNNING) { + return this->status; + } + + if (this->objective != core::Objective::SOLVE) { + bool changed = this->observer.update(measurements, localizer, this->wall_model, this->maze); + + if (this->objective == core::Objective::RETURN) { + changed = this->explorer.update(this->maze, this->config.edges_per_iteration) or changed; + } + + if (changed) { + this->flood(); + } + } + + const Segment* watched = this->executor.get_current(); + + const float commit_distance = this->get_commit_distance(); + + if (this->watching_front and watched != nullptr and watched->kind == SegmentKind::STRAIGHT and + watched->start.position.distance(this->get_entry_pose(this->watched_cell).position) < + std::max(commit_distance, 0.0F) + watch_tolerance) { + const float traveled = watched->start.position.distance(this->get_entry_pose(this->watched_cell).position) + + this->executor.get_reference().distance; + + const WallState wall = this->maze.get_wall(this->watched_cell); + + if (wall == WallState::NO_WALL) { + this->watching_front = false; + } else if (wall == WallState::WALL or traveled >= commit_distance) { + this->divert_to_center(traveled); + } + } + + if (this->executor.is_ending(elapsed_time)) { + if (this->finishing) { + if (this->executor.is_finished()) { + this->status = Status::FINISHED; + } + } else if (this->at_center) { + this->decide_at_center(); + } else { + this->decide_at_entry(); + } + } + + this->reference = this->executor.update(elapsed_time, time_scale, localizer.get_state()); + + const Segment* current = this->executor.get_current(); + + if (current == nullptr or current->kind == SegmentKind::STOP or current->kind == SegmentKind::ATTACH) { + localizer.correct_at_rest(measurements, elapsed_time); + + if (this->objective != core::Objective::SOLVE and this->at_center and current != nullptr and + localizer.get_cell().position != this->cell.position) { + this->status = Status::FAILED; + } + } + + return this->status; +} + +template +const Reference& TMission::get_reference() const { + return this->reference; +} + +template +const Executor& TMission::get_executor() const { + return this->executor; +} + +template +const GridPose& TMission::get_cell() const { + return this->cell; +} + +template +Pose TMission::get_entry_pose(const GridPose& cell) const { + const float cell_size = this->dynamics.get_model().maze.cell_size; + + return this->get_center_pose(cell).compose({.position = {.x = -cell_size / 2.0F, .y = 0.0F}, .orientation = 0.0F}); +} + +template +Pose TMission::get_center_pose(const GridPose& cell) const { + return { + .position = cell.position.to_vector(this->dynamics.get_model().maze.cell_size), + .orientation = static_cast(std::to_underlying(cell.orientation)) * std::numbers::pi_v / 2.0F, + }; +} + +template +void TMission::flood() { + if (this->objective != core::Objective::RETURN) { + this->maze.flood(this->maze.get_goal()); + return; + } + + std::array::max_targets> targets{}; + std::size_t number_of_targets = 0; + + if (this->explorer.has_targets()) { + for (const GridPoint& target : this->explorer.get_targets()) { + const bool worth_visiting = std::ranges::any_of(all_sides, [this, &target](Side side) { + return this->maze.get_wall({.position = target, .orientation = side}) == WallState::UNKNOWN; + }); + + if (worth_visiting) { + targets.at(number_of_targets++) = target; + } + } + } + + if (number_of_targets == 0) { + targets.at(number_of_targets++) = this->maze.get_start().position; + } + + this->maze.flood(std::span{targets}.first(number_of_targets)); +} + +template +void TMission::execute(Move& move, float start_speed, float end_speed) { + const std::span segments = std::span{move.segments}.first(move.size); + + VelocityPlanner::plan(segments, this->dynamics, this->config.search_profile, start_speed, end_speed); + this->executor.push(segments); +} + +template +void TMission::decide_at_entry() { + const float cell_size = this->dynamics.get_model().maze.cell_size; + + this->watching_front = false; + this->looks = 0; + + if (this->finish_at_entry()) { + return; + } + + const std::optional next = this->maze.get_next(this->cell); + + if (not next.has_value()) { + this->status = Status::FAILED; + return; + } + + const Pose entry = this->get_entry_pose(this->cell); + Move move{}; + + if (next->orientation == this->cell.orientation) { + const float commit_distance = this->get_commit_distance(); + + this->watching_front = this->maze.get_wall(this->cell) == WallState::UNKNOWN; + + if (this->watching_front and commit_distance > 0.0F) { + Segment watched = make_segment(SegmentKind::STRAIGHT, commit_distance, entry); + watched.max_speed = this->search_speed; + + move.add(watched); + move.add(make_segment( + SegmentKind::STRAIGHT, cell_size - commit_distance, + entry.compose({.position = {.x = commit_distance, .y = 0.0F}, .orientation = 0.0F}) + )); + } else { + Segment straight = make_segment(SegmentKind::STRAIGHT, cell_size, entry); + + if (this->watching_front) { + straight.max_speed = this->search_speed; + } + + move.add(straight); + } + + this->execute(move, this->search_speed, this->search_speed); + + this->watched_cell = this->cell; + this->cell = *next; + return; + } + + if (next->orientation == this->cell.turned_back().orientation) { + const GridPose reversed = this->cell.turned_back(); + + this->add_stop_at_center(move); + move.add(make_segment(SegmentKind::SPIN, std::numbers::pi_v, this->get_center_pose(this->cell))); + move.add(make_segment(SegmentKind::STRAIGHT, cell_size / 2.0F, this->get_center_pose(reversed))); + this->execute(move, this->search_speed, this->search_speed); + + this->cell = *next; + return; + } + + if (this->maze.get_wall({.position = this->cell.position, .orientation = next->orientation}) != + WallState::NO_WALL) { + this->add_stop_at_center(move); + this->execute(move, this->search_speed, 0.0F); + this->at_center = true; + return; + } + + const TurnShape& shape = this->dynamics.get_turn(this->config.search_profile, TurnId::SS90S); + const bool to_left = next->orientation == this->cell.turned_left().orientation; + + Segment turn = make_segment( + SegmentKind::TURN, to_left ? shape.length() : -shape.length(), + entry.compose({.position = {.x = shape.pre, .y = 0.0F}, .orientation = 0.0F}) + ); + turn.turn = TurnId::SS90S; + + if (shape.pre > RouteCompiler::min_straight) { + move.add(make_segment(SegmentKind::STRAIGHT, shape.pre, entry)); + } + + move.add(turn); + + if (shape.post > RouteCompiler::min_straight) { + move.add(make_segment( + SegmentKind::STRAIGHT, shape.post, + this->get_entry_pose(*next).compose({.position = {.x = -shape.post, .y = 0.0F}, .orientation = 0.0F}) + )); + } + + this->execute(move, this->search_speed, this->search_speed); + this->cell = *next; +} + +template +void TMission::decide_at_center() { + const float cell_size = this->dynamics.get_model().maze.cell_size; + const Pose center = this->get_center_pose(this->cell); + + Move move{}; + + if (this->objective == core::Objective::RETURN and not this->explorer.has_targets()) { + move.add(make_segment(SegmentKind::STOP, this->config.look_time, center)); + this->execute(move, 0.0F, 0.0F); + return; + } + + const std::optional next = this->maze.get_next(this->cell); + + if (not next.has_value()) { + this->status = Status::FAILED; + return; + } + + if (next->orientation != this->cell.orientation) { + float angle = std::numbers::pi_v; + + if (next->orientation == this->cell.turned_left().orientation) { + angle = std::numbers::pi_v / 2.0F; + } else if (next->orientation == this->cell.turned_right().orientation) { + angle = -std::numbers::pi_v / 2.0F; + } + + move.add(make_segment(SegmentKind::SPIN, angle, center)); + this->cell.orientation = next->orientation; + move.add(make_segment(SegmentKind::STOP, this->config.stop_time, this->get_center_pose(this->cell))); + this->execute(move, 0.0F, 0.0F); + return; + } + + if (this->maze.get_wall(this->cell) == WallState::NO_WALL) { + move.add(make_segment(SegmentKind::STRAIGHT, cell_size / 2.0F, center)); + this->execute(move, 0.0F, this->search_speed); + this->at_center = false; + this->looks = 0; + this->cell = *next; + return; + } + + this->looks++; + + if (this->looks > this->config.max_looks) { + this->status = Status::FAILED; + return; + } + + move.add(make_segment(SegmentKind::STOP, this->config.look_time, center)); + this->execute(move, 0.0F, 0.0F); +} + +template +void TMission::add_stop_at_center(Move& move) const { + const float cell_size = this->dynamics.get_model().maze.cell_size; + const Pose center = this->get_center_pose(this->cell); + + move.add(make_segment(SegmentKind::STRAIGHT, cell_size / 2.0F, this->get_entry_pose(this->cell))); + + if (this->maze.get_wall(this->cell) == WallState::WALL) { + move.add(make_segment(SegmentKind::ATTACH, this->config.attach_time, center)); + } else { + move.add(make_segment(SegmentKind::STOP, this->config.stop_time, center)); + } +} + +template +bool TMission::finish_at_entry() { + const float cell_size = this->dynamics.get_model().maze.cell_size; + + Move move{}; + + if (this->objective == core::Objective::EXPLORE and this->maze.is_goal(this->cell.position)) { + this->add_stop_at_center(move); + } else if ( + this->objective == core::Objective::RETURN and this->explorer.is_complete() and + this->cell.position == this->maze.get_start().position + ) { + const GridPose parked{.position = this->cell.position, .orientation = this->maze.get_start().orientation}; + + this->add_stop_at_center(move); + move.add(make_segment(SegmentKind::SPIN, std::numbers::pi_v, this->get_center_pose(this->cell))); + move.add(make_segment(SegmentKind::STOP, this->config.stop_time, this->get_center_pose(parked))); + move.add(make_segment( + SegmentKind::STRAIGHT, this->config.start_offset - cell_size / 2.0F, this->get_center_pose(parked) + )); + } else { + return false; + } + + this->execute(move, this->search_speed, 0.0F); + this->at_center = true; + this->finishing = true; + + return true; +} + +template +void TMission::divert_to_center(float traveled) { + const float cell_size = this->dynamics.get_model().maze.cell_size; + const Reference& current = this->executor.get_reference(); + const float remaining = cell_size / 2.0F - traveled; + + this->watching_front = false; + + if (remaining <= 0.0F) { + return; + } + + const Pose center = this->get_center_pose(this->watched_cell); + + Move move{}; + move.add(make_segment( + SegmentKind::STRAIGHT, remaining, + this->get_entry_pose(this->watched_cell).compose({.position = {.x = traveled, .y = 0.0F}, .orientation = 0.0F}) + )); + + if (this->maze.get_wall(this->watched_cell) == WallState::WALL) { + move.add(make_segment(SegmentKind::ATTACH, this->config.attach_time, center)); + } else { + move.add(make_segment(SegmentKind::STOP, this->config.stop_time, center)); + } + + const std::span segments = std::span{move.segments}.first(move.size); + + VelocityPlanner::plan(segments, this->dynamics, this->config.search_profile, current.twist.linear, 0.0F); + this->executor.divert(segments); + + this->cell = this->watched_cell; + this->at_center = true; +} + +template +float TMission::get_commit_distance() const { + const float cell_size = this->dynamics.get_model().maze.cell_size; + const float braking = SpeedProfile::get_braking_distance( + this->search_speed, 0.0F, this->dynamics.get_linear_limits(this->config.search_profile) + ); + + return cell_size / 2.0F - braking - this->config.commit_margin; +} + +template +Segment TMission::make_segment(SegmentKind kind, float length, const Pose& start) { + return { + .kind = kind, + .turn = TurnId::SS90S, + .length = length, + .start_speed = 0.0F, + .end_speed = 0.0F, + .max_speed = std::numeric_limits::infinity(), + .start = start, + }; +} +} // namespace micras::nav + +#endif // MICRAS_NAV_MISSION_TPP diff --git a/micras_nav/include/micras/nav/impl/planner.tpp b/micras_nav/include/micras/nav/impl/planner.tpp new file mode 100644 index 0000000..e432a4f --- /dev/null +++ b/micras_nav/include/micras/nav/impl/planner.tpp @@ -0,0 +1,884 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_PLANNER_TPP +#define MICRAS_NAV_PLANNER_TPP + +#include +#include +#include +#include +#include +#include +#include + +#include "micras/nav/lattice.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/speed_profile.hpp" + +namespace micras::nav { +template +TPlanner::TPlanner(const Dynamics& dynamics, const Config& config) : + dynamics{dynamics}, start_distance{config.start_distance} { } + +template +void TPlanner::begin( + const TMaze& maze, WallAssumption assumption, const RunProfile& profile +) { + this->maze = &maze; + this->assumption = assumption; + this->run_profile = profile; + this->limits = this->dynamics.get_linear_limits(profile); + + for (uint8_t i = 0; i < number_of_turns; i++) { + this->turn_speeds.at(i) = this->dynamics.get_turn_speed(profile, static_cast(i)); + } + + this->number_of_usable_turns.fill(0); + + for (uint8_t i = 0; i < number_of_turns; i++) { + const auto turn = static_cast(i); + const uint8_t diagonal = get_primitive(turn).diagonal_entry ? 1 : 0; + + if (this->is_usable(turn)) { + this->usable_turns.at(diagonal).at(this->number_of_usable_turns.at(diagonal)) = turn; + this->number_of_usable_turns.at(diagonal)++; + } + } + + for (auto& costs : this->edge_costs) { + costs.fill(-1.0F); + } + + for (auto& row : this->gaps) { + row.fill(-std::numeric_limits::infinity()); + } + + this->preparing = true; + this->prepare_first = 0; + this->prepare_second = 0; + this->prepare_run = 0; + this->expanding = none; + + this->goal_length = 1; + + for (const GridPoint& cell : maze.get_goal()) { + for (const Side side : {Side::RIGHT, Side::UP, Side::LEFT, Side::DOWN}) { + GridPose pose{.position = cell, .orientation = side}; + uint8_t length = 1; + + while (TMaze::contains(pose.front().position) and maze.is_goal(pose.front().position)) { + pose = pose.front(); + length++; + } + + this->goal_length = std::max(this->goal_length, length); + } + } + + this->heads.fill(none); + this->queue_size = 0; + this->allocated = 0; + this->free_list = none; + this->held = 0; + this->peak = 0; + this->exact = true; + this->number_of_terminals = 0; + + const GridPose& start_pose = maze.get_start(); + const auto heading = static_cast(2 * std::to_underlying(start_pose.orientation)); + const LatticePose center{ + .point = + {.x = static_cast(2 * start_pose.position.x + 1), + .y = static_cast(2 * start_pose.position.y + 1)}, + .heading = heading, + }; + const LatticePoint step = center.step(); + + this->start = { + .point = + {.x = static_cast(center.point.x + step.x / 2), + .y = static_cast(center.point.y + step.y / 2)}, + .heading = heading, + }; + + if (is_inside(this->start) and this->is_traversable(this->start)) { + this->insert({ + .cost = 0.0F, + .node = encode(this->start), + .parent = none, + .next = none, + .position = none, + .arrival = rest, + .speed_ratio = full_speed, + .run = 0, + .side = TurnSide::LEFT, + }); + } +} + +template +bool TPlanner::step(uint32_t max_edges) { + while (max_edges > 0) { + if (this->preparing) { + max_edges -= std::min(this->prepare(), max_edges); + continue; + } + + if (this->expanding == none) { + if (this->queue_size == 0) { + break; + } + + const uint16_t index = this->pop(); + + if (this->number_of_terminals == number_of_candidates and + this->labels.at(index).cost >= this->terminals.at(number_of_candidates - 1).cost) { + this->queue_size = 0; + break; + } + + this->expanding = index; + this->expansion_entry = decode(this->labels.at(index).node); + this->expansion_run = 0; + this->expansion_turn = 0; + } + + this->advance_expansion(); + max_edges--; + } + + return this->is_finished(); +} + +template +bool TPlanner::is_finished() const { + return not this->preparing and this->expanding == none and this->queue_size == 0; +} + +template +bool TPlanner::is_exact() const { + return this->exact; +} + +template +uint16_t TPlanner::get_peak_labels() const { + return this->peak; +} + +template +uint8_t TPlanner::get_number_of_routes() const { + return this->number_of_terminals; +} + +template +void TPlanner::get_route(uint8_t index, Route& route) const { + const Terminal& terminal = this->terminals.at(index); + + route.start = this->start; + route.steps.clear(); + route.stop_distance = terminal.stop_distance; + route.finish_distance = terminal.finish_distance; + route.time = terminal.cost; + + route.steps.push_back( + {.run = terminal.run, .has_turn = terminal.has_turn, .turn = terminal.turn, .side = terminal.side} + ); + + for (uint16_t i = terminal.label; this->labels.at(i).arrival != rest; i = this->labels.at(i).parent) { + const Label& label = this->labels.at(i); + route.steps.push_back({.run = label.run, .has_turn = true, .turn = to_turn(label.arrival), .side = label.side}); + } + + std::ranges::reverse(route.steps); +} + +template +template +// NOLINTNEXTLINE(cppcoreguidelines-missing-std-forward) called for every wall, so never forwarded +void TPlanner::for_each_wall(const Route& route, F&& function) { + LatticePose node = route.start; + function(node.wall()); + + for (const RouteStep& step : route.steps) { + for (uint8_t i = 0; i < step.run; i++) { + node = node.advanced(); + function(node.wall()); + } + + if (not step.has_turn) { + continue; + } + + for (uint8_t i = 0; i < get_primitive(step.turn).number_of_gates; i++) { + function(LatticePose{.point = get_turn_gate(node, step.turn, step.side, i), .heading = 0}.wall()); + } + + node = get_turn_exit(node, step.turn, step.side); + function(node.wall()); + } +} + +template +constexpr bool TPlanner::ends_diagonal(uint8_t arrival) { + if (arrival == rest) { + return false; + } + + const TurnPrimitive& primitive = get_primitive(to_turn(arrival)); + return (primitive.rotation % 2 == 1) != primitive.diagonal_entry; +} + +template +constexpr TurnId TPlanner::to_turn(uint8_t arrival) { + return static_cast(arrival - 1); +} + +template +constexpr bool TPlanner::is_inside(const LatticePose& node) { + return node.point.x >= 0 and node.point.x <= 2 * width and node.point.y >= 0 and node.point.y <= 2 * height and + (node.point.on_vertical_wall() or node.point.on_horizontal_wall()); +} + +template +constexpr uint16_t TPlanner::encode(const LatticePose& node) { + const LatticePoint& point = node.point; + + const int32_t wall = point.on_vertical_wall() ? + (point.x / 2) * height + (point.y - 1) / 2 : + (width + 1) * height + ((point.x - 1) / 2) * (height + 1) + point.y / 2; + + const int32_t sub = node.heading % 2 == 0 ? node.heading / 4 : 2 + node.heading / 2; + + return static_cast(wall * headings_per_wall + sub); +} + +template +constexpr LatticePose TPlanner::decode(uint16_t index) { + const uint16_t wall = index / headings_per_wall; + const uint16_t sub = index % headings_per_wall; + + LatticePose node{}; + + if (wall < (width + 1) * height) { + node.point = {.x = static_cast(2 * (wall / height)), .y = static_cast(2 * (wall % height) + 1)}; + } else { + const uint16_t horizontal = wall - (width + 1) * height; + + node.point = { + .x = static_cast(2 * (horizontal / (height + 1)) + 1), + .y = static_cast(2 * (horizontal % (height + 1))), + }; + } + + node.heading = sub < 2 ? static_cast((node.point.on_vertical_wall() ? 0 : 2) + 4 * sub) : + static_cast(2 * (sub - 2) + 1); + + return node; +} + +template +bool TPlanner::is_usable(TurnId turn) const { + return this->dynamics.get_turn(this->run_profile, turn).valid; +} + +template +bool TPlanner::is_traversable(const LatticePose& node) const { + const GridPose wall = node.wall(); + + return this->assumption == WallAssumption::OPTIMISTIC ? this->maze->is_possibly_open(wall) : + this->maze->is_known_open(wall); +} + +template +bool TPlanner::is_goal(const LatticePoint& cell) const { + return cell.x >= 0 and cell.y >= 0 and + this->maze->is_goal({.x = static_cast(cell.x), .y = static_cast(cell.y)}); +} + +template +float TPlanner::get_stop_distance(const LatticePose& node) const { + const float cell_size = this->dynamics.get_model().maze.cell_size; + const LatticePoint step = node.step(); + + LatticePoint cell = node.cell_ahead(); + LatticePose wall = node; + uint8_t cells = 0; + + while (this->is_goal(cell)) { + cells++; + wall = wall.advanced(); + + if (not is_inside(wall) or not this->maze->is_known_open(wall.wall())) { + break; + } + + cell = {.x = static_cast(cell.x + step.x / 2), .y = static_cast(cell.y + step.y / 2)}; + } + + return (static_cast(cells) - 0.5F) * cell_size; +} + +template +float TPlanner::get_turn_time(TurnId turn, float length, float speed) const { + const float nominal = this->turn_speeds.at(std::to_underlying(turn)); + + return length / (speed < nominal ? speed + turn_blend * (nominal - speed) : speed); +} + +template +float TPlanner::get_speed(uint8_t arrival, uint8_t speed_ratio) const { + if (arrival == rest) { + return 0.0F; + } + + return this->turn_speeds.at(arrival - 1) * static_cast(speed_ratio) / static_cast(full_speed); +} + +template +float TPlanner::get_offset(uint8_t arrival) const { + return arrival == rest ? this->start_distance : this->dynamics.get_turn(this->run_profile, to_turn(arrival)).post; +} + +template +TPlanner::Edge + TPlanner::get_edge(uint8_t arrival, uint8_t speed_ratio, uint8_t run, TurnId turn) { + const bool at_full_speed = arrival == rest or speed_ratio == full_speed; + const auto pair = static_cast(edge_pairs.index.at(arrival).at(std::to_underlying(turn))); + float& cached_cost = this->edge_costs.at(pair).at(run); + uint8_t& cached_speed_ratio = this->edge_speed_ratios.at(pair).at(run); + + if (at_full_speed and cached_cost >= 0.0F) { + return {.cost = cached_cost, .speed_ratio = cached_speed_ratio}; + } + + this->computed_edges++; + + const float cell_size = this->dynamics.get_model().maze.cell_size; + const float step = ends_diagonal(arrival) ? cell_size / std::numbers::sqrt2_v : cell_size; + + const TurnShape& shape = this->dynamics.get_turn(this->run_profile, turn); + const float distance = this->get_offset(arrival) + static_cast(run) * step + shape.pre; + const float nominal_speed = this->turn_speeds.at(std::to_underlying(turn)); + + const float start_speed = std::min( + this->get_speed(arrival, speed_ratio), SpeedProfile::get_brakeable_speed(distance, nominal_speed, this->limits) + ); + + const float end_speed = + std::min(nominal_speed, SpeedProfile::get_reachable_speed(distance, start_speed, this->limits)); + + const Edge edge{ + .cost = SpeedProfile{distance, start_speed, end_speed, this->limits}.duration() + + this->get_turn_time(turn, shape.length(), end_speed), + .speed_ratio = static_cast(std::lround(static_cast(full_speed) * end_speed / nominal_speed)), + }; + + if (at_full_speed) { + cached_cost = edge.cost; + cached_speed_ratio = edge.speed_ratio; + } + + return edge; +} + +template +bool TPlanner::is_comparable(uint8_t first, uint8_t second) const { + const auto is_reachable = [this](uint8_t arrival) { return arrival == rest or this->is_usable(to_turn(arrival)); }; + + return first != second and ends_diagonal(first) == ends_diagonal(second) and is_reachable(first) and + is_reachable(second); +} + +template +uint32_t TPlanner::prepare() { + while (this->prepare_first < number_of_arrivals and + not this->is_comparable(this->prepare_first, this->prepare_second)) { + this->prepare_second++; + + if (this->prepare_second == number_of_arrivals) { + this->prepare_second = 0; + this->prepare_first++; + } + } + + if (this->prepare_first == number_of_arrivals) { + this->preparing = false; + return 0; + } + + const bool diagonal = ends_diagonal(this->prepare_first); + const uint32_t computed_before = this->computed_edges; + + this->compare_arrivals(this->prepare_first, this->prepare_second, this->prepare_run); + this->prepare_run++; + + const uint32_t cost = this->computed_edges - computed_before + (diagonal ? 0U : this->goal_length); + + const uint8_t longest = diagonal ? max_run : max_straight_run; + + if (this->prepare_run > longest or std::isinf(this->gaps.at(this->prepare_first).at(this->prepare_second))) { + this->prepare_run = 0; + this->prepare_second++; + + if (this->prepare_second == number_of_arrivals) { + this->prepare_second = 0; + this->prepare_first++; + } + } + + return std::max(cost, 1); +} + +template +void TPlanner::compare_arrivals(uint8_t first, uint8_t second, uint8_t run) { + float& gap = this->gaps.at(first).at(second); + + const bool diagonal = ends_diagonal(first); + const uint8_t kind = diagonal ? 1 : 0; + + for (uint8_t i = 0; i < this->number_of_usable_turns.at(kind); i++) { + const TurnId turn = this->usable_turns.at(kind).at(i); + const TurnPrimitive& primitive = get_primitive(turn); + const auto arrival = static_cast(std::to_underlying(turn) + 1); + + const Edge kept = this->get_edge(first, full_speed, run, turn); + const Edge dropped = this->get_edge(second, full_speed, run, turn); + + if (kept.speed_ratio < dropped.speed_ratio) { + gap = std::numeric_limits::infinity(); + return; + } + + const TurnShape& shape = this->dynamics.get_turn(this->run_profile, turn); + const float kept_speed = this->get_speed(arrival, kept.speed_ratio); + const float dropped_speed = this->get_speed(arrival, dropped.speed_ratio); + + gap = std::max( + gap, (kept.cost - this->get_turn_time(turn, shape.length(), kept_speed)) - + (dropped.cost - this->get_turn_time(turn, shape.length(), dropped_speed)) + ); + + for (uint8_t j = 0; j < primitive.number_of_gates; j++) { + const float remaining = shape.pre + shape.length() - std::max(shape.gates.at(j), shape.pre); + gap = std::max( + gap, (kept.cost - this->get_turn_time(turn, remaining, kept_speed)) - + (dropped.cost - this->get_turn_time(turn, remaining, dropped_speed)) + ); + } + } + + if (diagonal) { + return; + } + + const float cell_size = this->dynamics.get_model().maze.cell_size; + + for (uint8_t cells = 1; cells <= this->goal_length; cells++) { + const float stop_distance = (static_cast(cells) - 0.5F) * cell_size; + const float kept_line = this->get_offset(first) + static_cast(run) * cell_size; + const float dropped_line = this->get_offset(second) + static_cast(run) * cell_size; + + const SpeedProfile kept{kept_line + stop_distance, this->get_speed(first, full_speed), 0.0F, this->limits}; + const SpeedProfile dropped{ + dropped_line + stop_distance, this->get_speed(second, full_speed), 0.0F, this->limits + }; + + gap = std::max(gap, kept.time_at(kept_line) - dropped.time_at(dropped_line)); + } +} + +template +bool TPlanner::dominates(const Label& first, const Label& second) { + if (first.arrival == second.arrival) { + if (first.speed_ratio < second.speed_ratio) { + return false; + } + + if (first.arrival == rest) { + return first.cost <= second.cost; + } + + const TurnId turn = to_turn(first.arrival); + const float length = this->dynamics.get_turn(this->run_profile, turn).length(); + + return first.cost - this->get_turn_time(turn, length, this->get_speed(first.arrival, first.speed_ratio)) <= + second.cost - this->get_turn_time(turn, length, this->get_speed(second.arrival, second.speed_ratio)); + } + + if (first.speed_ratio != full_speed) { + return false; + } + + float second_cost = second.cost; + + if (second.speed_ratio != full_speed) { + const TurnId turn = to_turn(second.arrival); + const float length = this->dynamics.get_turn(this->run_profile, turn).length(); + + second_cost -= this->get_turn_time(turn, length, this->get_speed(second.arrival, second.speed_ratio)) - + this->get_turn_time(turn, length, this->get_speed(second.arrival, full_speed)); + } + + return first.cost + this->gaps.at(first.arrival).at(second.arrival) <= second_cost; +} + +template +void TPlanner::advance_expansion() { + const Label label = this->labels.at(this->expanding); + const bool diagonal = this->expansion_entry.is_diagonal(); + const uint8_t kind = diagonal ? 1 : 0; + + if (this->expansion_turn == 0) { + if (this->expansion_run > 0) { + this->expansion_entry = this->expansion_entry.advanced(); + + if (not is_inside(this->expansion_entry) or not this->is_traversable(this->expansion_entry)) { + this->expanding = none; + return; + } + } + + if (this->is_goal(this->expansion_entry.cell_ahead())) { + if (not diagonal) { + const float cell_size = this->dynamics.get_model().maze.cell_size; + const float line = + this->get_offset(label.arrival) + static_cast(this->expansion_run) * cell_size; + const float stop_distance = this->get_stop_distance(this->expansion_entry); + + const SpeedProfile approach{ + line + stop_distance, this->get_speed(label.arrival, label.speed_ratio), 0.0F, this->limits + }; + + this->add_terminal({ + .cost = label.cost + approach.time_at(line), + .label = this->expanding, + .run = this->expansion_run, + .has_turn = false, + .turn = TurnId::SS90S, + .side = TurnSide::LEFT, + .stop_distance = stop_distance, + .finish_distance = stop_distance, + }); + } + + this->expanding = none; + return; + } + } + + if (this->expansion_turn < this->number_of_usable_turns.at(kind)) { + this->relax_turn( + this->expanding, this->expansion_entry, this->expansion_run, + this->usable_turns.at(kind).at(this->expansion_turn) + ); + this->expansion_turn++; + } + + if (this->expansion_turn >= this->number_of_usable_turns.at(kind)) { + this->expansion_turn = 0; + this->expansion_run++; + + if (this->expansion_run > max_run) { + this->expanding = none; + } + } +} + +template +// NOLINTNEXTLINE(readability-function-cognitive-complexity) one edge of the search, both sides at once +void TPlanner::relax_turn(uint16_t index, const LatticePose& entry, uint8_t run, TurnId turn) { + const TurnPrimitive& primitive = get_primitive(turn); + + Edge edge{.cost = -1.0F, .speed_ratio = 0}; + + for (const TurnSide side : {TurnSide::LEFT, TurnSide::RIGHT}) { + if (primitive.diagonal_entry and side != entry.diagonal_turn_side()) { + continue; + } + + const LatticePose exit = get_turn_exit(entry, turn, side); + + if (not is_inside(exit) or not exit.is_valid() or not this->is_traversable(exit)) { + continue; + } + + LatticePoint cell = entry.cell_ahead(); + int8_t finish_gate = -1; + bool feasible = true; + + for (uint8_t i = 0; i < primitive.number_of_gates and feasible; i++) { + const LatticePose gate{.point = get_turn_gate(entry, turn, side, i), .heading = 0}; + + if (not is_inside(gate) or not this->is_traversable(gate)) { + feasible = false; + break; + } + + const bool vertical = gate.point.on_vertical_wall(); + const LatticePoint low{ + .x = static_cast(vertical ? gate.point.x / 2 - 1 : (gate.point.x - 1) / 2), + .y = static_cast(vertical ? (gate.point.y - 1) / 2 : gate.point.y / 2 - 1), + }; + const LatticePoint high{ + .x = static_cast(vertical ? low.x + 1 : low.x), + .y = static_cast(vertical ? low.y : low.y + 1) + }; + + cell = cell == low ? high : low; + + if (this->is_goal(cell)) { + finish_gate = finish_gate < 0 ? static_cast(i) : finish_gate; + } else if (finish_gate >= 0) { + feasible = false; + } + } + + if (not feasible) { + continue; + } + + const Label& label = this->labels.at(index); + + if (edge.cost < 0.0F) { + edge = this->get_edge(label.arrival, label.speed_ratio, run, turn); + } + + const float cost = label.cost + edge.cost; + + if (finish_gate >= 0) { + if (exit.is_diagonal() or not this->is_goal(exit.cell_ahead())) { + continue; + } + + const TurnShape& shape = this->dynamics.get_turn(this->run_profile, turn); + const float remaining = + shape.pre + shape.length() - std::max(shape.gates.at(static_cast(finish_gate)), shape.pre); + const float speed = this->get_speed(std::to_underlying(turn) + 1, edge.speed_ratio); + + const float stop_distance = this->get_stop_distance(exit); + + this->add_terminal({ + .cost = cost - this->get_turn_time(turn, remaining, speed), + .label = index, + .run = run, + .has_turn = true, + .turn = turn, + .side = side, + .stop_distance = stop_distance, + .finish_distance = remaining + shape.post + stop_distance, + }); + + continue; + } + + this->insert({ + .cost = cost, + .node = encode(exit), + .parent = index, + .next = none, + .position = none, + .arrival = static_cast(std::to_underlying(turn) + 1), + .speed_ratio = edge.speed_ratio, + .run = run, + .side = side, + }); + } +} + +template +void TPlanner::add_terminal(const Terminal& terminal) { + uint8_t position = this->number_of_terminals; + + while (position > 0 and this->terminals.at(position - 1).cost > terminal.cost) { + if (position < number_of_candidates) { + this->terminals.at(position) = this->terminals.at(position - 1); + } + + position--; + } + + if (position < number_of_candidates) { + this->terminals.at(position) = terminal; + this->number_of_terminals = std::min(this->number_of_terminals + 1, number_of_candidates); + } +} + +template +void TPlanner::insert(Label label) { + uint16_t previous = none; + uint16_t current = this->heads.at(label.node); + + while (current != none) { + const Label& other = this->labels.at(current); + const uint16_t next = other.next; + + if (this->dominates(other, label)) { + return; + } + + if (other.position != none and this->dominates(label, other)) { + if (previous == none) { + this->heads.at(label.node) = next; + } else { + this->labels.at(previous).next = next; + } + + this->remove(other.position); + this->labels.at(current).next = this->free_list; + this->free_list = current; + this->held--; + } else { + previous = current; + } + + current = next; + } + + const uint16_t index = this->allocate(label.cost); + + if (index == none) { + return; + } + + label.next = this->heads.at(label.node); + this->labels.at(index) = label; + this->heads.at(label.node) = index; + this->push(index); +} + +template +uint16_t TPlanner::allocate(float cost) { + if (this->free_list == none and this->allocated == max_labels) { + this->exact = false; + + uint16_t costliest = none; + + for (uint16_t position = this->queue_size / 2; position < this->queue_size; position++) { + const uint16_t index = this->queue.at(position); + + if (costliest == none or this->labels.at(index).cost > this->labels.at(costliest).cost) { + costliest = index; + } + } + + if (costliest == none or this->labels.at(costliest).cost <= cost) { + return none; + } + + this->release(costliest); + } + + uint16_t index = this->free_list; + + if (index != none) { + this->free_list = this->labels.at(index).next; + } else { + index = this->allocated; + this->allocated++; + } + + this->held++; + this->peak = std::max(this->peak, this->held); + + return index; +} + +template +void TPlanner::release(uint16_t index) { + Label& label = this->labels.at(index); + uint16_t* link = &this->heads.at(label.node); + + while (*link != index) { + link = &this->labels.at(*link).next; + } + + *link = label.next; + this->remove(label.position); + label.next = this->free_list; + this->free_list = index; + this->held--; +} + +template +void TPlanner::push(uint16_t index) { + this->queue.at(this->queue_size) = index; + this->labels.at(index).position = this->queue_size; + this->queue_size++; + this->sift_up(this->queue_size - 1); +} + +template +uint16_t TPlanner::pop() { + const uint16_t top = this->queue.at(0); + this->remove(0); + return top; +} + +template +void TPlanner::remove(uint16_t position) { + this->labels.at(this->queue.at(position)).position = none; + this->queue_size--; + + if (position == this->queue_size) { + return; + } + + const uint16_t last = this->queue.at(this->queue_size); + this->queue.at(position) = last; + this->labels.at(last).position = position; + this->sift_down(position); + this->sift_up(this->labels.at(last).position); +} + +template +void TPlanner::sift_up(uint16_t position) { + const uint16_t index = this->queue.at(position); + const float cost = this->labels.at(index).cost; + + while (position > 0) { + const uint16_t parent = (position - 1) / 2; + + if (this->labels.at(this->queue.at(parent)).cost <= cost) { + break; + } + + this->queue.at(position) = this->queue.at(parent); + this->labels.at(this->queue.at(position)).position = position; + position = parent; + } + + this->queue.at(position) = index; + this->labels.at(index).position = position; +} + +template +void TPlanner::sift_down(uint16_t position) { + const uint16_t index = this->queue.at(position); + const float cost = this->labels.at(index).cost; + + while (2 * position + 1 < this->queue_size) { + uint16_t child = 2 * position + 1; + + if (child + 1 < this->queue_size and + this->labels.at(this->queue.at(child + 1)).cost < this->labels.at(this->queue.at(child)).cost) { + child++; + } + + if (cost <= this->labels.at(this->queue.at(child)).cost) { + break; + } + + this->queue.at(position) = this->queue.at(child); + this->labels.at(this->queue.at(position)).position = position; + position = child; + } + + this->queue.at(position) = index; + this->labels.at(index).position = position; +} +} // namespace micras::nav + +#endif // MICRAS_NAV_PLANNER_TPP diff --git a/micras_nav/include/micras/nav/impl/racing_line.tpp b/micras_nav/include/micras/nav/impl/racing_line.tpp new file mode 100644 index 0000000..d685a53 --- /dev/null +++ b/micras_nav/include/micras/nav/impl/racing_line.tpp @@ -0,0 +1,903 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_RACING_LINE_TPP +#define MICRAS_NAV_RACING_LINE_TPP + +#include +#include +#include +#include +#include +#include + +#include "micras/core/math.hpp" +#include "micras/core/vector.hpp" +#include "micras/nav/curve_speed.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/line.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/state.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::nav { +template +TRacingLine::TRacingLine(const Dynamics& dynamics, const Config& config) : + dynamics{dynamics}, config{config} { + const float front = dynamics.get_model().chassis.front_length; + const float rear = dynamics.get_model().chassis.rear_length; + const float half = dynamics.get_model().chassis.half_width; + + const std::array corners{{ + {.x = front, .y = half}, + {.x = -rear, .y = half}, + {.x = -rear, .y = -half}, + {.x = front, .y = -half}, + }}; + + for (uint8_t side = 0; side < corners.size(); side++) { + const core::Vector& from = corners.at(side); + const core::Vector& to = corners.at((side + 1) % corners.size()); + const auto steps = static_cast(std::ceil(from.distance(to) / outline_spacing)); + + for (uint8_t step = 0; step < steps and this->outline_size < max_outline_points; step++) { + const float fraction = static_cast(step) / static_cast(steps); + + this->outline.at(this->outline_size++) = { + .x = from.x + fraction * (to.x - from.x), + .y = from.y + fraction * (to.y - from.y), + }; + } + } + + for (uint8_t i = 0; i < this->outline_size; i++) { + this->reach = std::max(this->reach, this->outline.at(i).magnitude()); + } +} + +template +void TRacingLine::reset() { + this->line.ready = false; + this->phase = Phase::IDLE; +} + +template +void TRacingLine::begin( + const Route& route, std::span segments, const TMaze& maze, const RunProfile& profile +) { + this->reset(); + this->maze = &maze; + this->segments = segments; + this->run_profile = profile; + this->openings.fill(0); + this->sweeps = 0; + + TPlanner::for_each_wall(route, [this](const GridPose& wall) { this->open(wall); }); + + for (const GridPoint& cell : maze.get_goal()) { + for (const Side side : all_sides) { + const GridPoint neighbor = cell + side; + + if (TMaze::contains(neighbor) and maze.is_goal(neighbor) and + maze.get_wall({.position = cell, .orientation = side}) == WallState::NO_WALL) { + this->open({.position = cell, .orientation = side}); + } + } + } + + float total = 0.0F; + + for (const Segment& segment : segments) { + total += std::abs(segment.length); + } + + const auto samples = static_cast(std::lround(total / this->config.spacing)) + 1; + + if (segments.empty() or samples > Line::max_samples or samples < 2 * fixed_samples + 2) { + this->finish(false); + return; + } + + this->line.size = static_cast(samples); + this->line.spacing = total / static_cast(samples - 1); + this->cursor = 0; + this->segment_index = 0; + this->segment_start = 0.0F; + this->phase = Phase::SAMPLE; +} + +template +bool TRacingLine::step() { + uint32_t budget = this->config.samples_per_step; + + while (budget > 0) { + switch (this->phase) { + case Phase::IDLE: + return true; + + case Phase::SAMPLE: + this->sample_route(budget); + break; + + case Phase::BOUNDS: + this->find_bounds(budget); + break; + + case Phase::SOLVE: + this->solve_window(); + return false; + + case Phase::RESAMPLE: + this->resample(); + return false; + + case Phase::CHECK: + this->check(budget); + break; + + case Phase::SPEED: + this->plan_speeds(); + return this->phase == Phase::IDLE; + } + } + + return this->phase == Phase::IDLE; +} + +template +const Line& TRacingLine::get_line() const { + return this->line; +} + +template +uint8_t TRacingLine::get_sweeps() const { + return this->sweeps; +} + +template +void TRacingLine::sample_route(uint32_t& budget) { + for (; budget > 0 and this->cursor < this->line.size; budget--, this->cursor++) { + const float distance = static_cast(this->cursor) * this->line.spacing; + + while (this->segment_index + 1U < this->segments.size() and + distance > this->segment_start + std::abs(this->segments[this->segment_index].length)) { + this->segment_start += std::abs(this->segments[this->segment_index].length); + this->segment_index++; + } + + const Segment& segment = this->segments[this->segment_index]; + const float side = std::copysign(1.0F, segment.length); + const float along = std::clamp(distance - this->segment_start, 0.0F, std::abs(segment.length)); + + Pose local{.position = {.x = side * along, .y = 0.0F}, .orientation = 0.0F}; + + if (segment.kind == SegmentKind::TURN) { + const auto point = this->dynamics.get_turn(this->run_profile, segment.turn).template sample(along); + local = {.position = {.x = point.x, .y = side * point.y}, .orientation = side * point.heading}; + } + + const Pose pose = segment.start.compose(local); + + this->line.xs.at(this->cursor) = pose.position.x; + this->line.ys.at(this->cursor) = pose.position.y; + } + + if (this->cursor == this->line.size) { + this->start_sweep(); + } +} + +template +void TRacingLine::find_bounds(uint32_t& budget) { + for (; budget > 0 and this->cursor < this->window_size; budget--, this->cursor++) { + const auto index = static_cast(this->window_start + this->cursor); + const float heading = this->get_heading(index); + const float cosine = std::cos(heading); + const float sine = std::sin(heading); + + const core::Vector position{.x = this->line.xs.at(index), .y = this->line.ys.at(index)}; + + this->headings.at(this->cursor) = heading; + this->lower.at(this->cursor) = 0.0F; + this->upper.at(this->cursor) = 0.0F; + this->old_x.at(this->cursor) = position.x; + this->old_y.at(this->cursor) = position.y; + this->was_clear.at(this->cursor + 1U) = this->is_clear(position, cosine, sine, this->config.margin); + + if (not this->was_clear.at(this->cursor + 1U)) { + continue; + } + + const auto steps = static_cast( + std::min(std::floor(this->config.trust / this->config.scan_step), max_scan_steps) + ); + + for (const float direction : {1.0F, -1.0F}) { + float reached = 0.0F; + + for (uint8_t step = 1; step <= steps; step++) { + const float offset = direction * static_cast(step) * this->config.scan_step; + const core::Vector moved{.x = position.x - offset * sine, .y = position.y + offset * cosine}; + + if (not this->is_clear(moved, cosine, sine, this->config.margin)) { + break; + } + + reached = offset; + } + + if (direction > 0.0F) { + this->upper.at(this->cursor) = reached; + } else { + this->lower.at(this->cursor) = reached; + } + } + } + + if (this->cursor == this->window_size) { + this->phase = Phase::SOLVE; + } +} + +template +void TRacingLine::solve_window() { + const uint16_t first = this->window_start; + const uint8_t size = this->window_size; + + std::array normals_x{}; + std::array normals_y{}; + + for (uint8_t j = 0; j < size; j++) { + normals_x.at(j) = -std::sin(static_cast(this->headings.at(j))); + normals_y.at(j) = std::cos(static_cast(this->headings.at(j))); + } + + this->solve_slides(std::span{normals_x}.first(size), std::span{normals_y}.first(size)); + + const auto neighbor_clear = [this](uint16_t index) { + const float heading = this->get_heading(index); + + return this->is_clear( + {.x = this->line.xs.at(index), .y = this->line.ys.at(index)}, std::cos(heading), std::sin(heading), + this->config.margin + ); + }; + + this->was_clear.at(0) = neighbor_clear(first - 1U); + this->was_clear.at(size + 1U) = neighbor_clear(first + size); + + const auto place = [&]() { + for (uint8_t j = 0; j < size; j++) { + this->line.xs.at(first + j) = this->old_x.at(j) + static_cast(this->slides.at(j) * normals_x.at(j)); + this->line.ys.at(first + j) = this->old_y.at(j) + static_cast(this->slides.at(j) * normals_y.at(j)); + } + }; + + place(); + + for (uint8_t shrink = 0;; shrink++) { + std::array clashes{}; + bool clash = false; + + for (uint8_t j = 0; j < size + 2U; j++) { + clashes.at(j) = this->was_clear.at(j) and not neighbor_clear(first + j - 1U); + clash = clash or clashes.at(j); + } + + if (not clash) { + break; + } + + if (shrink == max_shrinks) { + this->slides.fill(0.0); + place(); + break; + } + + for (uint8_t j = 0; j < size + 2U; j++) { + if (not clashes.at(j)) { + continue; + } + + const auto from = static_cast(std::max(j - 4, 0)); + const auto to = static_cast(std::min(j + 2, size - 1)); + + for (std::size_t k = from; k <= to; k++) { + this->slides.at(k) *= 0.5; + } + } + + place(); + } + + for (uint8_t j = 0; j < size; j++) { + this->sweep_slide = std::max(this->sweep_slide, static_cast(std::abs(this->slides.at(j)))); + } + + this->window_start += stride; + + if (this->window_start + fixed_samples >= this->line.size) { + this->phase = Phase::RESAMPLE; + return; + } + + this->start_window(); +} + +template +void TRacingLine::resample() { + float length = 0.0F; + + for (uint16_t i = 0; i + 1U < this->line.size; i++) { + length += + std::hypot(this->line.xs.at(i + 1U) - this->line.xs.at(i), this->line.ys.at(i + 1U) - this->line.ys.at(i)); + } + + const auto samples = static_cast(std::lround(length / this->config.spacing)) + 1; + + if (samples > Line::max_samples or samples < 2 * fixed_samples + 2) { + this->finish(false); + return; + } + + const auto size = static_cast(samples); + const float spacing = length / static_cast(size - 1); + + uint16_t read_index = 0; + float read_length = 0.0F; + + for (uint16_t i = 0; i < size; i++) { + if (i + 1U == size) { + this->line.curvatures.at(i) = this->line.xs.at(this->line.size - 1); + this->line.speeds.at(i) = this->line.ys.at(this->line.size - 1); + continue; + } + + const float distance = static_cast(i) * spacing; + + float step_length = 0.0F; + + while (true) { + step_length = std::hypot( + this->line.xs.at(read_index + 1U) - this->line.xs.at(read_index), + this->line.ys.at(read_index + 1U) - this->line.ys.at(read_index) + ); + + if (read_length + step_length >= distance or read_index + 2U >= this->line.size) { + break; + } + + read_length += step_length; + read_index++; + } + + const float fraction = + step_length > 0.0F ? std::clamp((distance - read_length) / step_length, 0.0F, 1.0F) : 0.0F; + + this->line.curvatures.at(i) = this->line.xs.at(read_index) + + fraction * (this->line.xs.at(read_index + 1U) - this->line.xs.at(read_index)); + this->line.speeds.at(i) = this->line.ys.at(read_index) + + fraction * (this->line.ys.at(read_index + 1U) - this->line.ys.at(read_index)); + } + + std::copy_n(this->line.curvatures.begin(), size, this->line.xs.begin()); + std::copy_n(this->line.speeds.begin(), size, this->line.ys.begin()); + this->line.size = size; + this->line.spacing = spacing; + this->sweeps++; + + if (this->sweep_slide < this->config.convergence or this->sweeps >= this->config.max_sweeps) { + this->cursor = fixed_samples; + this->phase = Phase::CHECK; + return; + } + + this->start_sweep(); +} + +template +void TRacingLine::check(uint32_t& budget) { + const uint16_t end = this->line.size - fixed_samples; + + for (; budget > 0 and this->cursor < end; budget--, this->cursor++) { + const float heading = this->get_heading(this->cursor); + + if (not this->is_clear( + {.x = this->line.xs.at(this->cursor), .y = this->line.ys.at(this->cursor)}, std::cos(heading), + std::sin(heading), this->config.least_margin + )) { + this->finish(false); + return; + } + } + + if (this->cursor == end) { + this->phase = Phase::SPEED; + } +} + +template +void TRacingLine::measure_curvature() { + const uint16_t last = this->line.size - 1; + + const auto heading_after = [this](uint16_t index) { + return std::atan2( + this->line.ys.at(index + 1U) - this->line.ys.at(index), + this->line.xs.at(index + 1U) - this->line.xs.at(index) + ); + }; + + float heading = heading_after(0); + float before = 0.0F; + float at = 0.0F; + + for (uint16_t i = 0; i <= last; i++) { + float after = 0.0F; + + if (i + 1U < last) { + const float next = heading_after(i + 1U); + after = core::math::wrap_angle(next - heading) / this->line.spacing; + heading = next; + } + + float sum = 0.0F; + uint8_t count = 0; + + if (i > 0) { + sum += before; + count++; + } + + sum += at; + count++; + + if (i < last) { + sum += after; + count++; + } + + this->line.curvatures.at(i) = sum / static_cast(count); + before = at; + at = after; + } +} + +template +void TRacingLine::plan_speeds() { + this->measure_curvature(); + + const uint16_t size = this->line.size; + const float spacing = this->line.spacing; + + const auto bending = [this, size, spacing](std::size_t index) { + const std::size_t from = std::min(index, size - 2U); + + return Bending{ + .curvature = this->line.curvatures.at(index), + .sharpness = (this->line.curvatures.at(from + 1U) - this->line.curvatures.at(from)) / spacing, + }; + }; + + CurveLimits limits = this->dynamics.get_curve_limits(this->run_profile); + limits.lateral *= this->config.lateral_share; + + CurveSpeed::plan(bending, std::span{this->line.speeds}.first(size), spacing, 0.0F, 0.0F, limits); + CurveSpeed::integrate(std::span{this->line.speeds}.first(size), spacing, std::span{this->line.times}.first(size)); + + this->line.finish_time = this->line.times.at(size - 1); + + const float cell_size = this->dynamics.get_model().maze.cell_size; + + for (uint16_t i = 0; i < size; i++) { + const GridPoint cell = GridPoint::from_vector({.x = this->line.xs.at(i), .y = this->line.ys.at(i)}, cell_size); + + if (this->maze->is_goal(cell)) { + this->line.finish_time = this->line.times.at(i); + break; + } + } + + this->finish(true); +} + +template +void TRacingLine::start_sweep() { + this->sweep_slide = 0.0F; + this->window_start = fixed_samples; + this->start_window(); +} + +template +void TRacingLine::start_window() { + this->window_size = + static_cast(std::min(window, this->line.size - fixed_samples - this->window_start)); + this->cursor = 0; + this->phase = Phase::BOUNDS; +} + +template +void TRacingLine::finish(bool found) { + this->line.ready = found; + this->maze = nullptr; + this->segments = {}; + this->phase = Phase::IDLE; +} + +template +float TRacingLine::get_heading(uint16_t index) const { + return this->line.get_heading(index); +} + +template +void TRacingLine::solve_slides(std::span normals_x, std::span normals_y) { + const uint16_t first = this->window_start; + const std::size_t size = normals_x.size(); + const double length_weight = static_cast(this->config.length_weight) * + static_cast(this->line.spacing) * static_cast(this->line.spacing); + + this->diagonal.fill(0.0); + this->first_band.fill(0.0); + this->second_band.fill(0.0); + this->linear.fill(0.0); + + const auto add_row = [&](int32_t row, std::span weights, double scale) { + double base_x = 0.0; + double base_y = 0.0; + + for (std::size_t k = 0; k < weights.size(); k++) { + const auto index = static_cast(row + static_cast(k)); + base_x += weights[k] * static_cast(this->line.xs.at(index)); + base_y += weights[k] * static_cast(this->line.ys.at(index)); + } + + for (std::size_t k = 0; k < weights.size(); k++) { + const int32_t offset = row + static_cast(k) - first; + + if (offset < 0 or static_cast(offset) >= size) { + continue; + } + + const auto j = static_cast(offset); + + this->linear.at(j) += scale * weights[k] * (base_x * normals_x[j] + base_y * normals_y[j]); + + for (std::size_t l = k; l < weights.size(); l++) { + const std::size_t m = j + (l - k); + + if (m >= size) { + continue; + } + + const double value = + scale * weights[k] * weights[l] * (normals_x[j] * normals_x[m] + normals_y[j] * normals_y[m]); + + if (m == j) { + this->diagonal.at(j) += value; + } else if (m == j + 1) { + this->first_band.at(j) += value; + } else { + this->second_band.at(j) += value; + } + } + } + }; + + constexpr std::array second_difference{1.0, -2.0, 1.0}; + constexpr std::array first_difference{-1.0, 1.0}; + + const auto last_row = static_cast(first + size - 1); + + for (int32_t row = first - 2; row <= last_row; row++) { + add_row(row, second_difference, 1.0); + } + + for (int32_t row = first - 1; row <= last_row; row++) { + add_row(row, first_difference, length_weight); + } + + const auto product = [this, size](std::size_t j, std::span values) { + double result = this->diagonal.at(j) * values[j]; + + if (j + 1 < size) { + result += this->first_band.at(j) * values[j + 1]; + } + + if (j + 2 < size) { + result += this->second_band.at(j) * values[j + 2]; + } + + if (j >= 1) { + result += this->first_band.at(j - 1) * values[j - 1]; + } + + if (j >= 2) { + result += this->second_band.at(j - 2) * values[j - 2]; + } + + return result; + }; + + const auto objective = [&](std::span values) { + double result = 0.0; + + for (std::size_t j = 0; j < size; j++) { + result += values[j] * (product(j, values) + 2.0 * this->linear.at(j)); + } + + return result; + }; + + const auto coupling = [this](std::size_t from, std::size_t to) { + if (to - from == 1) { + return this->first_band.at(from); + } + + return to - from == 2 ? this->second_band.at(from) : 0.0; + }; + + this->slides.fill(0.0); + + const std::span current{this->slides.data(), size}; + const std::span candidate{this->trial.data(), size}; + + double value = 0.0; + + for (uint8_t iteration = 0; iteration < max_newton_iterations; iteration++) { + uint8_t free = 0; + + for (std::size_t j = 0; j < size; j++) { + const double slope = product(j, current) + this->linear.at(j); + const auto low = static_cast(this->lower.at(j)); + const auto high = static_cast(this->upper.at(j)); + + this->gradient.at(j) = slope; + this->direction.at(j) = 0.0; + + const bool at_lower = this->slides.at(j) <= low + 1.0e-9 and slope > 0.0; + const bool at_upper = this->slides.at(j) >= high - 1.0e-9 and slope < 0.0; + + if (not at_lower and not at_upper) { + this->free_indices.at(free++) = static_cast(j); + } + } + + if (free == 0) { + break; + } + + bool singular = false; + + for (uint8_t c = 0; c < free; c++) { + const uint8_t j = this->free_indices.at(c); + const double first_coupling = c >= 1 ? coupling(this->free_indices.at(c - 1), j) : 0.0; + const double second_coupling = c >= 2 ? coupling(this->free_indices.at(c - 2), j) : 0.0; + + const double second = c >= 2 ? second_coupling / this->factor_diagonal.at(c - 2) : 0.0; + const double first_factor = + c >= 1 ? (first_coupling - + (c >= 2 ? second * this->factor_first.at(c - 1) * this->factor_diagonal.at(c - 2) : 0.0)) / + this->factor_diagonal.at(c - 1) : + 0.0; + + this->factor_second.at(c) = second; + this->factor_first.at(c) = first_factor; + this->factor_diagonal.at(c) = + this->diagonal.at(j) - first_factor * first_factor * (c >= 1 ? this->factor_diagonal.at(c - 1) : 0.0) - + second * second * (c >= 2 ? this->factor_diagonal.at(c - 2) : 0.0); + + if (this->factor_diagonal.at(c) <= 0.0) { + singular = true; + break; + } + } + + if (singular) { + break; + } + + for (uint8_t c = 0; c < free; c++) { + double result = -this->gradient.at(this->free_indices.at(c)); + + if (c >= 1) { + result -= this->factor_first.at(c) * this->trial.at(c - 1); + } + + if (c >= 2) { + result -= this->factor_second.at(c) * this->trial.at(c - 2); + } + + this->trial.at(c) = result; + } + + for (std::size_t c = free; c-- > 0;) { + double result = this->trial.at(c) / this->factor_diagonal.at(c); + + if (c + 1 < free) { + result -= this->factor_first.at(c + 1) * this->direction.at(this->free_indices.at(c + 1)); + } + + if (c + 2 < free) { + result -= this->factor_second.at(c + 2) * this->direction.at(this->free_indices.at(c + 2)); + } + + this->direction.at(this->free_indices.at(c)) = result; + } + + double step = 1.0; + double trial_value = value; + double moved = 0.0; + + for (uint8_t search = 0; search < 20; search++) { + double decrease = 0.0; + moved = 0.0; + + for (std::size_t j = 0; j < size; j++) { + this->trial.at(j) = std::clamp( + this->slides.at(j) + step * this->direction.at(j), static_cast(this->lower.at(j)), + static_cast(this->upper.at(j)) + ); + decrease += 2.0 * this->gradient.at(j) * (this->trial.at(j) - this->slides.at(j)); + moved = std::max(moved, std::abs(this->trial.at(j) - this->slides.at(j))); + } + + trial_value = objective(candidate); + + if (trial_value <= value + 1.0e-4 * decrease) { + break; + } + + step /= 2.0; + } + + if (trial_value > value) { + break; + } + + std::copy_n(this->trial.begin(), size, this->slides.begin()); + value = trial_value; + + if (moved < 1.0e-7) { + break; + } + } +} + +template +bool TRacingLine::is_clear(const core::Vector& position, float cosine, float sine, float margin) const { + const float cell_size = this->dynamics.get_model().maze.cell_size; + const float half_cell = cell_size / 2.0F; + const float half_wall = this->dynamics.get_model().maze.wall_thickness / 2.0F; + + const int32_t post_x = 2 * static_cast(std::round(position.x / cell_size)); + const int32_t post_y = 2 * static_cast(std::round(position.y / cell_size)); + + const core::Vector post{.x = static_cast(post_x) * half_cell, .y = static_cast(post_y) * half_cell}; + + if (this->hits(position, cosine, sine, post, {.x = half_wall, .y = half_wall}, margin)) { + return false; + } + + constexpr std::array, 4> around{{{0, 1}, {0, -1}, {1, 0}, {-1, 0}}}; + + for (const auto& [dx, dy] : around) { + if (this->is_open(post_x + dx, post_y + dy)) { + continue; + } + + const core::Vector center{ + .x = static_cast(post_x + dx) * half_cell, .y = static_cast(post_y + dy) * half_cell + }; + const core::Vector half_size = + dx == 0 ? core::Vector{.x = half_wall, .y = half_cell} : core::Vector{.x = half_cell, .y = half_wall}; + + if (this->hits(position, cosine, sine, center, half_size, margin)) { + return false; + } + } + + return true; +} + +template +bool TRacingLine::hits( + const core::Vector& position, float cosine, float sine, const core::Vector& center, const core::Vector& half_size, + float margin +) const { + const auto outside = [¢er, &half_size, margin](float x, float y) { + const float dx = std::max(std::abs(x - center.x) - half_size.x, 0.0F); + const float dy = std::max(std::abs(y - center.y) - half_size.y, 0.0F); + return dx * dx + dy * dy >= margin * margin; + }; + + const float bound = this->reach + margin; + const float gap_x = std::max(std::abs(position.x - center.x) - half_size.x, 0.0F); + const float gap_y = std::max(std::abs(position.y - center.y) - half_size.y, 0.0F); + + if (gap_x * gap_x + gap_y * gap_y >= bound * bound) { + return false; + } + + for (uint8_t i = 0; i < this->outline_size; i++) { + const core::Vector& point = this->outline.at(i); + + if (not outside( + position.x + cosine * point.x - sine * point.y, position.y + sine * point.x + cosine * point.y + )) { + return true; + } + } + + const float front = this->dynamics.get_model().chassis.front_length; + const float rear = this->dynamics.get_model().chassis.rear_length; + const float half = this->dynamics.get_model().chassis.half_width; + + for (const float sx : {-1.0F, 1.0F}) { + for (const float sy : {-1.0F, 1.0F}) { + const float offset_x = center.x + sx * half_size.x - position.x; + const float offset_y = center.y + sy * half_size.y - position.y; + const float along = cosine * offset_x + sine * offset_y; + const float across = -sine * offset_x + cosine * offset_y; + + float dx = 0.0F; + + if (along > front) { + dx = along - front; + } else if (along < -rear) { + dx = -rear - along; + } + + const float dy = std::max(std::abs(across) - half, 0.0F); + + if (dx * dx + dy * dy < margin * margin) { + return true; + } + } + } + + return false; +} + +template +bool TRacingLine::is_open(int32_t x, int32_t y) const { + if (x < 0 or y < 0 or x >= lattice_width or y >= lattice_height) { + return false; + } + + const auto bit = static_cast(y * lattice_width + x); + return (this->openings.at(bit / 8U) & (1U << (bit % 8U))) != 0; +} + +template +void TRacingLine::open(const GridPose& wall) { + int32_t x = 2 * wall.position.x + 1; + int32_t y = 2 * wall.position.y + 1; + + switch (wall.orientation) { + case Side::RIGHT: + x++; + break; + case Side::UP: + y++; + break; + case Side::LEFT: + x--; + break; + case Side::DOWN: + y--; + break; + } + + if (x < 0 or y < 0 or x >= lattice_width or y >= lattice_height) { + return; + } + + const auto bit = static_cast(y * lattice_width + x); + this->openings.at(bit / 8U) = static_cast(this->openings.at(bit / 8U) | (1U << (bit % 8U))); +} +} // namespace micras::nav + +#endif // MICRAS_NAV_RACING_LINE_TPP diff --git a/micras_nav/include/micras/nav/impl/wall_model.tpp b/micras_nav/include/micras/nav/impl/wall_model.tpp new file mode 100644 index 0000000..db45844 --- /dev/null +++ b/micras_nav/include/micras/nav/impl/wall_model.tpp @@ -0,0 +1,145 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_WALL_MODEL_TPP +#define MICRAS_NAV_WALL_MODEL_TPP + +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +template +// NOLINTNEXTLINE(readability-function-cognitive-complexity) one ray, crossing up to four walls +RayHit WallModel::cast(const Pose& pose, uint8_t sensor, const TMaze& maze) const { + const RobotModel::WallSensor& mounting = this->sensors.at(sensor); + + const Pose origin = pose.compose({.position = mounting.position, .orientation = mounting.angle}); + const float direction_x = std::cos(origin.orientation); + const float direction_y = std::sin(origin.orientation); + + const float cell_size = this->maze_geometry.cell_size; + const float half_wall = this->maze_geometry.wall_thickness / 2.0F; + + auto column = static_cast(std::floor(origin.position.x / cell_size)); + auto row = static_cast(std::floor(origin.position.y / cell_size)); + + bool grazed_post = false; + + for (uint8_t crossing = 0; crossing < 4; crossing++) { + // NOLINTNEXTLINE(modernize-use-integer-sign-comparison) the comparison the robot's image was verified with + if (column < 0 or row < 0 or column >= width or row >= height) { + break; + } + + const float line_x = static_cast(direction_x > 0.0F ? column + 1 : column) * cell_size; + const float line_y = static_cast(direction_y > 0.0F ? row + 1 : row) * cell_size; + + const float distance_x = + direction_x != 0.0F ? (line_x - origin.position.x) / direction_x : std::numeric_limits::infinity(); + const float distance_y = + direction_y != 0.0F ? (line_y - origin.position.y) / direction_y : std::numeric_limits::infinity(); + + const bool vertical = distance_x < distance_y; + const float distance = vertical ? distance_x : distance_y; + const float across = vertical ? direction_x : direction_y; + const float along = vertical ? direction_y : direction_x; + const float range = distance - half_wall / std::abs(across); + + if (range > this->max_range) { + break; + } + + const GridPose wall{ + .position = {.x = static_cast(column), .y = static_cast(row)}, + // NOLINTNEXTLINE(readability-avoid-nested-conditional-operator) one of the four sides + .orientation = vertical ? (direction_x > 0.0F ? Side::RIGHT : Side::LEFT) : + (direction_y > 0.0F ? Side::UP : Side::DOWN), + }; + + const float base = static_cast(vertical ? row : column) * cell_size; + const float offset = + (vertical ? origin.position.y + distance * direction_y : origin.position.x + distance * direction_x) - base; + + const WallState state = maze.get_wall(wall); + const bool at_post = offset < half_wall or offset > cell_size - half_wall; + + if (state != WallState::WALL and (at_post or grazed_post)) { + if (state == WallState::UNKNOWN or grazed_post) { + break; + } + + grazed_post = true; + } + + if (state == WallState::NO_WALL) { + // NOLINTBEGIN(readability-avoid-nested-conditional-operator) one step along the ray + column = static_cast(column + (vertical ? (direction_x > 0.0F ? 1 : -1) : 0)); + row = static_cast(row + (vertical ? 0 : (direction_y > 0.0F ? 1 : -1))); + // NOLINTEND(readability-avoid-nested-conditional-operator) + continue; + } + + if (range < this->min_range) { + break; + } + + const float lever = vertical ? origin.position.y - pose.position.y : -(origin.position.x - pose.position.x); + const float gap = distance * across; + const float swing = + (vertical ? 1.0F : -1.0F) * along * (gap - std::copysign(half_wall, across)) / (across * across); + + RayHit hit{ + .valid = true, + .wall = wall, + .state = state, + .range = range, + .offset = offset, + .cosine = std::abs(across), + .jacobian = {}, + .vertical = vertical, + .face = (vertical ? line_x : line_y) - std::copysign(half_wall, across), + .base = base, + .low_end = half_wall, + .high_end = cell_size - half_wall, + }; + + hit.jacobian.at(vertical ? 0 : 1) = -1.0F / across; + hit.jacobian.at(2) = lever / across + swing; + + if (state == WallState::WALL) { + hit.low_end = -half_wall; + hit.high_end = cell_size + half_wall; + + GridPose lower = wall; + GridPose higher = wall; + + for (uint8_t i = 0; i < 2; i++) { + lower.position = lower.position + (vertical ? Side::DOWN : Side::LEFT); + higher.position = higher.position + (vertical ? Side::UP : Side::RIGHT); + + if (hit.low_end == -half_wall - static_cast(i) * cell_size and + TMaze::contains(lower.position) and maze.get_wall(lower) == WallState::WALL) { + hit.low_end -= cell_size; + } + + if (hit.high_end == cell_size + half_wall + static_cast(i) * cell_size and + TMaze::contains(higher.position) and maze.get_wall(higher) == WallState::WALL) { + hit.high_end += cell_size; + } + } + } + + return hit; + } + + return {}; +} +} // namespace micras::nav + +#endif // MICRAS_NAV_WALL_MODEL_TPP diff --git a/micras_nav/include/micras/nav/impl/wall_observer.tpp b/micras_nav/include/micras/nav/impl/wall_observer.tpp new file mode 100644 index 0000000..6b61f77 --- /dev/null +++ b/micras_nav/include/micras/nav/impl/wall_observer.tpp @@ -0,0 +1,102 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_WALL_OBSERVER_TPP +#define MICRAS_NAV_WALL_OBSERVER_TPP + +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/localizer.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/wall_model.hpp" + +namespace micras::nav { +template +TWallObserver::TWallObserver(const Config& config) : config{config} { } + +template +void TWallObserver::reset() { + this->votes.fill(0); +} + +template +bool TWallObserver::update( + const Measurements& measurements, const Localizer& localizer, const WallModel& wall_model, + TMaze& maze +) { + const State& state = localizer.get_state(); + + if (std::abs(state.velocity.angular) > this->config.max_angular_speed) { + return false; + } + + const Pose sampled = state.pose.compose({ + .position = {.x = -state.velocity.linear * this->config.range_delay, .y = 0.0F}, + .orientation = -state.velocity.angular * this->config.range_delay, + }); + + bool changed = false; + + for (uint8_t sensor = 0; sensor < number_of_wall_sensors; sensor++) { + const WallReading& reading = measurements.walls.at(sensor); + + if (not reading.is_new or reading.blind) { + continue; + } + + const RayHit hit = wall_model.cast(sampled, sensor, maze); + + if (not hit.valid or hit.state != WallState::UNKNOWN or + not wall_model.is_footprint_clear( + hit, sensor, localizer.get_position_deviation(), localizer.get_orientation_deviation() + )) { + continue; + } + + const float tolerance = this->config.tolerance + this->config.relative_tolerance * hit.range; + int8_t& count = this->votes.at(get_index(hit.wall)); + + if (reading.valid and std::abs(reading.distance - hit.range) <= tolerance) { + count = static_cast(std::min(count + 1, this->config.votes_to_decide)); + } else if ( + (not reading.valid or reading.distance > hit.range + tolerance) and + hit.range <= this->config.detection_range + ) { + count = static_cast(std::max(count - 1, -this->config.votes_to_decide)); + } else { + continue; + } + + if (std::abs(count) >= this->config.votes_to_decide) { + changed = maze.set_wall(hit.wall, count > 0) or changed; + } + } + + return changed; +} + +template +constexpr uint16_t TWallObserver::get_index(const GridPose& wall) { + const uint8_t x = wall.position.x; + const uint8_t y = wall.position.y; + + switch (wall.orientation) { + case Side::LEFT: + return static_cast(x * height + y); + case Side::RIGHT: + return static_cast((x + 1) * height + y); + case Side::DOWN: + return static_cast((width + 1) * height + x * (height + 1) + y); + case Side::UP: + default: + return static_cast((width + 1) * height + x * (height + 1) + y + 1); + } +} +} // namespace micras::nav + +#endif // MICRAS_NAV_WALL_OBSERVER_TPP diff --git a/micras_nav/include/micras/nav/lattice.hpp b/micras_nav/include/micras/nav/lattice.hpp new file mode 100644 index 0000000..6d214e4 --- /dev/null +++ b/micras_nav/include/micras/nav/lattice.hpp @@ -0,0 +1,424 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_LATTICE_HPP +#define MICRAS_NAV_LATTICE_HPP + +#include +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Slalom turns the robot can execute, named by where they start and end. + * + * @details S is a straight along the grid and D a diagonal, followed by the angle turned. SS90S is + * the short turn that fits inside one cell, which is also the turn used while searching, and SS90L + * the long one that needs a cell of straight on both sides. + * + * The turns from SD45E on enter a cell and leave it through one of its other three walls, two bends + * each. Their last letter is the wall they leave through, in the canonical frame of TurnPrimitive: T + * the wall on the left of the entry, E the wall across from it and B the wall on the right. Those that + * start on a diagonal may end turned to the right of it, since the side of a diagonal turn is fixed by + * the wall it starts on: DS45E, DD90E and DS135B turn right, and DD0E does not turn, + * shifting to the next diagonal. + */ +enum class TurnId : uint8_t { + SS90S = 0, + SS90L = 1, + SS180 = 2, + SD45 = 3, + SD135 = 4, + DS45 = 5, + DS135 = 6, + DD90 = 7, + SD45E = 8, + SD45T = 9, + SD135T = 10, + DS45T = 11, + DD0E = 12, + DS45E = 13, + DD90E = 14, + DS135B = 15, + NUMBER_OF_TURNS = 16, +}; + +/** + * @brief Number of different turns. + */ +inline constexpr uint8_t number_of_turns{std::to_underlying(TurnId::NUMBER_OF_TURNS)}; + +/** + * @brief First of the turns of two bends, whose shapes are designed before the firmware is compiled + * rather than while it is. + */ +inline constexpr uint8_t first_two_bend_turn{std::to_underlying(TurnId::SD45E)}; + +/** + * @brief Number of turns of two bends. + */ +inline constexpr uint8_t number_of_two_bend_turns{number_of_turns - first_two_bend_turn}; + +/** + * @brief Side a turn is made to. + */ +enum class TurnSide : uint8_t { + LEFT = 0, + RIGHT = 1, +}; + +/** + * @brief Point of the lattice the routes are planned on, in units of half a cell. + * + * @details Both coordinates even is a post, both odd is the center of a cell, and one of each is + * the midpoint of a wall: a vertical wall when x is even and a horizontal one when y is even. Every + * slalom turn starts and ends at the midpoint of a wall, which is why those are the nodes. + */ +struct LatticePoint { + /** + * @brief Check if the point is the midpoint of a wall that runs along y. + * + * @return True if the point is on a vertical wall. + */ + constexpr bool on_vertical_wall() const { return this->x % 2 == 0 and this->y % 2 != 0; } + + /** + * @brief Check if the point is the midpoint of a wall that runs along x. + * + * @return True if the point is on a horizontal wall. + */ + constexpr bool on_horizontal_wall() const { return this->x % 2 != 0 and this->y % 2 == 0; } + + /** + * @brief Add two points. + * + * @param other The point to add. + * @return The sum of the points. + */ + constexpr LatticePoint operator+(const LatticePoint& other) const { + return {.x = static_cast(this->x + other.x), .y = static_cast(this->y + other.y)}; + } + + /** + * @brief Compare two points for equality. + * + * @param other The other point to compare. + * @return True if the points are equal, false otherwise. + */ + constexpr bool operator==(const LatticePoint& other) const = default; + + int8_t x; + int8_t y; +}; + +/** + * @brief Node of the lattice: the midpoint of a wall, crossed with one of eight headings. + * + * @note The heading counts steps of 45 degrees from the positive x axis, counterclockwise, so the + * even ones run along the grid and the odd ones are diagonal. + */ +struct LatticePose { + /** + * @brief Number of headings. + */ + static constexpr uint8_t number_of_headings{8}; + + /** + * @brief Check if the heading is diagonal. + * + * @return True if the heading is diagonal, false if it runs along the grid. + */ + constexpr bool is_diagonal() const { return this->heading % 2 != 0; } + + /** + * @brief Check if the node exists, which takes a wall that the heading crosses. + * + * @return True if the node is the midpoint of a wall and the heading is not parallel to it. + */ + constexpr bool is_valid() const { + if (this->point.on_vertical_wall()) { + return this->heading % 4 != 2; + } + + return this->point.on_horizontal_wall() and this->heading % 4 != 0; + } + + /** + * @brief Get the displacement of one step along the heading. + * + * @note A step goes from one wall to the next, so it is a whole cell along the grid and half of + * the diagonal of a cell along a diagonal. + * + * @return The displacement in lattice units. + */ + constexpr LatticePoint step() const { + constexpr std::array steps{{ + {.x = 2, .y = 0}, + {.x = 1, .y = 1}, + {.x = 0, .y = 2}, + {.x = -1, .y = 1}, + {.x = -2, .y = 0}, + {.x = -1, .y = -1}, + {.x = 0, .y = -2}, + {.x = 1, .y = -1}, + }}; + + return steps.at(this->heading); + } + + /** + * @brief Get the node one step ahead. + * + * @return The node after moving one step along the heading. + */ + constexpr LatticePose advanced() const { return {.point = this->point + this->step(), .heading = this->heading}; } + + /** + * @brief Get the heading as an angle. + * + * @return The heading in radians, in [0, 2*pi). + */ + constexpr float angle() const { return static_cast(this->heading) * std::numbers::pi_v / 4.0F; } + + /** + * @brief Get the side a diagonal node can turn to. + * + * @details A robot that crosses a wall diagonally is veering to one side of the perpendicular + * of that wall, and every turn that starts there continues to that side: the ones to the other + * side only become possible one step later, at the next wall. + * + * @return The side of the turns that can start at this node. + */ + constexpr TurnSide diagonal_turn_side() const { + const bool even_quadrant = (this->heading / 2) % 2 == 0; + return this->point.on_vertical_wall() == even_quadrant ? TurnSide::LEFT : TurnSide::RIGHT; + } + + /** + * @brief Get the wall the node is on, as a side of a cell. + * + * @note The cell is the one behind the wall, except for the walls of the left and bottom + * borders, which only have a cell ahead. + * + * @return The cell and the side of it where the wall is. + */ + constexpr GridPose wall() const { + if (this->point.on_vertical_wall()) { + const auto row = static_cast((this->point.y - 1) / 2); + + if (this->point.x < 2) { + return {.position = {.x = 0, .y = row}, .orientation = Side::LEFT}; + } + + return { + .position = {.x = static_cast(this->point.x / 2 - 1), .y = row}, .orientation = Side::RIGHT + }; + } + + const auto column = static_cast((this->point.x - 1) / 2); + + if (this->point.y < 2) { + return {.position = {.x = column, .y = 0}, .orientation = Side::DOWN}; + } + + return {.position = {.x = column, .y = static_cast(this->point.y / 2 - 1)}, .orientation = Side::UP}; + } + + /** + * @brief Get the cell the robot is entering when it crosses the node. + * + * @note The coordinates are signed so that a node on the border, heading out, gives a cell + * outside of the maze instead of wrapping around. + * + * @return The coordinates of the cell ahead of the wall. + */ + constexpr LatticePoint cell_ahead() const { + const LatticePoint step = this->step(); + + if (this->point.on_vertical_wall()) { + return { + .x = static_cast(this->point.x / 2 - (step.x > 0 ? 0 : 1)), + .y = static_cast((this->point.y - 1) / 2), + }; + } + + return { + .x = static_cast((this->point.x - 1) / 2), + .y = static_cast(this->point.y / 2 - (step.y > 0 ? 0 : 1)), + }; + } + + /** + * @brief Get the pose of the node in the maze frame. + * + * @param cell_size The size of the cells in meters. + * @return The pose in meters and radians. + */ + constexpr Pose to_pose(float cell_size) const { + return { + .position = + {.x = static_cast(this->point.x) * cell_size / 2.0F, + .y = static_cast(this->point.y) * cell_size / 2.0F}, + .orientation = this->angle(), + }; + } + + /** + * @brief Compare two nodes for equality. + * + * @param other The other node to compare. + * @return True if the nodes are equal, false otherwise. + */ + constexpr bool operator==(const LatticePose& other) const = default; + + LatticePoint point; + uint8_t heading; +}; + +/** + * @brief Place of a turn in the lattice, for the turn to the left in its canonical frame. + * + * @details The canonical frame has the entry node at the origin, on a vertical wall, heading along + * +x for the turns that start on a straight and along the diagonal between +x and +y for the ones + * that start on a diagonal. The gates are the walls crossed between the entry and the exit nodes, + * in the order they are crossed. + */ +struct TurnPrimitive { + /** + * @brief Largest number of walls a turn crosses between its entry and exit nodes. + */ + static constexpr uint8_t max_gates{2}; + + bool diagonal_entry; + uint8_t rotation; + LatticePoint exit; + std::array gates; + uint8_t number_of_gates; +}; + +/** + * @brief Place of every turn in the lattice, indexed by TurnId. + */ +inline constexpr std::array turn_primitives{{ + {.diagonal_entry = false, .rotation = 2, .exit = {.x = 1, .y = 1}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = false, + .rotation = 2, + .exit = {.x = 3, .y = 3}, + .gates = {{{.x = 2, .y = 0}, {.x = 3, .y = 1}}}, + .number_of_gates = 2}, + {.diagonal_entry = false, + .rotation = 4, + .exit = {.x = 0, .y = 2}, + .gates = {{{.x = 1, .y = 1}, {}}}, + .number_of_gates = 1}, + {.diagonal_entry = false, + .rotation = 1, + .exit = {.x = 3, .y = 1}, + .gates = {{{.x = 2, .y = 0}, {}}}, + .number_of_gates = 1}, + {.diagonal_entry = false, + .rotation = 3, + .exit = {.x = 2, .y = 2}, + .gates = {{{.x = 2, .y = 0}, {.x = 3, .y = 1}}}, + .number_of_gates = 2}, + {.diagonal_entry = true, + .rotation = 1, + .exit = {.x = 1, .y = 3}, + .gates = {{{.x = 1, .y = 1}, {}}}, + .number_of_gates = 1}, + {.diagonal_entry = true, + .rotation = 3, + .exit = {.x = -2, .y = 2}, + .gates = {{{.x = 1, .y = 1}, {.x = 0, .y = 2}}}, + .number_of_gates = 2}, + {.diagonal_entry = true, + .rotation = 2, + .exit = {.x = 0, .y = 2}, + .gates = {{{.x = 1, .y = 1}, {}}}, + .number_of_gates = 1}, + {.diagonal_entry = false, .rotation = 1, .exit = {.x = 2, .y = 0}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = false, .rotation = 1, .exit = {.x = 1, .y = 1}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = false, .rotation = 3, .exit = {.x = 1, .y = 1}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = true, .rotation = 1, .exit = {.x = 1, .y = 1}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = true, .rotation = 0, .exit = {.x = 2, .y = 0}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = true, .rotation = 7, .exit = {.x = 2, .y = 0}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = true, .rotation = 6, .exit = {.x = 2, .y = 0}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = true, .rotation = 5, .exit = {.x = 1, .y = -1}, .gates = {}, .number_of_gates = 0}, +}}; + +/** + * @brief Get the place of a turn in the lattice. + * + * @param turn The turn. + * @return The primitive of the turn, for the turn to the left in its canonical frame. + */ +constexpr const TurnPrimitive& get_primitive(TurnId turn) { + return turn_primitives.at(std::to_underlying(turn)); +} + +/** + * @brief Move a displacement from the canonical frame of a turn to the frame of an entry node. + * + * @param canonical The displacement in the canonical frame of the turn to the left. + * @param diagonal_entry Whether the canonical heading is diagonal. + * @param heading The heading of the entry node. + * @param side The side the turn is made to. + * @return The displacement in the lattice, relative to the entry node. + */ +constexpr LatticePoint + from_canonical(const LatticePoint& canonical, bool diagonal_entry, uint8_t heading, TurnSide side) { + LatticePoint point = canonical; + + if (side == TurnSide::RIGHT) { + point = diagonal_entry ? LatticePoint{.x = canonical.y, .y = canonical.x} : + LatticePoint{.x = canonical.x, .y = static_cast(-canonical.y)}; + } + + for (uint8_t quarter = 0; quarter < heading / 2; quarter++) { + point = {.x = static_cast(-point.y), .y = point.x}; + } + + return point; +} + +/** + * @brief Get the node where a turn ends. + * + * @param entry The node where the turn starts. + * @param turn The turn. + * @param side The side the turn is made to. + * @return The exit node of the turn. + */ +constexpr LatticePose get_turn_exit(const LatticePose& entry, TurnId turn, TurnSide side) { + const TurnPrimitive& primitive = get_primitive(turn); + const uint8_t rotation = + side == TurnSide::LEFT ? primitive.rotation : LatticePose::number_of_headings - primitive.rotation; + + return { + .point = entry.point + from_canonical(primitive.exit, primitive.diagonal_entry, entry.heading, side), + .heading = static_cast((entry.heading + rotation) % LatticePose::number_of_headings), + }; +} + +/** + * @brief Get one of the walls a turn crosses between its entry and exit nodes. + * + * @param entry The node where the turn starts. + * @param turn The turn. + * @param side The side the turn is made to. + * @param index The index of the gate, in the order the gates are crossed. + * @return The midpoint of the wall. + */ +constexpr LatticePoint get_turn_gate(const LatticePose& entry, TurnId turn, TurnSide side, uint8_t index) { + const TurnPrimitive& primitive = get_primitive(turn); + + return entry.point + from_canonical(primitive.gates.at(index), primitive.diagonal_entry, entry.heading, side); +} +} // namespace micras::nav + +#endif // MICRAS_NAV_LATTICE_HPP diff --git a/micras_nav/include/micras/nav/line.hpp b/micras_nav/include/micras/nav/line.hpp new file mode 100644 index 0000000..39a6be8 --- /dev/null +++ b/micras_nav/include/micras/nav/line.hpp @@ -0,0 +1,156 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_LINE_HPP +#define MICRAS_NAV_LINE_HPP + +#include +#include + +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +template +class TRacingLine; + +/** + * @brief A path given by samples at equal steps of distance, with the speed of the robot at each. + * + * @details This is the racing line as the executor plays it back. Between two samples the position + * follows the cubic that leaves each sample along its heading, the heading and the curvature change + * linearly and the acceleration is constant, so the reference is smooth and its derivatives are the + * ones the path and the speeds imply. The heading of a sample is the direction from the one before + * it to the one after it. + * + * @note The samples live in static arrays, which is most of the memory the racing line takes. + */ +class Line { +public: + /** + * @brief Largest number of samples, which at 10 mm is a path of 25.6 m. + */ + static constexpr uint16_t max_samples{2560}; + + /** + * @brief Point of the path at a distance from its start. + */ + struct Point { + Pose pose; + float curvature; + float sharpness; + }; + + /** + * @brief Check if the line was planned and timed. + * + * @return True if it can be driven. + */ + bool is_ready() const; + + /** + * @brief Get the length of the path. + * + * @return The length in meters. + */ + float length() const; + + /** + * @brief Get the time the motion along the path takes, from rest to rest. + * + * @return The duration in seconds. + */ + float duration() const; + + /** + * @brief Get the time at which the robot crosses into the goal. + * + * @return The time in seconds, which is how a fast run is timed. + */ + float get_finish_time() const; + + /** + * @brief Get the pose at the start of the path. + * + * @return The pose in the maze frame. + */ + Pose get_start() const; + + /** + * @brief Evaluate the motion along the path at an instant. + * + * @param time Time since the start of the motion, clamped to its duration. + * @return The distance covered, the speed and the acceleration at that instant. + */ + SpeedProfile::Sample sample_motion(float time) const; + + /** + * @brief Evaluate the path at a distance from its start. + * + * @param distance The distance along the path, clamped to its length. + * @return The pose in the maze frame, the curvature and the sharpness there. + */ + Point sample_point(float distance) const; + +private: + template + friend class TRacingLine; + + /** + * @brief Get the heading of the path at a sample. + * + * @param index The index of the sample. + * @return The heading in radians. + */ + float get_heading(uint16_t index) const; + + /** + * @brief Position of every sample in the maze frame. + * + * @note While the line is optimized, the curvatures and the speeds hold the positions being + * resampled. + */ + ///@{ + std::array xs{}; + std::array ys{}; + ///@} + + /** + * @brief Curvature at every sample. + */ + std::array curvatures{}; + + /** + * @brief Speed at every sample. + */ + std::array speeds{}; + + /** + * @brief Time at which every sample is reached. + */ + std::array times{}; + + /** + * @brief Number of samples. + */ + uint16_t size{}; + + /** + * @brief Distance between two samples. + */ + float spacing{}; + + /** + * @brief Time at which the robot crosses into the goal. + */ + float finish_time{}; + + /** + * @brief Whether the line was planned and timed. + */ + bool ready{}; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_LINE_HPP diff --git a/micras_nav/include/micras/nav/localizer.hpp b/micras_nav/include/micras/nav/localizer.hpp new file mode 100644 index 0000000..e91dbd6 --- /dev/null +++ b/micras_nav/include/micras/nav/localizer.hpp @@ -0,0 +1,475 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_LOCALIZER_HPP +#define MICRAS_NAV_LOCALIZER_HPP + +#include +#include +#include + +#include "micras/nav/maze.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/robot_model.hpp" +#include "micras/nav/state.hpp" +#include "micras/nav/wall_model.hpp" + +namespace micras::nav { +/** + * @brief The one owner of the pose of the robot. + * + * @details An extended Kalman filter over the position, the orientation and the bias of the + * gyroscope, in the maze frame. It is advanced every iteration with the encoders and the gyroscope, + * and corrected with every range of a wall sensor that can be trusted, each as one scalar + * measurement against the map of the walls. Because the filter knows that a lateral drift and an + * orientation error go together, a wall seen for a moment identifies the bias of the gyroscope, and + * the estimate stays good through the gaps, turns and diagonals that follow. Standing still is one + * more measurement, of the bias alone. + * + * Every correction goes through three guards: the illuminated spot has to lie on the face of a + * single known wall, the innovation has to be plausible for what the filter believes, and the + * correction is capped, so that a wrong reading is rejected and a wrong wall in the map can only + * drag the pose slowly. + * + * @note The covariance is kept as the factors of `P = U * D * U^T`, with U unit upper triangular and + * D diagonal, and updated with the algorithms of Thornton and Bierman. In single precision this + * stays symmetric and positive by construction, which the conventional form does not. + */ +class Localizer { +public: + /** + * @brief Number of states of the filter: x, y, orientation and gyroscope bias. + */ + static constexpr uint8_t number_of_states{4}; + + /** + * @brief Largest number of iterations the speed can be measured over. + */ + static constexpr uint8_t max_speed_window{32}; + + /** + * @brief Configuration struct for the localizer. + * + * @note The gate is the largest squared innovation accepted, in variances, and the one used + * while standing still is wider since a reading taken at rest has no timing error. The caps + * bound what one correction may move the pose by. The range delay is the delay of the filter + * the fast ranges went through. That filter is slower than the ranges are sampled, so the noise + * of consecutive ranges is mostly the same noise, and the range correlation is how many of them + * share it: the variance of each is multiplied by it, so that what a second of ranges is worth + * does not depend on how many there are in it. The speed window is the number of iterations the + * linear speed is measured over, which trades noise for delay. The rest window is the time over + * which the gyroscope is compared with the encoders to measure its bias. The maximum range is + * the longest range a reading corrects the pose from: past it the beam of the emitter grows + * wide enough to land partly on the floor, the reading loses light the model of the sensor does + * not know about, and the range comes out long by several percent. A reading is used when + * either the range it reads or the range the pose predicts is within it, so that a pose that + * drifted away from a wall has its readings rejected, and counted towards a recovery, instead + * of dropped. + * + * The ends of the side walls are used as references along the path. An end is accepted while + * the robot moves forward faster than the edge speed, since the only time it reverses is to + * park, from a pose it has just measured. The wall has to be closer than the edge range, so + * that nothing can be hiding its end, and the end has to be found within the edge window of + * where it was expected. An end is a single event rather than a stream of readings, so it has a + * cap of its own. The range tolerance, a constant part plus a part proportional to the range, + * tells a reading that is on a wall from one that is past its end. + * + * A pose off by more than the gate allows rejects every reading, and nothing would bring it + * back. So after the recovery rejections in a row with the robot moving, the variance of the + * position grows by the square of the recovery deviation, which lets the next readings through + * and the pose converge to them again. + */ + struct Config { + RobotModel model; + float initial_position_deviation; + float initial_orientation_deviation; + float initial_bias_deviation; + float gate; + float stationary_gate; + float max_position_correction; + float max_orientation_correction; + float max_angular_speed; + float stationary_linear_speed; + float stationary_angular_speed; + float range_delay; + float range_correlation; + float max_range; + float rest_window; + float edge_deviation; + float edge_window; + float edge_speed; + float edge_range; + float max_edge_correction; + float range_tolerance; + float relative_range_tolerance; + uint16_t recovery_rejections; + float recovery_deviation; + uint8_t speed_window; + }; + + /** + * @brief Diagnostics of the filter, for a monitor. + * + * @note The innovation level is the running mean of the squared innovations in variances, which + * stays near one while the filter is consistent. The recoveries count the times the position + * was let loose after too many rejections in a row. + */ + struct Status { + float innovation_level; + uint32_t accepted; + uint32_t rejected; + uint32_t edges; + uint32_t recoveries; + }; + + /** + * @brief Construct a new Localizer object. + * + * @param config The configuration for the localizer. + */ + explicit Localizer(const Config& config); + + /** + * @brief Place the robot at a known pose, forgetting the previous estimate but not the bias. + * + * @param pose The pose of the robot in the maze frame. + * @param measurements The current measurements, which give the reference for the encoders. + */ + void reset(const Pose& pose, const Measurements& measurements); + + /** + * @brief Advance the estimate by one iteration. + * + * @note The wheels and the gyroscope give the motion along the heading. The tires also slide to + * the outside of a curve, in proportion to the lateral acceleration, which neither of them sees, + * so that slide comes from the lateral compliance of the model. + * + * @param measurements The current measurements. + * @param elapsed_time Time since the last iteration, in seconds. + */ + void predict(const Measurements& measurements, float elapsed_time); + + /** + * @brief Set how much of the fan downforce presses the tires, which sets the radius they roll on. + * + * @param downforce The share of the fan downforce, from 0 with the fan off to 1. + */ + void set_downforce(float downforce); + + /** + * @brief Correct the estimate with the ranges of the wall sensors that can be trusted. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + * @param measurements The current measurements. + * @param wall_model The geometry of the wall sensors. + * @param maze The map of the walls, of which only the known walls are used. + */ + template + void correct(const Measurements& measurements, const WallModel& wall_model, const TMaze& maze); + + /** + * @brief Correct the bias of the gyroscope while the robot is commanded to stand still. + * + * @details The rotation the gyroscope reports over a window of time is compared with the one + * the encoders report, and the difference is a measurement of the bias. The encoders are what + * makes this safe: a robot that is slowly turning while it believes it is at rest would + * otherwise have its rotation taken for bias, which is the very error that makes it turn. + * Nothing is measured while the wheels are moving forward, since wheels that slip would then + * report a rotation that did not happen. + * + * @param measurements The current measurements. + * @param elapsed_time Time since the last iteration, in seconds. + */ + void correct_at_rest(const Measurements& measurements, float elapsed_time); + + /** + * @brief Get the estimated state of the robot. + * + * @return The pose in the maze frame and the velocity in the body frame. + */ + const State& get_state() const; + + /** + * @brief Get the estimated pose of the robot. + * + * @return The pose in the maze frame. + */ + const Pose& get_pose() const; + + /** + * @brief Get the cell the robot is in and the direction it faces. + * + * @return The grid pose derived from the estimate. + */ + GridPose get_cell() const; + + /** + * @brief Get the estimated bias of the gyroscope. + * + * @return The bias in rad/s. + */ + float get_gyroscope_bias() const; + + /** + * @brief Get the standard deviation of the estimated position. + * + * @return The largest of the deviations along x and y, in meters. + */ + float get_position_deviation() const; + + /** + * @brief Get the standard deviation of the estimated orientation. + * + * @return The deviation in radians. + */ + float get_orientation_deviation() const; + + /** + * @brief Get the diagnostics of the filter. + * + * @return The status. + */ + const Status& get_status() const; + +private: + /** + * @brief Vector with one element per state. + */ + using Vector = std::array; + + /** + * @brief Matrix with one row and one column per state. + */ + using Matrix = std::array; + + /** + * @brief Number of walls a line is followed over to find an edge. + */ + static constexpr uint8_t edge_search_cells{4}; + + /** + * @brief Indexes of the states. + */ + ///@{ + static constexpr uint8_t x_index{0}; + static constexpr uint8_t y_index{1}; + static constexpr uint8_t orientation_index{2}; + static constexpr uint8_t bias_index{3}; + + ///@} + + /** + * @brief The line of walls a side looking sensor follows, and what its reading last said of it. + * + * @note A sensor locks on a line once its axis, cast from the estimated pose, meets a wall of it + * and the reading agrees. From then on the reading is compared with the plane of that line, and + * whether it is on the plane or past it is remembered, until the robot stops or turns. + */ + struct EdgeTracker { + bool locked; + bool reading_on; + RayHit hit; + }; + + /** + * @brief Look for the end or the start of a side wall in the reading of a sensor, and use it as + * a reference along the path. + * + * @note Only a change of the reading places an edge, and the map only says which edge it is: + * from the wall the sensor is locked on, the first end of the line of walls ahead, or the first + * start after a gap, in the direction the axis moves along it. So an edge is found whether the + * estimate is ahead of the robot or behind it. The reading changes where half of the spot is on + * the wall, with the axis of the sensor on the edge itself. Toward the start of a wall the beam, + * which always points ahead, also lights the end face of the wall, so the start is seen early, by + * the thickness of the wall times the slope of the axis to it. Leaving a wall that face is in the + * shade. An edge the map does not know, because a wall on the way is unknown, is not used. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + * @param sensor The index of the sensor. + * @param reading The reading of the sensor. + * @param sampled The pose of the robot when the reading was taken. + * @param hit What the axis of the sensor meets from that pose. + * @param wall_model The geometry of the wall sensors. + * @param maze The map of the walls. + */ + template + void track_edge( + uint8_t sensor, const WallReading& reading, const Pose& sampled, const RayHit& hit, const WallModel& wall_model, + const TMaze& maze + ); + + /** + * @brief Find the edge ahead along a line of walls. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + * @param maze The map of the walls. + * @param wall A known wall of the line. + * @param upwards Whether to look toward higher coordinates along the line. + * @param start Whether to look for the start of a wall after a gap, rather than for an end. + * @param face How far ahead of a start its end face is seen. + * @return The coordinate of the edge along the line, from the base of the wall, if the map + * knows every wall on the way. + */ + template + std::optional + find_edge(const TMaze& maze, const GridPose& wall, bool upwards, bool start, float face) const; + + /** + * @brief Set the covariance to a diagonal one. + * + * @param variances The variance of each state. + */ + void set_covariance(const Vector& variances); + + /** + * @brief Propagate the covariance through one iteration, with the algorithm of Thornton. + * + * @param transition The derivative of the new state with respect to the old one. + * @param noise_input How each source of process noise enters the state. + * @param noise_variances The variance of each source of process noise. + */ + void propagate(const Matrix& transition, const Matrix& noise_input, const Vector& noise_variances); + + /** + * @brief Count a range the gate accepted or rejected, and let the position loose after too many + * rejections in a row with the robot moving. + * + * @param accepted Whether the range was accepted. + * @param stationary Whether the robot is standing still. + */ + void count_range(bool accepted, bool stationary); + + /** + * @brief Apply one scalar measurement, with the algorithm of Bierman. + * + * @param innovation The measurement minus its prediction. + * @param jacobian The derivative of the measurement with respect to the state. + * @param variance The variance of the measurement noise. + * @param gate The largest squared innovation accepted, in variances. + * @param max_position_correction The most the measurement may move the position by. + * @return True if the measurement was accepted. + */ + bool update(float innovation, const Vector& jacobian, float variance, float gate, float max_position_correction); + + /** + * @brief Get the variance of one state. + * + * @param index The index of the state. + * @return The element of the diagonal of the covariance. + */ + float get_variance(uint8_t index) const; + + /** + * @brief Check if the robot is standing still, as far as its sensors can tell. + * + * @return True if both speeds are below the stationary thresholds. + */ + bool is_stationary() const; + + /** + * @brief Parameters of the filter, with the physical description of the robot. + */ + Config config; + + /** + * @brief Estimated state of the robot. + */ + State state{}; + + /** + * @brief Estimated bias of the gyroscope. + */ + float bias{}; + + /** + * @brief Unit upper triangular factor of the covariance. + */ + Matrix upper{}; + + /** + * @brief Diagonal factor of the covariance. + */ + Vector diagonal{}; + + /** + * @brief Radius the wheels roll on, under the load on their tires. + */ + float rolling_radius; + + /** + * @brief Wheel angles at the last iteration. + */ + ///@{ + float last_left_angle{}; + float last_right_angle{}; + ///@} + + /** + * @brief Time since the gyroscope last produced a sample. + */ + float time_since_imu{}; + + /** + * @brief Duration of the window over which the bias is being measured. + */ + float rest_time{}; + + /** + * @brief Rotation reported by the gyroscope over the window. + */ + float rest_rotation{}; + + /** + * @brief Difference between the wheel angles at the start of the window. + */ + float rest_wheel_difference{}; + + /** + * @brief Distance traveled at each of the last iterations, for the speed estimate. + */ + std::array distances{}; + + /** + * @brief Duration of each of the last iterations, for the speed estimate. + */ + std::array durations{}; + + /** + * @brief Index of the oldest entry of the speed window. + */ + uint8_t window_index{}; + + /** + * @brief Distance traveled over the speed window. + */ + float window_distance{}; + + /** + * @brief Duration of the speed window. + */ + float window_duration{}; + + /** + * @brief What each sensor was seeing at its previous sample. + */ + std::array edge_trackers{}; + + /** + * @brief Diagnostics of the filter. + */ + Status status{}; + + /** + * @brief Number of ranges rejected in a row with the robot moving. + */ + uint16_t rejection_streak{}; +}; +} // namespace micras::nav + +#include "micras/nav/impl/localizer.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_LOCALIZER_HPP diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp new file mode 100644 index 0000000..d4fef81 --- /dev/null +++ b/micras_nav/include/micras/nav/maze.hpp @@ -0,0 +1,252 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_MAZE_HPP +#define MICRAS_NAV_MAZE_HPP + +#include +#include +#include +#include +#include + +#include "micras/core/serializable.hpp" +#include "micras/nav/grid_pose.hpp" + +namespace micras::nav { +/** + * @brief What is known about a wall. + */ +enum class WallState : uint8_t { + UNKNOWN = 0, + NO_WALL = 1, + WALL = 2, +}; + +/** + * @brief Map of the walls of the maze, with the flood fill that guides the search. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + */ +template +class TMaze : public core::ISerializable { +public: + /** + * @brief Cost of a cell that cannot reach any target. + */ + static constexpr uint16_t unreachable{0xFFFF}; + + /** + * @brief Configuration struct for the maze. + * + * @note The goal is borrowed, so it has to outlive the maze, which a constant of the + * configuration does. + */ + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) no defaults, so that a missing field is a warning + struct Config { + GridPose start; + std::span goal; + }; + + /** + * @brief Construct a new TMaze object. + * + * @param config The configuration for the maze. + */ + explicit TMaze(const Config& config); + + /** + * @brief Forget every wall that was observed, keeping what the rules guarantee. + * + * @note The rules give the border, the walls around the start cell and the absence of walls + * between the cells of the goal. + */ + void reset(); + + /** + * @brief Check if a cell is inside the maze. + * + * @param position The position of the cell. + * @return True if the cell exists. + */ + static constexpr bool contains(const GridPoint& position) { return position.x < width and position.y < height; } + + /** + * @brief Get what is known about the wall ahead of a pose. + * + * @param pose The cell and the side of it. + * @return The state of the wall, which is a wall for anything outside of the maze. + */ + WallState get_wall(const GridPose& pose) const; + + /** + * @brief Record the observation of a wall that was unknown. + * + * @note A wall is decided once: an observation of a wall that is already known changes nothing. + * + * @param pose The cell and the side of it. + * @param present Whether there is a wall. + * @return True if the map changed. + */ + bool set_wall(const GridPose& pose, bool present); + + /** + * @brief Check if the robot cannot go through, as far as it is known. + * + * @param pose The cell and the side of it. + * @return True if there is a wall. + */ + bool is_blocked(const GridPose& pose) const; + + /** + * @brief Check if the robot might go through, which is what the search assumes. + * + * @param pose The cell and the side of it. + * @return True unless a wall was seen. + */ + bool is_possibly_open(const GridPose& pose) const; + + /** + * @brief Check if the robot can go through for certain, which is what a fast run requires. + * + * @param pose The cell and the side of it. + * @return True if the absence of a wall was seen. + */ + bool is_known_open(const GridPose& pose) const; + + /** + * @brief Check if a cell belongs to the goal. + * + * @param position The position of the cell. + * @return True if the cell is part of the goal. + */ + bool is_goal(const GridPoint& position) const; + + /** + * @brief Get the pose the robot starts from. + * + * @return The start cell and the direction the robot faces in it. + */ + const GridPose& get_start() const; + + /** + * @brief Get the cells of the goal. + * + * @return The cells of the goal. + */ + std::span get_goal() const; + + /** + * @brief Get a number that changes every time a wall is recorded. + * + * @return The number of changes since the map was last reset. + */ + uint32_t get_revision() const; + + /** + * @brief Compute the number of cells from every cell to the nearest target. + * + * @note Walls that were not seen count as absent. The whole maze is flooded again on every + * call, from a queue with room for every cell, so the cost is bounded and nothing is allocated. + * + * @param targets The cells with a cost of zero. + */ + void flood(std::span targets); + + /** + * @brief Get the number of cells from a cell to the nearest target of the last flood. + * + * @param position The position of the cell. + * @return The cost of the cell, or unreachable. + */ + uint16_t get_cost(const GridPoint& position) const; + + /** + * @brief Get the neighbor to move to in order to approach the targets of the last flood. + * + * @note Going straight is preferred to turning and turning to going back, among the neighbors + * that are equally close. + * + * @param pose The current cell and the direction the robot faces. + * @return The neighbor and the direction to reach it, or nothing if no target can be reached. + */ + std::optional get_next(const GridPose& pose) const; + + /** + * @brief Serialize the walls of the maze. + * + * @return The version, the size of the maze and the state of every wall. + */ + std::vector serialize() const override; + + /** + * @brief Load the walls of the maze. + * + * @note A record of another version or size is ignored. + * + * @param buffer The serialized data. + * @param size The size of the serialized data. + */ + void deserialize(const uint8_t* buffer, uint16_t size) override; + +private: + /** + * @brief Version of the serialized format. + */ + static constexpr uint8_t format_version{1}; + + /** + * @brief Number of bytes of the serialized walls, at two bits for each of two sides of a cell. + */ + static constexpr uint16_t walls_size{(width * height + 1) / 2}; + + /** + * @brief Number of bytes of the header of the serialized format. + */ + static constexpr uint16_t header_size{3}; + + /** + * @brief Write the state of a wall on both of the cells it belongs to. + * + * @param pose The cell and the side of it. + * @param state The state of the wall. + */ + void write_wall(const GridPose& pose, WallState state); + + /** + * @brief State of the walls of every cell, indexed by row, column and side. + */ + std::array, width>, height> walls{}; + + /** + * @brief Number of cells from every cell to the nearest target of the last flood. + */ + std::array, height> costs{}; + + /** + * @brief Queue of the flood fill, with room for every cell. + */ + std::array(width) * height> queue{}; + + /** + * @brief Pose the robot starts from. + */ + GridPose start; + + /** + * @brief Cells of the goal. + */ + std::span goal; + + /** + * @brief Number of walls recorded since the last reset. + */ + uint32_t revision{}; +}; +} // namespace micras::nav + +#include "micras/nav/impl/maze.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_MAZE_HPP diff --git a/micras_nav/include/micras/nav/measurements.hpp b/micras_nav/include/micras/nav/measurements.hpp new file mode 100644 index 0000000..3384300 --- /dev/null +++ b/micras_nav/include/micras/nav/measurements.hpp @@ -0,0 +1,53 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_MEASUREMENTS_HPP +#define MICRAS_NAV_MEASUREMENTS_HPP + +#include + +#include "micras/core/vector.hpp" +#include "micras/nav/robot_model.hpp" + +namespace micras::nav { +/** + * @brief Reading of one wall sensor. + * + * @details The distance is measured along the optical axis, in meters. It is valid when the receiver + * sees anything above its noise, so a reading that is not valid means that nothing is within range. A receiver that + * saturates reports the shortest distance it can measure. + * + * A blind reading is one whose receiver ambient light saturates, which says nothing about a wall. + * + * @note The new flag is set for a single iteration after the sensor produces a value. An estimator + * that is fed the same sample twice counts it as two independent observations and becomes + * overconfident, so that is who needs it. + */ +struct WallReading { + float distance; + bool valid; + bool blind; + bool is_new; +}; + +/** + * @brief Everything the navigation needs from the hardware, sampled once per iteration. + * + * @details The navigation takes plain data instead of holding devices, so it depends on no proxy and + * runs anywhere this struct can be filled: on the robot, in a simulator or in a test. The wheel + * angles are cumulative, in radians, positive when the robot moves forward. The angular rate is the + * raw one around the vertical axis, in rad/s, positive counterclockwise, with the bias still in it. + * The acceleration is in the body frame, x forward and y to the left, in m/s^2. + */ +struct Measurements { + float left_wheel_angle; + float right_wheel_angle; + float angular_rate; + core::Vector acceleration; + bool imu_is_new; + std::array walls; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_MEASUREMENTS_HPP diff --git a/micras_nav/include/micras/nav/mission.hpp b/micras_nav/include/micras/nav/mission.hpp new file mode 100644 index 0000000..e9465cf --- /dev/null +++ b/micras_nav/include/micras/nav/mission.hpp @@ -0,0 +1,503 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_MISSION_HPP +#define MICRAS_NAV_MISSION_HPP + +#include +#include +#include +#include + +#include "micras/core/types.hpp" +#include "micras/nav/executor.hpp" +#include "micras/nav/explorer.hpp" +#include "micras/nav/localizer.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/racing_line.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/wall_model.hpp" +#include "micras/nav/wall_observer.hpp" + +namespace micras::nav { +/** + * @brief Sequencing of a run: what to map, where to go next and which motion gets there. + * + * @details The mission owns the outer loop of the navigation: the wall observer fills the map from + * the readings, the map is flooded towards whatever is worth reaching, and one cell at a time the + * next move is handed to the executor, which turns it into the reference the controller follows. + * + * - Exploring, the robot descends the flood towards the goal and stops at it. + * - Returning, the explorer names the cells still worth visiting and the robot descends the flood + * towards them, until the map proves that the fastest route is known. Then it goes back to the + * start cell, squares up against its back wall and parks where a run starts from. + * - Solving, the route planned beforehand is executed as it is, or the racing line through its cells. + * + * The move through a cell is decided when the robot crosses into it, as late as it can be, from the + * walls that were decided by then. A turn is only made through a wall that was seen absent. A + * straight may start with the wall ahead still unknown, since that wall only comes into range + * inside the cell, but then it is watched: if it turns out to be there, or is still unknown at the + * last point from which the robot can stop at the center of the cell, the robot stops there and + * decides again. A wall seen late costs a stop instead of a crash. Up to that point the straight is + * held to the search speed, which the stop is computed from, and past it the straight is free, + * since by then the wall is either known to be absent or the robot is already stopping. + * + * The mission keeps its own account of which cell the robot is in, from the moves it made. Whenever + * the robot stands still at the center of a cell, that account is checked against the pose estimate, + * and a run in which the two disagree fails instead of going on to write walls in the wrong cells. + * + * @note Every decision takes a bounded time, since it happens while the robot is moving. The search + * of the planner, which does not, is spread over the iterations. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + */ +template +class TMission { +public: + /** + * @brief Progress of a run. + */ + enum class Status : uint8_t { + RUNNING = 0, + FINISHED = 1, + FAILED = 2, + }; + + /** + * @brief Configuration struct for the mission. + * + * @note The start offset is the distance from the back edge of the start cell to the axle of + * the robot when a run starts. The map profiles are the run profiles the map has to be complete + * for, borrowed for the lifetime of the mission. The times are how long to stand still before + * turning in place, at most while squaring up against a wall and between two looks at a wall + * that is still unknown, of which only so many are taken before giving up. The commit margin is + * kept between where the robot could still stop at the center of a cell and where it decides to. + * + * No field has a default value, here or in any other configuration of the navigation, so that + * leaving one out of a configuration is a compiler warning instead of a silent zero. + */ + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) see the note above + struct Config { + TMaze::Config maze; + TPlanner::Config planner; + TWallObserver::Config observer; + TRacingLine::Config racing_line; + Executor::Config executor; + RunProfile search_profile; + std::span map_profiles; + float start_offset; + float stop_time; + float attach_time; + float look_time; + uint16_t max_looks; + float commit_margin; + uint32_t edges_per_iteration; + }; + + /** + * @brief Construct a new TMission object. + * + * @param dynamics The physical limits of the robot, borrowed for the lifetime of the mission. + * @param wall_model The geometry of the wall sensors, borrowed for the same time. + * @param config The configuration for the mission. + */ + TMission(const Dynamics& dynamics, const WallModel& wall_model, const Config& config); + + /** + * @brief Get the map of the walls. + * + * @return The maze. + */ + TMaze& get_maze(); + + /** + * @brief Get the map of the walls. + * + * @return The maze. + */ + const TMaze& get_maze() const; + + /** + * @brief Get the pose a run starts from. + * + * @return The pose in the maze frame. + */ + Pose get_start_pose() const; + + /** + * @brief Start planning the route of a fast run. + * + * @param profile The profile of the run. + */ + void begin_plan(const RunProfile& profile); + + /** + * @brief Advance the planning of the route of a fast run. + * + * @note Only to be called with the robot stopped: when the search ends, the best candidates + * are compiled and timed, which is not bounded to fit in an iteration. When the profile asks + * for the racing line, it is optimized next, through the cells of the route chosen, and it + * replaces the route if it is found and faster. With the risky switch too, the line goes + * through the route planned without the risky turns instead, from a second search: a line + * keeps the margin of the careful turns, which a line through the risky ones never does, and + * it replaces the risky route only if it is faster. + * + * @param max_edges The largest number of edges the planner may try in this call. + * @return True if the planning has finished. + */ + bool update_plan(uint32_t max_edges); + + /** + * @brief Check if a route for a fast run exists. + * + * @return True if the last planning found a route. + */ + bool has_route() const; + + /** + * @brief Get the time the planned route is expected to take. + * + * @return The time from the start until the robot comes to rest inside the goal, in seconds. + */ + float get_route_time() const; + + /** + * @brief Start a run. + * + * @note Exploring and solving start from the start pose. Returning starts from wherever the + * exploration ended, with the robot stopped at the center of a cell. The first exploration + * after a reset forgets the map restored from the flash, which may be of another maze and whose + * walls could never be revised; a fast run after a reset uses it, and so does an exploration + * that is tried again before the next reset. + * + * @param objective What the run is for. + */ + void start(core::Objective objective); + + /** + * @brief Advance the run by one iteration. + * + * @param measurements The current measurements. + * @param localizer The pose estimate, which is corrected while the robot is commanded to rest. + * @param elapsed_time Time since the last iteration, in seconds. + * @param time_scale How much slower the reference has to be played, from the controller. + * @return The progress of the run. + */ + Status update(const Measurements& measurements, Localizer& localizer, float elapsed_time, float time_scale); + + /** + * @brief Get what the robot should be doing at this instant. + * + * @return The reference in the maze frame. + */ + const Reference& get_reference() const; + + /** + * @brief Get the playback of the moves, to see which segment is in progress. + * + * @return The executor. + */ + const Executor& get_executor() const; + + /** + * @brief Get the cell the mission believes the robot is in or entering. + * + * @note Comparing it with the cell of the pose estimate tells whether the two still agree. + * + * @return The cell and the direction of travel. + */ + const GridPose& get_cell() const; + +private: + /** + * @brief What the planning of a fast run is doing. + */ + enum class PlanStage : uint8_t { + IDLE = 0, + SEARCH = 1, + LINE = 2, + }; + + /** + * @brief Largest number of segments a move through one cell takes. + */ + static constexpr uint8_t max_move_segments{6}; + + /** + * @brief Distance within which a segment is taken to start on the part of the watched cell that + * is watched, which is where the cell is entered or the point the straight is split at. + */ + static constexpr float watch_tolerance{0.001F}; + + /** + * @brief Small list of segments, built on the stack for one move. + */ + struct Move { + /** + * @brief Append a segment. + * + * @param segment The segment. + */ + void add(const Segment& segment) { + if (this->size < max_move_segments) { + this->segments.at(this->size++) = segment; + } + } + + std::array segments{}; + uint8_t size{}; + }; + + /** + * @brief Time the candidate routes the planner found and keep the fastest. + * + * @param profile The profile the routes are compiled and timed for. + * @param route Filled with the fastest route. + * @param segments Filled with its segments, and left empty if there is no route. + * @return The time of the fastest route up to the goal line, or infinity. + */ + float choose_route(const RunProfile& profile, Route& route, std::vector& segments); + + /** + * @brief Get the nominal pose of the robot as it crosses into a cell. + * + * @param cell The cell entered and the direction of travel. + * @return The pose at the middle of the edge of the cell, in the maze frame. + */ + Pose get_entry_pose(const GridPose& cell) const; + + /** + * @brief Get the nominal pose of the robot at the center of a cell. + * + * @param cell The cell and the direction the robot faces. + * @return The pose in the maze frame. + */ + Pose get_center_pose(const GridPose& cell) const; + + /** + * @brief Flood the map towards whatever the current objective wants to reach. + */ + void flood(); + + /** + * @brief Plan the speeds of a move and hand it to the executor. + * + * @param move The segments of the move. + * @param start_speed The speed at the start of the move. + * @param end_speed The speed at the end of the move. + */ + void execute(Move& move, float start_speed, float end_speed); + + /** + * @brief Decide the next move, with the robot crossing into a cell at the search speed. + */ + void decide_at_entry(); + + /** + * @brief Decide the next move, with the robot stopped at the center of a cell. + */ + void decide_at_center(); + + /** + * @brief Add to a move the segments that stop the robot at the center of the cell it is entering. + * + * @note When the wall ahead is known to be there, the robot then squares up against it. + * Otherwise it just stands still for a moment. + * + * @param move The move. + */ + void add_stop_at_center(Move& move) const; + + /** + * @brief Check if the run is over now that the robot is entering a cell, and end it if so. + * + * @return True if the last move of the run was handed to the executor. + */ + bool finish_at_entry(); + + /** + * @brief Stop at the center of the cell being crossed instead of going through it. + * + * @param traveled The distance the reference covered since the robot crossed into the cell. + */ + void divert_to_center(float traveled); + + /** + * @brief Get the distance into a cell of the last point from which the robot, crossing it at + * the search speed, can still stop at its center. + * + * @return The distance from the edge of the cell, less the commit margin. + */ + float get_commit_distance() const; + + /** + * @brief Make a segment. + * + * @param kind The kind of the segment. + * @param length The length of the segment, in the unit of its kind. + * @param start The pose the segment nominally starts from. + * @return The segment, with no speeds. + */ + static Segment make_segment(SegmentKind kind, float length, const Pose& start); + + /** + * @brief Physical limits of the robot. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + const Dynamics& dynamics; + + /** + * @brief Geometry of the wall sensors. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + const WallModel& wall_model; + + /** + * @brief Parameters of the mission. + */ + Config config; + + /** + * @brief Map of the walls. + */ + TMaze maze; + + /** + * @brief Decision of which walls exist. + */ + TWallObserver observer; + + /** + * @brief Search for the fastest route. + */ + TPlanner planner; + + /** + * @brief Choice of what is still worth visiting. + */ + TExplorer explorer; + + /** + * @brief Smoothest line through the cells of the route of a fast run. + */ + TRacingLine racing_line; + + /** + * @brief Playback of the moves as a reference. + */ + Executor executor; + + /** + * @brief Speed the robot searches at, which every move through a cell starts and ends with. + */ + float search_speed; + + /** + * @brief What the run in progress is for. + */ + core::Objective objective{core::Objective::EXPLORE}; + + /** + * @brief Progress of the run in progress. + */ + Status status{Status::FINISHED}; + + /** + * @brief Cell the robot is in or entering, and the direction of travel. + */ + GridPose cell{}; + + /** + * @brief Whether the robot is stopped at the center of the cell, rather than crossing into it. + */ + bool at_center{}; + + /** + * @brief Whether the moves handed to the executor are the last ones of the run. + */ + bool finishing{}; + + /** + * @brief Whether the robot is crossing a cell whose wall ahead was unknown when it entered. + */ + bool watching_front{}; + + /** + * @brief Whether an exploration has cleared the map since the last reset. + */ + bool map_cleared{}; + + /** + * @brief Cell being crossed while the wall ahead of it is watched. + */ + GridPose watched_cell{}; + + /** + * @brief Number of times in a row the robot stood still waiting for a wall to be decided. + */ + uint16_t looks{}; + + /** + * @brief Profile of the fast run that was planned. + */ + RunProfile solve_profile{}; + + /** + * @brief Segments of the fast run that was planned. + */ + std::vector solve_segments; + + /** + * @brief Segments of a candidate route being timed. + */ + std::vector candidate_segments; + + /** + * @brief Candidate route being compiled, kept to reuse its storage. + */ + Route candidate_route; + + /** + * @brief Route of the fast run that was planned, which the racing line goes through. + */ + Route solve_route; + + /** + * @brief Route planned without the risky turns, and its segments, for the racing line of a run + * with the risky switch. + */ + ///@{ + Route careful_route; + std::vector careful_segments; + ///@} + + /** + * @brief Time the planned fast run is expected to take. + */ + float route_time{}; + + /** + * @brief What the planning of a fast run is doing. + */ + PlanStage stage{PlanStage::IDLE}; + + /** + * @brief Whether the search running is the one for the route without the risky turns. + */ + bool careful_search{}; + + /** + * @brief Last reference produced. + */ + Reference reference{}; +}; +} // namespace micras::nav + +#include "micras/nav/impl/mission.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_MISSION_HPP diff --git a/micras_nav/include/micras/nav/motion_limits.hpp b/micras_nav/include/micras/nav/motion_limits.hpp new file mode 100644 index 0000000..2f25bab --- /dev/null +++ b/micras_nav/include/micras/nav/motion_limits.hpp @@ -0,0 +1,257 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_MOTION_LIMITS_HPP +#define MICRAS_NAV_MOTION_LIMITS_HPP + +#include "micras/nav/robot_model.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::nav { +/** + * @brief How a run trades time for safety. + * + * @note The utilization is the fraction of the available traction the run may ask for, in (0, 1]. + * The fan adds its downforce to the traction. Whether the run is risky selects the turn table built + * with the smaller safety margin. The racing line replaces the route by the smoothest line through + * its cells, which only a fast run planned with the robot stopped can drive. The speed limit is what + * makes the search run slow: a fast run leaves it above what the robot can reach. + */ +struct RunProfile { + bool racing_line; + bool fan; + bool risky; + float utilization; + float max_speed; +}; + +/** + * @brief Limits of a motion along one axis, linear or angular. + * + * @details The acceleration is bounded by the tires up to the speed where the motors take over, and + * from there it falls linearly with the speed, reaching zero at the free running speed, which is + * how a DC motor fed from a fixed voltage behaves. The back EMF helps braking instead, so the + * braking the motors can give grows with the speed, from what they give accelerating from rest. + * Braking is bounded by the tires down to the speed where that falls below them, and by the motors + * under it, which only happens when the motors cannot accelerate the robot from rest as hard as the + * tires allow. + */ +struct MotionLimits { + /** + * @brief Get the largest acceleration available at a speed. + * + * @param speed The current speed. + * @return The acceleration limit at that speed. + */ + float acceleration_at(float speed) const; + + /** + * @brief Get the speed from which the motors, rather than the tires, limit the acceleration. + * + * @return The crossover speed, which is negative if the motors limit it from rest. + */ + float crossover_speed() const; + + /** + * @brief Get the largest deceleration available at a speed. + * + * @param speed The current speed. + * @return The deceleration limit at that speed, as a positive number. + */ + float deceleration_at(float speed) const; + + /** + * @brief Get the speed under which the motors, rather than the tires, limit the braking. + * + * @return The braking crossover speed, which is negative if the motors never limit it. + */ + float braking_crossover_speed() const; + + /** + * @brief Get the same limits with a lower top speed. + * + * @param speed The top speed, which is ignored where it is above the current one. + * @return The limits with the lower of the two top speeds. + */ + MotionLimits capped(float speed) const; + + float max_speed; + float acceleration; + float deceleration; + float motor_acceleration; + float motor_speed; +}; + +/** + * @brief Limits of a motion along a curve, where the tires share their grip between turning and + * changing speed. + * + * @details The speed at a point is limited by the lateral acceleration its curvature asks for and + * by the angular acceleration its sharpness asks for, each against what the tires can give. What is + * left of the grip there can change the speed: the lateral part takes its share as on a friction + * circle, and the angular part, which the two tires make by pushing in opposite directions, takes + * it from the push each tire has left. A change of speed on a curve also changes the angular speed, + * and that angular acceleration is counted too. The motors bound the change of speed along the path + * as they do on a straight. + * + * @note On a straight nothing is taken and the limits are those of the linear motion. + */ +struct CurveLimits { + /** + * @brief Get the largest speed at a point. + * + * @param bending How the path bends there. + * @return The speed in m/s. + */ + float get_speed_limit(const Bending& bending) const; + + /** + * @brief Get the largest acceleration at a point, at a speed. + * + * @param speed The speed there. + * @param bending How the path bends there. + * @return The acceleration along the path in m/s^2, which is zero where the grip is all used. + */ + float get_acceleration(float speed, const Bending& bending) const; + + /** + * @brief Get the largest deceleration at a point, at a speed. + * + * @param speed The speed there. + * @param bending How the path bends there. + * @return The deceleration along the path in m/s^2, as a positive number. + */ + float get_deceleration(float speed, const Bending& bending) const; + + /** + * @brief Get the share of the grip left for a change of speed at a point, at a speed. + * + * @param speed The speed there. + * @param bending How the path bends there. + * @return The share, which is zero or less where the grip is all used. + */ + float get_spare(float speed, const Bending& bending) const; + + MotionLimits linear; + float lateral; + float angular; +}; + +/** + * @brief Derivation of the limits of a run from the physical model of the robot. + */ +class Dynamics { +public: + /** + * @brief Configuration struct for the dynamics. + * + * @note The speed limits are not physical: they bound what the sensing and the control have + * been proven at, and only matter where the physics would allow more. The voltage reserve is + * the fraction of the motor supply kept for the feedback and for steering, which the planned + * accelerations may not use. + */ + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) no defaults, so that a missing field is a warning + struct Config { + RobotModel model; + TurnTable turns; + TurnTable risky_turns; + float max_linear_speed; + float max_angular_speed; + float voltage_reserve; + }; + + /** + * @brief Construct a new Dynamics object. + * + * @param config The configuration for the dynamics. + */ + explicit Dynamics(const Config& config); + + /** + * @brief Get the limits of the motion along a straight. + * + * @param profile The profile of the run. + * @return The linear limits in m/s and m/s^2. + */ + MotionLimits get_linear_limits(const RunProfile& profile) const; + + /** + * @brief Get the limits of a rotation in place. + * + * @param profile The profile of the run. + * @return The angular limits in rad/s and rad/s^2. + */ + MotionLimits get_angular_limits(const RunProfile& profile) const; + + /** + * @brief Get the limits of a motion along a curve. + * + * @param profile The profile of the run. + * @return The limits, the linear ones being those of a straight. + */ + CurveLimits get_curve_limits(const RunProfile& profile) const; + + /** + * @brief Get the shape of a turn for a run. + * + * @param profile The profile of the run. + * @param turn The turn. + * @return The shape of the turn. + */ + const TurnShape& get_turn(const RunProfile& profile, TurnId turn) const; + + /** + * @brief Get the speed a turn is priced at. + * + * @details The largest speed at which neither the lateral acceleration at the peak curvature + * nor the angular acceleration along the ramps exceeds what the tires can give to this run. It + * is the slowest point of the turn, which the robot may drive faster where the turn allows. + * + * @param profile The profile of the run. + * @param turn The turn. + * @return The linear speed along the turn in m/s. + */ + float get_turn_speed(const RunProfile& profile, TurnId turn) const; + + /** + * @brief Get the physical model of the robot. + * + * @return The model. + */ + const RobotModel& get_model() const; + +private: + /** + * @brief Physical description of the robot. + */ + RobotModel model; + + /** + * @brief Shape of the turns with the normal safety margin. + */ + TurnTable turns; + + /** + * @brief Shape of the turns with the reduced safety margin. + */ + TurnTable risky_turns; + + /** + * @brief Largest linear speed planned, in m/s. + */ + float max_linear_speed; + + /** + * @brief Largest angular speed planned for a rotation in place, in rad/s. + */ + float max_angular_speed; + + /** + * @brief Voltage the planned motions may count on. + */ + float available_voltage; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_MOTION_LIMITS_HPP diff --git a/micras_nav/include/micras/nav/planner.hpp b/micras_nav/include/micras/nav/planner.hpp new file mode 100644 index 0000000..1c58297 --- /dev/null +++ b/micras_nav/include/micras/nav/planner.hpp @@ -0,0 +1,742 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_PLANNER_HPP +#define MICRAS_NAV_PLANNER_HPP + +#include +#include +#include +#include +#include + +#include "micras/nav/lattice.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/motion_limits.hpp" + +namespace micras::nav { +/** + * @brief What the planner assumes about the walls that were never observed. + */ +enum class WallAssumption : uint8_t { + OPTIMISTIC = 0, // An unknown wall is absent: the fastest route that could still exist. + PESSIMISTIC = 1, // An unknown wall is present: the fastest route that is known to exist. +}; + +/** + * @brief One move of a route: a number of steps straight ahead, followed by a turn. + * + * @note The last move of a route may have no turn, when the goal is entered on a straight. + */ +struct RouteStep { + uint8_t run; + bool has_turn; + TurnId turn; + TurnSide side; +}; + +/** + * @brief A route from the start to the goal, as the moves that drive it. + * + * @note The stop distance is measured from the node where the last move ends to the place inside the + * goal where the robot comes to rest, and the finish distance from the line where the robot enters + * the goal, which is where the clock of a run stops, to that same place. The time is the one the + * search found for reaching that line. + */ +struct Route { + LatticePose start{}; + std::vector steps; + float stop_distance{}; + float finish_distance{}; + float time{std::numeric_limits::infinity()}; +}; + +/** + * @brief Search for the fastest route from the start to the goal. + * + * @details The search is a Dijkstra over the lattice of lattice.hpp. An edge is a motion the robot + * actually executes, a run of steps ahead followed by a turn, and it exists only if every wall it + * crosses passes the wall assumption. Its cost is the time of the run, from the evaluator the + * executor plays back, plus the time of the turn at the speed the run profile gives it, so the + * fastest path of the graph is the fastest route the robot can drive, to within what the velocity + * planner refines among the candidates. A turn the run cannot reach its speed for is priced at a + * blend of the two speeds, since the robot accelerates inside it, and a turn that has to leave + * slower for the next one costs nothing more, since the robot slows in its exit ramp. Over + * thousands of mazes the price of a route differs from what the velocity planner makes of it by + * 20 ms at one standard deviation, and by a few tens of milliseconds at worst. Diagonals, the turns + * of 135 degrees and the two sizes of the turn of 90 degrees are part of the search rather than a + * rewrite of its result. + * + * What the search holds are labels: a way of reaching a node, with its cost, the turn that reached + * it and how fast that turn is driven, since those fix the speed the next straight starts at and + * how much of it the turn already covered. A label is dropped only when another label of the same + * node is at least as fast for every continuation of the route, which is what keeps the search + * exact with far fewer labels than there are ways of reaching a node. + * + * The clock of a run stops when the robot enters the goal, so a route is priced up to that line, + * while still being required to come to rest inside the goal. + * + * @note The labels are members, which makes an object of this class large: it belongs in static + * storage, not on the stack. The search is advanced a bounded number of edges at a time, which is + * how it runs while the robot is moving: an edge costs about the same whatever the turns, while a + * node costs as many edges as there are turns to try from it. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + */ +template +class TPlanner { +public: + /** + * @brief Number of finished routes kept, for the caller to time exactly and pick from. + * + * @note The cost of an edge cannot see two turns so close together that one of them has to be + * driven slower, which the velocity planner can. Timing the few best candidates with it closes + * that gap. + */ + static constexpr uint8_t number_of_candidates{4}; + + /** + * @brief Largest number of labels held at once. + * + * @note Sized with a margin over the largest search seen on contest mazes, whole and with part + * of their walls unknown. A search that needs more drops the costliest labels waiting to be + * expanded, and reports that it is no longer exact. + */ + static constexpr uint16_t max_labels{6144}; + + /** + * @brief Configuration struct for the planner. + * + * @note The start distance is how far the robot is from the first wall it crosses when a run + * begins. + */ + struct Config { + float start_distance; + }; + + /** + * @brief Construct a new TPlanner object. + * + * @param dynamics The physical limits of the robot, borrowed for the lifetime of the planner. + * @param config The configuration for the planner. + */ + TPlanner(const Dynamics& dynamics, const Config& config); + + /** + * @brief Start a search. + * + * @param maze The map of the walls, borrowed until the search ends. + * @param assumption What to assume about the walls that were never observed. + * @param profile The profile of the run the route is for. + */ + void begin(const TMaze& maze, WallAssumption assumption, const RunProfile& profile); + + /** + * @brief Advance the search. + * + * @note Before the search itself, every pair of arrivals is compared for the dominance, one run + * of one pair at a time, each charged to the budget for the edges it times that were not + * timed before, so that no call takes longer than the budget allows. + * + * @param max_edges The largest number of edges to try in this call. + * @return True if the search has finished. + */ + bool step(uint32_t max_edges); + + /** + * @brief Check if the search has finished. + * + * @return True if there is nothing left to expand. + */ + bool is_finished() const; + + /** + * @brief Check if the search kept every label it needed. + * + * @return False if it ran out of labels, in which case the routes found may not be the fastest. + */ + bool is_exact() const; + + /** + * @brief Get the largest number of labels the search held at once. + * + * @return The number of labels, at most max_labels. + */ + uint16_t get_peak_labels() const; + + /** + * @brief Get the number of routes found. + * + * @return The number of candidates, which is zero if the goal cannot be reached. + */ + uint8_t get_number_of_routes() const; + + /** + * @brief Get one of the routes found. + * + * @param index The index of the candidate, the first being the fastest for the search. + * @param route The route to fill in, whose storage is reused. + */ + void get_route(uint8_t index, Route& route) const; + + /** + * @brief Call a function for every wall a route crosses. + * + * @tparam F Type of the function, taking the cell and the side of the wall as a GridPose. + * @param route The route. + * @param function The function to call. + */ + template + static void for_each_wall(const Route& route, F&& function); + +private: + /** + * @brief Way of reaching a node. + * + * @note The arrival is the turn that reached the node plus one, zero being the start, and the + * run and the side are those of the edge that ended with that turn. The position is the place + * of the label in the queue, and is not_queued once it has been expanded. + */ + struct Label { + float cost; + uint16_t node; + uint16_t parent; + uint16_t next; + uint16_t position; + uint8_t arrival; + uint8_t speed_ratio; + uint8_t run; + TurnSide side; + }; + + /** + * @brief End of a route inside the goal. + */ + struct Terminal { + float cost; + uint16_t label; + uint8_t run; + bool has_turn; + TurnId turn; + TurnSide side; + float stop_distance; + float finish_distance; + }; + + /** + * @brief Cost of an edge, and how fast its turn is driven. + * + * @note The speed is a fraction of the speed the run profile gives the turn, in steps of 1/255, + * and is lower than that only when the run before the turn is too short to reach it. + */ + struct Edge { + float cost; + uint8_t speed_ratio; + }; + + /** + * @brief Arrival of the label of the start, which is at rest. + */ + static constexpr uint8_t rest{0}; + + /** + * @brief Number of ways a node can be reached: at rest, or by one of the turns. + */ + static constexpr uint8_t number_of_arrivals{number_of_turns + 1}; + + /** + * @brief Speed ratio of a turn driven at the speed the run profile gives it. + */ + static constexpr uint8_t full_speed{255}; + + /** + * @brief Share of the way from the speed a turn is entered at to its own speed that the time of + * the turn is priced at. + */ + static constexpr float turn_blend{0.7F}; + + /** + * @brief Number of headings a wall can be crossed with: two along the grid and four diagonal. + */ + static constexpr uint8_t headings_per_wall{6}; + + /** + * @brief Number of walls of the maze, including the ones of the border. + */ + static constexpr uint16_t number_of_walls{(width + 1) * height + width * (height + 1)}; + + /** + * @brief Number of nodes of the lattice. + */ + static constexpr uint16_t number_of_nodes{number_of_walls * headings_per_wall}; + + /** + * @brief Largest number of steps of a run, along a diagonal, where a step is half a cell. + */ + static constexpr uint8_t max_run{2 * (width > height ? width : height)}; + + /** + * @brief Largest number of steps of a run along the grid, where a step is a whole cell. + */ + static constexpr uint8_t max_straight_run{width > height ? width : height}; + + /** + * @brief Marker of the end of a list of labels, and of a label that is not in the queue. + */ + static constexpr uint16_t none{0xFFFF}; + + /** + * @brief Pairs of an arrival and a turn that can follow it, which are the edges worth caching. + * + * @details A turn follows an arrival when it starts on the kind of heading, diagonal or along + * the grid, that the arrival ends on. The index of the pair is its row in the cache, or -1. + */ + struct EdgePairs { + /** + * @brief Number the pairs of arrivals and turns. + */ + constexpr EdgePairs() { + for (uint8_t arrival = 0; arrival < number_of_arrivals; arrival++) { + for (uint8_t turn = 0; turn < number_of_turns; turn++) { + const bool follows = + get_primitive(static_cast(turn)).diagonal_entry == ends_diagonal(arrival); + + this->index.at(arrival).at(turn) = follows ? this->count : -1; + this->count += follows ? 1 : 0; + } + } + } + + std::array, number_of_arrivals> index{}; + int16_t count{}; + }; + + /** + * @brief Index of the cached edge of every pair of an arrival and a turn. + */ + static constexpr EdgePairs edge_pairs{}; + + /** + * @brief Check if an arrival ends on a diagonal heading. + * + * @param arrival The arrival. + * @return True if the turn of the arrival ends on a diagonal. + */ + static constexpr bool ends_diagonal(uint8_t arrival); + + /** + * @brief Get the turn that produced an arrival. + * + * @param arrival The arrival, which must not be the start. + * @return The turn. + */ + static constexpr TurnId to_turn(uint8_t arrival); + + /** + * @brief Check if a node is inside the maze. + * + * @param node The node. + * @return True if the node is the midpoint of a wall of the maze. + */ + static constexpr bool is_inside(const LatticePose& node); + + /** + * @brief Get the index of a node. + * + * @param node The node, which must be valid and inside the maze. + * @return The index of the node, below number_of_nodes. + */ + static constexpr uint16_t encode(const LatticePose& node); + + /** + * @brief Get the node behind an index. + * + * @param index The index of the node. + * @return The node. + */ + static constexpr LatticePose decode(uint16_t index); + + /** + * @brief Check if a turn can be driven on the run being planned. + * + * @param turn The turn. + * @return True if the turn fits between its nodes with the margin of the run profile. + */ + bool is_usable(TurnId turn) const; + + /** + * @brief Check if the robot may cross the wall a node is on. + * + * @param node The node. + * @return True if the wall passes the wall assumption. + */ + bool is_traversable(const LatticePose& node) const; + + /** + * @brief Check if a cell of the lattice belongs to the goal. + * + * @param cell The coordinates of the cell, which may be outside of the maze. + * @return True if the cell is part of the goal. + */ + bool is_goal(const LatticePoint& cell) const; + + /** + * @brief Get how far past a node the robot can stop, when it enters the goal there. + * + * @param node The node where the goal is entered, heading along the grid. + * @return The distance from the node to the center of the last cell of the goal ahead. + */ + float get_stop_distance(const LatticePose& node) const; + + /** + * @brief Get the speed a label leaves its node with. + * + * @param arrival The arrival of the label. + * @param speed_ratio How fast the turn of the arrival is driven. + * @return The speed in m/s. + */ + float get_speed(uint8_t arrival, uint8_t speed_ratio) const; + + /** + * @brief Get the time a part of a turn takes, entered at a speed. + * + * @note Entered below its speed, a turn is not driven at the speed it was entered at: the + * robot accelerates inside it, where its curvature leaves grip to spare. The time is the length + * at a speed that blends the two, by the turn blend, which matches the motion the executor plays + * within a few milliseconds per turn over thousands of mazes, whatever the traction. + * + * @param turn The turn. + * @param length The length of the part of the turn, in meters. + * @param speed The speed the turn is entered at, in m/s. + * @return The time in seconds. + */ + float get_turn_time(TurnId turn, float length, float speed) const; + + /** + * @brief Get the straight left between the curve that reached a label and its node. + * + * @param arrival The arrival of the label. + * @return The distance in meters. + */ + float get_offset(uint8_t arrival) const; + + /** + * @brief Get the time of a run followed by a turn. + * + * @details When the run is too short to change from the speed of one turn to the speed of the + * other, one of the turns has to be driven slower, which is what the velocity planner will do + * to it. If it is the turn ahead, it is priced at the speed the run allows and the label it leads + * to remembers that speed. If it is the turn behind, which was already priced, the time it + * loses is added to this edge. + * + * @param arrival The arrival of the label the run starts from. + * @param speed_ratio How fast the turn of that arrival is driven. + * @param run The number of steps of the run. + * @param turn The turn after the run. + * @return The time from the end of the previous curve to the end of this one, and the speed of + * this one. + */ + Edge get_edge(uint8_t arrival, uint8_t speed_ratio, uint8_t run, TurnId turn); + + /** + * @brief Check if two arrivals can meet at a node and have to be compared. + * + * @param first The first arrival. + * @param second The second arrival. + * @return True if they are different, end on the same kind of heading and can both be driven. + */ + bool is_comparable(uint8_t first, uint8_t second) const; + + /** + * @brief Compare the next pair of arrivals over the next run, moving on to the next pair after. + * + * @return The number of edges the comparison timed, counting each way into the goal as one, + * at least one, or zero once every pair was compared. + */ + uint32_t prepare(); + + /** + * @brief Bound how much slower one arrival at full speed can be than another, over one run. + * + * @details Whatever follows a node starts with the same edge from both arrivals. The two routes + * differ by the time of that first edge, not counting its turn, and by how fast that turn is + * driven, after which the arrival at the faster turn can only be faster. So when the first + * arrival never leaves a turn slower than the second, the most its first edge, or its way into + * the goal, takes longer is the most any route from it can take longer. The bound is the largest + * over every run, and is infinite if the first arrival leaves any turn slower. + * + * @param first The arrival that would be kept. + * @param second The arrival that would be dropped. + * @param run The number of steps of the run before the first edge. + */ + void compare_arrivals(uint8_t first, uint8_t second, uint8_t run); + + /** + * @brief Check if a label is at least as fast as another of the same node, for every route. + * + * @note A label is compared with one of the same arrival by the time before its turn and the + * speed of its turn, and with one of another arrival only when the first turn is driven at full + * speed. A second turn driven slower is compared as if it were driven at full speed, less the + * time it loses, since a turn driven slower never leaves a route faster. + * + * @param first The label that would be kept. + * @param second The label that would be dropped. + * @return True if the second label can be dropped. + */ + bool dominates(const Label& first, const Label& second); + + /** + * @brief Advance the expansion of a label by one edge. + * + * @note The run is lengthened when every turn at its end has been tried, and the expansion ends + * at a wall that may not be crossed, at the goal or after the longest run. + */ + void advance_expansion(); + + /** + * @brief Relax the edges of one turn, to both sides, from the end of a run. + * + * @param index The index of the label the run starts from. + * @param entry The node where the run ends and the turn starts. + * @param run The number of steps of the run. + * @param turn The turn. + */ + void relax_turn(uint16_t index, const LatticePose& entry, uint8_t run, TurnId turn); + + /** + * @brief Record a route that ends inside the goal, if it is among the best. + * + * @param terminal The end of the route. + */ + void add_terminal(const Terminal& terminal); + + /** + * @brief Add a label to its node, unless another one there is at least as fast. + * + * @note The labels of the node that the new one is at least as fast as are dropped, unless they + * have been expanded, since other labels may lead back to them. + * + * @param label The label, with every field but the links filled in. + */ + void insert(Label label); + + /** + * @brief Take a label from the pool, making room if it is full. + * + * @param cost The cost of the label that needs the room. + * @return The index of the label, or none if every label held is cheaper than the cost. + */ + uint16_t allocate(float cost); + + /** + * @brief Remove a label that waits in the queue from its node and give it back to the pool. + * + * @param index The index of the label. + */ + void release(uint16_t index); + + /** + * @brief Add a label to the queue. + * + * @param index The index of the label. + */ + void push(uint16_t index); + + /** + * @brief Remove the label with the lowest cost from the queue. + * + * @return The index of the label. + */ + uint16_t pop(); + + /** + * @brief Remove a label from the queue, wherever it is. + * + * @param position The position of the label in the queue. + */ + void remove(uint16_t position); + + /** + * @brief Move an entry of the queue up to its place. + * + * @param position The position of the entry in the queue. + */ + void sift_up(uint16_t position); + + /** + * @brief Move an entry of the queue down to its place. + * + * @param position The position of the entry in the queue. + */ + void sift_down(uint16_t position); + + /** + * @brief Physical limits of the robot. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + const Dynamics& dynamics; + + /** + * @brief Distance from the start pose to the first node. + */ + float start_distance; + + /** + * @brief Map of the walls being searched. + */ + const TMaze* maze{nullptr}; + + /** + * @brief Assumption about the unknown walls. + */ + WallAssumption assumption{WallAssumption::PESSIMISTIC}; + + /** + * @brief Profile of the run being planned. + */ + RunProfile run_profile{}; + + /** + * @brief Limits of the motion along a straight, for the run being planned. + */ + MotionLimits limits{}; + + /** + * @brief Speed of each turn, for the run being planned. + */ + std::array turn_speeds{}; + + /** + * @brief Turns that can be driven on the run being planned, from a heading along the grid and + * from a diagonal one. + */ + std::array, 2> usable_turns{}; + + /** + * @brief Number of usable turns from a heading along the grid and from a diagonal one. + */ + std::array number_of_usable_turns{}; + + /** + * @brief Largest number of cells of the goal in a line, which bounds where the robot stops. + */ + uint8_t goal_length{}; + + /** + * @brief Number of edges timed rather than read from the cache, of which only differences + * count. + */ + uint32_t computed_edges{}; + + /** + * @brief Edge of every pair of an arrival at full speed and a turn, for every run, filled in when + * first needed. + */ + std::array, edge_pairs.count> edge_costs{}; + + /** + * @brief Speed ratio of the turn of every cached edge. + */ + std::array, edge_pairs.count> edge_speed_ratios{}; + + /** + * @brief Dominance gap of every pair of arrivals, computed before the search. + */ + std::array, number_of_arrivals> gaps{}; + + /** + * @brief First label of each node. + */ + std::array heads{}; + + /** + * @brief Labels of the search. + */ + std::array labels{}; + + /** + * @brief Binary heap of the labels waiting to be expanded, ordered by cost. + */ + std::array queue{}; + + /** + * @brief Number of labels in the queue. + */ + uint16_t queue_size{}; + + /** + * @brief Number of labels ever taken from the pool in this search. + */ + uint16_t allocated{}; + + /** + * @brief First label given back to the pool, the others following it. + */ + uint16_t free_list{none}; + + /** + * @brief Number of labels held. + */ + uint16_t held{}; + + /** + * @brief Largest number of labels held at once. + */ + uint16_t peak{}; + + /** + * @brief Whether every label the search needed was kept. + */ + bool exact{true}; + + /** + * @brief Whether the pairs of arrivals are still being compared. + */ + bool preparing{}; + + /** + * @brief Pair of arrivals and run the comparison is at. + */ + ///@{ + uint8_t prepare_first{}; + uint8_t prepare_second{}; + uint8_t prepare_run{}; + ///@} + + /** + * @brief Label being expanded, or none. + */ + uint16_t expanding{none}; + + /** + * @brief Node at the end of the run of the expansion, the number of steps of that run and the + * next turn to try there. + */ + ///@{ + LatticePose expansion_entry{}; + uint8_t expansion_run{}; + uint8_t expansion_turn{}; + ///@} + + /** + * @brief Best ends of route found, ordered by cost. + */ + std::array terminals{}; + + /** + * @brief Number of ends of route found. + */ + uint8_t number_of_terminals{}; + + /** + * @brief Start node of the search. + */ + LatticePose start{}; +}; +} // namespace micras::nav + +#include "micras/nav/impl/planner.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_PLANNER_HPP diff --git a/micras_nav/include/micras/nav/racing_line.hpp b/micras_nav/include/micras/nav/racing_line.hpp new file mode 100644 index 0000000..e913c18 --- /dev/null +++ b/micras_nav/include/micras/nav/racing_line.hpp @@ -0,0 +1,447 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_RACING_LINE_HPP +#define MICRAS_NAV_RACING_LINE_HPP + +#include +#include +#include +#include + +#include "micras/core/vector.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/line.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/segment.hpp" + +namespace micras::nav { +/** + * @brief The smoothest line through the cells of a planned route, and the speeds to drive it at. + * + * @details The route is sampled every few millimeters, and each sample may then slide sideways, + * along the normal of the line, as far as the outline of the robot, turned along the line, keeps + * the margin to every post and to every wall the route does not cross. The walls the route does not + * cross are taken as present, whatever the map says, so the line never leaves the cells of the + * route. Among the slides allowed, the ones chosen make the sum of the squared second differences + * of the samples smallest, which is the sum of the squared curvatures, plus a small weight on the + * squared first differences, which prefers the shorter line. + * + * That is a least squares problem with bounds on each unknown, where each unknown only meets its two + * neighbors on each side. It is solved in windows of a few tens of samples, the samples around a + * window held where they are, by a projected Newton method on the banded normal equations. The + * windows overlap and sweep the line from its start to its end, and the line is resampled at equal + * steps after each sweep. A slide is limited per sweep, and the bounds are found again for every + * window, since the robot needs more room as the line turns it. After the last sweep every sample + * is checked, the curvature is measured and averaged over three samples, and the speeds are planned + * along it with the same rule as a turn. + * + * Nothing here is bounded to one iteration of the loop as a whole. Sampling the route, finding the + * bounds and checking the line cost the most per sample, and advance by a budget of samples per + * call. Solving a window, resampling the line and planning the speeds take one call each, of a few + * milliseconds at most, the robot being stopped. + * + * @note The first and last samples never move: the line starts where the robot stands and ends + * where the route comes to rest. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + */ +template +class TRacingLine { +public: + /** + * @brief Configuration struct for the racing line. + * + * @note The margin is the distance the line keeps between the outline of the robot and any + * obstacle wherever the route leaves room for it, with the risky switch too. The least margin + * is what the final check requires everywhere, whatever the switch: the margin the turns were + * designed with, less what the resampling and the headings of the final line use up. Where the + * route itself comes closer than the margin, which the risky turns do, the line does not move, + * and so it fails the check there, since a line that close to the walls grazed them. The trust + * is how far a sample may slide in one sweep, the scan step the resolution of the bounds, and + * the length weight the weight of the first differences, in units of the spacing. A sweep that + * slides no sample farther than the convergence tolerance ends the optimization. The lateral + * share is the part of the lateral grip of the run the curves of the line are planned with: the + * tires slide sideways in proportion to what they are asked for, which the pose estimate only + * predicts, and the line holds its curves far longer than a turn does. + */ + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) no defaults, so that a missing field is a warning + struct Config { + float spacing; + float margin; + float least_margin; + float trust; + float scan_step; + float length_weight; + float convergence; + float lateral_share; + uint8_t max_sweeps; + uint16_t samples_per_step; + }; + + /** + * @brief Construct a new TRacingLine object. + * + * @param dynamics The physical limits of the robot, borrowed for the lifetime of the object. + * @param config The configuration for the racing line. + */ + TRacingLine(const Dynamics& dynamics, const Config& config); + + /** + * @brief Forget the line. + */ + void reset(); + + /** + * @brief Start optimizing the line of a route. + * + * @param route The route on the lattice, which tells the walls it crosses. + * @param segments The segments the route compiles to, borrowed until the optimization ends. + * @param maze The map of the walls, borrowed until the optimization ends. + * @param profile The profile of the run. + */ + void begin( + const Route& route, std::span segments, const TMaze& maze, + const RunProfile& profile + ); + + /** + * @brief Advance the optimization. + * + * @return True if it has finished, whether a line was found or not. + */ + bool step(); + + /** + * @brief Get the line. + * + * @return The line, which is ready only if the last optimization found one. + */ + const Line& get_line() const; + + /** + * @brief Get the number of sweeps the last optimization made. + * + * @return The number of sweeps. + */ + uint8_t get_sweeps() const; + +private: + /** + * @brief Largest number of samples solved for at once. + */ + static constexpr uint8_t window{64}; + + /** + * @brief Number of samples a window starts after the start of the one before. + */ + static constexpr uint8_t stride{48}; + + /** + * @brief Number of samples at each end of the line that never move. + */ + static constexpr uint8_t fixed_samples{3}; + + /** + * @brief Largest number of points along the outline of the robot that are checked. + */ + static constexpr uint8_t max_outline_points{32}; + + /** + * @brief Largest distance between two points of the outline that are checked, in meters. + * + * @note Below the thickness of a wall plus twice the smallest margin, so that no wall can pass + * between two points unseen. + */ + static constexpr float outline_spacing{0.02F}; + + /** + * @brief Largest number of iterations of the projected Newton method in a window. + */ + static constexpr uint8_t max_newton_iterations{30}; + + /** + * @brief Largest number of times a window that brings the robot too close is shrunk. + */ + static constexpr uint8_t max_shrinks{8}; + + /** + * @brief Largest number of steps of distance scanned on each side of a sample. + */ + static constexpr uint8_t max_scan_steps{32}; + + /** + * @brief Number of lattice points along each axis, a lattice unit being half a cell. + */ + ///@{ + static constexpr uint16_t lattice_width{2 * width + 1}; + static constexpr uint16_t lattice_height{2 * height + 1}; + ///@} + + /** + * @brief Phase of the optimization. + */ + enum class Phase : uint8_t { + IDLE = 0, + SAMPLE = 1, + BOUNDS = 2, + SOLVE = 3, + RESAMPLE = 4, + CHECK = 5, + SPEED = 6, + }; + + /** + * @brief Sample the compiled route at the next samples of the budget. + * + * @param budget The number of samples left in this call, which is decreased. + */ + void sample_route(uint32_t& budget); + + /** + * @brief Find the bounds of the next samples of the window. + * + * @param budget The number of samples left in this call, which is decreased. + */ + void find_bounds(uint32_t& budget); + + /** + * @brief Solve the window, move its samples and go on to the next window. + */ + void solve_window(); + + /** + * @brief Resample the line at equal steps, then start the next sweep or the checks. + * + * @note The new samples are written to the curvatures and the speeds of the line, which are + * not in use until the speeds are planned, and copied back once they are all known. + */ + void resample(); + + /** + * @brief Check the clearance of the next samples of the line. + * + * @param budget The number of samples left in this call, which is decreased. + */ + void check(uint32_t& budget); + + /** + * @brief Measure the curvature of the line, from the turn of its heading between samples, + * averaged over each sample and its two neighbors. + */ + void measure_curvature(); + + /** + * @brief Measure the curvature, plan the speeds along the line, time it and mark it ready. + */ + void plan_speeds(); + + /** + * @brief Start a sweep from the first window. + */ + void start_sweep(); + + /** + * @brief Start the window at the current window start. + */ + void start_window(); + + /** + * @brief End the optimization. + * + * @param found Whether a line was found. + */ + void finish(bool found); + + /** + * @brief Get the heading of the line at a sample, from the samples around it. + * + * @param index The index of the sample. + * @return The heading in radians. + */ + float get_heading(uint16_t index) const; + + /** + * @brief Solve for the slides of the window within their bounds. + * + * @param normals_x The x component of the normal of each sample of the window. + * @param normals_y The y component of the normal of each sample of the window. + */ + void solve_slides(std::span normals_x, std::span normals_y); + + /** + * @brief Check if the robot keeps a margin to every obstacle at a pose. + * + * @param position The position of the robot. + * @param cosine The cosine of its heading. + * @param sine The sine of its heading. + * @param margin The distance to keep. + * @return True if nothing is closer than the margin. + */ + bool is_clear(const core::Vector& position, float cosine, float sine, float margin) const; + + /** + * @brief Check if the robot comes within a margin of a rectangle aligned with the grid. + * + * @param position The position of the robot. + * @param cosine The cosine of its heading. + * @param sine The sine of its heading. + * @param center The center of the rectangle. + * @param half_size The half width and half height of the rectangle. + * @param margin The distance to keep. + * @return True if the robot is closer than the margin. + */ + bool hits( + const core::Vector& position, float cosine, float sine, const core::Vector& center, + const core::Vector& half_size, float margin + ) const; + + /** + * @brief Check if a wall may be crossed by the line. + * + * @param x The x coordinate of the midpoint of the wall on the lattice. + * @param y The y coordinate of the midpoint of the wall on the lattice. + * @return True if the route crosses the wall, or it is inside the goal and known to be absent. + */ + bool is_open(int32_t x, int32_t y) const; + + /** + * @brief Mark a wall as one the line may cross. + * + * @param wall The wall, as a side of a cell. + */ + void open(const GridPose& wall); + + /** + * @brief Physical limits of the robot. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + const Dynamics& dynamics; + + /** + * @brief Parameters of the optimization. + */ + Config config; + + /** + * @brief Points of the outline of the robot, in its own frame, and how many there are. + */ + ///@{ + std::array outline{}; + uint8_t outline_size{}; + ///@} + + /** + * @brief Distance from the axle to the farthest point of the outline. + */ + float reach{}; + + /** + * @brief The line being optimized, and then driven. + */ + Line line; + + /** + * @brief Walls the line may cross, one bit per point of the lattice. + */ + std::array(lattice_width) * lattice_height) + 7) / 8> openings{}; + + /** + * @brief Map of the walls, while the line is optimized. + */ + const TMaze* maze{nullptr}; + + /** + * @brief Segments of the route, while the line is optimized. + */ + std::span segments; + + /** + * @brief Profile of the run. + */ + RunProfile run_profile{}; + + /** + * @brief Phase of the optimization. + */ + Phase phase{Phase::IDLE}; + + /** + * @brief Next sample of the phase in progress. + */ + uint16_t cursor{}; + + /** + * @brief Segment being sampled, and the distance along the route at its start. + */ + ///@{ + uint16_t segment_index{}; + float segment_start{}; + ///@} + + /** + * @brief Number of sweeps done, and the farthest slide of the sweep in progress. + */ + ///@{ + uint8_t sweeps{}; + float sweep_slide{}; + ///@} + + /** + * @brief First sample of the window and its number of samples. + */ + ///@{ + uint16_t window_start{}; + uint8_t window_size{}; + ///@} + + /** + * @brief Heading, bounds and position before the move of each sample of the window. + */ + ///@{ + std::array headings{}; + std::array lower{}; + std::array upper{}; + std::array old_x{}; + std::array old_y{}; + ///@} + + /** + * @brief Whether each sample of the window and its two neighbors kept the margin before the move. + */ + std::array was_clear{}; + + /** + * @brief Banded normal equations of the window: the diagonal, the two bands above it and the + * linear term. + */ + ///@{ + std::array diagonal{}; + std::array first_band{}; + std::array second_band{}; + std::array linear{}; + ///@} + + /** + * @brief Slides of the window, and the work space of the projected Newton method. + */ + ///@{ + std::array slides{}; + std::array gradient{}; + std::array direction{}; + std::array trial{}; + std::array factor_diagonal{}; + std::array factor_first{}; + std::array factor_second{}; + std::array free_indices{}; + ///@} +}; +} // namespace micras::nav + +#include "micras/nav/impl/racing_line.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_RACING_LINE_HPP diff --git a/micras_nav/include/micras/nav/robot_model.hpp b/micras_nav/include/micras/nav/robot_model.hpp new file mode 100644 index 0000000..1e8f85a --- /dev/null +++ b/micras_nav/include/micras/nav/robot_model.hpp @@ -0,0 +1,237 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_ROBOT_MODEL_HPP +#define MICRAS_NAV_ROBOT_MODEL_HPP + +#include +#include + +#include "micras/core/vector.hpp" + +namespace micras::nav { +/** + * @brief Number of wall sensors the navigation is written for. + */ +inline constexpr uint8_t number_of_wall_sensors{4}; + +/** + * @brief Physical description of a robot and of the maze it runs in. + * + * @details Every limit, gain and geometric window of the navigation is derived from these constants, + * so tuning the robot is a matter of measuring them. All of them have a unit and a plausible range, + * which makes a wrong one visible, and another robot reuses the navigation by describing itself here. + * + * @note The body frame has its origin at the midpoint of the axle, x forward and y to the left. + */ +struct RobotModel { + /** + * @brief Standard acceleration of gravity, in m/s^2. + */ + static constexpr float gravity{9.80665F}; + + /** + * @brief Dimensions of the maze, in meters. + */ + struct Maze { + float cell_size; + float wall_thickness; + }; + + /** + * @brief Mass properties and outline of the robot. + * + * @note The outline is the rectangle that encloses everything that can touch a wall, given by + * its half width and by how far it extends ahead of and behind the axle. The rolling compliance + * is how much the radius a wheel rolls on shrinks for each newton on its tire, as the tire + * flattens under the load. + */ + struct Chassis { + float mass; + float yaw_inertia; + float wheel_radius; + float rolling_compliance; + float track_width; + float half_width; + float front_length; + float rear_length; + }; + + /** + * @brief What limits the force the tires can transmit. + * + * @note The fan downforce is the extra normal force, in newtons, with the fan at the speed it + * is run at, and the fan offset is how far ahead of the axle it pulls, in meters. Pulling ahead + * of the axle, the fan tips the robot onto the front edge of its board, which then carries the + * share of the downforce that the lever of the offset over the front length gives it. The + * lateral compliance is how fast the tires slide sideways, to the outside of a curve, per unit + * of lateral acceleration, in m/s per m/s^2: a tire only pushes sideways by slipping a little. + */ + struct Traction { + float friction_coefficient; + float fan_downforce; + float fan_offset; + float lateral_compliance; + }; + + /** + * @brief Electrical and mechanical constants of one side of the drive train. + * + * @note The torque constant, in N*m/A, is numerically equal to the back EMF constant in + * V*s/rad. The gear ratio is motor turns per wheel turn. The static friction voltage is what it + * takes to keep one side turning slowly, and the supply voltage is what a command of 100 % + * applies to a motor. + */ + struct Drive { + float torque_constant; + float resistance; + float gear_ratio; + float supply_voltage; + float static_friction_voltage; + }; + + /** + * @brief Mounting of one wall sensor. + * + * @note The angle of the optical axis is measured from the forward direction, positive to the + * left. The half angle is that of the cone the emitter illuminates, plus the mounting tolerance. + */ + struct WallSensor { + core::Vector position; + float angle; + float half_angle; + }; + + /** + * @brief Noise of the sensors, which is what the pose estimator weighs them by. + * + * @note The gyroscope noise is its rate noise density, in rad/s per square root of hertz, which + * unlike the deviation of a sample does not depend on how often it is sampled. Its bias walk + * is how much the bias may drift per square root of second. The wheel angle noise is the + * resolution of the encoders, in radians of wheel. The slip allowances are the + * standard deviation of the traveled distance and of the sideways motion per square root of + * meter traveled, the first of them growing with the acceleration. The range noise of a wall + * sensor is a constant part plus a part proportional to the range, for a reading that shares + * its noise with no other, which is for the estimator to account for when the readings come + * faster than their filter lets them change. + */ + struct Noise { + float gyroscope; + float gyroscope_bias_walk; + float wheel_angle; + float longitudinal_slip; + float longitudinal_slip_per_acceleration; + float lateral_slip; + float wall_range; + float wall_range_per_meter; + }; + + /** + * @brief Get the part of the fan downforce that the tires carry. + * + * @return The downforce on the tires in newtons. + */ + constexpr float tire_downforce() const { + return this->traction.fan_downforce * (1.0F - this->traction.fan_offset / this->chassis.front_length); + } + + /** + * @brief Get the radius the wheels roll on under the load on their tires. + * + * @param downforce The share of the fan downforce there is, from 0 with the fan off to 1. + * @return The rolling radius in meters. + */ + constexpr float rolling_radius(float downforce) const { + const float load = (this->chassis.mass * gravity + downforce * this->tire_downforce()) / 2.0F; + + return this->chassis.wheel_radius - this->chassis.rolling_compliance * load; + } + + /** + * @brief Get the largest acceleration the tires can transmit, in any direction. + * + * @param fan_on Whether the fan is adding downforce. + * @return The traction limited acceleration in m/s^2. + */ + constexpr float traction_acceleration(bool fan_on) const { + return this->traction.friction_coefficient * + (gravity + (fan_on ? this->tire_downforce() / this->chassis.mass : 0.0F)); + } + + /** + * @brief Get the largest angular acceleration the tires can transmit. + * + * @param fan_on Whether the fan is adding downforce. + * @return The traction limited angular acceleration in rad/s^2. + */ + constexpr float traction_angular_acceleration(bool fan_on) const { + return this->traction_acceleration(fan_on) * this->chassis.mass * this->chassis.track_width / + (2.0F * this->chassis.yaw_inertia); + } + + /** + * @brief Get the distance over which a turn ramps its curvature up. + * + * @details With the lateral and the angular accelerations both limited by the tires, the ratio + * between them is a property of the chassis alone, and it fixes the length of the clothoid that + * takes the robot from a straight into the tightest arc it can hold. It does not depend on the + * friction, on the fan or on the speed. + * + * @return The length of the curvature ramp in meters. + */ + constexpr float turn_ramp_length() const { + return 2.0F * this->chassis.yaw_inertia / (this->chassis.mass * this->chassis.track_width); + } + + /** + * @brief Get the voltage per unit of linear speed, applied to both motors. + * + * @return The speed constant in V*s/m. + */ + constexpr float speed_constant() const { + return this->drive.torque_constant * this->drive.gear_ratio / this->chassis.wheel_radius; + } + + /** + * @brief Get the voltage per unit of linear acceleration, applied to both motors. + * + * @return The acceleration constant in V*s^2/m. + */ + constexpr float acceleration_constant() const { + return this->chassis.wheel_radius * this->drive.resistance * this->chassis.mass / + (2.0F * this->drive.gear_ratio * this->drive.torque_constant); + } + + /** + * @brief Get the differential voltage per unit of angular speed. + * + * @return The angular speed constant in V*s/rad. + */ + constexpr float angular_speed_constant() const { return this->speed_constant() * this->chassis.track_width / 2.0F; } + + /** + * @brief Get the differential voltage per unit of angular acceleration. + * + * @return The angular acceleration constant in V*s^2/rad. + */ + constexpr float angular_acceleration_constant() const { + return this->chassis.yaw_inertia * this->chassis.wheel_radius * this->drive.resistance / + (this->drive.gear_ratio * this->drive.torque_constant * this->chassis.track_width); + } + + Maze maze; + Chassis chassis; + Traction traction; + Drive drive; + std::array wall_sensors; + Noise noise; + + /** + * @brief Factor that corrects the sensitivity of the gyroscope, from the scale calibration. + */ + float gyroscope_scale; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_ROBOT_MODEL_HPP diff --git a/micras_nav/include/micras/nav/route_compiler.hpp b/micras_nav/include/micras/nav/route_compiler.hpp new file mode 100644 index 0000000..d6ee2a4 --- /dev/null +++ b/micras_nav/include/micras/nav/route_compiler.hpp @@ -0,0 +1,53 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_ROUTE_COMPILER_HPP +#define MICRAS_NAV_ROUTE_COMPILER_HPP + +#include + +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/segment.hpp" + +namespace micras::nav { +/** + * @brief Translation of a route on the lattice into the segments that drive it. + * + * @details The moves of a route already are the commands, so this is a direct mapping. The straights + * that a turn carries before and after its curve are merged with the runs around it, which leaves + * one straight between every two curves, and each segment gets the pose it nominally starts from, + * computed from the nodes of the lattice so that no error accumulates along the route. + */ +class RouteCompiler { +public: + /** + * @brief Shortest straight worth a segment of its own, in meters. + */ + static constexpr float min_straight{1.0e-4F}; + + /** + * @brief Deleted constructor for static class. + */ + RouteCompiler() = delete; + + /** + * @brief Compile a route. + * + * @note The speeds of the segments are left for the velocity planner to fill in. + * + * @param route The route. + * @param dynamics The physical limits of the robot, which hold the shape of the turns. + * @param profile The profile of the run. + * @param start_distance The distance from where the robot starts to the first node of the route. + * @param segments The segments of the route, whose storage is reused. + */ + static void compile( + const Route& route, const Dynamics& dynamics, const RunProfile& profile, float start_distance, + std::vector& segments + ); +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_ROUTE_COMPILER_HPP diff --git a/micras_nav/include/micras/nav/segment.hpp b/micras_nav/include/micras/nav/segment.hpp new file mode 100644 index 0000000..ca249dd --- /dev/null +++ b/micras_nav/include/micras/nav/segment.hpp @@ -0,0 +1,65 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_SEGMENT_HPP +#define MICRAS_NAV_SEGMENT_HPP + +#include + +#include "micras/nav/lattice.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Kinds of motion a route is made of. + */ +enum class SegmentKind : uint8_t { + STRAIGHT = 0, // Straight line, along the grid or along a diagonal. + TURN = 1, // Slalom turn from the turn table, braked and accelerated as its curvature allows. + SPIN = 2, // Rotation in place. + STOP = 3, // Stand still for a given time. + ATTACH = 4, // Stand still facing a wall until the pose settles, or for a given time at most. + LINE = 5, // The racing line, from the start to the goal, at the speeds it was planned with. +}; + +/** + * @brief One motion of a route, as a plain value. + * + * @details The length is in meters for a straight and negative to drive it backwards, in meters of + * curve for a turn, negative for the mirror of the turn of the table, which is the turn to the right, + * in radians for a rotation in place, positive to the left, in seconds for the two kinds that stand + * still, and in meters for the racing line. The speeds are those at the ends of the segment and are + * filled in by the velocity planner, except for the racing line, which carries its own. The largest + * speed caps a straight below the limit of the run, and is infinite otherwise. + * + * @note The start is the pose the segment nominally starts from in the maze frame. It comes from the + * route on the grid and never from a measurement, so the references generated from it do not move + * when the pose estimate is corrected, and whatever error is left at the end of a segment shows up + * as an error at the start of the next one instead of being forgotten. + */ +struct Segment { + SegmentKind kind; + TurnId turn; + float length; + float start_speed; + float end_speed; + float max_speed; + Pose start; +}; + +/** + * @brief What the robot should be doing at one instant. + * + * @note The acceleration is the exact derivative of the twist, which is what lets the feed forward + * be computed without differentiating anything numerically. + */ +struct Reference { + Pose pose; + Twist twist; + Twist acceleration; + float distance; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_SEGMENT_HPP diff --git a/micras_nav/include/micras/nav/speed_profile.hpp b/micras_nav/include/micras/nav/speed_profile.hpp new file mode 100644 index 0000000..096b551 --- /dev/null +++ b/micras_nav/include/micras/nav/speed_profile.hpp @@ -0,0 +1,211 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_SPEED_PROFILE_HPP +#define MICRAS_NAV_SPEED_PROFILE_HPP + +#include +#include + +#include "micras/nav/motion_limits.hpp" + +namespace micras::nav { +/** + * @brief Fastest motion along one axis between two speeds, as a function of time. + * + * @details The motion accelerates as hard as the limits allow, cruises if it reaches the speed + * limit and brakes at the last moment, which is the time optimal way to cover a distance. It times + * and plays back the straights and the rotations in place: the planner prices a straight with + * duration(), the velocity planner chains straights with get_reachable_speed() and + * get_brakeable_speed(), and the executor plays the very same object back with sample(), so that the + * time a route is chosen by is the time it takes. The axis is a distance for a straight and an angle + * for a rotation in place. + */ +class SpeedProfile { +public: + /** + * @brief State of the motion at one instant. + */ + struct Sample { + float distance; + float speed; + float acceleration; + }; + + /** + * @brief Construct an empty profile, of zero duration. + */ + SpeedProfile() = default; + + /** + * @brief Construct a new Speed Profile object. + * + * @note Speeds that the distance does not allow are not an error. A start speed too high to + * brake from is braked harder than the limit, the tires and the motors alike by the same + * factor, and an end speed that cannot be reached leaves the motion accelerating all the way, + * so the profile always covers exactly the distance. It is the velocity planner that keeps the + * speeds it asks for feasible. + * + * @param distance The distance to cover, which must not be negative. + * @param start_speed The speed at the start. + * @param end_speed The speed wanted at the end. + * @param limits The limits of the motion. + */ + SpeedProfile(float distance, float start_speed, float end_speed, const MotionLimits& limits); + + /** + * @brief Get the time the motion takes. + * + * @return The duration in seconds. + */ + float duration() const; + + /** + * @brief Get the distance the motion covers. + * + * @return The distance. + */ + float distance() const; + + /** + * @brief Evaluate the motion at an instant. + * + * @param time Time since the start of the motion, clamped to its duration. + * @return The distance covered, the speed and the acceleration at that instant. + */ + Sample sample(float time) const; + + /** + * @brief Get the instant at which a distance has been covered. + * + * @param distance The distance, clamped to the one the motion covers. + * @return The time since the start of the motion in seconds. + */ + float time_at(float distance) const; + + /** + * @brief Get the largest speed that can be reached over a distance. + * + * @param distance The distance available to accelerate. + * @param start_speed The speed at the start. + * @param limits The limits of the motion. + * @return The speed at the end when accelerating all the way, capped at the speed limit. + */ + static float get_reachable_speed(float distance, float start_speed, const MotionLimits& limits); + + /** + * @brief Get the largest speed that can be braked from over a distance. + * + * @param distance The distance available to brake. + * @param end_speed The speed at the end. + * @param limits The limits of the motion. + * @return The speed at the start when braking all the way, capped at the speed limit. + */ + static float get_brakeable_speed(float distance, float end_speed, const MotionLimits& limits); + + /** + * @brief Get the distance needed to brake between two speeds. + * + * @param start_speed The speed at the start. + * @param end_speed The speed at the end, not negative. + * @param limits The limits of the motion. + * @return The distance covered while braking as hard as the limits allow, or zero if the end + * speed is not lower than the start. + */ + static float get_braking_distance(float start_speed, float end_speed, const MotionLimits& limits); + +private: + /** + * @brief Stretch of the motion with one law for the acceleration. + * + * @note The rate is zero for a constant acceleration. Otherwise the acceleration decays as the + * speed approaches the target, as `rate * (target - speed)`, which is a motor limited stretch: + * accelerating towards the free speed, or braking towards the free speed backwards. + */ + struct Phase { + float duration{}; + float start_distance{}; + float start_speed{}; + float acceleration{}; + float rate{}; + float target{}; + }; + + /** + * @brief Number of phases: traction and motor limited acceleration, cruise, and traction and + * motor limited braking. + */ + static constexpr uint8_t number_of_phases{5}; + + /** + * @brief Newton steps that find the speed that can be braked from where the motors limit it. + * + * @note The braking distance grows faster the higher the speed there, so the steps, started + * from the crossover above the answer, close in on it from above without overshooting. + */ + static constexpr uint8_t newton_steps{8}; + + /** + * @brief Get the distance needed to accelerate between two speeds. + * + * @param start_speed The speed at the start. + * @param end_speed The speed at the end, not lower than the start and below the free speed. + * @param limits The limits of the motion. + * @return The distance covered while accelerating as hard as the limits allow. + */ + static float get_acceleration_distance(float start_speed, float end_speed, const MotionLimits& limits); + + /** + * @brief Append the phases that brake between two speeds as hard as the limits allow. + * + * @param start_speed The speed at the start of the braking. + * @param end_speed The speed at its end, not higher than the start. + * @param limits The limits of the motion. + */ + void append_braking(float start_speed, float end_speed, const MotionLimits& limits); + + /** + * @brief Append a phase, advancing the state of the motion to its end. + * + * @param phase The phase, with its duration, acceleration, rate and target filled in. + */ + void append(Phase phase); + + /** + * @brief Evaluate a phase at an instant. + * + * @param phase The phase. + * @param time Time since the start of the phase. + * @return The distance covered since the start of the motion, the speed and the acceleration. + */ + static Sample evaluate(const Phase& phase, float time); + + /** + * @brief Phases of the motion, in order, the unused ones with a null duration. + */ + std::array phases{}; + + /** + * @brief Number of phases in use. + */ + uint8_t size{}; + + /** + * @brief Time the motion takes. + */ + float total_duration{}; + + /** + * @brief Distance the motion covers. + */ + float total_distance{}; + + /** + * @brief Speed at the end of the last phase appended. + */ + float last_speed{}; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_SPEED_PROFILE_HPP diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp new file mode 100644 index 0000000..685ef33 --- /dev/null +++ b/micras_nav/include/micras/nav/state.hpp @@ -0,0 +1,82 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_STATE_HPP +#define MICRAS_NAV_STATE_HPP + +#include "micras/core/vector.hpp" +#include "micras/nav/grid_pose.hpp" + +namespace micras::nav { +/** + * @brief Type to store a pose in 2D space. + */ +struct Pose { + /** + * @brief Convert the pose to a grid pose. + * + * @param cell_size The size of the grid cells. + * @return The grid pose corresponding to the pose. + */ + GridPose to_grid(float cell_size) const; + + /** + * @brief Express in the frame of this pose a pose given relative to it. + * + * @param local The pose in the frame defined by this pose. + * @return The same pose in the frame this pose is expressed in. + */ + Pose compose(const Pose& local) const; + + /** + * @brief Express another pose in the frame defined by this pose. + * + * @param other A pose in the same frame as this pose. + * @return The other pose as seen from this one, with the orientation wrapped to [-pi, pi]. + */ + Pose relative(const Pose& other) const; + + /** + * @brief The position of the pose. + */ + core::Vector position; + + /** + * @brief The orientation of the pose. + */ + float orientation; +}; + +/** + * @brief Type to store a twist in 2D space. + */ +struct Twist { + /** + * @brief The linear velocity of the twist. + */ + float linear; + + /** + * @brief The angular velocity of the twist. + */ + float angular; +}; + +/** + * @brief Type to store the state of the robot. + */ +struct State { + /** + * @brief The pose of the robot. + */ + Pose pose; + + /** + * @brief The velocity of the robot. + */ + Twist velocity; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_STATE_HPP diff --git a/micras_nav/include/micras/nav/turn_table.hpp b/micras_nav/include/micras/nav/turn_table.hpp new file mode 100644 index 0000000..7e2ead2 --- /dev/null +++ b/micras_nav/include/micras/nav/turn_table.hpp @@ -0,0 +1,1244 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_TURN_TABLE_HPP +#define MICRAS_NAV_TURN_TABLE_HPP + +#include +#include +#include +#include +#include +#include +#include + +#include "micras/core/math.hpp" +#include "micras/nav/lattice.hpp" +#include "micras/nav/robot_model.hpp" + +namespace micras::nav { +/** + * @brief How a path bends at one point: its curvature and the rate at which the curvature changes + * with the distance traveled. + * + * @note Both are signed, positive to the left. + */ +struct Bending { + float curvature; + float sharpness; +}; + +/** + * @brief One bend of a turn: a clothoid that ramps the curvature up, an arc at the peak curvature and + * the mirrored clothoid that ramps it down. + * + * @details It is the path of a robot that holds its linear speed while its angular speed follows a + * trapezoid. Only the path is stored: the speed it is driven at is chosen when a run is planned, and + * scaling the angular speed with it and the angular acceleration with its square keeps the robot on + * this same path at any speed. + * + * @note The angle is positive to the left. The curvature and the sharpness are magnitudes, and the + * sign of the angle gives them their direction. + */ +struct TurnBend { + /** + * @brief Point of a curve, in the frame of its start. + * + * @tparam T Floating point type. + */ + template + struct Sample { + T x; + T y; + T heading; + T curvature; + T sharpness; + }; + + /** + * @brief Evaluate the bend at a distance from its start. + * + * @tparam T Floating point type. + * @param distance Distance traveled along the bend, clamped to its length. + * @return The point of the bend, with the curvature and its rate of change there. + */ + template + constexpr Sample sample(T distance) const { + const T ramp = static_cast(this->ramp_length); + const T arc = static_cast(this->arc_length); + const T rate = static_cast(this->sharpness); + const T peak = static_cast(this->curvature); + const T total = T{2} * ramp + arc; + const T side = this->angle < 0.0F ? T{-1} : T{1}; + + if (distance < T{0}) { + distance = T{0}; + } else if (distance > total) { + distance = total; + } + + if (distance <= ramp) { + const auto point = core::math::clothoid(distance, rate); + + return { + .x = point.x, + .y = side * point.y, + .heading = side * rate * distance * distance / T{2}, + .curvature = side * rate * distance, + .sharpness = side * rate, + }; + } + + const auto ramp_end = core::math::clothoid(ramp, rate); + const T ramp_heading = rate * ramp * ramp / T{2}; + + if (distance <= ramp + arc) { + const T heading = ramp_heading + peak * (distance - ramp); + + return { + .x = ramp_end.x + (core::math::sin(heading) - core::math::sin(ramp_heading)) / peak, + .y = side * (ramp_end.y - (core::math::cos(heading) - core::math::cos(ramp_heading)) / peak), + .heading = side * heading, + .curvature = side * peak, + .sharpness = T{0}, + }; + } + + const T angle = T{2} * ramp_heading + peak * arc; + const T cosine = core::math::cos(angle); + const T sine = core::math::sin(angle); + + const T arc_heading = ramp_heading + peak * arc; + const T arc_end_x = arc > T{0} ? + ramp_end.x + (core::math::sin(arc_heading) - core::math::sin(ramp_heading)) / peak : + ramp_end.x; + const T arc_end_y = arc > T{0} ? + ramp_end.y - (core::math::cos(arc_heading) - core::math::cos(ramp_heading)) / peak : + ramp_end.y; + + const T end_x = arc_end_x + cosine * ramp_end.x + sine * ramp_end.y; + const T end_y = arc_end_y + sine * ramp_end.x - cosine * ramp_end.y; + + const T remaining = total - distance; + const auto mirrored = core::math::clothoid(remaining, rate); + + return { + .x = end_x - (cosine * mirrored.x + sine * mirrored.y), + .y = side * (end_y - (sine * mirrored.x - cosine * mirrored.y)), + .heading = side * (angle - rate * remaining * remaining / T{2}), + .curvature = side * rate * remaining, + .sharpness = -side * rate, + }; + } + + /** + * @brief Get how the bend bends at a distance from its start, without evaluating its points. + * + * @param distance Distance traveled along the bend, clamped to its length. + * @return The curvature and the sharpness there. + */ + constexpr Bending bending_at(float distance) const { + const float side = this->angle < 0.0F ? -1.0F : 1.0F; + const float total = this->length(); + + distance = std::clamp(distance, 0.0F, total); + + if (distance <= this->ramp_length) { + return {.curvature = side * this->sharpness * distance, .sharpness = side * this->sharpness}; + } + + if (distance <= this->ramp_length + this->arc_length) { + return {.curvature = side * this->curvature, .sharpness = 0.0F}; + } + + return {.curvature = side * this->sharpness * (total - distance), .sharpness = -side * this->sharpness}; + } + + /** + * @brief Get the length of the bend. + * + * @return The length of the two ramps and the arc in meters. + */ + constexpr float length() const { return 2.0F * this->ramp_length + this->arc_length; } + + float angle{}; + float curvature{}; + float sharpness{}; + float ramp_length{}; + float arc_length{}; +}; + +/** + * @brief Geometry of a slalom turn. + * + * @details A turn is one bend, or two bends joined by a straight, which is how a turn that fits inside + * one cell swings out before it turns in. The planner prices the whole curve at one speed, so the + * curvature and the sharpness that limit that speed are the largest of its bends. The robot then + * drives it at the speed each point allows, which is only faster. + * + * @note The straights before and after the curve place it between its entry and exit nodes. All + * lengths are in meters and the angles, in radians, are those of the turn to the left. + */ +struct TurnShape { + /** + * @brief Largest number of bends of a turn. + */ + static constexpr uint8_t max_bends{2}; + + /** + * @brief Point of the curve, in the frame of its start, for the turn to the left. + * + * @tparam T Floating point type. + */ + template + using Sample = TurnBend::Sample; + + /** + * @brief Evaluate the curve at a distance from its start. + * + * @tparam T Floating point type. + * @param distance Distance traveled along the curve, clamped to its length. + * @return The point of the curve, with the curvature and its rate of change there. + */ + template + constexpr Sample sample(T distance) const { + const TurnBend& first = this->bends.at(0); + const T first_length = static_cast(first.length()); + + if (this->number_of_bends < 2 or distance <= first_length) { + return first.template sample(distance); + } + + const Sample joint = first.template sample(first_length); + const T cosine = core::math::cos(joint.heading); + const T sine = core::math::sin(joint.heading); + const T straight = static_cast(this->straight_length); + const T along = distance - first_length; + + if (along <= straight) { + return { + .x = joint.x + along * cosine, + .y = joint.y + along * sine, + .heading = joint.heading, + .curvature = T{0}, + .sharpness = T{0}, + }; + } + + const Sample local = this->bends.at(1).template sample(along - straight); + const T start_x = joint.x + straight * cosine; + const T start_y = joint.y + straight * sine; + + return { + .x = start_x + cosine * local.x - sine * local.y, + .y = start_y + sine * local.x + cosine * local.y, + .heading = joint.heading + local.heading, + .curvature = local.curvature, + .sharpness = local.sharpness, + }; + } + + /** + * @brief Get how the curve bends at a distance from its start, without evaluating its points. + * + * @param distance Distance traveled along the curve. + * @return The curvature and the sharpness there. + */ + constexpr Bending bending_at(float distance) const { + const float first_length = this->bends.at(0).length(); + + if (this->number_of_bends < 2 or distance <= first_length) { + return this->bends.at(0).bending_at(distance); + } + + if (distance <= first_length + this->straight_length) { + return {.curvature = 0.0F, .sharpness = 0.0F}; + } + + return this->bends.at(1).bending_at(distance - first_length - this->straight_length); + } + + /** + * @brief Get the length of the curve. + * + * @return The length of the bends and of the straight between them in meters. + */ + constexpr float length() const { + if (this->number_of_bends < 2) { + return this->bends.at(0).length(); + } + + return this->bends.at(0).length() + this->straight_length + this->bends.at(1).length(); + } + + /** + * @brief Get the length of the whole turn, from its entry node to its exit node. + * + * @return The length of the curve and of the straights around it in meters. + */ + constexpr float total_length() const { return this->pre + this->length() + this->post; } + + std::array bends{}; + uint8_t number_of_bends{}; + float straight_length{}; + + /** + * @brief Largest curvature and sharpness. + */ + float curvature{}; + float sharpness{}; + + float pre{}; + float post{}; + + /** + * @brief Distance from the entry node at which each wall between the nodes is crossed. + */ + std::array gates{}; + + /** + * @brief Whether a curve that fits between the nodes with the required clearance exists. + */ + bool valid{}; +}; + +/** + * @brief Parameters of a turn of two bends, found by a search too long to run in the compiler. + * + * @details The angles are those of the turn to the left, positive to the left, and the curvatures + * are the peaks asked of each bend. The straight between the bends and the one after them follow + * from the place of the exit node, so a design is only the choice of how the turn swings. + */ +struct TwoBendDesign { + float first_angle; + float first_curvature; + float second_angle; + float second_curvature; + float pre; +}; + +/** + * @brief Shape of every turn, computed from the geometry of the maze and of the robot. + * + * @details Each turn is made as wide as it can be while it still fits between its two nodes and + * keeps the outline of the robot a safety margin away from every post and from every wall that is + * not known to be absent, since a wider turn is a faster one. The clearance is measured on the path + * itself, rather than on a circular arc of equivalent displacement. + * + * @note Everything here is a constant expression, so a table is normally built when the firmware is + * compiled, where a turn that does not fit can be rejected by a static_assert on is_valid(). + */ +class TurnTable { +public: + /** + * @brief Designs of the turns of two bends, from first_two_bend_turn on. + */ + using TwoBendDesigns = std::array; + + /** + * @brief Shapes of the turns of two bends, from first_two_bend_turn on. + */ + using TwoBendShapes = std::array; + + /** + * @brief Compute the shape of every turn of one bend, and take those of two. + * + * @note The turns of two bends are placed between their nodes by place(), and checked to clear + * the walls by clears(), each a constant expression of its own, since the compiler gives each one + * a budget and designing the turns of one bend takes most of this one. + * + * @param model Physical description of the robot and of the maze. + * @param margin Smallest distance allowed between the outline of the robot and an obstacle. + * @param two_bend_shapes Shape of each turn of two bends, for this margin. + */ + constexpr TurnTable(const RobotModel& model, float margin, const TwoBendShapes& two_bend_shapes) { + const Designer designer{model, margin}; + + for (uint8_t i = 0; i < first_two_bend_turn; i++) { + this->shapes.at(i) = designer.design(static_cast(i)); + } + + std::ranges::copy(two_bend_shapes, std::next(this->shapes.begin(), first_two_bend_turn)); + } + + /** + * @brief Place every turn of two bends between its nodes. + * + * @param model Physical description of the robot and of the maze. + * @param margin Smallest distance allowed between the outline of the robot and an obstacle. + * @param designs Design of each turn of two bends, for this margin. + * @return The shapes, with the valid flag of each cleared if it does not end on the exit node of + * its turn. + */ + static constexpr TwoBendShapes place(const RobotModel& model, float margin, const TwoBendDesigns& designs) { + const Designer designer{model, margin}; + TwoBendShapes shapes{}; + + for (uint8_t i = 0; i < number_of_two_bend_turns; i++) { + shapes.at(i) = designer.build(static_cast(first_two_bend_turn + i), designs.at(i)); + } + + return shapes; + } + + /** + * @brief Build the shape of a turn of two bends and check that it fits. + * + * @note This is what the compiler runs on every design, and what the search for the designs runs + * on every candidate, so a design is accepted by the same test that found it. + * + * @param model Physical description of the robot and of the maze. + * @param margin Smallest distance allowed between the outline of the robot and an obstacle. + * @param turn The turn. + * @param design The parameters of the bends. + * @return The shape, with its valid flag cleared if it does not end on the exit node of the turn + * or does not clear every obstacle. + */ + static constexpr TurnShape check(const RobotModel& model, float margin, TurnId turn, const TwoBendDesign& design) { + return Designer{model, margin}.check(turn, design); + } + + /** + * @brief Check that every turn of two bends clears the walls. + * + * @param model Physical description of the robot and of the maze. + * @param margin Smallest distance allowed between the outline of the robot and an obstacle. + * @param designs Design of each turn of two bends, from first_two_bend_turn on, for this margin. + * @return True if every design ends on its exit node and clears every obstacle. + */ + static constexpr bool clears(const RobotModel& model, float margin, const TwoBendDesigns& designs) { + const Designer designer{model, margin}; + + for (uint8_t i = 0; i < number_of_two_bend_turns; i++) { + if (not designer.check(static_cast(first_two_bend_turn + i), designs.at(i)).valid) { + return false; + } + } + + return true; + } + + /** + * @brief Build one bend the way the turns are built. + * + * @param model Physical description of the robot. + * @param angle The angle of the bend, positive to the left. + * @param curvature The peak curvature asked for, which a bend too short for its ramps lowers. + * @return The bend. + */ + static constexpr TurnBend make_bend(const RobotModel& model, double angle, double curvature) { + return Designer{model, 0.0F}.make_bend(angle, curvature); + } + + /** + * @brief Get the shape of a turn. + * + * @param turn The turn. + * @return The shape of the turn. + */ + constexpr const TurnShape& get(TurnId turn) const { return this->shapes.at(std::to_underlying(turn)); } + + /** + * @brief Check if every turn fits. + * + * @return True if a shape was found for every turn, false otherwise. + */ + constexpr bool is_valid() const { + return std::ranges::all_of(this->shapes, [](const TurnShape& shape) { return shape.valid; }); + } + +private: + /** + * @brief Solver for the shape of one turn, in double precision. + */ + class Designer { + public: + /** + * @brief Construct a new Designer object. + * + * @param model Physical description of the robot and of the maze. + * @param margin Smallest distance allowed between the outline of the robot and an obstacle. + */ + constexpr Designer(const RobotModel& model, float margin) : + half_cell{static_cast(model.maze.cell_size) / 2.0}, + half_wall{static_cast(model.maze.wall_thickness) / 2.0}, + ramp_length{static_cast(model.turn_ramp_length())}, + half_width{static_cast(model.chassis.half_width)}, + front_length{static_cast(model.chassis.front_length)}, + rear_length{static_cast(model.chassis.rear_length)}, + margin{static_cast(margin)} { } + + /** + * @brief Find the widest shape of a turn that fits. + * + * @param turn The turn. + * @return The shape, with its valid flag cleared if none fits. + */ + constexpr TurnShape design(TurnId turn) const { + const TurnPrimitive& primitive = get_primitive(turn); + const Layout layout = this->get_layout(primitive); + + if (primitive.rotation == 4) { + return this->design_half_turn(primitive, layout); + } + + double infeasible = 1.0 / (16.0 * this->half_cell); + double feasible = 0.0; + double curvature = infeasible; + + for (uint8_t i = 0; i < coarse_steps; i++) { + if (this->fits(this->place(layout, curvature), layout)) { + feasible = curvature; + break; + } + + infeasible = curvature; + curvature *= coarse_ratio; + } + + if (feasible == 0.0) { + return {}; + } + + for (uint8_t i = 0; i < 14; i++) { + const double middle = (infeasible + feasible) / 2.0; + + if (this->fits(this->place(layout, middle), layout)) { + feasible = middle; + } else { + infeasible = middle; + } + } + + return finish(this->place(layout, feasible), layout); + } + + /** + * @brief Build the shape of a turn of two bends and check that it fits. + * + * @param turn The turn. + * @param design The parameters of the bends. + * @return The shape, with its valid flag cleared if it does not end on the exit node of the + * turn or does not clear every obstacle. + */ + constexpr TurnShape check(TurnId turn, const TwoBendDesign& design) const { + TurnShape shape = this->build(turn, design); + + if (not shape.valid) { + return shape; + } + + const Layout layout = this->get_layout(get_primitive(turn)); + const auto total = static_cast(shape.total_length()); + const auto samples = static_cast((total / sample_spacing) + 2.0); + + for (uint16_t i = 0; i < samples; i++) { + if (not this->is_clear(get_placement(shape, layout, total * i / (samples - 1)), layout)) { + shape.valid = false; + return shape; + } + } + + return shape; + } + + /** + * @brief Place a turn of two bends between its nodes. + * + * @details Given the bends and the straight before them, the exit node leaves two unknowns, + * the straight between the bends and the one after them, and two equations for its position. + * The shape is valid when both straights come out positive and the bends turn the angle of + * the turn. Whether it clears the walls is left to check(). + * + * @param turn The turn. + * @param design The parameters of the bends. + * @return The shape, with its valid flag cleared if it does not end on the exit node. + */ + constexpr TurnShape build(TurnId turn, const TwoBendDesign& design) const { + const TurnPrimitive& primitive = get_primitive(turn); + + if (design.first_curvature <= 0.0F or design.second_curvature <= 0.0F or design.pre < 0.0F) { + return {}; + } + + const auto first_angle = static_cast(design.first_angle); + const auto second_angle = static_cast(design.second_angle); + const double turned = (first_angle + second_angle) / (std::numbers::pi / 4.0) - primitive.rotation; + const double revolutions = turned / 8.0; + // NOLINTNEXTLINE(bugprone-incorrect-roundings) lround is not constexpr, and the value is positive + const auto whole = static_cast(static_cast(core::math::abs(revolutions) + 0.5)); + const double rounded = revolutions < 0.0 ? -whole : whole; + + if (core::math::abs(turned - 8.0 * rounded) > 1.0e-4) { + return {}; + } + + const Candidate first = + this->make_curve(core::math::abs(first_angle), static_cast(design.first_curvature)); + const Candidate second = + this->make_curve(core::math::abs(second_angle), static_cast(design.second_curvature)); + + TurnShape shape{}; + shape.bends.at(0) = to_bend(first, first_angle); + shape.bends.at(1) = to_bend(second, second_angle); + shape.number_of_bends = 2; + shape.curvature = std::max(shape.bends.at(0).curvature, shape.bends.at(1).curvature); + shape.sharpness = std::max(shape.bends.at(0).sharpness, shape.bends.at(1).sharpness); + shape.pre = design.pre; + + const Layout layout = this->get_layout(primitive); + + const double entry = layout.entry_heading; + const double middle = entry + first_angle; + const double last = middle + second_angle; + const double determinant = core::math::sin(second_angle); + + if (core::math::abs(determinant) < 1.0e-6) { + return {}; + } + + const auto first_end = shape.bends.at(0).template sample(1.0e3); + const auto second_end = shape.bends.at(1).template sample(1.0e3); + const double remaining_x = + layout.exit_x - static_cast(shape.pre) * core::math::cos(entry) - + (core::math::cos(entry) * first_end.x - core::math::sin(entry) * first_end.y) - + (core::math::cos(middle) * second_end.x - core::math::sin(middle) * second_end.y); + const double remaining_y = + layout.exit_y - static_cast(shape.pre) * core::math::sin(entry) - + (core::math::sin(entry) * first_end.x + core::math::cos(entry) * first_end.y) - + (core::math::sin(middle) * second_end.x + core::math::cos(middle) * second_end.y); + + const double straight = + (remaining_x * core::math::sin(last) - remaining_y * core::math::cos(last)) / determinant; + const double post = + (core::math::cos(middle) * remaining_y - core::math::sin(middle) * remaining_x) / determinant; + + if (straight < -1.0e-6 or post < -1.0e-6) { + return {}; + } + + shape.straight_length = static_cast(straight > 0.0 ? straight : 0.0); + shape.post = static_cast(post > 0.0 ? post : 0.0); + shape.valid = true; + + return shape; + } + + /** + * @brief Build one bend. + * + * @param angle The angle of the bend, positive to the left. + * @param curvature The peak curvature asked for. + * @return The bend. + */ + constexpr TurnBend make_bend(double angle, double curvature) const { + return to_bend(this->make_curve(core::math::abs(angle), curvature), angle); + } + + private: + /** + * @brief Largest distance between the points of a turn of two bends where the clearance is + * checked, in meters. + * + * @note The points are spread over the whole turn, straights included. + */ + static constexpr double sample_spacing{0.005}; + + /** + * @brief Number of sizes tried, from the widest down, before refining the first that fits. + */ + static constexpr uint8_t coarse_steps{48}; + + /** + * @brief Number of lengths tried for the straights around a turn of half a revolution. + */ + static constexpr uint8_t straight_steps{40}; + + /** + * @brief Ratio between the curvatures of two consecutive sizes tried. + */ + static constexpr double coarse_ratio{1.12}; + + /** + * @brief Largest number of obstacles considered around a turn. + */ + static constexpr uint8_t max_obstacles{96}; + + /** + * @brief Number of points of the curve where the clearance is checked. + */ + static constexpr uint8_t clearance_samples{32}; + + /** + * @brief Axis aligned rectangle in the canonical frame of a turn, in meters. + */ + struct Rectangle { + double center_x; + double center_y; + double half_x; + double half_y; + }; + + /** + * @brief Where a turn sits in the canonical frame, in meters and radians. + */ + struct Layout { + double entry_heading{}; + double angle{}; + double exit_x{}; + double exit_y{}; + double tangent_in{}; + double tangent_out{}; + std::array obstacles{}; + uint8_t number_of_obstacles{}; + std::array gates{}; + uint8_t number_of_gates{}; + }; + + /** + * @brief Candidate shape, in double precision. + */ + struct Candidate { + double curvature{}; + double sharpness{}; + double ramp{}; + double arc{}; + double pre{}; + double post{}; + }; + + /** + * @brief Pose of the robot in the canonical frame. + */ + struct Placement { + double x; + double y; + double heading; + }; + + /** + * @brief Build the curve of a candidate as a shape, to evaluate it. + * + * @param candidate The candidate. + * @param angle The angle of the turn. + * @return The shape with the fields of the curve filled in. + */ + static constexpr TurnShape to_shape(const Candidate& candidate, double angle) { + TurnShape shape{}; + shape.bends.at(0) = to_bend(candidate, angle); + shape.number_of_bends = 1; + shape.curvature = static_cast(candidate.curvature); + shape.sharpness = static_cast(candidate.sharpness); + shape.pre = static_cast(candidate.pre); + shape.post = static_cast(candidate.post); + return shape; + } + + /** + * @brief Build the bend of a candidate. + * + * @param candidate The candidate. + * @param angle The angle of the bend, positive to the left. + * @return The bend. + */ + static constexpr TurnBend to_bend(const Candidate& candidate, double angle) { + return { + .angle = static_cast(angle), + .curvature = static_cast(candidate.curvature), + .sharpness = static_cast(candidate.sharpness), + .ramp_length = static_cast(candidate.ramp), + .arc_length = static_cast(candidate.arc), + }; + } + + /** + * @brief Build the curve that turns a given angle with a given peak curvature. + * + * @note When the ramps alone would turn more than the angle, they are shortened and the arc + * disappears, which keeps the rate of change of the curvature. + * + * @param angle The angle of the turn. + * @param curvature The peak curvature requested. + * @return The candidate, without the straights. + */ + constexpr Candidate make_curve(double angle, double curvature) const { + Candidate candidate{}; + candidate.sharpness = curvature / this->ramp_length; + candidate.ramp = this->ramp_length; + candidate.curvature = curvature; + + if (curvature * this->ramp_length > angle) { + candidate.ramp = core::math::sqrt(angle / candidate.sharpness); + candidate.curvature = candidate.sharpness * candidate.ramp; + } + + candidate.arc = (angle - candidate.curvature * candidate.ramp) / candidate.curvature; + + return candidate; + } + + /** + * @brief Get the pose of the robot at a distance from the entry node. + * + * @param candidate The candidate shape. + * @param layout The place of the turn. + * @param distance Distance traveled from the entry node. + * @return The pose in the canonical frame. + */ + static constexpr Placement get_placement(const Candidate& candidate, const Layout& layout, double distance) { + const double cosine = core::math::cos(layout.entry_heading); + const double sine = core::math::sin(layout.entry_heading); + const double length = 2.0 * candidate.ramp + candidate.arc; + + double local_x = distance; + double local_y = 0.0; + double heading = 0.0; + + if (distance > candidate.pre) { + const auto sample = to_shape(candidate, layout.angle).template sample(distance - candidate.pre); + + local_x = candidate.pre + sample.x; + local_y = sample.y; + heading = sample.heading; + + if (distance > candidate.pre + length) { + const double extra = distance - candidate.pre - length; + local_x += extra * core::math::cos(layout.angle); + local_y += extra * core::math::sin(layout.angle); + } + } + + return { + .x = cosine * local_x - sine * local_y, + .y = sine * local_x + cosine * local_y, + .heading = layout.entry_heading + heading, + }; + } + + /** + * @brief Get the pose of the robot at a distance from the entry node, along a shape. + * + * @param shape The shape, with its straights. + * @param layout The place of the turn. + * @param distance Distance traveled from the entry node. + * @return The pose in the canonical frame. + */ + static constexpr Placement get_placement(const TurnShape& shape, const Layout& layout, double distance) { + const double cosine = core::math::cos(layout.entry_heading); + const double sine = core::math::sin(layout.entry_heading); + const auto pre = static_cast(shape.pre); + const auto length = static_cast(shape.length()); + + double local_x = distance; + double local_y = 0.0; + double heading = 0.0; + + if (distance > pre) { + const auto sample = shape.template sample(distance > pre + length ? length : distance - pre); + + local_x = pre + sample.x; + local_y = sample.y; + heading = sample.heading; + + if (distance > pre + length) { + const double extra = distance - pre - length; + local_x += extra * core::math::cos(heading); + local_y += extra * core::math::sin(heading); + } + } + + return { + .x = cosine * local_x - sine * local_y, + .y = sine * local_x + cosine * local_y, + .heading = layout.entry_heading + heading, + }; + } + + /** + * @brief Collect the geometry of a turn in its canonical frame. + * + * @param primitive The place of the turn in the lattice. + * @return The layout, with every post and every wall that may exist around the turn. + */ + constexpr Layout get_layout(const TurnPrimitive& primitive) const { + Layout layout{}; + layout.entry_heading = primitive.diagonal_entry ? std::numbers::pi / 4.0 : 0.0; + layout.angle = primitive.rotation * std::numbers::pi / 4.0; + layout.exit_x = primitive.exit.x * this->half_cell; + layout.exit_y = primitive.exit.y * this->half_cell; + layout.number_of_gates = primitive.number_of_gates; + + const double cosine = core::math::cos(layout.entry_heading); + const double sine = core::math::sin(layout.entry_heading); + const double forward = cosine * layout.exit_x + sine * layout.exit_y; + const double lateral = -sine * layout.exit_x + cosine * layout.exit_y; + + if (primitive.rotation % 4 != 0) { + layout.tangent_out = lateral / core::math::sin(layout.angle); + layout.tangent_in = forward - layout.tangent_out * core::math::cos(layout.angle); + } + + for (uint8_t i = 0; i < primitive.number_of_gates; i++) { + layout.gates.at(i) = this->get_wall(primitive.gates.at(i)); + } + + const double reach = 3.0 * this->half_cell; + const double low_x = (layout.exit_x < 0.0 ? layout.exit_x : 0.0) - reach; + const double high_x = (layout.exit_x > 0.0 ? layout.exit_x : 0.0) + reach; + const double low_y = (layout.exit_y < 0.0 ? layout.exit_y : 0.0) - reach; + const double high_y = (layout.exit_y > 0.0 ? layout.exit_y : 0.0) + reach; + + for (int8_t column = -6; column <= 8; column++) { + for (int8_t row = -7; row <= 7; row++) { + const LatticePoint point{.x = column, .y = row}; + + if (column % 2 != 0 and row % 2 == 0) { + continue; + } + + if (is_open(primitive, point) or layout.number_of_obstacles == max_obstacles) { + continue; + } + + const Rectangle obstacle = this->get_wall(point); + + if (obstacle.center_x < low_x or obstacle.center_x > high_x or obstacle.center_y < low_y or + obstacle.center_y > high_y) { + continue; + } + + layout.obstacles.at(layout.number_of_obstacles) = obstacle; + layout.number_of_obstacles++; + } + } + + return layout; + } + + /** + * @brief Check if a point of the canonical frame is a wall the turn needs to be absent. + * + * @note In the canonical frame the entry node is on a vertical wall, so the posts are the + * points with an even x and an odd y, the vertical walls have both coordinates even and + * the horizontal ones have both odd. + * + * @param primitive The place of the turn in the lattice. + * @param point The point, relative to the entry node. + * @return True if the robot crosses the wall at that point. + */ + static constexpr bool is_open(const TurnPrimitive& primitive, const LatticePoint& point) { + if (point == LatticePoint{.x = 0, .y = 0} or point == primitive.exit) { + return true; + } + + for (uint8_t i = 0; i < primitive.number_of_gates; i++) { + if (point == primitive.gates.at(i)) { + return true; + } + } + + return false; + } + + /** + * @brief Get the rectangle occupied by a post or by a wall. + * + * @param point The post or the midpoint of the wall, relative to the entry node. + * @return The rectangle in the canonical frame. + */ + constexpr Rectangle get_wall(const LatticePoint& point) const { + const bool is_post = point.x % 2 == 0 and point.y % 2 != 0; + const bool is_vertical = point.x % 2 == 0; + + return { + .center_x = point.x * this->half_cell, + .center_y = point.y * this->half_cell, + .half_x = (is_post or is_vertical) ? this->half_wall : this->half_cell, + .half_y = (is_post or not is_vertical) ? this->half_wall : this->half_cell, + }; + } + + /** + * @brief Place a curve between the nodes of a turn. + * + * @param layout The place of the turn. + * @param curvature The peak curvature requested. + * @return The candidate, whose straights are negative when the curve does not fit. + */ + constexpr Candidate place(const Layout& layout, double curvature) const { + Candidate candidate = this->make_curve(layout.angle, curvature); + const auto end = to_shape(candidate, layout.angle).template sample(1.0e3); + const double tangent = end.y / core::math::sin(layout.angle); + + candidate.pre = layout.tangent_in - tangent; + candidate.post = layout.tangent_out - tangent; + + return candidate; + } + + /** + * @brief Check if a candidate fits between its nodes and clears every obstacle. + * + * @param candidate The candidate shape. + * @param layout The place of the turn. + * @return True if the candidate can be driven. + */ + constexpr bool fits(const Candidate& candidate, const Layout& layout) const { + if (candidate.pre < 0.0 or candidate.post < 0.0) { + return false; + } + + const double length = 2.0 * candidate.ramp + candidate.arc; + + for (uint8_t i = 0; i <= clearance_samples + 1; i++) { + double distance = candidate.pre + length * (i - 1) / (clearance_samples - 1); + + if (i == 0) { + distance = 0.0; + } else if (i == clearance_samples + 1) { + distance = candidate.pre + length + candidate.post; + } + + if (not this->is_clear(get_placement(candidate, layout, distance), layout)) { + return false; + } + } + + return true; + } + + /** + * @brief Check if the robot keeps clear of every obstacle at one pose. + * + * @param placement The pose of the robot. + * @param layout The place of the turn. + * @return True if the robot stays at least the margin away from every obstacle. + */ + constexpr bool is_clear(const Placement& placement, const Layout& layout) const { + const double longest = this->front_length > this->rear_length ? this->front_length : this->rear_length; + const double reach = + core::math::sqrt(this->half_width * this->half_width + longest * longest) + this->margin; + + for (uint8_t j = 0; j < layout.number_of_obstacles; j++) { + const Rectangle& obstacle = layout.obstacles.at(j); + + if (core::math::abs(obstacle.center_x - placement.x) > reach + obstacle.half_x or + core::math::abs(obstacle.center_y - placement.y) > reach + obstacle.half_y) { + continue; + } + + if (this->touches(placement, obstacle)) { + return false; + } + } + + return true; + } + + /** + * @brief Check if a point is closer than the margin to a rectangle centered at the origin. + * + * @param x The x coordinate of the point in the frame of the rectangle. + * @param y The y coordinate of the point in the frame of the rectangle. + * @param low_x The lowest x coordinate of the rectangle. + * @param high_x The highest x coordinate of the rectangle. + * @param half_y Half of the size of the rectangle along y. + * @return True if the point is within the margin of the rectangle. + */ + constexpr bool is_near(double x, double y, double low_x, double high_x, double half_y) const { + double excess_x = 0.0; + + if (x > high_x) { + excess_x = x - high_x; + } else if (x < low_x) { + excess_x = low_x - x; + } + + const double excess_y = core::math::abs(y) > half_y ? core::math::abs(y) - half_y : 0.0; + + return excess_x * excess_x + excess_y * excess_y < this->margin * this->margin; + } + + /** + * @brief Check if the outline of the robot comes within the margin of an obstacle. + * + * @note Both shapes are convex and none of them can be inside the other, so the smallest + * distance between them is always measured from a corner of one of the two. + * + * @param placement The pose of the robot. + * @param obstacle The obstacle. + * @return True if the robot is closer to the obstacle than the margin. + */ + constexpr bool touches(const Placement& placement, const Rectangle& obstacle) const { + const double cosine = core::math::cos(placement.heading); + const double sine = core::math::sin(placement.heading); + + for (uint8_t corner = 0; corner < 4; corner++) { + const double along = corner < 2 ? this->front_length : -this->rear_length; + const double across = corner % 2 == 0 ? this->half_width : -this->half_width; + + const double corner_x = placement.x + cosine * along - sine * across - obstacle.center_x; + const double corner_y = placement.y + sine * along + cosine * across - obstacle.center_y; + + if (this->is_near(corner_x, corner_y, -obstacle.half_x, obstacle.half_x, obstacle.half_y)) { + return true; + } + + const double offset_x = + obstacle.center_x + (corner < 2 ? obstacle.half_x : -obstacle.half_x) - placement.x; + const double offset_y = + obstacle.center_y + (corner % 2 == 0 ? obstacle.half_y : -obstacle.half_y) - placement.y; + + const double body_x = cosine * offset_x + sine * offset_y; + const double body_y = -sine * offset_x + cosine * offset_y; + + if (this->is_near(body_x, body_y, -this->rear_length, this->front_length, this->half_width)) { + return true; + } + } + + return false; + } + + /** + * @brief Find the shape of a turn of half a revolution. + * + * @note The exit is parallel to the entry, so the size of the curve is fixed by the distance + * between them and what is left to choose is how far into the cell it is made. + * + * @param primitive The place of the turn in the lattice. + * @param layout The place of the turn. + * @return The shape, with its valid flag cleared if none fits. + */ + constexpr TurnShape design_half_turn(const TurnPrimitive& primitive, const Layout& layout) const { + const double lateral = primitive.exit.y * this->half_cell; + + double tight = 1.0 / (0.1 * this->half_cell); + double wide = 1.0 / (16.0 * this->half_cell); + + for (uint8_t i = 0; i < 48; i++) { + const double middle = (tight + wide) / 2.0; + const Candidate candidate = this->make_curve(layout.angle, middle); + const auto end = to_shape(candidate, layout.angle).template sample(1.0e3); + + if (end.y > lateral) { + wide = middle; + } else { + tight = middle; + } + } + + Candidate candidate = this->make_curve(layout.angle, (tight + wide) / 2.0); + double blocked = -1.0; + double clear = -1.0; + + for (uint8_t i = 0; i < straight_steps; i++) { + const double straight = 2.0 * this->half_cell * i / straight_steps; + + candidate.pre = straight; + candidate.post = straight; + + if (this->fits(candidate, layout)) { + clear = straight; + break; + } + + blocked = straight; + } + + if (clear < 0.0) { + return {}; + } + + for (uint8_t i = 0; i < 10 and blocked >= 0.0; i++) { + const double middle = (blocked + clear) / 2.0; + candidate.pre = middle; + candidate.post = middle; + + if (this->fits(candidate, layout)) { + clear = middle; + } else { + blocked = middle; + } + } + + candidate.pre = clear; + candidate.post = clear; + + return finish(candidate, layout); + } + + /** + * @brief Turn a candidate into a shape, locating where it crosses each gate. + * + * @note A gate is looked for from the end of the turn backwards, since a turn that doubles + * back can touch the line of a wall before the place where it actually crosses it. + * + * @param candidate The candidate shape. + * @param layout The place of the turn. + * @return The shape. + */ + static constexpr TurnShape finish(const Candidate& candidate, const Layout& layout) { + TurnShape shape = to_shape(candidate, layout.angle); + shape.valid = true; + + const double total = candidate.pre + 2.0 * candidate.ramp + candidate.arc + candidate.post; + + for (uint8_t i = 0; i < layout.number_of_gates; i++) { + const Rectangle& gate = layout.gates.at(i); + const bool vertical = gate.half_x < gate.half_y; + + const auto side = [&](double distance) { + const Placement placement = get_placement(candidate, layout, distance); + return vertical ? placement.x - gate.center_x : placement.y - gate.center_y; + }; + + const double final_side = side(total); + double after = total; + double before = 0.0; + + for (uint8_t j = 1; j <= 64; j++) { + const double distance = total * (64 - j) / 64.0; + + if ((side(distance) > 0.0) != (final_side > 0.0)) { + before = distance; + break; + } + + after = distance; + } + + for (uint8_t j = 0; j < 30; j++) { + const double middle = (before + after) / 2.0; + + if ((side(middle) > 0.0) != (final_side > 0.0)) { + before = middle; + } else { + after = middle; + } + } + + shape.gates.at(i) = static_cast((before + after) / 2.0); + } + + return shape; + } + + double half_cell; + double half_wall; + double ramp_length; + double half_width; + double front_length; + double rear_length; + double margin; + }; + + /** + * @brief Shape of each turn, indexed by TurnId. + */ + std::array shapes{}; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_TURN_TABLE_HPP diff --git a/micras_nav/include/micras/nav/velocity_planner.hpp b/micras_nav/include/micras/nav/velocity_planner.hpp new file mode 100644 index 0000000..fcb4c0d --- /dev/null +++ b/micras_nav/include/micras/nav/velocity_planner.hpp @@ -0,0 +1,60 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_VELOCITY_PLANNER_HPP +#define MICRAS_NAV_VELOCITY_PLANNER_HPP + +#include + +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/segment.hpp" + +namespace micras::nav { +/** + * @brief Choice of the speed at every junction of a route. + * + * @details Every turn starts and ends at the speed its shape allows there and every straight at the + * speed limit. A pass from the end to the start then lowers each junction to what can be braked from + * before the next one, and a pass from the start to the end lowers it to what can be reached from + * the previous one. A turn is braked and accelerated through as its curvature leaves grip to spare, + * which is how two turns too close together slow each other down: the grip they have left has to + * make up for the straight they lack. What is left is the fastest set of speeds the limits allow. + */ +class VelocityPlanner { +public: + /** + * @brief Deleted constructor for static class. + */ + VelocityPlanner() = delete; + + /** + * @brief Fill in the speeds of a route. + * + * @param route The segments of the route, in order, with their kinds, lengths and turns set. + * @param dynamics The physical limits of the robot. + * @param profile The profile of the run. + * @param start_speed The speed at the start of the first segment. + * @param end_speed The speed at the end of the last segment. + * @return The time the route takes in seconds. + */ + static float plan( + std::span route, const Dynamics& dynamics, const RunProfile& profile, float start_speed, + float end_speed + ); + + /** + * @brief Get the time a segment takes, with the speeds it already has. + * + * @note A racing line is timed by the line itself, and gets zero here. + * + * @param segment The segment. + * @param dynamics The physical limits of the robot. + * @param profile The profile of the run. + * @return The duration of the segment in seconds. + */ + static float get_duration(const Segment& segment, const Dynamics& dynamics, const RunProfile& profile); +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_VELOCITY_PLANNER_HPP diff --git a/micras_nav/include/micras/nav/wall_model.hpp b/micras_nav/include/micras/nav/wall_model.hpp new file mode 100644 index 0000000..e8d2780 --- /dev/null +++ b/micras_nav/include/micras/nav/wall_model.hpp @@ -0,0 +1,229 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_WALL_MODEL_HPP +#define MICRAS_NAV_WALL_MODEL_HPP + +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/robot_model.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief What the optical axis of a wall sensor meets. + * + * @details The wall is the first one along the axis that is not known to be absent, so its state is + * either a wall or unknown. The range is the distance from the sensor to the face of that wall, the + * offset is where along the wall the axis lands, measured from the end of the wall with the lowest + * coordinate, and the cosine is that of the angle between the axis and the perpendicular of the + * wall. The jacobian is the derivative of the range with respect to the x, y and orientation of + * the robot. The next three fields place the wall in the maze frame: whether it runs along y, the + * coordinate of its face across it, and the coordinate along it where the offset is measured from. + * The ends are the offsets between which the face is known to be one continuous surface: a known + * wall continues over its posts and over the known walls in line with it, while an unknown one is + * only considered between its posts. + * + * @note A hit is not valid when the axis meets nothing within range, or meets a post that has no + * known wall flush with it. + */ +struct RayHit { + bool valid; + GridPose wall; + WallState state; + float range; + float offset; + float cosine; + std::array jacobian; + bool vertical; + float face; + float base; + float low_end; + float high_end; +}; + +/** + * @brief Where the optical axis of a sensor crosses the plane of a wall, extended past its ends. + * + * @note The offset is measured along the wall like the one of a RayHit, the range is the distance + * from the sensor to the plane along the axis, the slope is how far the axis advances along the wall + * for each meter it closes on the plane, and the jacobian is the derivative of the offset with + * respect to the x, y and orientation of the robot. + */ +struct PlaneCrossing { + float offset; + float range; + float slope; + std::array jacobian; +}; + +/** + * @brief Geometry of the wall sensors against the map of the walls. + * + * @details It answers one question, what should a sensor see from a pose, for the two users that + * need it. The localizer compares the answer with the reading to correct the pose, and the wall + * observer compares it to decide whether a wall that is still unknown is there. Both only trust a + * reading when the whole spot the emitter illuminates, enlarged by how uncertain the pose is, lies + * on the face of a single wall: not on a post, not across the gap at the end of the wall. + */ +class WallModel { +public: + /** + * @brief Configuration struct for the wall model. + * + * @note A wall is only looked for between the two ranges, which are what the sensors measure + * well. The edge margin is kept between the illuminated spot and the ends of a wall, and the + * confidence is how many standard deviations of the pose the spot is enlarged by. + */ + struct Config { + RobotModel model; + float min_range; + float max_range; + float edge_margin; + float confidence; + }; + + /** + * @brief Construct a new Wall Model object. + * + * @param config The configuration for the wall model. + */ + explicit WallModel(const Config& config); + + /** + * @brief Find what the optical axis of a sensor meets. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + * @param pose The pose of the robot in the maze frame. + * @param sensor The index of the sensor. + * @param maze The map of the walls. + * @return The first wall along the axis that is not known to be absent. + */ + template + RayHit cast(const Pose& pose, uint8_t sensor, const TMaze& maze) const; + + /** + * @brief Find where the optical axis of a sensor crosses the plane of a wall it met before. + * + * @param pose The pose of the robot in the maze frame. + * @param sensor The index of the sensor. + * @param hit The hit that identified the wall. + * @return The crossing of the axis with the plane of the face of that wall. + */ + PlaneCrossing cross(const Pose& pose, uint8_t sensor, const RayHit& hit) const; + + /** + * @brief Check if a sensor looks far enough to the side to sweep along the walls beside the robot. + * + * @param sensor The index of the sensor. + * @return True if the sensor can see the ends of the side walls go by. + */ + bool is_side_looking(uint8_t sensor) const; + + /** + * @brief Check if the spot a sensor illuminates lies entirely on the face of the wall it meets. + * + * @param hit What the axis of the sensor meets. + * @param sensor The index of the sensor. + * @param position_deviation The standard deviation of the position of the robot, in meters. + * @param orientation_deviation The standard deviation of the orientation of the robot, in radians. + * @return True if a reading of the sensor can only come from that wall. + */ + bool is_footprint_clear( + const RayHit& hit, uint8_t sensor, float position_deviation, float orientation_deviation + ) const; + + /** + * @brief Get the range a sensor reads with the robot centered in a cell, facing along it. + * + * @note This is the pose the sensors are calibrated in: the side sensors against the walls of + * a corridor and the front ones against a wall ahead. A side sensor points 45 degrees ahead and + * reaches the plane of the side walls past the cell the robot is in, so its range is the one to + * the side wall of the next cell: the corridor has to be two cells long, with no wall ahead. + * + * @param sensor The index of the sensor. + * @return The range to the plane of the wall the sensor points at. + */ + float get_centered_range(uint8_t sensor) const; + + /** + * @brief Get the range to a wall from the distance a sensor reads for it. + * + * @details A wall reflects diffusely, so the light a sensor gets back from it falls with the + * cosine of the angle between the axis and the perpendicular of the wall, and a reading is turned + * into a distance as if the wall were met at the angle of the calibration: square on for a front + * sensor and at 45 degrees for a diagonal one. Met at another angle, a wall reads as one nearer + * or farther by the square root of the ratio of the two cosines. A diagonal sensor that meets a + * wall square on, as it does all along a diagonal, reads it 16 % short. + * + * @param distance The distance read by the sensor. + * @param sensor The index of the sensor. + * @param hit What the axis of the sensor meets. + * @return The range to the face of the wall along the axis. + */ + float get_range(float distance, uint8_t sensor, const RayHit& hit) const; + + /** + * @brief Get the standard deviation of a range reading. + * + * @param range The range in meters. + * @return The standard deviation in meters. + */ + float get_range_deviation(float range) const; + +private: + /** + * @brief Sine of the angle of the optical axis from which a sensor counts as looking sideways. + */ + static constexpr float side_looking_sine{0.3F}; + + /** + * @brief Dimensions of the maze. + */ + RobotModel::Maze maze_geometry; + + /** + * @brief Mounting of each sensor. + */ + std::array sensors; + + /** + * @brief Constant part of the range noise. + */ + float range_noise; + + /** + * @brief Part of the range noise proportional to the range. + */ + float range_noise_per_meter; + + /** + * @brief Shortest range a wall is looked for at. + */ + float min_range; + + /** + * @brief Largest range a wall is looked for at. + */ + float max_range; + + /** + * @brief Distance kept between the illuminated spot and the ends of a wall. + */ + float edge_margin; + + /** + * @brief Number of standard deviations of the pose the spot is enlarged by. + */ + float confidence; +}; +} // namespace micras::nav + +#include "micras/nav/impl/wall_model.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_WALL_MODEL_HPP diff --git a/micras_nav/include/micras/nav/wall_observer.hpp b/micras_nav/include/micras/nav/wall_observer.hpp new file mode 100644 index 0000000..0c5eb71 --- /dev/null +++ b/micras_nav/include/micras/nav/wall_observer.hpp @@ -0,0 +1,110 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_WALL_OBSERVER_HPP +#define MICRAS_NAV_WALL_OBSERVER_HPP + +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/localizer.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/wall_model.hpp" + +namespace micras::nav { +/** + * @brief Decision of which walls exist, from the readings of the wall sensors. + * + * @details A reading only says something about a wall while the whole spot the emitter illuminates + * would lie on the face of that wall, allowing for how uncertain the pose is, which the wall model + * answers from the geometry of the sensors. Every reading taken in those conditions is a vote: + * for the wall when something is seen at the range the wall would be at, against it when nothing is + * seen up to beyond that range. A blind reading does not vote, since a receiver that ambient light + * saturates sees nothing whether the wall is there or not. A wall is decided when enough votes + * agree, and it is decided once. + * A wall that never gets enough votes stays unknown rather than being guessed. + * + * @note Every sensor votes on whatever wall it happens to point at, so the wall ahead is decided by + * the sensors that look forward as soon as it comes within their range, with no special case. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + */ +template +class TWallObserver { +public: + /** + * @brief Configuration struct for the wall observer. + * + * @note The tolerance on the range is a constant part plus a part proportional to the range. + * A wall can only be voted absent while it would be closer than the detection range, which has + * to be inside of what the sensors can measure. The range delay is the delay of the filter the + * fast ranges went through. + */ + struct Config { + float tolerance; + float relative_tolerance; + float detection_range; + float max_angular_speed; + float range_delay; + int8_t votes_to_decide; + }; + + /** + * @brief Construct a new TWallObserver object. + * + * @param config The configuration for the wall observer. + */ + explicit TWallObserver(const Config& config); + + /** + * @brief Forget every vote. + */ + void reset(); + + /** + * @brief Count the votes of the current readings and record the walls they decide. + * + * @param measurements The current measurements. + * @param localizer The pose estimate and its uncertainty. + * @param wall_model The geometry of the wall sensors. + * @param maze The map of the walls, which is updated. + * @return True if a wall was decided. + */ + bool update( + const Measurements& measurements, const Localizer& localizer, const WallModel& wall_model, + TMaze& maze + ); + +private: + /** + * @brief Number of walls of the maze, including the ones of the border. + */ + static constexpr uint16_t number_of_walls{(width + 1) * height + width * (height + 1)}; + + /** + * @brief Get the index of a wall. + * + * @param wall The cell and the side of it where the wall is. + * @return The index of the wall, which is the same from both of its sides. + */ + static constexpr uint16_t get_index(const GridPose& wall); + + /** + * @brief Parameters of the observer. + */ + Config config; + + /** + * @brief Votes for each wall minus votes against it. + */ + std::array votes{}; +}; +} // namespace micras::nav + +#include "micras/nav/impl/wall_observer.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_WALL_OBSERVER_HPP diff --git a/micras_nav/src/controller.cpp b/micras_nav/src/controller.cpp new file mode 100644 index 0000000..01c3150 --- /dev/null +++ b/micras_nav/src/controller.cpp @@ -0,0 +1,233 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/nav/controller.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Find the real roots of a quadratic, in the form that keeps its precision. + * + * @param quadratic The coefficient of the square. + * @param linear The coefficient of the variable. + * @param constant The constant term. + * @return The roots, not a number where there is none, and only the first one of a linear equation. + */ +static std::array solve_quadratic(float quadratic, float linear, float constant) { + constexpr float none = std::numeric_limits::quiet_NaN(); + + if (std::abs(quadratic) < 1.0e-9F) { + return {std::abs(linear) > 0.0F ? -constant / linear : none, none}; + } + + const float discriminant = linear * linear - 4.0F * quadratic * constant; + + if (discriminant < 0.0F) { + return {none, none}; + } + + const float half = -0.5F * (linear + std::copysign(std::sqrt(discriminant), linear)); + + return {half / quadratic, std::abs(half) > 0.0F ? constant / half : none}; +} + +Controller::Controller(const Config& config) : + config{config}, + linear_gains{compute_gains(config.linear, config.model.speed_constant(), config.model.acceleration_constant())}, + angular_gains{compute_gains( + config.angular, config.model.angular_speed_constant(), config.model.angular_acceleration_constant() + )} { } + +void Controller::reset() { + this->time_scale = 1.0F; + this->saturated = false; +} + +Controller::Command Controller::update(const Reference& unscaled, const State& estimate, float elapsed_time) { + const RobotModel& model = this->config.model; + + const float scale = this->find_next_time_scale(unscaled, elapsed_time); + const float scale_rate = (scale - this->time_scale) / elapsed_time; + + this->time_scale = scale; + + Reference reference = unscaled; + reference.twist.linear = scale * unscaled.twist.linear; + reference.twist.angular = scale * unscaled.twist.angular; + reference.acceleration.linear = scale * scale * unscaled.acceleration.linear + scale_rate * unscaled.twist.linear; + reference.acceleration.angular = + scale * scale * unscaled.acceleration.angular + scale_rate * unscaled.twist.angular; + + const float half_track = model.chassis.track_width / 2.0F; + const float left_speed = reference.twist.linear - reference.twist.angular * half_track; + const float right_speed = reference.twist.linear + reference.twist.angular * half_track; + + const float left_friction = std::clamp(left_speed / this->config.friction_speed, -1.0F, 1.0F); + const float right_friction = std::clamp(right_speed / this->config.friction_speed, -1.0F, 1.0F); + + const float forward_feed_forward = model.speed_constant() * reference.twist.linear + + model.acceleration_constant() * reference.acceleration.linear + + model.drive.static_friction_voltage * (right_friction + left_friction) / 2.0F; + + const float rotation_feed_forward = model.angular_speed_constant() * reference.twist.angular + + model.angular_acceleration_constant() * reference.acceleration.angular + + model.drive.static_friction_voltage * (right_friction - left_friction) / 2.0F; + + const Pose seen = reference.pose.relative(estimate.pose); + + const float along_error = + std::clamp(-seen.position.x, -this->config.linear.max_error, this->config.linear.max_error); + const float blend = std::clamp(reference.twist.linear / this->config.steering_blend_speed, -1.0F, 1.0F); + const float steering = + blend * std::clamp( + -this->config.steering_gain * seen.position.y, -this->config.max_steering, this->config.max_steering + ); + const float slide_angle = model.traction.lateral_compliance * reference.twist.angular; + const float slide_rate = model.traction.lateral_compliance * reference.acceleration.angular; + const float orientation_error = std::clamp( + -seen.orientation + slide_angle + steering, -this->config.angular.max_error, this->config.angular.max_error + ); + + const float forward_feedback = this->linear_gains.proportional * along_error + + this->linear_gains.derivative * (reference.twist.linear - estimate.velocity.linear); + + const float rotation_feedback = + this->angular_gains.proportional * orientation_error + + this->angular_gains.derivative * (reference.twist.angular + slide_rate - estimate.velocity.angular); + + this->status = { + .along_error = -seen.position.x, + .across_error = -seen.position.y, + .orientation_error = -seen.orientation, + .forward_feed_forward = forward_feed_forward, + .rotation_feed_forward = rotation_feed_forward, + .forward_feedback = forward_feedback, + .rotation_feedback = rotation_feedback, + }; + + const float to_percent = 100.0F / model.drive.supply_voltage; + + const Command command{ + .forward = to_percent * (forward_feed_forward + forward_feedback), + .rotation = to_percent * (rotation_feed_forward + rotation_feedback), + }; + + this->saturated = std::abs(command.forward) + std::abs(command.rotation) > 100.0F; + + return command; +} + +float Controller::find_next_time_scale(const Reference& reference, float elapsed_time) const { + const float half_track = this->config.model.chassis.track_width / 2.0F; + const float fastest_wheel = std::abs(reference.twist.linear) + std::abs(reference.twist.angular) * half_track; + const float largest_change = this->config.max_time_scale_acceleration * elapsed_time; + const float step = largest_change / std::max(fastest_wheel, largest_change); + const float target = this->find_time_scale(reference); + + if (target <= this->time_scale) { + return std::max(target, this->time_scale - step); + } + + if (this->saturated) { + return this->time_scale; + } + + const float highest = std::min(target, this->time_scale + step); + const float available = this->get_available_voltage(); + + float rate = (highest - this->time_scale) / elapsed_time; + + for (const std::array& wheel : this->get_wheel_terms(reference)) { + const float voltage = wheel.at(0) + highest * (wheel.at(1) + highest * wheel.at(2)); + const float rate_term = std::abs(wheel.at(3)); + + if (rate_term > 0.0F) { + rate = std::min(rate, std::max((available - std::copysign(1.0F, wheel.at(3)) * voltage) / rate_term, 0.0F)); + } + } + + return this->time_scale + rate * elapsed_time; +} + +float Controller::find_time_scale(const Reference& reference) const { + const std::array, 2> wheels = this->get_wheel_terms(reference); + const float available = this->get_available_voltage(); + + const auto fits = [&wheels, available](float scale) { + return std::ranges::all_of(wheels, [scale, available](const std::array& wheel) { + return std::abs(wheel.at(0) + scale * (wheel.at(1) + scale * wheel.at(2))) <= available * 1.0001F; + }); + }; + + if (fits(1.0F)) { + return 1.0F; + } + + float best = min_time_scale; + + for (const std::array& wheel : wheels) { + for (const float limit : {available, -available}) { + for (const float root : solve_quadratic(wheel.at(2), wheel.at(1), wheel.at(0) - limit)) { + if (root > best and root < 1.0F and fits(root)) { + best = root; + } + } + } + } + + return best; +} + +std::array, 2> Controller::get_wheel_terms(const Reference& reference) const { + const RobotModel& model = this->config.model; + + const float half_track = model.chassis.track_width / 2.0F; + + std::array, 2> wheels{}; + + for (uint8_t i = 0; i < 2; i++) { + const float side = i == 0 ? -1.0F : 1.0F; + const float speed = reference.twist.linear + side * reference.twist.angular * half_track; + + wheels.at(i) = { + model.drive.static_friction_voltage * std::clamp(speed / this->config.friction_speed, -1.0F, 1.0F), + model.speed_constant() * reference.twist.linear + + side * model.angular_speed_constant() * reference.twist.angular, + model.acceleration_constant() * reference.acceleration.linear + + side * model.angular_acceleration_constant() * reference.acceleration.angular, + model.acceleration_constant() * reference.twist.linear + + side * model.angular_acceleration_constant() * reference.twist.angular, + }; + } + + return wheels; +} + +float Controller::get_available_voltage() const { + return this->config.model.drive.supply_voltage * (1.0F - this->config.voltage_reserve); +} + +float Controller::get_time_scale() const { + return this->time_scale; +} + +const Controller::Status& Controller::get_status() const { + return this->status; +} + +Controller::Gains Controller::compute_gains(const Axis& axis, float speed_constant, float acceleration_constant) { + return { + .proportional = acceleration_constant * axis.natural_frequency * axis.natural_frequency, + .derivative = + std::max(2.0F * axis.damping * axis.natural_frequency * acceleration_constant - speed_constant, 0.0F), + }; +} +} // namespace micras::nav diff --git a/micras_nav/src/curve_speed.cpp b/micras_nav/src/curve_speed.cpp new file mode 100644 index 0000000..a21285d --- /dev/null +++ b/micras_nav/src/curve_speed.cpp @@ -0,0 +1,159 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include "micras/nav/curve_speed.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::nav { +namespace { +/** + * @brief Bending of a turn at its samples, as the passes of CurveSpeed read it. + */ +struct TurnBending { + /** + * @brief Get the curvature at a sample and the sharpness between it and the next one. + * + * @param index The index of the sample. + * @return The bending. + */ + Bending operator()(std::size_t index) const { + const auto position = static_cast(index); + const float middle = std::min(position + 0.5F, static_cast(this->intervals) - 0.5F); + + return { + .curvature = this->shape.bending_at(position * this->spacing).curvature, + .sharpness = this->shape.bending_at(middle * this->spacing).sharpness, + }; + } + + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) a view that lives for one call + const TurnShape& shape; + uint8_t intervals; + float spacing; +}; +} // namespace + +CurveSpeed::CurveSpeed(const TurnShape& shape, float start_speed, float end_speed, const CurveLimits& limits) : + intervals{get_intervals(shape)}, spacing{shape.length() / static_cast(get_intervals(shape))} { + const std::span speeds = std::span{this->speeds}.first(this->intervals + 1U); + + plan( + TurnBending{.shape = shape, .intervals = this->intervals, .spacing = this->spacing}, speeds, this->spacing, + start_speed, end_speed, limits + ); + integrate(speeds, this->spacing, std::span{this->times}.first(this->intervals + 1U)); +} + +float CurveSpeed::duration() const { + return this->times.at(this->intervals); +} + +SpeedProfile::Sample CurveSpeed::sample(float time) const { + return sample( + std::span{this->speeds}.first(this->intervals + 1U), std::span{this->times}.first(this->intervals + 1U), + this->spacing, time + ); +} + +float CurveSpeed::get_entry_speed(const TurnShape& shape, float end_speed, const CurveLimits& limits) { + const uint8_t intervals = get_intervals(shape); + const float spacing = shape.length() / static_cast(intervals); + const TurnBending bending{.shape = shape, .intervals = intervals, .spacing = spacing}; + + Bending after = bending(intervals); + float speed = std::min(end_speed, get_limit(bending(intervals - 1U), after, limits)); + + for (uint8_t i = intervals; i > 0; i--) { + const Bending at = bending(i - 1U); + const Bending before = bending(i > 1 ? i - 2U : 0U); + const float deceleration = + limits.get_deceleration(speed, {.curvature = after.curvature, .sharpness = at.sharpness}); + + speed = std::min(std::sqrt(speed * speed + 2.0F * spacing * deceleration), get_limit(before, at, limits)); + after = at; + } + + return speed; +} + +float CurveSpeed::get_exit_speed(const TurnShape& shape, float start_speed, const CurveLimits& limits) { + const uint8_t intervals = get_intervals(shape); + const float spacing = shape.length() / static_cast(intervals); + const TurnBending bending{.shape = shape, .intervals = intervals, .spacing = spacing}; + + Bending at = bending(0); + float speed = std::min(start_speed, get_limit(at, at, limits)); + + for (uint8_t i = 0; i < intervals; i++) { + const Bending next = bending(i + 1U); + const float acceleration = limits.get_acceleration(speed, at); + + speed = std::min(std::sqrt(speed * speed + 2.0F * spacing * acceleration), get_limit(at, next, limits)); + at = next; + } + + return speed; +} + +void CurveSpeed::integrate(std::span speeds, float spacing, std::span times) { + if (times.empty()) { + return; + } + + times.front() = 0.0F; + + auto speed = speeds.begin(); + + for (auto time = times.begin(); std::next(time) != times.end(); ++time, ++speed) { + *std::next(time) = *time + 2.0F * spacing / std::max(*speed + *std::next(speed), min_speed_sum); + } +} + +SpeedProfile::Sample + CurveSpeed::sample(std::span speeds, std::span times, float spacing, float time) { + if (times.size() < 2) { + return {.distance = 0.0F, .speed = speeds.empty() ? 0.0F : speeds.front(), .acceleration = 0.0F}; + } + + time = std::clamp(time, 0.0F, times.back()); + + const auto next = std::ranges::upper_bound(times, time); + std::size_t index = next == times.begin() ? 0 : static_cast(next - times.begin()) - 1; + index = std::min(index, times.size() - 2); + + const std::span interval_speeds = speeds.subspan(index, 2); + const std::span interval_times = times.subspan(index, 2); + const float start = interval_speeds.front(); + const float end = interval_speeds.back(); + const float acceleration = (end * end - start * start) / (2.0F * spacing); + const float elapsed = std::min(time - interval_times.front(), interval_times.back() - interval_times.front()); + + return { + .distance = static_cast(index) * spacing + + std::min(start * elapsed + acceleration * elapsed * elapsed / 2.0F, spacing), + .speed = start + acceleration * elapsed, + .acceleration = acceleration, + }; +} + +uint8_t CurveSpeed::get_intervals(const TurnShape& shape) { + const auto wanted = static_cast(std::ceil(shape.length() / turn_spacing)); + return static_cast(std::clamp(wanted, 1, max_intervals)); +} + +float CurveSpeed::get_limit(const Bending& before, const Bending& at, const CurveLimits& limits) { + return limits.get_speed_limit( + {.curvature = at.curvature, .sharpness = std::max(std::abs(before.sharpness), std::abs(at.sharpness))} + ); +} +} // namespace micras::nav diff --git a/micras_nav/src/drive_identification.cpp b/micras_nav/src/drive_identification.cpp new file mode 100644 index 0000000..1f1fa64 --- /dev/null +++ b/micras_nav/src/drive_identification.cpp @@ -0,0 +1,207 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/nav/controller.hpp" +#include "micras/nav/drive_identification.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/robot_model.hpp" + +namespace micras::nav { +DriveIdentification::DriveIdentification(const Config& config) : config{config} { } + +void DriveIdentification::start(const Measurements& measurements) { + this->phase = Phase::RAMP; + this->phase_time = 0.0F; + this->rest_left = this->config.rest_time; + this->last_left_angle = measurements.left_wheel_angle; + this->last_right_angle = measurements.right_wheel_angle; + this->distance = 0.0F; + this->breakaway_voltage = 0.0F; + this->linear_fit = {}; + this->angular_fit = {}; + this->valid = false; +} + +Controller::Command DriveIdentification::update(const Measurements& measurements, float elapsed_time) { + const RobotModel& model = this->config.model; + + const float radius = model.chassis.wheel_radius; + const float left = radius * (measurements.left_wheel_angle - this->last_left_angle); + const float right = radius * (measurements.right_wheel_angle - this->last_right_angle); + + this->last_left_angle = measurements.left_wheel_angle; + this->last_right_angle = measurements.right_wheel_angle; + + const float linear_speed = (left + right) / (2.0F * elapsed_time); + const float angular_speed = model.gyroscope_scale * measurements.angular_rate; + + this->distance += (left + right) / 2.0F; + + if (this->phase == Phase::FINISHED) { + return {}; + } + + if (this->rest_left > 0.0F) { + this->rest_left -= elapsed_time; + this->start_speed = this->phase == Phase::LEFT or this->phase == Phase::RIGHT ? angular_speed : linear_speed; + return {}; + } + + this->phase_time += elapsed_time; + + const float to_volts = model.drive.supply_voltage / 100.0F; + + if (this->phase == Phase::RAMP) { + const float command = this->config.ramp_rate * this->phase_time; + + if (std::abs(linear_speed) > this->config.breakaway_speed or command >= this->config.linear_command) { + this->breakaway_voltage = command * to_volts; + this->advance(); + return {}; + } + + return {.forward = command, .rotation = 0.0F}; + } + + const bool rotating = this->phase == Phase::LEFT or this->phase == Phase::RIGHT; + const bool positive = this->phase == Phase::FORWARD or this->phase == Phase::LEFT; + const float level = this->phase_time < this->config.step_time ? 1.0F : second_level; + const float magnitude = level * (rotating ? this->config.angular_command : this->config.linear_command); + const float command = positive ? magnitude : -magnitude; + const float speed = rotating ? angular_speed : linear_speed; + + this->voltage_integral += command * to_volts * elapsed_time; + this->sign_integral += (speed != 0.0F ? std::copysign(1.0F, speed) : 0.0F) * elapsed_time; + this->position += speed * elapsed_time; + + (rotating ? this->angular_fit : this->linear_fit) + .add({this->sign_integral, this->position, speed - this->start_speed}, this->voltage_integral); + + const float coast_end = this->distance + linear_speed * model.acceleration_constant() / model.speed_constant(); + const bool too_far = not rotating and (positive ? coast_end >= this->config.max_distance : coast_end <= 0.0F); + + if (this->phase_time >= 2.0F * this->config.step_time or too_far) { + this->advance(); + return {}; + } + + return rotating ? Controller::Command{.forward = 0.0F, .rotation = command} : + Controller::Command{.forward = command, .rotation = 0.0F}; +} + +bool DriveIdentification::is_finished() const { + return this->phase == Phase::FINISHED; +} + +bool DriveIdentification::is_valid() const { + return this->valid; +} + +float DriveIdentification::get_breakaway_voltage() const { + return this->breakaway_voltage; +} + +const DriveIdentification::Axis& DriveIdentification::get_linear() const { + return this->linear; +} + +const DriveIdentification::Axis& DriveIdentification::get_angular() const { + return this->angular; +} + +RobotModel DriveIdentification::get_model() const { + RobotModel model = this->config.model; + + const RobotModel::Chassis& chassis = model.chassis; + + model.drive.static_friction_voltage = this->linear.static_friction; + model.drive.torque_constant = this->linear.speed_constant * chassis.wheel_radius / model.drive.gear_ratio; + model.drive.resistance = this->linear.acceleration_constant * 2.0F * model.drive.gear_ratio * + model.drive.torque_constant / (chassis.wheel_radius * chassis.mass); + model.chassis.yaw_inertia = this->angular.acceleration_constant * model.drive.gear_ratio * + model.drive.torque_constant * chassis.track_width / + (chassis.wheel_radius * model.drive.resistance); + + return model; +} + +void DriveIdentification::Fit::add(const std::array& regressors, float response) { + for (uint8_t i = 0; i < 3; i++) { + for (uint8_t j = 0; j < 3; j++) { + this->normal.at(i).at(j) += static_cast(regressors.at(i)) * static_cast(regressors.at(j)); + } + + this->moment.at(i) += static_cast(regressors.at(i)) * static_cast(response); + } +} + +bool DriveIdentification::Fit::solve(Axis& axis) const { + std::array, 3> system{}; + + for (uint8_t i = 0; i < 3; i++) { + for (uint8_t j = 0; j < 3; j++) { + system.at(i).at(j) = this->normal.at(i).at(j); + } + + system.at(i).at(3) = this->moment.at(i); + } + + for (uint8_t column = 0; column < 3; column++) { + uint8_t pivot = column; + + for (uint8_t row = column + 1; row < 3; row++) { + if (std::abs(system.at(row).at(column)) > std::abs(system.at(pivot).at(column))) { + pivot = row; + } + } + + if (std::abs(system.at(pivot).at(column)) < 1.0e-12) { + return false; + } + + std::swap(system.at(column), system.at(pivot)); + + for (uint8_t row = 0; row < 3; row++) { + if (row == column) { + continue; + } + + const double factor = system.at(row).at(column) / system.at(column).at(column); + + for (uint8_t k = column; k < 4; k++) { + system.at(row).at(k) -= factor * system.at(column).at(k); + } + } + } + + axis = { + .static_friction = static_cast(system.at(0).at(3) / system.at(0).at(0)), + .speed_constant = static_cast(system.at(1).at(3) / system.at(1).at(1)), + .acceleration_constant = static_cast(system.at(2).at(3) / system.at(2).at(2)), + }; + + return axis.speed_constant > 0.0F and axis.acceleration_constant > 0.0F; +} + +void DriveIdentification::advance() { + this->phase = static_cast(static_cast(this->phase) + 1); + this->phase_time = 0.0F; + this->rest_left = this->config.rest_time; + this->voltage_integral = 0.0F; + this->sign_integral = 0.0F; + this->position = 0.0F; + + if (this->phase == Phase::FINISHED) { + const bool linear_found = this->linear_fit.solve(this->linear); + const bool angular_found = this->angular_fit.solve(this->angular); + + this->valid = linear_found and angular_found; + } +} +} // namespace micras::nav diff --git a/micras_nav/src/executor.cpp b/micras_nav/src/executor.cpp new file mode 100644 index 0000000..fe75e79 --- /dev/null +++ b/micras_nav/src/executor.cpp @@ -0,0 +1,258 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/nav/curve_speed.hpp" +#include "micras/nav/executor.hpp" +#include "micras/nav/line.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/state.hpp" +#include "micras/nav/turn_table.hpp" +#include "micras/nav/velocity_planner.hpp" + +namespace micras::nav { +Executor::Executor(const Dynamics& dynamics, const Line& line, const Config& config) : + dynamics{dynamics}, line{line}, config{config} { + this->segments.reserve(config.capacity); + this->durations.reserve(config.capacity); +} + +void Executor::reset(const Pose& pose, const RunProfile& profile) { + this->run_profile = profile; + this->segments.clear(); + this->durations.clear(); + this->index = 0; + this->clock = 0.0F; + this->duration = 0.0F; + this->queued_time = 0.0F; + this->settled_time = 0.0F; + this->reference = {.pose = pose, .twist = {}, .acceleration = {}, .distance = 0.0F}; +} + +void Executor::push(std::span segments) { + const bool was_finished = this->is_finished(); + + if (was_finished) { + this->segments.clear(); + this->durations.clear(); + this->index = 0; + this->queued_time = 0.0F; + } else { + const auto done = static_cast(this->index); + + this->segments.erase(this->segments.begin(), this->segments.begin() + done); + this->durations.erase(this->durations.begin(), this->durations.begin() + done); + this->index = 0; + } + + for (const Segment& segment : segments) { + this->segments.push_back(segment); + this->durations.push_back( + segment.kind == SegmentKind::LINE ? + this->line.duration() : + VelocityPlanner::get_duration(segment, this->dynamics, this->run_profile) + ); + this->queued_time += this->durations.back(); + } + + if (was_finished and not this->segments.empty()) { + this->start_next(); + } +} + +Reference Executor::update(float elapsed_time, float time_scale, const State& estimate) { + if (this->is_finished()) { + return this->reference; + } + + this->clock += elapsed_time * time_scale; + + while (not this->is_finished()) { + const bool attaching = this->segments.at(this->index).kind == SegmentKind::ATTACH; + + if (this->clock < this->duration and not(attaching and this->has_settled(estimate, elapsed_time))) { + break; + } + + const float carried = attaching ? 0.0F : this->clock - this->duration; + + this->clock = this->duration; + this->reference = this->evaluate(); + this->index++; + + if (this->is_finished()) { + this->reference.twist = {}; + this->reference.acceleration = {}; + return this->reference; + } + + this->start_next(); + this->clock = carried; + } + + this->reference = this->evaluate(); + + return this->reference; +} + +bool Executor::is_finished() const { + return this->index >= this->segments.size(); +} + +bool Executor::is_ending(float horizon) const { + if (this->is_finished()) { + return true; + } + + return this->clock + horizon >= this->duration + this->queued_time; +} + +void Executor::divert(std::span segments) { + this->segments.resize(std::min(this->index, this->segments.size())); + this->durations.resize(this->segments.size()); + this->index = this->segments.size(); + this->duration = 0.0F; + this->clock = 0.0F; + this->queued_time = 0.0F; + this->push(segments); +} + +const Segment* Executor::get_current() const { + return this->is_finished() ? nullptr : &this->segments.at(this->index); +} + +const Reference& Executor::get_reference() const { + return this->reference; +} + +void Executor::start_next() { + const Segment& segment = this->segments.at(this->index); + + this->clock = 0.0F; + this->settled_time = 0.0F; + this->speed_profile = {}; + this->queued_time = std::max(this->queued_time - this->durations.at(this->index), 0.0F); + + switch (segment.kind) { + case SegmentKind::STRAIGHT: + this->speed_profile = SpeedProfile{ + std::abs(segment.length), segment.start_speed, segment.end_speed, + this->dynamics.get_linear_limits(this->run_profile).capped(segment.max_speed) + }; + this->duration = this->speed_profile.duration(); + break; + + case SegmentKind::SPIN: + this->speed_profile = SpeedProfile{ + std::abs(segment.length), 0.0F, 0.0F, this->dynamics.get_angular_limits(this->run_profile) + }; + this->duration = this->speed_profile.duration(); + break; + + case SegmentKind::TURN: + this->curve_speed = CurveSpeed{ + this->dynamics.get_turn(this->run_profile, segment.turn), segment.start_speed, segment.end_speed, + this->dynamics.get_curve_limits(this->run_profile) + }; + this->duration = this->curve_speed.duration(); + break; + + default: + this->duration = this->durations.at(this->index); + break; + } +} + +Reference Executor::evaluate() const { + const Segment& segment = this->segments.at(this->index); + const float side = std::copysign(1.0F, segment.length); + + switch (segment.kind) { + case SegmentKind::STRAIGHT: { + const SpeedProfile::Sample sample = this->speed_profile.sample(this->clock); + + return { + .pose = + segment.start.compose({.position = {.x = side * sample.distance, .y = 0.0F}, .orientation = 0.0F}), + .twist = {.linear = side * sample.speed, .angular = 0.0F}, + .acceleration = {.linear = side * sample.acceleration, .angular = 0.0F}, + .distance = sample.distance, + }; + } + + case SegmentKind::TURN: { + const TurnShape& shape = this->dynamics.get_turn(this->run_profile, segment.turn); + const SpeedProfile::Sample motion = this->curve_speed.sample(this->clock); + const auto point = shape.sample(motion.distance); + + return { + .pose = segment.start.compose( + {.position = {.x = point.x, .y = side * point.y}, .orientation = side * point.heading} + ), + .twist = {.linear = motion.speed, .angular = side * point.curvature * motion.speed}, + .acceleration = + { + .linear = motion.acceleration, + .angular = side * (point.sharpness * motion.speed * motion.speed + + point.curvature * motion.acceleration), + }, + .distance = motion.distance, + }; + } + + case SegmentKind::LINE: { + const SpeedProfile::Sample motion = this->line.sample_motion(this->clock); + const Line::Point point = this->line.sample_point(motion.distance); + + return { + .pose = point.pose, + .twist = {.linear = motion.speed, .angular = point.curvature * motion.speed}, + .acceleration = + { + .linear = motion.acceleration, + .angular = + point.sharpness * motion.speed * motion.speed + point.curvature * motion.acceleration, + }, + .distance = motion.distance, + }; + } + + case SegmentKind::SPIN: { + const SpeedProfile::Sample sample = this->speed_profile.sample(this->clock); + + return { + .pose = segment.start.compose({.position = {}, .orientation = side * sample.distance}), + .twist = {.linear = 0.0F, .angular = side * sample.speed}, + .acceleration = {.linear = 0.0F, .angular = side * sample.acceleration}, + .distance = sample.distance, + }; + } + + case SegmentKind::STOP: + case SegmentKind::ATTACH: + break; + } + + return {.pose = segment.start, .twist = {}, .acceleration = {}, .distance = 0.0F}; +} + +bool Executor::has_settled(const State& estimate, float elapsed_time) { + const Pose error = this->segments.at(this->index).start.relative(estimate.pose); + + const bool within_tolerance = error.position.magnitude() < this->config.settle_distance and + std::abs(error.orientation) < this->config.settle_angle and + std::abs(estimate.velocity.linear) < this->config.settle_linear_speed and + std::abs(estimate.velocity.angular) < this->config.settle_angular_speed; + + this->settled_time = within_tolerance ? this->settled_time + elapsed_time : 0.0F; + + return this->settled_time >= this->config.settle_time; +} +} // namespace micras::nav diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp new file mode 100644 index 0000000..3242504 --- /dev/null +++ b/micras_nav/src/grid_pose.cpp @@ -0,0 +1,115 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/core/vector.hpp" +#include "micras/nav/grid_pose.hpp" + +namespace micras::nav { +Side angle_to_grid(float angle) { + const auto grid_angle = std::lround(2.0F * angle / std::numbers::pi_v); + return static_cast(grid_angle < 0 ? (4 + (grid_angle % 4)) % 4 : grid_angle % 4); +} + +Side GridPoint::direction(const GridPoint& next) const { + if (next.x > this->x) { + return Side::RIGHT; + } + + if (next.y > this->y) { + return Side::UP; + } + + if (next.x < this->x) { + return Side::LEFT; + } + + if (next.y < this->y) { + return Side::DOWN; + } + + return Side::UP; +} + +GridPoint GridPoint::from_vector(const core::Vector& point, float cell_size) { + const auto to_index = [cell_size](float coordinate) { + return static_cast(std::clamp(std::floor(coordinate / cell_size), 0.0F, 255.0F)); + }; + + return {.x = to_index(point.x), .y = to_index(point.y)}; +} + +core::Vector GridPoint::to_vector(float cell_size) const { + return {.x = cell_size * (this->x + 0.5F), .y = cell_size * (this->y + 0.5F)}; +} + +GridPoint GridPoint::operator+(const Side& side) const { + switch (side) { + case Side::RIGHT: + return {.x = static_cast(this->x + 1), .y = this->y}; + case Side::UP: + return {.x = this->x, .y = static_cast(this->y + 1)}; + case Side::LEFT: + return {.x = static_cast(this->x - 1), .y = this->y}; + case Side::DOWN: + return {.x = this->x, .y = static_cast(this->y - 1)}; + } + + return *this; +} + +bool GridPoint::operator==(const GridPoint& other) const { + return this->x == other.x and this->y == other.y; +} + +GridPose GridPose::front() const { + return {.position = this->position + this->orientation, .orientation = this->orientation}; +} + +GridPose GridPose::turned_back() const { + return { + .position = this->position, .orientation = static_cast((std::to_underlying(this->orientation) + 2) % 4) + }; +} + +GridPose GridPose::turned_left() const { + return { + .position = this->position, .orientation = static_cast((std::to_underlying(this->orientation) + 1) % 4) + }; +} + +GridPose GridPose::turned_right() const { + return { + .position = this->position, .orientation = static_cast((std::to_underlying(this->orientation) + 3) % 4) + }; +} + +Side GridPose::get_relative_side(const GridPoint& other) const { + if (((this->orientation == Side::RIGHT or this->orientation == Side::LEFT) and this->position.y == other.y) or + ((this->orientation == Side::UP or this->orientation == Side::DOWN) and this->position.x == other.x)) { + return this->position.direction(other) == this->orientation ? Side::UP : Side::DOWN; + } + + switch (this->orientation) { + case Side::RIGHT: + return this->position.y < other.y ? Side::LEFT : Side::RIGHT; + case Side::UP: + return this->position.x > other.x ? Side::LEFT : Side::RIGHT; + case Side::LEFT: + return this->position.y > other.y ? Side::LEFT : Side::RIGHT; + case Side::DOWN: + default: + return this->position.x < other.x ? Side::LEFT : Side::RIGHT; + } +} + +bool GridPose::operator==(const GridPose& other) const { + return this->position == other.position and this->orientation == other.orientation; +} +} // namespace micras::nav diff --git a/micras_nav/src/gyroscope_calibration.cpp b/micras_nav/src/gyroscope_calibration.cpp new file mode 100644 index 0000000..44318d9 --- /dev/null +++ b/micras_nav/src/gyroscope_calibration.cpp @@ -0,0 +1,129 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/core/vector.hpp" +#include "micras/nav/gyroscope_calibration.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/robot_model.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +GyroscopeCalibration::GyroscopeCalibration(const Config& config) : config{config} { } + +void GyroscopeCalibration::start(const Pose& pose, const MotionLimits& limits) { + this->phase = Phase::BEFORE; + this->phase_time = 0.0F; + this->pose = pose; + this->spin = SpeedProfile{2.0F * std::numbers::pi_v * this->config.turns, 0.0F, 0.0F, limits}; + this->angle_sum = 0.0F; + this->angle_count = 0; + this->raw_rotation = 0.0F; + this->turning_time = 0.0F; + this->valid = true; +} + +Reference GyroscopeCalibration::update(const Measurements& measurements, float bias, float elapsed_time) { + Reference reference{.pose = this->pose, .twist = {}, .acceleration = {}, .distance = 0.0F}; + + if (this->phase == Phase::FINISHED) { + return reference; + } + + this->phase_time += elapsed_time; + + if (this->phase == Phase::TURNING) { + const SpeedProfile::Sample sample = this->spin.sample(this->phase_time); + + this->raw_rotation += measurements.angular_rate * elapsed_time; + this->turning_time += elapsed_time; + + reference.pose.orientation += sample.distance; + reference.twist.angular = sample.speed; + reference.acceleration.angular = sample.acceleration; + reference.distance = sample.distance; + + if (this->phase_time >= this->spin.duration()) { + this->phase = Phase::AFTER; + this->phase_time = 0.0F; + } + + return reference; + } + + if (this->phase_time >= this->config.settle_time / 2.0F) { + const WallReading& left = measurements.walls.at(this->config.left_sensor); + const WallReading& right = measurements.walls.at(this->config.right_sensor); + + if (left.valid and right.valid) { + this->angle_sum += this->get_wall_angle(measurements); + this->angle_count++; + } + } + + if (this->phase_time < this->config.settle_time) { + return reference; + } + + const float angle = this->angle_count > 0 ? this->angle_sum / static_cast(this->angle_count) : 0.0F; + + this->valid = this->valid and this->angle_count > 0; + this->angle_sum = 0.0F; + this->angle_count = 0; + this->phase_time = 0.0F; + + if (this->phase == Phase::BEFORE) { + this->angle_before = angle; + this->bias = bias; + this->phase = Phase::TURNING; + return reference; + } + + const float rotation = this->spin.distance() + angle - this->angle_before; + + if (std::abs(this->raw_rotation) > 0.0F) { + this->scale = (rotation + this->bias * this->turning_time) / this->raw_rotation; + } else { + this->valid = false; + } + + this->phase = Phase::FINISHED; + + return reference; +} + +bool GyroscopeCalibration::is_finished() const { + return this->phase == Phase::FINISHED; +} + +bool GyroscopeCalibration::is_valid() const { + return this->valid; +} + +float GyroscopeCalibration::get_scale() const { + return this->scale; +} + +float GyroscopeCalibration::get_wall_angle(const Measurements& measurements) const { + const auto get_point = [this, &measurements](uint8_t sensor) { + const RobotModel::WallSensor& mounting = this->config.model.wall_sensors.at(sensor); + const float range = measurements.walls.at(sensor).distance; + + return core::Vector{ + .x = mounting.position.x + range * std::cos(mounting.angle), + .y = mounting.position.y + range * std::sin(mounting.angle), + }; + }; + + const core::Vector along = get_point(this->config.left_sensor) - get_point(this->config.right_sensor); + + return std::atan2(along.x, along.y); +} +} // namespace micras::nav diff --git a/micras_nav/src/line.cpp b/micras_nav/src/line.cpp new file mode 100644 index 0000000..8e2ef50 --- /dev/null +++ b/micras_nav/src/line.cpp @@ -0,0 +1,83 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/core/math.hpp" +#include "micras/nav/curve_speed.hpp" +#include "micras/nav/line.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +bool Line::is_ready() const { + return this->ready; +} + +float Line::length() const { + return this->size < 2 ? 0.0F : static_cast(this->size - 1) * this->spacing; +} + +float Line::duration() const { + return this->size == 0 ? 0.0F : this->times.at(this->size - 1); +} + +float Line::get_finish_time() const { + return this->finish_time; +} + +Pose Line::get_start() const { + return {.position = {.x = this->xs.at(0), .y = this->ys.at(0)}, .orientation = this->get_heading(0)}; +} + +SpeedProfile::Sample Line::sample_motion(float time) const { + return CurveSpeed::sample( + std::span{this->speeds}.first(this->size), std::span{this->times}.first(this->size), this->spacing, time + ); +} + +Line::Point Line::sample_point(float distance) const { + const float position = std::clamp(distance / this->spacing, 0.0F, static_cast(this->size - 1)); + const auto index = static_cast(std::min(std::floor(position), static_cast(this->size - 2))); + const auto next = static_cast(index + 1); + const float fraction = position - static_cast(index); + + const float start_heading = this->get_heading(index); + const float turn = core::math::wrap_angle(this->get_heading(next) - start_heading); + + const float h00 = (1.0F + 2.0F * fraction) * (1.0F - fraction) * (1.0F - fraction); + const float h10 = fraction * (1.0F - fraction) * (1.0F - fraction) * this->spacing; + const float h01 = fraction * fraction * (3.0F - 2.0F * fraction); + const float h11 = fraction * fraction * (fraction - 1.0F) * this->spacing; + + const float start_curvature = this->curvatures.at(index); + const float end_curvature = this->curvatures.at(next); + + return { + .pose = + { + .position = + { + .x = h00 * this->xs.at(index) + h10 * std::cos(start_heading) + h01 * this->xs.at(next) + + h11 * std::cos(start_heading + turn), + .y = h00 * this->ys.at(index) + h10 * std::sin(start_heading) + h01 * this->ys.at(next) + + h11 * std::sin(start_heading + turn), + }, + .orientation = core::math::wrap_angle(start_heading + fraction * turn), + }, + .curvature = start_curvature + fraction * (end_curvature - start_curvature), + .sharpness = (end_curvature - start_curvature) / this->spacing, + }; +} + +float Line::get_heading(uint16_t index) const { + const uint16_t before = index == 0 ? 0 : index - 1; + const uint16_t after = std::min(index + 1, this->size - 1); + + return std::atan2(this->ys.at(after) - this->ys.at(before), this->xs.at(after) - this->xs.at(before)); +} +} // namespace micras::nav diff --git a/micras_nav/src/localizer.cpp b/micras_nav/src/localizer.cpp new file mode 100644 index 0000000..75768d2 --- /dev/null +++ b/micras_nav/src/localizer.cpp @@ -0,0 +1,377 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/core/utils.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/localizer.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +Localizer::Localizer(const Config& config) : config{config}, rolling_radius{config.model.rolling_radius(0.0F)} { + this->config.speed_window = std::clamp(config.speed_window, 1, max_speed_window); + + this->set_covariance({ + config.initial_position_deviation * config.initial_position_deviation, + config.initial_position_deviation * config.initial_position_deviation, + config.initial_orientation_deviation * config.initial_orientation_deviation, + config.initial_bias_deviation * config.initial_bias_deviation, + }); +} + +void Localizer::reset(const Pose& pose, const Measurements& measurements) { + const float bias_variance = this->get_variance(bias_index); + + this->state = {.pose = pose, .velocity = {}}; + this->last_left_angle = measurements.left_wheel_angle; + this->last_right_angle = measurements.right_wheel_angle; + + this->distances.fill(0.0F); + this->durations.fill(0.0F); + this->window_distance = 0.0F; + this->window_duration = 0.0F; + this->status = {}; + this->rejection_streak = 0; + + this->set_covariance({ + this->config.initial_position_deviation * this->config.initial_position_deviation, + this->config.initial_position_deviation * this->config.initial_position_deviation, + this->config.initial_orientation_deviation * this->config.initial_orientation_deviation, + bias_variance, + }); +} + +void Localizer::predict(const Measurements& measurements, float elapsed_time) { + const float radius = this->rolling_radius; + + const float left_distance = radius * (measurements.left_wheel_angle - this->last_left_angle); + const float right_distance = radius * (measurements.right_wheel_angle - this->last_right_angle); + + this->last_left_angle = measurements.left_wheel_angle; + this->last_right_angle = measurements.right_wheel_angle; + + const float distance = (left_distance + right_distance) / 2.0F; + const float rate = this->config.model.gyroscope_scale * measurements.angular_rate - this->bias; + const float rotation = rate * elapsed_time; + + const float half_rotation = rotation / 2.0F; + const float squared = half_rotation * half_rotation; + const float chord = distance * (1.0F - squared / 6.0F + squared * squared / 120.0F); + + const float heading = this->state.pose.orientation + half_rotation; + const float cosine = std::cos(heading); + const float sine = std::sin(heading); + + const float slide = -this->config.model.traction.lateral_compliance * distance * rate; + + this->state.pose.position.x += chord * cosine - slide * sine; + this->state.pose.position.y += chord * sine + slide * cosine; + this->state.pose.orientation = core::assert_angle(this->state.pose.orientation + rotation); + + this->window_distance += distance - this->distances.at(this->window_index); + this->window_duration += elapsed_time - this->durations.at(this->window_index); + this->distances.at(this->window_index) = distance; + this->durations.at(this->window_index) = elapsed_time; + this->window_index = static_cast((this->window_index + 1) % this->config.speed_window); + + this->state.velocity = { + .linear = this->window_duration > 0.0F ? this->window_distance / this->window_duration : 0.0F, + .angular = rate, + }; + + Matrix transition{}; + + for (uint8_t i = 0; i < number_of_states; i++) { + transition.at(i).at(i) = 1.0F; + } + + transition.at(x_index).at(orientation_index) = -chord * sine; + transition.at(y_index).at(orientation_index) = chord * cosine; + transition.at(x_index).at(bias_index) = chord * sine * elapsed_time / 2.0F; + transition.at(y_index).at(bias_index) = -chord * cosine * elapsed_time / 2.0F; + transition.at(orientation_index).at(bias_index) = -elapsed_time; + + Matrix noise_input{}; + noise_input.at(x_index) = {cosine, -sine, 0.0F, 0.0F}; + noise_input.at(y_index) = {sine, cosine, 0.0F, 0.0F}; + noise_input.at(orientation_index) = {0.0F, 0.0F, 1.0F, 0.0F}; + noise_input.at(bias_index) = {0.0F, 0.0F, 0.0F, 1.0F}; + + const RobotModel::Noise& noise = this->config.model.noise; + + const float traveled = std::abs(distance); + const float longitudinal = + noise.longitudinal_slip + noise.longitudinal_slip_per_acceleration * std::abs(measurements.acceleration.x); + + this->time_since_imu += elapsed_time; + float orientation_variance = 0.0F; + + if (measurements.imu_is_new) { + orientation_variance = noise.gyroscope * noise.gyroscope * this->time_since_imu; + this->time_since_imu = 0.0F; + } + + this->propagate( + transition, noise_input, + { + longitudinal * longitudinal * traveled, + noise.lateral_slip * noise.lateral_slip * traveled, + orientation_variance, + noise.gyroscope_bias_walk * noise.gyroscope_bias_walk * elapsed_time, + } + ); +} + +void Localizer::correct_at_rest(const Measurements& measurements, float elapsed_time) { + const float wheel_difference = measurements.right_wheel_angle - measurements.left_wheel_angle; + + if (std::abs(this->state.velocity.linear) > this->config.stationary_linear_speed) { + this->rest_time = 0.0F; + return; + } + + if (this->rest_time <= 0.0F) { + this->rest_rotation = 0.0F; + this->rest_wheel_difference = wheel_difference; + } + + this->rest_time += elapsed_time; + this->rest_rotation += this->config.model.gyroscope_scale * measurements.angular_rate * elapsed_time; + + if (this->rest_time < this->config.rest_window) { + return; + } + + const RobotModel& model = this->config.model; + + const float wheel_to_rotation = model.chassis.wheel_radius / model.chassis.track_width; + const float wheel_rotation = wheel_to_rotation * (wheel_difference - this->rest_wheel_difference); + const float measured_bias = (this->rest_rotation - wheel_rotation) / this->rest_time; + + const float gyroscope_deviation = model.noise.gyroscope / std::sqrt(this->rest_time); + const float wheel_deviation = 2.0F * wheel_to_rotation * model.noise.wheel_angle / this->rest_time; + + this->update( + measured_bias - this->bias, {0.0F, 0.0F, 0.0F, 1.0F}, + gyroscope_deviation * gyroscope_deviation + wheel_deviation * wheel_deviation, this->config.stationary_gate, + this->config.max_position_correction + ); + + this->rest_time = 0.0F; +} + +const State& Localizer::get_state() const { + return this->state; +} + +const Pose& Localizer::get_pose() const { + return this->state.pose; +} + +GridPose Localizer::get_cell() const { + return this->state.pose.to_grid(this->config.model.maze.cell_size); +} + +float Localizer::get_gyroscope_bias() const { + return this->bias; +} + +float Localizer::get_position_deviation() const { + return std::sqrt(std::max(this->get_variance(x_index), this->get_variance(y_index))); +} + +float Localizer::get_orientation_deviation() const { + return std::sqrt(this->get_variance(orientation_index)); +} + +const Localizer::Status& Localizer::get_status() const { + return this->status; +} + +void Localizer::set_covariance(const Vector& variances) { + this->upper = {}; + + for (uint8_t i = 0; i < number_of_states; i++) { + this->upper.at(i).at(i) = 1.0F; + } + + this->diagonal = variances; +} + +void Localizer::propagate(const Matrix& transition, const Matrix& noise_input, const Vector& noise_variances) { + static constexpr uint8_t columns{2 * number_of_states}; + + std::array, number_of_states> work{}; + std::array weights{}; + + for (uint8_t i = 0; i < number_of_states; i++) { + for (uint8_t j = 0; j < number_of_states; j++) { + float sum = 0.0F; + + for (uint8_t k = 0; k <= j; k++) { + sum += transition.at(i).at(k) * this->upper.at(k).at(j); + } + + work.at(i).at(j) = sum; + work.at(i).at(number_of_states + j) = noise_input.at(i).at(j); + } + + weights.at(i) = this->diagonal.at(i); + weights.at(number_of_states + i) = noise_variances.at(i); + } + + for (uint8_t j = number_of_states; j-- > 0;) { + float sum = 0.0F; + + for (uint8_t k = 0; k < columns; k++) { + sum += work.at(j).at(k) * work.at(j).at(k) * weights.at(k); + } + + this->diagonal.at(j) = sum; + + for (uint8_t i = 0; i < j; i++) { + float cross = 0.0F; + + for (uint8_t k = 0; k < columns; k++) { + cross += work.at(i).at(k) * work.at(j).at(k) * weights.at(k); + } + + const float factor = sum > 0.0F ? cross / sum : 0.0F; + this->upper.at(i).at(j) = factor; + + for (uint8_t k = 0; k < columns; k++) { + work.at(i).at(k) -= factor * work.at(j).at(k); + } + } + } +} + +void Localizer::count_range(bool accepted, bool stationary) { + if (accepted or stationary) { + this->rejection_streak = 0; + return; + } + + this->rejection_streak++; + + if (this->rejection_streak < this->config.recovery_rejections) { + return; + } + + Matrix identity{}; + + for (uint8_t i = 0; i < number_of_states; i++) { + identity.at(i).at(i) = 1.0F; + } + + const float variance = this->config.recovery_deviation * this->config.recovery_deviation; + + this->propagate(identity, identity, {variance, variance, 0.0F, 0.0F}); + this->rejection_streak = 0; + this->status.recoveries++; +} + +bool Localizer::update( + float innovation, const Vector& jacobian, float variance, float gate, float max_position_correction +) { + Vector projected{}; + Vector scaled{}; + + float innovation_variance = variance; + + for (uint8_t j = 0; j < number_of_states; j++) { + float sum = jacobian.at(j); + + for (uint8_t i = 0; i < j; i++) { + sum += this->upper.at(i).at(j) * jacobian.at(i); + } + + projected.at(j) = sum; + scaled.at(j) = this->diagonal.at(j) * sum; + innovation_variance += sum * scaled.at(j); + } + + const float normalized = innovation * innovation / innovation_variance; + + if (normalized > gate) { + this->status.rejected++; + return false; + } + + this->status.accepted++; + this->status.innovation_level += 0.01F * (normalized - this->status.innovation_level); + + Vector cross{}; + + for (uint8_t i = 0; i < number_of_states; i++) { + float sum = scaled.at(i); + + for (uint8_t j = i + 1; j < number_of_states; j++) { + sum += this->upper.at(i).at(j) * scaled.at(j); + } + + cross.at(i) = sum; + } + + const float position_step = std::hypot(cross.at(x_index), cross.at(y_index)) * std::abs(innovation); + const float orientation_step = std::abs(cross.at(orientation_index) * innovation); + + const float required = std::max( + {innovation_variance, position_step / max_position_correction, + orientation_step / this->config.max_orientation_correction} + ); + + float alpha = variance + (required - innovation_variance); + Vector gain{}; + + for (uint8_t j = 0; j < number_of_states; j++) { + const float beta = alpha; + alpha += projected.at(j) * scaled.at(j); + + const float lambda = -projected.at(j) / beta; + this->diagonal.at(j) *= beta / alpha; + gain.at(j) = scaled.at(j); + + for (uint8_t i = 0; i < j; i++) { + const float previous = this->upper.at(i).at(j); + this->upper.at(i).at(j) = previous + gain.at(i) * lambda; + gain.at(i) += scaled.at(j) * previous; + } + } + + const float scale = innovation / alpha; + + this->state.pose.position.x += gain.at(x_index) * scale; + this->state.pose.position.y += gain.at(y_index) * scale; + this->state.pose.orientation = + core::assert_angle(this->state.pose.orientation + gain.at(orientation_index) * scale); + this->bias += gain.at(bias_index) * scale; + + return true; +} + +void Localizer::set_downforce(float downforce) { + this->rolling_radius = this->config.model.rolling_radius(downforce); +} + +float Localizer::get_variance(uint8_t index) const { + float variance = this->diagonal.at(index); + + for (uint8_t j = index + 1; j < number_of_states; j++) { + variance += this->upper.at(index).at(j) * this->upper.at(index).at(j) * this->diagonal.at(j); + } + + return variance; +} + +bool Localizer::is_stationary() const { + return std::abs(this->state.velocity.linear) < this->config.stationary_linear_speed and + std::abs(this->state.velocity.angular) < this->config.stationary_angular_speed; +} +} // namespace micras::nav diff --git a/micras_nav/src/motion_limits.cpp b/micras_nav/src/motion_limits.cpp new file mode 100644 index 0000000..16ec3d2 --- /dev/null +++ b/micras_nav/src/motion_limits.cpp @@ -0,0 +1,142 @@ +/** + * @file + */ + +#include +#include + +#include "micras/nav/lattice.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::nav { +float MotionLimits::acceleration_at(float speed) const { + return std::min(this->acceleration, this->motor_acceleration * (1.0F - speed / this->motor_speed)); +} + +float MotionLimits::crossover_speed() const { + return this->motor_speed * (1.0F - this->acceleration / this->motor_acceleration); +} + +float MotionLimits::deceleration_at(float speed) const { + return std::min(this->deceleration, this->motor_acceleration * (1.0F + speed / this->motor_speed)); +} + +float MotionLimits::braking_crossover_speed() const { + return this->motor_speed * (this->deceleration / this->motor_acceleration - 1.0F); +} + +MotionLimits MotionLimits::capped(float speed) const { + MotionLimits limits = *this; + limits.max_speed = std::min(limits.max_speed, speed); + return limits; +} + +float CurveLimits::get_speed_limit(const Bending& bending) const { + const float curvature = std::abs(bending.curvature); + const float sharpness = std::abs(bending.sharpness); + + float limit = this->linear.max_speed; + + if (curvature > 0.0F) { + limit = std::min(limit, std::sqrt(this->lateral / curvature)); + } + + if (sharpness > 0.0F) { + limit = std::min(limit, std::sqrt(this->angular / sharpness)); + } + + return limit; +} + +float CurveLimits::get_acceleration(float speed, const Bending& bending) const { + const float spare = this->get_spare(speed, bending); + + if (spare <= 0.0F) { + return 0.0F; + } + + const float grip = spare / (1.0F / this->linear.acceleration + std::abs(bending.curvature) / this->angular); + + return std::max(std::min(grip, this->linear.acceleration_at(speed)), 0.0F); +} + +float CurveLimits::get_deceleration(float speed, const Bending& bending) const { + const float spare = this->get_spare(speed, bending); + + if (spare <= 0.0F) { + return 0.0F; + } + + const float grip = spare / (1.0F / this->linear.deceleration + std::abs(bending.curvature) / this->angular); + + return std::min(grip, this->linear.deceleration_at(speed)); +} + +float CurveLimits::get_spare(float speed, const Bending& bending) const { + const float lateral_use = speed * speed * std::abs(bending.curvature) / this->lateral; + const float angular_use = speed * speed * std::abs(bending.sharpness) / this->angular; + + return std::sqrt(std::max(1.0F - lateral_use * lateral_use, 0.0F)) - angular_use; +} + +Dynamics::Dynamics(const Config& config) : + model{config.model}, + turns{config.turns}, + risky_turns{config.risky_turns}, + max_linear_speed{config.max_linear_speed}, + max_angular_speed{config.max_angular_speed}, + available_voltage{ + config.model.drive.supply_voltage * (1.0F - config.voltage_reserve) - config.model.drive.static_friction_voltage + } { } + +MotionLimits Dynamics::get_linear_limits(const RunProfile& profile) const { + const float traction = profile.utilization * this->model.traction_acceleration(profile.fan); + const float free_speed = this->available_voltage / this->model.speed_constant(); + + return { + .max_speed = std::min({profile.max_speed, this->max_linear_speed, 0.95F * free_speed}), + .acceleration = traction, + .deceleration = traction, + .motor_acceleration = this->available_voltage / this->model.acceleration_constant(), + .motor_speed = free_speed, + }; +} + +MotionLimits Dynamics::get_angular_limits(const RunProfile& profile) const { + const float traction = profile.utilization * this->model.traction_angular_acceleration(profile.fan); + const float free_speed = this->available_voltage / this->model.angular_speed_constant(); + + return { + .max_speed = std::min(this->max_angular_speed, 0.95F * free_speed), + .acceleration = traction, + .deceleration = traction, + .motor_acceleration = this->available_voltage / this->model.angular_acceleration_constant(), + .motor_speed = free_speed, + }; +} + +CurveLimits Dynamics::get_curve_limits(const RunProfile& profile) const { + return { + .linear = this->get_linear_limits(profile), + .lateral = profile.utilization * this->model.traction_acceleration(profile.fan), + .angular = profile.utilization * this->model.traction_angular_acceleration(profile.fan), + }; +} + +const TurnShape& Dynamics::get_turn(const RunProfile& profile, TurnId turn) const { + return profile.risky ? this->risky_turns.get(turn) : this->turns.get(turn); +} + +float Dynamics::get_turn_speed(const RunProfile& profile, TurnId turn) const { + const TurnShape& shape = this->get_turn(profile, turn); + + return this->get_curve_limits(profile).get_speed_limit( + {.curvature = shape.curvature, .sharpness = shape.sharpness} + ); +} + +const RobotModel& Dynamics::get_model() const { + return this->model; +} +} // namespace micras::nav diff --git a/micras_nav/src/route_compiler.cpp b/micras_nav/src/route_compiler.cpp new file mode 100644 index 0000000..08e7c7f --- /dev/null +++ b/micras_nav/src/route_compiler.cpp @@ -0,0 +1,83 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/nav/lattice.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/route_compiler.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/state.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::nav { +void RouteCompiler::compile( + const Route& route, const Dynamics& dynamics, const RunProfile& profile, float start_distance, + std::vector& segments +) { + const float cell_size = dynamics.get_model().maze.cell_size; + + const auto retreat = [cell_size](const LatticePose& node, float distance) { + return node.to_pose(cell_size).compose({.position = {.x = -distance, .y = 0.0F}, .orientation = 0.0F}); + }; + + const auto emit_straight = [&segments](const Pose& start, float length) { + if (length > min_straight) { + segments.push_back({ + .kind = SegmentKind::STRAIGHT, + .turn = TurnId::SS90S, + .length = length, + .start_speed = 0.0F, + .end_speed = 0.0F, + .max_speed = std::numeric_limits::infinity(), + .start = start, + }); + } + }; + + segments.clear(); + + LatticePose node = route.start; + Pose straight_start = retreat(node, start_distance); + float straight_length = start_distance; + + for (const RouteStep& step : route.steps) { + const float step_length = node.is_diagonal() ? cell_size / std::numbers::sqrt2_v : cell_size; + + for (uint8_t i = 0; i < step.run; i++) { + node = node.advanced(); + } + + straight_length += static_cast(step.run) * step_length; + + if (not step.has_turn) { + continue; + } + + const TurnShape& shape = dynamics.get_turn(profile, step.turn); + + emit_straight(straight_start, straight_length + shape.pre); + + segments.push_back({ + .kind = SegmentKind::TURN, + .turn = step.turn, + .length = step.side == TurnSide::LEFT ? shape.length() : -shape.length(), + .start_speed = 0.0F, + .end_speed = 0.0F, + .max_speed = std::numeric_limits::infinity(), + .start = retreat(node, -shape.pre), + }); + + node = get_turn_exit(node, step.turn, step.side); + straight_start = retreat(node, shape.post); + straight_length = shape.post; + } + + emit_straight(straight_start, straight_length + route.stop_distance); +} +} // namespace micras::nav diff --git a/micras_nav/src/speed_profile.cpp b/micras_nav/src/speed_profile.cpp new file mode 100644 index 0000000..fa9f2e0 --- /dev/null +++ b/micras_nav/src/speed_profile.cpp @@ -0,0 +1,323 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/speed_profile.hpp" + +namespace micras::nav { +SpeedProfile::SpeedProfile(float distance, float start_speed, float end_speed, const MotionLimits& limits) : + total_distance{std::max(distance, 0.0F)}, last_speed{start_speed} { + if (this->total_distance <= 0.0F) { + return; + } + + const float braking_distance = get_braking_distance(start_speed, end_speed, limits); + + if (braking_distance >= this->total_distance) { + if (limits.braking_crossover_speed() <= end_speed) { + const float deceleration = + (start_speed * start_speed - end_speed * end_speed) / (2.0F * this->total_distance); + + this->append({.duration = (start_speed - end_speed) / deceleration, .acceleration = -deceleration}); + return; + } + + MotionLimits harder = limits; + harder.deceleration *= braking_distance / this->total_distance; + harder.motor_acceleration *= braking_distance / this->total_distance; + + this->append_braking(start_speed, end_speed, harder); + return; + } + + const float cap = std::max(limits.max_speed, start_speed); + float low = std::max(start_speed, end_speed); + float peak = cap; + + const auto excess = [&](float speed) { + return get_acceleration_distance(start_speed, speed, limits) + get_braking_distance(speed, end_speed, limits) - + this->total_distance; + }; + + if (excess(low) >= 0.0F) { + peak = get_reachable_speed(this->total_distance, start_speed, limits); + } else if (excess(cap) > 0.0F) { + float high = cap; + + for (uint8_t i = 0; i < 32; i++) { + const float middle = (low + high) / 2.0F; + + if (excess(middle) > 0.0F) { + high = middle; + } else { + low = middle; + } + } + + peak = low; + } + + const float crossover = limits.crossover_speed(); + const float traction_end = std::clamp(crossover, start_speed, peak); + + if (traction_end > start_speed) { + this->append( + {.duration = (traction_end - start_speed) / limits.acceleration, .acceleration = limits.acceleration} + ); + } + + if (peak > traction_end) { + const float rate = limits.motor_acceleration / limits.motor_speed; + + this->append({ + .duration = std::log((limits.motor_speed - traction_end) / (limits.motor_speed - peak)) / rate, + .rate = rate, + .target = limits.motor_speed, + }); + } + + const float final_speed = std::min(peak, end_speed); + const float braking = get_braking_distance(peak, final_speed, limits); + + float accelerated = 0.0F; + + if (this->size > 0) { + const Phase& last = this->phases.at(this->size - 1); + accelerated = evaluate(last, last.duration).distance; + } + + const float cruise = this->total_distance - accelerated - braking; + + if (cruise > 0.0F and peak > 0.0F) { + this->append({.duration = cruise / peak}); + } + + this->append_braking(peak, final_speed, limits); +} + +float SpeedProfile::duration() const { + return this->total_duration; +} + +float SpeedProfile::distance() const { + return this->total_distance; +} + +SpeedProfile::Sample SpeedProfile::sample(float time) const { + if (this->size == 0) { + return {.distance = this->total_distance, .speed = this->last_speed, .acceleration = 0.0F}; + } + + time = std::clamp(time, 0.0F, this->total_duration); + + for (uint8_t i = 0; i < this->size; i++) { + const Phase& phase = this->phases.at(i); + + if (time <= phase.duration or i + 1 == this->size) { + Sample result = evaluate(phase, std::min(time, phase.duration)); + result.distance = std::min(result.distance, this->total_distance); + return result; + } + + time -= phase.duration; + } + + return {.distance = this->total_distance, .speed = this->last_speed, .acceleration = 0.0F}; +} + +float SpeedProfile::time_at(float distance) const { + distance = std::clamp(distance, 0.0F, this->total_distance); + float elapsed = 0.0F; + + for (uint8_t i = 0; i < this->size; i++) { + const Phase& phase = this->phases.at(i); + const float end_distance = evaluate(phase, phase.duration).distance; + + if (distance > end_distance and i + 1 < this->size) { + elapsed += phase.duration; + continue; + } + + float low = 0.0F; + float high = phase.duration; + + for (uint8_t j = 0; j < 32; j++) { + const float middle = (low + high) / 2.0F; + + if (evaluate(phase, middle).distance < distance) { + low = middle; + } else { + high = middle; + } + } + + return elapsed + (low + high) / 2.0F; + } + + return elapsed; +} + +float SpeedProfile::get_reachable_speed(float distance, float start_speed, const MotionLimits& limits) { + if (distance <= 0.0F or start_speed >= limits.max_speed) { + return start_speed; + } + + if (get_acceleration_distance(start_speed, limits.max_speed, limits) <= distance) { + return limits.max_speed; + } + + const float crossover = limits.crossover_speed(); + const float unlimited = std::sqrt(start_speed * start_speed + 2.0F * limits.acceleration * distance); + + if (unlimited <= crossover) { + return unlimited; + } + + float low = start_speed; + float high = limits.max_speed; + + for (uint8_t i = 0; i < 32; i++) { + const float middle = (low + high) / 2.0F; + + if (get_acceleration_distance(start_speed, middle, limits) > distance) { + high = middle; + } else { + low = middle; + } + } + + return low; +} + +float SpeedProfile::get_brakeable_speed(float distance, float end_speed, const MotionLimits& limits) { + if (distance <= 0.0F) { + return end_speed; + } + + const float crossover = limits.braking_crossover_speed(); + + if (crossover <= end_speed or end_speed >= limits.max_speed) { + return std::min(limits.max_speed, std::sqrt(end_speed * end_speed + 2.0F * limits.deceleration * distance)); + } + + const float motor_distance = get_braking_distance(crossover, end_speed, limits); + + if (distance >= motor_distance) { + return std::min( + limits.max_speed, + std::sqrt(crossover * crossover + 2.0F * limits.deceleration * (distance - motor_distance)) + ); + } + + float speed = crossover; + + for (uint8_t i = 0; i < newton_steps; i++) { + speed -= (get_braking_distance(speed, end_speed, limits) - distance) * limits.deceleration_at(speed) / speed; + } + + return std::clamp(speed, end_speed, limits.max_speed); +} + +float SpeedProfile::get_braking_distance(float start_speed, float end_speed, const MotionLimits& limits) { + if (start_speed <= end_speed) { + return 0.0F; + } + + const float braking_end = std::clamp(limits.braking_crossover_speed(), end_speed, start_speed); + + float distance = (start_speed * start_speed - braking_end * braking_end) / (2.0F * limits.deceleration); + + if (braking_end > end_speed) { + const float rate = limits.motor_acceleration / limits.motor_speed; + const float span = braking_end - end_speed; + + distance += + std::max(span - limits.motor_speed * std::log1p(span / (limits.motor_speed + end_speed)), 0.0F) / rate; + } + + return distance; +} + +void SpeedProfile::append_braking(float start_speed, float end_speed, const MotionLimits& limits) { + const float braking_end = std::clamp(limits.braking_crossover_speed(), end_speed, start_speed); + + if (start_speed > braking_end) { + this->append( + {.duration = (start_speed - braking_end) / limits.deceleration, .acceleration = -limits.deceleration} + ); + } + + if (braking_end > end_speed) { + const float rate = limits.motor_acceleration / limits.motor_speed; + + this->append({ + .duration = std::log1p((braking_end - end_speed) / (limits.motor_speed + end_speed)) / rate, + .rate = rate, + .target = -limits.motor_speed, + }); + } +} + +float SpeedProfile::get_acceleration_distance(float start_speed, float end_speed, const MotionLimits& limits) { + if (end_speed <= start_speed) { + return 0.0F; + } + + const float traction_end = std::clamp(limits.crossover_speed(), start_speed, end_speed); + + float distance = (traction_end * traction_end - start_speed * start_speed) / (2.0F * limits.acceleration); + + if (end_speed > traction_end) { + const float rate = limits.motor_acceleration / limits.motor_speed; + + distance += + (limits.motor_speed * std::log((limits.motor_speed - traction_end) / (limits.motor_speed - end_speed)) - + (end_speed - traction_end)) / + rate; + } + + return distance; +} + +void SpeedProfile::append(Phase phase) { + if (phase.duration <= 0.0F or this->size == number_of_phases) { + return; + } + + if (this->size > 0) { + const Phase& last = this->phases.at(this->size - 1); + phase.start_distance = evaluate(last, last.duration).distance; + } + + phase.start_speed = this->last_speed; + + this->last_speed = evaluate(phase, phase.duration).speed; + this->total_duration += phase.duration; + this->phases.at(this->size) = phase; + this->size++; +} + +SpeedProfile::Sample SpeedProfile::evaluate(const Phase& phase, float time) { + if (phase.rate > 0.0F) { + const float gap = phase.target - phase.start_speed; + const float decay = std::exp(-phase.rate * time); + + return { + .distance = phase.start_distance + phase.target * time - gap * (1.0F - decay) / phase.rate, + .speed = phase.target - gap * decay, + .acceleration = phase.rate * gap * decay, + }; + } + + return { + .distance = phase.start_distance + phase.start_speed * time + phase.acceleration * time * time / 2.0F, + .speed = phase.start_speed + phase.acceleration * time, + .acceleration = phase.acceleration, + }; +} +} // namespace micras::nav diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp new file mode 100644 index 0000000..36b6fec --- /dev/null +++ b/micras_nav/src/state.cpp @@ -0,0 +1,41 @@ +/** + * @file + */ + +#include + +#include "micras/core/utils.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +GridPose Pose::to_grid(float cell_size) const { + return { + .position = GridPoint::from_vector(this->position, cell_size), .orientation = angle_to_grid(this->orientation) + }; +} + +Pose Pose::compose(const Pose& local) const { + const float cosine = std::cos(this->orientation); + const float sine = std::sin(this->orientation); + + return { + .position = + {.x = this->position.x + cosine * local.position.x - sine * local.position.y, + .y = this->position.y + sine * local.position.x + cosine * local.position.y}, + .orientation = this->orientation + local.orientation, + }; +} + +Pose Pose::relative(const Pose& other) const { + const float cosine = std::cos(this->orientation); + const float sine = std::sin(this->orientation); + + const core::Vector offset = other.position - this->position; + + return { + .position = {.x = cosine * offset.x + sine * offset.y, .y = -sine * offset.x + cosine * offset.y}, + .orientation = core::assert_angle(other.orientation - this->orientation), + }; +} +} // namespace micras::nav diff --git a/micras_nav/src/velocity_planner.cpp b/micras_nav/src/velocity_planner.cpp new file mode 100644 index 0000000..345a26f --- /dev/null +++ b/micras_nav/src/velocity_planner.cpp @@ -0,0 +1,129 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/nav/curve_speed.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/turn_table.hpp" +#include "micras/nav/velocity_planner.hpp" + +namespace micras::nav { +float VelocityPlanner::plan( + std::span route, const Dynamics& dynamics, const RunProfile& profile, float start_speed, float end_speed +) { + if (route.empty()) { + return 0.0F; + } + + const MotionLimits linear_limits = dynamics.get_linear_limits(profile); + const CurveLimits curve_limits = dynamics.get_curve_limits(profile); + + for (Segment& segment : route) { + segment.start_speed = 0.0F; + segment.end_speed = 0.0F; + + if (segment.kind == SegmentKind::STRAIGHT) { + segment.start_speed = linear_limits.capped(segment.max_speed).max_speed; + segment.end_speed = segment.start_speed; + } else if (segment.kind == SegmentKind::TURN) { + const TurnShape& shape = dynamics.get_turn(profile, segment.turn); + + segment.start_speed = curve_limits.get_speed_limit(shape.bending_at(0.0F)); + segment.end_speed = curve_limits.get_speed_limit(shape.bending_at(shape.length())); + } + } + + float next_speed = end_speed; + + for (Segment& segment : route | std::views::reverse) { + segment.end_speed = std::min(segment.end_speed, next_speed); + + if (segment.kind == SegmentKind::STRAIGHT) { + segment.start_speed = std::min( + segment.start_speed, + SpeedProfile::get_brakeable_speed( + std::abs(segment.length), segment.end_speed, linear_limits.capped(segment.max_speed) + ) + ); + } else if (segment.kind == SegmentKind::TURN) { + segment.start_speed = std::min( + segment.start_speed, + CurveSpeed::get_entry_speed(dynamics.get_turn(profile, segment.turn), segment.end_speed, curve_limits) + ); + } else { + segment.start_speed = std::min(segment.start_speed, segment.end_speed); + } + + next_speed = segment.start_speed; + } + + float previous_speed = start_speed; + + for (Segment& segment : route) { + segment.start_speed = std::min(segment.start_speed, previous_speed); + + if (segment.kind == SegmentKind::STRAIGHT) { + segment.end_speed = std::min( + segment.end_speed, + SpeedProfile::get_reachable_speed( + std::abs(segment.length), segment.start_speed, linear_limits.capped(segment.max_speed) + ) + ); + } else if (segment.kind == SegmentKind::TURN) { + segment.end_speed = std::min( + segment.end_speed, + CurveSpeed::get_exit_speed(dynamics.get_turn(profile, segment.turn), segment.start_speed, curve_limits) + ); + } else { + segment.end_speed = std::min(segment.end_speed, segment.start_speed); + } + + previous_speed = segment.end_speed; + } + + float total_time = 0.0F; + + for (const Segment& segment : route) { + total_time += get_duration(segment, dynamics, profile); + } + + return total_time; +} + +float VelocityPlanner::get_duration(const Segment& segment, const Dynamics& dynamics, const RunProfile& profile) { + switch (segment.kind) { + case SegmentKind::STRAIGHT: + return SpeedProfile{ + std::abs(segment.length), segment.start_speed, segment.end_speed, + dynamics.get_linear_limits(profile).capped(segment.max_speed) + } + .duration(); + + case SegmentKind::TURN: + return CurveSpeed{ + dynamics.get_turn(profile, segment.turn), segment.start_speed, segment.end_speed, + dynamics.get_curve_limits(profile) + } + .duration(); + + case SegmentKind::LINE: + return 0.0F; + + case SegmentKind::SPIN: + return SpeedProfile{std::abs(segment.length), 0.0F, 0.0F, dynamics.get_angular_limits(profile)}.duration(); + + case SegmentKind::STOP: + case SegmentKind::ATTACH: + return segment.length; + } + + return 0.0F; +} +} // namespace micras::nav diff --git a/micras_nav/src/wall_model.cpp b/micras_nav/src/wall_model.cpp new file mode 100644 index 0000000..d3f60d7 --- /dev/null +++ b/micras_nav/src/wall_model.cpp @@ -0,0 +1,96 @@ +/** + * @file + */ + +#include +#include + +#include "micras/nav/state.hpp" +#include "micras/nav/wall_model.hpp" + +namespace micras::nav { +WallModel::WallModel(const Config& config) : + maze_geometry{config.model.maze}, + sensors{config.model.wall_sensors}, + range_noise{config.model.noise.wall_range}, + range_noise_per_meter{config.model.noise.wall_range_per_meter}, + min_range{config.min_range}, + max_range{config.max_range}, + edge_margin{config.edge_margin}, + confidence{config.confidence} { } + +PlaneCrossing WallModel::cross(const Pose& pose, uint8_t sensor, const RayHit& hit) const { + const RobotModel::WallSensor& mounting = this->sensors.at(sensor); + + const Pose origin = pose.compose({.position = mounting.position, .orientation = mounting.angle}); + const float direction_x = std::cos(origin.orientation); + const float direction_y = std::sin(origin.orientation); + + const float across = hit.vertical ? direction_x : direction_y; + const float along = hit.vertical ? direction_y : direction_x; + const float slope = along / across; + + const float gap = hit.face - (hit.vertical ? origin.position.x : origin.position.y); + const float start = hit.vertical ? origin.position.y : origin.position.x; + + const float lever_x = origin.position.x - pose.position.x; + const float lever_y = origin.position.y - pose.position.y; + const float swing = gap / (across * across); + + PlaneCrossing crossing{ + .offset = start + gap * slope - hit.base, .range = gap / across, .slope = slope, .jacobian = {} + }; + + if (hit.vertical) { + crossing.jacobian = {-slope, 1.0F, lever_x + lever_y * slope + swing}; + } else { + crossing.jacobian = {1.0F, -slope, -lever_y - lever_x * slope - swing}; + } + + return crossing; +} + +bool WallModel::is_side_looking(uint8_t sensor) const { + return std::abs(std::sin(this->sensors.at(sensor).angle)) > side_looking_sine; +} + +bool WallModel::is_footprint_clear( + const RayHit& hit, uint8_t sensor, float position_deviation, float orientation_deviation +) const { + if (not hit.valid) { + return false; + } + + const float spot = hit.range * std::tan(this->sensors.at(sensor).half_angle) / hit.cosine; + const float uncertainty = this->confidence * (position_deviation + hit.range * orientation_deviation / hit.cosine); + + const float reach = spot + uncertainty + this->edge_margin; + + return hit.offset - reach >= hit.low_end and hit.offset + reach <= hit.high_end; +} + +float WallModel::get_centered_range(uint8_t sensor) const { + const RobotModel::WallSensor& mounting = this->sensors.at(sensor); + + const float face = this->maze_geometry.cell_size / 2.0F - this->maze_geometry.wall_thickness / 2.0F; + const float sine = std::sin(mounting.angle); + + if (std::abs(sine) > 0.1F) { + return (face - std::abs(mounting.position.y)) / std::abs(sine); + } + + return (face - mounting.position.x) / std::cos(mounting.angle); +} + +float WallModel::get_range(float distance, uint8_t sensor, const RayHit& hit) const { + const float angle = this->sensors.at(sensor).angle; + const float sine = std::abs(std::sin(angle)); + const float calibrated = sine > 0.1F ? sine : std::abs(std::cos(angle)); + + return distance * std::sqrt(hit.cosine / calibrated); +} + +float WallModel::get_range_deviation(float range) const { + return this->range_noise + this->range_noise_per_meter * range; +} +} // namespace micras::nav diff --git a/micras_proxy/CMakeLists.txt b/micras_proxy/CMakeLists.txt new file mode 100644 index 0000000..bc9254a --- /dev/null +++ b/micras_proxy/CMakeLists.txt @@ -0,0 +1,52 @@ +file(GLOB_RECURSE MICRAS_PROXY_SOURCES CONFIGURE_DEPENDS "src/*.c*") + +# The one declaration of ST's register driver, pinned by the commit of v5.1.1, so that every project +# in a build shares one copy +include(FetchContent) +FetchContent_Declare( + lsm6dsv-pid + GIT_REPOSITORY https://github.com/STMicroelectronics/lsm6dsv-pid.git + GIT_TAG 6ede05a3fde7eae80bc1b47affc08257191c1aba +) + +FetchContent_MakeAvailable(lsm6dsv-pid) +file(GLOB_RECURSE LSM6DSV_SOURCES CONFIGURE_DEPENDS "${lsm6dsv-pid_SOURCE_DIR}/*.c") + +# The vendor sources are not ours to warn about. Source properties belong to the directory that sets +# them, so this stays next to the target that compiles the sources. +set_source_files_properties( + ${LSM6DSV_SOURCES} + PROPERTIES + COMPILE_FLAGS "-w" +) + +add_library(micras_proxy STATIC + ${MICRAS_PROXY_SOURCES} + ${LSM6DSV_SOURCES} +) + +add_library(micras::proxy ALIAS micras_proxy) + +target_include_directories(micras_proxy PUBLIC + include + ${lsm6dsv-pid_SOURCE_DIR} +) + +target_link_libraries(micras_proxy PUBLIC + micras::hal + micras::core +) + +# The vendor driver is C17 with extensions, whatever the consumer compiles its own C with +set_target_properties(micras_proxy PROPERTIES + C_STANDARD 17 + C_STANDARD_REQUIRED ON + C_EXTENSIONS ON +) + +micras_lib_setup_library(micras_proxy) + +# The chip models are SPI devices of the host backend, so they exist only there +if(MICRAS_LIB_HAL_BACKEND STREQUAL "host") + add_subdirectory(models) +endif() diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp new file mode 100644 index 0000000..921b70a --- /dev/null +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -0,0 +1,170 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_ARGB_HPP +#define MICRAS_PROXY_ARGB_HPP + +#include +#include + +#include "micras/hal/pwm_dma.hpp" + +namespace micras::proxy { +/** + * @brief Class for controlling an addressable RGB LED. + */ +template +class TArgb { +public: + /** + * @brief Configuration struct for the addressable RGB LED. + */ + struct Config { + hal::PwmDma::Config pwm; + float low_duty_cycle; + float high_duty_cycle; + float max_brightness; + }; + + /** + * @brief Struct for storing color information. + */ + struct Color { + uint8_t red; + uint8_t green; + uint8_t blue; + + Color operator*(float brightness) const { + return { + static_cast(this->red * brightness), + static_cast(this->green * brightness), + static_cast(this->blue * brightness), + }; + } + }; + + /** + * @brief Predefined colors. + */ + struct Colors { + Colors() = delete; + + static constexpr Color red{255, 0, 0}; + static constexpr Color green{0, 255, 0}; + static constexpr Color blue{0, 0, 255}; + static constexpr Color yellow{255, 255, 0}; + static constexpr Color cyan{0, 255, 255}; + static constexpr Color magenta{255, 0, 255}; + static constexpr Color white{255, 255, 255}; + }; + + /** + * @brief Construct a new Argb object. + * + * @param config Configuration for the addressable RGB LED. + */ + explicit TArgb(const Config& config); + + /** + * @brief Set the color of the ARGB at the specified index. + * + * @param index The index of the ARGB to set the color of. + * @param color The color to set the ARGB to. + */ + void set_color(const Color& color, uint8_t index); + + /** + * @brief Set the color of all ARGBs. + * + * @param color The color to set all ARGBs to. + */ + void set_color(const Color& color); + + /** + * @brief Set the colors of all ARGBs. + * + * @param colors The colors to set the ARGBs to. + */ + void set_colors(const std::array& colors); + + /** + * @brief Turn off the ARGB at the specified index. + * + * @param index The index of the ARGB to turn off. + */ + void turn_off(uint8_t index); + + /** + * @brief Turn off all ARGBs. + */ + void turn_off(); + + /** + * @brief Send the colors to the addressable RGB LED. + * + * This function is called automatically when the colors are set. + */ + void update(); + + /** + * @brief Check if the PWM was successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief Encode a color into the data buffer. + * + * @param color The color to encode. + * @param index The index to encode the color at. + */ + void encode_color(const Color& color, uint8_t index); + + /** + * @brief Protocol constants for the addressable RGB LED communication. + */ + ///@{ + static constexpr uint8_t bits_per_color{8}; + static constexpr uint8_t colors_per_led{3}; + static constexpr uint8_t bits_per_led{bits_per_color * colors_per_led}; + static constexpr uint8_t reset_length{225}; + ///@} + + /** + * @brief PWM control object. + */ + hal::PwmDma pwm; + + /** + * @brief PWM autoreload value for the low signal. + */ + uint32_t low_bit; + + /** + * @brief PWM autoreload value for the high signal. + */ + uint32_t high_bit; + + /** + * @brief Maximum brightness of the addressable RGB LED. + */ + float brightness; + + /** + * @brief Array to store the color of each LED. + */ + std::array colors{}; + + /** + * @brief Data buffer to send to the addressable RGB LED. + */ + std::array(num_of_leds) * bits_per_led + reset_length> buffer{}; +}; +} // namespace micras::proxy + +#include "micras/proxy/impl/argb.tpp" // IWYU pragma: export + +#endif // MICRAS_PROXY_ARGB_HPP diff --git a/micras_proxy/include/micras/proxy/battery.hpp b/micras_proxy/include/micras/proxy/battery.hpp new file mode 100644 index 0000000..0244a06 --- /dev/null +++ b/micras_proxy/include/micras/proxy/battery.hpp @@ -0,0 +1,98 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_BATTERY_HPP +#define MICRAS_PROXY_BATTERY_HPP + +#include + +#include "micras/core/butterworth_filter.hpp" +#include "micras/hal/adc_dma.hpp" + +namespace micras::proxy { +/** + * @brief Class for getting the battery voltage. + */ +class Battery { +public: + /** + * @brief Configuration struct for the battery. + */ + struct Config { + hal::AdcDma::Config adc; + float voltage_divider; + core::ButterworthFilter::Config filter; + }; + + /** + * @brief Construct a new Battery object. + * + * @param config Configuration for the battery. + */ + explicit Battery(const Config& config); + + /** + * @brief Update the battery reading. + * + * @note Restarts the converter first if an error stopped it. + */ + void update(); + + /** + * @brief Get the battery voltage. + * + * @return Battery voltage in volts. + */ + float get_voltage() const; + + /** + * @brief Get the battery voltage in volts without the filter applied. + * + * @return Battery voltage in volts. + */ + float get_voltage_raw() const; + + /** + * @brief Get the battery reading from the ADC. + * + * @return Battery reading from 0 to 1. + */ + float get_adc_reading() const; + + /** + * @brief Check if the ADC was successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief Adc object. + */ + hal::AdcDma adc; + + /** + * @brief Raw reading from the battery. + */ + uint16_t raw_reading{}; + + /** + * @brief Maximum voltage that can be read. + */ + float max_voltage; + + /** + * @brief Butterworth filter for the battery reading. + */ + core::ButterworthFilter filter; + + /** + * @brief Flag to check if the ADC was initialized. + */ + bool initialized{}; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_BATTERY_HPP diff --git a/micras_proxy/include/micras/proxy/bluetooth_serial.hpp b/micras_proxy/include/micras/proxy/bluetooth_serial.hpp new file mode 100644 index 0000000..ad885e8 --- /dev/null +++ b/micras_proxy/include/micras/proxy/bluetooth_serial.hpp @@ -0,0 +1,100 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_BLUETOOTH_SERIAL_HPP +#define MICRAS_PROXY_BLUETOOTH_SERIAL_HPP + +#include +#include +#include + +#include "micras/core/byte_stream.hpp" +#include "micras/hal/uart_dma.hpp" + +namespace micras::proxy { +/** + * @brief Byte stream over the UART the radio module is wired to. + * + * @note The module has no flow control on this board and drops silently when its buffer fills, so + * the ring here only bounds what the firmware can hand to the peripheral. What bounds what the + * module can take is the credit window in the session layer. + */ +class BluetoothSerial : public core::IByteStream { +public: + /** + * @brief Configuration struct for the bluetooth serial. + */ + struct Config { + void (*init_function)(); + UART_HandleTypeDef* handle; + }; + + /** + * @brief Construct a new BluetoothSerial object. + * + * @note The buffers are borrowed and have to outlive the object, and both are written by the + * DMA controller rather than by the processor. + * + * @param config Configuration for the bluetooth serial. + * @param rx_buffer Buffer the reception wraps around, which bounds how long the link can go + * unpolled before bytes are lost. + * @param tx_buffer Buffer the frames waiting to be sent are queued in. + */ + BluetoothSerial(const Config& config, std::span rx_buffer, std::span tx_buffer); + + /** + * @brief Hand the peripheral whatever is queued. + */ + void update(); + + /** + * @brief Take the bytes that arrived since the last call. + * + * @param into Buffer to read into. + * @return Number of bytes read. + */ + std::size_t read(std::span into) override; + + /** + * @brief Queue data to be sent. + * + * @note Either all of the data is queued or none of it is, so a frame is never cut in half. + * A frame that does not fit is refused, and the session drops it and counts it. + * + * @param from Data to send. + * @return Number of bytes queued. + */ + std::size_t write(std::span from) override; + + /** + * @brief Check if the peripheral was initialized and is receiving. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + hal::UartDma uart; + + /** + * @brief Bytes queued to be sent, written at the head and sent from the tail. + */ + std::span tx_buffer; + + std::size_t head{}; + std::size_t tail{}; + + /** + * @brief Number of bytes in the ring, including the ones already handed to the DMA. + */ + std::size_t stored{}; + + /** + * @brief Number of bytes the current transfer is sending, counted from the tail. + */ + std::size_t pending{}; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_BLUETOOTH_SERIAL_HPP diff --git a/micras_proxy/include/micras/proxy/button.hpp b/micras_proxy/include/micras/proxy/button.hpp new file mode 100644 index 0000000..1e349fa --- /dev/null +++ b/micras_proxy/include/micras/proxy/button.hpp @@ -0,0 +1,152 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_BUTTON_HPP +#define MICRAS_PROXY_BUTTON_HPP + +#include + +#include "micras/hal/gpio.hpp" +#include "micras/proxy/stopwatch.hpp" + +namespace micras::proxy { +/** + * @brief Class for acquiring button data. + */ +class Button { +public: + /** + * @brief Enum for button status. + */ + enum class Status : uint8_t { + NO_PRESS = 0, + SHORT_PRESS = 1, + LONG_PRESS = 2, + EXTRA_LONG_PRESS = 3 + }; + + /** + * @brief Enum for button pull resistor. + */ + enum class PullResistor : uint8_t { + PULL_UP = 0, + PULL_DOWN = 1, + }; + + /** + * @brief Configuration struct for the button. + */ + struct Config { + hal::Gpio::Config gpio{}; + PullResistor pull_resistor{}; + uint16_t debounce_delay{10}; + uint16_t long_press_delay{500}; + uint16_t extra_long_press_delay{2000}; + }; + + /** + * @brief Construct a new Button object. + * + * @param config Button configuration. + */ + explicit Button(const Config& config); + + /** + * @brief Check if button is pressed. + * + * @return True if button is pressed, false otherwise. + */ + bool is_pressed() const; + + /** + * @brief Get button status. + * + * @return Current button status. + */ + Status get_status() const; + + /** + * @brief Update the status of the button. + */ + void update(); + +private: + /** + * @brief Get raw button reading. + * + * @return Button reading without debounce. + */ + bool get_raw_reading() const; + + /** + * @brief Update button state. + */ + void update_state(); + + /** + * @brief Check if button was just pressed. + * + * @return True if button was just pressed, false otherwise. + */ + bool is_rising_edge() const; + + /** + * @brief Check if button was just released. + * + * @return True if button was just released, false otherwise. + */ + bool is_falling_edge() const; + + /** + * @brief Button pressing delays in ms. + */ + ///@{ + uint16_t debounce_delay; + uint16_t long_press_delay; + uint16_t extra_long_press_delay; + ///@} + + /** + * @brief Gpio object for button. + */ + hal::Gpio gpio; + + /** + * @brief Pull resistor configuration. + */ + PullResistor pull_resistor; + + /** + * @brief Stopwatch to check if button is debouncing. + */ + proxy::Stopwatch debounce_stopwatch; + + /** + * @brief Stopwatch to determine type of button press. + */ + proxy::Stopwatch status_stopwatch; + + /** + * @brief Flag to know when button is debouncing. + */ + bool is_debouncing{false}; + + /** + * @brief Flag to know if button was being pressed. + */ + bool previous_state{false}; + + /** + * @brief Flag to know if button is being pressed. + */ + bool current_state{false}; + + /** + * @brief Current status of the button. + */ + Status current_status{Status::NO_PRESS}; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_BUTTON_HPP diff --git a/micras_proxy/include/micras/proxy/buzzer.hpp b/micras_proxy/include/micras/proxy/buzzer.hpp new file mode 100644 index 0000000..38f05be --- /dev/null +++ b/micras_proxy/include/micras/proxy/buzzer.hpp @@ -0,0 +1,88 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_BUZZER_HPP +#define MICRAS_PROXY_BUZZER_HPP + +#include + +#include "micras/hal/pwm.hpp" +#include "micras/proxy/stopwatch.hpp" + +namespace micras::proxy { +/** + * @brief Class for controlling a buzzer. + */ +class Buzzer { +public: + /** + * @brief Configuration struct for the buzzer. + */ + struct Config { + hal::Pwm::Config pwm; + }; + + /** + * @brief Construct a new Buzzer object. + * + * @param config Configuration for the buzzer. + */ + explicit Buzzer(const Config& config); + + /** + * @brief Play a tone for a duration. + * + * @param frequency Buzzer sound frequency in Hz. + * @param duration Duration of the sound in ms. + */ + void play(uint32_t frequency, uint32_t duration = 0); + + /** + * @brief Update the buzzer state. + */ + void update(); + + /** + * @brief Stop the buzzer sound. + */ + void stop(); + + /** + * @brief Wait for a time interval updating the buzzer. + * + * @param interval Time to wait in ms. + */ + void wait(uint32_t interval); + + /** + * @brief Check if the PWM was successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief PWM object. + */ + hal::Pwm pwm; + + /** + * @brief Stopwatch to play the sound. + */ + proxy::Stopwatch stopwatch; + + /** + * @brief Flag to check if the buzzer is playing. + */ + bool is_playing{}; + + /** + * @brief Duration of the sound. + */ + uint32_t duration{}; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_BUZZER_HPP diff --git a/micras_proxy/include/micras/proxy/dip_switch.hpp b/micras_proxy/include/micras/proxy/dip_switch.hpp new file mode 100644 index 0000000..84ba872 --- /dev/null +++ b/micras_proxy/include/micras/proxy/dip_switch.hpp @@ -0,0 +1,68 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_DIP_SWITCH_HPP +#define MICRAS_PROXY_DIP_SWITCH_HPP + +#include +#include + +#include "micras/hal/gpio.hpp" + +namespace micras::proxy { +/** + * @brief Class for acquiring dip switch data. + */ +template +class TDipSwitch { +public: + /** + * @brief Configuration struct for the Dip Switch. + * + * @note A switch wired between the pin and ground with an internal pull up reads low when it is + * closed, so the sense of the reading is a property of the board and not of this class. + */ + struct Config { + std::array gpio_array; + bool active_low; + }; + + /** + * @brief Construct a new Dip Switch object. + * + * @param config Configuration struct for the DipSwitch. + */ + explicit TDipSwitch(const Config& config); + + /** + * @brief Get the state of a switch. + * + * @param switch_index Index of the switch. + * @return True if the switch is on, false otherwise. + */ + bool get_switch_state(uint8_t switch_index) const; + + /** + * @brief Get the value of all switches. + * + * @return Value of all switches. + */ + uint8_t get_switches_value() const; + +private: + /** + * @brief Array of GPIOs for the switches. + */ + std::array gpio_array; + + /** + * @brief Whether a closed switch pulls its pin low. + */ + bool active_low; +}; +} // namespace micras::proxy + +#include "micras/proxy/impl/dip_switch.tpp" // IWYU pragma: export + +#endif // MICRAS_PROXY_DIP_SWITCH_HPP diff --git a/micras_proxy/include/micras/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp new file mode 100644 index 0000000..930640a --- /dev/null +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -0,0 +1,129 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_FAN_HPP +#define MICRAS_PROXY_FAN_HPP + +#include "micras/hal/gpio.hpp" +#include "micras/hal/pwm.hpp" +#include "micras/proxy/stopwatch.hpp" + +namespace micras::proxy { +/** + * @brief Class for controlling the fan driver. + * + * @note The driver on this board has its two half bridges paralleled and its phase input strapped, + * so the drive is unidirectional by construction and there is no direction pin to control. + */ +class Fan { +public: + /** + * @brief Configuration struct for the fan. + */ + struct Config { + hal::Pwm::Config pwm; + hal::Gpio::Config enable_gpio; + float max_acceleration; + }; + + /** + * @brief Construct a new fan object. + * + * @param config Configuration for the fan driver. + */ + explicit Fan(const Config& config); + + /** + * @brief Enable the fan. + */ + void enable(); + + /** + * @brief Disable the fan. + */ + void disable(); + + /** + * @brief Set the speed of the fans. + * + * @param speed Speed percentage of the fan. + */ + void set_speed(float speed); + + /** + * @brief Update the speed of the fan. + * + * @return Current speed percentage of the fan. + */ + float update(); + + /** + * @brief Stop the fan and cancel any speed it was ramping towards. + */ + void stop(); + + /** + * @brief Check if the fan has finished ramping to the speed it was asked for. + * + * @return True once the current speed is the target one, which a stopped fan always is. + */ + bool is_at_speed() const; + + /** + * @brief Check if the driver is reporting a fault. + * + * @note The enable pin is bidirectional: the driver pulls it low to signal an over current or a + * thermal shutdown, which is only observable while the fan is meant to be enabled and the pin + * is configured as an open drain output. + * + * @return True if the driver is pulling the enable pin low against the firmware, false otherwise. + */ + bool check_fault() const; + + /** + * @brief Check if the fan PWM was successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief PWM object for controlling the fan speed. + */ + hal::Pwm pwm; + + /** + * @brief GPIO handle for the fan enable and fault pin. + */ + hal::Gpio enable_gpio; + + /** + * @brief Current speed of the fan. + */ + float current_speed{}; + + /** + * @brief Target speed of the fan. + */ + float target_speed{}; + + /** + * @brief Maximum acceleration of the fan in percentage per millisecond. + */ + float max_acceleration; + + /** + * @brief Whether the firmware is currently asking the driver to be enabled. + */ + bool enabled{}; + + /** + * @brief Stopwatch for limiting the acceleration of the fan. + */ + proxy::Stopwatch acceleration_stopwatch; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_FAN_HPP diff --git a/micras_proxy/include/micras/proxy/fmac_filter.hpp b/micras_proxy/include/micras/proxy/fmac_filter.hpp new file mode 100644 index 0000000..6671174 --- /dev/null +++ b/micras_proxy/include/micras/proxy/fmac_filter.hpp @@ -0,0 +1,124 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_FMAC_FILTER_HPP +#define MICRAS_PROXY_FMAC_FILTER_HPP + +#include +#include + +#include "micras/core/butterworth_filter.hpp" +#include "micras/hal/fmac.hpp" + +namespace micras::proxy { +/** + * @brief Second order Butterworth low-pass filter running on the filter math accelerator. + * + * @note Interface compatible with core::ButterworthFilter, so the two are interchangeable at the + * point of use. Three properties of the accelerator decide whether that substitution is a good + * idea, and none of them is a property of this class: + * + * - The accelerator works in q1.15, so every coefficient has to fall inside [-1, 1) and be large + * enough not to quantize to zero. A second order Butterworth has a feed forward coefficient of + * roughly (pi * fc / fs)^2 and a feedback coefficient close to -2, so the useful range starts + * around a cutoff of a tenth of the sampling rate. Halving the coefficients and doubling the + * output with the gain of the accelerator, which this class does not do, brings that to about a + * fiftieth, still well above what the sensors of this robot use. was_initialized reports that + * refusal instead of filtering with silently truncated coefficients. + * - Samples and results are q1.15 too, so the input has to be normalized to [-1, 1). That suits a + * ratiometric reading such as an ADC fraction, and not a physical quantity such as a rate in + * radians per second. + * - The accelerator runs one function at a time, so one instance of this class owns it. Several + * filters would have to share a cutoff and be interleaved through one set of coefficients spaced + * apart, or have their history loaded again at every switch, which costs far more than the + * handful of multiply accumulates it would save. Neither is what this class does. + */ +class FmacFilter { +public: + /** + * @brief Configuration struct for the filter. + */ + struct Config { + hal::Fmac::Config fmac; + core::ButterworthFilter::Config filter; + }; + + /** + * @brief Construct a new FmacFilter object. + * + * @param config Configuration for the filter. + */ + explicit FmacFilter(const Config& config); + + /** + * @brief Produce a new value from measured data. + * + * @param x0 Last measure, normalized to the range [-1, 1). + * @return Filtered value, or zero if the coefficients could not be represented. + */ + float update(float x0); + + /** + * @brief Get the last filtered value. + * + * @return Last filtered value. + */ + float get_last() const; + + /** + * @brief Check if the coefficients fit the fixed point format and the filter is running. + * + * @return True if the filter is usable, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief Number of fractional bits of the q1.15 format of the accelerator. + */ + static constexpr uint8_t fractional_bits{15}; + + /** + * @brief Scale between a float in [-1, 1) and its q1.15 representation. + */ + static constexpr float fixed_point_scale{static_cast(1U << fractional_bits)}; + + /** + * @brief Convert a coefficient to q1.15, reporting whether it survives the conversion. + * + * @param value Coefficient to convert. + * @param result Converted coefficient. + * @return True if the coefficient is inside the representable range and does not quantize to + * zero, false otherwise. + */ + static bool to_fixed_point(float value, int16_t& result); + + /** + * @brief Filter math accelerator running the filter. + */ + hal::Fmac fmac; + + /** + * @brief Coefficients applied to the input samples, most recent first. + */ + std::array feed_forward{}; + + /** + * @brief Coefficients applied to the previous outputs, most recent first. + */ + std::array feedback{}; + + /** + * @brief Last filtered value. + */ + float last{}; + + /** + * @brief Flag to check if the filter is running. + */ + bool initialized{}; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_FMAC_FILTER_HPP diff --git a/micras_proxy/include/micras/proxy/impl/argb.tpp b/micras_proxy/include/micras/proxy/impl/argb.tpp new file mode 100644 index 0000000..64d930f --- /dev/null +++ b/micras_proxy/include/micras/proxy/impl/argb.tpp @@ -0,0 +1,80 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_ARGB_TPP +#define MICRAS_PROXY_ARGB_TPP + +#include + +namespace micras::proxy { +template +TArgb::TArgb(const Config& config) : + pwm{config.pwm}, + low_bit{pwm.get_compare(config.low_duty_cycle)}, + high_bit{pwm.get_compare(config.high_duty_cycle)}, + brightness{config.max_brightness / 100.0F} { + this->turn_off(); +} + +template +void TArgb::set_color(const Color& color, uint8_t index) { + this->colors.at(index) = color; + this->update(); +} + +template +void TArgb::set_color(const Color& color) { + for (uint8_t i = 0; i < num_of_leds; i++) { + this->colors.at(i) = color; + } + + this->update(); +} + +template +void TArgb::set_colors(const std::array& colors) { + this->colors = colors; + this->update(); +} + +template +void TArgb::turn_off(uint8_t index) { + this->set_color({0, 0, 0}, index); +} + +template +void TArgb::turn_off() { + this->set_color({0, 0, 0}); +} + +template +void TArgb::update() { + if (this->pwm.is_busy()) { + return; + } + + for (uint8_t i = 0; i < num_of_leds; i++) { + this->encode_color(this->colors.at(i) * this->brightness, i); + } + + this->pwm.start_dma(this->buffer); +} + +template +void TArgb::encode_color(const Color& color, uint8_t index) { + uint32_t data = (static_cast(color.green) << (2 * bits_per_color)) | + (static_cast(color.red) << bits_per_color) | color.blue; + + for (uint32_t i = bits_per_led * index, j = bits_per_led - 1; i < bits_per_led * (index + 1U); i++, j--) { + this->buffer.at(i) = ((data >> j) & 1) == 1 ? this->high_bit : this->low_bit; + } +} + +template +bool TArgb::was_initialized() const { + return this->pwm.was_initialized(); +} +} // namespace micras::proxy + +#endif // MICRAS_PROXY_ARGB_TPP diff --git a/micras_proxy/include/micras/proxy/impl/dip_switch.tpp b/micras_proxy/include/micras/proxy/impl/dip_switch.tpp new file mode 100644 index 0000000..817892a --- /dev/null +++ b/micras_proxy/include/micras/proxy/impl/dip_switch.tpp @@ -0,0 +1,34 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_DIP_SWITCH_TPP +#define MICRAS_PROXY_DIP_SWITCH_TPP + +#include +#include "micras/core/utils.hpp" +#include "micras/hal/gpio.hpp" + +namespace micras::proxy { +template +TDipSwitch::TDipSwitch(const Config& config) : + gpio_array{core::make_array(config.gpio_array)}, active_low{config.active_low} { } + +template +bool TDipSwitch::get_switch_state(uint8_t switch_index) const { + return this->gpio_array.at(switch_index).read() != this->active_low; +} + +template +uint8_t TDipSwitch::get_switches_value() const { + uint8_t switches_value = 0; + + for (uint8_t i = 0; i < num_of_sensors; i++) { + switches_value |= static_cast(this->get_switch_state(i) << i); + } + + return switches_value; +} +} // namespace micras::proxy + +#endif // MICRAS_PROXY_DIP_SWITCH_TPP diff --git a/micras_proxy/include/micras/proxy/impl/torque_sensors.tpp b/micras_proxy/include/micras/proxy/impl/torque_sensors.tpp new file mode 100644 index 0000000..53da7c7 --- /dev/null +++ b/micras_proxy/include/micras/proxy/impl/torque_sensors.tpp @@ -0,0 +1,71 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_TORQUE_SENSORS_TPP +#define MICRAS_PROXY_TORQUE_SENSORS_TPP + +#include + +#include "micras/core/butterworth_filter.hpp" +#include "micras/core/utils.hpp" + +namespace micras::proxy { +template +TTorqueSensors::TTorqueSensors(const Config& config) : + adc{config.adc}, + base_reading{core::make_array(config.zero_reading)}, + max_current{config.adc.reference_voltage / config.shunt_resistor}, + max_torque{config.max_torque}, + filters{core::make_array(config.filter)}, + initialized{this->adc.start_dma(this->buffer) and this->adc.was_initialized()} { } + +template +void TTorqueSensors::calibrate() { + for (uint8_t i = 0; i < num_of_sensors; i++) { + this->base_reading.at(i) += this->filters.at(i).get_last(); + } +} + +template +void TTorqueSensors::update() { + this->adc.recover(); + + for (uint8_t i = 0; i < num_of_sensors; i++) { + this->filters.at(i).update(this->get_adc_reading(i)); + } +} + +template +float TTorqueSensors::get_torque(uint8_t sensor_index) const { + return this->filters.at(sensor_index).get_last() * this->max_torque; +} + +template +float TTorqueSensors::get_torque_raw(uint8_t sensor_index) const { + return this->get_adc_reading(sensor_index) * this->max_torque; +} + +template +float TTorqueSensors::get_current(uint8_t sensor_index) const { + return this->filters.at(sensor_index).get_last() * this->max_current; +} + +template +float TTorqueSensors::get_current_raw(uint8_t sensor_index) const { + return this->get_adc_reading(sensor_index) * this->max_current; +} + +template +float TTorqueSensors::get_adc_reading(uint8_t sensor_index) const { + return static_cast(this->buffer.at(sensor_index)) / this->adc.get_max_reading() - + this->base_reading.at(sensor_index); +} + +template +bool TTorqueSensors::was_initialized() const { + return this->initialized; +} +} // namespace micras::proxy + +#endif // MICRAS_PROXY_TORQUE_SENSORS_TPP diff --git a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp new file mode 100644 index 0000000..03101df --- /dev/null +++ b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp @@ -0,0 +1,229 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_WALL_SENSORS_TPP +#define MICRAS_PROXY_WALL_SENSORS_TPP + +#include +#include +#include +#include +#include + +#include "micras/core/butterworth_filter.hpp" +#include "micras/core/utils.hpp" +#include "micras/hal/pwm.hpp" + +namespace micras::proxy { +template +TWallSensors::TWallSensors(const Config& config) : + adc{config.adc}, + led_pwms{core::make_array(config.led_pwms)}, + emitter_duty_cycle{config.emitter_duty_cycle}, + fast_filters{core::make_array(config.fast_filter)}, + slow_filters{core::make_array(config.slow_filter)}, + reference_readings{config.reference_readings}, + reference_distances{config.reference_distances}, + receiver_offset{config.receiver_offset}, + receiver_half_angle{config.receiver_half_angle}, + noise_floor{config.noise_floor}, + max_distance{config.max_distance}, + max_reading{config.max_reading}, + blind_reading{config.blind_reading}, + wall_distance{config.wall_distance}, + wall_hysteresis{config.wall_hysteresis}, + calibration_samples{config.calibration_samples}, + initialized{ + this->adc.start_dma(this->buffer, this->snapshot) and this->adc.was_initialized() and + config.adc.handle->Init.NbrOfConversion == num_of_sensors and + std::ranges::all_of(this->led_pwms, [&config](const hal::Pwm& led_pwm) { + return led_pwm.was_initialized() and + std::abs(led_pwm.get_frequency() - config.fast_filter.sampling_frequency) < + frequency_tolerance * config.fast_filter.sampling_frequency; + }) + } { + if (this->receiver_offset > 0.0F) { + const float step = this->max_distance / static_cast(4 * shape_points); + float peak = step; + + for (float distance = step; distance < this->max_distance; distance += step) { + if (this->shape(distance) > this->shape(peak)) { + peak = distance; + } + } + + const float ratio = std::pow(this->max_distance / peak, 1.0F / static_cast(shape_points - 1)); + float distance = peak; + + for (uint8_t i = 0; i < shape_points; i++) { + this->shape_distances.at(i) = distance; + this->shape_scales.at(i) = 1.0F / std::sqrt(this->shape(distance)); + distance *= ratio; + } + } + + this->turn_off(); +} + +template +float TWallSensors::shape(float distance) const { + const float angle = std::atan(this->receiver_offset / distance) / this->receiver_half_angle; + return std::exp2(-angle * angle) / (distance * distance); +} + +template +float TWallSensors::to_distance(uint8_t sensor_index, float intensity) const { + const float reference_distance = this->reference_distances.at(sensor_index); + const float ratio = this->reference_readings.at(sensor_index) / intensity; + + if (this->receiver_offset <= 0.0F) { + return reference_distance * std::sqrt(ratio); + } + + const float scale = std::sqrt(ratio) / std::sqrt(this->shape(reference_distance)); + + if (scale <= this->shape_scales.front()) { + return this->shape_distances.front(); + } + + if (scale >= this->shape_scales.back()) { + return this->max_distance; + } + + const auto upper = std::ranges::upper_bound(this->shape_scales, scale); + const auto index = static_cast(std::distance(this->shape_scales.begin(), upper)); + const float low = this->shape_scales.at(index - 1); + const float fraction = (scale - low) / (this->shape_scales.at(index) - low); + + return this->shape_distances.at(index - 1) + + fraction * (this->shape_distances.at(index) - this->shape_distances.at(index - 1)); +} + +template +void TWallSensors::turn_on() { + for (auto& led_pwm : this->led_pwms) { + led_pwm.set_duty_cycle(this->emitter_duty_cycle); + } +} + +template +void TWallSensors::turn_off() { + for (auto& led_pwm : this->led_pwms) { + led_pwm.set_duty_cycle(0.0F); + } +} + +template +void TWallSensors::update() { + this->adc.recover(); + + const uint32_t current_sequence = this->adc.read_snapshot(this->scans); + const bool is_new = current_sequence != this->sequence; + + this->sequence = current_sequence; + + for (uint8_t i = 0; i < num_of_sensors; i++) { + Reading& reading = this->readings.at(i); + reading.is_new = is_new; + + if (not is_new) { + continue; + } + + const float intensity = this->get_intensity(i); + + const float distance = intensity >= this->noise_floor ? + this->to_distance(i, std::min(intensity, this->max_reading)) : + this->max_distance; + + reading.valid = distance < this->max_distance; + reading.dark = this->get_dark_reading(i); + reading.blind = reading.dark >= this->blind_reading; + reading.distance = this->fast_filters.at(i).update(std::min(distance, this->max_distance)); + reading.slow_distance = this->slow_filters.at(i).update(std::min(distance, this->max_distance)); + + if (not reading.valid or reading.slow_distance > this->wall_distance + this->wall_hysteresis) { + this->walls.at(i) = false; + } else if (reading.slow_distance < this->wall_distance) { + this->walls.at(i) = true; + } + + Calibration& calibration = this->calibrations.at(i); + + if (calibration.samples_left == 0) { + continue; + } + + calibration.sum += intensity; + calibration.squared_sum += intensity * intensity; + calibration.samples_left--; + + if (calibration.samples_left == 0) { + const float mean = calibration.sum / static_cast(this->calibration_samples); + const float variance = + calibration.squared_sum / static_cast(this->calibration_samples) - mean * mean; + + calibration.spread = mean > 0.0F ? std::sqrt(std::max(variance, 0.0F)) / mean : 0.0F; + this->reference_readings.at(i) = std::max(mean, this->noise_floor); + } + } +} + +template +const typename TWallSensors::Reading& + TWallSensors::get_reading(uint8_t sensor_index) const { + return this->readings.at(sensor_index); +} + +template +bool TWallSensors::get_wall(uint8_t sensor_index) const { + return this->walls.at(sensor_index); +} + +template +float TWallSensors::get_intensity(uint8_t sensor_index) const { + return static_cast(std::abs(this->scans.at(sensor_index) - this->scans.at(sensor_index + num_of_sensors))) / + this->adc.get_max_reading(); +} + +template +float TWallSensors::get_dark_reading(uint8_t sensor_index) const { + return static_cast(std::min(this->scans.at(sensor_index), this->scans.at(sensor_index + num_of_sensors))) / + this->adc.get_max_reading(); +} + +template +void TWallSensors::calibrate_sensor(uint8_t sensor_index) { + this->calibrations.at(sensor_index) = { + .sum = 0.0F, + .squared_sum = 0.0F, + .samples_left = this->calibration_samples, + .spread = 0.0F, + }; +} + +template +bool TWallSensors::is_calibrating() const { + return std::ranges::any_of(this->calibrations, [](const Calibration& calibration) { + return calibration.samples_left > 0; + }); +} + +template +float TWallSensors::get_reference_reading(uint8_t sensor_index) const { + return this->reference_readings.at(sensor_index); +} + +template +float TWallSensors::get_calibration_spread(uint8_t sensor_index) const { + return this->calibrations.at(sensor_index).spread; +} + +template +bool TWallSensors::was_initialized() const { + return this->initialized; +} +} // namespace micras::proxy + +#endif // MICRAS_PROXY_WALL_SENSORS_TPP diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp new file mode 100644 index 0000000..669210b --- /dev/null +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -0,0 +1,246 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_IMU_HPP +#define MICRAS_PROXY_IMU_HPP + +#include +#include +#include +#include + +#include "micras/hal/spi.hpp" + +namespace micras::proxy { +/** + * @brief Class for acquiring IMU data. + * + * @note The rates are delivered as the sensor reports them, with their bias still in them. The bias + * of a gyroscope drifts with temperature and time, so it is estimated continuously by whoever + * integrates the rate, not measured once here. + * + * @note The samples are read by the DMA into a buffer of this object, which therefore has to live in + * memory that the DMA reaches. + */ +class Imu { +public: + /** + * @brief IMU configuration struct. + * + * @note The data rates of the high accuracy mode, which are the ones that are round numbers, + * exist only with both sensors in that mode. + */ + struct Config { + hal::Spi::Config spi; + lsm6dsv_gy_mode_t gyroscope_mode; + lsm6dsv_xl_mode_t accelerometer_mode; + lsm6dsv_data_rate_t gyroscope_data_rate; + lsm6dsv_data_rate_t accelerometer_data_rate; + lsm6dsv_gy_full_scale_t gyroscope_scale; + lsm6dsv_xl_full_scale_t accelerometer_scale; + lsm6dsv_filt_gy_lp1_bandwidth_t gyroscope_filter; + lsm6dsv_filt_xl_lp2_bandwidth_t accelerometer_filter; + }; + + /** + * @brief Enum to select the axis of the IMU. + */ + enum class Axis : uint8_t { + X = 0, + Y = 1, + Z = 2 + }; + + /** + * @brief Construct a new Imu object. + * + * @note The high accuracy data rate set is chosen while both sensors are still powered down, + * since the datasheet only allows high accuracy mode to change in power down, and the data rates + * are read back afterwards: a sensor that kept the default set runs at 7680 Hz instead of + * 8000 Hz, and fails the initialization instead of passing silently. + * + * @param config Configuration for the IMU, whose two data rates have to share one set. + */ + explicit Imu(const Config& config); + + /** + * @brief Take the sample that the last call asked for, and ask for the next one. + * + * @details The status, the angular rate and the acceleration are read in one transfer that the + * DMA carries out while the caller does something else, so a call costs the time to start a + * transfer and never the time the transfer takes. The price is that a sample is one call old + * when it is used. + */ + void update(); + + /** + * @brief Check whether the last update brought an angular rate that was not seen before. + * + * @note The sensor samples on its own clock, so a caller running faster than the output data + * rate, or close to it, gets the same sample more than once. + * + * @return True if the angular rate is a new sample, false otherwise. + */ + bool is_new() const; + + /** + * @brief Get the IMU angular velocity over an axis. + * + * @param axis Axis to get the angular velocity from. + * @return Angular velocity over the desired axis in rad/s. + */ + float get_angular_velocity(Axis axis) const; + + /** + * @brief Get the IMU linear acceleration over an axis. + * + * @param axis Axis to get the linear acceleration from. + * @return Linear acceleration over the desired axis in m/sยฒ. + */ + float get_linear_acceleration(Axis axis) const; + + /** + * @brief Check if IMU was initialized. + * + * @return True if the device was successfully initialized, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief Check the IMU device. + * + * @return True if the device is correct, false otherwise. + */ + bool check_whoami(); + + /** + * @brief Convert the registers that the last transfer brought. + * + * @note A status that the sensor cannot have sent brings no sample. + */ + void read_response(); + + /** + * @brief Read data from the IMU. + * + * @param handle Pointer to a SPI object. + * @param reg Register to read from. + * @param bufp Buffer to read. + * @param len Length of the buffer. + * @return 0 if the operation was successful, -1 otherwise. + */ + static int32_t platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len); + + /** + * @brief Write data to the IMU. + * + * @param handle Pointer to a SPI object. + * @param reg Register to write to. + * @param bufp Buffer to write. + * @param len Length of the buffer. + * @return 0 if the operation was successful, -1 otherwise. + */ + static int32_t platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len); + + /** + * @brief Bit of the register address that makes a transfer a read. + */ + static constexpr uint8_t read_flag{0x80}; + + /** + * @brief Number of registers from the status to the last byte of the acceleration. + */ + static constexpr uint8_t burst_size{16}; + + /** + * @brief Offsets of the angular rate and of the acceleration from the status register. + */ + ///@{ + static constexpr uint8_t angular_rate_offset{4}; + static constexpr uint8_t acceleration_offset{10}; + ///@} + + /** + * @brief Bits of the high accuracy data rate configuration register that select the set, which + * the data rate identifiers of the driver carry in their high nibble. + */ + static constexpr uint8_t haodr_sel_mask{0x03}; + + /** + * @brief Bits of the status register that tell which sensor has a new sample. + */ + ///@{ + static constexpr uint8_t accelerometer_ready{0x01}; + static constexpr uint8_t gyroscope_ready{0x02}; + ///@} + + /** + * @brief Bits of the status register that always read zero. + * + * @note A status with any of them set did not come from the sensor: a data line held high, as + * when the sensor does not answer, reads all ones, which would otherwise look like a new + * sample. + */ + static constexpr uint8_t unused_status_bits{0x48}; + + /** + * @brief Conversion constants. + */ + static constexpr float mdps_to_radps{std::numbers::pi_v / 180000.0F}; + static constexpr float mg_to_mps2{0.00980665F}; + + /** + * @brief SPI for the IMU communication. + */ + hal::Spi spi; + + /** + * @brief Device context for the IMU library. + */ + stmdev_ctx_t dev_ctx{}; + + /** + * @brief Address of the status register, followed by the padding that clocks the registers out. + */ + std::array command{LSM6DSV_STATUS_REG | read_flag}; + + /** + * @brief Registers received by the last transfer, after the byte that answers the address. + */ + std::array response{}; + + /** + * @brief Current angular velocity on each axis. + */ + std::array angular_velocity{}; + + /** + * @brief Current linear acceleration on each axis. + */ + std::array linear_acceleration{}; + + /** + * @brief Gyroscope conversion factor. + */ + float gy_factor; + + /** + * @brief Accelerometer conversion factor. + */ + float xl_factor; + + /** + * @brief Flag to check if the last update brought a new angular rate. + */ + bool fresh{}; + + /** + * @brief Flag to check if the IMU was initialized. + */ + bool initialized{}; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_IMU_HPP diff --git a/micras_proxy/include/micras/proxy/led.hpp b/micras_proxy/include/micras/proxy/led.hpp new file mode 100644 index 0000000..f4f2a3e --- /dev/null +++ b/micras_proxy/include/micras/proxy/led.hpp @@ -0,0 +1,53 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_LED_HPP +#define MICRAS_PROXY_LED_HPP + +#include "micras/hal/gpio.hpp" + +namespace micras::proxy { +/** + * @brief Class for controlling an LED. + */ +class Led { +public: + /** + * @brief Configuration struct for LED. + */ + struct Config { + hal::Gpio::Config gpio; + }; + + /** + * @brief Construct a new Led object. + * + * @param config Configuration for the LED. + */ + explicit Led(const Config& config); + + /** + * @brief Turn the LED on. + */ + void turn_on(); + + /** + * @brief Turn the LED off. + */ + void turn_off(); + + /** + * @brief Toggle the LED. + */ + void toggle(); + +private: + /** + * @brief Gpio object for the LED. + */ + hal::Gpio gpio; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_LED_HPP diff --git a/micras_proxy/include/micras/proxy/locomotion.hpp b/micras_proxy/include/micras/proxy/locomotion.hpp new file mode 100644 index 0000000..e3fd1cf --- /dev/null +++ b/micras_proxy/include/micras/proxy/locomotion.hpp @@ -0,0 +1,120 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_LOCOMOTION_HPP +#define MICRAS_PROXY_LOCOMOTION_HPP + +#include + +#include "micras/hal/gpio.hpp" +#include "micras/proxy/motor.hpp" + +namespace micras::proxy { +/** + * @brief Class for controlling the locomotion driver. + */ +class Locomotion { +public: + /** + * @brief Configuration struct for the locomotion. + * + * @note The reserved rotation is the part of the range, in percent, that the rotation command + * keeps when the two commands do not fit in it together. + */ + struct Config { + Motor::Config left_motor; + Motor::Config right_motor; + hal::Gpio::Config enable_gpio; + float reserved_rotation; + }; + + /** + * @brief Linear and angular commands, in percent of the motor supply. + */ + struct Command { + float linear; + float angular; + }; + + /** + * @brief Construct a new locomotion object. + * + * @param config Configuration for the locomotion driver. + */ + explicit Locomotion(const Config& config); + + /** + * @brief Enable the locomotion driver. + */ + void enable(); + + /** + * @brief Disable the locomotion driver. + */ + void disable(); + + /** + * @brief Set the command of the wheels. + * + * @param left_command Command of the left wheels. + * @param right_command Command of the right wheels. + */ + void set_wheel_command(float left_command, float right_command); + + /** + * @brief Set the linear and angular commands of the robot. + * + * @details When the two commands do not fit in the range of the motors together, it is the + * linear one that gives way: a robot that slows down stays on its path, while one that turns + * less than it was asked to leaves it. The rotation is only guaranteed up to the reserved + * part of the range, so that a rotation command gone wrong cannot stall the robot. + * + * @param linear Linear command of the robot. + * @param angular Angular command of the robot. + * @return The commands that were applied, which differ from the ones asked for when the motors + * saturate. + */ + Command set_command(float linear, float angular); + + /** + * @brief Stop the motors. + */ + void stop(); + + /** + * @brief Check if both motors were successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief Largest command a motor takes, in percent. + */ + static constexpr float max_command{100.0F}; + + /** + * @brief Left motor of the robot. + */ + Motor left_motor; + + /** + * @brief Right motor of the robot. + */ + Motor right_motor; + + /** + * @brief GPIO handle for the motor driver enable pin. + */ + hal::Gpio enable_gpio; + + /** + * @brief Part of the range that the rotation command keeps under saturation, in percent. + */ + float reserved_rotation; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_LOCOMOTION_HPP diff --git a/micras_proxy/include/micras/proxy/motor.hpp b/micras_proxy/include/micras/proxy/motor.hpp new file mode 100644 index 0000000..bcb9768 --- /dev/null +++ b/micras_proxy/include/micras/proxy/motor.hpp @@ -0,0 +1,70 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_MOTOR_HPP +#define MICRAS_PROXY_MOTOR_HPP + +#include "micras/hal/pwm.hpp" + +namespace micras::proxy { +/** + * @brief Class for controlling a motor driver. + */ +class Motor { +public: + /** + * @brief Configuration struct for the motor. + */ + struct Config { + hal::Pwm::Config backwards_pwm; + hal::Pwm::Config forward_pwm; + float max_stopped_command; + float deadzone; + }; + + /** + * @brief Construct a new motor object. + * + * @param config Configuration for the motor driver. + */ + explicit Motor(const Config& config); + + /** + * @brief Set the command for the motor. + * + * @param command Command for the motor in percentage. + */ + void set_command(float command); + + /** + * @brief Check if both PWM channels were successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief PWM object for controlling the motor in the backwards direction. + */ + hal::Pwm backwards_pwm; + + /** + * @brief PWM object for controlling the motor in the forward direction. + */ + hal::Pwm forward_pwm; + + /** + * @brief Maximum command value for the motor to be considered stopped. + */ + float max_stopped_command; + + /** + * @brief Deadzone for the motor. + */ + float deadzone; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_MOTOR_HPP diff --git a/micras_proxy/include/micras/proxy/rotary_sensor.hpp b/micras_proxy/include/micras/proxy/rotary_sensor.hpp new file mode 100644 index 0000000..97a6813 --- /dev/null +++ b/micras_proxy/include/micras/proxy/rotary_sensor.hpp @@ -0,0 +1,202 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_ROTARY_SENSOR_HPP +#define MICRAS_PROXY_ROTARY_SENSOR_HPP + +#include +#include +#include + +#include "micras/hal/crc.hpp" +#include "micras/hal/encoder.hpp" +#include "micras/hal/spi.hpp" + +namespace micras::proxy { +/** + * @brief Class for acquiring rotary sensor data. + * + * @note The angle is decoded from the quadrature output of the sensor by a hardware timer, while + * SPI is used only to configure the sensor at construction and to read back that configuration. + */ +class RotarySensor { +public: +#include "micras/proxy/rotary_sensor_reg.hpp" + + /** + * @brief Rotary sensor configuration struct. + * + * @note There is no resolution field: the pulses per revolution are whatever the ABIRES field + * of the sensor says after configuration, so the scale factor is read back from the hardware + * instead of being asserted here. + */ + struct Config { + hal::Spi::Config spi; + hal::Encoder::Config encoder; + hal::Crc::Config crc; + Registers registers; + }; + + /** + * @brief Frame sent to the sensor to address one of its registers. + */ + union CommandFrame { + struct __attribute__((__packed__)) Fields { + uint8_t crc : 8; + uint16_t address : 14; + uint8_t rw : 1; + uint8_t do_not_care : 1; + }; + + Fields fields; + uint32_t raw; + }; + + /** + * @brief Frame carrying a register value in either direction. + */ + union DataFrame { + struct __attribute__((__packed__)) Fields { + uint8_t crc : 8; + uint16_t data : 14; + uint8_t error : 1; + uint8_t warning : 1; + }; + + Fields fields; + uint32_t raw; + }; + + /** + * @brief Construct a new RotarySensor object. + * + * @note The error register is read before anything is written, since reading it is what clears + * it, and the sensor flags an error in every answer while it holds one. An error left over from + * the power up, or from a frame garbled while the bus was being set up, would otherwise fail the + * read back of the resolution. + * + * @param config Configuration for the rotary sensor. + */ + explicit RotarySensor(const Config& config); + + /** + * @brief Get the rotary sensor position over an axis. + * + * @return Current angular position of the sensor in radians. + */ + float get_position() const; + + /** + * @brief Get the number of counts the quadrature decoder produces per revolution. + * + * @return Counts per revolution, as read back from the sensor. + */ + uint32_t get_resolution() const; + + /** + * @brief Check if the sensor was configured and its configuration read back successfully. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + + /** + * @brief Read a register from the rotary sensor. + * + * @note The sensor answers a command in the frame that follows it, so the command is sent + * twice: the first transfer carries it and the second clocks the answer out while repeating + * it. + * + * @param address Address of the register. + * @return The register value, or nothing if a transfer failed or a frame was rejected. + */ + std::optional read_register(uint16_t address); + + /** + * @brief Write a register to the rotary sensor. + * + * @param address Address of the register. + * @param data Value to write. + * @return True if both frames were transferred, false otherwise. + */ + bool write_register(uint16_t address, uint16_t data); + +private: + /** + * @brief Number of bytes of a sensor SPI frame. + */ + static constexpr uint8_t frame_size{3}; + + /** + * @brief Time the chip select is kept high after a frame, in microseconds. + */ + static constexpr uint32_t min_deselect_time_us{1}; + + /** + * @brief Number of edges the quadrature decoder counts per pulse of a single channel. + */ + static constexpr uint32_t edges_per_pulse{4}; + + /** + * @brief Pulses per revolution selected by each value of the ABIRES field, zero when reserved. + */ + static constexpr std::array pulses_per_revolution{{1024, 512, 256, 2048, 4096, 0, 0, 0}}; + + /** + * @brief Serialize a frame into the wire order of the sensor, filling in its CRC. + * + * @note The sensor defines the CRC over the two most significant bytes of the frame, which + * are the first two on the wire and the last two in memory on a little endian core. + * + * @param raw The 24 bit frame, without a valid CRC. + * @return The frame as three bytes, most significant first, with the CRC in the last one. + */ + std::array serialize(uint32_t raw); + + /** + * @brief Exchange one frame with the sensor. + * + * @note The chip select is kept high for a while after the frame, since the sensor needs 350 ns + * between two frames and the next one would otherwise start within about 200 ns. + * + * @param frame The frame to send. + * @return The frame received while sending, or nothing if the transfer failed. + */ + std::optional exchange_frame(const std::array& frame); + + /** + * @brief Read back the ABIRES field and derive the counts per revolution from it. + * + * @return Counts per revolution, or nothing if the read failed or reported a reserved value. + */ + std::optional read_resolution(); + + /** + * @brief SPI for the rotary sensor configuration. + */ + hal::Spi spi; + + /** + * @brief Encoder for getting the rotary sensor data. + */ + hal::Encoder encoder; + + /** + * @brief CRC for the rotary sensor configuration. + */ + hal::Crc crc; + + /** + * @brief Counts per revolution of the quadrature decoder, read back from the sensor. + */ + uint32_t resolution{}; + + /** + * @brief Flag to check if the sensor was configured successfully. + */ + bool initialized{}; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_ROTARY_SENSOR_HPP diff --git a/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp new file mode 100644 index 0000000..a30078d --- /dev/null +++ b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp @@ -0,0 +1,129 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_ROTARY_SENSOR_REG_HPP +#define MICRAS_PROXY_ROTARY_SENSOR_REG_HPP + +#include + +/** + * @brief Registers struct for the rotary sensor configuration. + */ +struct Registers { + /** + * @brief Registers union types definition. + */ + ///@{ + union Disable { + struct __attribute__((__packed__)) Fields { + uint8_t UVW_off : 1; + uint8_t ABI_off : 1; + uint8_t na : 4; + uint8_t FILTER_disable : 1; + }; + + Fields fields; + uint8_t raw; + }; + + union Zposm { + struct __attribute__((__packed__)) Fields { + uint8_t ZPOSM : 8; + }; + + Fields fields; + uint8_t raw; + }; + + union Zposl { + struct __attribute__((__packed__)) Fields { + uint8_t ZPOSL : 6; + uint8_t Dia1_en : 1; + uint8_t Dia2_en : 1; + }; + + Fields fields; + uint8_t raw; + }; + + union Settings1 { + struct __attribute__((__packed__)) Fields { + uint8_t K_max : 3; + uint8_t K_min : 3; + uint8_t Dia3_en : 1; + uint8_t Dia4_en : 1; + }; + + Fields fields; + uint8_t raw; + }; + + union Settings2 { + struct __attribute__((__packed__)) Fields { + uint8_t IWIDTH : 1; + uint8_t NOISESET : 1; + uint8_t DIR : 1; + uint8_t UVW_ABI : 1; + uint8_t DAECDIS : 1; + uint8_t ABI_DEC : 1; + uint8_t Data_select : 1; + uint8_t PWMon : 1; + }; + + Fields fields; + uint8_t raw; + }; + + union Settings3 { + struct __attribute__((__packed__)) Fields { + uint8_t UVWPP : 3; + uint8_t HYS : 2; + uint8_t ABIRES : 3; + }; + + Fields fields; + uint8_t raw; + }; + + union Ecc { + struct __attribute__((__packed__)) Fields { + uint8_t ECC_chsum : 7; + uint8_t ECC_en : 1; + }; + + Fields fields; + uint8_t raw; + }; + + ///@} + + /** + * @brief Register addresses in the rotary sensor memory. + */ + ///@{ + static constexpr uint16_t errfl_addr{0x0001}; + static constexpr uint16_t disable_addr{0x0015}; + static constexpr uint16_t zposm_addr{0x0016}; + static constexpr uint16_t zposl_addr{0x0017}; + static constexpr uint16_t settings1_addr{0x0018}; + static constexpr uint16_t settings2_addr{0x0019}; + static constexpr uint16_t settings3_addr{0x001A}; + static constexpr uint16_t ecc_addr{0x001B}; + ///@} + + /** + * @brief Member variables to be configured in the rotary sensor. + */ + ///@{ + Disable disable; + Zposm zposm; + Zposl zposl; + Settings1 settings1; + Settings2 settings2; + Settings3 settings3; + Ecc ecc; + ///@} +}; + +#endif // MICRAS_PROXY_ROTARY_SENSOR_REG_HPP diff --git a/micras_proxy/include/micras/proxy/stopwatch.hpp b/micras_proxy/include/micras/proxy/stopwatch.hpp new file mode 100644 index 0000000..ee6c591 --- /dev/null +++ b/micras_proxy/include/micras/proxy/stopwatch.hpp @@ -0,0 +1,75 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_STOPWATCH_HPP +#define MICRAS_PROXY_STOPWATCH_HPP + +#include + +namespace micras::proxy { +/** + * @brief Class to measure the time elapsed between two events. + * + * @note The millisecond and microsecond timebases are independent, so an object can be reset in one + * unit and read in the other without the two interfering. + */ +class Stopwatch { +public: + /** + * @brief Construct a new Stopwatch object, starting both timebases. + */ + Stopwatch(); + + /** + * @brief Reset the milliseconds timer counter. + */ + void reset_ms(); + + /** + * @brief Reset the microseconds timer counter. + */ + void reset_us(); + + /** + * @brief Get the time elapsed since the last reset. + * + * @return Time elapsed in miliseconds. + */ + uint32_t elapsed_time_ms() const; + + /** + * @brief Get the time elapsed since the last reset. + * + * @return Time elapsed in microseconds. + */ + uint32_t elapsed_time_us() const; + + /** + * @brief Sleep for a given amount of time. + * + * @param time Time to sleep in milliseconds. + */ + static void sleep_ms(uint32_t time); + + /** + * @brief Sleep for a given amount of time. + * + * @param time Time to sleep in microseconds. + */ + static void sleep_us(uint32_t time); + +private: + /** + * @brief Value of the millisecond counter at the last reset. + */ + uint32_t counter_ms{}; + + /** + * @brief Value of the cycle counter at the last reset. + */ + uint32_t counter_cycles{}; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_STOPWATCH_HPP diff --git a/micras_proxy/include/micras/proxy/storage.hpp b/micras_proxy/include/micras/proxy/storage.hpp new file mode 100644 index 0000000..fd2fdd3 --- /dev/null +++ b/micras_proxy/include/micras/proxy/storage.hpp @@ -0,0 +1,155 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_STORAGE_HPP +#define MICRAS_PROXY_STORAGE_HPP + +#include +#include +#include +#include + +#include "micras/core/variable_pool.hpp" +#include "micras/hal/flash.hpp" + +namespace micras::proxy { +/** + * @brief Class for storing the persistent variables of a pool in the flash memory. + * + * @note Registration and loading are two separate phases. Everything registers into the pool + * first, and a single call to restore then fills whatever the image happens to carry, so nothing + * has to remember which variables have already been claimed. + */ +class Storage { +public: + /** + * @brief Configuration struct for the storage. + */ + struct Config { + uint16_t start_sector; + uint16_t number_of_sectors; + }; + + /** + * @brief Construct a new Storage object, reading and validating the image in the flash memory. + * + * @param config Configuration for the storage. + */ + explicit Storage(const Config& config); + + /** + * @brief Check if a valid image was found in the flash memory. + * + * @return True if an image was found and parsed, false if the storage started empty. + */ + bool is_valid() const; + + /** + * @brief Load every variable of the pool that the image carries. + * + * @note Entries whose name is not registered are ignored, and so are entries whose type no + * longer matches the one registered under that name, which is what stops a float saved by one + * firmware from being read back as an integer by the next. + * + * @param pool Pool to load the variables into. + * @return Number of variables that were loaded. + */ + std::size_t restore(core::VariablePool& pool); + + /** + * @brief Write every persistent variable of the pool to the flash memory. + * + * @note This operation blocks the processor for a few seconds, so it must only be called with + * the robot stopped. + * + * @note The header is written last and occupies a flash word of its own, so an interrupted + * save leaves it erased and the image is rejected on the next boot. That is what makes a torn + * write safe without a checksum, which the error correction of the flash memory would make + * redundant anyway. + * + * @param pool Pool to take the variables from. + * @return True if the data was successfully written to the flash, false otherwise. + */ + bool save(const core::VariablePool& pool); + +private: + /** + * @brief One variable as it is described in the flash image. + */ + struct Entry { + std::string_view name; + core::TypeCode type; + uint16_t offset; + uint16_t size; + }; + + /** + * @brief Read and validate the image currently in the flash memory. + */ + void load(); + + /** + * @brief Parse the entry at the front of a table and advance past it. + * + * @param table Remaining entry table, advanced past the entry that was read. + * @param entry Entry to fill. + * @return True if an entry could be read and its data fits in the value area, false otherwise. + */ + bool take_entry(std::span& table, Entry& entry) const; + + /** + * @brief Start symbol, to tell a written image apart from erased flash. + */ + static constexpr uint16_t start_symbol{0xABAB}; + + /** + * @brief Version of the layout described by this class, so that an image written by a firmware + * with a different layout is ignored instead of being misread. + */ + static constexpr uint8_t format_version{1}; + + /** + * @brief Number of bytes reserved for the header, which is a whole flash word so that writing + * it cannot disturb the rest of the image. + */ + static constexpr uint32_t header_size{hal::FlashWord::size}; + + /** + * @brief Number of bytes of an entry, not counting its name. + */ + static constexpr uint32_t entry_overhead{6}; + + /** + * @brief Description of every variable in the image, as a view over the flash memory. + */ + std::span entries; + + /** + * @brief Values of every variable in the image, as a view over the flash memory. + */ + std::span values; + + /** + * @brief Number of entries in the image. + */ + uint8_t entry_count{}; + + /** + * @brief Start sector of the storage in the flash memory. + */ + uint16_t start_sector; + + /** + * @brief Maximum number of sectors used by the storage in the flash memory. + */ + uint16_t number_of_sectors; + + /** + * @brief Whether a valid image was found in the flash memory. + */ + bool valid{}; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_STORAGE_HPP diff --git a/micras_proxy/include/micras/proxy/tick.hpp b/micras_proxy/include/micras/proxy/tick.hpp new file mode 100644 index 0000000..36de2c2 --- /dev/null +++ b/micras_proxy/include/micras/proxy/tick.hpp @@ -0,0 +1,75 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_TICK_HPP +#define MICRAS_PROXY_TICK_HPP + +#include + +namespace micras::proxy { +/** + * @brief Class that paces a loop at a fixed period. + * + * @details Every tick is scheduled from the previous one and not from when the wait was called, so + * the period does not drift with the time the loop body takes, and a body that overruns is reported + * rather than absorbed. The deadlines are kept in cycles of the core clock, which resolves a + * period of a hundred microseconds to a few parts per million, where a timebase in microseconds + * would jitter by a percent. + * + * @note The loop stays a plain loop: the wait spins on the cycle counter and no interrupt is + * involved. + */ +class Tick { +public: + /** + * @brief Configuration struct for the tick. + */ + struct Config { + uint32_t period_us; + }; + + /** + * @brief Construct a new Tick object, whose first tick is one period from now. + * + * @param config Configuration for the tick. + */ + explicit Tick(const Config& config); + + /** + * @brief Wait for the next tick. + * + * @return The number of periods since the previous tick, which is one unless the caller took + * longer than a period, in which case the ticks that were missed are included. + */ + uint32_t wait(); + + /** + * @brief Make the next tick one period from now. + * + * @note For after an operation that stalled the loop on purpose, with the robot stopped, so + * that the stall is not reported as missed ticks nor integrated as the time of one iteration. + */ + void restart(); + + /** + * @brief Get the time since the last tick. + * + * @return The time in microseconds, which is how long the body of the loop has taken so far. + */ + uint32_t elapsed_time_us() const; + +private: + /** + * @brief Period in cycles of the core clock. + */ + uint32_t period; + + /** + * @brief Value of the cycle counter at the last tick. + */ + uint32_t last_tick; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_TICK_HPP diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp new file mode 100644 index 0000000..a458b02 --- /dev/null +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -0,0 +1,147 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_TORQUE_SENSORS_HPP +#define MICRAS_PROXY_TORQUE_SENSORS_HPP + +#include +#include + +#include "micras/core/butterworth_filter.hpp" +#include "micras/hal/adc_dma.hpp" + +namespace micras::proxy { +/** + * @brief Class for acquiring torque sensors data. + */ +template +class TTorqueSensors { +public: + /** + * @brief Configuration struct for torque sensors. + * + * @note The zero reading is the reading with no current, as a fraction of the range of the + * converter, which is where a bidirectional amplifier sits. The currents and torques are signed + * about it. + */ + struct Config { + hal::AdcDma::Config adc; + float shunt_resistor; + float zero_reading; + float max_torque; + core::ButterworthFilter::Config filter; + }; + + /** + * @brief Construct a new TorqueSensors object. + * + * @param config Configuration for the torque sensors. + */ + explicit TTorqueSensors(const Config& config); + + /** + * @brief Calibrate the torque sensors. + * + * @note The readings start out signed about the zero reading of the configuration, which leaves + * the offset of each amplifier. This takes the current reading as the one of no current, so it + * is to be called with the motors disabled. Calling it again refines the baseline rather than + * discarding it. + */ + void calibrate(); + + /** + * @brief Update the torque sensors readings. + * + * @note Restarts the converter first if an error stopped it. + */ + void update(); + + /** + * @brief Get the torque from the sensor. + * + * @param sensor_index Index of the sensor. + * @return Torque reading from the sensor in N*m. + */ + float get_torque(uint8_t sensor_index) const; + + /** + * @brief Get the raw torque from the sensor without filtering. + * + * @param sensor_index Index of the sensor. + * @return Raw torque reading from the sensor. + */ + float get_torque_raw(uint8_t sensor_index) const; + + /** + * @brief Get the electric current through the sensor. + * + * @param sensor_index Index of the sensor. + * @return Current reading from the sensor in amps. + */ + float get_current(uint8_t sensor_index) const; + + /** + * @brief Get the raw electric current through the sensor without filtering. + * + * @param sensor_index Index of the sensor. + * @return Raw current reading from the sensor in amps. + */ + float get_current_raw(uint8_t sensor_index) const; + + /** + * @brief Get the ADC reading from the sensor. + * + * @param sensor_index Index of the sensor. + * @return Adc reading from the sensor from 0 to 1. + */ + float get_adc_reading(uint8_t sensor_index) const; + + /** + * @brief Check if the ADC was successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief ADC DMA handle. + */ + hal::AdcDma adc; + + /** + * @brief Buffer to store the ADC values. + */ + std::array buffer{}; + + /** + * @brief Reading of each sensor when no current is flowing. + */ + std::array base_reading{}; + + /** + * @brief Current that moves the reading across the whole range of the converter, in amps. + */ + float max_current; + + /** + * @brief Maximum torque that can be measured by the sensor. + */ + float max_torque; + + /** + * @brief Butterworth filters for the torque reading. + */ + std::array filters; + + /** + * @brief Flag to check if the ADC was initialized. + */ + bool initialized{}; +}; +} // namespace micras::proxy + +#include "micras/proxy/impl/torque_sensors.tpp" // IWYU pragma: export + +#endif // MICRAS_PROXY_TORQUE_SENSORS_HPP diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp new file mode 100644 index 0000000..87b4352 --- /dev/null +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -0,0 +1,386 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_WALL_SENSORS_HPP +#define MICRAS_PROXY_WALL_SENSORS_HPP + +#include +#include + +#include "micras/core/butterworth_filter.hpp" +#include "micras/hal/adc_dma.hpp" +#include "micras/hal/pwm.hpp" + +namespace micras::proxy { +/** + * @brief Class for controlling Wall Sensors. + * + * @details Each sensor is an infrared emitter next to a phototransistor, and what comes out of this + * class is the distance to whatever the emitter lights up, along its optical axis, in meters. The + * emitter has a narrow beam that lands entirely on the wall, which scatters it in every direction, + * so the light that comes back falls with the square of the distance. The receiver sits beside the + * emitter, not on its axis, and only takes in light close to its own axis, so it sees the lit spot + * at an angle that grows as the wall comes closer and loses light to its directivity: + * + * reading = k * 2^(-(atan(receiver_offset / distance) / receiver_half_angle)^2) / distance^2 + * + * One reading at a known distance therefore calibrates a sensor, by fixing k. With no receiver + * offset this is the inverse square law, distance = reference_distance * sqrt(reference_reading / + * reading). With one, the reading peaks at a short distance and falls again closer than that, so a + * reading above the peak is reported as the distance of the peak, as a saturated one is. + * + * @tparam num_of_sensors Number of sensors. + */ +template +class TWallSensors { +public: + /** + * @brief Configuration struct for wall sensors. + * + * @note The reference reading of a sensor is what it reads at its reference distance, and is + * what a calibration replaces. Both filters run at the rate the sensors produce values, not at + * the rate of the control loop, so that is the sampling frequency they have to be given. + * A reading below the noise floor, or one that works out to more than the maximum distance, + * means nothing is within range. A reading above the maximum is saturated, and is reported as + * the distance of the maximum reading. A wall is considered present when the slow distance is + * below the wall distance, and absent again when it goes above it by the hysteresis. The + * receiver offset is the distance from the optical axis of the emitter to the receiver, and the + * receiver half angle the angle off its own axis at which the receiver's sensitivity halves. A + * dark reading, taken with the emitter off, at or above the blind reading means that ambient + * light saturates the receiver, which then cannot see a wall with the emitter on either. + */ + struct Config { + hal::AdcDma::Config adc; + std::array led_pwms; + float emitter_duty_cycle; + core::ButterworthFilter::Config fast_filter; + core::ButterworthFilter::Config slow_filter; + std::array reference_readings; + std::array reference_distances; + float receiver_offset; + float receiver_half_angle; + float noise_floor; + float max_reading; + float max_distance; + float blind_reading; + float wall_distance; + float wall_hysteresis; + uint16_t calibration_samples; + }; + + /** + * @brief Distance measured by one sensor. + * + * @note The fast distance is for everything that is a position and the slow one for deciding + * whether there is a wall. The reading is valid when the sensor sees anything above its noise, + * and it is new for a single update after the sensor produces a value. The dark reading is the + * one taken with the emitter off, as a fraction of the full scale, which is the ambient light + * the receiver sees. The reading is blind when that light saturates the receiver, and then says + * nothing about a wall. + */ + struct Reading { + float distance; + float slow_distance; + float dark; + bool valid; + bool blind; + bool is_new; + }; + + /** + * @brief Construct a new WallSensors object. + * + * @param config Configuration for the wall sensors. + */ + explicit TWallSensors(const Config& config); + + /** + * @brief Turn on the wall sensors IR LED. + */ + void turn_on(); + + /** + * @brief Turn off the wall sensors IR LED. + */ + void turn_off(); + + /** + * @brief Update the wall sensors readings. + * + * @note Nothing is recomputed unless the converter completed a sequence since the last call, + * so this can be called faster than the sensors produce values. Restarts the converter first if + * an error stopped it. + */ + void update(); + + /** + * @brief Get the distance measured by a sensor. + * + * @param sensor_index Index of the sensor. + * @return The reading of the sensor. + */ + const Reading& get_reading(uint8_t sensor_index) const; + + /** + * @brief Get the observation from a sensor. + * + * @param sensor_index Index of the sensor. + * @return True if the sensor detects a wall, false otherwise. + */ + bool get_wall(uint8_t sensor_index) const; + + /** + * @brief Get the light a sensor receives from its emitter, with the ambient light removed. + * + * @param sensor_index Index of the sensor. + * @return Reading from the sensor from 0 to 1. + */ + float get_intensity(uint8_t sensor_index) const; + + /** + * @brief Get the reading of a sensor with its emitter off, as a fraction of the full scale. + * + * @note Ambient light raises the output of the receiver, and the emitter raises it further, so + * the dark reading is the lower of the pair. + * + * @param sensor_index Index of the sensor. + * @return The dark reading. + */ + float get_dark_reading(uint8_t sensor_index) const; + + /** + * @brief Start calibrating a sensor, with the robot placed at the reference distance. + * + * @note The readings are averaged over the configured number of samples, which takes that many + * updates with a new value. + * + * @param sensor_index Index of the sensor. + */ + void calibrate_sensor(uint8_t sensor_index); + + /** + * @brief Check if a calibration is in progress. + * + * @return True while any sensor is still averaging. + */ + bool is_calibrating() const; + + /** + * @brief Get the reference reading of a sensor, which is the result of its last calibration. + * + * @param sensor_index Index of the sensor. + * @return The reading at the reference distance, from 0 to 1. + */ + float get_reference_reading(uint8_t sensor_index) const; + + /** + * @brief Get how much the readings varied during the last calibration of a sensor. + * + * @note A spread that is not small means the robot moved, or something else was wrong, while + * the sensor was being calibrated. + * + * @param sensor_index Index of the sensor. + * @return The standard deviation of the readings, as a fraction of their mean. + */ + float get_calibration_spread(uint8_t sensor_index) const; + + /** + * @brief Check if the ADC was initialized, its scan matches the buffer layout, and the emitters + * run at the rate the filters were designed for. + * + * @note The rate of the emitters lives in the peripheral configuration and the one of the filters + * in the constants, and nothing else would notice the day only one of them changes. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief Largest relative difference between the rate of the emitters and the one of the filters. + */ + static constexpr float frequency_tolerance{0.01F}; + + /** + * @brief Number of distances the shape of the reading is tabulated at. + */ + static constexpr uint8_t shape_points{64}; + + /** + * @brief Get how the reading varies with the distance, up to the calibrated constant. + * + * @param distance Distance to the wall in meters. + * @return The reading at that distance divided by k. + */ + float shape(float distance) const; + + /** + * @brief Get the distance a reading corresponds to. + * + * @param sensor_index Index of the sensor. + * @param intensity Reading from 0 to 1, above the noise floor. + * @return Distance in meters. + */ + float to_distance(uint8_t sensor_index, float intensity) const; + + /** + * @brief Averaging of the readings of one sensor during its calibration. + */ + struct Calibration { + float sum; + float squared_sum; + uint16_t samples_left; + float spread; + }; + + /** + * @brief ADC DMA handle. + */ + hal::AdcDma adc; + + /** + * @brief PWM handles for the infrared LEDs, one for each sensor. + */ + std::array led_pwms; + + /** + * @brief Duty cycle of the emitters while the sensors are on. + */ + float emitter_duty_cycle; + + /** + * @brief Buffer the DMA writes to, holding one emitter on and one emitter off scan. + * + * @details The emitter PWM timer is center aligned and triggers the ADC on its update event, so + * the conversion sequence runs twice per emitter period: once at the underflow and once at the + * overflow. The emitters fire in two groups, one centered on each of those instants, so every + * scan reads half of the sensors lit and the other half dark, no sensor is ever lit by the + * emitter of its neighbor, and two consecutive scans hold a lit and a dark reading of every + * sensor. Their difference is the reflected signal with the ambient light canceled, and taking + * its absolute value makes the result independent of which half currently holds which scan. + * + * @note This depends on the ADC scanning exactly num_of_sensors channels, on the emitter timer + * being center aligned with its trigger on the update event, on the two groups being told + * apart by the inverted flag of their PWM configuration, and on the emitter being on for at + * least the settling time of the receiver before the scan starts and the duration of the scan + * after it. The first of those is checked by the constructor; the others live in the + * configuration. + */ + std::array buffer{}; + + /** + * @brief Copy of the buffer taken when a pair of scans completes, so that a pair is never torn. + */ + std::array snapshot{}; + + /** + * @brief The pair of scans the readings are computed from. + */ + std::array scans{}; + + /** + * @brief Number of pairs of scans completed when the readings were last computed. + */ + uint32_t sequence{}; + + /** + * @brief Butterworth filters for the fast distances. + */ + std::array fast_filters; + + /** + * @brief Butterworth filters for the slow distances. + */ + std::array slow_filters; + + /** + * @brief Reading of each sensor at its reference distance. + */ + std::array reference_readings; + + /** + * @brief Distance each sensor was calibrated at. + */ + std::array reference_distances; + + /** + * @brief Distance from the axis of each emitter to its receiver. + */ + float receiver_offset; + + /** + * @brief Angle off its axis at which the sensitivity of the receiver halves. + */ + float receiver_half_angle; + + /** + * @brief Distances the shape of the reading is tabulated at, from its peak to the maximum distance. + */ + std::array shape_distances{}; + + /** + * @brief One over the square root of the shape at each tabulated distance, which grows with it. + */ + std::array shape_scales{}; + + /** + * @brief Reading below which there is only noise. + */ + float noise_floor; + + /** + * @brief Distance beyond which nothing is considered within range. + */ + float max_distance; + + /** + * @brief Reading above which the sensor is saturated. + */ + float max_reading; + + /** + * @brief Dark reading at and above which the receiver is blinded by ambient light. + */ + float blind_reading; + + /** + * @brief Slow distance below which a wall is present. + */ + float wall_distance; + + /** + * @brief How far above the wall distance the slow distance has to go for the wall to be absent. + */ + float wall_hysteresis; + + /** + * @brief Number of readings averaged by a calibration. + */ + uint16_t calibration_samples; + + /** + * @brief Distance measured by each sensor. + */ + std::array readings{}; + + /** + * @brief Whether each sensor detects a wall. + */ + std::array walls{}; + + /** + * @brief Calibration of each sensor. + */ + std::array calibrations{}; + + /** + * @brief Flag to check if the ADC was initialized. + */ + bool initialized{}; +}; +} // namespace micras::proxy + +#include "micras/proxy/impl/wall_sensors.tpp" // IWYU pragma: export + +#endif // MICRAS_PROXY_WALL_SENSORS_HPP diff --git a/micras_proxy/include/micras/proxy/watchdog.hpp b/micras_proxy/include/micras/proxy/watchdog.hpp new file mode 100644 index 0000000..5d0d1c5 --- /dev/null +++ b/micras_proxy/include/micras/proxy/watchdog.hpp @@ -0,0 +1,93 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_WATCHDOG_HPP +#define MICRAS_PROXY_WATCHDOG_HPP + +#include + +namespace micras::proxy { +/** + * @brief Class for keeping the watchdog of the microcontroller fed. + * + * @note Once started the watchdog cannot be stopped, so anything that hangs for longer than its + * timeout resets the microcontroller, which brings every actuator back to its inactive state. + */ +class Watchdog { +public: + /** + * @brief Configuration struct for the watchdog. + */ + struct Config { + uint32_t timeout_ms; + }; + + /** + * @brief Temporary widening of the timeout, which ends with the life of the object. + * + * @note Meant for the few operations that legitimately stall the core for longer than the + * control loop budget, such as erasing a flash sector, and only with the robot stopped. + */ + class Extension { + public: + /** + * @brief Widen the timeout of a watchdog. + * + * @param watchdog The watchdog. + * @param timeout_ms The timeout while the extension lives, in milliseconds. + */ + Extension(Watchdog& watchdog, uint32_t timeout_ms); + + /** + * @brief Bring the timeout back to the configured one. + */ + ~Extension(); + + /** + * @brief Special member functions deleted, since an extension ends exactly once. + */ + ///@{ + Extension(const Extension&) = delete; + Extension(Extension&&) = delete; + Extension& operator=(const Extension&) = delete; + Extension& operator=(Extension&&) = delete; + ///@} + + private: + /** + * @brief Watchdog whose timeout was widened. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) the extension cannot outlive the watchdog + Watchdog& watchdog; + }; + + /** + * @brief Construct a new Watchdog object, starting the watchdog. + * + * @param config Configuration for the watchdog. + */ + explicit Watchdog(const Config& config); + + /** + * @brief Tell the watchdog that the program is alive. + */ + void refresh(); + + /** + * @brief Widen the timeout for as long as the returned object lives. + * + * @param timeout_ms The timeout during the extension, in milliseconds. + * @return The extension. + */ + [[nodiscard]] Extension extend(uint32_t timeout_ms); + +private: + /** + * @brief Timeout outside of any extension, in milliseconds. + */ + uint32_t timeout_ms; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_WATCHDOG_HPP diff --git a/micras_proxy/models/CMakeLists.txt b/micras_proxy/models/CMakeLists.txt new file mode 100644 index 0000000..cc9be43 --- /dev/null +++ b/micras_proxy/models/CMakeLists.txt @@ -0,0 +1,18 @@ +# SPI devices for the host micras_hal: they know the SPI device slot of the host backend and nothing +# else. A consumer's bindings attach them to its buses and feed them. +add_library(micras_proxy_models STATIC + src/as5047u_model.cpp + src/lsm6dsv_model.cpp +) + +add_library(micras::proxy_models ALIAS micras_proxy_models) + +target_include_directories(micras_proxy_models PUBLIC + include +) + +target_link_libraries(micras_proxy_models PUBLIC + micras::hal +) + +micras_lib_setup_library(micras_proxy_models) diff --git a/micras_proxy/models/include/micras/models/as5047u_model.hpp b/micras_proxy/models/include/micras/models/as5047u_model.hpp new file mode 100644 index 0000000..c24ecdb --- /dev/null +++ b/micras_proxy/models/include/micras/models/as5047u_model.hpp @@ -0,0 +1,182 @@ +/** + * @file + * + * @brief The AS5047U magnetic position sensor, as its SPI interface shows it. + */ + +#ifndef MICRAS_MODELS_AS5047U_MODEL_HPP +#define MICRAS_MODELS_AS5047U_MODEL_HPP + +#include +#include +#include +#include +#include + +#include "micras/hal/host/spi_device.hpp" + +namespace micras::models { +/** + * @brief The registers of an AS5047U behind an SPI chip select, in its 24-bit frame mode. + * + * @details Every frame is three bytes. A command frame carries a don't care + * bit, the read flag, a 14-bit address and a CRC; a data frame carries + * the warning and error flags, 14 bits of data and a CRC. The CRC is a + * CRC-8 of the first two bytes, polynomial 0x1D, initial value 0xC4, + * final XOR 0xFF. Answers are pipelined by one frame: what the sensor + * sends during a frame answers the frame before it, whatever the chip + * select did in between. + * + * @details A read command is answered with the register's content. A write + * command is answered with the register's old content and is followed + * by a data frame holding the new one, which is answered with the new + * content. + * + * @note Every frame's CRC is checked. A frame with a wrong CRC is ignored, and + * a write whose data frame it was is dropped; it sets the CRC error bit of + * ERRFL. A chip select raised in the middle of a frame drops it and sets + * the framing error bit. The error flag of every answer is set while + * ERRFL is not zero, and reading ERRFL clears it. + * + * @note DISABLE, ZPOSM, ZPOSL, SETTINGS1 to SETTINGS3 and ECC are held, zero + * after power-on, as volatile registers. Writes to any other address are + * ignored and every other register reads zero: the angle, the magnitude + * and the diagnostics are not modeled. So is the position: on the robot it + * reaches the firmware through the quadrature output and a timer in + * encoder mode, and so it does in a simulation. + */ +class As5047uModel : public hal::host::SpiDevice { +public: + /** + * @brief Bytes in a frame. + */ + static constexpr std::size_t frame_size{3}; + + /** + * @brief Addresses of the registers the model holds. + */ + ///@{ + static constexpr uint16_t nop_address{0x0000}; + static constexpr uint16_t errfl_address{0x0001}; + static constexpr uint16_t disable_address{0x0015}; + static constexpr uint16_t zposm_address{0x0016}; + static constexpr uint16_t zposl_address{0x0017}; + static constexpr uint16_t settings1_address{0x0018}; + static constexpr uint16_t settings2_address{0x0019}; + static constexpr uint16_t settings3_address{0x001A}; + static constexpr uint16_t ecc_address{0x001B}; + ///@} + + /** + * @brief Bits of ERRFL the model sets. + */ + ///@{ + static constexpr uint16_t framing_error{1U << 4U}; + static constexpr uint16_t crc_error{1U << 6U}; + ///@} + + /** + * @brief Power the sensor on, in SPI mode 1. + */ + As5047uModel(); + + /** + * @brief Start a transaction at the start of a frame. + */ + void select() override; + + /** + * @brief Shift the pending answer out and the frame in, acting on every frame completed. + * + * @param transmitted Bytes the controller sends. + * @param received Bytes of the answer to the previous frame. + */ + void exchange(std::span transmitted, std::span received) override; + + /** + * @brief End the transaction, dropping an incomplete frame. + */ + void deselect() override; + + /** + * @brief Get a register without the side effects of reading it over SPI. + * + * @param address Register address. + * @return The register's content, zero for one not modeled. + */ + uint16_t peek(uint16_t address) const; + + /** + * @brief Compute the CRC of a frame. + * + * @param high First byte of the frame. + * @param low Second byte of the frame. + * @return The third byte of a valid frame. + */ + static uint8_t crc(uint8_t high, uint8_t low); + +private: + /** + * @brief Act on one complete frame and prepare the answer to it. + * + * @param frame The frame, most significant byte first. + */ + void process(const std::array& frame); + + /** + * @brief Build a data frame answering with a value, the error flag set while ERRFL is not zero. + * + * @param data The value. + * @return The frame, in its low 24 bits. + */ + uint32_t answer_with(uint16_t data) const; + + /** + * @brief Get the register a write to an address changes. + * + * @param address Register address. + * @return The register, or null when the address is not writable. + */ + uint8_t* writable(uint16_t address); + + /** + * @brief Volatile registers, each 8 bits wide. + */ + ///@{ + uint8_t disable{0}; + uint8_t zposm{0}; + uint8_t zposl{0}; + uint8_t settings1{0}; + uint8_t settings2{0}; + uint8_t settings3{0}; + uint8_t ecc{0}; + ///@} + + /** + * @brief Error flags, in ERRFL's layout. + */ + uint16_t errfl{0}; + + /** + * @brief Frame shifted out next: the answer to the last complete frame. + */ + uint32_t answer; + + /** + * @brief Bytes of the frame being received. + */ + std::array frame{}; + + /** + * @brief Number of bytes of the frame received so far. + */ + std::size_t received_bytes{0}; + + /** + * @brief Address whose write command was received, so the next frame holds its data. + */ + std::optional pending_write; +}; +} // namespace micras::models + +#endif // MICRAS_MODELS_AS5047U_MODEL_HPP diff --git a/micras_proxy/models/include/micras/models/lsm6dsv_model.hpp b/micras_proxy/models/include/micras/models/lsm6dsv_model.hpp new file mode 100644 index 0000000..6f53b4a --- /dev/null +++ b/micras_proxy/models/include/micras/models/lsm6dsv_model.hpp @@ -0,0 +1,200 @@ +/** + * @file + * + * @brief The LSM6DSV inertial measurement unit, as its SPI interface shows it. + */ + +#ifndef MICRAS_MODELS_LSM6DSV_MODEL_HPP +#define MICRAS_MODELS_LSM6DSV_MODEL_HPP + +#include +#include +#include +#include + +#include "micras/hal/host/spi_device.hpp" + +namespace micras::models { +/** + * @brief The registers of an LSM6DSV behind an SPI chip select, fed with samples of the motion it measures. + * + * @details The first byte of a transaction is the command: the read flag in the + * top bit and the register address below it. Every following byte + * reads or writes one register, and the address advances after each + * one while CTRL3 IF_INC is set, as it is after reset. + * + * @note The register file is the 128 registers of the main page, with the + * datasheet's defaults: WHO_AM_I 0x70, PIN_CTRL 0x23, CTRL3 0x44 + * (IF_INC and BDU) and zero everywhere else. Writes to read-only and + * reserved registers are ignored. While FUNC_CFG_ACCESS selects the + * embedded functions or the sensor hub page, which are not modeled, every + * other register reads zero and ignores writes. + * + * @note Writing SW_POR in FUNC_CFG_ACCESS or SW_RESET in CTRL3 restores the + * defaults, and the rest of that transaction is ignored. + * + * @note A sample updates the outputs of each sensor whose output data rate is + * not off (ODR_G in CTRL2, ODR_XL in CTRL1), encoded with the full scale + * its register holds (FS_G in CTRL6, FS_XL in CTRL8), and sets that + * sensor's data ready bit in STATUS_REG (GDA, XLDA). Reading the high + * byte of one of the sensor's axes clears the bit, so the burst that + * reads a sample clears it. A transaction happens between two samples, + * which is what block data update guarantees on the chip. + * + * @note The rates in CTRL1, CTRL2 and HAODR_CFG only say whether a sensor is on + * and are decoded for the one feeding the samples: the samples arrive at + * whatever rate push_sample is called at. + */ +class Lsm6dsvModel : public hal::host::SpiDevice { +public: + /** + * @brief Registers of the main page. + */ + static constexpr std::size_t register_count{128}; + + /** + * @brief Value of WHO_AM_I. + */ + static constexpr uint8_t device_id{0x70}; + + /** + * @brief Power the chip on, in SPI mode 3. + */ + Lsm6dsvModel(); + + /** + * @brief Start a transaction. + */ + void select() override; + + /** + * @brief Take the command, then read or write one register per byte. + * + * @param transmitted Bytes the controller sends. + * @param received Register values read, zero while the command is being sent and during writes. + */ + void exchange(std::span transmitted, std::span received) override; + + /** + * @brief End the transaction. + */ + void deselect() override; + + /** + * @brief Deliver a new sample of the motion the chip measures. + * + * @param angular_velocity Angular velocity about the chip's x, y and z axes, in rad/s. + * @param linear_acceleration Specific force along the chip's x, y and z axes, in m/s^2. + */ + void push_sample(const std::array& angular_velocity, const std::array& linear_acceleration); + + /** + * @brief Get a register without the side effects of reading it over SPI. + * + * @param address Register address; the top bit is ignored. + * @return The register's value. + */ + uint8_t peek(uint8_t address) const; + + /** + * @brief Get the output data rate of the gyroscope, from CTRL2 and HAODR_CFG. + * + * @return The rate, in hertz, zero when the gyroscope is off. + */ + double gyroscope_data_rate() const; + + /** + * @brief Get the output data rate of the accelerometer, from CTRL1 and HAODR_CFG. + * + * @return The rate, in hertz, zero when the accelerometer is off. + */ + double accelerometer_data_rate() const; + + /** + * @brief Get the sensitivity of the gyroscope at the full scale in CTRL6. + * + * @return Angular velocity of one count, in rad/s, zero for a reserved full scale. + */ + double gyroscope_sensitivity() const; + + /** + * @brief Get the sensitivity of the accelerometer at the full scale in CTRL8. + * + * @return Specific force of one count, in m/s^2. + */ + double accelerometer_sensitivity() const; + +private: + /** + * @brief Restore every register to its default. + */ + void reset(); + + /** + * @brief Read a register over SPI, clearing the data ready bit it completes. + * + * @param address Register address. + * @return The register's value. + */ + uint8_t read(uint8_t address); + + /** + * @brief Write a register over SPI, applying what the write triggers. + * + * @param address Register address. + * @param value Value written. + */ + void write(uint8_t address, uint8_t value); + + /** + * @brief Check whether the main page is the one the SPI interface addresses. + * + * @return True unless FUNC_CFG_ACCESS selects another page. + */ + bool main_page() const; + + /** + * @brief Write the three axes of one sensor into its output registers. + * + * @param first Address of the low byte of the x axis. + * @param values Values of the three axes. + * @param sensitivity Value of one count; zero writes zero. + */ + void encode(uint8_t first, const std::array& values, double sensitivity); + + /** + * @brief Decode an output data rate. + * + * @param odr ODR field of CTRL1 or CTRL2. + * @return The rate, in hertz, with the high accuracy set HAODR_CFG selects. + */ + double data_rate(uint8_t odr) const; + + /** + * @brief The register file. + */ + std::array registers{}; + + /** + * @brief Register the next byte of the transaction reads or writes. + */ + uint8_t address{0}; + + /** + * @brief Whether the command byte of the transaction was received. + */ + bool addressed{false}; + + /** + * @brief Whether the transaction reads. + */ + bool reading{false}; + + /** + * @brief Whether the rest of the transaction is ignored, after a reset inside it. + */ + bool ignoring{false}; +}; +} // namespace micras::models + +#endif // MICRAS_MODELS_LSM6DSV_MODEL_HPP diff --git a/micras_proxy/models/src/as5047u_model.cpp b/micras_proxy/models/src/as5047u_model.cpp new file mode 100644 index 0000000..989820f --- /dev/null +++ b/micras_proxy/models/src/as5047u_model.cpp @@ -0,0 +1,191 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/hal/host/spi_device.hpp" +#include "micras/models/as5047u_model.hpp" + +namespace micras::models { +namespace { +/** + * @brief Generator polynomial of the frame CRC, without its top bit. + */ +constexpr uint8_t crc_polynomial{0x1D}; + +/** + * @brief Initial value of the frame CRC. + */ +constexpr uint8_t crc_initial_value{0xC4}; + +/** + * @brief Final XOR of the frame CRC. + */ +constexpr uint8_t crc_final_xor{0xFF}; + +/** + * @brief Bits of the 14-bit address or data field. + */ +constexpr uint16_t field_mask{0x3FFF}; + +/** + * @brief Position of the address or data field in a frame. + */ +constexpr uint32_t field_shift{8}; + +/** + * @brief Read flag of a command frame, and error flag of a data frame. + */ +constexpr uint32_t flag_bit{1U << 22U}; +} // namespace + +As5047uModel::As5047uModel() : SpiDevice{Mode::MODE_1}, answer{this->answer_with(0)} { } + +void As5047uModel::select() { + this->received_bytes = 0; +} + +void As5047uModel::exchange(std::span transmitted, std::span received) { + const std::size_t size = std::min(transmitted.size(), received.size()); + + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) below the size of both spans + for (std::size_t index = 0; index < size; index++) { + const uint32_t shift = 8U * static_cast(frame_size - 1 - this->received_bytes); + received[index] = static_cast(this->answer >> shift); + this->frame.at(this->received_bytes) = transmitted[index]; + this->received_bytes++; + + if (this->received_bytes == frame_size) { + this->received_bytes = 0; + this->process(this->frame); + } + } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) +} + +void As5047uModel::deselect() { + if (this->received_bytes != 0) { + this->errfl |= framing_error; + this->pending_write.reset(); + } + + this->received_bytes = 0; +} + +uint16_t As5047uModel::peek(uint16_t address) const { + switch (address) { + case errfl_address: + return this->errfl; + + case disable_address: + return this->disable; + + case zposm_address: + return this->zposm; + + case zposl_address: + return this->zposl; + + case settings1_address: + return this->settings1; + + case settings2_address: + return this->settings2; + + case settings3_address: + return this->settings3; + + case ecc_address: + return this->ecc; + + default: + return 0; + } +} + +uint8_t As5047uModel::crc(uint8_t high, uint8_t low) { + uint8_t remainder = crc_initial_value; + + for (const uint8_t byte : {high, low}) { + remainder ^= byte; + + for (uint8_t bit = 0; bit < 8; bit++) { + remainder = (remainder & 0x80U) != 0 ? static_cast((remainder << 1U) ^ crc_polynomial) : + static_cast(remainder << 1U); + } + } + + return remainder ^ crc_final_xor; +} + +void As5047uModel::process(const std::array& frame) { + const uint32_t raw = static_cast(std::get<0>(frame)) << 16U | + static_cast(std::get<1>(frame)) << 8U | std::get<2>(frame); + const auto field = static_cast((raw >> field_shift) & field_mask); + + if (crc(std::get<0>(frame), std::get<1>(frame)) != std::get<2>(frame)) { + this->errfl |= crc_error; + this->pending_write.reset(); + this->answer = this->answer_with(0); + return; + } + + if (this->pending_write.has_value()) { + const uint16_t address = this->pending_write.value(); + this->pending_write.reset(); + + if (uint8_t* const target = this->writable(address); target != nullptr) { + *target = static_cast(field); + } + + this->answer = this->answer_with(this->peek(address)); + return; + } + + this->answer = this->answer_with(this->peek(field)); + + if ((raw & flag_bit) == 0) { + this->pending_write = field; + } else if (field == errfl_address) { + this->errfl = 0; + } +} + +uint32_t As5047uModel::answer_with(uint16_t data) const { + const uint32_t upper = (this->errfl != 0 ? flag_bit : 0U) | static_cast(data & field_mask) << field_shift; + return upper | crc(static_cast(upper >> 16U), static_cast(upper >> 8U)); +} + +uint8_t* As5047uModel::writable(uint16_t address) { + switch (address) { + case disable_address: + return &this->disable; + + case zposm_address: + return &this->zposm; + + case zposl_address: + return &this->zposl; + + case settings1_address: + return &this->settings1; + + case settings2_address: + return &this->settings2; + + case settings3_address: + return &this->settings3; + + case ecc_address: + return &this->ecc; + + default: + return nullptr; + } +} +} // namespace micras::models diff --git a/micras_proxy/models/src/lsm6dsv_model.cpp b/micras_proxy/models/src/lsm6dsv_model.cpp new file mode 100644 index 0000000..c794723 --- /dev/null +++ b/micras_proxy/models/src/lsm6dsv_model.cpp @@ -0,0 +1,279 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/hal/host/spi_device.hpp" +#include "micras/models/lsm6dsv_model.hpp" + +namespace micras::models { +namespace { +/** + * @brief Top bit of the command byte: set to read, clear to write. + */ +constexpr uint8_t read_flag{0x80}; + +/** + * @brief Bits of the command byte and of an address that name a register. + */ +constexpr uint8_t address_mask{0x7F}; + +/** + * @brief Addresses of the registers the model gives a behavior to. + */ +///@{ +constexpr uint8_t func_cfg_access{0x01}; +constexpr uint8_t pin_ctrl{0x02}; +constexpr uint8_t who_am_i{0x0F}; +constexpr uint8_t ctrl1{0x10}; +constexpr uint8_t ctrl2{0x11}; +constexpr uint8_t ctrl3{0x12}; +constexpr uint8_t ctrl6{0x15}; +constexpr uint8_t ctrl8{0x17}; +constexpr uint8_t status_reg{0x1E}; +constexpr uint8_t outx_l_g{0x22}; +constexpr uint8_t outx_l_a{0x28}; +constexpr uint8_t haodr_cfg{0x62}; +///@} + +/** + * @brief Bits of FUNC_CFG_ACCESS: the software power-on reset, and the two that select another page. + */ +///@{ +constexpr uint8_t sw_por{0x04}; +constexpr uint8_t other_pages{0xC0}; +///@} + +/** + * @brief Bits of CTRL3: the software reset, the self-clearing reboot, and the address auto-increment. + */ +///@{ +constexpr uint8_t sw_reset{0x01}; +constexpr uint8_t boot{0x80}; +constexpr uint8_t if_inc{0x04}; +///@} + +/** + * @brief Data ready bits of STATUS_REG. + */ +///@{ +constexpr uint8_t xlda{0x01}; +constexpr uint8_t gda{0x02}; +///@} + +/** + * @brief Defaults of the registers that do not reset to zero. + */ +constexpr std::array, 3> nonzero_defaults{{ + {pin_ctrl, 0x23}, + {who_am_i, Lsm6dsvModel::device_id}, + {ctrl3, 0x44}, +}}; + +/** + * @brief First and last address of every range of registers the SPI interface can write. + */ +constexpr std::array, 7> writable_ranges{{ + {0x01, 0x03}, + {0x06, 0x0E}, + {0x10, 0x19}, + {0x50, 0x51}, + {0x54, 0x5F}, + {0x62, 0x6B}, + {0x6F, 0x75}, +}}; + +/** + * @brief Sensitivity of the gyroscope for each FS_G code, in mdps per count; zero for the reserved codes. + */ +constexpr std::array gyroscope_mdps_per_count{ + 4.375, 8.75, 17.5, 35.0, 70.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 140.0, 0.0, 0.0, 0.0, +}; + +/** + * @brief Sensitivity of the accelerometer for each FS_XL code, in mg per count. + */ +constexpr std::array accelerometer_mg_per_count{0.061, 0.122, 0.244, 0.488}; + +/** + * @brief Conversions from the datasheet's units. + */ +///@{ +constexpr double mdps_to_radps{std::numbers::pi / 180000.0}; +constexpr double mg_to_mps2{0.00980665}; +///@} + +/** + * @brief Rate of ODR code 3 for each high accuracy set of HAODR_CFG, in hertz; each code above doubles it. + */ +constexpr std::array base_rates{15.0, 15.625, 12.5, 15.0}; +} // namespace + +/** + * @brief Check whether the SPI interface can write a register. + * + * @param address Register address. + * @return True for a read-write register. + */ +static bool writable(uint8_t address) { + return std::ranges::any_of(writable_ranges, [address](const auto& range) { + return address >= range.first and address <= range.second; + }); +} + +Lsm6dsvModel::Lsm6dsvModel() : SpiDevice{Mode::MODE_3} { + this->reset(); +} + +void Lsm6dsvModel::select() { + this->addressed = false; + this->ignoring = false; +} + +void Lsm6dsvModel::exchange(std::span transmitted, std::span received) { + const std::size_t size = std::min(transmitted.size(), received.size()); + + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) below the size of both spans + for (std::size_t index = 0; index < size; index++) { + received[index] = 0; + + if (this->ignoring) { + continue; + } + + if (not this->addressed) { + this->reading = (transmitted[index] & read_flag) != 0; + this->address = transmitted[index] & address_mask; + this->addressed = true; + continue; + } + + if (this->reading) { + received[index] = this->read(this->address); + } else { + this->write(this->address, transmitted[index]); + } + + if ((this->registers.at(ctrl3) & if_inc) != 0) { + this->address = (this->address + 1) & address_mask; + } + } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) +} + +void Lsm6dsvModel::deselect() { + this->addressed = false; + this->ignoring = false; +} + +void Lsm6dsvModel::push_sample( + const std::array& angular_velocity, const std::array& linear_acceleration +) { + if ((this->registers.at(ctrl2) & 0x0F) != 0) { + this->encode(outx_l_g, angular_velocity, this->gyroscope_sensitivity()); + this->registers.at(status_reg) |= gda; + } + + if ((this->registers.at(ctrl1) & 0x0F) != 0) { + this->encode(outx_l_a, linear_acceleration, this->accelerometer_sensitivity()); + this->registers.at(status_reg) |= xlda; + } +} + +uint8_t Lsm6dsvModel::peek(uint8_t address) const { + return this->registers.at(address & address_mask); +} + +double Lsm6dsvModel::gyroscope_data_rate() const { + return this->data_rate(this->registers.at(ctrl2) & 0x0F); +} + +double Lsm6dsvModel::accelerometer_data_rate() const { + return this->data_rate(this->registers.at(ctrl1) & 0x0F); +} + +double Lsm6dsvModel::gyroscope_sensitivity() const { + return gyroscope_mdps_per_count.at(this->registers.at(ctrl6) & 0x0F) * mdps_to_radps; +} + +double Lsm6dsvModel::accelerometer_sensitivity() const { + return accelerometer_mg_per_count.at(this->registers.at(ctrl8) & 0x03) * mg_to_mps2; +} + +void Lsm6dsvModel::reset() { + this->registers.fill(0); + + for (const auto& [address, value] : nonzero_defaults) { + this->registers.at(address) = value; + } +} + +uint8_t Lsm6dsvModel::read(uint8_t address) { + if (not this->main_page() and address != func_cfg_access) { + return 0; + } + + const uint8_t value = this->registers.at(address); + + if (address >= outx_l_g and address < outx_l_a and (address & 1) != 0) { + this->registers.at(status_reg) &= static_cast(~gda); + } else if (address >= outx_l_a and address < outx_l_a + 6 and (address & 1) != 0) { + this->registers.at(status_reg) &= static_cast(~xlda); + } + + return value; +} + +void Lsm6dsvModel::write(uint8_t address, uint8_t value) { + if ((not this->main_page() and address != func_cfg_access) or not writable(address)) { + return; + } + + if ((address == func_cfg_access and (value & sw_por) != 0) or (address == ctrl3 and (value & sw_reset) != 0)) { + this->reset(); + this->ignoring = true; + return; + } + + this->registers.at(address) = address == ctrl3 ? static_cast(value & ~boot) : value; +} + +bool Lsm6dsvModel::main_page() const { + return (this->registers.at(func_cfg_access) & other_pages) == 0; +} + +void Lsm6dsvModel::encode(uint8_t first, const std::array& values, double sensitivity) { + for (std::size_t axis = 0; axis < values.size(); axis++) { + const double counts = sensitivity > 0.0 ? std::round(values.at(axis) / sensitivity) : 0.0; + const auto word = static_cast(static_cast(std::clamp(counts, -32768.0, 32767.0))); + const std::size_t low = first + (2 * axis); + + this->registers.at(low) = static_cast(word); + this->registers.at(low + 1) = static_cast(word >> 8U); + } +} + +double Lsm6dsvModel::data_rate(uint8_t odr) const { + if (odr == 0 or odr > 12) { + return 0.0; + } + + if (odr == 1) { + return 1.875; + } + + if (odr == 2) { + return 7.5; + } + + return base_rates.at(this->registers.at(haodr_cfg) & 0x03) * static_cast(1U << (odr - 3U)); +} +} // namespace micras::models diff --git a/micras_proxy/src/battery.cpp b/micras_proxy/src/battery.cpp new file mode 100644 index 0000000..5a50a01 --- /dev/null +++ b/micras_proxy/src/battery.cpp @@ -0,0 +1,34 @@ +/** + * @file + */ + +#include "micras/proxy/battery.hpp" + +namespace micras::proxy { +Battery::Battery(const Config& config) : + adc{config.adc}, + max_voltage{config.adc.reference_voltage * config.voltage_divider}, + filter{config.filter}, + initialized{this->adc.start_dma({&(this->raw_reading), 1}) and this->adc.was_initialized()} { } + +void Battery::update() { + this->adc.recover(); + this->filter.update(this->get_adc_reading()); +} + +float Battery::get_voltage() const { + return this->filter.get_last() * this->max_voltage; +} + +float Battery::get_voltage_raw() const { + return this->get_adc_reading() * this->max_voltage; +} + +float Battery::get_adc_reading() const { + return static_cast(this->raw_reading) / this->adc.get_max_reading(); +} + +bool Battery::was_initialized() const { + return this->initialized; +} +} // namespace micras::proxy diff --git a/micras_proxy/src/bluetooth_serial.cpp b/micras_proxy/src/bluetooth_serial.cpp new file mode 100644 index 0000000..2eadf3e --- /dev/null +++ b/micras_proxy/src/bluetooth_serial.cpp @@ -0,0 +1,67 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/proxy/bluetooth_serial.hpp" + +namespace micras::proxy { +BluetoothSerial::BluetoothSerial(const Config& config, std::span rx_buffer, std::span tx_buffer) : + uart{{.init_function = config.init_function, .handle = config.handle}}, tx_buffer{tx_buffer} { + this->uart.start_rx(rx_buffer); +} + +void BluetoothSerial::update() { + if (this->uart.is_transmitting()) { + return; + } + + this->tail = (this->tail + this->pending) % this->tx_buffer.size(); + this->stored -= this->pending; + this->pending = 0; + + if (this->stored == 0) { + return; + } + + const std::size_t run = std::min(this->stored, this->tx_buffer.size() - this->tail); + + if (this->uart.start_tx(this->tx_buffer.subspan(this->tail, run))) { + this->pending = run; + } +} + +std::size_t BluetoothSerial::read(std::span into) { + return this->uart.read(into); +} + +std::size_t BluetoothSerial::write(std::span from) { + if (from.size() > this->tx_buffer.size() - this->stored) { + return 0; + } + + const std::size_t first = std::min(from.size(), this->tx_buffer.size() - this->head); + + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) bounded just above + std::memcpy(&this->tx_buffer[this->head], from.data(), first); + + if (first < from.size()) { + std::memcpy(this->tx_buffer.data(), &from[first], from.size() - first); + } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + + this->head = (this->head + from.size()) % this->tx_buffer.size(); + this->stored += from.size(); + + return from.size(); +} + +bool BluetoothSerial::was_initialized() const { + return this->uart.was_initialized(); +} +} // namespace micras::proxy diff --git a/micras_proxy/src/button.cpp b/micras_proxy/src/button.cpp new file mode 100644 index 0000000..5e9473b --- /dev/null +++ b/micras_proxy/src/button.cpp @@ -0,0 +1,74 @@ +/** + * @file + */ + +#include "micras/proxy/button.hpp" + +namespace micras::proxy { +Button::Button(const Config& config) : + debounce_delay{config.debounce_delay}, + long_press_delay{config.long_press_delay}, + extra_long_press_delay{config.extra_long_press_delay}, + gpio{config.gpio}, + pull_resistor{config.pull_resistor} { } + +bool Button::is_pressed() const { + return this->current_state; +} + +Button::Status Button::get_status() const { + return this->current_status; +} + +void Button::update() { + this->previous_state = this->current_state; + this->update_state(); + + if (this->is_rising_edge()) { + this->status_stopwatch.reset_ms(); + } else if (this->is_falling_edge()) { + if (this->status_stopwatch.elapsed_time_ms() > extra_long_press_delay) { + this->current_status = Status::EXTRA_LONG_PRESS; + return; + } + + if (this->status_stopwatch.elapsed_time_ms() > long_press_delay) { + this->current_status = Status::LONG_PRESS; + return; + } + + this->current_status = Status::SHORT_PRESS; + return; + } + + this->current_status = Status::NO_PRESS; +} + +bool Button::get_raw_reading() const { + return this->gpio.read() == static_cast(this->pull_resistor); +} + +void Button::update_state() { + const bool raw_reading = this->get_raw_reading(); + + if ((raw_reading != this->current_state) and not this->is_debouncing) { + this->is_debouncing = true; + this->debounce_stopwatch.reset_ms(); + } else if ((this->debounce_stopwatch.elapsed_time_ms() < debounce_delay) and this->is_debouncing) { + if (this->current_state == raw_reading) { + this->is_debouncing = false; + } + } else { + this->is_debouncing = false; + this->current_state = raw_reading; + } +} + +bool Button::is_rising_edge() const { + return this->current_state and not this->previous_state; +} + +bool Button::is_falling_edge() const { + return not this->current_state and this->previous_state; +} +} // namespace micras::proxy diff --git a/micras_proxy/src/buzzer.cpp b/micras_proxy/src/buzzer.cpp new file mode 100644 index 0000000..8100e22 --- /dev/null +++ b/micras_proxy/src/buzzer.cpp @@ -0,0 +1,48 @@ +/** + * @file + */ + +#include +#include "micras/proxy/buzzer.hpp" + +namespace micras::proxy { +Buzzer::Buzzer(const Config& config) : pwm{config.pwm} { + this->stop(); +} + +void Buzzer::play(uint32_t frequency, uint32_t duration) { + this->pwm.set_frequency(frequency); + this->pwm.set_duty_cycle(50.0F); + this->is_playing = true; + this->duration = duration; + + if (duration > 0) { + this->stopwatch.reset_ms(); + } +} + +void Buzzer::update() { + if (this->is_playing and this->duration > 0 and this->stopwatch.elapsed_time_ms() > this->duration) { + this->stop(); + } +} + +void Buzzer::wait(uint32_t interval) { + while (this->duration > 0 and this->is_playing) { + this->update(); + } + + this->stopwatch.reset_ms(); + + while (this->stopwatch.elapsed_time_ms() < interval) { } +} + +void Buzzer::stop() { + this->is_playing = false; + this->pwm.set_duty_cycle(0.0F); +} + +bool Buzzer::was_initialized() const { + return this->pwm.was_initialized(); +} +} // namespace micras::proxy diff --git a/micras_proxy/src/fan.cpp b/micras_proxy/src/fan.cpp new file mode 100644 index 0000000..745e7ce --- /dev/null +++ b/micras_proxy/src/fan.cpp @@ -0,0 +1,61 @@ +/** + * @file + */ + +#include + +#include "micras/core/utils.hpp" +#include "micras/proxy/fan.hpp" + +namespace micras::proxy { +Fan::Fan(const Config& config) : + pwm{config.pwm}, enable_gpio{config.enable_gpio}, max_acceleration{config.max_acceleration} { + this->stop(); + this->enable(); +} + +void Fan::enable() { + this->enabled = true; + this->enable_gpio.write(true); +} + +void Fan::disable() { + this->enabled = false; + this->enable_gpio.write(false); +} + +void Fan::set_speed(float speed) { + this->update(); + this->target_speed = speed; +} + +float Fan::update() { + this->current_speed = core::move_towards( + this->current_speed, this->target_speed, this->acceleration_stopwatch.elapsed_time_ms() * this->max_acceleration + ); + + this->acceleration_stopwatch.reset_ms(); + + this->pwm.set_duty_cycle(std::max(this->current_speed, 0.0F)); + + return this->current_speed; +} + +bool Fan::is_at_speed() const { + return this->current_speed == this->target_speed; +} + +void Fan::stop() { + this->target_speed = 0.0F; + this->current_speed = 0.0F; + this->pwm.set_duty_cycle(0.0F); +} + +bool Fan::check_fault() const { + return this->enabled and not this->enable_gpio.read(); +} + +bool Fan::was_initialized() const { + return this->pwm.was_initialized(); +} +} // namespace micras::proxy diff --git a/micras_proxy/src/fmac_filter.cpp b/micras_proxy/src/fmac_filter.cpp new file mode 100644 index 0000000..1c92719 --- /dev/null +++ b/micras_proxy/src/fmac_filter.cpp @@ -0,0 +1,69 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/core/butterworth_filter.hpp" +#include "micras/proxy/fmac_filter.hpp" + +namespace micras::proxy { +FmacFilter::FmacFilter(const Config& config) : fmac{config.fmac} { + const core::ButterworthFilter::Coefficients coefficients = + core::ButterworthFilter::compute_coefficients(config.filter); + + bool representable = true; + + for (std::size_t i = 0; i < coefficients.feed_forward.size(); i++) { + representable &= to_fixed_point(coefficients.feed_forward.at(i), this->feed_forward.at(i)); + } + + for (std::size_t i = 0; i < coefficients.feedback.size(); i++) { + representable &= to_fixed_point(-coefficients.feedback.at(i), this->feedback.at(i)); + } + + if (not representable) { + return; + } + + this->initialized = this->fmac.configure_iir(this->feed_forward, this->feedback); +} + +float FmacFilter::update(float x0) { + if (not this->initialized) { + return 0.0F; + } + + const float clamped = std::clamp(x0, -1.0F, 1.0F - 1.0F / fixed_point_scale); + const auto sample = static_cast(std::lround(clamped * fixed_point_scale)); + + this->last = static_cast(this->fmac.update(sample)) / fixed_point_scale; + + return this->last; +} + +float FmacFilter::get_last() const { + return this->last; +} + +bool FmacFilter::was_initialized() const { + return this->initialized; +} + +bool FmacFilter::to_fixed_point(float value, int16_t& result) { + const float scaled = value * fixed_point_scale; + + if (std::abs(scaled) >= fixed_point_scale) { + result = 0; + return false; + } + + result = static_cast(std::lround(scaled)); + + return result != 0 or value == 0.0F; +} +} // namespace micras::proxy diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp new file mode 100644 index 0000000..1b3d028 --- /dev/null +++ b/micras_proxy/src/imu.cpp @@ -0,0 +1,182 @@ +/** + * @file + */ + +#include +#include +#include + +#include "lsm6dsv_reg.h" +#include "micras/proxy/imu.hpp" +#include "micras/proxy/stopwatch.hpp" + +namespace micras::proxy { +Imu::Imu(const Config& config) : + spi{config.spi}, + gy_factor{ + mdps_to_radps * 4.375F * + (1 << (config.gyroscope_scale == LSM6DSV_4000dps ? 5 : static_cast(config.gyroscope_scale))) + }, + xl_factor{mg_to_mps2 * (0.061F * (1 << static_cast(config.accelerometer_scale)))} { + this->dev_ctx.read_reg = platform_read; + this->dev_ctx.write_reg = platform_write; + this->dev_ctx.mdelay = proxy::Stopwatch::sleep_ms; + this->dev_ctx.handle = &this->spi; + + proxy::Stopwatch::sleep_ms(10); + + if (not this->spi.was_initialized() or not this->check_whoami()) { + return; + } + + uint8_t haodr_cfg = (static_cast(config.gyroscope_data_rate) >> 4) & haodr_sel_mask; + + int32_t status = lsm6dsv_sw_por(&(this->dev_ctx)); + status |= lsm6dsv_block_data_update_set(&(this->dev_ctx), PROPERTY_ENABLE); + status |= lsm6dsv_write_reg(&(this->dev_ctx), LSM6DSV_HAODR_CFG, &haodr_cfg, 1); + + status |= lsm6dsv_gy_mode_set(&(this->dev_ctx), config.gyroscope_mode); + status |= lsm6dsv_xl_mode_set(&(this->dev_ctx), config.accelerometer_mode); + + status |= lsm6dsv_gy_data_rate_set(&(this->dev_ctx), config.gyroscope_data_rate); + status |= lsm6dsv_xl_data_rate_set(&(this->dev_ctx), config.accelerometer_data_rate); + + status |= lsm6dsv_gy_full_scale_set(&(this->dev_ctx), config.gyroscope_scale); + status |= lsm6dsv_xl_full_scale_set(&(this->dev_ctx), config.accelerometer_scale); + + status |= lsm6dsv_filt_settling_mask_set(&dev_ctx, {.drdy = 1, .ois_drdy = 0, .irq_xl = 0, .irq_g = 0}); + + status |= lsm6dsv_filt_gy_lp1_set(&(this->dev_ctx), PROPERTY_ENABLE); + status |= lsm6dsv_filt_gy_lp1_bandwidth_set(&(this->dev_ctx), config.gyroscope_filter); + status |= lsm6dsv_filt_xl_lp2_set(&(this->dev_ctx), PROPERTY_ENABLE); + status |= lsm6dsv_filt_xl_lp2_bandwidth_set(&(this->dev_ctx), config.accelerometer_filter); + + lsm6dsv_data_rate_t gyroscope_data_rate{}; + lsm6dsv_data_rate_t accelerometer_data_rate{}; + status |= lsm6dsv_gy_data_rate_get(&(this->dev_ctx), &gyroscope_data_rate); + status |= lsm6dsv_xl_data_rate_get(&(this->dev_ctx), &accelerometer_data_rate); + + this->initialized = status == 0 and gyroscope_data_rate == config.gyroscope_data_rate and + accelerometer_data_rate == config.accelerometer_data_rate; +} + +bool Imu::check_whoami() { + uint8_t whoami = 0; + + lsm6dsv_device_id_get(&(this->dev_ctx), &whoami); + + return whoami == LSM6DSV_ID; +} + +void Imu::update() { + const hal::Spi::Transfer transfer = this->spi.get_transfer(); + this->fresh = false; + + if (transfer == hal::Spi::Transfer::RUNNING) { + return; + } + + if (transfer == hal::Spi::Transfer::COMPLETE) { + std::atomic_signal_fence(std::memory_order_seq_cst); + this->read_response(); + } + + this->spi.start_transfer(this->command, this->response); +} + +void Imu::read_response() { + const auto get_axis = [this](uint8_t offset, uint8_t axis) { + const uint8_t index = 1 + offset + 2 * axis; + return static_cast(this->response.at(index) | (this->response.at(index + 1) << 8)); + }; + + const uint8_t status = std::get<1>(this->response); + + if ((status & unused_status_bits) != 0) { + return; + } + + if ((status & accelerometer_ready) != 0) { + for (uint8_t axis = 0; axis < 3; axis++) { + this->linear_acceleration.at(axis) = get_axis(acceleration_offset, axis) * this->xl_factor; + } + } + + if ((status & gyroscope_ready) == 0) { + return; + } + + for (uint8_t axis = 0; axis < 3; axis++) { + this->angular_velocity.at(axis) = get_axis(angular_rate_offset, axis) * this->gy_factor; + } + + this->fresh = true; +} + +bool Imu::is_new() const { + return this->fresh; +} + +float Imu::get_angular_velocity(Axis axis) const { + switch (axis) { + case Axis::X: + return std::get<0>(this->angular_velocity); + + case Axis::Y: + return std::get<1>(this->angular_velocity); + + case Axis::Z: + return std::get<2>(this->angular_velocity); + + default: + return 0.0F; + } +} + +float Imu::get_linear_acceleration(Axis axis) const { + switch (axis) { + case Axis::X: + return std::get<0>(this->linear_acceleration); + + case Axis::Y: + return std::get<1>(this->linear_acceleration); + + case Axis::Z: + return std::get<2>(this->linear_acceleration); + + default: + return 0.0F; + } +} + +int32_t Imu::platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len) { + auto* spi = static_cast(handle); + + if (not spi->select_device()) { + return -1; + } + + reg |= read_flag; + const bool transferred = spi->transmit({®, 1}) and spi->receive({bufp, len}); + spi->unselect_device(); + + return transferred ? 0 : -1; +} + +int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len) { + auto* spi = static_cast(handle); + + if (not spi->select_device()) { + return -1; + } + + const bool transferred = spi->transmit({®, 1}) and spi->transmit({bufp, len}); + spi->unselect_device(); + + return transferred ? 0 : -1; +} + +bool Imu::was_initialized() const { + return this->initialized; +} +} // namespace micras::proxy diff --git a/micras_proxy/src/led.cpp b/micras_proxy/src/led.cpp new file mode 100644 index 0000000..62fcbdd --- /dev/null +++ b/micras_proxy/src/led.cpp @@ -0,0 +1,21 @@ +/** + * @file + */ + +#include "micras/proxy/led.hpp" + +namespace micras::proxy { +Led::Led(const Config& config) : gpio{config.gpio} { } + +void Led::turn_on() { + this->gpio.write(true); +} + +void Led::turn_off() { + this->gpio.write(false); +} + +void Led::toggle() { + this->gpio.toggle(); +} +} // namespace micras::proxy diff --git a/micras_proxy/src/locomotion.cpp b/micras_proxy/src/locomotion.cpp new file mode 100644 index 0000000..d1bc3f3 --- /dev/null +++ b/micras_proxy/src/locomotion.cpp @@ -0,0 +1,54 @@ +/** + * @file + */ + +#include +#include + +#include "micras/proxy/locomotion.hpp" + +namespace micras::proxy { +Locomotion::Locomotion(const Config& config) : + left_motor{config.left_motor}, + right_motor{config.right_motor}, + enable_gpio{config.enable_gpio}, + reserved_rotation{config.reserved_rotation} { + this->stop(); + this->disable(); +} + +void Locomotion::enable() { + this->enable_gpio.write(true); +} + +void Locomotion::disable() { + this->enable_gpio.write(false); +} + +void Locomotion::set_wheel_command(float left_command, float right_command) { + this->left_motor.set_command(left_command); + this->right_motor.set_command(right_command); +} + +Locomotion::Command Locomotion::set_command(float linear, float angular) { + const float angular_limit = std::max(this->reserved_rotation, max_command - std::abs(linear)); + + angular = std::clamp(angular, -angular_limit, angular_limit); + + const float linear_limit = max_command - std::abs(angular); + + linear = std::clamp(linear, -linear_limit, linear_limit); + + this->set_wheel_command(linear - angular, linear + angular); + + return {.linear = linear, .angular = angular}; +} + +void Locomotion::stop() { + this->set_wheel_command(0.0F, 0.0F); +} + +bool Locomotion::was_initialized() const { + return this->left_motor.was_initialized() and this->right_motor.was_initialized(); +} +} // namespace micras::proxy diff --git a/micras_proxy/src/motor.cpp b/micras_proxy/src/motor.cpp new file mode 100644 index 0000000..2bd924f --- /dev/null +++ b/micras_proxy/src/motor.cpp @@ -0,0 +1,41 @@ +/** + * @file + */ + +#include + +#include "micras/core/utils.hpp" +#include "micras/proxy/motor.hpp" + +namespace micras::proxy { +Motor::Motor(const Config& config) : + backwards_pwm{config.backwards_pwm}, + forward_pwm{config.forward_pwm}, + max_stopped_command{config.max_stopped_command}, + deadzone{config.deadzone} { + this->set_command(0.0F); +} + +void Motor::set_command(float command) { + const bool is_positive = (command >= 0.0F); + command = std::abs(command); + + if (command <= max_stopped_command) { + command = 0.0F; + } else { + command = core::remap(command, 0.0F, 100.0F, this->deadzone, 100.0F); + } + + if (is_positive) { + this->forward_pwm.set_duty_cycle(command); + this->backwards_pwm.set_duty_cycle(0.0F); + } else { + this->forward_pwm.set_duty_cycle(0.0F); + this->backwards_pwm.set_duty_cycle(command); + } +} + +bool Motor::was_initialized() const { + return this->backwards_pwm.was_initialized() and this->forward_pwm.was_initialized(); +} +} // namespace micras::proxy diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp new file mode 100644 index 0000000..f391e21 --- /dev/null +++ b/micras_proxy/src/rotary_sensor.cpp @@ -0,0 +1,139 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/proxy/rotary_sensor.hpp" +#include "micras/proxy/stopwatch.hpp" + +namespace micras::proxy { +RotarySensor::RotarySensor(const Config& config) : spi{config.spi}, encoder{config.encoder}, crc{config.crc} { + if (not this->spi.was_initialized() or not this->encoder.was_initialized()) { + return; + } + + this->read_register(Registers::errfl_addr); + + this->write_register(Registers::disable_addr, config.registers.disable.raw); + this->write_register(Registers::zposm_addr, config.registers.zposm.raw); + this->write_register(Registers::zposl_addr, config.registers.zposl.raw); + this->write_register(Registers::settings1_addr, config.registers.settings1.raw); + this->write_register(Registers::settings2_addr, config.registers.settings2.raw); + this->write_register(Registers::settings3_addr, config.registers.settings3.raw); + this->write_register(Registers::ecc_addr, config.registers.ecc.raw); + + const std::optional read_back = this->read_resolution(); + + if (not read_back.has_value()) { + return; + } + + this->resolution = read_back.value(); + this->initialized = true; +} + +float RotarySensor::get_position() const { + if (this->resolution == 0) { + return 0.0F; + } + + return static_cast(this->encoder.get_counter()) * 2.0F * std::numbers::pi_v / + static_cast(this->resolution); +} + +uint32_t RotarySensor::get_resolution() const { + return this->resolution; +} + +bool RotarySensor::was_initialized() const { + return this->initialized; +} + +std::array RotarySensor::serialize(uint32_t raw) { + std::array bytes{ + static_cast(raw >> 16), + static_cast(raw >> 8), + static_cast(raw), + }; + + std::get<2>(bytes) = static_cast(this->crc.calculate({bytes.data(), frame_size - 1}) ^ 0xFF); + + return bytes; +} + +std::optional RotarySensor::exchange_frame(const std::array& frame) { + std::array received{}; + + if (not this->spi.select_device()) { + return std::nullopt; + } + + const bool transferred = this->spi.transmit_receive(frame, received); + this->spi.unselect_device(); + proxy::Stopwatch::sleep_us(min_deselect_time_us); + + if (not transferred) { + return std::nullopt; + } + + return static_cast(std::get<0>(received)) << 16 | static_cast(std::get<1>(received)) << 8 | + std::get<2>(received); +} + +std::optional RotarySensor::read_register(uint16_t address) { + const CommandFrame command{{.crc = 0, .address = address, .rw = 1, .do_not_care = 0}}; + const std::array command_bytes = this->serialize(command.raw); + + if (not this->exchange_frame(command_bytes).has_value()) { + return std::nullopt; + } + + const std::optional answer = this->exchange_frame(command_bytes); + + if (not answer.has_value()) { + return std::nullopt; + } + + DataFrame data{}; + data.raw = answer.value(); + + const std::array expected = this->serialize(answer.value()); + + if (std::get<2>(expected) != data.fields.crc or data.fields.error != 0) { + return std::nullopt; + } + + return static_cast(data.fields.data); +} + +bool RotarySensor::write_register(uint16_t address, uint16_t data) { + const CommandFrame command{{.crc = 0, .address = address, .rw = 0, .do_not_care = 0}}; + const DataFrame value{{.crc = 0, .data = data, .error = 0, .warning = 0}}; + + return this->exchange_frame(this->serialize(command.raw)).has_value() and + this->exchange_frame(this->serialize(value.raw)).has_value(); +} + +std::optional RotarySensor::read_resolution() { + const std::optional settings3 = this->read_register(Registers::settings3_addr); + + if (not settings3.has_value()) { + return std::nullopt; + } + + Registers::Settings3 fields{}; + fields.raw = static_cast(settings3.value()); + + const uint16_t pulses = pulses_per_revolution.at(fields.fields.ABIRES); + + if (pulses == 0) { + return std::nullopt; + } + + return edges_per_pulse * pulses; +} +} // namespace micras::proxy diff --git a/micras_proxy/src/stopwatch.cpp b/micras_proxy/src/stopwatch.cpp new file mode 100644 index 0000000..88c52a0 --- /dev/null +++ b/micras_proxy/src/stopwatch.cpp @@ -0,0 +1,44 @@ +/** + * @file + */ + +#include + +#include "micras/hal/timer.hpp" +#include "micras/proxy/stopwatch.hpp" + +namespace micras::proxy { +Stopwatch::Stopwatch() { + this->reset_ms(); + this->reset_us(); +} + +void Stopwatch::reset_ms() { + this->counter_ms = hal::Timer::get_counter_ms(); +} + +void Stopwatch::reset_us() { + this->counter_cycles = hal::Timer::get_counter(); +} + +uint32_t Stopwatch::elapsed_time_ms() const { + return hal::Timer::get_counter_ms() - this->counter_ms; +} + +uint32_t Stopwatch::elapsed_time_us() const { + return hal::Timer::to_microseconds(hal::Timer::get_counter() - this->counter_cycles); +} + +void Stopwatch::sleep_ms(uint32_t time) { + const uint32_t start = hal::Timer::get_counter_ms(); + + while (hal::Timer::get_counter_ms() - start < time) { } +} + +void Stopwatch::sleep_us(uint32_t time) { + const uint32_t start = hal::Timer::get_counter(); + const uint32_t cycles = hal::Timer::to_cycles(time); + + while (hal::Timer::get_counter() - start < cycles) { } +} +} // namespace micras::proxy diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp new file mode 100644 index 0000000..ba79c80 --- /dev/null +++ b/micras_proxy/src/storage.cpp @@ -0,0 +1,209 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/core/serializable.hpp" +#include "micras/core/variable_pool.hpp" +#include "micras/hal/flash.hpp" +#include "micras/proxy/storage.hpp" + +namespace micras::proxy { +static uint16_t read_uint16(std::span buffer, uint32_t address) { + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + return static_cast(buffer[address] | buffer[address + 1U] << 8); +} + +static void append_uint16(std::vector& buffer, uint16_t value) { + buffer.emplace_back(value); + buffer.emplace_back(value >> 8); +} + +static constexpr uint32_t align_size(std::size_t size) { + return (size + hal::FlashWord::size - 1) / hal::FlashWord::size * hal::FlashWord::size; +} + +Storage::Storage(const Config& config) : + start_sector{config.start_sector}, number_of_sectors{config.number_of_sectors} { + this->load(); +} + +void Storage::load() { + this->entries = {}; + this->values = {}; + this->entry_count = 0; + this->valid = false; + + const std::span header = hal::Flash::read(this->start_sector, 0, header_size); + + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + if (header.size() < header_size or read_uint16(header, 0) != start_symbol or header[2] != format_version) { + return; + } + + this->entry_count = header[3]; + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + + const uint32_t body_size = read_uint16(header, 4) * hal::FlashWord::size; + const std::span body = hal::Flash::read(this->start_sector, header_size, body_size); + + if (body.size() < body_size) { + return; + } + + std::span table = body; + + for (uint8_t index = 0; index < this->entry_count; index++) { + if (table.empty() or table.size() < table.front() + entry_overhead) { + return; + } + + table = table.subspan(table.front() + entry_overhead); + } + + this->entries = body.first(body.size() - table.size()); + this->values = table; + this->valid = true; +} + +bool Storage::is_valid() const { + return this->valid; +} + +bool Storage::take_entry(std::span& table, Entry& entry) const { + const uint8_t name_size = table.front(); + + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) + entry.name = {std::bit_cast(table.data() + 1), name_size}; + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + entry.type = static_cast(table[name_size + 1U]); + entry.offset = read_uint16(table, name_size + 2U); + entry.size = read_uint16(table, name_size + 4U); + + table = table.subspan(name_size + entry_overhead); + + return entry.offset + static_cast(entry.size) <= this->values.size(); +} + +std::size_t Storage::restore(core::VariablePool& pool) { + if (not this->valid) { + return 0; + } + + std::span table = this->entries; + std::size_t restored = 0; + + for (uint8_t index = 0; index < this->entry_count; index++) { + Entry entry{}; + + if (not this->take_entry(table, entry)) { + continue; + } + + const auto id = pool.find(entry.name); + + if (not id.has_value()) { + continue; + } + + const core::Variable& variable = pool.at(id.value()); + + if (not variable.access.persist or variable.type != entry.type) { + continue; + } + + const std::span data = this->values.subspan(entry.offset, entry.size); + + if (variable.type == core::TypeCode::BLOB) { + static_cast(variable.address)->deserialize(data.data(), data.size()); + } else if (variable.size != entry.size) { + continue; + } else { + std::memcpy(variable.address, data.data(), data.size()); + } + + restored++; + } + + return restored; +} + +bool Storage::save(const core::VariablePool& pool) { + std::vector table; + std::vector payload; + uint8_t count = 0; + + for (const core::Variable& variable : pool.all()) { + if (not variable.access.persist) { + continue; + } + + const std::size_t name_size = variable.prefix.size() + variable.name.size(); + const std::size_t offset = payload.size(); + + if (variable.type == core::TypeCode::BLOB) { + const std::vector data = static_cast(variable.address)->serialize(); + payload.insert(payload.end(), data.begin(), data.end()); + } else { + const auto* bytes = std::bit_cast(variable.address); + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) + payload.insert(payload.end(), bytes, bytes + variable.size); + } + + const std::size_t size = payload.size() - offset; + + if (count == UINT8_MAX or name_size > UINT8_MAX or offset > UINT16_MAX or size > UINT16_MAX) { + return false; + } + + table.emplace_back(name_size); + table.insert(table.end(), variable.prefix.begin(), variable.prefix.end()); + table.insert(table.end(), variable.name.begin(), variable.name.end()); + table.emplace_back(std::to_underlying(variable.type)); + append_uint16(table, offset); + append_uint16(table, size); + + count++; + } + + std::vector body = std::move(table); + body.insert(body.end(), payload.begin(), payload.end()); + body.resize(align_size(body.size()), hal::FlashWord::erased_value); + + if (body.size() / hal::FlashWord::size > UINT16_MAX or + header_size + body.size() > static_cast(this->number_of_sectors) * hal::Flash::sector_size) { + return false; + } + + if (hal::Flash::erase_sectors(this->start_sector, this->number_of_sectors) != hal::Flash::Status::OK) { + return false; + } + + if (hal::Flash::write(this->start_sector, header_size, body) != hal::Flash::Status::OK) { + return false; + } + + std::vector header; + header.reserve(header_size); + append_uint16(header, start_symbol); + header.emplace_back(format_version); + header.emplace_back(count); + append_uint16(header, body.size() / hal::FlashWord::size); + header.resize(header_size, hal::FlashWord::erased_value); + + if (hal::Flash::write(this->start_sector, 0, header) != hal::Flash::Status::OK) { + return false; + } + + this->load(); + return this->valid; +} +} // namespace micras::proxy diff --git a/micras_proxy/src/tick.cpp b/micras_proxy/src/tick.cpp new file mode 100644 index 0000000..def4eec --- /dev/null +++ b/micras_proxy/src/tick.cpp @@ -0,0 +1,30 @@ +/** + * @file + */ + +#include + +#include "micras/hal/timer.hpp" +#include "micras/proxy/tick.hpp" + +namespace micras::proxy { +Tick::Tick(const Config& config) : + period{hal::Timer::to_cycles(config.period_us)}, last_tick{hal::Timer::get_counter()} { } + +uint32_t Tick::wait() { + while (hal::Timer::get_counter() - this->last_tick < this->period) { } + + const uint32_t ticks = (hal::Timer::get_counter() - this->last_tick) / this->period; + this->last_tick += ticks * this->period; + + return ticks; +} + +void Tick::restart() { + this->last_tick = hal::Timer::get_counter(); +} + +uint32_t Tick::elapsed_time_us() const { + return hal::Timer::to_microseconds(hal::Timer::get_counter() - this->last_tick); +} +} // namespace micras::proxy diff --git a/micras_proxy/src/watchdog.cpp b/micras_proxy/src/watchdog.cpp new file mode 100644 index 0000000..66d3e1a --- /dev/null +++ b/micras_proxy/src/watchdog.cpp @@ -0,0 +1,31 @@ +/** + * @file + */ + +#include + +#include "micras/hal/mcu.hpp" +#include "micras/proxy/watchdog.hpp" + +namespace micras::proxy { +Watchdog::Extension::Extension(Watchdog& watchdog, uint32_t timeout_ms) : watchdog{watchdog} { + hal::Mcu::set_watchdog_timeout(timeout_ms); +} + +Watchdog::Extension::~Extension() { + hal::Mcu::set_watchdog_timeout(this->watchdog.timeout_ms); +} + +Watchdog::Watchdog(const Config& config) : timeout_ms{config.timeout_ms} { + hal::Mcu::set_watchdog_timeout(this->timeout_ms); +} + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) there is one watchdog, and this object is it +void Watchdog::refresh() { + hal::Mcu::refresh_watchdog(); +} + +Watchdog::Extension Watchdog::extend(uint32_t timeout_ms) { + return Extension{*this, timeout_ms}; +} +} // namespace micras::proxy diff --git a/reference/common/bench.cpp b/reference/common/bench.cpp new file mode 100644 index 0000000..a8877e4 --- /dev/null +++ b/reference/common/bench.cpp @@ -0,0 +1,301 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "bench.hpp" +#include "micras/core/butterworth_filter.hpp" +#include "micras/core/variable_pool.hpp" +#include "micras/hal/adc_dma.hpp" +#include "micras/hal/crc.hpp" +#include "micras/hal/encoder.hpp" +#include "micras/hal/family.hpp" +#include "micras/hal/gpio.hpp" +#include "micras/hal/mcu.hpp" +#include "micras/hal/pwm.hpp" +#include "micras/hal/pwm_dma.hpp" +#include "micras/hal/spi.hpp" +#include "micras/hal/timer.hpp" +#include "micras/hal/uart_dma.hpp" +#include "micras/proxy/fmac_filter.hpp" +#include "micras/proxy/storage.hpp" +#include "micras/proxy/watchdog.hpp" + +extern "C" { +void SystemClock_Config(); +} + +namespace { +// NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) +volatile bool storage_passed{}; +volatile bool watchdog_passed{}; +volatile float timer_clock_error{}; +volatile bool timer_passed{}; +volatile uint32_t crc_value{}; +volatile bool crc_passed{}; +volatile float fmac_error{}; +volatile bool fmac_passed{}; +volatile uint32_t vdda_mv{}; +volatile bool adc_passed{}; +volatile bool bench_done{}; +volatile bool start_pin_drivers{}; + +std::array adc_buffer{}; +std::array adc_snapshot{}; +std::array pwm_dma_compares{}; +std::array spi_transmitted{}; +std::array spi_received{}; +std::array uart_ring{}; + +// NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) + +constexpr uint32_t storage_magic{0x5EED1E55}; + +constexpr micras::proxy::Storage::Config storage_config{.start_sector = 0, .number_of_sectors = 1}; + +constexpr uint32_t watchdog_timeout_ms{50}; + +constexpr uint32_t timer_window_us{10000}; + +constexpr float timer_tolerance{1e-3F}; + +constexpr uint8_t crc_polynomial{0x1D}; + +constexpr uint8_t crc_initial_value{0xC4}; + +constexpr micras::core::ButterworthFilter::Config filter_config{ + .cutoff_frequency = 200.0F, + .sampling_frequency = 1000.0F, +}; + +constexpr float fmac_tolerance{2e-3F}; + +constexpr uint32_t adc_timeout_us{10000}; + +constexpr uint32_t min_vdda_mv{1620}; + +constexpr uint32_t max_vdda_mv{3600}; +} // namespace + +namespace micras::reference { +static void save_before_reset() { + core::TVariablePool<1> pool; + uint32_t magic = storage_magic; + pool.add("bench/", "magic", magic, {.persist = true}); + + proxy::Storage storage{storage_config}; + storage.save(pool); +} + +static bool restored_after_reset() { + core::TVariablePool<1> pool; + uint32_t magic{}; + pool.add("bench/", "magic", magic, {.persist = true}); + + proxy::Storage storage{storage_config}; + return storage.restore(pool) == 1 and magic == storage_magic; +} + +[[noreturn]] static void expire_watchdog() { + const proxy::Watchdog watchdog{{.timeout_ms = watchdog_timeout_ms}}; + + while (true) { + bench_done = false; + } +} + +static bool check_timer_clock() { + MX_TIM2_Init(); + HAL_TIM_Base_Start(&htim2); + + const uint32_t start_cycles = hal::Timer::get_counter(); + const uint32_t start_counts = __HAL_TIM_GET_COUNTER(&htim2); + const uint32_t window = hal::Timer::to_cycles(timer_window_us); + + while (hal::Timer::get_counter() - start_cycles < window) { } + + const uint32_t cycles = hal::Timer::get_counter() - start_cycles; + const uint32_t counts = __HAL_TIM_GET_COUNTER(&htim2) - start_counts; + + const uint32_t timer_clock = hal::family::timer_clock(htim2.Instance); + const float measured = + static_cast(counts) * static_cast(SystemCoreClock) / static_cast(cycles); + const float error = measured / static_cast(timer_clock) - 1.0F; + + timer_clock_error = error; + return std::abs(error) < timer_tolerance; +} + +static uint8_t software_crc(std::span data) { + uint8_t crc = crc_initial_value; + + for (const uint8_t byte : data) { + crc ^= byte; + + for (uint8_t bit = 0; bit < 8; bit++) { + crc = (crc & 0x80U) != 0 ? static_cast((crc << 1U) ^ crc_polynomial) : + static_cast(crc << 1U); + } + } + + return crc; +} + +static bool check_crc() { + constexpr std::string_view check{"123456789"}; + const std::vector bytes(check.begin(), check.end()); + + hal::Crc crc{{.handle = &hcrc}}; + const uint32_t value = crc.calculate(bytes); + + crc_value = value; + return value == software_crc(bytes); +} + +static bool check_fmac() { + proxy::FmacFilter filter{{ + .fmac = {.init_function = MX_FMAC_Init, .handle = &hfmac}, + .filter = filter_config, + }}; + + if (not filter.was_initialized()) { + return false; + } + + const core::ButterworthFilter::Coefficients coefficients = + core::ButterworthFilter::compute_coefficients(filter_config); + std::array inputs{}; + std::array outputs{}; + float largest_error{}; + + for (uint32_t sample = 0; sample < 300; sample++) { + const float input = (sample < 150 ? 0.5F : -0.25F) + 0.1F * std::sin(0.9F * static_cast(sample)); + + inputs = {input, inputs.at(0), inputs.at(1)}; + + const float expected = + coefficients.feed_forward.at(0) * inputs.at(0) + coefficients.feed_forward.at(1) * inputs.at(1) + + coefficients.feed_forward.at(2) * inputs.at(2) - coefficients.feedback.at(0) * outputs.at(0) - + coefficients.feedback.at(1) * outputs.at(1); + + outputs = {expected, outputs.at(0)}; + largest_error = std::max(largest_error, std::abs(filter.update(input) - expected)); + } + + fmac_error = largest_error; + return largest_error < fmac_tolerance; +} + +static bool check_adc(const Board& board) { + hal::AdcDma adc{{ + .init_function = MX_ADC3_Init, + .handle = &hadc3, + .max_reading = 4095, + .reference_voltage = 3.3F, + }}; + + if (not adc.start_dma(adc_buffer, adc_snapshot)) { + return false; + } + + std::array readings{}; + const uint32_t start = hal::Timer::get_counter(); + const uint32_t timeout = hal::Timer::to_cycles(adc_timeout_us); + + while (adc.read_snapshot(readings) == 0) { + if (hal::Timer::get_counter() - start > timeout) { + adc.stop_dma(); + return false; + } + } + + adc.stop_dma(); + + const uint32_t vdda = board.vdda_mv(readings.at(0)); + + vdda_mv = vdda; + return vdda >= min_vdda_mv and vdda <= max_vdda_mv; +} + +static void run_pin_drivers(const Board& board) { + MX_GPIO_Init(); + + hal::Gpio led{{.port = Status_LED_GPIO_Port, .pin = Status_LED_Pin}}; + const hal::Gpio button{{.port = Button_GPIO_Port, .pin = Button_Pin}}; + led.write(button.read()); + + hal::Pwm pwm{ + {.init_function = MX_TIM15_Init, .handle = &htim15, .timer_channel = TIM_CHANNEL_1, .inverted = false} + }; + pwm.set_duty_cycle(50.0F); + + hal::PwmDma pwm_dma{{.init_function = MX_TIM8_Init, .handle = &htim8, .timer_channel = TIM_CHANNEL_1}}; + pwm_dma_compares.fill(static_cast(pwm_dma.get_compare(50.0F))); + pwm_dma.start_dma(pwm_dma_compares); + + const hal::Encoder encoder{{.init_function = MX_TIM5_Init, .handle = &htim5, .timer_channel = TIM_CHANNEL_ALL}}; + led.write(encoder.get_counter() > 0); + + hal::Spi spi{{ + .init_function = MX_SPI3_Init, + .handle = &hspi3, + .cs_gpio = {.port = SPI_CSn_GPIO_Port, .pin = SPI_CSn_Pin}, + .timeout = 2, + .clock_polarity = SPI_POLARITY_HIGH, + .clock_phase = SPI_PHASE_2EDGE, + }}; + spi.start_transfer(spi_transmitted, spi_received); + + hal::UartDma uart{{.init_function = board.uart_init, .handle = board.uart}}; + uart.start_rx(uart_ring); + uart.start_tx(spi_received); +} + +void run_bench(const Board& board) { + constexpr std::array peripheral_inits{MX_DMA_Init, MX_CRC_Init}; + + hal::Mcu::init({ + .clock_init = SystemClock_Config, + .peripheral_clock_init = nullptr, + .peripheral_inits = peripheral_inits, + .cpu_frequency_boost = false, + }); + + if (not hal::Mcu::was_reset_by_watchdog()) { + save_before_reset(); + expire_watchdog(); + } + + watchdog_passed = true; + storage_passed = restored_after_reset(); + timer_passed = check_timer_clock(); + crc_passed = check_crc(); + fmac_passed = check_fmac(); + adc_passed = check_adc(board); + bench_done = true; + + if (start_pin_drivers) { + run_pin_drivers(board); + } + + while (true) { + bench_done = true; + } +} +} // namespace micras::reference diff --git a/reference/common/bench.hpp b/reference/common/bench.hpp new file mode 100644 index 0000000..860b35f --- /dev/null +++ b/reference/common/bench.hpp @@ -0,0 +1,54 @@ +/** + * @file + * + * @brief The bench program every reference project runs, over the peripherals its board names. + * + * @note The bench initializes only peripherals that drive no pin, so its image is safe to flash on any + * board with the part. It writes each result into a volatile global for the debugger or + * STM32CubeMonitor, and ends with bench_done set. + */ + +#ifndef MICRAS_LIB_REFERENCE_BENCH_HPP +#define MICRAS_LIB_REFERENCE_BENCH_HPP + +#include +#include + +#include "micras/hal/mcu.hpp" + +namespace micras::reference { +/** + * @brief What differs between the reference projects of the families. + */ +struct Board { + /** + * @brief Initialization function of the UART with DMA. + */ + hal::Mcu::InitFunction uart_init; + + /** + * @brief Handle of the UART with DMA. + */ + UART_HandleTypeDef* uart; + + /** + * @brief Convert a conversion of the internal reference into the analog supply voltage. + * + * @note Each family reads its factory calibration of the internal reference through its own + * low-layer ADC header. + */ + uint32_t (*vdda_mv)(uint16_t vrefint_reading); +}; + +/** + * @brief Initialize the microcontroller and run every check of the bench. + * + * @note The first run saves to the storage and lets the watchdog expire, the run after the reset + * checks the reset cause and restores the storage before the other checks. + * + * @param board Peripherals of the reference project that differ between families. + */ +[[noreturn]] void run_bench(const Board& board); +} // namespace micras::reference + +#endif // MICRAS_LIB_REFERENCE_BENCH_HPP diff --git a/reference/common/reference.cmake b/reference/common/reference.cmake new file mode 100644 index 0000000..83e1562 --- /dev/null +++ b/reference/common/reference.cmake @@ -0,0 +1,105 @@ +############################################################################### +## Reference project: generated tree, micras-lib, bench program and lint +############################################################################### + +# Included by reference//CMakeLists.txt after its project(), with common/toolchain.cmake read +# before it. + +# The STM32CubeMX CMake generator writes its sources into ${CMAKE_PROJECT_NAME} instead of a target of +# its own, so the variable is pointed at a target created to hold them +add_library(cube_app OBJECT) +set(MICRAS_REFERENCE_PROJECT_NAME ${CMAKE_PROJECT_NAME}) +set(CMAKE_PROJECT_NAME cube_app) +add_subdirectory(${MICRAS_REFERENCE_DIR}/cmake/stm32cubemx stm32cubemx) +set(CMAKE_PROJECT_NAME ${MICRAS_REFERENCE_PROJECT_NAME}) + +# The generated headers are not the project's to warn about +set_target_properties(stm32cubemx PROPERTIES SYSTEM TRUE) + +foreach(CUBE_OBJECT_LIBRARY cube_app STM32_Drivers) + target_compile_options(${CUBE_OBJECT_LIBRARY} PRIVATE -w) +endforeach() + +############################################################################### +## micras-lib +############################################################################### + +set(MICRAS_LIB_WERROR ON) +add_subdirectory(${MICRAS_REFERENCE_COMMON_DIR}/../.. micras-lib EXCLUDE_FROM_ALL) + +############################################################################### +## Bench program +############################################################################### + +set(MICRAS_BENCH_TARGET micras_lib_bench_${MICRAS_REFERENCE_FAMILY}) + +# The vector table, the interrupt handlers and the MX_*_Init functions are linked into the executable, +# which is where micras-lib's callbacks have to win over the vendor's weak ones +add_executable(${MICRAS_BENCH_TARGET} + ${MICRAS_REFERENCE_DIR}/bench.cpp + ${MICRAS_REFERENCE_COMMON_DIR}/bench.cpp +) + +target_include_directories(${MICRAS_BENCH_TARGET} PRIVATE + ${MICRAS_REFERENCE_COMMON_DIR} +) + +target_link_libraries(${MICRAS_BENCH_TARGET} PRIVATE + micras::proxy + micras::nav + micras::comm + cube_app + STM32_Drivers +) + +micras_apply_warnings(${MICRAS_BENCH_TARGET} WERROR ON) + +target_link_options(${MICRAS_BENCH_TARGET} PRIVATE -Wl,-Map=$.map) + +add_custom_command(TARGET ${MICRAS_BENCH_TARGET} POST_BUILD + COMMAND ${CMAKE_OBJCOPY} -O ihex $ $.hex + COMMAND ${CMAKE_SIZE} $ +) + +############################################################################### +## Lint +############################################################################### + +# The stm32 backend of this family and the bench are linted here, against the family's ARM headers, +# since no host build compiles them +include(${MICRAS_REFERENCE_COMMON_DIR}/../../cmake/micras_lint.cmake) + +# clang finds the ARM target from the compiler's name, but not the headers of its toolchain +execute_process( + COMMAND ${CMAKE_CXX_COMPILER} -print-search-dirs + OUTPUT_VARIABLE MICRAS_ARM_SEARCH_DIRS + OUTPUT_STRIP_TRAILING_WHITESPACE +) + +execute_process( + COMMAND ${CMAKE_CXX_COMPILER} -dumpmachine + OUTPUT_VARIABLE MICRAS_ARM_TRIPLE + OUTPUT_STRIP_TRAILING_WHITESPACE +) + +string(REGEX MATCH "install: ([^\n]+)/" MICRAS_ARM_INSTALL_MATCH ${MICRAS_ARM_SEARCH_DIRS}) +set(MICRAS_ARM_INSTALL_DIR ${CMAKE_MATCH_1}) +get_filename_component(MICRAS_ARM_GCC_VERSION ${MICRAS_ARM_INSTALL_DIR} NAME) +get_filename_component(MICRAS_ARM_PREFIX "${MICRAS_ARM_INSTALL_DIR}/../../../" REALPATH) +set(MICRAS_ARM_SYSROOT ${MICRAS_ARM_PREFIX}/${MICRAS_ARM_TRIPLE}) +set(MICRAS_ARM_CXX_INCLUDE_DIR ${MICRAS_ARM_SYSROOT}/include/c++/${MICRAS_ARM_GCC_VERSION}) + +file(GLOB MICRAS_REFERENCE_LINT_SOURCES CONFIGURE_DEPENDS + ${MICRAS_LIB_ROOT}/micras_hal/stm32/src/*.cpp + ${MICRAS_LIB_ROOT}/micras_hal/stm32/${MICRAS_REFERENCE_FAMILY}/src/*.cpp + ${MICRAS_LIB_ROOT}/reference/common/*.cpp + ${MICRAS_LIB_ROOT}/reference/${MICRAS_REFERENCE_FAMILY}/bench.cpp +) + +micras_add_lint_targets( + SOURCES ${MICRAS_REFERENCE_LINT_SOURCES} + EXTRA_ARGS + --sysroot=${MICRAS_ARM_SYSROOT}/ + -I${MICRAS_ARM_CXX_INCLUDE_DIR}/ + -I${MICRAS_ARM_CXX_INCLUDE_DIR}/${MICRAS_ARM_TRIPLE}/ +) diff --git a/reference/common/toolchain.cmake b/reference/common/toolchain.cmake new file mode 100644 index 0000000..7339142 --- /dev/null +++ b/reference/common/toolchain.cmake @@ -0,0 +1,55 @@ +############################################################################### +## Reference project: language, flags and the generated toolchain file +############################################################################### + +# Included by reference//CMakeLists.txt before its project(), with MICRAS_REFERENCE_FAMILY set. +# The Cube files are generated from reference//reference.ioc (the CI generates them with +# .docker/Dockerfile) and never committed. + +set(CMAKE_C_STANDARD 17) +set(CMAKE_C_STANDARD_REQUIRED ON) +set(CMAKE_C_EXTENSIONS ON) + +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_CXX_SCAN_FOR_MODULES OFF) + +get_filename_component(MICRAS_REFERENCE_DIR ${CMAKE_CURRENT_LIST_DIR}/../${MICRAS_REFERENCE_FAMILY} ABSOLUTE) +set(MICRAS_REFERENCE_COMMON_DIR ${CMAKE_CURRENT_LIST_DIR}) + +if(NOT EXISTS ${MICRAS_REFERENCE_DIR}/cmake/stm32cubemx/CMakeLists.txt) + message(FATAL_ERROR + "micras-lib reference/${MICRAS_REFERENCE_FAMILY}: the Cube files are not generated. Generate them into " + "reference/${MICRAS_REFERENCE_FAMILY} with 'docker build --file .docker/Dockerfile --target " + "generated-${MICRAS_REFERENCE_FAMILY} --output type=local,dest=reference/${MICRAS_REFERENCE_FAMILY} .' " + "from the root of micras-lib, or with STM32CubeMX from reference.ioc.") +endif() + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +# The generated toolchain file names the linker script relative to CMAKE_SOURCE_DIR, so it is pointed +# at the generated tree while the file is read +set(MICRAS_REFERENCE_SOURCE_DIR ${CMAKE_SOURCE_DIR}) +set(CMAKE_SOURCE_DIR ${MICRAS_REFERENCE_DIR}) +include(${MICRAS_REFERENCE_DIR}/cmake/gcc-arm-none-eabi.cmake) +set(CMAKE_SOURCE_DIR ${MICRAS_REFERENCE_SOURCE_DIR}) + +# One map file per target instead of the one name the generated file gives every target +string(REGEX REPLACE " *-Wl,-Map=[^ ]*" "" CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS}") + +# The flags a consumer builds its firmware with: the same as the Micras firmware's +set(MICRAS_MATH_FLAGS "-fno-math-errno -ffp-contract=fast") + +set(CMAKE_C_FLAGS_DEBUG "-Og -g3") +set(CMAKE_CXX_FLAGS_DEBUG "-Og -g3") +set(CMAKE_C_FLAGS_RELEASE "-O2 -g3 ${MICRAS_MATH_FLAGS}") +set(CMAKE_CXX_FLAGS_RELEASE "-O2 -g3 ${MICRAS_MATH_FLAGS}") + +# Link time optimization through the property, so that CMake archives with gcc-ar +set(CMAKE_INTERPROCEDURAL_OPTIMIZATION_RELEASE TRUE) +set(CMAKE_EXE_LINKER_FLAGS_RELEASE "-O2") diff --git a/reference/g4/.gitignore b/reference/g4/.gitignore new file mode 100644 index 0000000..765b11c --- /dev/null +++ b/reference/g4/.gitignore @@ -0,0 +1,6 @@ +# The Cube files STM32CubeMX generates from reference.ioc +* +!.gitignore +!CMakeLists.txt +!bench.cpp +!reference.ioc diff --git a/reference/g4/CMakeLists.txt b/reference/g4/CMakeLists.txt new file mode 100644 index 0000000..2688a2a --- /dev/null +++ b/reference/g4/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.28...4.4) + +############################################################################### +## Reference STM32G474 project +############################################################################### + +# The smallest CubeMX project that exercises every peripheral micras-lib uses, built and linked with +# its bench program the way a consumer of the stm32 backend builds its firmware. What every family's +# reference shares is in ../common. + +set(MICRAS_REFERENCE_FAMILY g4) +include(${CMAKE_CURRENT_LIST_DIR}/../common/toolchain.cmake) + +project(micras_lib_reference_g4 C CXX ASM) + +include(${CMAKE_CURRENT_LIST_DIR}/../common/reference.cmake) diff --git a/reference/g4/bench.cpp b/reference/g4/bench.cpp new file mode 100644 index 0000000..e628aa8 --- /dev/null +++ b/reference/g4/bench.cpp @@ -0,0 +1,22 @@ +/** + * @file + */ + +#include + +#include +#include + +#include "bench.hpp" + +static uint32_t vdda_mv(uint16_t vrefint_reading) { + return __LL_ADC_CALC_VREFANALOG_VOLTAGE(static_cast(vrefint_reading), LL_ADC_RESOLUTION_12B); +} + +int main() { + micras::reference::run_bench({ + .uart_init = MX_USART3_UART_Init, + .uart = &huart3, + .vdda_mv = vdda_mv, + }); +} diff --git a/reference/g4/reference.ioc b/reference/g4/reference.ioc new file mode 100644 index 0000000..79c21eb --- /dev/null +++ b/reference/g4/reference.ioc @@ -0,0 +1,391 @@ +#MicroXplorer Configuration settings - do not modify +ADC3.Channel-0\#ChannelRegularConversion=ADC_CHANNEL_VREFINT +ADC3.Channel-1\#ChannelRegularConversion=ADC_CHANNEL_VBAT +ADC3.ClockPrescaler=ADC_CLOCK_ASYNC_DIV4 +ADC3.CommonPathInternal=ADC_CHANNEL_VREFINT|null|ADC_CHANNEL_VBAT|null +ADC3.ContinuousConvMode=ENABLE +ADC3.DMAContinuousRequests=ENABLE +ADC3.IPParameters=Channel-0\#ChannelRegularConversion,Rank-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,OffsetNumber-0\#ChannelRegularConversion,Channel-1\#ChannelRegularConversion,Rank-1\#ChannelRegularConversion,SamplingTime-1\#ChannelRegularConversion,OffsetNumber-1\#ChannelRegularConversion,NbrOfConversion,NbrOfConversionFlag,ScanConvMode,ClockPrescaler,ContinuousConvMode,DMAContinuousRequests,Overrun,master,CommonPathInternal +ADC3.NbrOfConversion=2 +ADC3.NbrOfConversionFlag=1 +ADC3.OffsetNumber-0\#ChannelRegularConversion=ADC_OFFSET_NONE +ADC3.OffsetNumber-1\#ChannelRegularConversion=ADC_OFFSET_NONE +ADC3.Overrun=ADC_OVR_DATA_OVERWRITTEN +ADC3.Rank-0\#ChannelRegularConversion=1 +ADC3.Rank-1\#ChannelRegularConversion=2 +ADC3.SamplingTime-0\#ChannelRegularConversion=ADC_SAMPLETIME_640CYCLES_5 +ADC3.SamplingTime-1\#ChannelRegularConversion=ADC_SAMPLETIME_640CYCLES_5 +ADC3.ScanConvMode=ADC_SCAN_ENABLE +ADC3.master=1 +CAD.formats= +CAD.pinconfig= +CAD.provider= +CRC.CRCLength=CRC_POLYLENGTH_8B +CRC.DefaultInitValueUse=DEFAULT_INIT_VALUE_DISABLE +CRC.DefaultPolynomialUse=DEFAULT_POLYNOMIAL_DISABLE +CRC.GeneratingPolynomial=X4+X3+X2+X0 +CRC.IPParameters=DefaultPolynomialUse,DefaultInitValueUse,CRCLength,GeneratingPolynomial,InitValue +CRC.InitValue=0xC4 +Dma.ADC3.3.Direction=DMA_PERIPH_TO_MEMORY +Dma.ADC3.3.EventEnable=DISABLE +Dma.ADC3.3.Instance=DMA1_Channel4 +Dma.ADC3.3.MemDataAlignment=DMA_MDATAALIGN_HALFWORD +Dma.ADC3.3.MemInc=DMA_MINC_ENABLE +Dma.ADC3.3.Mode=DMA_CIRCULAR +Dma.ADC3.3.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD +Dma.ADC3.3.PeriphInc=DMA_PINC_DISABLE +Dma.ADC3.3.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.ADC3.3.Priority=DMA_PRIORITY_LOW +Dma.ADC3.3.RequestNumber=1 +Dma.ADC3.3.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.ADC3.3.SignalID=NONE +Dma.ADC3.3.SyncEnable=DISABLE +Dma.ADC3.3.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.ADC3.3.SyncRequestNumber=1 +Dma.ADC3.3.SyncSignalID=NONE +Dma.Request0=TIM8_CH1 +Dma.Request1=USART3_RX +Dma.Request2=USART3_TX +Dma.Request3=ADC3 +Dma.Request4=SPI3_RX +Dma.Request5=SPI3_TX +Dma.RequestsNb=6 +Dma.SPI3_RX.4.Direction=DMA_PERIPH_TO_MEMORY +Dma.SPI3_RX.4.EventEnable=DISABLE +Dma.SPI3_RX.4.Instance=DMA1_Channel5 +Dma.SPI3_RX.4.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.SPI3_RX.4.MemInc=DMA_MINC_ENABLE +Dma.SPI3_RX.4.Mode=DMA_NORMAL +Dma.SPI3_RX.4.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.SPI3_RX.4.PeriphInc=DMA_PINC_DISABLE +Dma.SPI3_RX.4.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.SPI3_RX.4.Priority=DMA_PRIORITY_LOW +Dma.SPI3_RX.4.RequestNumber=1 +Dma.SPI3_RX.4.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.SPI3_RX.4.SignalID=NONE +Dma.SPI3_RX.4.SyncEnable=DISABLE +Dma.SPI3_RX.4.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.SPI3_RX.4.SyncRequestNumber=1 +Dma.SPI3_RX.4.SyncSignalID=NONE +Dma.SPI3_TX.5.Direction=DMA_MEMORY_TO_PERIPH +Dma.SPI3_TX.5.EventEnable=DISABLE +Dma.SPI3_TX.5.Instance=DMA1_Channel6 +Dma.SPI3_TX.5.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.SPI3_TX.5.MemInc=DMA_MINC_ENABLE +Dma.SPI3_TX.5.Mode=DMA_NORMAL +Dma.SPI3_TX.5.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.SPI3_TX.5.PeriphInc=DMA_PINC_DISABLE +Dma.SPI3_TX.5.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.SPI3_TX.5.Priority=DMA_PRIORITY_LOW +Dma.SPI3_TX.5.RequestNumber=1 +Dma.SPI3_TX.5.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.SPI3_TX.5.SignalID=NONE +Dma.SPI3_TX.5.SyncEnable=DISABLE +Dma.SPI3_TX.5.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.SPI3_TX.5.SyncRequestNumber=1 +Dma.SPI3_TX.5.SyncSignalID=NONE +Dma.TIM8_CH1.0.Direction=DMA_MEMORY_TO_PERIPH +Dma.TIM8_CH1.0.EventEnable=DISABLE +Dma.TIM8_CH1.0.Instance=DMA1_Channel1 +Dma.TIM8_CH1.0.MemDataAlignment=DMA_MDATAALIGN_HALFWORD +Dma.TIM8_CH1.0.MemInc=DMA_MINC_ENABLE +Dma.TIM8_CH1.0.Mode=DMA_NORMAL +Dma.TIM8_CH1.0.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD +Dma.TIM8_CH1.0.PeriphInc=DMA_PINC_DISABLE +Dma.TIM8_CH1.0.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.TIM8_CH1.0.Priority=DMA_PRIORITY_LOW +Dma.TIM8_CH1.0.RequestNumber=1 +Dma.TIM8_CH1.0.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.TIM8_CH1.0.SignalID=NONE +Dma.TIM8_CH1.0.SyncEnable=DISABLE +Dma.TIM8_CH1.0.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.TIM8_CH1.0.SyncRequestNumber=1 +Dma.TIM8_CH1.0.SyncSignalID=NONE +Dma.USART3_RX.1.Direction=DMA_PERIPH_TO_MEMORY +Dma.USART3_RX.1.EventEnable=DISABLE +Dma.USART3_RX.1.Instance=DMA1_Channel2 +Dma.USART3_RX.1.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.USART3_RX.1.MemInc=DMA_MINC_ENABLE +Dma.USART3_RX.1.Mode=DMA_CIRCULAR +Dma.USART3_RX.1.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.USART3_RX.1.PeriphInc=DMA_PINC_DISABLE +Dma.USART3_RX.1.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.USART3_RX.1.Priority=DMA_PRIORITY_LOW +Dma.USART3_RX.1.RequestNumber=1 +Dma.USART3_RX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.USART3_RX.1.SignalID=NONE +Dma.USART3_RX.1.SyncEnable=DISABLE +Dma.USART3_RX.1.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.USART3_RX.1.SyncRequestNumber=1 +Dma.USART3_RX.1.SyncSignalID=NONE +Dma.USART3_TX.2.Direction=DMA_MEMORY_TO_PERIPH +Dma.USART3_TX.2.EventEnable=DISABLE +Dma.USART3_TX.2.Instance=DMA1_Channel3 +Dma.USART3_TX.2.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.USART3_TX.2.MemInc=DMA_MINC_ENABLE +Dma.USART3_TX.2.Mode=DMA_NORMAL +Dma.USART3_TX.2.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.USART3_TX.2.PeriphInc=DMA_PINC_DISABLE +Dma.USART3_TX.2.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.USART3_TX.2.Priority=DMA_PRIORITY_LOW +Dma.USART3_TX.2.RequestNumber=1 +Dma.USART3_TX.2.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.USART3_TX.2.SignalID=NONE +Dma.USART3_TX.2.SyncEnable=DISABLE +Dma.USART3_TX.2.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.USART3_TX.2.SyncRequestNumber=1 +Dma.USART3_TX.2.SyncSignalID=NONE +File.Version=6 +GPIO.groupedBy=Group By Peripherals +KeepUserPlacement=false +Mcu.CPN=STM32G474RET6 +Mcu.Family=STM32G4 +Mcu.IP0=ADC3 +Mcu.IP1=CRC +Mcu.IP10=TIM5 +Mcu.IP11=TIM8 +Mcu.IP12=TIM15 +Mcu.IP13=USART3 +Mcu.IP2=DMA +Mcu.IP3=FMAC +Mcu.IP4=IWDG +Mcu.IP5=NVIC +Mcu.IP6=RCC +Mcu.IP7=SPI3 +Mcu.IP8=SYS +Mcu.IP9=TIM2 +Mcu.IPNb=14 +Mcu.Name=STM32G474R(B-C-E)Tx +Mcu.Package=LQFP64 +Mcu.Pin0=PA0 +Mcu.Pin1=PA1 +Mcu.Pin10=PC10 +Mcu.Pin11=PC11 +Mcu.Pin12=PC12 +Mcu.Pin13=PD2 +Mcu.Pin14=VP_ADC3_Vref_Input +Mcu.Pin15=VP_ADC3_Vbat_Input +Mcu.Pin16=VP_CRC_VS_CRC +Mcu.Pin17=VP_FMAC_VS_FMAC +Mcu.Pin18=VP_IWDG_VS_IWDG +Mcu.Pin19=VP_SYS_VS_Systick +Mcu.Pin2=PA2 +Mcu.Pin20=VP_SYS_VS_DBSignals +Mcu.Pin21=VP_TIM2_VS_ClockSourceINT +Mcu.Pin3=PA7 +Mcu.Pin4=PB10 +Mcu.Pin5=PB11 +Mcu.Pin6=PB12 +Mcu.Pin7=PC6 +Mcu.Pin8=PA13 +Mcu.Pin9=PA14 +Mcu.PinsNb=22 +Mcu.ThirdPartyNb=0 +Mcu.UserConstants= +Mcu.UserName=STM32G474RETx +MxCube.Version=6.18.1 +MxDb.Version=DB.6.0.181 +NVIC.ADC3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.DMA1_Channel1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Channel2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Channel3_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Channel4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Channel5_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Channel6_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.ForceEnableDMAVector=true +NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 +NVIC.SPI3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false +NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +PA0.GPIOParameters=GPIO_PuPd,GPIO_Label +PA0.GPIO_Label=Encoder_A +PA0.GPIO_PuPd=GPIO_PULLUP +PA0.Locked=true +PA0.Signal=S_TIM5_CH1 +PA1.GPIOParameters=GPIO_PuPd,GPIO_Label +PA1.GPIO_Label=Encoder_B +PA1.GPIO_PuPd=GPIO_PULLUP +PA1.Locked=true +PA1.Signal=S_TIM5_CH2 +PA13.Locked=true +PA13.Mode=Serial_Wire +PA13.Signal=SYS_JTMS-SWDIO +PA14.Locked=true +PA14.Mode=Serial_Wire +PA14.Signal=SYS_JTCK-SWCLK +PA2.GPIOParameters=GPIO_Label +PA2.GPIO_Label=PWM +PA2.Locked=true +PA2.Signal=S_TIM15_CH1 +PA7.GPIOParameters=GPIO_PuPd,GPIO_Label +PA7.GPIO_Label=Button +PA7.GPIO_PuPd=GPIO_PULLUP +PA7.Locked=true +PA7.Signal=GPIO_Input +PB10.Locked=true +PB10.Mode=Asynchronous +PB10.Signal=USART3_TX +PB11.Locked=true +PB11.Mode=Asynchronous +PB11.Signal=USART3_RX +PB12.GPIOParameters=GPIO_Label +PB12.GPIO_Label=Status_LED +PB12.Locked=true +PB12.Signal=GPIO_Output +PC10.Locked=true +PC10.Mode=Full_Duplex_Master +PC10.Signal=SPI3_SCK +PC11.Locked=true +PC11.Mode=Full_Duplex_Master +PC11.Signal=SPI3_MISO +PC12.Locked=true +PC12.Mode=Full_Duplex_Master +PC12.Signal=SPI3_MOSI +PC6.GPIOParameters=GPIO_Label +PC6.GPIO_Label=PWM_DMA +PC6.Locked=true +PC6.Signal=S_TIM8_CH1 +PD2.GPIOParameters=PinState,GPIO_Label +PD2.GPIO_Label=SPI_CSn +PD2.Locked=true +PD2.PinState=GPIO_PIN_SET +PD2.Signal=GPIO_Output +PinOutPanel.RotationAngle=0 +ProjectManager.AskForMigrate=true +ProjectManager.BackupPrevious=false +ProjectManager.CompilerLinker=GCC +ProjectManager.CompilerOptimize=6 +ProjectManager.ComputerToolchain=false +ProjectManager.CoupleFile=true +ProjectManager.CustomerFirmwarePackage= +ProjectManager.DefaultFWLocation=true +ProjectManager.DeletePrevious=true +ProjectManager.DeviceId=STM32G474RETx +ProjectManager.FirmwarePackage=STM32Cube FW_G4 V1.6.3 +ProjectManager.FreePins=false +ProjectManager.FreePinsContext= +ProjectManager.HalAssertFull=false +ProjectManager.HeapSize=0x1000 +ProjectManager.KeepUserCode=false +ProjectManager.LastFirmware=true +ProjectManager.LibraryCopy=1 +ProjectManager.MainLocation=Src +ProjectManager.NoMain=true +ProjectManager.PreviousToolchain= +ProjectManager.ProjectBuild=false +ProjectManager.ProjectFileName=reference.ioc +ProjectManager.ProjectName=reference +ProjectManager.ProjectStructure= +ProjectManager.RegisterCallBack= +ProjectManager.StackSize=0x4000 +ProjectManager.TargetToolchain=CMake +ProjectManager.ToolChainLocation= +ProjectManager.UAScriptAfterPath= +ProjectManager.UAScriptBeforePath= +ProjectManager.UnderRoot=false +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_SPI3_Init-SPI3-false-HAL-true,5-MX_TIM2_Init-TIM2-false-HAL-true,6-MX_TIM5_Init-TIM5-false-HAL-true,7-MX_USART3_UART_Init-USART3-false-HAL-true,8-MX_TIM8_Init-TIM8-false-HAL-true,9-MX_TIM15_Init-TIM15-false-HAL-true,10-MX_ADC3_Init-ADC3-false-HAL-true,11-MX_CRC_Init-CRC-false-HAL-true,12-MX_FMAC_Init-FMAC-false-HAL-true,13-MX_IWDG_Init-IWDG-false-HAL-true +RCC.ADC12Freq_Value=170000000 +RCC.ADC345Freq_Value=170000000 +RCC.AHBFreq_Value=170000000 +RCC.APB1Freq_Value=170000000 +RCC.APB1TimFreq_Value=170000000 +RCC.APB2Freq_Value=170000000 +RCC.APB2TimFreq_Value=170000000 +RCC.CRSFreq_Value=48000000 +RCC.CortexFreq_Value=170000000 +RCC.EXTERNAL_CLOCK_VALUE=12288000 +RCC.FCLKCortexFreq_Value=170000000 +RCC.FDCANFreq_Value=170000000 +RCC.FamilyName=M +RCC.HCLKFreq_Value=170000000 +RCC.HRTIM1Freq_Value=170000000 +RCC.HSE_VALUE=8000000 +RCC.HSI48_VALUE=48000000 +RCC.HSI_VALUE=16000000 +RCC.I2C1Freq_Value=170000000 +RCC.I2C2Freq_Value=170000000 +RCC.I2C3Freq_Value=170000000 +RCC.I2C4Freq_Value=170000000 +RCC.I2SFreq_Value=170000000 +RCC.IPParameters=ADC12Freq_Value,ADC345Freq_Value,AHBFreq_Value,APB1Freq_Value,APB1TimFreq_Value,APB2Freq_Value,APB2TimFreq_Value,CRSFreq_Value,CortexFreq_Value,EXTERNAL_CLOCK_VALUE,FCLKCortexFreq_Value,FDCANFreq_Value,FamilyName,HCLKFreq_Value,HRTIM1Freq_Value,HSE_VALUE,HSI48_VALUE,HSI_VALUE,I2C1Freq_Value,I2C2Freq_Value,I2C3Freq_Value,I2C4Freq_Value,I2SFreq_Value,LPTIM1Freq_Value,LPUART1Freq_Value,LSCOPinFreq_Value,LSE_VALUE,LSI_VALUE,MCO1PinFreq_Value,PLLM,PLLN,PLLPoutputFreq_Value,PLLQoutputFreq_Value,PLLRCLKFreq_Value,PWRFreq_Value,QSPIFreq_Value,RNGFreq_Value,SAI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,UART4Freq_Value,UART5Freq_Value,USART1Freq_Value,USART2Freq_Value,USART3Freq_Value,USBFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value +RCC.LPTIM1Freq_Value=170000000 +RCC.LPUART1Freq_Value=170000000 +RCC.LSCOPinFreq_Value=32000 +RCC.LSE_VALUE=32768 +RCC.LSI_VALUE=32000 +RCC.MCO1PinFreq_Value=16000000 +RCC.PLLM=RCC_PLLM_DIV4 +RCC.PLLN=85 +RCC.PLLPoutputFreq_Value=170000000 +RCC.PLLQoutputFreq_Value=170000000 +RCC.PLLRCLKFreq_Value=170000000 +RCC.PWRFreq_Value=170000000 +RCC.QSPIFreq_Value=170000000 +RCC.RNGFreq_Value=170000000 +RCC.SAI1Freq_Value=170000000 +RCC.SYSCLKFreq_VALUE=170000000 +RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK +RCC.UART4Freq_Value=170000000 +RCC.UART5Freq_Value=170000000 +RCC.USART1Freq_Value=170000000 +RCC.USART2Freq_Value=170000000 +RCC.USART3Freq_Value=170000000 +RCC.USBFreq_Value=170000000 +RCC.VCOInputFreq_Value=4000000 +RCC.VCOOutputFreq_Value=340000000 +SH.S_TIM15_CH1.0=TIM15_CH1,PWM Generation1 CH1 +SH.S_TIM15_CH1.ConfNb=1 +SH.S_TIM5_CH1.0=TIM5_CH1,Encoder_Interface +SH.S_TIM5_CH1.ConfNb=1 +SH.S_TIM5_CH2.0=TIM5_CH2,Encoder_Interface +SH.S_TIM5_CH2.ConfNb=1 +SH.S_TIM8_CH1.0=TIM8_CH1,PWM Generation1 CH1 +SH.S_TIM8_CH1.ConfNb=1 +SPI3.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_64 +SPI3.CLKPhase=SPI_PHASE_2EDGE +SPI3.CLKPolarity=SPI_POLARITY_HIGH +SPI3.CalculateBaudRate=2.65625 MBits/s +SPI3.DataSize=SPI_DATASIZE_8BIT +SPI3.Direction=SPI_DIRECTION_2LINES +SPI3.IPParameters=VirtualType,Mode,Direction,BaudRatePrescaler,DataSize,CLKPhase,CLKPolarity,CalculateBaudRate +SPI3.Mode=SPI_MODE_MASTER +SPI3.VirtualType=VM_MASTER +TIM15.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 +TIM15.IPParameters=Channel-PWM Generation1 CH1,Prescaler,PeriodNoDither +TIM15.PeriodNoDither=999 +TIM15.Prescaler=169 +TIM2.IPParameters=Prescaler,Period +TIM2.Period=4294967295 +TIM2.Prescaler=0 +TIM5.EncoderMode=TIM_ENCODERMODE_TI12 +TIM5.IPParameters=EncoderMode +TIM8.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 +TIM8.IPParameters=Channel-PWM Generation1 CH1,Prescaler,PeriodNoDither +TIM8.PeriodNoDither=212 +TIM8.Prescaler=0 +USART3.IPParameters=VirtualMode-Asynchronous +USART3.VirtualMode-Asynchronous=VM_ASYNC +VP_ADC3_Vbat_Input.Mode=IN-Vbat +VP_ADC3_Vbat_Input.Signal=ADC3_Vbat_Input +VP_ADC3_Vref_Input.Mode=IN-Vrefint +VP_ADC3_Vref_Input.Signal=ADC3_Vref_Input +VP_CRC_VS_CRC.Mode=CRC_Activate +VP_CRC_VS_CRC.Signal=CRC_VS_CRC +VP_FMAC_VS_FMAC.Mode=FMAC_Activate +VP_FMAC_VS_FMAC.Signal=FMAC_VS_FMAC +VP_IWDG_VS_IWDG.Mode=IWDG_Activate +VP_IWDG_VS_IWDG.Signal=IWDG_VS_IWDG +VP_SYS_VS_DBSignals.Mode=DisableDeadBatterySignals +VP_SYS_VS_DBSignals.Signal=SYS_VS_DBSignals +VP_SYS_VS_Systick.Mode=SysTick +VP_SYS_VS_Systick.Signal=SYS_VS_Systick +VP_TIM2_VS_ClockSourceINT.Mode=Internal +VP_TIM2_VS_ClockSourceINT.Signal=TIM2_VS_ClockSourceINT +board=custom diff --git a/reference/h7/.gitignore b/reference/h7/.gitignore new file mode 100644 index 0000000..765b11c --- /dev/null +++ b/reference/h7/.gitignore @@ -0,0 +1,6 @@ +# The Cube files STM32CubeMX generates from reference.ioc +* +!.gitignore +!CMakeLists.txt +!bench.cpp +!reference.ioc diff --git a/reference/h7/CMakeLists.txt b/reference/h7/CMakeLists.txt new file mode 100644 index 0000000..77f6bb8 --- /dev/null +++ b/reference/h7/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.28...4.4) + +############################################################################### +## Reference STM32H725 project +############################################################################### + +# The smallest CubeMX project that exercises every peripheral micras-lib uses, built and linked with +# its bench program the way a consumer of the stm32 backend builds its firmware. What every family's +# reference shares is in ../common. + +set(MICRAS_REFERENCE_FAMILY h7) +include(${CMAKE_CURRENT_LIST_DIR}/../common/toolchain.cmake) + +project(micras_lib_reference_h7 C CXX ASM) + +include(${CMAKE_CURRENT_LIST_DIR}/../common/reference.cmake) diff --git a/reference/h7/bench.cpp b/reference/h7/bench.cpp new file mode 100644 index 0000000..4297f4a --- /dev/null +++ b/reference/h7/bench.cpp @@ -0,0 +1,22 @@ +/** + * @file + */ + +#include + +#include +#include + +#include "bench.hpp" + +static uint32_t vdda_mv(uint16_t vrefint_reading) { + return __LL_ADC_CALC_VREFANALOG_VOLTAGE(static_cast(vrefint_reading), LL_ADC_RESOLUTION_12B); +} + +int main() { + micras::reference::run_bench({ + .uart_init = MX_UART4_Init, + .uart = &huart4, + .vdda_mv = vdda_mv, + }); +} diff --git a/reference/h7/reference.ioc b/reference/h7/reference.ioc new file mode 100644 index 0000000..4e6f42f --- /dev/null +++ b/reference/h7/reference.ioc @@ -0,0 +1,494 @@ +#MicroXplorer Configuration settings - do not modify +ADC3.Channel-0\#ChannelRegularConversion=ADC_CHANNEL_VREFINT +ADC3.Channel-1\#ChannelRegularConversion=ADC_CHANNEL_VBAT +ADC3.ClockPrescaler=ADC_CLOCK_ASYNC_DIV4 +ADC3.ClockPrescalerADC3=ADC_CLOCK_ASYNC_DIV8 +ADC3.ContinuousConvMode=ENABLE +ADC3.ConversionDataManagement=ADC_CONVERSIONDATA_DMA_CIRCULAR +ADC3.DMAContinuousRequests=ENABLE +ADC3.IPParameters=Channel-0#ChannelRegularConversion,Rank-0#ChannelRegularConversion,SamplingTime-0#ChannelRegularConversion,OffsetNumber-0#ChannelRegularConversion,OffsetSign-0#ChannelRegularConversion,Channel-1#ChannelRegularConversion,Rank-1#ChannelRegularConversion,SamplingTime-1#ChannelRegularConversion,OffsetNumber-1#ChannelRegularConversion,OffsetSign-1#ChannelRegularConversion,NbrOfConversion,NbrOfConversionFlag,ScanConvMode,ClockPrescaler,ClockPrescalerADC3,ContinuousConvMode,ConversionDataManagement,DMAContinuousRequests,Overrun +ADC3.NbrOfConversion=2 +ADC3.NbrOfConversionFlag=1 +ADC3.OffsetNumber-0\#ChannelRegularConversion=ADC_OFFSET_NONE +ADC3.OffsetNumber-1\#ChannelRegularConversion=ADC_OFFSET_NONE +ADC3.OffsetSign-0\#ChannelRegularConversion=ADC3_OFFSET_SIGN_NEGATIVE +ADC3.OffsetSign-1\#ChannelRegularConversion=ADC3_OFFSET_SIGN_NEGATIVE +ADC3.Overrun=ADC_OVR_DATA_OVERWRITTEN +ADC3.Rank-0\#ChannelRegularConversion=1 +ADC3.Rank-1\#ChannelRegularConversion=2 +ADC3.SamplingTime-0\#ChannelRegularConversion=ADC3_SAMPLETIME_640CYCLES_5 +ADC3.SamplingTime-1\#ChannelRegularConversion=ADC3_SAMPLETIME_640CYCLES_5 +ADC3.ScanConvMode=ADC_SCAN_ENABLE +CAD.formats= +CAD.pinconfig= +CAD.provider= +CORTEX_M7.CPU_DCache=Disabled +CORTEX_M7.CPU_ICache=Enabled +CORTEX_M7.IPParameters=default_mode_Activation,CPU_ICache,CPU_DCache +CORTEX_M7.default_mode_Activation=0 +CRC.CRCLength=CRC_POLYLENGTH_8B +CRC.DefaultInitValueUse=DEFAULT_INIT_VALUE_DISABLE +CRC.DefaultPolynomialUse=DEFAULT_POLYNOMIAL_DISABLE +CRC.GeneratingPolynomial=X4+X3+X2+X0 +CRC.IPParameters=DefaultPolynomialUse,DefaultInitValueUse,CRCLength,GeneratingPolynomial,InitValue +CRC.InitValue=0xC4 +Dma.ADC3.3.Direction=DMA_PERIPH_TO_MEMORY +Dma.ADC3.3.EventEnable=DISABLE +Dma.ADC3.3.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.ADC3.3.Instance=DMA1_Stream5 +Dma.ADC3.3.MemDataAlignment=DMA_MDATAALIGN_HALFWORD +Dma.ADC3.3.MemInc=DMA_MINC_ENABLE +Dma.ADC3.3.Mode=DMA_CIRCULAR +Dma.ADC3.3.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD +Dma.ADC3.3.PeriphInc=DMA_PINC_DISABLE +Dma.ADC3.3.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.ADC3.3.Priority=DMA_PRIORITY_LOW +Dma.ADC3.3.RequestNumber=1 +Dma.ADC3.3.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.ADC3.3.SignalID=NONE +Dma.ADC3.3.SyncEnable=DISABLE +Dma.ADC3.3.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.ADC3.3.SyncRequestNumber=1 +Dma.ADC3.3.SyncSignalID=NONE +Dma.Request0=TIM8_CH1 +Dma.Request1=UART4_RX +Dma.Request2=UART4_TX +Dma.Request3=ADC3 +Dma.Request4=SPI3_RX +Dma.Request5=SPI3_TX +Dma.RequestsNb=6 +Dma.SPI3_RX.4.Direction=DMA_PERIPH_TO_MEMORY +Dma.SPI3_RX.4.EventEnable=DISABLE +Dma.SPI3_RX.4.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.SPI3_RX.4.Instance=DMA1_Stream6 +Dma.SPI3_RX.4.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.SPI3_RX.4.MemInc=DMA_MINC_ENABLE +Dma.SPI3_RX.4.Mode=DMA_NORMAL +Dma.SPI3_RX.4.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.SPI3_RX.4.PeriphInc=DMA_PINC_DISABLE +Dma.SPI3_RX.4.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.SPI3_RX.4.Priority=DMA_PRIORITY_LOW +Dma.SPI3_RX.4.RequestNumber=1 +Dma.SPI3_RX.4.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.SPI3_RX.4.SignalID=NONE +Dma.SPI3_RX.4.SyncEnable=DISABLE +Dma.SPI3_RX.4.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.SPI3_RX.4.SyncRequestNumber=1 +Dma.SPI3_RX.4.SyncSignalID=NONE +Dma.SPI3_TX.5.Direction=DMA_MEMORY_TO_PERIPH +Dma.SPI3_TX.5.EventEnable=DISABLE +Dma.SPI3_TX.5.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.SPI3_TX.5.Instance=DMA1_Stream7 +Dma.SPI3_TX.5.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.SPI3_TX.5.MemInc=DMA_MINC_ENABLE +Dma.SPI3_TX.5.Mode=DMA_NORMAL +Dma.SPI3_TX.5.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.SPI3_TX.5.PeriphInc=DMA_PINC_DISABLE +Dma.SPI3_TX.5.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.SPI3_TX.5.Priority=DMA_PRIORITY_LOW +Dma.SPI3_TX.5.RequestNumber=1 +Dma.SPI3_TX.5.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.SPI3_TX.5.SignalID=NONE +Dma.SPI3_TX.5.SyncEnable=DISABLE +Dma.SPI3_TX.5.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.SPI3_TX.5.SyncRequestNumber=1 +Dma.SPI3_TX.5.SyncSignalID=NONE +Dma.TIM8_CH1.0.Direction=DMA_MEMORY_TO_PERIPH +Dma.TIM8_CH1.0.EventEnable=DISABLE +Dma.TIM8_CH1.0.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.TIM8_CH1.0.Instance=DMA1_Stream2 +Dma.TIM8_CH1.0.MemDataAlignment=DMA_MDATAALIGN_HALFWORD +Dma.TIM8_CH1.0.MemInc=DMA_MINC_ENABLE +Dma.TIM8_CH1.0.Mode=DMA_NORMAL +Dma.TIM8_CH1.0.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD +Dma.TIM8_CH1.0.PeriphInc=DMA_PINC_DISABLE +Dma.TIM8_CH1.0.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.TIM8_CH1.0.Priority=DMA_PRIORITY_LOW +Dma.TIM8_CH1.0.RequestNumber=1 +Dma.TIM8_CH1.0.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.TIM8_CH1.0.SignalID=NONE +Dma.TIM8_CH1.0.SyncEnable=DISABLE +Dma.TIM8_CH1.0.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.TIM8_CH1.0.SyncRequestNumber=1 +Dma.TIM8_CH1.0.SyncSignalID=NONE +Dma.UART4_RX.1.Direction=DMA_PERIPH_TO_MEMORY +Dma.UART4_RX.1.EventEnable=DISABLE +Dma.UART4_RX.1.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.UART4_RX.1.Instance=DMA1_Stream3 +Dma.UART4_RX.1.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.UART4_RX.1.MemInc=DMA_MINC_ENABLE +Dma.UART4_RX.1.Mode=DMA_CIRCULAR +Dma.UART4_RX.1.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.UART4_RX.1.PeriphInc=DMA_PINC_DISABLE +Dma.UART4_RX.1.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.UART4_RX.1.Priority=DMA_PRIORITY_LOW +Dma.UART4_RX.1.RequestNumber=1 +Dma.UART4_RX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.UART4_RX.1.SignalID=NONE +Dma.UART4_RX.1.SyncEnable=DISABLE +Dma.UART4_RX.1.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.UART4_RX.1.SyncRequestNumber=1 +Dma.UART4_RX.1.SyncSignalID=NONE +Dma.UART4_TX.2.Direction=DMA_MEMORY_TO_PERIPH +Dma.UART4_TX.2.EventEnable=DISABLE +Dma.UART4_TX.2.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.UART4_TX.2.Instance=DMA1_Stream4 +Dma.UART4_TX.2.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.UART4_TX.2.MemInc=DMA_MINC_ENABLE +Dma.UART4_TX.2.Mode=DMA_NORMAL +Dma.UART4_TX.2.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.UART4_TX.2.PeriphInc=DMA_PINC_DISABLE +Dma.UART4_TX.2.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.UART4_TX.2.Priority=DMA_PRIORITY_LOW +Dma.UART4_TX.2.RequestNumber=1 +Dma.UART4_TX.2.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.UART4_TX.2.SignalID=NONE +Dma.UART4_TX.2.SyncEnable=DISABLE +Dma.UART4_TX.2.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.UART4_TX.2.SyncRequestNumber=1 +Dma.UART4_TX.2.SyncSignalID=NONE +File.Version=6 +GPIO.groupedBy=Group By Peripherals +KeepUserPlacement=false +MMTAppReg1.MEMORYMAP.AppRegionName=RAM +MMTAppReg1.MEMORYMAP.ContextName=Cortex-M7NS +MMTAppReg1.MEMORYMAP.CoreName=Arm Cortex-M7 +MMTAppReg1.MEMORYMAP.DefaultDataRegion=true +MMTAppReg1.MEMORYMAP.IPParameters=StartAddress,Size,CoreName,ContextName,Name,DefaultDataRegion +MMTAppReg1.MEMORYMAP.Name=RAM +MMTAppReg1.MEMORYMAP.Size=327680 +MMTAppReg1.MEMORYMAP.StartAddress=0x24000000 +MMTAppReg2.MEMORYMAP.AppRegionName=RAM_D2 +MMTAppReg2.MEMORYMAP.ContextName=Cortex-M7NS +MMTAppReg2.MEMORYMAP.CoreName=Arm Cortex-M7 +MMTAppReg2.MEMORYMAP.IPParameters=StartAddress,Size,CoreName,ContextName,Name +MMTAppReg2.MEMORYMAP.Name=RAM_D2 +MMTAppReg2.MEMORYMAP.Size=32768 +MMTAppReg2.MEMORYMAP.StartAddress=0x30000000 +MMTAppReg3.MEMORYMAP.AppRegionName=RAM_D3 +MMTAppReg3.MEMORYMAP.ContextName=Cortex-M7NS +MMTAppReg3.MEMORYMAP.CoreName=Arm Cortex-M7 +MMTAppReg3.MEMORYMAP.IPParameters=StartAddress,Size,CoreName,ContextName,Name +MMTAppReg3.MEMORYMAP.Name=RAM_D3 +MMTAppReg3.MEMORYMAP.Size=16384 +MMTAppReg3.MEMORYMAP.StartAddress=0x38000000 +MMTAppReg4.MEMORYMAP.AppRegionName=ITCMRAM +MMTAppReg4.MEMORYMAP.ContextName=Cortex-M7NS +MMTAppReg4.MEMORYMAP.CoreName=Arm Cortex-M7 +MMTAppReg4.MEMORYMAP.IPParameters=StartAddress,Size,CoreName,ContextName,Name +MMTAppReg4.MEMORYMAP.Name=ITCMRAM +MMTAppReg4.MEMORYMAP.Size=65536 +MMTAppReg4.MEMORYMAP.StartAddress=0x00000000 +MMTAppReg5.MEMORYMAP.AppRegionName=FLASH +MMTAppReg5.MEMORYMAP.ContextName=Cortex-M7NS +MMTAppReg5.MEMORYMAP.CoreName=Arm Cortex-M7 +MMTAppReg5.MEMORYMAP.DefaultCodeRegion=true +MMTAppReg5.MEMORYMAP.IPParameters=StartAddress,Size,CoreName,MemType,ContextName,Name,DefaultCodeRegion,ISRRegion,RootBootRegion +MMTAppReg5.MEMORYMAP.ISRRegion=true +MMTAppReg5.MEMORYMAP.MemType=ROM +MMTAppReg5.MEMORYMAP.Name=FLASH +MMTAppReg5.MEMORYMAP.RootBootRegion=true +MMTAppReg5.MEMORYMAP.Size=1048576 +MMTAppReg5.MEMORYMAP.StartAddress=0x08000000 +MMTAppReg6.MEMORYMAP.AppRegionName=DTCMRAM +MMTAppReg6.MEMORYMAP.ContextName=Cortex-M7NS +MMTAppReg6.MEMORYMAP.CoreName=Arm Cortex-M7 +MMTAppReg6.MEMORYMAP.IPParameters=StartAddress,Size,CoreName,ContextName,Name +MMTAppReg6.MEMORYMAP.Name=DTCMRAM +MMTAppReg6.MEMORYMAP.Size=131072 +MMTAppReg6.MEMORYMAP.StartAddress=0x20000000 +MMTAppRegionsCount=6 +MMTConfigApplied=false +MMTProjectManagerApplied=true +MMTSectionSuffix=_Section +Mcu.CPN=STM32H725RGV3 +Mcu.Family=STM32H7 +Mcu.IP0=ADC3 +Mcu.IP1=CORTEX_M7 +Mcu.IP10=SPI3 +Mcu.IP11=SYS +Mcu.IP12=TIM15 +Mcu.IP13=TIM2 +Mcu.IP14=TIM5 +Mcu.IP15=TIM8 +Mcu.IP16=UART4 +Mcu.IP2=CRC +Mcu.IP3=DEBUG +Mcu.IP4=DMA +Mcu.IP5=FMAC +Mcu.IP6=IWDG1 +Mcu.IP7=MEMORYMAP +Mcu.IP8=NVIC +Mcu.IP9=RCC +Mcu.IPNb=17 +Mcu.Name=STM32H725RGVx +Mcu.Package=VFQFPN68 +Mcu.Pin0=PA0 +Mcu.Pin1=PA1 +Mcu.Pin10=PC10 +Mcu.Pin11=PC11 +Mcu.Pin12=PC12 +Mcu.Pin13=PD2 +Mcu.Pin14=VP_ADC3_Vbat_Input +Mcu.Pin15=VP_ADC3_Vref_Input +Mcu.Pin16=VP_CRC_VS_CRC +Mcu.Pin17=VP_FMAC_VS_FMAC +Mcu.Pin18=VP_IWDG1_VS_IWDG +Mcu.Pin19=VP_SYS_VS_Systick +Mcu.Pin2=PA2 +Mcu.Pin20=VP_MEMORYMAP_VS_MEMORYMAP +Mcu.Pin21=VP_TIM2_VS_ClockSourceINT +Mcu.Pin3=PA7 +Mcu.Pin4=PA11 +Mcu.Pin5=PA12 +Mcu.Pin6=PA13(JTMS/SWDIO) +Mcu.Pin7=PA14(JTCK/SWCLK) +Mcu.Pin8=PB12 +Mcu.Pin9=PC6 +Mcu.PinsNb=22 +Mcu.ThirdPartyNb=0 +Mcu.UserConstants= +Mcu.UserName=STM32H725RGVx +MxCube.Version=6.18.1 +MxDb.Version=DB.6.0.181 +NVIC.ADC3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.DMA1_Stream2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream3_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream5_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream6_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream7_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.ForceEnableDMAVector=true +NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:false\:false\:false\:false +NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 +NVIC.SPI3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false +NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +PA0.GPIOParameters=GPIO_PuPd,GPIO_Label +PA0.GPIO_Label=Encoder_A +PA0.GPIO_PuPd=GPIO_PULLUP +PA0.Locked=true +PA0.Signal=S_TIM5_CH1 +PA1.GPIOParameters=GPIO_PuPd,GPIO_Label +PA1.GPIO_Label=Encoder_B +PA1.GPIO_PuPd=GPIO_PULLUP +PA1.Locked=true +PA1.Signal=S_TIM5_CH2 +PA11.Locked=true +PA11.Mode=Asynchronous +PA11.Signal=UART4_RX +PA12.Locked=true +PA12.Mode=Asynchronous +PA12.Signal=UART4_TX +PA13(JTMS/SWDIO).Locked=true +PA13(JTMS/SWDIO).Mode=Serial_Wire +PA13(JTMS/SWDIO).Signal=DEBUG_JTMS-SWDIO +PA14(JTCK/SWCLK).Locked=true +PA14(JTCK/SWCLK).Mode=Serial_Wire +PA14(JTCK/SWCLK).Signal=DEBUG_JTCK-SWCLK +PA2.GPIOParameters=GPIO_Label +PA2.GPIO_Label=PWM +PA2.Locked=true +PA2.Signal=S_TIM15_CH1 +PA7.GPIOParameters=GPIO_PuPd,GPIO_Label +PA7.GPIO_Label=Button +PA7.GPIO_PuPd=GPIO_PULLUP +PA7.Locked=true +PA7.Signal=GPIO_Input +PB12.GPIOParameters=GPIO_Label +PB12.GPIO_Label=Status_LED +PB12.Locked=true +PB12.Signal=GPIO_Output +PC10.Locked=true +PC10.Mode=Full_Duplex_Master +PC10.Signal=SPI3_SCK +PC11.Locked=true +PC11.Mode=Full_Duplex_Master +PC11.Signal=SPI3_MISO +PC12.Locked=true +PC12.Mode=Full_Duplex_Master +PC12.Signal=SPI3_MOSI +PC6.GPIOParameters=GPIO_Label +PC6.GPIO_Label=PWM_DMA +PC6.Locked=true +PC6.Signal=S_TIM8_CH1 +PD2.GPIOParameters=PinState,GPIO_Label +PD2.GPIO_Label=SPI_CSn +PD2.Locked=true +PD2.PinState=GPIO_PIN_SET +PD2.Signal=GPIO_Output +PinOutPanel.RotationAngle=0 +ProjectManager.AskForMigrate=true +ProjectManager.BackupPrevious=false +ProjectManager.CompilerLinker=GCC +ProjectManager.CompilerOptimize=6 +ProjectManager.ComputerToolchain=false +ProjectManager.CoupleFile=true +ProjectManager.CustomerFirmwarePackage= +ProjectManager.DefaultFWLocation=true +ProjectManager.DeletePrevious=true +ProjectManager.DeviceId=STM32H725RGVx +ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.13.0 +ProjectManager.FreePins=false +ProjectManager.FreePinsContext= +ProjectManager.HalAssertFull=false +ProjectManager.HeapSize=0x1000 +ProjectManager.KeepUserCode=false +ProjectManager.LastFirmware=true +ProjectManager.LibraryCopy=1 +ProjectManager.MainLocation=Src +ProjectManager.NoMain=true +ProjectManager.PreviousToolchain= +ProjectManager.ProjectBuild=false +ProjectManager.ProjectFileName=reference.ioc +ProjectManager.ProjectName=reference +ProjectManager.ProjectStructure= +ProjectManager.RegisterCallBack= +ProjectManager.StackSize=0x4000 +ProjectManager.TargetToolchain=CMake +ProjectManager.ToolChainLocation= +ProjectManager.UAScriptAfterPath= +ProjectManager.UAScriptBeforePath= +ProjectManager.UnderRoot=false +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_SPI3_Init-SPI3-false-HAL-true,5-MX_TIM2_Init-TIM2-false-HAL-true,6-MX_TIM5_Init-TIM5-false-HAL-true,7-MX_UART4_Init-UART4-false-HAL-true,8-MX_TIM8_Init-TIM8-false-HAL-true,9-MX_TIM15_Init-TIM15-false-HAL-true,10-MX_ADC3_Init-ADC3-false-HAL-true,11-MX_CRC_Init-CRC-false-HAL-true,12-MX_FMAC_Init-FMAC-false-HAL-true,13-MX_IWDG1_Init-IWDG1-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true +RCC.ADCFreq_Value=160000000 +RCC.AHB12Freq_Value=200000000 +RCC.AHB4Freq_Value=200000000 +RCC.APB1Freq_Value=100000000 +RCC.APB2Freq_Value=100000000 +RCC.APB3Freq_Value=100000000 +RCC.APB4Freq_Value=100000000 +RCC.AXIClockFreq_Value=200000000 +RCC.CECFreq_Value=32000 +RCC.CKPERFreq_Value=64000000 +RCC.CortexFreq_Value=400000000 +RCC.CpuClockFreq_Value=400000000 +RCC.D1CPREFreq_Value=400000000 +RCC.D1PPRE=RCC_APB3_DIV2 +RCC.D2PPRE1=RCC_APB1_DIV2 +RCC.D2PPRE2=RCC_APB2_DIV2 +RCC.D3PPRE=RCC_APB4_DIV2 +RCC.DFSDMACLkFreq_Value=400000000 +RCC.DFSDMFreq_Value=100000000 +RCC.DIVM1=4 +RCC.DIVM2=63 +RCC.DIVN1=50 +RCC.DIVN2=315 +RCC.DIVN3=125 +RCC.DIVP1=2 +RCC.DIVP1Freq_Value=400000000 +RCC.DIVP2Freq_Value=160000000 +RCC.DIVP3=2 +RCC.DIVP3Freq_Value=125000000 +RCC.DIVQ1Freq_Value=400000000 +RCC.DIVQ2=1 +RCC.DIVQ2Freq_Value=320000000 +RCC.DIVQ3=1 +RCC.DIVQ3Freq_Value=250000000 +RCC.DIVR1Freq_Value=400000000 +RCC.DIVR2Freq_Value=160000000 +RCC.DIVR3=1 +RCC.DIVR3Freq_Value=250000000 +RCC.FDCANFreq_Value=400000000 +RCC.FMCFreq_Value=200000000 +RCC.FamilyName=M +RCC.HCLK3ClockFreq_Value=200000000 +RCC.HCLKFreq_Value=200000000 +RCC.HPRE=RCC_HCLK_DIV2 +RCC.I2C123Freq_Value=100000000 +RCC.I2C4Freq_Value=100000000 +RCC.IPParameters=ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVN1,DIVN2,DIVN3,DIVP1,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,I2C123Freq_Value,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL1_VCI_Range-AdvancedSettings,PLL2FRACN,PLL2_VCO_SEL-AdvancedSettings,PLL3FRACN,PLLFRACN,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123CLockSelection,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,SupplySource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value +RCC.LPTIM1Freq_Value=100000000 +RCC.LPTIM2Freq_Value=100000000 +RCC.LPTIM345Freq_Value=100000000 +RCC.LPUART1Freq_Value=100000000 +RCC.LTDCFreq_Value=250000000 +RCC.MCO1PinFreq_Value=64000000 +RCC.MCO2PinFreq_Value=400000000 +RCC.PLL1_VCI_Range-AdvancedSettings=RCC_PLL1VCIRANGE_1 +RCC.PLL2FRACN=0 +RCC.PLL2_VCO_SEL-AdvancedSettings=RCC_PLL2VCOWIDE +RCC.PLL3FRACN=0 +RCC.PLLFRACN=0 +RCC.QSPIFreq_Value=200000000 +RCC.RNGFreq_Value=48000000 +RCC.RTCFreq_Value=32000 +RCC.SAI1Freq_Value=400000000 +RCC.SAI4AFreq_Value=400000000 +RCC.SAI4BFreq_Value=400000000 +RCC.SDMMCFreq_Value=400000000 +RCC.SPDIFRXFreq_Value=400000000 +RCC.SPI123CLockSelection=RCC_SPI123CLKSOURCE_PLL3 +RCC.SPI123Freq_Value=125000000 +RCC.SPI45Freq_Value=100000000 +RCC.SPI6Freq_Value=100000000 +RCC.SWPMI1Freq_Value=100000000 +RCC.SYSCLKFreq_VALUE=400000000 +RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK +RCC.SupplySource=PWR_DIRECT_SMPS_SUPPLY +RCC.Tim1OutputFreq_Value=200000000 +RCC.Tim2OutputFreq_Value=200000000 +RCC.TraceFreq_Value=400000000 +RCC.USART16Freq_Value=100000000 +RCC.USART234578Freq_Value=100000000 +RCC.USBFreq_Value=400000000 +RCC.VCO1OutputFreq_Value=800000000 +RCC.VCO2OutputFreq_Value=320000000 +RCC.VCO3OutputFreq_Value=250000000 +RCC.VCOInput1Freq_Value=16000000 +RCC.VCOInput2Freq_Value=1015873.0158730159 +RCC.VCOInput3Freq_Value=2000000 +SH.S_TIM15_CH1.0=TIM15_CH1,PWM Generation1 CH1 +SH.S_TIM15_CH1.ConfNb=1 +SH.S_TIM5_CH1.0=TIM5_CH1,Encoder_Interface +SH.S_TIM5_CH1.ConfNb=1 +SH.S_TIM5_CH2.0=TIM5_CH2,Encoder_Interface +SH.S_TIM5_CH2.ConfNb=1 +SH.S_TIM8_CH1.0=TIM8_CH1,PWM Generation1 CH1 +SH.S_TIM8_CH1.ConfNb=1 +SPI3.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_32 +SPI3.CLKPhase=SPI_PHASE_2EDGE +SPI3.CLKPolarity=SPI_POLARITY_HIGH +SPI3.CalculateBaudRate=3.90625 MBits/s +SPI3.DataSize=SPI_DATASIZE_8BIT +SPI3.Direction=SPI_DIRECTION_2LINES +SPI3.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,CLKPolarity,CLKPhase,BaudRatePrescaler,MasterKeepIOState +SPI3.MasterKeepIOState=SPI_MASTER_KEEP_IO_STATE_ENABLE +SPI3.Mode=SPI_MODE_MASTER +SPI3.VirtualType=VM_MASTER +TIM15.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 +TIM15.IPParameters=Channel-PWM Generation1 CH1,Prescaler,Period +TIM15.Period=999 +TIM15.Prescaler=199 +TIM2.IPParameters=Prescaler,Period +TIM2.Period=4294967295 +TIM2.Prescaler=0 +TIM5.EncoderMode=TIM_ENCODERMODE_TI12 +TIM5.IPParameters=EncoderMode +TIM8.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 +TIM8.IPParameters=Channel-PWM Generation1 CH1,Prescaler,Period +TIM8.Period=249 +TIM8.Prescaler=0 +VP_ADC3_Vbat_Input.Mode=IN-Vbat +VP_ADC3_Vbat_Input.Signal=ADC3_Vbat_Input +VP_ADC3_Vref_Input.Mode=IN-Vrefint +VP_ADC3_Vref_Input.Signal=ADC3_Vref_Input +VP_CRC_VS_CRC.Mode=CRC_Activate +VP_CRC_VS_CRC.Signal=CRC_VS_CRC +VP_FMAC_VS_FMAC.Mode=FMAC_Activate +VP_FMAC_VS_FMAC.Signal=FMAC_VS_FMAC +VP_IWDG1_VS_IWDG.Mode=IWDG_Activate +VP_IWDG1_VS_IWDG.Signal=IWDG1_VS_IWDG +VP_MEMORYMAP_VS_MEMORYMAP.Mode=CurAppReg +VP_MEMORYMAP_VS_MEMORYMAP.Signal=MEMORYMAP_VS_MEMORYMAP +VP_SYS_VS_Systick.Mode=SysTick +VP_SYS_VS_Systick.Signal=SYS_VS_Systick +VP_TIM2_VS_ClockSourceINT.Mode=Internal +VP_TIM2_VS_ClockSourceINT.Signal=TIM2_VS_ClockSourceINT +board=custom diff --git a/scripts/check_no_chip_macros.sh b/scripts/check_no_chip_macros.sh new file mode 100755 index 0000000..0a73bd9 --- /dev/null +++ b/scripts/check_no_chip_macros.sh @@ -0,0 +1,46 @@ +#!/usr/bin/env bash +# Fails when the code every STM32 family shares tests a chip in the preprocessor. +# +# Usage: scripts/check_no_chip_macros.sh [] +# +# The chip-specific code of the stm32 backend lives in micras_hal/stm32//, and CMake compiles +# exactly one family. Everything else is shared, so a conditional directive there (#if, #ifdef, +# #ifndef, #elif, #elifdef, #elifndef) that names a chip, a family, a core or a peripheral that only +# some parts have is an error: that code belongs in a family function or a family header. The include +# guards (#ifndef MICRAS_..._HPP) name none of them. + +set -euo pipefail + +root=${1:-$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)} + +shared_paths=( + micras_hal/include + micras_hal/stm32/src + micras_proxy + micras_core + micras_nav + micras_comm +) + +directive='^[[:space:]]*#[[:space:]]*(if|ifdef|ifndef|elif|elifdef|elifndef)\b' +chip_names='\b(STM32\w*|IWDG\w*|FLASH_\w+|RCC_\w+|DBGMCU\w*|DWT\w*|SCB_\w+|ITM_\w+|__CORTEX_M|__ARM_ARCH\w*|__FPU_\w+|__[ID]CACHE_PRESENT|__MPU_PRESENT|CORE_CM\w*|ADC_CALIB\w*|\w+_TypeDef)\b' + +cd "$root" + +for path in "${shared_paths[@]}"; do + if [ ! -d "$path" ]; then + echo "check_no_chip_macros: $path does not exist under $root" >&2 + exit 2 + fi +done + +matches=$(grep -rnE --include='*.hpp' --include='*.tpp' --include='*.cpp' --include='*.h' --include='*.c' \ + "$directive.*($chip_names)" "${shared_paths[@]}" || true) + +if [ -n "$matches" ]; then + echo "Chip macros in code every family shares; move them into micras_hal/stm32//:" >&2 + echo "$matches" >&2 + exit 1 +fi + +echo "No chip macro in ${shared_paths[*]}" diff --git a/tests/.clang-tidy b/tests/.clang-tidy new file mode 100644 index 0000000..ce6b632 --- /dev/null +++ b/tests/.clang-tidy @@ -0,0 +1,7 @@ +--- +# The test macros expand into branches that count against the cognitive complexity of every test +# case, whatever the test itself does +InheritParentConfig: true +Checks: + -readability-function-cognitive-complexity, +... diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt new file mode 100644 index 0000000..dcd6c1e --- /dev/null +++ b/tests/CMakeLists.txt @@ -0,0 +1,86 @@ +############################################################################### +## Tests, on the host backend +############################################################################### + +include(FetchContent) + +# Pinned by the commit of v2.5.3. SYSTEM keeps the project warnings out of its header. +FetchContent_Declare( + doctest + GIT_REPOSITORY https://github.com/doctest/doctest.git + GIT_TAG 2d0a9359a60c51affe2a9bebb1be1dca47868151 + SYSTEM +) + +set(DOCTEST_NO_INSTALL ON) +set(DOCTEST_WITH_TESTS OFF) +FetchContent_MakeAvailable(doctest) + +include(${doctest_SOURCE_DIR}/scripts/cmake/doctest.cmake) + +############################################################################### +## Support +############################################################################### + +add_library(micras_lib_test_main STATIC + support/main.cpp +) + +target_link_libraries(micras_lib_test_main PUBLIC + doctest::doctest +) + +target_include_directories(micras_lib_test_main PUBLIC + support +) + +micras_apply_warnings(micras_lib_test_main WERROR ${MICRAS_LIB_WERROR}) + +# micras_lib_add_suite( ...) +# +# One executable per folder of tests, whose test cases CTest discovers one by one +function(micras_lib_add_suite SUITE) + file(GLOB SUITE_SOURCES CONFIGURE_DEPENDS ${SUITE}/*.cpp) + + add_executable(micras_lib_${SUITE}_tests + ${SUITE_SOURCES} + ) + + target_link_libraries(micras_lib_${SUITE}_tests PRIVATE + micras_lib_test_main + ${ARGN} + ) + + micras_apply_warnings(micras_lib_${SUITE}_tests WERROR ${MICRAS_LIB_WERROR}) + + doctest_discover_tests(micras_lib_${SUITE}_tests + TEST_PREFIX "${SUITE}/" + ) +endfunction() + +############################################################################### +## Suites +############################################################################### + +micras_lib_add_suite(core micras::core) + +if(MICRAS_LIB_COMM) + micras_lib_add_suite(comm micras::comm) +endif() + +if(MICRAS_LIB_NAV) + micras_lib_add_suite(nav micras::nav) + + target_compile_definitions(micras_lib_nav_tests PRIVATE + MICRAS_LIB_TEST_MAZES="${CMAKE_CURRENT_SOURCE_DIR}/data/mazes" + ) +endif() + +if(MICRAS_LIB_HAL) + micras_lib_add_suite(hal micras::hal) +endif() + +if(MICRAS_LIB_PROXY) + micras_lib_add_suite(proxy micras::proxy micras::proxy_models) + micras_lib_add_suite(models micras::proxy_models micras::proxy) +endif() diff --git a/tests/comm/test_frame.cpp b/tests/comm/test_frame.cpp new file mode 100644 index 0000000..d4dc09a --- /dev/null +++ b/tests/comm/test_frame.cpp @@ -0,0 +1,127 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include + +#include "micras/comm/frame.hpp" +#include "micras/comm/protocol.hpp" + +namespace micras::comm { +namespace { +struct Vector { + std::string_view name; + MessageType type; + std::vector payload; + std::vector frame; +}; +} // namespace + +static std::vector monitor_vectors() { + return { + {.name = "hello", .type = MessageType{0x01}, .payload = {}, .frame = {4, 1, 1, 1, 0}}, + {.name = "hello_ack", + .type = MessageType{0x81}, + .payload = {1, 133, 114, 115, 247, 4, 0, 125, 0, 0, 0, 0, 1}, + .frame = {8, 129, 1, 133, 114, 115, 247, 4, 2, 125, 1, 1, 1, 4, 1, 104, 155, 0}}, + {.name = "schema_request", .type = MessageType{0x02}, .payload = {0, 0}, .frame = {2, 2, 1, 3, 2, 6, 0}}, + {.name = "group_define", + .type = MessageType{0x03}, + .payload = {0, 80, 0, 2, 0, 0, 1, 0}, + .frame = {2, 3, 2, 80, 2, 2, 1, 2, 1, 3, 86, 89, 0}}, + {.name = "credit", .type = MessageType{0x05}, .payload = {0, 1}, .frame = {2, 5, 4, 1, 6, 16, 0}}, + {.name = "write", + .type = MessageType{0x06}, + .payload = {2, 0, 0, 0, 128, 63}, + .frame = {3, 6, 2, 1, 1, 5, 128, 63, 199, 118, 0}}, + {.name = "zeros", + .type = MessageType{0x85}, + .payload = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + .frame = {2, 133, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 3, 133, 221, 0}}, + {.name = "ramp", + // NOLINTNEXTLINE(clang-analyzer-optin.core.EnumCastOutOfRange) the framing carries any type byte + .type = MessageType{0x8B}, + .payload = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, + 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, + 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, + 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, + 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, + 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, + 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, + 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, + 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, + 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, + 190, 191, 192, 193, 194, 195, 196, 197, 198, 199}, + .frame = {2, 139, 202, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, + 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, + 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, + 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, + 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, + 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, + 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, + 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, + 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, + 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, + 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 149, 49, 0}}, + {.name = "high", + .type = MessageType{0x89}, + .payload = {200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, + 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, + 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239}, + .frame = {44, 137, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, + 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, + 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 247, 247, 0}}, + }; +} + +TEST_SUITE("frame") { + TEST_CASE("builds and reads the golden frames of micras-monitor's src/lib/comm/__tests__/frameVectors.ts") { + for (const Vector& vector : monitor_vectors()) { + CAPTURE(vector.name); + + std::vector built(max_frame_size); + built.resize(encode_frame(vector.type, vector.payload, built)); + + CHECK(built == vector.frame); + + FrameReader reader; + bool complete = false; + + for (const uint8_t byte : vector.frame) { + complete = reader.push(byte); + } + + CHECK(complete); + CHECK(reader.type() == vector.type); + CHECK(std::vector(reader.payload().begin(), reader.payload().end()) == vector.payload); + CHECK(reader.discarded() == 0); + } + } + + TEST_CASE("discards a frame that lost a byte and reads the next one") { + const std::vector good = monitor_vectors().at(3).frame; + FrameReader reader; + + for (std::size_t index = 0; index + 3 < good.size(); index++) { + CHECK_FALSE(reader.push(good.at(index))); + } + + CHECK_FALSE(reader.push(0x00)); + CHECK(reader.discarded() == 1); + + bool complete = false; + + for (const uint8_t byte : good) { + complete = reader.push(byte); + } + + CHECK(complete); + CHECK(reader.discarded() == 1); + } +} +} // namespace micras::comm diff --git a/tests/comm/test_link.cpp b/tests/comm/test_link.cpp new file mode 100644 index 0000000..ea88a5a --- /dev/null +++ b/tests/comm/test_link.cpp @@ -0,0 +1,370 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "micras/comm/frame.hpp" +#include "micras/comm/link.hpp" +#include "micras/comm/protocol.hpp" +#include "micras/core/byte_stream.hpp" +#include "micras/core/variable_pool.hpp" + +namespace micras::comm { +namespace { +using Bytes = std::vector; + +class Loopback : public core::IByteStream { +public: + std::size_t read(std::span into) override { + const std::size_t count = std::min(into.size(), this->to_robot.size()); + const auto end = std::next(this->to_robot.begin(), static_cast(count)); + + std::copy(this->to_robot.begin(), end, into.begin()); + this->to_robot.erase(this->to_robot.begin(), end); + + return count; + } + + std::size_t write(std::span from) override { + if (capacity - this->from_robot.size() < from.size()) { + return 0; + } + + this->from_robot.insert(this->from_robot.end(), from.begin(), from.end()); + return from.size(); + } + + std::deque& incoming() { return this->to_robot; } + + std::deque& outgoing() { return this->from_robot; } + +private: + static constexpr std::size_t capacity{4096}; + + std::deque to_robot; + std::deque from_robot; +}; + +class Commands : public ICommandHandler { +public: + static constexpr uint8_t known_command{42}; + + CommandResult handle_command(uint8_t code, uint32_t argument) override { + this->received.emplace_back(code, argument); + return code == known_command ? CommandResult::OK : CommandResult::UNKNOWN; + } + + const std::vector>& seen() const { return this->received; } + +private: + std::vector> received; +}; + +struct Message { + MessageType type; + Bytes payload; +}; +} // namespace + +static float to_float(uint32_t bits) { + return std::bit_cast(bits); +} + +static std::vector of_type(const std::vector& messages, MessageType type) { + std::vector matching; + std::ranges::copy_if(messages, std::back_inserter(matching), [type](const Message& message) { + return message.type == type; + }); + return matching; +} + +namespace { +class Session { +public: + static constexpr uint32_t loop_time_us{125}; + + Session() { + this->pool.add("cmd/", "linear", this->linear, {.stream = true}); + this->pool.add("cmd/", "angular", this->angular, {.stream = true}); + this->pool.add("model/", "gain", this->gain, {.stream = true, .write = true, .idle = true, .persist = true}); + this->pool.add("", "run_profile", this->profile, {.stream = true, .write = true}); + } + + void send(MessageType type, const Bytes& payload) { + Bytes frame(max_frame_size); + frame.resize(encode_frame(type, payload, frame)); + REQUIRE_FALSE(frame.empty()); + this->io.incoming().insert(this->io.incoming().end(), frame.begin(), frame.end()); + } + + void run(int iterations, bool idle = true) { + for (int iteration = 0; iteration < iterations; iteration++) { + this->now += loop_time_us; + this->link.poll(idle); + this->link.pump(this->now); + } + } + + std::vector drain() { + std::vector messages; + const std::size_t taken = this->io.outgoing().size(); + + while (not this->io.outgoing().empty()) { + const uint8_t byte = this->io.outgoing().front(); + this->io.outgoing().pop_front(); + + if (this->application.push(byte)) { + messages.push_back( + {.type = this->application.type(), + .payload = Bytes(this->application.payload().begin(), this->application.payload().end())} + ); + } + } + + if (taken > 0) { + this->send(MessageType::CREDIT, {static_cast(taken), static_cast(taken >> 8U)}); + } + + return messages; + } + + Message only(MessageType type) { + const std::vector matching = of_type(this->drain(), type); + REQUIRE(matching.size() == 1); + return matching.front(); + } + + uint32_t dropped_samples() { + core::TVariablePool<8> probe; + this->link.register_variables(probe, "link/"); + + std::array buffer{}; + const auto id = probe.find("link/dropped_samples"); + + if (not id.has_value()) { + FAIL_CHECK("the link registers no dropped samples counter"); + return 0; + } + + probe.read(*id, buffer); + return Reader{buffer}.u32(); + } + + core::TVariablePool<32> pool; + float linear{1.0F}; + float angular{2.0F}; + float gain{0.5F}; + uint8_t profile{0}; + Commands commands; + Loopback io; + Link link{io, pool, commands, {.loop_time_us = loop_time_us}}; + FrameReader application; + uint32_t now{0}; +}; +} // namespace + +TEST_SUITE("link") { + TEST_CASE_FIXTURE(Session, "answers hello with the version the schema hash and the loop time") { + this->send(MessageType::HELLO, {}); + this->run(4); + + const std::vector messages = this->drain(); + REQUIRE(messages.size() == 1); + REQUIRE(messages.front().type == MessageType::HELLO_ACK); + + Reader reader{messages.front().payload}; + CHECK(reader.u8() == protocol_version); + CHECK(reader.u32() == this->pool.schema_hash()); + CHECK(reader.u16() == 4); + CHECK(reader.u32() == loop_time_us); + } + + TEST_CASE_FIXTURE(Session, "sends the schema in pages") { + this->send(MessageType::SCHEMA_REQUEST, {0, 0}); + this->run(40); + + std::map names; + + for (const Message& message : this->drain()) { + REQUIRE(message.type == MessageType::SCHEMA_PAGE); + + Reader reader{message.payload}; + CHECK(reader.u32() == this->pool.schema_hash()); + + const uint16_t first = reader.u16(); + CHECK(reader.u16() == 4); + + const uint8_t count = reader.u8(); + + for (uint8_t entry = 0; entry < count; entry++) { + reader.u16(); + + const uint8_t length = reader.u8(); + const std::span name = reader.rest().first(length); + names[static_cast(first + entry)] = std::string(name.begin(), name.end()); + + for (uint8_t skipped = 0; skipped < length; skipped++) { + reader.u8(); + } + } + + CHECK(reader.valid()); + } + + CHECK(names.size() == 4); + CHECK(names[0] == "cmd/linear"); + CHECK(names[2] == "model/gain"); + CHECK(names[3] == "run_profile"); + } + + TEST_CASE_FIXTURE(Session, "streams a group at its period with coherent samples") { + this->send(MessageType::GROUP_DEFINE, {0, 4, 0, 2, 0, 0, 1, 0}); + this->send(MessageType::GROUP_ENABLE, {0, 1}); + this->run(4); + + const std::vector acks = of_type(this->drain(), MessageType::GROUP_ACK); + REQUIRE(acks.size() == 2); + + Reader ack{acks.front().payload}; + ack.u8(); + CHECK(ack.u16() == 4); + CHECK(ack.u16() == 8); + + this->linear = 3.5F; + this->angular = -1.25F; + this->drain(); + this->run(16); + + const std::vector samples = this->drain(); + REQUIRE(samples.size() == 4); + + uint16_t first_sequence{}; + + for (std::size_t index = 0; index < samples.size(); index++) { + CAPTURE(index); + REQUIRE(samples.at(index).type == MessageType::SAMPLE); + + Reader reader{samples.at(index).payload}; + CHECK(reader.u8() == 0); + + const uint16_t sequence = reader.u16(); + + if (index == 0) { + first_sequence = sequence; + } + + CHECK(sequence == first_sequence + index); + reader.u32(); + CHECK(to_float(reader.u32()) == 3.5F); + CHECK(to_float(reader.u32()) == -1.25F); + } + } + + TEST_CASE_FIXTURE(Session, "refuses a guarded write while the robot moves and accepts it when idle") { + const Bytes write_gain{2, 0, 0, 0, 0x80, 0x3F}; + + this->send(MessageType::WRITE, write_gain); + this->run(4, false); + + const Message refusal = this->only(MessageType::WRITE_ACK); + Reader refused{refusal.payload}; + refused.u16(); + CHECK(refused.u8() == static_cast(core::VariablePool::WriteStatus::NEEDS_IDLE)); + CHECK(this->gain == 0.5F); + + this->send(MessageType::WRITE, write_gain); + this->run(4, true); + + const Message acceptance = this->only(MessageType::WRITE_ACK); + Reader accepted{acceptance.payload}; + accepted.u16(); + CHECK(accepted.u8() == static_cast(core::VariablePool::WriteStatus::OK)); + CHECK(this->gain == 1.0F); + + this->send(MessageType::READ, {2, 0}); + this->run(4); + + const Message answer = this->only(MessageType::VALUE); + Reader value{answer.payload}; + value.u16(); + CHECK(to_float(value.u32()) == 1.0F); + } + + TEST_CASE_FIXTURE(Session, "acts on a command once") { + this->send(MessageType::COMMAND, {Commands::known_command, 7, 0, 0, 0}); + this->run(8); + + const Message answer = this->only(MessageType::COMMAND_ACK); + Reader ack{answer.payload}; + ack.u8(); + CHECK(ack.u8() == static_cast(CommandResult::OK)); + + REQUIRE(this->commands.seen().size() == 1); + CHECK(this->commands.seen().front().first == Commands::known_command); + CHECK(this->commands.seen().front().second == 7); + } + + TEST_CASE_FIXTURE(Session, "drops the samples a closed credit window cannot carry and counts them") { + this->send(MessageType::HELLO, {}); + this->run(4); + this->drain(); + this->send(MessageType::GROUP_DEFINE, {0, 1, 0, 2, 0, 0, 1, 0}); + this->send(MessageType::GROUP_ENABLE, {0, 1}); + this->run(4); + this->drain(); + + const uint32_t baseline = this->dropped_samples(); + constexpr int iterations{400}; + + this->run(iterations); + + const std::vector sent = of_type(this->drain(), MessageType::SAMPLE); + REQUIRE_FALSE(sent.empty()); + CHECK(sent.size() < 40); + CHECK(this->dropped_samples() - baseline + sent.size() == iterations); + + Reader last{sent.back().payload}; + last.u8(); + const uint16_t last_sent = last.u16(); + + this->send(MessageType::CREDIT, {0x00, 0x01}); + this->run(8); + + const std::vector resumed = of_type(this->drain(), MessageType::SAMPLE); + REQUIRE_FALSE(resumed.empty()); + + Reader first{resumed.front().payload}; + first.u8(); + CHECK(first.u16() > last_sent + 1); + } + + TEST_CASE_FIXTURE(Session, "loses only the frame a corrupted byte hits") { + const std::size_t corrupt_at = this->io.incoming().size() + 2; + + this->send(MessageType::READ, {0, 0}); + this->io.incoming().at(corrupt_at) ^= 0xFFU; + this->send(MessageType::READ, {1, 0}); + this->run(8); + + const std::vector values = of_type(this->drain(), MessageType::VALUE); + REQUIRE(values.size() == 1); + + Reader reader{values.front().payload}; + CHECK(reader.u16() == 1); + } +} +} // namespace micras::comm diff --git a/tests/core/test_cobs.cpp b/tests/core/test_cobs.cpp new file mode 100644 index 0000000..f4bc781 --- /dev/null +++ b/tests/core/test_cobs.cpp @@ -0,0 +1,105 @@ +/** + * @file + */ + +#include +#include +#include + +#include + +#include "micras/core/cobs.hpp" + +namespace micras::core { +namespace { +using Bytes = std::vector; +} // namespace + +static Bytes encode(const Bytes& raw) { + Bytes encoded(cobs_encoded_size(raw.size())); + encoded.resize(cobs_encode(raw, encoded)); + return encoded; +} + +static Bytes decode(const Bytes& encoded) { + Bytes decoded(encoded.size() + 1); + decoded.resize(cobs_decode(encoded, decoded)); + return decoded; +} + +static Bytes count(int first, int last) { + Bytes bytes; + + for (int value = first; value <= last; value++) { + bytes.push_back(static_cast(value)); + } + + return bytes; +} + +static Bytes join(const Bytes& head, const Bytes& tail) { + Bytes joined = head; + joined.insert(joined.end(), tail.begin(), tail.end()); + return joined; +} + +static void check_vector(const Bytes& raw, const Bytes& expected) { + CAPTURE(raw.size()); + + const Bytes encoded = encode(raw); + + CHECK(encoded == expected); + CHECK(decode(encoded) == raw); +} + +TEST_SUITE("cobs") { + TEST_CASE("encodes the reference vectors and decodes them back") { + check_vector({}, {0x01}); + check_vector({0x00}, {0x01, 0x01}); + check_vector({0x00, 0x00}, {0x01, 0x01, 0x01}); + check_vector({0x00, 0x11, 0x00}, {0x01, 0x02, 0x11, 0x01}); + check_vector({0x11, 0x22, 0x00, 0x33}, {0x03, 0x11, 0x22, 0x02, 0x33}); + check_vector({0x11, 0x22, 0x33, 0x44}, {0x05, 0x11, 0x22, 0x33, 0x44}); + check_vector({0x11, 0x00, 0x00, 0x00}, {0x02, 0x11, 0x01, 0x01, 0x01}); + check_vector(count(1, 254), join({0xFF}, count(1, 254))); + check_vector(count(0, 254), join({0x01, 0xFF}, count(1, 254))); + check_vector(count(1, 255), join(join({0xFF}, count(1, 254)), {0x02, 0xFF})); + check_vector(join(count(2, 255), {0x00}), join(join({0xFF}, count(2, 255)), {0x01, 0x01})); + check_vector(join(count(3, 255), {0x00, 0x01}), join(join({0xFE}, count(3, 255)), {0x02, 0x01})); + } + + TEST_CASE("round trips random frames without a delimiter in the encoding") { + // NOLINTNEXTLINE(bugprone-random-generator-seed) a fixed seed keeps the test reproducible + std::mt19937 generator{7}; + + for (int trial = 0; trial < 200000; trial++) { + Bytes raw(generator() % 600); + const uint32_t alphabet = trial % 3 == 0 ? 3 : 256; + + for (uint8_t& byte : raw) { + byte = static_cast(generator() % alphabet); + } + + const Bytes encoded = encode(raw); + bool delimiter_free = true; + + for (const uint8_t byte : encoded) { + delimiter_free = delimiter_free and byte != cobs_delimiter; + } + + REQUIRE(encoded.size() <= cobs_encoded_size(raw.size())); + REQUIRE(delimiter_free); + REQUIRE(decode(encoded) == raw); + } + } + + TEST_CASE("rejects malformed frames instead of decoding half of them") { + Bytes scratch(8); + CHECK(cobs_decode(Bytes{0x00}, scratch) == 0); + CHECK(cobs_decode(Bytes{0x05, 0x01}, scratch) == 0); + + Bytes small(2); + CHECK(cobs_decode(Bytes{0x06, 1, 2, 3, 4, 5}, small) == 0); + } +} +} // namespace micras::core diff --git a/tests/core/test_variable_pool.cpp b/tests/core/test_variable_pool.cpp new file mode 100644 index 0000000..df03139 --- /dev/null +++ b/tests/core/test_variable_pool.cpp @@ -0,0 +1,127 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "micras/core/serializable.hpp" +#include "micras/core/variable_pool.hpp" + +namespace micras::core { +namespace { +class Blob : public ISerializable { +public: + std::vector serialize() const override { return this->data; } + + void deserialize(const uint8_t* buffer, uint16_t size) override { + this->data.assign(buffer, std::next(buffer, size)); + } + +private: + std::vector data{1, 2, 3}; +}; + +enum class Profile : uint8_t { + A, + B +}; +} // namespace + +static std::span bytes_of(const float& value) { + return {std::bit_cast(&value), sizeof(value)}; +} + +TEST_SUITE("variable_pool") { + TEST_CASE("registers finds reads and writes variables with their access") { + TVariablePool<8> pool; + float speed = 1.5F; + const float gain = 2.0F; + Profile profile = Profile::A; + Blob blob; + + const auto speed_id = pool.add("speed/", "linear", speed, {.stream = true, .write = true}); + const auto gain_id = + pool.add("model/", "kv", gain, {.stream = true, .write = true, .idle = true, .persist = true}); + const auto profile_id = pool.add("", "run_profile", profile, {.write = true, .idle = true, .persist = true}); + const auto blob_id = pool.add("", "maze", blob, {.persist = true}); + + CHECK(speed_id == 0); + CHECK(gain_id == 1); + CHECK(profile_id == 2); + CHECK(blob_id == 3); + CHECK(pool.all().size() == 4); + CHECK_FALSE(pool.at(gain_id).access.write); + CHECK(pool.at(profile_id).type == TypeCode::U8); + CHECK(pool.at(blob_id).type == TypeCode::BLOB); + CHECK_FALSE(pool.at(blob_id).access.stream); + + CHECK(pool.find("speed/linear") == speed_id); + CHECK(pool.find("model/kv") == gain_id); + CHECK(pool.find("run_profile") == profile_id); + CHECK_FALSE(pool.find("speed/linea").has_value()); + CHECK_FALSE(pool.find("speed/linearr").has_value()); + + std::array buffer{}; + CHECK(pool.read(speed_id, buffer) == sizeof(float)); + + float read_back{}; + std::memcpy(&read_back, buffer.data(), sizeof(read_back)); + CHECK(read_back == 1.5F); + CHECK(pool.read(blob_id, buffer) == 0); + + const float written = 9.0F; + const std::array one{1}; + + CHECK(pool.write(speed_id, bytes_of(written), false) == VariablePool::WriteStatus::OK); + CHECK(speed == 9.0F); + CHECK(pool.write(gain_id, bytes_of(written), true) == VariablePool::WriteStatus::READ_ONLY); + CHECK(pool.write(profile_id, one, false) == VariablePool::WriteStatus::NEEDS_IDLE); + CHECK(pool.write(profile_id, one, true) == VariablePool::WriteStatus::OK); + CHECK(profile == Profile::B); + CHECK(pool.write(speed_id, one, true) == VariablePool::WriteStatus::WRONG_SIZE); + CHECK(pool.write(99, one, true) == VariablePool::WriteStatus::NO_SUCH_ID); + } + + TEST_CASE("hashes the schema by the names types and access") { + float speed = 1.5F; + const float gain = 2.0F; + Profile profile = Profile::A; + Blob blob; + + const auto fill = [&](VariablePool& pool) { + pool.add("speed/", "linear", speed, {.stream = true, .write = true}); + pool.add("model/", "kv", gain, {.stream = true, .write = true, .idle = true, .persist = true}); + pool.add("", "run_profile", profile, {.write = true, .idle = true, .persist = true}); + pool.add("", "maze", blob, {.persist = true}); + }; + + TVariablePool<8> first; + TVariablePool<8> second; + fill(first); + fill(second); + + CHECK(second.schema_hash() == first.schema_hash()); + + second.add("", "extra", speed, {}); + CHECK(second.schema_hash() != first.schema_hash()); + } + + TEST_CASE("refuses a variable once the pool is full") { + TVariablePool<1> pool; + float value = 0.0F; + + pool.add("", "a", value, {}); + + CHECK(pool.add("", "b", value, {}) == VariablePool::invalid_id); + CHECK(pool.all().size() == 1); + } +} +} // namespace micras::core diff --git a/tests/data/mazes/alljapan-033-2012-exp-fin.txt b/tests/data/mazes/alljapan-033-2012-exp-fin.txt new file mode 100644 index 0000000..6b41392 --- /dev/null +++ b/tests/data/mazes/alljapan-033-2012-exp-fin.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | | | | +o o o o---o---o---o o---o---o---o---o o o o o o +| | | | | | | | | | | +o o o o o o o---o o---o---o---o---o o o o o +| | | | | | | | | +o o---o---o o o---o---o---o---o---o---o o o---o---o o +| | | | | | +o o---o---o---o o o---o---o---o---o---o o---o---o o o +| | | | | | | | +o o o---o o---o o o o---o o o---o o---o o o +| | | | | | | | | | | | +o o---o o---o o o o o o o---o o o o---o o +| | | | | | | | | | +o o o---o---o o o o o---o o o---o o---o o o +| | | | | G G | | | | | +o o---o o---o o o o o o o o o---o o---o o +| | | | | G G | | | | | +o o o---o o o o o o---o o o---o o---o---o o +| | | | | | | | | | +o o---o o---o o o o o o---o---o o---o---o o o +| | | | | | | | +o---o---o---o---o o o o o o o---o o---o o o o +| | | | | | | | | +o o---o---o---o o o o o o o---o---o o---o o o +| | | | | | | | | +o---o---o o---o o---o---o o---o o o o o o---o o +| | | | | | | | | +o o o o o o---o o---o---o---o o o o o---o---o +| | | | | | | | | +o o o o o o---o---o---o---o---o---o---o---o---o---o o +| S | | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/apec2016.txt b/tests/data/mazes/apec2016.txt new file mode 100644 index 0000000..0b171b8 --- /dev/null +++ b/tests/data/mazes/apec2016.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | | | | | | +o o o o---o o---o o o o o o---o---o---o o o +| | | | | | | | | | | | | +o o o o o---o---o---o o o o o---o o o---o o +| | | | | | | | | | +o o o o o---o---o---o o o o o---o o---o o o +| | | | | | | | | | | | | | +o o o o o o o o---o o---o---o o---o o o o +| | | | | | | | | | +o o o o---o---o o o---o---o---o o---o---o o o o +| | | | | | G G | | | | | | +o o o o o o---o o o o o---o---o o o o o +| | | | | | | G G | | | | +o o o o o---o o o---o---o---o---o o o o o o +| | | | | | | | | | | | | +o o o o o o---o---o---o---o o o o o---o---o o +| | | | | | | | | | | +o o o o o o o o---o o---o o o---o---o o o +| | | | | | | | | | | +o o o o---o---o o---o o---o o o---o---o o o o +| | | | | | | | | | +o o---o o o---o---o o---o o---o o o o o o o +| | | | | | | | +o o o o---o---o---o---o---o---o---o---o---o---o---o o o +| | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/apec2017.txt b/tests/data/mazes/apec2017.txt new file mode 100644 index 0000000..b03869e --- /dev/null +++ b/tests/data/mazes/apec2017.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o o o +| | | | | | | | +o o o---o---o o o o o---o---o o o---o o o o +| | | | | | | | | | | | +o o o o o---o o---o---o o o o---o o o o o +| | | | | | | | | | | | | | +o o o o---o o---o o o---o o o o---o o o o +| | | | | | | | | | | | | +o o o o o---o o o---o o o o---o o o o o +| | | | | | | | | | | | +o o o o o o o---o---o---o---o---o---o o o o o +| | | | | G G | | | | | +o o o---o---o o---o o o o o---o o o o o o +| | | | G G | | | | | | +o o o o---o---o---o---o---o o---o o o---o o o o +| | | | | | | | | | +o o o---o o o---o o---o---o o o---o o---o o o +| | | | | | | | | | | | +o o o o---o o o---o---o o o---o o---o o o o +| | | | | | | | | | | | | +o o o---o o o---o o o---o---o o---o o o o o +| | | | | | | | | | | +o o---o o o---o o o---o o o---o o---o o o o +| | | | | | | | | +o o o o---o---o---o---o---o---o---o---o---o---o o o o +| | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/apec2018.txt b/tests/data/mazes/apec2018.txt new file mode 100644 index 0000000..f38c3ef --- /dev/null +++ b/tests/data/mazes/apec2018.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | | +o o o o o---o---o o---o---o---o---o---o---o---o---o o +| | | | | | | | | +o o o o---o---o o---o---o---o o o o o o o o +| | | | | | | | | | | +o o o o---o o---o o---o---o---o o o o o---o o +| | | | | | | | | | +o o o o o o---o---o---o---o---o o o o---o o o +| | | | | | | | | | | | | +o o o o o---o o o---o---o o o o o o---o o +| | | | | | | G G | | | | | | | +o o o o---o o o o o o o o o o---o o o +| | | | | G G | | | | | | +o o o o o---o o o---o---o o o o---o o o o +| | | | | | | | | | | +o o o o---o o o---o---o o---o---o o o---o o o +| | | | | | | | | | | | | +o o o o o o---o o o---o o o---o---o o---o o +| | | | | | | | | | | | +o o o o o---o o o---o o---o o---o---o---o o o +| | | | | | | | | | +o o o o---o o---o---o---o---o---o o o o o o o +| | | | | | | +o o o o---o---o---o---o---o---o---o---o---o---o---o o o +| | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/apec2019.txt b/tests/data/mazes/apec2019.txt new file mode 100644 index 0000000..02cf47b --- /dev/null +++ b/tests/data/mazes/apec2019.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | | | | | | +o o o o---o o---o o---o o---o o---o o---o o o +| | | | | | | | | | | +o o o o o---o o---o o---o o---o o---o o o o +| | | | | | | | | | | +o o o o---o o---o o---o o---o o---o---o---o o o +| | | | | | | | +o o o o---o---o---o---o---o---o---o---o o o---o o o +| | | | | | | | | +o o o o o o o---o---o---o---o---o---o---o o---o o +| | | | | | G G | | | +o o o o o o---o---o o o o---o---o---o---o o o +| | | | | | G G | | | | +o o o o o---o---o o---o---o---o---o o o---o---o o +| | | | | | | | | +o o o o---o o o---o---o---o o o---o---o---o o o +| | | | | | | | | | +o o o o o---o o o---o o---o---o o---o---o o o +| | | | | | | | | | +o o o o---o---o o---o o---o---o o---o---o o o o +| | | | | | | | | | | +o o o o o---o---o---o o o---o---o o o o o o +| | | | | | | | +o o o o---o---o---o---o---o---o---o---o---o---o---o o o +| | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/japan2013ef.txt b/tests/data/mazes/japan2013ef.txt new file mode 100644 index 0000000..05d8df4 --- /dev/null +++ b/tests/data/mazes/japan2013ef.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | | | +o o---o o---o---o---o---o---o---o---o---o---o o o o o +| | | | | | | +o o---o---o o o---o---o---o---o---o---o o---o o o o +| | | | | | | | +o o---o o---o o o---o o---o o---o---o o---o---o o +| | | | | | | | | +o o---o o---o---o o---o o---o o o o o---o o o +| | | | | | | | | | +o o---o o---o o---o---o o---o---o---o o o---o o o +| | | | | | | | | | +o o o---o o---o o---o o---o o o o---o o o o +| | | | | | | | | | +o o o---o---o o o---o o---o---o o---o o o o o +| | | | | G G | | | | | | | +o o---o---o o o---o o o o o---o o o o o o +| | | | | G G | | | | | | +o o o---o---o o o---o---o---o---o o---o o o o o +| | | | | | | | | | +o o---o---o o---o---o o o o o---o o---o o o o +| | | | | | | | | | | +o o o---o---o o o---o---o---o---o o o---o o o o +| | | | | | | | | +o o o---o---o---o o o o---o o---o o o---o o o +| | | | | | | | | | | +o o o o o o---o---o---o---o o o---o o o o o +| | | | | | | | | | +o o o---o---o o o---o o o---o---o o o o---o o +| | | | | | | | | | +o o o o o---o---o---o---o---o---o---o---o---o o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/japan2017ef.txt b/tests/data/mazes/japan2017ef.txt new file mode 100644 index 0000000..42b70e0 --- /dev/null +++ b/tests/data/mazes/japan2017ef.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | | +o o o o---o---o---o---o---o---o---o---o---o---o o o---o +| | | | | | | | +o o---o o o---o---o o o---o---o o o o---o---o o +| | | | | | | | | +o o---o o o o o---o---o o o---o---o---o---o o o +| | | | | | | +o o---o---o---o---o---o---o---o---o---o---o---o o o---o o +| | | | | | | | | +o o o o o o o o o o o o o---o---o o o +| | | | | | | | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o o---o o +| | | | | | | | +o o---o o o o---o o o---o o o o---o o---o o +| | | | | | G G | | | | +o o o---o o---o o o o o---o---o---o---o o---o o +| | | | | | G G | | | | +o o o o---o o---o o---o---o o o o---o---o---o o +| | | | | | | +o o---o---o o---o---o---o---o---o---o---o---o---o---o o o +| | | | | | | | | | +o o o---o---o o o o o o o o o---o o---o o +| | | | | | | | | | +o o---o o o---o---o o---o---o o---o o o---o o o +| | | | | | | | | | | +o o o---o---o o o---o o o---o o---o o---o---o o +| | | | | | | | | +o o o o---o---o o---o---o---o---o---o o o o o o +| | | | | | | | | +o o---o o o o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/maze1.txt b/tests/data/mazes/maze1.txt new file mode 100644 index 0000000..85890fb --- /dev/null +++ b/tests/data/mazes/maze1.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | | +o o---o---o---o o---o o---o---o---o---o o---o o o o +| | | | | | | +o o---o---o o---o---o o o o---o o o o---o---o o +| | | | | | | +o o---o o---o---o---o o o---o---o o o---o o o---o +| | | | | +o o---o---o---o---o---o o---o---o---o o o---o---o---o o +| | | | | +o o---o---o---o---o o---o o---o o o o---o---o o o +| | | | | | | | +o o o---o---o---o---o o---o o---o---o o o---o o o +| | | | | | | | | +o o o o---o---o---o o---o---o---o o o o---o o o +| | | | | G G | | | | | | +o o o---o---o---o o o o o o o o---o---o o o +| | | | G G | | | | +o o---o---o---o---o---o o---o---o o o o---o o---o o +| | | | | +o o---o---o---o o o---o---o---o---o---o---o o o o o +| | | | | | | | | | +o o o---o---o o o o o o---o---o o o o o o +| | | | | | | | | | +o---o---o---o o o o o o---o---o---o o o o o o +| | | | | | | | | | +o o---o---o o o o o o o---o o o o o o o +| | | | | | | | | +o o o---o---o o o---o o---o---o---o o o o o o +| | | | | | | | | | +o o---o---o---o o o o o---o---o o o o o o o +| S | | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/maze2.txt b/tests/data/mazes/maze2.txt new file mode 100644 index 0000000..ef6b2a5 --- /dev/null +++ b/tests/data/mazes/maze2.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o o---o o o---o o---o---o o---o o---o o o +| | | | | | | | | | | | | | | +o o---o o o o o---o o o o o---o o o---o o +| | | | | | | | | | | | +o o---o o---o o o o o o o o o o---o o o +| | | | | +o o---o---o---o---o---o---o---o o o o o o---o o o +| | | | | | | | +o o o---o---o---o---o---o o---o---o---o---o---o o---o---o +| | | | | | | +o o o---o---o---o---o---o o o o o o o---o o o +| | | | | | | | | +o o---o---o---o---o---o o---o---o---o---o---o---o o o o +| | | G G | | | | | | | | +o o o---o---o---o---o---o o o o o o o o o o +| | | G G | | | +o o---o---o---o---o---o o---o---o o o o o o o o +| | | | | | | | | +o o o---o---o---o---o---o---o---o o---o---o---o---o o o +| | | | | | | +o o o o---o o---o o---o o---o o---o o---o o o +| | | | | | | | | | +o o o---o o---o o---o o---o o---o o---o o---o o +| | | | | | | | | | | +o o o o---o o---o o---o o---o o---o o---o o o +| | | | | | | | | | | +o o---o o o---o o---o o---o o---o o---o---o o o +| | | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/uk2016f.txt b/tests/data/mazes/uk2016f.txt new file mode 100644 index 0000000..e1123b0 --- /dev/null +++ b/tests/data/mazes/uk2016f.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o o---o---o---o o---o---o---o---o o +| | | | +o---o o o o---o---o---o---o---o o---o o---o o o o +| | | | | | | +o o o o---o o---o---o o---o---o---o o o o o o +| | | | | | | | | +o o o o o o o---o---o---o---o---o o o o o o +| | | | | | | | +o o---o---o o---o o o---o---o---o o---o o o o o +| | | | | | | | | | +o o o o---o o o o---o o o o o o o o o +| | | | | | | | | | | | | +o o o---o o o o o---o---o o o o o---o o o +| | | | | G G | | | | | | | +o o---o o---o---o---o o o o o o o o o---o o +| | | | G G | | | | | +o o o o o---o---o o---o---o---o o o o o o---o +| | | | | | | | | +o o o o---o---o---o---o o---o---o---o o o o o o +| | | | | | | | | +o o o o o---o---o o---o---o---o o o o o o o +| | | | | | | | | | +o o---o o o o o---o---o o---o---o o o o o o +| | | | | | | | +o o o---o o o---o o---o---o---o---o---o o o o o +| | | | | | | +o o---o o o---o---o---o---o---o o o---o---o---o o---o +| | | | | | +o o o---o---o o---o---o---o o---o---o o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/hal/test_adc.cpp b/tests/hal/test_adc.cpp new file mode 100644 index 0000000..d9111d4 --- /dev/null +++ b/tests/hal/test_adc.cpp @@ -0,0 +1,74 @@ +/** + * @file + */ + +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/adc_dma.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" + +namespace micras::test { +TEST_SUITE("adc") { + TEST_CASE_FIXTURE(HostBoard, "copies a finished sequence into the snapshot") { + hal::AdcDma adc{adc_config}; + std::array buffer{}; + std::array snapshot{}; + std::array read{}; + + REQUIRE(adc.was_initialized()); + REQUIRE(adc.start_dma(buffer, snapshot)); + + const hal::host::AdcPort& port = hal::host::Board::adc(adc_config.handle); + CHECK(port.size() == 2); + + port.write(0, 1500); + port.write(1, 900); + port.write(2, 100); + CHECK(adc.read_snapshot(read) == 0); + CHECK(read == std::array{0, 0}); + + port.finish_sequence(); + CHECK(adc.read_snapshot(read) == 1); + CHECK(read == std::array{1500, 900}); + CHECK(adc.get_max_reading() == 4095); + CHECK(static_cast(adc.get_reference_voltage()) == doctest::Approx(3.3)); + } + + TEST_CASE_FIXTURE(HostBoard, "stops updating after an error until it recovers") { + hal::AdcDma adc{adc_config}; + std::array buffer{}; + std::array snapshot{}; + std::array read{}; + REQUIRE(adc.start_dma(buffer, snapshot)); + + const hal::host::AdcPort& port = hal::host::Board::adc(adc_config.handle); + const uint32_t restarts = hal::AdcDma::get_restarts(); + + hal::AdcDma::on_error(adc_config.handle); + port.write(0, 7); + port.finish_sequence(); + CHECK(adc.read_snapshot(read) == 0); + + adc.recover(); + CHECK(hal::AdcDma::get_restarts() == restarts + 1); + + port.finish_sequence(); + CHECK(adc.read_snapshot(read) == 1); + CHECK(read.front() == 7); + } + + TEST_CASE_FIXTURE(HostBoard, "refuses a snapshot of another size than the buffer") { + hal::AdcDma adc{adc_config}; + std::array buffer{}; + std::array snapshot{}; + + CHECK_FALSE(adc.start_dma(buffer, snapshot)); + CHECK_FALSE(adc.was_initialized()); + } +} +} // namespace micras::test diff --git a/tests/hal/test_board.cpp b/tests/hal/test_board.cpp new file mode 100644 index 0000000..ee4b6bc --- /dev/null +++ b/tests/hal/test_board.cpp @@ -0,0 +1,59 @@ +/** + * @file + */ + +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/gpio.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" + +namespace micras::test { +TEST_SUITE("board") { + TEST_CASE_FIXTURE(HostBoard, "reads what the outside drives and otherwise what the firmware wrote") { + hal::Gpio led{status_led_config}; + led.write(true); + + hal::host::GpioPort& port = hal::host::Board::gpio(status_led_config.port, status_led_config.pin); + CHECK(port.output); + CHECK(led.read()); + + port.input = false; + CHECK_FALSE(led.read()); + CHECK(port.touched); + + led.toggle(); + CHECK_FALSE(port.output); + } + + TEST_CASE_FIXTURE(HostBoard, "names a port after the label of its pin") { + const hal::Gpio button{button_config}; + static_cast(button.read()); + + CHECK(hal::host::Board::gpio(button_config.port, button_config.pin).name == "Button"); + } + + TEST_CASE_FIXTURE(HostBoard, "reports the ports the firmware touched that nothing is bound to") { + hal::Gpio led{status_led_config}; + led.write(true); + + CHECK(hal::host::Board::unbound() == std::vector{"Status_LED"}); + + hal::host::Board::gpio(status_led_config.port, status_led_config.pin).bound = true; + CHECK(hal::host::Board::unbound().empty()); + } + + TEST_CASE_FIXTURE(HostBoard, "forgets every port on a reset") { + hal::Gpio led{status_led_config}; + led.write(true); + hal::host::Board::reset(); + + CHECK(hal::host::Board::unbound().empty()); + CHECK_FALSE(hal::host::Board::gpio(status_led_config.port, status_led_config.pin).output); + } +} +} // namespace micras::test diff --git a/tests/hal/test_clock.cpp b/tests/hal/test_clock.cpp new file mode 100644 index 0000000..af87543 --- /dev/null +++ b/tests/hal/test_clock.cpp @@ -0,0 +1,49 @@ +/** + * @file + */ + +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/host/clock.hpp" +#include "micras/hal/timer.hpp" + +namespace micras::test { +TEST_SUITE("clock") { + TEST_CASE_FIXTURE(HostBoard, "charges a quantum per read and hands over at every step") { + int steps = 0; + hal::host::Clock::instance().set_handover(125, [&steps] { steps++; }); + + const uint32_t start = hal::Timer::get_counter(); + + while (hal::Timer::to_microseconds(hal::Timer::get_counter() - start) < 1000) { } + + CHECK(steps == 8); + CHECK(hal::host::Clock::instance().steps() == 8); + CHECK(hal::Timer::get_counter_ms() == 1); + } + + TEST_CASE_FIXTURE(HostBoard, "counts cycles at the core clock and converts them") { + const uint32_t cycles_per_microsecond = SystemCoreClock / 1000000; + + CHECK(hal::Timer::to_cycles(10) == 10 * cycles_per_microsecond); + CHECK(hal::Timer::to_microseconds(10 * cycles_per_microsecond) == 10); + + const uint32_t first = hal::Timer::get_counter(); + const uint32_t second = hal::Timer::get_counter(); + CHECK(second - first == cycles_per_microsecond); + } + + TEST_CASE_FIXTURE(HostBoard, "stops handing over once cleared") { + int steps = 0; + hal::host::Clock::instance().set_handover(1, [&steps] { steps++; }); + hal::Timer::get_counter(); + hal::host::Clock::instance().clear_handover(); + wait_us(10); + + CHECK(steps == 1); + } +} +} // namespace micras::test diff --git a/tests/hal/test_crc.cpp b/tests/hal/test_crc.cpp new file mode 100644 index 0000000..08f05d8 --- /dev/null +++ b/tests/hal/test_crc.cpp @@ -0,0 +1,81 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/crc.hpp" + +namespace micras::test { +static uint8_t reference_crc8(std::span data, uint8_t polynomial, uint8_t initial) { + uint8_t crc = initial; + + for (const uint8_t byte : data) { + crc ^= byte; + + for (int bit = 0; bit < 8; bit++) { + crc = (crc & 0x80U) != 0 ? static_cast((crc << 1U) ^ polynomial) : static_cast(crc << 1U); + } + } + + return crc; +} + +TEST_SUITE("crc") { + TEST_CASE_FIXTURE(HostBoard, "computes the check value of CRC-8/SAE-J1850") { + CRC_HandleTypeDef handle{}; + handle.Init = { + .DefaultPolynomialUse = DEFAULT_POLYNOMIAL_DISABLE, + .DefaultInitValueUse = DEFAULT_INIT_VALUE_DISABLE, + .GeneratingPolynomial = 0x1D, + .CRCLength = CRC_POLYLENGTH_8B, + .InitValue = 0xFF, + .InputDataInversionMode = CRC_INPUTDATA_INVERSION_NONE, + .OutputDataInversionMode = CRC_OUTPUTDATA_INVERSION_DISABLE, + }; + hal::Crc crc{{.handle = &handle}}; + + constexpr std::string_view check{"123456789"}; + const std::vector bytes(check.begin(), check.end()); + + CHECK((crc.calculate(bytes) ^ 0xFFU) == 0x4B); + } + + TEST_CASE_FIXTURE(HostBoard, "computes what the unit is configured for") { + hal::Crc crc{crc_config}; + + for (const std::array bytes : std::vector>{ + {0x00, 0x00}, {0x40, 0x01}, {0x40, 0x1A}, {0x00, 0x80}, {0x3F, 0xFF}, {0xC0, 0x15} + }) { + CAPTURE(bytes); + CHECK(crc.calculate(bytes) == reference_crc8(bytes, 0x1D, 0xC4)); + } + } + + TEST_CASE_FIXTURE(HostBoard, "computes the standard CRC-32 with the default polynomial") { + CRC_HandleTypeDef handle{}; + handle.Init = { + .DefaultPolynomialUse = DEFAULT_POLYNOMIAL_ENABLE, + .DefaultInitValueUse = DEFAULT_INIT_VALUE_ENABLE, + .GeneratingPolynomial = 0, + .CRCLength = CRC_POLYLENGTH_32B, + .InitValue = 0, + .InputDataInversionMode = CRC_INPUTDATA_INVERSION_BYTE, + .OutputDataInversionMode = CRC_OUTPUTDATA_INVERSION_ENABLE, + }; + hal::Crc crc{{.handle = &handle}}; + + constexpr std::string_view check{"123456789"}; + const std::vector bytes(check.begin(), check.end()); + + CHECK((crc.calculate(bytes) ^ 0xFFFFFFFFU) == 0xCBF43926U); + } +} +} // namespace micras::test diff --git a/tests/hal/test_family.cpp b/tests/hal/test_family.cpp new file mode 100644 index 0000000..4b18b7b --- /dev/null +++ b/tests/hal/test_family.cpp @@ -0,0 +1,55 @@ +/** + * @file + */ + +#include + +#include "host_fixture.hpp" +#include "micras/hal/family.hpp" +#include "micras/hal/family/flash.hpp" +#include "micras/hal/flash.hpp" +#include "tim.h" + +namespace micras::test { +TEST_SUITE("family") { + TEST_CASE("reads the flash geometry from the board's macros") { + CHECK(hal::family::flash_word_bits == 256); + CHECK(hal::family::sector_size == 128 * 1024); + CHECK(hal::family::storage_first_sector == 4); + CHECK(hal::family::storage_sectors == 4); + + CHECK(hal::FlashWord::words == 8); + CHECK(hal::FlashWord::size == 32); + CHECK(hal::Flash::total_size == hal::family::storage_sectors * hal::family::sector_size); + } + + TEST_CASE_FIXTURE(HostBoard, "takes a timer's clock from its kernel clock") { + MX_TIM15_Init(); + + CHECK(hal::family::timer_clock(htim15.Instance) == htim15.Instance->kernel_clock); + CHECK(hal::family::timer_clock(htim15.Instance) == 200000000); + } + + TEST_CASE("has nothing to calibrate, cache or unlock on a host") { + ADC_HandleTypeDef adc{}; + + CHECK(hal::family::calibrate_adc(&adc)); + CHECK_FALSE(hal::family::was_reset_by_watchdog()); + CHECK(hal::family::is_cpu_frequency_supported(true)); + CHECK(hal::family::is_cpu_frequency_supported(false)); + CHECK(hal::family::watchdog() != nullptr); + CHECK(hal::family::watchdog() == hal::family::watchdog()); + + hal::family::enable_caches(); + hal::family::freeze_watchdog_in_debug(); + hal::family::enable_cycle_counter(); + } + + TEST_CASE("never stops or ends a transfer on a host") { + const DMA_HandleTypeDef dma{}; + + CHECK(hal::family::is_dma_enabled(&dma)); + CHECK_FALSE(hal::family::has_dma_finished(&dma)); + } +} +} // namespace micras::test diff --git a/tests/hal/test_flash.cpp b/tests/hal/test_flash.cpp new file mode 100644 index 0000000..3d17cab --- /dev/null +++ b/tests/hal/test_flash.cpp @@ -0,0 +1,71 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/flash.hpp" +#include "micras/hal/host/board.hpp" + +namespace micras::test { +namespace { +constexpr std::array data{1, 2, 3, 4, 5}; +} // namespace + +static bool is_erased(std::span bytes) { + return std::ranges::all_of(bytes, [](uint8_t byte) { return byte == hal::FlashWord::erased_value; }); +} + +TEST_SUITE("flash") { + TEST_CASE_FIXTURE(HostBoard, "reserves the upper half of the sectors") { + CHECK(hal::Flash::sector_size == 128 * 1024); + CHECK(hal::Flash::total_sectors == 4); + CHECK(hal::FlashWord::size == 32); + CHECK(is_erased(hal::Flash::read(0, hal::Flash::total_size))); + } + + TEST_CASE_FIXTURE(HostBoard, "writes a flash word once and reads it back padded") { + REQUIRE(hal::Flash::write(1, 0, data) == hal::Flash::Status::OK); + + const std::span word = hal::Flash::read(1, 0, hal::FlashWord::size); + CHECK(std::ranges::equal(word.first(data.size()), data)); + CHECK(is_erased(word.subspan(data.size()))); + CHECK(hal::host::Board::flash().touched); + + CHECK(hal::Flash::write(1, 0, data) == hal::Flash::Status::ERROR); + } + + TEST_CASE_FIXTURE(HostBoard, "writes again after the sector is erased") { + REQUIRE(hal::Flash::write(0, 0, data) == hal::Flash::Status::OK); + REQUIRE(hal::Flash::erase_sectors(0) == hal::Flash::Status::OK); + + CHECK(is_erased(hal::Flash::read(0, 0, hal::FlashWord::size))); + CHECK(hal::Flash::write(0, 0, data) == hal::Flash::Status::OK); + } + + TEST_CASE_FIXTURE(HostBoard, "refuses misaligned and out of bounds accesses") { + CHECK(hal::Flash::write(0, 4, data) == hal::Flash::Status::MISALIGNED); + CHECK(hal::Flash::write(hal::Flash::total_sectors, 0, data) == hal::Flash::Status::OUT_OF_BOUNDS); + CHECK(hal::Flash::write(hal::Flash::total_size, data) == hal::Flash::Status::OUT_OF_BOUNDS); + CHECK(hal::Flash::erase_sectors(3, 2) == hal::Flash::Status::OUT_OF_BOUNDS); + CHECK(hal::Flash::read(hal::Flash::total_size - 1, 2).empty()); + } + + TEST_CASE_FIXTURE(HostBoard, "stops a write where the simulated power loss cuts it off") { + std::array(3 * hal::FlashWord::size)> long_data{}; + long_data.fill(0x42); + hal::host::Board::flash().write_budget = 2; + + CHECK(hal::Flash::write(0, 0, long_data) == hal::Flash::Status::ERROR); + CHECK_FALSE(is_erased(hal::Flash::read(0, 0, 2 * hal::FlashWord::size))); + CHECK(is_erased(hal::Flash::read(0, 2 * hal::FlashWord::size, hal::FlashWord::size))); + } +} +} // namespace micras::test diff --git a/tests/hal/test_fmac.cpp b/tests/hal/test_fmac.cpp new file mode 100644 index 0000000..a9bb93f --- /dev/null +++ b/tests/hal/test_fmac.cpp @@ -0,0 +1,98 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/fmac.hpp" + +namespace micras::test { +namespace { +constexpr double q15_scale{32768.0}; + +constexpr std::array feed_forward{2048, 4096, 2048}; + +constexpr std::array feedback{29491, -13107}; +} // namespace + +static std::vector direct_form(const std::vector& inputs) { + std::vector outputs; + std::array input_history{}; + std::array output_history{}; + + for (const double input : inputs) { + input_history = {input, input_history.at(0), input_history.at(1)}; + + double output = 0.0; + + for (std::size_t tap = 0; tap < feed_forward.size(); tap++) { + output += feed_forward.at(tap) / q15_scale * input_history.at(tap); + } + + for (std::size_t tap = 0; tap < feedback.size(); tap++) { + output += feedback.at(tap) / q15_scale * output_history.at(tap); + } + + output_history = {output, output_history.at(0)}; + outputs.push_back(output); + } + + return outputs; +} + +TEST_SUITE("fmac") { + TEST_CASE_FIXTURE(HostBoard, "runs the configured filter as a direct form reference does") { + hal::Fmac fmac{fmac_config}; + REQUIRE(fmac.configure_iir(feed_forward, feedback)); + CHECK(fmac.was_initialized()); + + std::vector inputs; + + inputs.reserve(200); + for (int sample = 0; sample < 200; sample++) { + inputs.push_back(0.5 * std::sin(0.1 * sample) + (sample > 100 ? 0.25 : 0.0)); + } + + const std::vector expected = direct_form(inputs); + + for (std::size_t sample = 0; sample < inputs.size(); sample++) { + CAPTURE(sample); + const auto input = static_cast(std::lround(inputs.at(sample) * q15_scale)); + const double output = fmac.update(input) / q15_scale; + + CHECK(std::abs(output - expected.at(sample)) < 1e-3); + } + } + + TEST_CASE_FIXTURE(HostBoard, "clips the output to the q1.15 range") { + hal::Fmac fmac{fmac_config}; + constexpr std::array gain{32767}; + constexpr std::array integrator{32767}; + REQUIRE(fmac.configure_iir(gain, integrator)); + + int16_t output = 0; + + for (int sample = 0; sample < 10; sample++) { + output = fmac.update(30000); + } + + CHECK(output == 32767); + } + + TEST_CASE_FIXTURE(HostBoard, "refuses a filter that does not fit its buffers") { + hal::Fmac fmac{fmac_config}; + constexpr std::array too_many{}; + + CHECK_FALSE(fmac.configure_iir(too_many, too_many)); + CHECK_FALSE(fmac.configure_iir({}, feedback)); + CHECK_FALSE(fmac.was_initialized()); + } +} +} // namespace micras::test diff --git a/tests/hal/test_mcu.cpp b/tests/hal/test_mcu.cpp new file mode 100644 index 0000000..6cb785d --- /dev/null +++ b/tests/hal/test_mcu.cpp @@ -0,0 +1,65 @@ +/** + * @file + */ + +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/gpio.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/mcu.hpp" +#include "micras/hal/pwm.hpp" +#include "tim.h" +#include "usart.h" + +extern "C" { +void SystemClock_Config(); +} + +namespace micras::test { +TEST_SUITE("mcu") { + TEST_CASE_FIXTURE(HostBoard, "runs the clock and peripheral init functions it is given") { + constexpr std::array inits{MX_TIM15_Init, MX_UART4_Init}; + + hal::Mcu::init({ + .clock_init = SystemClock_Config, + .peripheral_clock_init = nullptr, + .peripheral_inits = inits, + .cpu_frequency_boost = false, + }); + + CHECK(htim15.State == HAL_TIM_STATE_READY); + CHECK(huart4.gState == HAL_UART_STATE_READY); + CHECK(hal::Mcu::is_cpu_frequency_supported()); + CHECK_FALSE(hal::Mcu::was_reset_by_watchdog()); + } + + TEST_CASE_FIXTURE(HostBoard, "counts a watchdog expiry when a refresh comes late") { + hal::Mcu::set_watchdog_timeout(10); + wait_us(5000); + hal::Mcu::refresh_watchdog(); + CHECK(hal::host::Board::mcu().watchdog_expiries == 0); + + wait_us(11000); + hal::Mcu::refresh_watchdog(); + CHECK(hal::host::Board::mcu().watchdog_expiries == 1); + } + + TEST_CASE_FIXTURE(HostBoard, "stops the outputs in an emergency") { + hal::Pwm pwm{pwm_config}; + hal::Gpio enable{status_led_config}; + pwm.set_duty_cycle(80.0F); + enable.write(true); + + const std::array outputs{pwm_config}; + const std::array enables{status_led_config}; + hal::Mcu::emergency_stop(outputs, enables); + + CHECK(hal::host::Board::pwm(pwm_config.handle, pwm_config.timer_channel).duty_cycle == 0.0F); + CHECK_FALSE(hal::host::Board::gpio(status_led_config.port, status_led_config.pin).output); + CHECK(hal::host::Board::mcu().emergency_stops == 1); + } +} +} // namespace micras::test diff --git a/tests/hal/test_spi.cpp b/tests/hal/test_spi.cpp new file mode 100644 index 0000000..efd5d37 --- /dev/null +++ b/tests/hal/test_spi.cpp @@ -0,0 +1,209 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/clock.hpp" +#include "micras/hal/host/ports.hpp" +#include "micras/hal/host/spi_device.hpp" +#include "micras/hal/spi.hpp" +#include "micras/hal/timer.hpp" + +namespace micras::test { +namespace { +using hal::host::Board; +using hal::host::SpiDevice; + +class RecordingDevice : public SpiDevice { +public: + RecordingDevice(Mode mode, uint8_t offset) : SpiDevice{mode}, offset{offset} { } + + void select() override { this->log.emplace_back("select"); } + + void exchange(std::span transmitted, std::span received) override { + this->log.push_back("exchange " + std::to_string(transmitted.size())); + + this->received_bytes.insert(this->received_bytes.end(), transmitted.begin(), transmitted.end()); + std::ranges::transform(transmitted, received.begin(), [this](uint8_t byte) { + return static_cast(byte + this->offset); + }); + } + + void deselect() override { this->log.emplace_back("deselect"); } + + const std::vector& events() const { return this->log; } + + const std::vector& bytes() const { return this->received_bytes; } + +private: + uint8_t offset; + std::vector log; + std::vector received_bytes; +}; +} // namespace + +static void attach(const hal::Spi::Config& config, SpiDevice& device) { + Board::spi_device(config.handle, config.cs_gpio.port, config.cs_gpio.pin, device); +} + +static uint32_t wait_for_transfer(const hal::Spi& spi) { + const uint64_t start = hal::host::Clock::instance().now(); + + while (spi.get_transfer() == hal::Spi::Transfer::RUNNING) { + hal::Timer::get_counter(); + } + + return static_cast((hal::host::Clock::instance().now() - start) / (SystemCoreClock / 1000000)); +} + +TEST_SUITE("spi") { + TEST_CASE_FIXTURE(HostBoard, "routes each transfer to the device its chip select selects") { + RecordingDevice first_chip{SpiDevice::Mode::MODE_3, 0x10}; + RecordingDevice second_chip{SpiDevice::Mode::MODE_1, 0x20}; + attach(spi_config, first_chip); + attach(second_spi_config, second_chip); + hal::Spi first{spi_config}; + hal::Spi second{second_spi_config}; + + const std::array command{0x8F}; + const std::array frame{0x40, 0x01}; + std::array answer{}; + + REQUIRE(first.select_device()); + CHECK(first.transmit(command)); + first.unselect_device(); + REQUIRE(second.select_device()); + CHECK(second.transmit_receive(frame, answer)); + second.unselect_device(); + + CHECK(first_chip.bytes() == std::vector{0x8F}); + CHECK(second_chip.bytes() == std::vector{0x40, 0x01}); + CHECK(answer == std::array{0x60, 0x21}); + CHECK(Board::unbound().empty()); + } + + TEST_CASE_FIXTURE(HostBoard, "makes one transaction of the calls between select and unselect") { + RecordingDevice chip{SpiDevice::Mode::MODE_3, 1}; + attach(spi_config, chip); + hal::Spi spi{spi_config}; + + const std::array command{0x8F}; + std::array data{}; + + REQUIRE(spi.select_device()); + CHECK(spi.transmit(command)); + CHECK(spi.receive(data)); + spi.unselect_device(); + + CHECK(chip.events() == std::vector{"select", "exchange 1", "exchange 2", "deselect"}); + CHECK(data == std::array{1, 1}); + } + + TEST_CASE_FIXTURE(HostBoard, "reaches no device whose chip select is high") { + RecordingDevice chip{SpiDevice::Mode::MODE_3, 1}; + attach(spi_config, chip); + hal::Spi spi{spi_config}; + + std::array data{}; + CHECK(spi.receive(data)); + + CHECK(chip.events().empty()); + CHECK(data == std::array{0xFF, 0xFF}); + } + + TEST_CASE_FIXTURE(HostBoard, "answers all ones where no device is attached") { + hal::Spi spi{spi_config}; + + std::array data{}; + REQUIRE(spi.select_device()); + CHECK(spi.receive(data)); + spi.unselect_device(); + + CHECK(data == std::array{0xFF, 0xFF}); + CHECK(Board::unbound() == std::vector{"SPI_CSn", "hspi3 SPI_CSn"}); + } + + TEST_CASE_FIXTURE(HostBoard, "answers all ones in a mode the device does not answer in") { + RecordingDevice chip{SpiDevice::Mode::MODE_0, 1}; + attach(spi_config, chip); + hal::Spi spi{spi_config}; + + const std::array transmitted{0x8F, 0x00}; + std::array received{}; + REQUIRE(spi.select_device()); + CHECK(spi.transmit_receive(transmitted, received)); + spi.unselect_device(); + + CHECK(chip.events() == std::vector{"select", "deselect"}); + CHECK(received == std::array{0xFF, 0xFF}); + } + + TEST_CASE_FIXTURE(HostBoard, "keeps the device selected while a transfer runs") { + RecordingDevice chip{SpiDevice::Mode::MODE_3, 1}; + attach(spi_config, chip); + hal::Spi spi{spi_config}; + + std::array transmitted{}; + std::array received{}; + const hal::host::GpioPort& chip_select = Board::gpio(spi_config.cs_gpio.port, spi_config.cs_gpio.pin); + + REQUIRE(spi.start_transfer(transmitted, received)); + + CHECK(spi.get_transfer() == hal::Spi::Transfer::RUNNING); + CHECK_FALSE(chip_select.output); + CHECK(chip.events() == std::vector{"select", "exchange 17"}); + } + + TEST_CASE_FIXTURE(HostBoard, "completes a transfer when its last bit is out") { + RecordingDevice chip{SpiDevice::Mode::MODE_3, 1}; + attach(spi_config, chip); + hal::Spi spi{spi_config}; + + std::array transmitted{}; + std::array received{}; + const hal::host::GpioPort& chip_select = Board::gpio(spi_config.cs_gpio.port, spi_config.cs_gpio.pin); + + REQUIRE(spi.start_transfer(transmitted, received)); + const uint32_t elapsed = wait_for_transfer(spi); + + CHECK(spi.get_transfer() == hal::Spi::Transfer::COMPLETE); + CHECK(chip_select.output); + CHECK(chip.events() == std::vector{"select", "exchange 17", "deselect"}); + CHECK(received.back() == 1); + CHECK(elapsed >= 35); + CHECK(elapsed <= 36); + } + + TEST_CASE_FIXTURE(HostBoard, "waits for the bus before selecting another device") { + RecordingDevice first_chip{SpiDevice::Mode::MODE_3, 1}; + RecordingDevice second_chip{SpiDevice::Mode::MODE_1, 1}; + attach(spi_config, first_chip); + attach(second_spi_config, second_chip); + hal::Spi first{spi_config}; + hal::Spi second{second_spi_config}; + + std::array transmitted{}; + std::array received{}; + const uint64_t start = hal::host::Clock::instance().now(); + + REQUIRE(first.start_transfer(transmitted, received)); + REQUIRE(second.select_device()); + const uint64_t waited = hal::host::Clock::instance().now() - start; + second.unselect_device(); + + CHECK(first.get_transfer() == hal::Spi::Transfer::COMPLETE); + CHECK(first_chip.events().back() == "deselect"); + CHECK(waited >= 35U * (SystemCoreClock / 1000000)); + } +} +} // namespace micras::test diff --git a/tests/hal/test_timers.cpp b/tests/hal/test_timers.cpp new file mode 100644 index 0000000..2a3339e --- /dev/null +++ b/tests/hal/test_timers.cpp @@ -0,0 +1,104 @@ +/** + * @file + */ + +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/encoder.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" +#include "micras/hal/pwm.hpp" +#include "micras/hal/pwm_dma.hpp" +#include "tim.h" + +namespace micras::test { +TEST_SUITE("timers") { + TEST_CASE_FIXTURE(HostBoard, "runs a PWM channel at the frequency its registers give") { + const hal::Pwm pwm{pwm_config}; + + CHECK(pwm.was_initialized()); + CHECK(static_cast(pwm.get_frequency()) == doctest::Approx(1000.0)); + CHECK( + static_cast(hal::host::Board::pwm(pwm_config.handle, pwm_config.timer_channel).frequency) == + doctest::Approx(1000.0) + ); + } + + TEST_CASE_FIXTURE(HostBoard, "reports the duty cycle the compare register gives") { + hal::Pwm pwm{pwm_config}; + const hal::host::PwmPort& port = hal::host::Board::pwm(pwm_config.handle, pwm_config.timer_channel); + + pwm.set_duty_cycle(25.0F); + CHECK(static_cast(port.duty_cycle) == doctest::Approx(25.0)); + CHECK(port.touched); + + pwm.set_duty_cycle(150.0F); + CHECK(static_cast(port.duty_cycle) == doctest::Approx(100.0)); + + pwm.set_duty_cycle(-10.0F); + CHECK(static_cast(port.duty_cycle) == doctest::Approx(0.0)); + } + + TEST_CASE_FIXTURE(HostBoard, "reports the active fraction of an inverted channel") { + hal::Pwm::Config config = pwm_config; + config.inverted = true; + hal::Pwm pwm{config}; + + pwm.set_duty_cycle(30.0F); + + CHECK( + static_cast(hal::host::Board::pwm(config.handle, config.timer_channel).duty_cycle) == + doctest::Approx(30.0) + ); + CHECK((htim15.Instance->CCER & TIM_CCER_CC1P) != 0); + } + + TEST_CASE_FIXTURE(HostBoard, "changes the frequency through the autoreload register") { + hal::Pwm pwm{pwm_config}; + + pwm.set_frequency(2000); + + CHECK(htim15.Instance->ARR == 499); + CHECK(static_cast(pwm.get_frequency()) == doctest::Approx(2000.0)); + CHECK( + static_cast(hal::host::Board::pwm(pwm_config.handle, pwm_config.timer_channel).frequency) == + doctest::Approx(2000.0) + ); + } + + TEST_CASE_FIXTURE(HostBoard, "keeps a DMA transfer of compare values busy for one period per value") { + hal::PwmDma pwm_dma{pwm_dma_config}; + std::array compares{}; + compares.fill(pwm_dma.get_compare(50.0F)); + + CHECK(pwm_dma.was_initialized()); + CHECK(compares.front() == 125); + + pwm_dma.start_dma(compares); + CHECK(pwm_dma.is_busy()); + CHECK(hal::host::Board::pwm_dma(pwm_dma_config.handle, pwm_dma_config.timer_channel).compares.size() == 8); + + wait_us(7); + CHECK(pwm_dma.is_busy()); + + wait_us(2); + CHECK_FALSE(pwm_dma.is_busy()); + } + + TEST_CASE_FIXTURE(HostBoard, "counts encoder pulses from where the encoder started") { + hal::host::EncoderPort& port = hal::host::Board::encoder(encoder_config.handle); + port.count = 1000; + + const hal::Encoder encoder{encoder_config}; + CHECK(encoder.was_initialized()); + CHECK(encoder.get_counter() == 0); + + port.count = 900; + CHECK(encoder.get_counter() == -100); + } +} +} // namespace micras::test diff --git a/tests/hal/test_uart.cpp b/tests/hal/test_uart.cpp new file mode 100644 index 0000000..4cd9787 --- /dev/null +++ b/tests/hal/test_uart.cpp @@ -0,0 +1,63 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" +#include "micras/hal/uart_dma.hpp" + +namespace micras::test { +TEST_SUITE("uart") { + TEST_CASE_FIXTURE(HostBoard, "reads what arrived through the circular receive buffer") { + hal::UartDma uart{uart_config}; + std::array ring{}; + REQUIRE(uart.start_rx(ring)); + CHECK(uart.was_initialized()); + + hal::host::UartPort& port = hal::host::Board::uart(uart_config.handle); + std::array received{}; + + for (uint8_t byte = 1; byte <= 6; byte++) { + port.receive(byte); + } + + CHECK(uart.read(received) == 6); + CHECK(received == std::array{1, 2, 3, 4, 5, 6, 0, 0}); + + for (uint8_t byte = 7; byte <= 11; byte++) { + port.receive(byte); + } + + CHECK(uart.read(std::span{received}.first(3)) == 3); + CHECK(received == std::array{7, 8, 9, 4, 5, 6, 0, 0}); + + received = {}; + CHECK(uart.read(received) == 2); + CHECK(received == std::array{10, 11, 0, 0, 0, 0, 0, 0}); + CHECK(uart.read(received) == 0); + } + + TEST_CASE_FIXTURE(HostBoard, "hands the bytes to transmit to the port and refuses more until they leave") { + hal::UartDma uart{uart_config}; + const std::array message{0x10, 0x20, 0x30}; + + REQUIRE(uart.start_tx(message)); + CHECK(uart.is_transmitting()); + CHECK_FALSE(uart.start_tx(message)); + + hal::host::UartPort& port = hal::host::Board::uart(uart_config.handle); + CHECK(port.tx == std::deque{0x10, 0x20, 0x30}); + + port.tx.clear(); + CHECK_FALSE(uart.is_transmitting()); + } +} +} // namespace micras::test diff --git a/tests/models/test_as5047u_model.cpp b/tests/models/test_as5047u_model.cpp new file mode 100644 index 0000000..9282131 --- /dev/null +++ b/tests/models/test_as5047u_model.cpp @@ -0,0 +1,224 @@ +/** + * @file + */ + +#include +#include + +#include + +#include "micras/hal/host/spi_device.hpp" +#include "micras/models/as5047u_model.hpp" + +namespace micras::models { +namespace { +using Frame = std::array; + +constexpr uint32_t read_flag{1U << 22U}; +constexpr uint32_t error_flag{1U << 22U}; +} // namespace + +static Frame frame_of(uint32_t upper) { + const auto high = static_cast(upper >> 16U); + const auto low = static_cast(upper >> 8U); + return {high, low, As5047uModel::crc(high, low)}; +} + +static Frame read_command(uint16_t address) { + return frame_of(read_flag | static_cast(address) << 8U); +} + +static Frame write_command(uint16_t address) { + return frame_of(static_cast(address) << 8U); +} + +static Frame data_frame(uint16_t data) { + return frame_of(static_cast(data) << 8U); +} + +static uint32_t transfer(As5047uModel& chip, const Frame& frame) { + Frame received{}; + chip.select(); + chip.exchange(frame, received); + chip.deselect(); + return static_cast(std::get<0>(received)) << 16U | static_cast(std::get<1>(received)) << 8U | + std::get<2>(received); +} + +static uint16_t data_of(uint32_t answer) { + return static_cast((answer >> 8U) & 0x3FFFU); +} + +static bool valid(uint32_t answer) { + return As5047uModel::crc(static_cast(answer >> 16U), static_cast(answer >> 8U)) == + static_cast(answer); +} + +static uint16_t read_register(As5047uModel& chip, uint16_t address) { + transfer(chip, read_command(address)); + return data_of(transfer(chip, read_command(As5047uModel::nop_address))); +} + +static void write_register(As5047uModel& chip, uint16_t address, uint16_t data) { + transfer(chip, write_command(address)); + transfer(chip, data_frame(data)); +} + +TEST_SUITE("as5047u_model") { + TEST_CASE("answers in SPI mode 1") { + const As5047uModel chip; + + CHECK(chip.mode() == hal::host::SpiDevice::Mode::MODE_1); + } + + TEST_CASE("computes the frame CRC") { + CHECK(As5047uModel::crc(0x00, 0x00) == 0xF1); + CHECK(As5047uModel::crc(0x40, 0x01) == 0x06); + CHECK(As5047uModel::crc(0x40, 0x1A) == 0x04); + CHECK(As5047uModel::crc(0x00, 0x80) == 0xD7); + } + + TEST_CASE("powers on with zeroed registers and an empty answer") { + As5047uModel chip; + + for (const uint16_t address : + {As5047uModel::errfl_address, As5047uModel::disable_address, As5047uModel::zposm_address, + As5047uModel::zposl_address, As5047uModel::settings1_address, As5047uModel::settings2_address, + As5047uModel::settings3_address, As5047uModel::ecc_address}) { + INFO("register " << address); + CHECK(chip.peek(address) == 0); + } + + CHECK(transfer(chip, read_command(As5047uModel::nop_address)) == 0x0000F1U); + } + + TEST_CASE("answers each frame in the next one") { + As5047uModel chip; + write_register(chip, As5047uModel::settings3_address, 0x80); + write_register(chip, As5047uModel::zposm_address, 0x12); + + transfer(chip, read_command(As5047uModel::settings3_address)); + const uint32_t settings3 = transfer(chip, read_command(As5047uModel::zposm_address)); + const uint32_t zposm = transfer(chip, read_command(As5047uModel::nop_address)); + + CHECK(data_of(settings3) == 0x80); + CHECK(data_of(zposm) == 0x12); + CHECK(valid(settings3)); + CHECK(valid(zposm)); + } + + TEST_CASE("writes with a command and a data frame") { + As5047uModel chip; + write_register(chip, As5047uModel::settings2_address, 0x04); + + transfer(chip, write_command(As5047uModel::settings2_address)); + const uint32_t old_content = transfer(chip, data_frame(0x21)); + const uint32_t new_content = transfer(chip, read_command(As5047uModel::nop_address)); + + CHECK(data_of(old_content) == 0x04); + CHECK(data_of(new_content) == 0x21); + CHECK(chip.peek(As5047uModel::settings2_address) == 0x21); + } + + TEST_CASE("holds every volatile register") { + As5047uModel chip; + uint16_t value = 0x11; + + for (const uint16_t address : + {As5047uModel::disable_address, As5047uModel::zposm_address, As5047uModel::zposl_address, + As5047uModel::settings1_address, As5047uModel::settings2_address, As5047uModel::settings3_address, + As5047uModel::ecc_address}) { + write_register(chip, address, value); + INFO("register " << address); + CHECK(read_register(chip, address) == value); + value += 0x11; + } + } + + TEST_CASE("ignores writes to other addresses") { + As5047uModel chip; + + write_register(chip, As5047uModel::errfl_address, 0x7F); + write_register(chip, 0x3FFF, 0x7F); + + CHECK(chip.peek(As5047uModel::errfl_address) == 0); + CHECK(read_register(chip, 0x3FFF) == 0); + } + + TEST_CASE("rejects a frame with a wrong CRC") { + As5047uModel chip; + Frame command = write_command(As5047uModel::settings3_address); + std::get<2>(command) ^= 0x01; + + transfer(chip, command); + const uint32_t after_error = transfer(chip, data_frame(0x80)); + + CHECK(chip.peek(As5047uModel::settings3_address) == 0); + CHECK(chip.peek(As5047uModel::errfl_address) == As5047uModel::crc_error); + CHECK((after_error & error_flag) != 0U); + CHECK(valid(after_error)); + } + + TEST_CASE("drops a write whose data frame has a wrong CRC") { + As5047uModel chip; + Frame data = data_frame(0x80); + std::get<2>(data) ^= 0x80; + + transfer(chip, write_command(As5047uModel::settings3_address)); + transfer(chip, data); + transfer(chip, data_frame(0x80)); + + CHECK(chip.peek(As5047uModel::settings3_address) == 0); + CHECK(chip.peek(As5047uModel::errfl_address) == As5047uModel::crc_error); + } + + TEST_CASE("clears the error flags when ERRFL is read") { + As5047uModel chip; + Frame command = read_command(As5047uModel::settings3_address); + std::get<0>(command) ^= 0x01; + transfer(chip, command); + + transfer(chip, read_command(As5047uModel::errfl_address)); + const uint32_t errfl = transfer(chip, read_command(As5047uModel::nop_address)); + const uint32_t after = transfer(chip, read_command(As5047uModel::nop_address)); + + CHECK(data_of(errfl) == As5047uModel::crc_error); + CHECK((errfl & error_flag) != 0U); + CHECK((after & error_flag) == 0U); + CHECK(chip.peek(As5047uModel::errfl_address) == 0); + } + + TEST_CASE("flags a frame cut short by the chip select") { + As5047uModel chip; + const Frame command = write_command(As5047uModel::settings3_address); + const std::array partial{std::get<0>(command), std::get<1>(command)}; + std::array received{}; + + chip.select(); + chip.exchange(partial, received); + chip.deselect(); + transfer(chip, data_frame(0x80)); + + CHECK(chip.peek(As5047uModel::settings3_address) == 0); + CHECK(chip.peek(As5047uModel::errfl_address) == As5047uModel::framing_error); + } + + TEST_CASE("takes a frame split across exchanges") { + As5047uModel chip; + const Frame command = write_command(As5047uModel::zposl_address); + const std::array first{std::get<0>(command)}; + const std::array rest{std::get<1>(command), std::get<2>(command)}; + std::array first_received{}; + std::array rest_received{}; + + chip.select(); + chip.exchange(first, first_received); + chip.exchange(rest, rest_received); + chip.deselect(); + transfer(chip, data_frame(0x3F)); + + CHECK(chip.peek(As5047uModel::zposl_address) == 0x3F); + CHECK(chip.peek(As5047uModel::errfl_address) == 0); + } +} +} // namespace micras::models diff --git a/tests/models/test_lsm6dsv_model.cpp b/tests/models/test_lsm6dsv_model.cpp new file mode 100644 index 0000000..e2d61ce --- /dev/null +++ b/tests/models/test_lsm6dsv_model.cpp @@ -0,0 +1,333 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include "lsm6dsv_reg.h" +#include "micras/hal/host/spi_device.hpp" +#include "micras/models/lsm6dsv_model.hpp" + +namespace micras::models { +namespace { +constexpr uint8_t read_flag{0x80}; +} // namespace + +static std::vector transact(Lsm6dsvModel& chip, std::vector transmitted) { + std::vector received(transmitted.size()); + chip.select(); + chip.exchange(transmitted, received); + chip.deselect(); + return received; +} + +static void write(Lsm6dsvModel& chip, uint8_t address, uint8_t value) { + transact(chip, {address, value}); +} + +static uint8_t read(Lsm6dsvModel& chip, uint8_t address) { + return transact(chip, {static_cast(address | read_flag), 0}).at(1); +} + +static int16_t word(const std::vector& bytes, std::size_t low) { + return static_cast(bytes.at(low) | (bytes.at(low + 1) << 8U)); +} + +static void configure(Lsm6dsvModel& chip, uint8_t gyroscope_scale, uint8_t accelerometer_scale) { + write(chip, LSM6DSV_HAODR_CFG, 0x01); + write(chip, LSM6DSV_CTRL1, 0x1C); + write(chip, LSM6DSV_CTRL2, 0x1C); + write(chip, LSM6DSV_CTRL6, gyroscope_scale); + write(chip, LSM6DSV_CTRL8, accelerometer_scale); +} + +static std::vector read_burst(Lsm6dsvModel& chip) { + std::vector command(17); + command.at(0) = LSM6DSV_STATUS_REG | read_flag; + return transact(chip, command); +} + +TEST_SUITE("lsm6dsv_model") { + TEST_CASE("answers in SPI mode 3") { + const Lsm6dsvModel chip; + + CHECK(chip.mode() == hal::host::SpiDevice::Mode::MODE_3); + } + + TEST_CASE("powers on with the datasheet defaults") { + const Lsm6dsvModel chip; + + for (uint8_t address = 0; address < Lsm6dsvModel::register_count; address++) { + uint8_t expected = 0x00; + + if (address == LSM6DSV_WHO_AM_I) { + expected = LSM6DSV_ID; + } else if (address == LSM6DSV_PIN_CTRL) { + expected = 0x23; + } else if (address == LSM6DSV_CTRL3) { + expected = 0x44; + } + + INFO("register " << static_cast(address)); + CHECK(chip.peek(address) == expected); + } + } + + TEST_CASE("answers WHO_AM_I with the device id") { + Lsm6dsvModel chip; + + const std::vector received = transact(chip, {LSM6DSV_WHO_AM_I | read_flag, 0xFF}); + + CHECK(received.at(0) == 0x00); + CHECK(received.at(1) == LSM6DSV_ID); + } + + TEST_CASE("advances the address after every byte") { + Lsm6dsvModel chip; + + transact(chip, {LSM6DSV_CTRL1, 0x16, 0x1C}); + const std::vector received = transact(chip, {LSM6DSV_CTRL1 | read_flag, 0, 0}); + + CHECK(received.at(1) == 0x16); + CHECK(received.at(2) == 0x1C); + } + + TEST_CASE("keeps one transaction across several exchanges") { + Lsm6dsvModel chip; + write(chip, LSM6DSV_CTRL2, 0x1C); + + const std::array command{LSM6DSV_CTRL1 | read_flag}; + std::array ignored{}; + const std::array dummy{}; + std::array received{}; + + chip.select(); + chip.exchange(command, ignored); + chip.exchange(dummy, received); + chip.deselect(); + + CHECK(std::get<0>(received) == 0x00); + CHECK(std::get<1>(received) == 0x1C); + } + + TEST_CASE("starts every transaction with a command") { + Lsm6dsvModel chip; + + transact(chip, {LSM6DSV_CTRL1, 0x16}); + transact(chip, {LSM6DSV_CTRL2, 0x1C}); + + CHECK(chip.peek(LSM6DSV_CTRL1) == 0x16); + CHECK(chip.peek(LSM6DSV_CTRL2) == 0x1C); + } + + TEST_CASE("stays on one register without auto increment") { + Lsm6dsvModel chip; + write(chip, LSM6DSV_CTRL3, 0x40); + + transact(chip, {LSM6DSV_CTRL1, 0x16, 0x1C}); + const std::vector received = transact(chip, {LSM6DSV_CTRL1 | read_flag, 0, 0}); + + CHECK(received.at(1) == 0x1C); + CHECK(received.at(2) == 0x1C); + CHECK(chip.peek(LSM6DSV_CTRL2) == 0x00); + } + + TEST_CASE("ignores writes to read only and reserved registers") { + Lsm6dsvModel chip; + + write(chip, LSM6DSV_WHO_AM_I, 0x00); + write(chip, LSM6DSV_STATUS_REG, 0xFF); + write(chip, LSM6DSV_OUTX_L_G, 0x12); + write(chip, 0x04, 0x34); + + CHECK(chip.peek(LSM6DSV_WHO_AM_I) == LSM6DSV_ID); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x00); + CHECK(chip.peek(LSM6DSV_OUTX_L_G) == 0x00); + CHECK(chip.peek(0x04) == 0x00); + } + + TEST_CASE("restores the defaults on a software power on reset") { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + + transact(chip, {LSM6DSV_FUNC_CFG_ACCESS, 0x04, 0x55}); + + CHECK(chip.peek(LSM6DSV_FUNC_CFG_ACCESS) == 0x00); + CHECK(chip.peek(LSM6DSV_PIN_CTRL) == 0x23); + CHECK(chip.peek(LSM6DSV_CTRL1) == 0x00); + CHECK(chip.peek(LSM6DSV_CTRL2) == 0x00); + CHECK(chip.peek(LSM6DSV_CTRL3) == 0x44); + CHECK(chip.peek(LSM6DSV_CTRL6) == 0x00); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x00); + CHECK(chip.peek(LSM6DSV_OUTX_L_G) == 0x00); + CHECK(chip.peek(LSM6DSV_HAODR_CFG) == 0x00); + CHECK(read(chip, LSM6DSV_WHO_AM_I) == LSM6DSV_ID); + } + + TEST_CASE("restores the defaults on a software reset") { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + + write(chip, LSM6DSV_CTRL3, 0x45); + + CHECK(chip.peek(LSM6DSV_CTRL3) == 0x44); + CHECK(chip.peek(LSM6DSV_CTRL2) == 0x00); + CHECK(chip.peek(LSM6DSV_CTRL6) == 0x00); + } + + TEST_CASE("hides the main page while another is selected") { + Lsm6dsvModel chip; + + write(chip, LSM6DSV_FUNC_CFG_ACCESS, 0x80); + CHECK(read(chip, LSM6DSV_WHO_AM_I) == 0x00); + write(chip, LSM6DSV_CTRL1, 0x1C); + + write(chip, LSM6DSV_FUNC_CFG_ACCESS, 0x00); + CHECK(read(chip, LSM6DSV_WHO_AM_I) == LSM6DSV_ID); + CHECK(chip.peek(LSM6DSV_CTRL1) == 0x00); + } + + TEST_CASE("ignores samples while the sensors are off") { + Lsm6dsvModel chip; + + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x00); + CHECK(chip.peek(LSM6DSV_OUTX_L_G) == 0x00); + CHECK(chip.peek(LSM6DSV_OUTX_L_A) == 0x00); + } + + TEST_CASE("sets the data ready bit of each sensor that is on") { + Lsm6dsvModel chip; + write(chip, LSM6DSV_CTRL2, 0x1C); + + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x02); + CHECK(chip.peek(LSM6DSV_OUTX_L_A) == 0x00); + + write(chip, LSM6DSV_CTRL1, 0x1C); + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x03); + } + + TEST_CASE("clears the data ready bits with the burst that reads the sample") { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + + const std::vector first = read_burst(chip); + const std::vector second = read_burst(chip); + + CHECK(first.at(1) == 0x03); + CHECK(second.at(1) == 0x00); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x00); + CHECK(word(second, 5) == word(first, 5)); + } + + TEST_CASE("clears each data ready bit with a high byte of its sensor") { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + + read(chip, LSM6DSV_OUTX_L_G); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x03); + + read(chip, LSM6DSV_OUTY_H_G); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x01); + + read(chip, LSM6DSV_OUTZ_H_A); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x00); + } + + TEST_CASE("encodes the sample with the full scale in the registers") { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + chip.push_sample({1.0, -1.0, 0.0}, {9.80665, -9.80665, 0.0}); + + const std::vector wide = read_burst(chip); + + CHECK(word(wide, 5) == 409); + CHECK(word(wide, 7) == -409); + CHECK(word(wide, 9) == 0); + CHECK(word(wide, 11) == 4098); + CHECK(word(wide, 13) == -4098); + CHECK(word(wide, 15) == 0); + + write(chip, LSM6DSV_CTRL6, LSM6DSV_125dps); + write(chip, LSM6DSV_CTRL8, LSM6DSV_2g); + chip.push_sample({1.0, 100.0, -100.0}, {9.80665, 100.0, -100.0}); + + const std::vector narrow = read_burst(chip); + + CHECK(word(narrow, 5) == 13096); + CHECK(word(narrow, 7) == 32767); + CHECK(word(narrow, 9) == -32768); + CHECK(word(narrow, 11) == 16393); + CHECK(word(narrow, 13) == 32767); + CHECK(word(narrow, 15) == -32768); + } + + TEST_CASE("gives the sensitivity of every full scale") { + Lsm6dsvModel chip; + constexpr double mdps{std::numbers::pi / 180000.0}; + constexpr double mg{0.00980665}; + + const std::array, 6> gyroscope{{ + {LSM6DSV_125dps, 4.375}, + {LSM6DSV_250dps, 8.75}, + {LSM6DSV_500dps, 17.5}, + {LSM6DSV_1000dps, 35.0}, + {LSM6DSV_2000dps, 70.0}, + {LSM6DSV_4000dps, 140.0}, + }}; + const std::array, 4> accelerometer{{ + {LSM6DSV_2g, 0.061}, + {LSM6DSV_4g, 0.122}, + {LSM6DSV_8g, 0.244}, + {LSM6DSV_16g, 0.488}, + }}; + + for (const auto& [code, sensitivity] : gyroscope) { + write(chip, LSM6DSV_CTRL6, code); + CHECK(chip.gyroscope_sensitivity() == doctest::Approx(sensitivity * mdps)); + } + + for (const auto& [code, sensitivity] : accelerometer) { + write(chip, LSM6DSV_CTRL8, code); + CHECK(chip.accelerometer_sensitivity() == doctest::Approx(sensitivity * mg)); + } + } + + TEST_CASE("decodes the output data rates") { + Lsm6dsvModel chip; + + CHECK(chip.gyroscope_data_rate() == 0.0); + CHECK(chip.accelerometer_data_rate() == 0.0); + + write(chip, LSM6DSV_CTRL2, 0x10 | (LSM6DSV_ODR_HA01_AT_8000Hz & 0x0F)); + write(chip, LSM6DSV_CTRL1, 0x10 | (LSM6DSV_ODR_HA01_AT_125Hz & 0x0F)); + + CHECK(chip.gyroscope_data_rate() == 7680.0); + + write(chip, LSM6DSV_HAODR_CFG, 0x01); + CHECK(chip.gyroscope_data_rate() == 8000.0); + CHECK(chip.accelerometer_data_rate() == 125.0); + + write(chip, LSM6DSV_HAODR_CFG, 0x02); + CHECK(chip.gyroscope_data_rate() == 6400.0); + + write(chip, LSM6DSV_CTRL1, LSM6DSV_ODR_AT_1Hz875); + CHECK(chip.accelerometer_data_rate() == 1.875); + } +} +} // namespace micras::models diff --git a/tests/nav/reference_robot.cpp b/tests/nav/reference_robot.cpp new file mode 100644 index 0000000..f4fd7b8 --- /dev/null +++ b/tests/nav/reference_robot.cpp @@ -0,0 +1,116 @@ +/** + * @file + */ + +#include +#include +#include + +#include + +#include "maze_reader.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/robot_model.hpp" +#include "micras/nav/turn_table.hpp" +#include "reference_robot.hpp" + +namespace micras::test { +const nav::RobotModel reference_robot{ + .maze = {.cell_size = 0.18F, .wall_thickness = 0.0126F}, + .chassis = + { + .mass = 0.07F, + .yaw_inertia = 2.9e-5F, + .wheel_radius = 0.011F, + .rolling_compliance = 56e-6F, + .track_width = 0.04575F, + .half_width = 0.0251F, + .front_length = 0.0549F, + .rear_length = 0.0365F, + }, + .traction = + { + .friction_coefficient = 1.0F, + .fan_downforce = 3.0F, + .fan_offset = 0.0175F, + .lateral_compliance = 0.0048F, + }, + .drive = + { + .torque_constant = 0.00336F, + .resistance = 12.62F, + .gear_ratio = 5.25F, + .supply_voltage = 19.63F, + .static_friction_voltage = 0.09F, + }, + .wall_sensors = {{ + {.position = {.x = 0.0439F, .y = 0.0215F}, .angle = 0.0F, .half_angle = 0.09F}, + {.position = {.x = 0.0526F, .y = 0.0153F}, .angle = std::numbers::pi_v / 4.0F, .half_angle = 0.09F}, + {.position = {.x = 0.0526F, .y = -0.0153F}, .angle = -std::numbers::pi_v / 4.0F, .half_angle = 0.09F}, + {.position = {.x = 0.0439F, .y = -0.0215F}, .angle = 0.0F, .half_angle = 0.09F}, + }}, + .noise = + { + .gyroscope = 6.5e-5F, + .gyroscope_bias_walk = 1.0e-4F, + .wheel_angle = 2.0F * std::numbers::pi_v / 16384.0F, + .longitudinal_slip = 0.005F, + .longitudinal_slip_per_acceleration = 0.0005F, + .lateral_slip = 0.003F, + .wall_range = 0.001F, + .wall_range_per_meter = 0.017F, + }, + .gyroscope_scale = 1.0F, +}; + +const nav::Dynamics& reference_dynamics() { + static const nav::Dynamics dynamics{{ + .model = reference_robot, + .turns = + nav::TurnTable{ + reference_robot, turn_margin, nav::TurnTable::place(reference_robot, turn_margin, two_bend_designs) + }, + .risky_turns = + nav::TurnTable{ + reference_robot, risky_turn_margin, + nav::TurnTable::place(reference_robot, risky_turn_margin, risky_two_bend_designs) + }, + .max_linear_speed = 3.0F, + .max_angular_speed = 12.0F, + .voltage_reserve = 0.15F, + }}; + + return dynamics; +} + +nav::Route plan_route(const MazeText& maze, const nav::RunProfile& profile) { + using Planner = nav::TPlanner<16, 16>; + + nav::TMaze<16, 16> map{{.start = {.position = maze.start, .orientation = nav::Side::UP}, .goal = maze.goal}}; + fill_maze(maze, map); + + const auto planner = + std::make_unique(reference_dynamics(), Planner::Config{.start_distance = start_distance}); + planner->begin(map, nav::WallAssumption::PESSIMISTIC, profile); + + while (not planner->step(1000)) { } + + CHECK(planner->is_finished()); + CHECK(planner->is_exact()); + REQUIRE(planner->get_number_of_routes() > 0); + + nav::Route route; + planner->get_route(0, route); + + for (uint8_t index = 1; index < planner->get_number_of_routes(); index++) { + nav::Route candidate; + planner->get_route(index, candidate); + CHECK(candidate.time >= route.time); + } + + return route; +} +} // namespace micras::test diff --git a/tests/nav/reference_robot.hpp b/tests/nav/reference_robot.hpp new file mode 100644 index 0000000..257b355 --- /dev/null +++ b/tests/nav/reference_robot.hpp @@ -0,0 +1,169 @@ +/** + * @file + * + * @brief The robot the nav tests plan for: Micras v1 as the firmware describes it at MicrasFirmware + * 4dac6e2 (config/targets/v1/robot.hpp, config/turn_margins.hpp and config/two_bend_turns.hpp). + * + * @note A real robot, so that the planner and the turn tables are tested on the geometry and the + * dynamics they are tuned for. The model is defined in its own source, so that nothing built from it + * here is a constant expression. + */ + +#ifndef MICRAS_LIB_TESTS_NAV_REFERENCE_ROBOT_HPP +#define MICRAS_LIB_TESTS_NAV_REFERENCE_ROBOT_HPP + +#include +#include +#include + +#include "maze_reader.hpp" +#include "micras/nav/lattice.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/robot_model.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::test { +/** + * @brief Physical description of the robot and of the classic maze. + */ +extern const nav::RobotModel reference_robot; + +/** + * @brief Distance kept between the outline of the robot and any obstacle, without and with the + * risky switch. + */ +///@{ +inline constexpr float turn_margin{0.015F}; +inline constexpr float risky_turn_margin{0.010F}; +///@} + +/** + * @brief Distance from the start pose of a run to the first wall it crosses. + */ +inline constexpr float start_distance{0.18F - (0.04F + 0.0126F / 2.0F)}; + +/** + * @brief Designs of the turns of two bends, with the normal margin. + */ +inline constexpr std::array two_bend_designs{{ + {.first_angle = -0.436332315F, + .first_curvature = 11.8644524F, + .second_angle = 1.22173047F, + .second_curvature = 11.8644524F, + .pre = 0.0F}, + {.first_angle = 1.26536369F, + .first_curvature = 17.934948F, + .second_angle = -0.479965538F, + .second_curvature = 17.934948F, + .pre = 0.0F}, + {.first_angle = -0.0872664601F, + .first_curvature = 5.19209814F, + .second_angle = 2.44346094F, + .second_curvature = 19.8867226F, + .pre = 0.00999999978F}, + {.first_angle = -0.479965538F, + .first_curvature = 17.934948F, + .second_angle = 1.26536369F, + .second_curvature = 17.934948F, + .pre = 0.0F}, + {.first_angle = -1.39626336F, + .first_curvature = 19.8867226F, + .second_angle = 1.39626336F, + .second_curvature = 19.8867226F, + .pre = 0.0F}, + {.first_angle = -1.13446403F, + .first_curvature = 13.1556044F, + .second_angle = 0.34906584F, + .second_curvature = 9.64987087F, + .pre = 0.0F}, + {.first_angle = -0.741764903F, + .first_curvature = 13.1556044F, + .second_angle = -0.829031408F, + .second_curvature = 13.1556044F, + .pre = 0.0F}, + {.first_angle = -2.44346094F, + .first_curvature = 19.8867226F, + .second_angle = 0.0872664601F, + .second_curvature = 4.22295713F, + .pre = 0.0F}, +}}; + +/** + * @brief Designs of the turns of two bends, with the risky margin. + */ +inline constexpr std::array risky_two_bend_designs{{ + {.first_angle = -0.436332315F, + .first_curvature = 11.8644524F, + .second_angle = 1.22173047F, + .second_curvature = 11.8644524F, + .pre = 0.0F}, + {.first_angle = 1.26536369F, + .first_curvature = 17.934948F, + .second_angle = -0.479965538F, + .second_curvature = 17.934948F, + .pre = 0.0F}, + {.first_angle = -0.0872664601F, + .first_curvature = 5.19209814F, + .second_angle = 2.44346094F, + .second_curvature = 19.8867226F, + .pre = 0.00999999978F}, + {.first_angle = -0.479965538F, + .first_curvature = 17.934948F, + .second_angle = 1.26536369F, + .second_curvature = 17.934948F, + .pre = 0.0F}, + {.first_angle = -1.39626336F, + .first_curvature = 19.8867226F, + .second_angle = 1.39626336F, + .second_curvature = 19.8867226F, + .pre = 0.0F}, + {.first_angle = -1.22173047F, + .first_curvature = 11.8644524F, + .second_angle = 0.436332315F, + .second_curvature = 11.8644524F, + .pre = 0.0F}, + {.first_angle = -0.741764903F, + .first_curvature = 9.64987087F, + .second_angle = -0.829031408F, + .second_curvature = 9.64987087F, + .pre = 0.0F}, + {.first_angle = -2.44346094F, + .first_curvature = 19.8867226F, + .second_angle = 0.0872664601F, + .second_curvature = 4.22295713F, + .pre = 0.0F}, +}}; + +/** + * @brief Profile of a fast run with the fan on, the normal turns and the normal share of the traction. + */ +inline constexpr nav::RunProfile fast_profile{ + .racing_line = false, + .fan = true, + .risky = false, + .utilization = 0.6F, + .max_speed = std::numeric_limits::infinity(), +}; + +/** + * @brief Get the dynamics of the robot, with both turn tables designed when first asked for. + * + * @return The dynamics, shared by every test of the program. + */ +const nav::Dynamics& reference_dynamics(); + +/** + * @brief Plan the fastest route of a maze whose walls are all known, from the start cell facing up. + * + * @note Checks on the way that the search finished, kept every label it needed, and ranked its + * candidates by their time. + * + * @param maze The maze, with its goal. + * @param profile The profile of the run. + * @return The fastest route the search found. + */ +nav::Route plan_route(const MazeText& maze, const nav::RunProfile& profile); +} // namespace micras::test + +#endif // MICRAS_LIB_TESTS_NAV_REFERENCE_ROBOT_HPP diff --git a/tests/nav/test_maze.cpp b/tests/nav/test_maze.cpp new file mode 100644 index 0000000..603b6ff --- /dev/null +++ b/tests/nav/test_maze.cpp @@ -0,0 +1,136 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include "maze_reader.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze.hpp" + +namespace micras::test { +namespace { +using Maze = nav::TMaze<16, 16>; + +constexpr nav::GridPose maze_start{.position = {.x = 0, .y = 0}, .orientation = nav::Side::UP}; + +constexpr std::array maze_goal{ + {{.x = 8, .y = 8}, {.x = 7, .y = 8}, {.x = 8, .y = 7}, {.x = 7, .y = 7}} +}; +} // namespace + +static void check_same_walls(const Maze& first, const Maze& second) { + for (uint8_t row = 0; row < 16; row++) { + for (uint8_t column = 0; column < 16; column++) { + for (const nav::Side side : nav::all_sides) { + const nav::GridPose wall{.position = {.x = column, .y = row}, .orientation = side}; + CAPTURE(column); + CAPTURE(row); + CAPTURE(static_cast(side)); + REQUIRE(first.get_wall(wall) == second.get_wall(wall)); + } + } + } +} + +TEST_SUITE("maze") { + TEST_CASE("reads the contest mazes with their start and goal") { + for (const auto& entry : std::filesystem::directory_iterator{MICRAS_LIB_TEST_MAZES}) { + CAPTURE(entry.path().filename().string()); + const MazeText text = read_maze(entry.path()); + + CHECK(text.width == 16); + CHECK(text.height == 16); + CHECK(text.walls.size() == 2 * 16 * 16 + 2 * 16); + CHECK(text.start == maze_start.position); + CHECK(text.goal.size() == 4); + } + } + + TEST_CASE("round trips a whole maze through its serialization") { + const MazeText text = read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / "maze1.txt"); + Maze maze{{.start = maze_start, .goal = maze_goal}}; + fill_maze(text, maze); + + const std::vector bytes = maze.serialize(); + CHECK(bytes.size() == 3 + 16 * 16 / 2); + CHECK(bytes.at(1) == 16); + CHECK(bytes.at(2) == 16); + + Maze copy{{.start = maze_start, .goal = maze_goal}}; + copy.deserialize(bytes.data(), static_cast(bytes.size())); + + check_same_walls(maze, copy); + CHECK(copy.get_cost(maze_start.position) == maze.get_cost(maze_start.position)); + } + + TEST_CASE("keeps the unknown walls unknown through the serialization") { + const MazeText text = read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / "japan2017ef.txt"); + Maze maze{{.start = maze_start, .goal = maze_goal}}; + + for (std::size_t index = 0; index < text.walls.size(); index += 3) { + maze.set_wall(text.walls.at(index).first, text.walls.at(index).second); + } + + const std::vector bytes = maze.serialize(); + Maze copy{{.start = maze_start, .goal = maze_goal}}; + copy.deserialize(bytes.data(), static_cast(bytes.size())); + + check_same_walls(maze, copy); + } + + TEST_CASE("ignores a serialization of another format or size") { + const MazeText text = read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / "maze1.txt"); + Maze maze{{.start = maze_start, .goal = maze_goal}}; + fill_maze(text, maze); + + const Maze fresh{{.start = maze_start, .goal = maze_goal}}; + Maze copy{{.start = maze_start, .goal = maze_goal}}; + + std::vector bytes = maze.serialize(); + bytes.at(0) ^= 0xFFU; + copy.deserialize(bytes.data(), static_cast(bytes.size())); + check_same_walls(copy, fresh); + + bytes = maze.serialize(); + copy.deserialize(bytes.data(), static_cast(bytes.size() - 1)); + check_same_walls(copy, fresh); + } + + TEST_CASE("floods the cost to the goal through the known walls") { + const MazeText text = read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / "maze1.txt"); + Maze maze{{.start = maze_start, .goal = maze_goal}}; + fill_maze(text, maze); + + for (const nav::GridPoint& cell : maze_goal) { + CHECK(maze.get_cost(cell) == 0); + } + + nav::GridPose pose = maze_start; + uint16_t steps = 0; + + while (not maze.is_goal(pose.position) and steps < 256) { + const std::optional next = maze.get_next(pose); + + if (not next.has_value()) { + FAIL_CHECK("no neighbor leads to the goal"); + break; + } + + CHECK(maze.get_cost(next->position) + 1 == maze.get_cost(pose.position)); + pose = *next; + steps++; + } + + CHECK(maze.is_goal(pose.position)); + CHECK(steps == maze.get_cost(maze_start.position)); + } +} +} // namespace micras::test diff --git a/tests/nav/test_planner.cpp b/tests/nav/test_planner.cpp new file mode 100644 index 0000000..91d4a13 --- /dev/null +++ b/tests/nav/test_planner.cpp @@ -0,0 +1,72 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include + +#include "maze_reader.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "reference_robot.hpp" + +namespace micras::test { +namespace { +struct Golden { + std::string_view maze; + float time; + std::size_t steps; +}; + +constexpr std::array golden_routes{{ + {.maze = "alljapan-033-2012-exp-fin", .time = 5.28452778F, .steps = 19}, + {.maze = "apec2016", .time = 9.0424633F, .steps = 26}, + {.maze = "apec2017", .time = 7.20180178F, .steps = 24}, + {.maze = "apec2018", .time = 5.90276146F, .steps = 17}, + {.maze = "apec2019", .time = 7.93852806F, .steps = 34}, + {.maze = "japan2013ef", .time = 7.11609554F, .steps = 27}, + {.maze = "japan2017ef", .time = 7.34178638F, .steps = 51}, + {.maze = "maze1", .time = 3.97810149F, .steps = 21}, + {.maze = "maze2", .time = 3.71197319F, .steps = 10}, + {.maze = "uk2016f", .time = 4.59111643F, .steps = 23}, +}}; +} // namespace + +TEST_SUITE("planner") { + TEST_CASE("finds the golden route of every contest maze") { + for (const Golden& golden : golden_routes) { + CAPTURE(golden.maze); + const MazeText text = + read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / (std::string{golden.maze} + ".txt")); + const nav::Route route = plan_route(text, fast_profile); + + CHECK(static_cast(route.time) == doctest::Approx(static_cast(golden.time)).epsilon(1e-6)); + CHECK(route.steps.size() == golden.steps); + } + } + + TEST_CASE("drives no slower once walls are taken out of the maze") { + const MazeText text = read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / "maze2.txt"); + MazeText opened = text; + + for (std::size_t index = 0; index < opened.walls.size(); index += 16) { + opened.walls.at(index).second = false; + } + + CHECK(plan_route(opened, fast_profile).time <= plan_route(text, fast_profile).time); + } + + TEST_CASE("drives faster with the fan than without") { + const MazeText text = read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / "maze1.txt"); + nav::RunProfile no_fan = fast_profile; + no_fan.fan = false; + + CHECK(plan_route(text, fast_profile).time < plan_route(text, no_fan).time); + } +} +} // namespace micras::test diff --git a/tests/nav/test_route_compiler.cpp b/tests/nav/test_route_compiler.cpp new file mode 100644 index 0000000..137b7cb --- /dev/null +++ b/tests/nav/test_route_compiler.cpp @@ -0,0 +1,164 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include "maze_reader.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/lattice.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/route_compiler.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/state.hpp" +#include "micras/nav/turn_table.hpp" +#include "reference_robot.hpp" + +namespace micras::test { +namespace { +constexpr float tolerance{1e-3F}; + +const nav::LatticePose first_node{.point = {.x = 1, .y = 2}, .heading = 2}; +} // namespace + +static nav::Pose end_of(const nav::Segment& segment, const nav::RunProfile& profile) { + if (segment.kind == nav::SegmentKind::STRAIGHT) { + return segment.start.compose({.position = {.x = segment.length, .y = 0.0F}, .orientation = 0.0F}); + } + + const nav::TurnShape& shape = reference_dynamics().get_turn(profile, segment.turn); + const auto end = shape.sample(shape.length()); + const float side = segment.length < 0.0F ? -1.0F : 1.0F; + + return segment.start.compose({.position = {.x = end.x, .y = side * end.y}, .orientation = side * end.heading}); +} + +static bool same_pose(const nav::Pose& first, const nav::Pose& second) { + const float heading = std::remainder(first.orientation - second.orientation, 2.0F * std::numbers::pi_v); + + return std::abs(first.position.x - second.position.x) < tolerance and + std::abs(first.position.y - second.position.y) < tolerance and std::abs(heading) < tolerance; +} + +static std::vector compile(const nav::Route& route) { + std::vector segments; + nav::RouteCompiler::compile(route, reference_dynamics(), fast_profile, start_distance, segments); + return segments; +} + +TEST_SUITE("route_compiler") { + TEST_CASE("drives a straight route as one straight from the start pose") { + const nav::Route route{ + .start = first_node, + .steps = {{.run = 3, .has_turn = false, .turn = nav::TurnId::SS90S, .side = nav::TurnSide::LEFT}}, + .stop_distance = 0.09F, + .finish_distance = 0.0F, + .time = 1.0F, + }; + const float cell_size = reference_robot.maze.cell_size; + + const std::vector segments = compile(route); + + REQUIRE(segments.size() == 1); + CHECK(segments.front().kind == nav::SegmentKind::STRAIGHT); + CHECK( + static_cast(segments.front().length) == + doctest::Approx(static_cast(start_distance + 3 * cell_size + 0.09F)) + ); + CHECK(same_pose( + segments.front().start, {.position = {.x = cell_size / 2.0F, .y = cell_size - start_distance}, + .orientation = std::numbers::pi_v / 2.0F} + )); + } + + TEST_CASE("places a turn between the straights of its nodes") { + const nav::Route route{ + .start = first_node, + .steps = + {{.run = 1, .has_turn = true, .turn = nav::TurnId::SS90S, .side = nav::TurnSide::RIGHT}, + {.run = 2, .has_turn = false, .turn = nav::TurnId::SS90S, .side = nav::TurnSide::LEFT}}, + .stop_distance = 0.09F, + .finish_distance = 0.0F, + .time = 1.0F, + }; + const nav::TurnShape& shape = reference_dynamics().get_turn(fast_profile, nav::TurnId::SS90S); + const float cell_size = reference_robot.maze.cell_size; + + const std::vector segments = compile(route); + + REQUIRE(segments.size() == 3); + CHECK(segments.at(0).kind == nav::SegmentKind::STRAIGHT); + CHECK(segments.at(1).kind == nav::SegmentKind::TURN); + CHECK(segments.at(2).kind == nav::SegmentKind::STRAIGHT); + CHECK( + static_cast(segments.at(0).length) == + doctest::Approx(static_cast(start_distance + cell_size + shape.pre)) + ); + CHECK(static_cast(segments.at(1).length) == doctest::Approx(static_cast(-shape.length()))); + CHECK( + static_cast(segments.at(2).length) == + doctest::Approx(static_cast(shape.post + 2 * cell_size + 0.09F)) + ); + CHECK(same_pose(end_of(segments.at(0), fast_profile), segments.at(1).start)); + CHECK(same_pose(end_of(segments.at(1), fast_profile), segments.at(2).start)); + CHECK(same_pose( + end_of(segments.at(2), fast_profile), + {.position = {.x = cell_size + 2 * cell_size + 0.09F, .y = 2.5F * cell_size}, .orientation = 0.0F} + )); + } + + TEST_CASE("chains the segments of a planned route into the goal") { + const MazeText text = read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / "japan2017ef.txt"); + const nav::Route route = plan_route(text, fast_profile); + const float cell_size = reference_robot.maze.cell_size; + + const std::vector segments = compile(route); + + std::size_t turns = 0; + + for (std::size_t index = 0; index < segments.size(); index++) { + CAPTURE(index); + const nav::Segment& segment = segments.at(index); + + if (segment.kind == nav::SegmentKind::TURN) { + turns++; + } else { + CHECK(segment.length > 0.0F); + } + + if (index + 1 < segments.size()) { + CHECK(same_pose(end_of(segment, fast_profile), segments.at(index + 1).start)); + } + } + + std::size_t route_turns = 0; + + for (const nav::RouteStep& step : route.steps) { + route_turns += step.has_turn ? 1 : 0; + } + + CHECK(turns == route_turns); + + const nav::Pose stop = end_of(segments.back(), fast_profile); + const nav::GridPoint cell{ + .x = static_cast(stop.position.x / cell_size), + .y = static_cast(stop.position.y / cell_size) + }; + bool in_goal = false; + + for (const nav::GridPoint& goal : text.goal) { + in_goal = in_goal or goal == cell; + } + + CHECK(in_goal); + } +} +} // namespace micras::test diff --git a/tests/nav/test_turn_table.cpp b/tests/nav/test_turn_table.cpp new file mode 100644 index 0000000..76e76d4 --- /dev/null +++ b/tests/nav/test_turn_table.cpp @@ -0,0 +1,98 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include + +#include "micras/nav/lattice.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/state.hpp" +#include "micras/nav/turn_table.hpp" +#include "reference_robot.hpp" + +namespace micras::test { +TEST_SUITE("turn_table") { + TEST_CASE("fits every turn in the maze with the normal margin") { + const nav::TurnTable table{ + reference_robot, turn_margin, nav::TurnTable::place(reference_robot, turn_margin, two_bend_designs) + }; + + CHECK(table.is_valid()); + } + + TEST_CASE("fits every turn in the maze with the risky margin") { + const nav::TurnTable table{ + reference_robot, risky_turn_margin, + nav::TurnTable::place(reference_robot, risky_turn_margin, risky_two_bend_designs) + }; + + CHECK(table.is_valid()); + } + + TEST_CASE("clears the walls with every turn of two bends") { + CHECK(nav::TurnTable::clears(reference_robot, turn_margin, two_bend_designs)); + CHECK(nav::TurnTable::clears(reference_robot, risky_turn_margin, risky_two_bend_designs)); + } + + TEST_CASE("rejects the designs of a smaller margin with a larger one") { + CHECK_FALSE(nav::TurnTable::clears(reference_robot, 0.03F, risky_two_bend_designs)); + } + + TEST_CASE("rejects a turn nothing fits") { + std::array designs = two_bend_designs; + designs.front().first_curvature = 0.0F; + + CHECK_FALSE(nav::TurnTable::clears(reference_robot, turn_margin, designs)); + + const nav::TurnTable table{ + reference_robot, turn_margin, nav::TurnTable::place(reference_robot, turn_margin, designs) + }; + CHECK_FALSE(table.is_valid()); + } + + TEST_CASE("ends every turn on the exit node of its primitive") { + const float cell_size = reference_robot.maze.cell_size; + + for (const bool risky : {false, true}) { + nav::RunProfile profile = fast_profile; + profile.risky = risky; + + for (uint8_t index = 0; index < nav::number_of_turns; index++) { + const auto turn = static_cast(index); + CAPTURE(risky); + CAPTURE(index); + + const nav::TurnShape& shape = reference_dynamics().get_turn(profile, turn); + const nav::LatticePose entry{ + .point = {.x = 2, .y = 1}, + .heading = static_cast(nav::get_primitive(turn).diagonal_entry ? 1 : 0), + }; + const nav::LatticePose exit = nav::get_turn_exit(entry, turn, nav::TurnSide::LEFT); + const nav::Pose expected = exit.to_pose(cell_size); + + const auto end = shape.sample(shape.length()); + const nav::Pose curve_start = + entry.to_pose(cell_size).compose({.position = {.x = shape.pre, .y = 0.0F}, .orientation = 0.0F}); + const nav::Pose curve_end = + curve_start.compose({.position = {.x = end.x, .y = end.y}, .orientation = end.heading}); + const nav::Pose reached = + curve_end.compose({.position = {.x = shape.post, .y = 0.0F}, .orientation = 0.0F}); + + CHECK(shape.valid); + CHECK(std::abs(reached.position.x - expected.position.x) < 1e-3F); + CHECK(std::abs(reached.position.y - expected.position.y) < 1e-3F); + CHECK( + std::abs( + std::remainder(reached.orientation - expected.orientation, 2.0F * std::numbers::pi_v) + ) < 1e-3F + ); + } + } + } +} +} // namespace micras::test diff --git a/tests/proxy/test_fmac_filter.cpp b/tests/proxy/test_fmac_filter.cpp new file mode 100644 index 0000000..a168e49 --- /dev/null +++ b/tests/proxy/test_fmac_filter.cpp @@ -0,0 +1,83 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/core/butterworth_filter.hpp" +#include "micras/proxy/fmac_filter.hpp" + +namespace micras::test { +namespace { +const core::ButterworthFilter::Config filter_config{ + .cutoff_frequency = 200.0F, + .sampling_frequency = 1000.0F, +}; +} // namespace + +static std::vector direct_form(const std::vector& inputs) { + const core::ButterworthFilter::Coefficients coefficients = + core::ButterworthFilter::compute_coefficients(filter_config); + std::array input_history{}; + std::array output_history{}; + std::vector outputs; + + for (const double input : inputs) { + input_history = {input, input_history.at(0), input_history.at(1)}; + + double output = 0.0; + + for (std::size_t tap = 0; tap < input_history.size(); tap++) { + output += static_cast(coefficients.feed_forward.at(tap)) * input_history.at(tap); + } + + for (std::size_t tap = 0; tap < output_history.size(); tap++) { + output -= static_cast(coefficients.feedback.at(tap)) * output_history.at(tap); + } + + output_history = {output, output_history.at(0)}; + outputs.push_back(output); + } + + return outputs; +} + +TEST_SUITE("fmac_filter") { + TEST_CASE_FIXTURE(HostBoard, "filters as the Butterworth filter it was designed as") { + proxy::FmacFilter filter{{.fmac = fmac_config, .filter = filter_config}}; + REQUIRE(filter.was_initialized()); + + std::vector inputs; + + inputs.reserve(300); + for (int sample = 0; sample < 300; sample++) { + inputs.push_back((sample < 150 ? 0.5 : -0.25) + 0.1 * std::sin(0.9 * sample)); + } + + const std::vector expected = direct_form(inputs); + + for (std::size_t sample = 0; sample < inputs.size(); sample++) { + CAPTURE(sample); + const auto output = static_cast(filter.update(static_cast(inputs.at(sample)))); + + CHECK(std::abs(output - expected.at(sample)) < 2e-3); + CHECK(static_cast(filter.get_last()) == output); + } + } + + TEST_CASE_FIXTURE(HostBoard, "stays off for a filter whose coefficients q1.15 cannot hold") { + proxy::FmacFilter filter{ + {.fmac = fmac_config, .filter = {.cutoff_frequency = 50.0F, .sampling_frequency = 1000.0F}} + }; + + CHECK_FALSE(filter.was_initialized()); + CHECK(filter.update(0.5F) == 0.0F); + } +} +} // namespace micras::test diff --git a/tests/proxy/test_imu.cpp b/tests/proxy/test_imu.cpp new file mode 100644 index 0000000..fe30f2d --- /dev/null +++ b/tests/proxy/test_imu.cpp @@ -0,0 +1,154 @@ +/** + * @file + */ + +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/clock.hpp" +#include "micras/hal/host/spi_device.hpp" +#include "micras/models/lsm6dsv_model.hpp" +#include "micras/proxy/imu.hpp" + +namespace micras::test { +namespace { +const proxy::Imu::Config imu_config{ + .spi = spi_config, + .gyroscope_mode = LSM6DSV_GY_HIGH_ACCURACY_ODR_MD, + .accelerometer_mode = LSM6DSV_XL_HIGH_ACCURACY_ODR_MD, + .gyroscope_data_rate = LSM6DSV_ODR_HA01_AT_8000Hz, + .accelerometer_data_rate = LSM6DSV_ODR_HA01_AT_8000Hz, + .gyroscope_scale = LSM6DSV_4000dps, + .accelerometer_scale = LSM6DSV_8g, + .gyroscope_filter = LSM6DSV_GY_ULTRA_LIGHT, + .accelerometer_filter = LSM6DSV_XL_MEDIUM, +}; +} // namespace + +static void attach(const proxy::Imu::Config& config, hal::host::SpiDevice& device) { + hal::host::Board::spi_device(config.spi.handle, config.spi.cs_gpio.port, config.spi.cs_gpio.pin, device); +} + +static bool near(float value, double expected, double tolerance) { + const auto actual = static_cast(value); + return actual >= expected - tolerance and actual <= expected + tolerance; +} + +TEST_SUITE("imu") { + TEST_CASE_FIXTURE(HostBoard, "sets the chip up as its configuration says") { + models::Lsm6dsvModel chip; + attach(imu_config, chip); + + const proxy::Imu imu{imu_config}; + + CHECK(imu.was_initialized()); + CHECK(chip.gyroscope_data_rate() == 8000.0); + CHECK(chip.accelerometer_data_rate() == 8000.0); + CHECK((chip.peek(LSM6DSV_CTRL6) & 0x0FU) == LSM6DSV_4000dps); + CHECK((chip.peek(LSM6DSV_CTRL8) & 0x03U) == LSM6DSV_8g); + CHECK((chip.peek(LSM6DSV_CTRL3) & 0x40U) == 0x40); + CHECK((chip.peek(LSM6DSV_CTRL7) & 0x01U) == 0x01); + CHECK((chip.peek(LSM6DSV_CTRL9) & 0x08U) == 0x08); + } + + TEST_CASE_FIXTURE(HostBoard, "waits for the chip to start before configuring it") { + models::Lsm6dsvModel chip; + attach(imu_config, chip); + const uint64_t start = hal::host::Clock::instance().now(); + + const proxy::Imu imu{imu_config}; + const uint64_t elapsed_us = (hal::host::Clock::instance().now() - start) / (SystemCoreClock / 1000000); + + CHECK(imu.was_initialized()); + CHECK(elapsed_us >= 39900); + CHECK(elapsed_us < 41000); + } + + TEST_CASE_FIXTURE(HostBoard, "reads a sample one update after the update that asked for it") { + models::Lsm6dsvModel chip; + attach(imu_config, chip); + proxy::Imu imu{imu_config}; + + chip.push_sample({0.1, -0.2, 3.0}, {1.0, -2.0, 9.80665}); + imu.update(); + CHECK_FALSE(imu.is_new()); + + wait_us(125); + imu.update(); + + REQUIRE(imu.is_new()); + const double gyroscope = chip.gyroscope_sensitivity(); + const double accelerometer = chip.accelerometer_sensitivity(); + CHECK(near(imu.get_angular_velocity(proxy::Imu::Axis::X), 0.1, gyroscope)); + CHECK(near(imu.get_angular_velocity(proxy::Imu::Axis::Y), -0.2, gyroscope)); + CHECK(near(imu.get_angular_velocity(proxy::Imu::Axis::Z), 3.0, gyroscope)); + CHECK(near(imu.get_linear_acceleration(proxy::Imu::Axis::X), 1.0, accelerometer)); + CHECK(near(imu.get_linear_acceleration(proxy::Imu::Axis::Y), -2.0, accelerometer)); + CHECK(near(imu.get_linear_acceleration(proxy::Imu::Axis::Z), 9.80665, accelerometer)); + + wait_us(125); + imu.update(); + CHECK_FALSE(imu.is_new()); + } + + TEST_CASE_FIXTURE(HostBoard, "reads the same motion at another full scale") { + proxy::Imu::Config config = imu_config; + config.gyroscope_scale = LSM6DSV_250dps; + config.accelerometer_scale = LSM6DSV_2g; + models::Lsm6dsvModel chip; + attach(config, chip); + proxy::Imu imu{config}; + + chip.push_sample({0.5, 0.0, -1.5}, {0.0, 3.0, 9.80665}); + imu.update(); + wait_us(125); + imu.update(); + + REQUIRE(imu.is_new()); + const double gyroscope = chip.gyroscope_sensitivity(); + const double accelerometer = chip.accelerometer_sensitivity(); + CHECK((chip.peek(LSM6DSV_CTRL6) & 0x0FU) == LSM6DSV_250dps); + CHECK(near(imu.get_angular_velocity(proxy::Imu::Axis::X), 0.5, gyroscope)); + CHECK(near(imu.get_angular_velocity(proxy::Imu::Axis::Z), -1.5, gyroscope)); + CHECK(near(imu.get_linear_acceleration(proxy::Imu::Axis::Y), 3.0, accelerometer)); + CHECK(near(imu.get_linear_acceleration(proxy::Imu::Axis::Z), 9.80665, accelerometer)); + } + + TEST_CASE_FIXTURE(HostBoard, "saturates a motion beyond the full scale it was set to") { + proxy::Imu::Config config = imu_config; + config.gyroscope_scale = LSM6DSV_250dps; + models::Lsm6dsvModel chip; + attach(config, chip); + proxy::Imu imu{config}; + + chip.push_sample({10.0, 0.0, 0.0}, {0.0, 0.0, 9.80665}); + imu.update(); + wait_us(125); + imu.update(); + + REQUIRE(imu.is_new()); + CHECK(near(imu.get_angular_velocity(proxy::Imu::Axis::X), 32767 * chip.gyroscope_sensitivity(), 1e-3)); + CHECK(imu.get_angular_velocity(proxy::Imu::Axis::X) < 5.1F); + } + + TEST_CASE_FIXTURE(HostBoard, "fails without its chip") { + const proxy::Imu imu{imu_config}; + + CHECK_FALSE(imu.was_initialized()); + } + + TEST_CASE_FIXTURE(HostBoard, "fails in the wrong SPI mode") { + proxy::Imu::Config config = imu_config; + config.spi.clock_polarity = SPI_POLARITY_LOW; + models::Lsm6dsvModel chip; + attach(config, chip); + + const proxy::Imu imu{config}; + + CHECK_FALSE(imu.was_initialized()); + } +} +} // namespace micras::test diff --git a/tests/proxy/test_rotary_sensor.cpp b/tests/proxy/test_rotary_sensor.cpp new file mode 100644 index 0000000..1dcada3 --- /dev/null +++ b/tests/proxy/test_rotary_sensor.cpp @@ -0,0 +1,117 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include + +#include "crc.h" +#include "host_fixture.hpp" +#include "micras/hal/crc.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/spi_device.hpp" +#include "micras/models/as5047u_model.hpp" +#include "micras/proxy/rotary_sensor.hpp" +#include "spi.h" + +namespace micras::test { +namespace { +const proxy::RotarySensor::Registers registers{ + .disable = {{.UVW_off = 1, .ABI_off = 0, .na = 0, .FILTER_disable = 0}}, + .zposm = {{.ZPOSM = 0}}, + .zposl = {{.ZPOSL = 0, .Dia1_en = 0, .Dia2_en = 0}}, + .settings1 = {{.K_max = 0, .K_min = 0, .Dia3_en = 0, .Dia4_en = 0}}, + .settings2 = {{ + .IWIDTH = 0, + .NOISESET = 0, + .DIR = 0, + .UVW_ABI = 0, + .DAECDIS = 0, + .ABI_DEC = 0, + .Data_select = 0, + .PWMon = 0, + }}, + .settings3 = {{.UVWPP = 0, .HYS = 0, .ABIRES = 0b100}}, + .ecc = {{.ECC_chsum = 0, .ECC_en = 0}}, +}; +} // namespace + +static proxy::RotarySensor::Config rotary_sensor_config() { + return { + .spi = + { + .init_function = MX_SPI3_Init, + .handle = &hspi3, + .cs_gpio = spi_config.cs_gpio, + .timeout = 2, + .clock_polarity = SPI_POLARITY_LOW, + .clock_phase = SPI_PHASE_2EDGE, + }, + .encoder = encoder_config, + .crc = crc_config, + .registers = registers, + }; +} + +static void attach(const proxy::RotarySensor::Config& config, hal::host::SpiDevice& device) { + hal::host::Board::spi_device(config.spi.handle, config.spi.cs_gpio.port, config.spi.cs_gpio.pin, device); +} + +TEST_SUITE("rotary_sensor") { + TEST_CASE_FIXTURE(HostBoard, "sets the chip up and reads its resolution back") { + models::As5047uModel chip; + attach(rotary_sensor_config(), chip); + + const proxy::RotarySensor sensor{rotary_sensor_config()}; + + CHECK(sensor.was_initialized()); + CHECK(sensor.get_resolution() == 16384); + CHECK(chip.peek(models::As5047uModel::settings3_address) == registers.settings3.raw); + CHECK(chip.peek(models::As5047uModel::disable_address) == registers.disable.raw); + CHECK(chip.peek(models::As5047uModel::errfl_address) == 0); + } + + TEST_CASE_FIXTURE(HostBoard, "reads the position the encoder counted") { + models::As5047uModel chip; + attach(rotary_sensor_config(), chip); + const proxy::RotarySensor sensor{rotary_sensor_config()}; + + hal::host::Board::encoder(rotary_sensor_config().encoder.handle).count = 4096; + + CHECK( + static_cast(sensor.get_position()) == + doctest::Approx(static_cast(std::numbers::pi_v / 2.0F)) + ); + } + + TEST_CASE_FIXTURE(HostBoard, "fails when the chip rejects the frames for their CRC") { + CRC_HandleTypeDef wrong_crc = hcrc; + wrong_crc.Init.InitValue = 0x00; + proxy::RotarySensor::Config config = rotary_sensor_config(); + config.crc.handle = &wrong_crc; + models::As5047uModel chip; + attach(config, chip); + + const proxy::RotarySensor sensor{config}; + + CHECK_FALSE(sensor.was_initialized()); + CHECK(chip.peek(models::As5047uModel::settings3_address) == 0); + CHECK((chip.peek(models::As5047uModel::errfl_address) & models::As5047uModel::crc_error) != 0); + } + + TEST_CASE_FIXTURE(HostBoard, "computes the frame CRC as the CRC unit does") { + hal::Crc crc{crc_config}; + + for (const std::array bytes : std::vector>{ + {0x00, 0x00}, {0x40, 0x01}, {0x40, 0x1A}, {0x00, 0x80}, {0x3F, 0xFF}, {0xC0, 0x15} + }) { + CAPTURE(bytes); + CHECK(models::As5047uModel::crc(bytes.at(0), bytes.at(1)) == (crc.calculate(bytes) ^ 0xFFU)); + } + } +} +} // namespace micras::test diff --git a/tests/proxy/test_storage.cpp b/tests/proxy/test_storage.cpp new file mode 100644 index 0000000..1555f53 --- /dev/null +++ b/tests/proxy/test_storage.cpp @@ -0,0 +1,143 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/core/serializable.hpp" +#include "micras/core/variable_pool.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/proxy/storage.hpp" + +namespace micras::test { +namespace { +class Blob : public core::ISerializable { +public: + Blob() = default; + + explicit Blob(std::vector data) : data{std::move(data)} { } + + std::vector serialize() const override { return this->data; } + + void deserialize(const uint8_t* buffer, uint16_t size) override { + this->data.assign(buffer, std::next(buffer, size)); + } + + const std::vector& get() const { return this->data; } + +private: + std::vector data; +}; + +const proxy::Storage::Config storage_config{.start_sector = 0, .number_of_sectors = 1}; + +class StoredPool : public HostBoard { +public: + StoredPool() { + core::TVariablePool<8> pool; + float gain = 3.25F; + uint8_t profile = 7; + bool flag = true; + float transient = 1.0F; + Blob maze{{9, 8, 7, 6, 5}}; + + pool.add("model/", "kv", gain, {.persist = true}); + pool.add("", "run_profile", profile, {.persist = true}); + pool.add("", "flag", flag, {.persist = true}); + pool.add("", "transient", transient, {.stream = true}); + pool.add("", "maze", maze, {.persist = true}); + + proxy::Storage storage{storage_config}; + CHECK_FALSE(storage.is_valid()); + CHECK(storage.restore(pool) == 0); + CHECK(storage.save(pool)); + CHECK(storage.is_valid()); + } +}; +} // namespace + +TEST_SUITE("storage") { + TEST_CASE_FIXTURE(StoredPool, "restores every persistent variable and leaves the others alone") { + core::TVariablePool<8> pool; + float gain = 0.0F; + uint8_t profile = 0; + bool flag = false; + float transient = 42.0F; + Blob maze; + + pool.add("model/", "kv", gain, {.persist = true}); + pool.add("", "run_profile", profile, {.persist = true}); + pool.add("", "flag", flag, {.persist = true}); + pool.add("", "transient", transient, {.stream = true}); + pool.add("", "maze", maze, {.persist = true}); + + proxy::Storage storage{storage_config}; + CHECK(storage.is_valid()); + CHECK(storage.restore(pool) == 4); + CHECK(gain == 3.25F); + CHECK(profile == 7); + CHECK(flag); + CHECK(maze.get() == std::vector{9, 8, 7, 6, 5}); + CHECK(transient == 42.0F); + } + + TEST_CASE_FIXTURE(StoredPool, "finds the variables it knows in a reordered pool") { + core::TVariablePool<8> pool; + uint8_t profile = 0; + float gain = 0.0F; + float fresh = 1.0F; + + pool.add("", "fresh", fresh, {.persist = true}); + pool.add("", "run_profile", profile, {.persist = true}); + pool.add("model/", "kv", gain, {.persist = true}); + + proxy::Storage storage{storage_config}; + CHECK(storage.restore(pool) == 2); + CHECK(profile == 7); + CHECK(gain == 3.25F); + CHECK(fresh == 1.0F); + } + + TEST_CASE_FIXTURE(StoredPool, "refuses a variable whose type changed instead of misreading it") { + core::TVariablePool<8> pool; + uint32_t gain = 0; + pool.add("model/", "kv", gain, {.persist = true}); + + proxy::Storage storage{storage_config}; + CHECK(storage.restore(pool) == 0); + CHECK(gain == 0); + } + + TEST_CASE_FIXTURE(StoredPool, "leaves no image to restore after a write the power cut off") { + core::TVariablePool<8> pool; + float gain = 99.0F; + pool.add("model/", "kv", gain, {.persist = true}); + + proxy::Storage storage{storage_config}; + hal::host::Board::flash().write_budget = 0; + CHECK_FALSE(storage.save(pool)); + + const proxy::Storage after{storage_config}; + CHECK_FALSE(after.is_valid()); + } + + TEST_CASE_FIXTURE(HostBoard, "treats a body written without its header as never saved") { + core::TVariablePool<8> pool; + float gain = 1.0F; + pool.add("model/", "kv", gain, {.persist = true}); + + proxy::Storage storage{storage_config}; + hal::host::Board::flash().write_budget = 1; + CHECK_FALSE(storage.save(pool)); + + const proxy::Storage after{storage_config}; + CHECK_FALSE(after.is_valid()); + } +} +} // namespace micras::test diff --git a/tests/support/host_board/CMakeLists.txt b/tests/support/host_board/CMakeLists.txt new file mode 100644 index 0000000..8d9617f --- /dev/null +++ b/tests/support/host_board/CMakeLists.txt @@ -0,0 +1,16 @@ +# The fake Cube layer of the reference projects. micras_hal links it as its board target, and it links +# micras_hal back for the host backend's headers and registry, a cycle of two static libraries that +# CMake links by repeating them. +add_library(micras_lib_host_board STATIC + handles.cpp +) + +target_include_directories(micras_lib_host_board PUBLIC + . +) + +target_link_libraries(micras_lib_host_board PUBLIC + micras::hal +) + +micras_apply_warnings(micras_lib_host_board WERROR ${MICRAS_LIB_WERROR}) diff --git a/tests/support/host_board/adc.h b/tests/support/host_board/adc.h new file mode 100644 index 0000000..e4b5114 --- /dev/null +++ b/tests/support/host_board/adc.h @@ -0,0 +1,22 @@ +/** + * @file + * + * @brief Fake STM32CubeMX adc.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_ADC_H +#define MICRAS_LIB_TESTS_HOST_BOARD_ADC_H + +#include "main.h" + +/** + * @brief Handle hadc3, which converts the internal reference and the temperature sensor. + */ +extern ADC_HandleTypeDef hadc3; + +/** + * @brief Configure what MX_ADC3_Init configures. + */ +void MX_ADC3_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_ADC_H diff --git a/tests/support/host_board/crc.h b/tests/support/host_board/crc.h new file mode 100644 index 0000000..c30a1e7 --- /dev/null +++ b/tests/support/host_board/crc.h @@ -0,0 +1,22 @@ +/** + * @file + * + * @brief Fake STM32CubeMX crc.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_CRC_H +#define MICRAS_LIB_TESTS_HOST_BOARD_CRC_H + +#include "main.h" + +/** + * @brief Handle hcrc, set up for the AS5047U frames: 8 bits, polynomial 0x1D, initial value 0xC4. + */ +extern CRC_HandleTypeDef hcrc; + +/** + * @brief Configure what MX_CRC_Init configures. + */ +void MX_CRC_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_CRC_H diff --git a/tests/support/host_board/dma.h b/tests/support/host_board/dma.h new file mode 100644 index 0000000..88d9ebb --- /dev/null +++ b/tests/support/host_board/dma.h @@ -0,0 +1,17 @@ +/** + * @file + * + * @brief Fake STM32CubeMX dma.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_DMA_H +#define MICRAS_LIB_TESTS_HOST_BOARD_DMA_H + +#include "main.h" + +/** + * @brief Configure what MX_DMA_Init configures, which the host needs nothing of. + */ +void MX_DMA_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_DMA_H diff --git a/tests/support/host_board/fmac.h b/tests/support/host_board/fmac.h new file mode 100644 index 0000000..5ac2057 --- /dev/null +++ b/tests/support/host_board/fmac.h @@ -0,0 +1,22 @@ +/** + * @file + * + * @brief Fake STM32CubeMX fmac.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_FMAC_H +#define MICRAS_LIB_TESTS_HOST_BOARD_FMAC_H + +#include "main.h" + +/** + * @brief Handle hfmac. + */ +extern FMAC_HandleTypeDef hfmac; + +/** + * @brief Configure what MX_FMAC_Init configures. + */ +void MX_FMAC_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_FMAC_H diff --git a/tests/support/host_board/gpio.h b/tests/support/host_board/gpio.h new file mode 100644 index 0000000..198fdf2 --- /dev/null +++ b/tests/support/host_board/gpio.h @@ -0,0 +1,17 @@ +/** + * @file + * + * @brief Fake STM32CubeMX gpio.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_GPIO_H +#define MICRAS_LIB_TESTS_HOST_BOARD_GPIO_H + +#include "main.h" + +/** + * @brief Configure what MX_GPIO_Init configures: the labels of the pins. + */ +void MX_GPIO_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_GPIO_H diff --git a/tests/support/host_board/handles.cpp b/tests/support/host_board/handles.cpp new file mode 100644 index 0000000..890f2e1 --- /dev/null +++ b/tests/support/host_board/handles.cpp @@ -0,0 +1,140 @@ +/** + * @file + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/hal/host/board.hpp" + +namespace { +constexpr uint32_t timer_clock{200000000}; + +constexpr uint32_t spi3_kernel_clock{125000000}; + +// NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) +TIM_TypeDef tim2_registers{}; +TIM_TypeDef tim5_registers{}; +TIM_TypeDef tim8_registers{}; +TIM_TypeDef tim15_registers{}; +SPI_TypeDef spi3_registers{}; +} // namespace + +// NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) + +static void init_timer( + TIM_HandleTypeDef& handle, TIM_TypeDef& registers, const char* name, uint32_t prescaler, uint32_t counter_mode, + uint32_t period +) { + handle.Instance = ®isters; + handle.Init = {.Prescaler = prescaler, .CounterMode = counter_mode, .Period = period}; + registers = {.CR1 = counter_mode, .CCER = 0, .PSC = prescaler, .ARR = period, .kernel_clock = timer_clock}; + handle.State = HAL_TIM_STATE_READY; + micras::hal::host::Board::name_handle(&handle, name); +} + +// NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) +uint32_t SystemCoreClock{400000000}; + +GPIO_TypeDef GPIOA_instance{'A'}; +GPIO_TypeDef GPIOB_instance{'B'}; +GPIO_TypeDef GPIOC_instance{'C'}; +GPIO_TypeDef GPIOD_instance{'D'}; + +ADC_HandleTypeDef hadc3{}; +CRC_HandleTypeDef hcrc{}; +FMAC_HandleTypeDef hfmac{}; +SPI_HandleTypeDef hspi3{}; +TIM_HandleTypeDef htim2{}; +TIM_HandleTypeDef htim5{}; +TIM_HandleTypeDef htim8{}; +TIM_HandleTypeDef htim15{}; +UART_HandleTypeDef huart4{}; + +// NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) + +extern "C" { +void SystemClock_Config() { } +} + +void MX_GPIO_Init() { + using micras::hal::host::Board; + Board::name_gpio(Encoder_A_GPIO_Port, Encoder_A_Pin, "Encoder_A"); + Board::name_gpio(Encoder_B_GPIO_Port, Encoder_B_Pin, "Encoder_B"); + Board::name_gpio(PWM_GPIO_Port, PWM_Pin, "PWM"); + Board::name_gpio(Button_GPIO_Port, Button_Pin, "Button"); + Board::name_gpio(Status_LED_GPIO_Port, Status_LED_Pin, "Status_LED"); + Board::name_gpio(PWM_DMA_GPIO_Port, PWM_DMA_Pin, "PWM_DMA"); + Board::name_gpio(SPI_CSn_GPIO_Port, SPI_CSn_Pin, "SPI_CSn"); +} + +void MX_DMA_Init() { } + +void MX_ADC3_Init() { + hadc3.Init.NbrOfConversion = 2; + hadc3.State = HAL_ADC_STATE_READY; + micras::hal::host::Board::name_handle(&hadc3, "hadc3"); +} + +void MX_CRC_Init() { + hcrc.Init = { + .DefaultPolynomialUse = DEFAULT_POLYNOMIAL_DISABLE, + .DefaultInitValueUse = DEFAULT_INIT_VALUE_DISABLE, + .GeneratingPolynomial = 0x1D, + .CRCLength = CRC_POLYLENGTH_8B, + .InitValue = 0xC4, + .InputDataInversionMode = CRC_INPUTDATA_INVERSION_NONE, + .OutputDataInversionMode = CRC_OUTPUTDATA_INVERSION_DISABLE, + }; + hcrc.InputDataFormat = CRC_INPUTDATA_FORMAT_BYTES; + micras::hal::host::Board::name_handle(&hcrc, "hcrc"); +} + +void MX_FMAC_Init() { + hfmac.State = HAL_FMAC_STATE_READY; + micras::hal::host::Board::name_handle(&hfmac, "hfmac"); +} + +void MX_SPI3_Init() { + spi3_registers.kernel_clock = spi3_kernel_clock; + hspi3.Instance = &spi3_registers; + hspi3.Init = { + .CLKPolarity = SPI_POLARITY_HIGH, + .CLKPhase = SPI_PHASE_2EDGE, + .BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32, + }; + hspi3.State = HAL_SPI_STATE_READY; + micras::hal::host::Board::name_handle(&hspi3, "hspi3"); +} + +void MX_TIM2_Init() { + init_timer(htim2, tim2_registers, "htim2", 0, TIM_COUNTERMODE_UP, 0xFFFFFFFF); +} + +void MX_TIM5_Init() { + init_timer(htim5, tim5_registers, "htim5", 0, TIM_COUNTERMODE_UP, 0xFFFFFFFF); +} + +void MX_TIM8_Init() { + init_timer(htim8, tim8_registers, "htim8", 0, TIM_COUNTERMODE_UP, 249); +} + +void MX_TIM15_Init() { + init_timer(htim15, tim15_registers, "htim15", 199, TIM_COUNTERMODE_UP, 999); +} + +void MX_UART4_Init() { + huart4.Init.BaudRate = 115200; + huart4.gState = HAL_UART_STATE_READY; + huart4.RxState = HAL_UART_STATE_READY; + micras::hal::host::Board::name_handle(&huart4, "huart4"); +} diff --git a/tests/support/host_board/main.h b/tests/support/host_board/main.h new file mode 100644 index 0000000..4c35429 --- /dev/null +++ b/tests/support/host_board/main.h @@ -0,0 +1,83 @@ +/** + * @file + * + * @brief Fake STM32CubeMX main.h of the reference projects, for the host tests. + * + * @note The host counterpart of reference/h7: the same peripherals under the + * same handle and pin names, one instance each, with the values its + * CubeMX project generates. The independent watchdog has no handle here, + * since the HAL drives its registers directly. The configurations the + * tests build from them compile unchanged against the generated tree of + * the reference. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_MAIN_H +#define MICRAS_LIB_TESTS_HOST_BOARD_MAIN_H + +#include + +#include "stm32_host.h" + +/***************************************** + * Microcontroller + *****************************************/ + +/** + * @brief Core clock after SystemClock_Config: HSI 64 MHz / PLLM 4 * PLLN 50 / PLLP 2. + */ +extern uint32_t SystemCoreClock; + +/** + * @brief Flash geometry of the STM32H725xG: one bank of eight 128 KB sectors, 256-bit flash words. + */ +///@{ +#define FLASH_NB_32BITWORD_IN_FLASHWORD 8U +inline constexpr uint32_t FLASH_SECTOR_SIZE{0x00020000U}; +inline constexpr uint32_t FLASH_SECTOR_TOTAL{8U}; +///@} + +/***************************************** + * GPIO ports and pins + *****************************************/ + +/** + * @brief The GPIO ports of the reference, one instance each. + */ +///@{ +extern GPIO_TypeDef GPIOA_instance; +extern GPIO_TypeDef GPIOB_instance; +extern GPIO_TypeDef GPIOC_instance; +extern GPIO_TypeDef GPIOD_instance; +///@} + +/** + * @brief The CMSIS names of the GPIO ports, the addresses of their instances. + */ +///@{ +#define GPIOA (&GPIOA_instance) +#define GPIOB (&GPIOB_instance) +#define GPIOC (&GPIOC_instance) +#define GPIOD (&GPIOD_instance) +///@} + +/** + * @brief The pin labels of the reference's CubeMX project: each pin and its port. + */ +///@{ +#define Encoder_A_Pin GPIO_PIN_0 +#define Encoder_A_GPIO_Port GPIOA +#define Encoder_B_Pin GPIO_PIN_1 +#define Encoder_B_GPIO_Port GPIOA +#define PWM_Pin GPIO_PIN_2 +#define PWM_GPIO_Port GPIOA +#define Button_Pin GPIO_PIN_7 +#define Button_GPIO_Port GPIOA +#define Status_LED_Pin GPIO_PIN_12 +#define Status_LED_GPIO_Port GPIOB +#define PWM_DMA_Pin GPIO_PIN_6 +#define PWM_DMA_GPIO_Port GPIOC +#define SPI_CSn_Pin GPIO_PIN_2 +#define SPI_CSn_GPIO_Port GPIOD +///@} + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_MAIN_H diff --git a/tests/support/host_board/spi.h b/tests/support/host_board/spi.h new file mode 100644 index 0000000..bf4e948 --- /dev/null +++ b/tests/support/host_board/spi.h @@ -0,0 +1,22 @@ +/** + * @file + * + * @brief Fake STM32CubeMX spi.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_SPI_H +#define MICRAS_LIB_TESTS_HOST_BOARD_SPI_H + +#include "main.h" + +/** + * @brief Handle hspi3, a full duplex master in SPI mode 3. + */ +extern SPI_HandleTypeDef hspi3; + +/** + * @brief Configure what MX_SPI3_Init configures. + */ +void MX_SPI3_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_SPI_H diff --git a/tests/support/host_board/tim.h b/tests/support/host_board/tim.h new file mode 100644 index 0000000..cc2ba65 --- /dev/null +++ b/tests/support/host_board/tim.h @@ -0,0 +1,52 @@ +/** + * @file + * + * @brief Fake STM32CubeMX tim.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_TIM_H +#define MICRAS_LIB_TESTS_HOST_BOARD_TIM_H + +#include "main.h" + +/** + * @brief Handle htim2, a free-running 32-bit timer. + */ +extern TIM_HandleTypeDef htim2; + +/** + * @brief Handle htim5, a timer in encoder mode. + */ +extern TIM_HandleTypeDef htim5; + +/** + * @brief Handle htim8, whose first channel a DMA stream feeds. + */ +extern TIM_HandleTypeDef htim8; + +/** + * @brief Handle htim15, whose first channel is a PWM output. + */ +extern TIM_HandleTypeDef htim15; + +/** + * @brief Configure what MX_TIM2_Init configures. + */ +void MX_TIM2_Init(); + +/** + * @brief Configure what MX_TIM5_Init configures. + */ +void MX_TIM5_Init(); + +/** + * @brief Configure what MX_TIM8_Init configures. + */ +void MX_TIM8_Init(); + +/** + * @brief Configure what MX_TIM15_Init configures. + */ +void MX_TIM15_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_TIM_H diff --git a/tests/support/host_board/usart.h b/tests/support/host_board/usart.h new file mode 100644 index 0000000..18f52c5 --- /dev/null +++ b/tests/support/host_board/usart.h @@ -0,0 +1,22 @@ +/** + * @file + * + * @brief Fake STM32CubeMX usart.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_USART_H +#define MICRAS_LIB_TESTS_HOST_BOARD_USART_H + +#include "main.h" + +/** + * @brief Handle huart4, with DMA on receive and transmit. + */ +extern UART_HandleTypeDef huart4; + +/** + * @brief Configure what MX_UART4_Init configures. + */ +void MX_UART4_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_USART_H diff --git a/tests/support/host_fixture.hpp b/tests/support/host_fixture.hpp new file mode 100644 index 0000000..62bc947 --- /dev/null +++ b/tests/support/host_fixture.hpp @@ -0,0 +1,186 @@ +/** + * @file + * + * @brief The peripherals of the reference projects as the host tests configure them, and the fixture + * that starts every test from a fresh board. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_FIXTURE_HPP +#define MICRAS_LIB_TESTS_HOST_FIXTURE_HPP + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/hal/adc_dma.hpp" +#include "micras/hal/crc.hpp" +#include "micras/hal/encoder.hpp" +#include "micras/hal/fmac.hpp" +#include "micras/hal/gpio.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/clock.hpp" +#include "micras/hal/pwm.hpp" +#include "micras/hal/pwm_dma.hpp" +#include "micras/hal/spi.hpp" +#include "micras/hal/timer.hpp" +#include "micras/hal/uart_dma.hpp" + +namespace micras::test { +/** + * @brief The output pin of the reference projects. + */ +inline const hal::Gpio::Config status_led_config{ + .port = Status_LED_GPIO_Port, + .pin = Status_LED_Pin, +}; + +/** + * @brief The input pin of the reference projects. + */ +inline const hal::Gpio::Config button_config{ + .port = Button_GPIO_Port, + .pin = Button_Pin, +}; + +/** + * @brief The SPI bus and its chip select, in SPI mode 3. + */ +inline const hal::Spi::Config spi_config{ + .init_function = MX_SPI3_Init, + .handle = &hspi3, + .cs_gpio = {.port = SPI_CSn_GPIO_Port, .pin = SPI_CSn_Pin}, + .timeout = 2, + .clock_polarity = SPI_POLARITY_HIGH, + .clock_phase = SPI_PHASE_2EDGE, +}; + +/** + * @brief A second device on the same bus, in SPI mode 1, behind the output pin used as a chip select. + */ +inline const hal::Spi::Config second_spi_config{ + .init_function = MX_SPI3_Init, + .handle = &hspi3, + .cs_gpio = status_led_config, + .timeout = 2, + .clock_polarity = SPI_POLARITY_LOW, + .clock_phase = SPI_PHASE_2EDGE, +}; + +/** + * @brief The PWM channel. + */ +inline const hal::Pwm::Config pwm_config{ + .init_function = MX_TIM15_Init, + .handle = &htim15, + .timer_channel = TIM_CHANNEL_1, + .inverted = false, +}; + +/** + * @brief The PWM channel a DMA stream feeds. + */ +inline const hal::PwmDma::Config pwm_dma_config{ + .init_function = MX_TIM8_Init, + .handle = &htim8, + .timer_channel = TIM_CHANNEL_1, +}; + +/** + * @brief The timer in encoder mode. + */ +inline const hal::Encoder::Config encoder_config{ + .init_function = MX_TIM5_Init, + .handle = &htim5, + .timer_channel = TIM_CHANNEL_ALL, +}; + +/** + * @brief The UART. + */ +inline const hal::UartDma::Config uart_config{ + .init_function = MX_UART4_Init, + .handle = &huart4, +}; + +/** + * @brief The ADC, twelve bits referenced to 3.3 V. + */ +inline const hal::AdcDma::Config adc_config{ + .init_function = MX_ADC3_Init, + .handle = &hadc3, + .max_reading = 4095, + .reference_voltage = 3.3F, +}; + +/** + * @brief The CRC unit, set up for the AS5047U frames. + */ +inline const hal::Crc::Config crc_config{ + .handle = &hcrc, +}; + +/** + * @brief The filter math accelerator. + */ +inline const hal::Fmac::Config fmac_config{ + .init_function = MX_FMAC_Init, + .handle = &hfmac, +}; + +/** + * @brief Starts a test from the state the board has after a reset. + * + * @note Forgets every port and rewinds the clock, as the host backend asks of independent runs in one + * process, and returns every handle to its reset state, so that each driver runs its init function + * again. The pins are named, and the SPI bus initialized, before any driver creates a port for them, + * since a port takes its name when it is created. + */ +class HostBoard { +public: + /** + * @brief Reset the board, the clock and the handles. + */ + HostBoard() { + hal::host::Board::reset(); + hal::host::Clock::instance().reset(); + hal::host::Clock::instance().configure(SystemCoreClock / 1000000); + hal::Timer::init(); + + hadc3 = {}; + hcrc = {}; + hfmac = {}; + hspi3 = {}; + htim2 = {}; + htim5 = {}; + htim8 = {}; + htim15 = {}; + huart4 = {}; + + MX_GPIO_Init(); + MX_DMA_Init(); + MX_CRC_Init(); + MX_SPI3_Init(); + } + + /** + * @brief Advance the host clock as a firmware waiting that long does, one timer read at a time. + * + * @param microseconds How long to wait. + */ + static void wait_us(uint32_t microseconds) { + const uint32_t start = hal::Timer::get_counter(); + + while (hal::Timer::to_microseconds(hal::Timer::get_counter() - start) < microseconds) { } + } +}; +} // namespace micras::test + +#endif // MICRAS_LIB_TESTS_HOST_FIXTURE_HPP diff --git a/tests/support/main.cpp b/tests/support/main.cpp new file mode 100644 index 0000000..8791533 --- /dev/null +++ b/tests/support/main.cpp @@ -0,0 +1,6 @@ +/** + * @file + */ + +#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN +#include diff --git a/tests/support/maze_reader.hpp b/tests/support/maze_reader.hpp new file mode 100644 index 0000000..e67770a --- /dev/null +++ b/tests/support/maze_reader.hpp @@ -0,0 +1,145 @@ +/** + * @file + * + * @brief Reader of the text mazes the simulator draws and the nav tests plan on. + */ + +#ifndef MICRAS_LIB_TESTS_MAZE_READER_HPP +#define MICRAS_LIB_TESTS_MAZE_READER_HPP + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze.hpp" + +namespace micras::test { +/** + * @brief The walls of a maze, with its start and its goal. + * + * @note The text has a line of posts and horizontal walls (`o---o`) above and below every row of + * cells, and a line of vertical walls (`|`) through every row, the top line first. A cell is four + * characters wide, with an `S` in the start cell and a `G` in every goal cell. + */ +struct MazeText { + /** + * @brief Size of the maze in cells. + */ + ///@{ + uint8_t width{}; + uint8_t height{}; + ///@} + + /** + * @brief Every wall of the maze, once: the right and upper wall of every cell, and the left and + * lower walls of the border, each with whether it is present. + */ + std::vector> walls; + + /** + * @brief Cell marked as the start. + */ + nav::GridPoint start{}; + + /** + * @brief Cells marked as the goal. + */ + std::vector goal; +}; + +/** + * @brief Read a maze from its text. + * + * @param path Path of the text file. + * @return The maze. + */ +inline MazeText read_maze(const std::filesystem::path& path) { + std::ifstream file{path}; + std::vector lines; + + for (std::string line; std::getline(file, line);) { + lines.push_back(line); + } + + if (lines.size() < 3 or lines.size() % 2 == 0 or lines.front().size() < 5 or lines.front().size() % 4 != 1) { + throw std::runtime_error{"not a maze: " + path.string()}; + } + + MazeText maze{ + .width = static_cast(lines.front().size() / 4), + .height = static_cast(lines.size() / 2), + .walls = {}, + .start = {}, + .goal = {}, + }; + + const auto character = [&lines](std::size_t row, std::size_t column) { + const std::string& line = lines.at(row); + return column < line.size() ? line.at(column) : ' '; + }; + + for (uint8_t y = 0; y < maze.height; y++) { + const std::size_t row = 2 * static_cast(maze.height - 1 - y) + 1; + + for (uint8_t x = 0; x < maze.width; x++) { + const std::size_t column = 4 * static_cast(x); + const nav::GridPoint cell{.x = x, .y = y}; + + maze.walls.emplace_back( + nav::GridPose{.position = cell, .orientation = nav::Side::RIGHT}, character(row, column + 4) == '|' + ); + maze.walls.emplace_back( + nav::GridPose{.position = cell, .orientation = nav::Side::UP}, character(row - 1, column + 2) == '-' + ); + + if (x == 0) { + maze.walls.emplace_back( + nav::GridPose{.position = cell, .orientation = nav::Side::LEFT}, character(row, column) == '|' + ); + } + + if (y == 0) { + maze.walls.emplace_back( + nav::GridPose{.position = cell, .orientation = nav::Side::DOWN}, + character(row + 1, column + 2) == '-' + ); + } + + const char mark = character(row, column + 2); + + if (mark == 'S') { + maze.start = cell; + } else if (mark == 'G') { + maze.goal.push_back(cell); + } + } + } + + return maze; +} + +/** + * @brief Make every wall of a maze known to a map of the same size. + * + * @tparam width Width of the maze in cells. + * @tparam height Height of the maze in cells. + * @param text The maze read from its text. + * @param maze The map to fill. + */ +template +void fill_maze(const MazeText& text, nav::TMaze& maze) { + for (const auto& [wall, present] : text.walls) { + maze.set_wall(wall, present); + } + + maze.flood(maze.get_goal()); +} +} // namespace micras::test + +#endif // MICRAS_LIB_TESTS_MAZE_READER_HPP