diff --git a/.gitignore b/.gitignore index d6efb9cf7..eb4610723 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,14 @@ +# PyCharm and CLion +/.idea/* +!/.idea/runConfigurations +!/.idea/cmake.xml +!/.idea/codeStyles + +# Eclipse .cproject .project .settings .metadata /build* +/cmake-build* diff --git a/.idea/codeStyles/Project.xml b/.idea/codeStyles/Project.xml new file mode 100644 index 000000000..0f3b1a4bf --- /dev/null +++ b/.idea/codeStyles/Project.xml @@ -0,0 +1,14 @@ + + + + + + + + + + \ No newline at end of file diff --git a/.idea/codeStyles/codeStyleConfig.xml b/.idea/codeStyles/codeStyleConfig.xml new file mode 100644 index 000000000..79ee123c2 --- /dev/null +++ b/.idea/codeStyles/codeStyleConfig.xml @@ -0,0 +1,5 @@ + + + + \ No newline at end of file diff --git a/CHANGELOG.md b/CHANGELOG.md index 335c0f7bb..f83c5078f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,170 @@ and this project adheres to [Semantic Versioning](http://semver.org/). # [unreleased] +# [v5.0.0] 25.07.2022 + +## Changes + +- Renamed auto-formatting script to `auto-formatter.sh` and made it more robust. + If `cmake-format` is installed, it will also auto-format the `CMakeLists.txt` files now. + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/625 + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/626 +- Bump C++ required version to C++17. Every project which uses the FSFW and every modern + compiler supports it + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/622 +- New CMake option `FSFW_HAL_LINUX_ADD_LIBGPIOD` to specifically exclude `gpiod` code. + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/572 +- HAL Devicehandlers: Periodic printout is run-time configurable now +- `oneShotAction` flag in the `TestTask` class is not static anymore +- `SimpleRingBuffer::writeData` now checks if the amount is larger than the total size of the + Buffer and rejects such writeData calls with `HasReturnvaluesIF::RETURN_FAILED` + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/586 +- Major update for version handling, using `git describe` to fetch version information with git. + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/601 + - Add helper functions provided by [`cmake-modules`](https://github.com/bilke/cmake-modules) + manually now. Those should not change too often and only a small subset is needed + - Separate folder for easier update and for distinction + - LICENSE file included + - use `int` for version numbers to allow unset or uninitialized version + - Initialize Version object with numbers set to -1 + - Instead of hardcoding the git hash, it is now retrieved from git + - `Version` now allows specifying additional version information like the git SHA1 hash and the + versions since the last tag + - Additional information is set to the last part of the git describe output for `FSFW_VERSION` now. + - Version still need to be hand-updated if the FSFW is not included as a submodule for now. +- IPC Message Queue Handling: Allow passing an optional `MqArgs` argument into the MessageQueue + creation call. It allows passing context information and an arbitrary user argument into + the message queue. Also streamlined and simplified `MessageQueue` implementation for all OSALs + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/583 +- Internal API change: Moved the `fsfw_hal` to the `src` folder and integration and internal + tests part of `fsfw_tests` to `src`. Unittests are now in a dedicated folder called `unittests` + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/653 + +### Task Module Refactoring + +PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/636 + +**Refactoring general task code** + +- There was a lot of duplicate/boilerplate code inside the individual task IF OSAL implementations. + Remove it by introducing base classes `PeriodicTaskBase` and `FixedTimeslotTaskBase`. + +**Refactor PeriodicTaskIF** + +- Convert `virtual ReturnValue_t addComponent(object_id_t object)` to + `virtual ReturnValue_t addComponent(object_id_t object, uint8_t opCode = 0)`, allowing to pass + the operation code passed to `performOperation`. Updated API taking + an `ExecutableObjectIF` accordingly + +**Refactor FixedTimeslotTaskIF** + +- Add additional `addSlot` function which takes an `ExecutableObjectIF` pointer and its Object ID + +**Refactor FixedSequenceSlot** + +- Introduce typedef `CustomCheckFunc` for `ReturnValue_t (*customCheckFunction)(const SlotList&)`. +- Convert `ReturnValue_t (*customCheckFunction)(const SlotList&)` to + `ReturnValue_t (*customCheckFunction)(const SlotList&, void*)`, allowing arbitrary user arguments + for the custom checker + +**Linux Task Module** + +- Use composition instead of inheritance for the `PeriodicPosixTask` and make the `PosixTask` a + member of the class + +### HAL + +- HAL Linux Uart: Baudrate and bits per word are enums now, avoiding misconfigurations + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/585 +- HAL Linux SPI: Set the Clock Default State when setting new SPI speed + and mode + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/573 +- GPIO HAL: `Direction`, `GpioOperation` and `Levels` are enum classes now, which prevents + name clashes with Windows defines. + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/572 +- HAL Linux Uart: Baudrate and bits per word are enums now, avoiding misconfigurations + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/585 + +### Time + +PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/584 and +https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/593 + +- `timeval` to `TimeOfDay_t` +- Added Mutex for gmtime calls: (compare http://www.opengate.at/blog/2020/01/timeless/) +- Moved the statics used by Clock in ClockCommon.cpp to this file +- Better check for leap seconds +- Added Unittests for Clock (only getter) + +### Power + +- `PowerSwitchIF`: Remove `const` specifier from `sendSwitchCommand` and `sendFuseOnCommand` and + also specify a `ReturnValue_t` return type + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/590 +- Extend `PowerSwitcher` module to optionally check current state when calling `turnOn` or + `turnOff`. Tis can be helpful to avoid commanding switches which do not need commanding + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/590 + +## Removed + +- Removed the `HkSwitchHelper`. This module should not be needed anymore, now that the local + datapools have been implemented. + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/557 + +## Additions + +- New constructor for PoolEntry which allows to simply specify the length of the pool entry. + This is also the new default constructor for scalar value with 0 as an initial value +- Added options for CI/CD builds: `FSFW_CICD_BUILD`. This allows the source code to know + whether it is running in CI/CD + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/623 +- Basic `clion` support: Update `.gitignore` and add some basic run configurations + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/625 +- LTO support: Allow using LTO/IPO by setting `FSFW_ENABLE_LTO=1`. CMake is able to detect whether + the user compiler supports IPO/LPO. LTO is on by default now. Most modern compilers support it, + can make good use of it and it usually makes the code faster and/or smaller. + After some more research: + Enabling LTO will actually cause the compiler to only produce thin LTO by adding + `-flto -fno-fat-lto-objects` to the compiler options. I am not sure this is an ideal choice + because if an application linking against the FSFW does not use LTO, there can be compile + issues (e.g. observed when compiling the FSFW tests without LTO). This is a known issue as + can be seen in the multiple CMake issues for it: + - https://gitlab.kitware.com/cmake/cmake/-/issues/22913, + - https://gitlab.kitware.com/cmake/cmake/-/issues/16808, + - https://gitlab.kitware.com/cmake/cmake/-/issues/21696 + Easiest solution for now: Keep this option OFF by default. + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/616 +- Linux HAL: Add wiretapping option for I2C. Enabled with `FSFW_HAL_I2C_WIRETAPPING` defined to 1 +- Dedicated Version class and constant `fsfw::FSFW_VERSION` containing version information + inside `fsfw/version.h` + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/559 +- Added generic PUS TC Scheduler Service 11. It depends on the new added Emebeded Template Library + (ETL) dependency. + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/594 +- Added ETL dependency and improved library dependency management + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/592 +- Add a `DummyPowerSwitcher` module which can be useful for test setups when no PCDU is available + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/590 +- New typedef for switcher type + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/590 +- `Subsystem`: New API to add table and sequence entries + +## Fixed + +- TCP TMTC Server: `MutexGuard` was not created properly in + `TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent)` call. + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/618 +- Fix infinite recursion in `prepareHealthSetReply` of PUS Health Service 201. + Is not currently used right now but might be used in the future + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/617 +- Move some CMake directives further up top so they are not ignored + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/621 +- Small bugfix in STM32 HAL for SPI + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/599 +- HAL GPIO: Improved error checking in `LinuxLibgpioIF::configureGpios(...)`. If a GPIO + configuration fails, the function will exit prematurely with a dedicated error code + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/602 + # [v4.0.0] ## Additions diff --git a/CMakeLists.txt b/CMakeLists.txt index 79258db2d..cfae2acb9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,23 +1,115 @@ cmake_minimum_required(VERSION 3.13) -set(FSFW_VERSION 4) -set(FSFW_SUBVERSION 0) -set(FSFW_REVISION 0) +set(MSG_PREFIX "fsfw |") # Add the cmake folder so the FindSphinx module is found -set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") +list(APPEND CMAKE_MODULE_PATH + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake-modules/bilke") +list(APPEND CMAKE_MODULE_PATH + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake-modules/rpavlik") -option(FSFW_GENERATE_SECTIONS - "Generate function and data sections. Required to remove unused code" ON -) -if(FSFW_GENERATE_SECTIONS) - option(FSFW_REMOVE_UNUSED_CODE "Remove unused code" ON) +# ############################################################################## +# Version file handling # +# ############################################################################## + +set(FSFW_VERSION_IF_GIT_FAILS 5) +set(FSFW_SUBVERSION_IF_GIT_FAILS 0) +set(FSFW_REVISION_IF_GIT_FAILS 0) + +set(FSFW_GIT_VER_HANDLING_OK FALSE) +# Version handling +if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/.git) + message(STATUS "${MSG_PREFIX} Determining version information with git") + include(FsfwHelpers) + determine_version_with_git("--exclude" "docker_*") + if(GIT_INFO) + set(FSFW_GIT_INFO + ${GIT_INFO} + CACHE STRING "Version information retrieved with git describe") + list(GET FSFW_GIT_INFO 1 FSFW_VERSION) + list(GET FSFW_GIT_INFO 2 FSFW_SUBVERSION) + list(GET FSFW_GIT_INFO 3 FSFW_REVISION) + list(GET FSFW_GIT_INFO 4 FSFW_VCS_INFO) + if(NOT FSFW_VERSION) + set(FSFW_VERSION ${FSFW_VERSION_IF_GIT_FAILS}) + endif() + if(NOT FSFW_SUBVERSION) + set(FSFW_SUBVERSION ${FSFW_SUBVERSION_IF_GIT_FAILS}) + endif() + if(NOT FSFW_REVISION) + set(FSFW_REVISION ${FSFW_REVISION_IF_GIT_FAILS}) + endif() + set(FSFW_GIT_VER_HANDLING_OK TRUE) + else() + set(FSFW_GIT_VER_HANDLING_OK FALSE) + endif() +endif() +if(NOT FSFW_GIT_VER_HANDLING_OK) + set(FSFW_VERSION ${FSFW_VERSION_IF_GIT_FAILS}) + set(FSFW_SUBVERSION ${FSFW_SUBVERSION_IF_GIT_FAILS}) + set(FSFW_REVISION ${FSFW_REVISION_IF_GIT_FAILS}) endif() -option(FSFW_BUILD_UNITTESTS "Build unittest binary in addition to static library" OFF) +set(LIB_FSFW_NAME fsfw) +project(${LIB_FSFW_NAME} + VERSION ${FSFW_VERSION}.${FSFW_SUBVERSION}.${FSFW_REVISION}) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED True) +elseif(${CMAKE_CXX_STANDARD} LESS 17) + message( + FATAL_ERROR + "${MSG_PREFIX} Compiling the FSFW requires a minimum of C++17 support") +endif() + +set(FSFW_SOURCES_DIR "${CMAKE_SOURCE_DIR}/src/fsfw") + +set(FSFW_ETL_LIB_NAME etl) +set(FSFW_ETL_LINK_TARGET etl::etl) +set(FSFW_ETL_LIB_MAJOR_VERSION + 20 + CACHE STRING "ETL library major version requirement") +set(FSFW_ETL_LIB_VERSION + ${FSFW_ETL_LIB_MAJOR_VERSION}.28.0 + CACHE STRING "ETL library exact version requirement") +set(FSFW_ETL_LINK_TARGET etl::etl) + +set(FSFW_CATCH2_LIB_MAJOR_VERSION + 3 + CACHE STRING "Catch2 library major version requirement") +set(FSFW_CATCH2_LIB_VERSION + v${FSFW_CATCH2_LIB_MAJOR_VERSION}.0.0-preview5 + CACHE STRING "Catch2 library exact version requirement") + +# Keep this off by default for now. See PR: +# https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/616 for information which +# keeping this on by default is problematic +option( + FSFW_ENABLE_IPO + "Enable interprocedural optimization or link-time optimization if available" + OFF) +if(FSFW_ENABLE_IPO) + include(CheckIPOSupported) + check_ipo_supported(RESULT IPO_SUPPORTED OUTPUT IPO_ERROR) + if(NOT IPO_SUPPORTED) + message(STATUS "FSFW | IPO/LTO not supported: ${IPO_ERROR}") + endif() +endif() + +option(FSFW_GENERATE_SECTIONS + "Generate function and data sections. Required to remove unused code" ON) +if(FSFW_GENERATE_SECTIONS) + option(FSFW_REMOVE_UNUSED_CODE "Remove unused code" ON) +endif() + +option(FSFW_BUILD_TESTS "Build unittest binary in addition to static library" + OFF) +option(FSFW_CICD_BUILD "Build for CI/CD. This can disable problematic test" OFF) option(FSFW_BUILD_DOCS "Build documentation with Sphinx and Doxygen" OFF) -if(FSFW_BUILD_UNITTESTS) - option(FSFW_TESTS_GEN_COV "Generate coverage data for unittests" ON) +if(FSFW_BUILD_TESTS) + option(FSFW_TESTS_GEN_COV "Generate coverage data for unittests" ON) endif() option(FSFW_WARNING_SHADOW_LOCAL_GCC "Enable -Wshadow=local warning in GCC" ON) @@ -38,54 +130,92 @@ option(FSFW_ADD_TMSTORAGE "Compile with tm storage components" OFF) # Contrib sources option(FSFW_ADD_SGP4_PROPAGATOR "Add SGP4 propagator code" OFF) -set(LIB_FSFW_NAME fsfw) set(FSFW_TEST_TGT fsfw-tests) set(FSFW_DUMMY_TGT fsfw-dummy) -project(${LIB_FSFW_NAME}) add_library(${LIB_FSFW_NAME}) -if(FSFW_BUILD_UNITTESTS) - message(STATUS "Building the FSFW unittests in addition to the static library") - # Check whether the user has already installed Catch2 first - find_package(Catch2 3) - # Not installed, so use FetchContent to download and provide Catch2 - if(NOT Catch2_FOUND) - include(FetchContent) +if(IPO_SUPPORTED AND FSFW_ENABLE_IPO) + set_property(TARGET ${LIB_FSFW_NAME} PROPERTY INTERPROCEDURAL_OPTIMIZATION + TRUE) +endif() - FetchContent_Declare( - Catch2 - GIT_REPOSITORY https://github.com/catchorg/Catch2.git - GIT_TAG v3.0.0-preview4 - ) +if(FSFW_BUILD_TESTS) + message( + STATUS + "${MSG_PREFIX} Building the FSFW unittests in addition to the static library" + ) + # Check whether the user has already installed Catch2 first + find_package(Catch2 ${FSFW_CATCH2_LIB_MAJOR_VERSION}) + # Not installed, so use FetchContent to download and provide Catch2 + if(NOT Catch2_FOUND) + message( + STATUS + "${MSG_PREFIX} Catch2 installation not found. Downloading Catch2 library with FetchContent" + ) + include(FetchContent) - FetchContent_MakeAvailable(Catch2) - #fixes regression -preview4, to be confirmed in later releases - set_target_properties(Catch2 PROPERTIES DEBUG_POSTFIX "") - endif() + FetchContent_Declare( + Catch2 + GIT_REPOSITORY https://github.com/catchorg/Catch2.git + GIT_TAG ${FSFW_CATCH2_LIB_VERSION}) - set(FSFW_CONFIG_PATH tests/src/fsfw_tests/unit/testcfg) - configure_file(tests/src/fsfw_tests/unit/testcfg/FSFWConfig.h.in FSFWConfig.h) - configure_file(tests/src/fsfw_tests/unit/testcfg/TestsConfig.h.in tests/TestsConfig.h) + list(APPEND FSFW_FETCH_CONTENT_TARGETS Catch2) + endif() - project(${FSFW_TEST_TGT} CXX C) - add_executable(${FSFW_TEST_TGT}) + set(FSFW_CONFIG_PATH unittests/testcfg) + configure_file(unittests/testcfg/FSFWConfig.h.in FSFWConfig.h) + configure_file(unittests/testcfg/TestsConfig.h.in tests/TestsConfig.h) - if(FSFW_TESTS_GEN_COV) - message(STATUS "Generating coverage data for the library") - message(STATUS "Targets linking against ${LIB_FSFW_NAME} " - "will be compiled with coverage data as well" - ) - include(FetchContent) - FetchContent_Declare( - cmake-modules - GIT_REPOSITORY https://github.com/bilke/cmake-modules.git - ) - FetchContent_MakeAvailable(cmake-modules) - set(CMAKE_BUILD_TYPE "Debug") - list(APPEND CMAKE_MODULE_PATH ${cmake-modules_SOURCE_DIR}) - include(CodeCoverage) - endif() + project(${FSFW_TEST_TGT} CXX C) + add_executable(${FSFW_TEST_TGT}) + if(IPO_SUPPORTED AND FSFW_ENABLE_IPO) + set_property(TARGET ${FSFW_TEST_TGT} PROPERTY INTERPROCEDURAL_OPTIMIZATION + TRUE) + endif() + + if(FSFW_TESTS_GEN_COV) + message(STATUS "${MSG_PREFIX} Generating coverage data for the library") + message(STATUS "${MSG_PREFIX} Targets linking against ${LIB_FSFW_NAME} " + "will be compiled with coverage data as well") + set(CMAKE_BUILD_TYPE "Debug") + include(CodeCoverage) + endif() +endif() + +message(STATUS "${MSG_PREFIX} Finding and/or providing ETL library") + +# Check whether the user has already installed ETL first +find_package(${FSFW_ETL_LIB_NAME} ${FSFW_ETL_LIB_MAJOR_VERSION} QUIET) +# Not installed, so use FetchContent to download and provide etl +if(NOT ${FSFW_ETL_LIB_NAME}_FOUND) + message( + STATUS + "No ETL installation was found with find_package. Installing and providing " + "etl with FindPackage") + include(FetchContent) + + FetchContent_Declare( + ${FSFW_ETL_LIB_NAME} + GIT_REPOSITORY https://github.com/ETLCPP/etl + GIT_TAG ${FSFW_ETL_LIB_VERSION}) + + list(APPEND FSFW_FETCH_CONTENT_TARGETS ${FSFW_ETL_LIB_NAME}) +endif() + +# The documentation for FetchContent recommends declaring all the dependencies +# before making them available. We make all declared dependency available here +# after their declaration +if(FSFW_FETCH_CONTENT_TARGETS) + FetchContent_MakeAvailable(${FSFW_FETCH_CONTENT_TARGETS}) + if(TARGET ${FSFW_ETL_LIB_NAME}) + add_library(${FSFW_ETL_LINK_TARGET} ALIAS ${FSFW_ETL_LIB_NAME}) + endif() + if(TARGET Catch2) + # Fixes regression -preview4, to be confirmed in later releases Related + # GitHub issue: https://github.com/catchorg/Catch2/issues/2417 + set_target_properties(Catch2 PROPERTIES DEBUG_POSTFIX "") + endif() endif() set(FSFW_CORE_INC_PATH "inc") @@ -93,256 +223,241 @@ set(FSFW_CORE_INC_PATH "inc") set_property(CACHE FSFW_OSAL PROPERTY STRINGS host linux rtems freertos) # For configure files -target_include_directories(${LIB_FSFW_NAME} PRIVATE - ${CMAKE_CURRENT_BINARY_DIR} -) -target_include_directories(${LIB_FSFW_NAME} INTERFACE - ${CMAKE_CURRENT_BINARY_DIR} -) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 11) - set(CMAKE_CXX_STANDARD_REQUIRED True) -elseif(${CMAKE_CXX_STANDARD} LESS 11) - message(FATAL_ERROR "Compiling the FSFW requires a minimum of C++11 support") -endif() +target_include_directories(${LIB_FSFW_NAME} PRIVATE ${CMAKE_CURRENT_BINARY_DIR}) +target_include_directories(${LIB_FSFW_NAME} + INTERFACE ${CMAKE_CURRENT_BINARY_DIR}) # Backwards comptability if(OS_FSFW AND NOT FSFW_OSAL) - message(WARNING "Please pass the FSFW OSAL as FSFW_OSAL instead of OS_FSFW") - set(FSFW_OSAL OS_FSFW) + message( + WARNING + "${MSG_PREFIX} Please pass the FSFW OSAL as FSFW_OSAL instead of OS_FSFW") + set(FSFW_OSAL OS_FSFW) endif() if(NOT FSFW_OSAL) - message(STATUS "No OS for FSFW via FSFW_OSAL set. Assuming host OS") - # Assume host OS and autodetermine from OS_FSFW - if(UNIX) - set(FSFW_OSAL "linux" - CACHE STRING - "OS abstraction layer used in the FSFW" - ) - elseif(WIN32) - set(FSFW_OSAL "host" - CACHE STRING "OS abstraction layer used in the FSFW" - ) - endif() - + message(STATUS "No OS for FSFW via FSFW_OSAL set. Assuming host OS") + # Assume host OS and autodetermine from OS_FSFW + if(UNIX) + set(FSFW_OSAL + "linux" + CACHE STRING "OS abstraction layer used in the FSFW") + elseif(WIN32) + set(FSFW_OSAL + "host" + CACHE STRING "OS abstraction layer used in the FSFW") + endif() endif() set(FSFW_OSAL_DEFINITION FSFW_OSAL_HOST) if(FSFW_OSAL MATCHES host) - set(FSFW_OS_NAME "Host") - set(FSFW_OSAL_HOST ON) + set(FSFW_OS_NAME "Host") + set(FSFW_OSAL_HOST ON) elseif(FSFW_OSAL MATCHES linux) - set(FSFW_OS_NAME "Linux") - set(FSFW_OSAL_LINUX ON) + set(FSFW_OS_NAME "Linux") + set(FSFW_OSAL_LINUX ON) elseif(FSFW_OSAL MATCHES freertos) - set(FSFW_OS_NAME "FreeRTOS") - set(FSFW_OSAL_FREERTOS ON) - target_link_libraries(${LIB_FSFW_NAME} PRIVATE - ${LIB_OS_NAME} - ) + set(FSFW_OS_NAME "FreeRTOS") + set(FSFW_OSAL_FREERTOS ON) + target_link_libraries(${LIB_FSFW_NAME} PRIVATE ${LIB_OS_NAME}) elseif(FSFW_OSAL STREQUAL rtems) - set(FSFW_OS_NAME "RTEMS") - set(FSFW_OSAL_RTEMS ON) + set(FSFW_OS_NAME "RTEMS") + set(FSFW_OSAL_RTEMS ON) else() - message(WARNING - "Invalid operating system for FSFW specified! Setting to host.." - ) - set(FSFW_OS_NAME "Host") - set(OS_FSFW "host") + message( + WARNING + "${MSG_PREFIX} Invalid operating system for FSFW specified! Setting to host.." + ) + set(FSFW_OS_NAME "Host") + set(OS_FSFW "host") endif() configure_file(src/fsfw/FSFW.h.in fsfw/FSFW.h) configure_file(src/fsfw/FSFWVersion.h.in fsfw/FSFWVersion.h) -message(STATUS "Compiling FSFW for the ${FSFW_OS_NAME} operating system.") +message( + STATUS "${MSG_PREFIX} Compiling FSFW for the ${FSFW_OS_NAME} operating system" +) add_subdirectory(src) -add_subdirectory(tests) -if(FSFW_ADD_HAL) - add_subdirectory(hal) -endif() add_subdirectory(contrib) +if(FSFW_BUILD_TESTS) + add_subdirectory(unittests) +endif() if(FSFW_BUILD_DOCS) - add_subdirectory(docs) + add_subdirectory(docs) endif() -if(FSFW_BUILD_UNITTESTS) - if(FSFW_TESTS_GEN_COV) - if(CMAKE_COMPILER_IS_GNUCXX) - include(CodeCoverage) +if(FSFW_BUILD_TESTS) + if(FSFW_TESTS_GEN_COV) + if(CMAKE_COMPILER_IS_GNUCXX) + include(CodeCoverage) - # Remove quotes. - separate_arguments(COVERAGE_COMPILER_FLAGS - NATIVE_COMMAND "${COVERAGE_COMPILER_FLAGS}" - ) + # Remove quotes. + separate_arguments(COVERAGE_COMPILER_FLAGS NATIVE_COMMAND + "${COVERAGE_COMPILER_FLAGS}") - # Add compile options manually, we don't want coverage for Catch2 - target_compile_options(${FSFW_TEST_TGT} PRIVATE - "${COVERAGE_COMPILER_FLAGS}" - ) - target_compile_options(${LIB_FSFW_NAME} PRIVATE - "${COVERAGE_COMPILER_FLAGS}" - ) + # Add compile options manually, we don't want coverage for Catch2 + target_compile_options(${FSFW_TEST_TGT} + PRIVATE "${COVERAGE_COMPILER_FLAGS}") + target_compile_options(${LIB_FSFW_NAME} + PRIVATE "${COVERAGE_COMPILER_FLAGS}") - # Exclude directories here - if(WIN32) - set(GCOVR_ADDITIONAL_ARGS - "--exclude-throw-branches" - "--exclude-unreachable-branches" - ) - set(COVERAGE_EXCLUDES - "/c/msys64/mingw64/*" "*/fsfw_hal/*" - ) - elseif(UNIX) - set(COVERAGE_EXCLUDES - "/usr/include/*" "/usr/bin/*" "Catch2/*" - "/usr/local/include/*" "*/fsfw_tests/*" - "*/catch2-src/*" "*/fsfw_hal/*" - ) - endif() + # Exclude directories here + if(WIN32) + set(GCOVR_ADDITIONAL_ARGS "--exclude-throw-branches" + "--exclude-unreachable-branches") + set(COVERAGE_EXCLUDES "/c/msys64/mingw64/*" "*/fsfw_hal/*") + elseif(UNIX) + set(COVERAGE_EXCLUDES + "/usr/include/*" + "/usr/bin/*" + "Catch2/*" + "/usr/local/include/*" + "*/fsfw_tests/*" + "*/catch2-src/*" + "*/fsfw_hal/*") + endif() - target_link_options(${FSFW_TEST_TGT} PRIVATE - -fprofile-arcs - -ftest-coverage - ) - target_link_options(${LIB_FSFW_NAME} PRIVATE - -fprofile-arcs - -ftest-coverage - ) - # Need to specify this as an interface, otherwise there will the compile issues - target_link_options(${LIB_FSFW_NAME} INTERFACE - -fprofile-arcs - -ftest-coverage - ) + target_link_options(${FSFW_TEST_TGT} PRIVATE -fprofile-arcs + -ftest-coverage) + target_link_options(${LIB_FSFW_NAME} PRIVATE -fprofile-arcs + -ftest-coverage) + # Need to specify this as an interface, otherwise there will the compile + # issues + target_link_options(${LIB_FSFW_NAME} INTERFACE -fprofile-arcs + -ftest-coverage) - if(WIN32) - setup_target_for_coverage_gcovr_html( - NAME ${FSFW_TEST_TGT}_coverage - EXECUTABLE ${FSFW_TEST_TGT} - DEPENDENCIES ${FSFW_TEST_TGT} - ) - else() - setup_target_for_coverage_lcov( - NAME ${FSFW_TEST_TGT}_coverage - EXECUTABLE ${FSFW_TEST_TGT} - DEPENDENCIES ${FSFW_TEST_TGT} - ) - endif() - endif() + if(WIN32) + setup_target_for_coverage_gcovr_html( + NAME ${FSFW_TEST_TGT}_coverage EXECUTABLE ${FSFW_TEST_TGT} + DEPENDENCIES ${FSFW_TEST_TGT}) + else() + setup_target_for_coverage_lcov( + NAME ${FSFW_TEST_TGT}_coverage EXECUTABLE ${FSFW_TEST_TGT} + DEPENDENCIES ${FSFW_TEST_TGT}) + endif() endif() - target_link_libraries(${FSFW_TEST_TGT} PRIVATE Catch2::Catch2 ${LIB_FSFW_NAME}) + endif() + target_link_libraries(${FSFW_TEST_TGT} PRIVATE Catch2::Catch2 + ${LIB_FSFW_NAME}) endif() -# The project CMakeLists file has to set the FSFW_CONFIG_PATH and add it. -# If this is not given, we include the default configuration and emit a warning. +# The project CMakeLists file has to set the FSFW_CONFIG_PATH and add it. If +# this is not given, we include the default configuration and emit a warning. if(NOT FSFW_CONFIG_PATH) - set(DEF_CONF_PATH misc/defaultcfg/fsfwconfig) - if(NOT FSFW_BUILD_DOCS) - message(WARNING "Flight Software Framework configuration path not set!") - message(WARNING "Setting default configuration from ${DEF_CONF_PATH} ..") - endif() - add_subdirectory(${DEF_CONF_PATH}) - set(FSFW_CONFIG_PATH ${DEF_CONF_PATH}) + set(DEF_CONF_PATH misc/defaultcfg/fsfwconfig) + if(NOT FSFW_BUILD_DOCS) + message( + WARNING + "${MSG_PREFIX} Flight Software Framework configuration path not set") + message( + WARNING + "${MSG_PREFIX} Setting default configuration from ${DEF_CONF_PATH} ..") + endif() + add_subdirectory(${DEF_CONF_PATH}) + set(FSFW_CONFIG_PATH ${DEF_CONF_PATH}) endif() -# FSFW might be part of a possibly complicated folder structure, so we -# extract the absolute path of the fsfwconfig folder. +# FSFW might be part of a possibly complicated folder structure, so we extract +# the absolute path of the fsfwconfig folder. if(IS_ABSOLUTE ${FSFW_CONFIG_PATH}) - set(FSFW_CONFIG_PATH_ABSOLUTE ${FSFW_CONFIG_PATH}) + set(FSFW_CONFIG_PATH_ABSOLUTE ${FSFW_CONFIG_PATH}) else() - get_filename_component(FSFW_CONFIG_PATH_ABSOLUTE - ${FSFW_CONFIG_PATH} REALPATH BASE_DIR ${CMAKE_SOURCE_DIR} - ) + get_filename_component(FSFW_CONFIG_PATH_ABSOLUTE ${FSFW_CONFIG_PATH} REALPATH + BASE_DIR ${CMAKE_SOURCE_DIR}) endif() foreach(INCLUDE_PATH ${FSFW_ADDITIONAL_INC_PATHS}) - if(IS_ABSOLUTE ${INCLUDE_PATH}) - set(CURR_ABS_INC_PATH "${INCLUDE_PATH}") - else() - get_filename_component(CURR_ABS_INC_PATH - ${INCLUDE_PATH} REALPATH BASE_DIR ${CMAKE_SOURCE_DIR}) - endif() + if(IS_ABSOLUTE ${INCLUDE_PATH}) + set(CURR_ABS_INC_PATH "${INCLUDE_PATH}") + else() + get_filename_component(CURR_ABS_INC_PATH ${INCLUDE_PATH} REALPATH BASE_DIR + ${CMAKE_SOURCE_DIR}) + endif() - if(CMAKE_VERBOSE) - message(STATUS "FSFW include path: ${CURR_ABS_INC_PATH}") - endif() + if(CMAKE_VERBOSE) + message(STATUS "FSFW include path: ${CURR_ABS_INC_PATH}") + endif() - list(APPEND FSFW_ADD_INC_PATHS_ABS ${CURR_ABS_INC_PATH}) + list(APPEND FSFW_ADD_INC_PATHS_ABS ${CURR_ABS_INC_PATH}) endforeach() if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - if(NOT DEFINED FSFW_WARNING_FLAGS) - set(FSFW_WARNING_FLAGS - -Wall - -Wextra - -Wimplicit-fallthrough=1 - -Wno-unused-parameter - -Wno-psabi - ) - endif() + if(NOT DEFINED FSFW_WARNING_FLAGS) + set(FSFW_WARNING_FLAGS + -Wall + -Wextra + -Wimplicit-fallthrough=1 + -Wno-unused-parameter + -Wno-psabi + -Wduplicated-cond # check for duplicate conditions + -Wduplicated-branches # check for duplicate branches + -Wlogical-op # Search for bitwise operations instead of logical + -Wnull-dereference # Search for NULL dereference + -Wundef # Warn if undefind marcos are used + -Wformat=2 # Format string problem detection + -Wformat-overflow=2 # Formatting issues in printf + -Wformat-truncation=2 # Formatting issues in printf + -Wformat-security # Search for dangerous printf operations + -Wstrict-overflow=3 # Warn if integer overflows might happen + -Warray-bounds=2 # Some array bounds violations will be found + -Wshift-overflow=2 # Search for bit left shift overflows ( +mkdir build && cd build +cmake .. +cmake --install . +``` + +It is recommended to install `20.27.2` or newer for the package version handling of +ETL to work. + ## Adding the library The following steps show how to add and use FSFW components. It is still recommended to @@ -71,9 +99,9 @@ add and link against the FSFW library in general. 4. Link against the FSFW library - ```cmake - target_link_libraries( PRIVATE fsfw) - ``` + ```sh + target_link_libraries(${YourProjectName} PRIVATE fsfw) + ``` 5. It should now be possible use the FSFW as a static library from the user code. @@ -83,6 +111,19 @@ The FSFW also has unittests which use the [Catch2 library](https://github.com/ca These are built by setting the CMake option `FSFW_BUILD_UNITTESTS` to `ON` or `TRUE` from your project `CMakeLists.txt` file or from the command line. +You can install the Catch2 library, which prevents the build system to avoid re-downloading +the dependency if the unit tests are completely rebuilt. The current recommended version +can be found inside the fsfw `CMakeLists.txt` file or by using `ccmake` and looking up +the `FSFW_CATCH2_LIB_VERSION` variable. + +```sh +git clone https://github.com/catchorg/Catch2.git +cd Catch2 +git checkout +cmake -Bbuild -H. -DBUILD_TESTING=OFF +sudo cmake --build build/ --target install +``` + The fsfw-tests binary will be built as part of the static library and dropped alongside it. If the unittests are built, the library and the tests will be built with coverage information by default. This can be disabled by setting the `FSFW_TESTS_COV_GEN` option to `OFF` or `FALSE`. @@ -90,7 +131,7 @@ default. This can be disabled by setting the `FSFW_TESTS_COV_GEN` option to `OFF You can use the following commands inside the `fsfw` folder to set up the build system ```sh -mkdir build-Unittest && cd build-Unittest +mkdir build-tests && cd build-tests cmake -DFSFW_BUILD_UNITTESTS=ON -DFSFW_OSAL=host -DCMAKE_BUILD_TYPE=Debug .. ``` @@ -98,7 +139,7 @@ You can also use `-DFSFW_OSAL=linux` on Linux systems. Coverage data in HTML format can be generated using the `CodeCoverage` [CMake module](https://github.com/bilke/cmake-modules/tree/master). -To build the unittests, run them and then generare the coverage data in this format, +To build the unittests, run them and then generate the coverage data in this format, the following command can be used inside the build directory after the build system was set up ```sh @@ -147,7 +188,10 @@ and open the documentation conveniently. Try `helper.py -h for more information. The formatting is done by the `clang-format` tool. The configuration is contained within the `.clang-format` file in the repository root. As long as `clang-format` is installed, you -can run the `apply-clang-format.sh` helper script to format all source files consistently. +can run the `auto-format.sh` helper script to format all source files consistently. Furthermore cmake-format is required to format CMake files which can be installed with: +````sh +sudo pip install cmakelang +```` ## Index diff --git a/automation/Dockerfile b/automation/Dockerfile index 9df67fc82..2ed2a7d9a 100644 --- a/automation/Dockerfile +++ b/automation/Dockerfile @@ -6,3 +6,15 @@ RUN apt-get --yes upgrade #tzdata is a dependency, won't install otherwise ARG DEBIAN_FRONTEND=noninteractive RUN apt-get --yes install gcc g++ cmake make lcov git valgrind nano iputils-ping + +RUN git clone https://github.com/catchorg/Catch2.git && \ + cd Catch2 && \ + git checkout v3.0.0-preview5 && \ + cmake -Bbuild -H. -DBUILD_TESTING=OFF && \ + cmake --build build/ --target install + +RUN git clone https://github.com/ETLCPP/etl.git && \ + cd etl && \ + git checkout 20.28.0 && \ + cmake -B build . && \ + cmake --install build/ diff --git a/automation/Jenkinsfile b/automation/Jenkinsfile index 3424f986c..c5dcbe029 100644 --- a/automation/Jenkinsfile +++ b/automation/Jenkinsfile @@ -3,7 +3,7 @@ pipeline { BUILDDIR = 'build-tests' } agent { - docker { image 'fsfw-ci:d1'} + docker { image 'fsfw-ci:d3'} } stages { stage('Clean') { @@ -14,21 +14,21 @@ pipeline { stage('Configure') { steps { dir(BUILDDIR) { - sh 'cmake -DFSFW_OSAL=host -DFSFW_BUILD_UNITTESTS=ON ..' + sh 'cmake -DFSFW_OSAL=host -DFSFW_BUILD_TESTS=ON -DFSFW_CICD_BUILD=ON ..' } } } stage('Build') { steps { dir(BUILDDIR) { - sh 'cmake --build . -j' + sh 'cmake --build . -j4' } } } stage('Unittests') { steps { dir(BUILDDIR) { - sh 'cmake --build . -- fsfw-tests_coverage -j' + sh 'cmake --build . -- fsfw-tests_coverage -j4' } } } diff --git a/cmake/FsfwHelpers.cmake b/cmake/FsfwHelpers.cmake new file mode 100644 index 000000000..3f22ebde1 --- /dev/null +++ b/cmake/FsfwHelpers.cmake @@ -0,0 +1,28 @@ +# Determines the git version with git describe and returns it by setting +# the GIT_INFO list in the parent scope. The list has the following entries +# 1. Full version string +# 2. Major version +# 3. Minor version +# 4. Revision +# 5. git SHA hash and commits since tag +function(determine_version_with_git) + include(GetGitRevisionDescription) + git_describe(VERSION ${ARGN}) + string(FIND ${VERSION} "." VALID_VERSION) + if(VALID_VERSION EQUAL -1) + message(WARNING "Version string ${VERSION} retrieved with git describe is invalid") + return() + endif() + # Parse the version information into pieces. + string(REGEX REPLACE "^v([0-9]+)\\..*" "\\1" _VERSION_MAJOR "${VERSION}") + string(REGEX REPLACE "^v[0-9]+\\.([0-9]+).*" "\\1" _VERSION_MINOR "${VERSION}") + string(REGEX REPLACE "^v[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1" _VERSION_PATCH "${VERSION}") + string(REGEX REPLACE "^v[0-9]+\\.[0-9]+\\.[0-9]+-(.*)" "\\1" VERSION_SHA1 "${VERSION}") + set(GIT_INFO ${VERSION}) + list(APPEND GIT_INFO ${_VERSION_MAJOR}) + list(APPEND GIT_INFO ${_VERSION_MINOR}) + list(APPEND GIT_INFO ${_VERSION_PATCH}) + list(APPEND GIT_INFO ${VERSION_SHA1}) + set(GIT_INFO ${GIT_INFO} PARENT_SCOPE) + message(STATUS "${MSG_PREFIX} Set git version info into GIT_INFO from the git tag ${VERSION}") +endfunction() diff --git a/cmake/cmake-modules/README.md b/cmake/cmake-modules/README.md new file mode 100644 index 000000000..b6355c3fc --- /dev/null +++ b/cmake/cmake-modules/README.md @@ -0,0 +1,7 @@ +The files in the `bilke` folder were manually copy and pasted from the +[cmake-modules repository](https://github.com/bilke/cmake-modules). It was decided to do +this because only a small subset of its provided functions are needed. + +The files in the `rpavlik` folder were manually copy and pasted from the +[cmake-modules repository](https://github.com/rpavlik/cmake-modules). It was decided to do +this because only a small subset of its provided functions are needed. diff --git a/cmake/cmake-modules/bilke/CodeCoverage.cmake b/cmake/cmake-modules/bilke/CodeCoverage.cmake new file mode 100644 index 000000000..aef3d9436 --- /dev/null +++ b/cmake/cmake-modules/bilke/CodeCoverage.cmake @@ -0,0 +1,719 @@ +# Copyright (c) 2012 - 2017, Lars Bilke +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its contributors +# may be used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +# ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# +# CHANGES: +# +# 2012-01-31, Lars Bilke +# - Enable Code Coverage +# +# 2013-09-17, Joakim Söderberg +# - Added support for Clang. +# - Some additional usage instructions. +# +# 2016-02-03, Lars Bilke +# - Refactored functions to use named parameters +# +# 2017-06-02, Lars Bilke +# - Merged with modified version from github.com/ufz/ogs +# +# 2019-05-06, Anatolii Kurotych +# - Remove unnecessary --coverage flag +# +# 2019-12-13, FeRD (Frank Dana) +# - Deprecate COVERAGE_LCOVR_EXCLUDES and COVERAGE_GCOVR_EXCLUDES lists in favor +# of tool-agnostic COVERAGE_EXCLUDES variable, or EXCLUDE setup arguments. +# - CMake 3.4+: All excludes can be specified relative to BASE_DIRECTORY +# - All setup functions: accept BASE_DIRECTORY, EXCLUDE list +# - Set lcov basedir with -b argument +# - Add automatic --demangle-cpp in lcovr, if 'c++filt' is available (can be +# overridden with NO_DEMANGLE option in setup_target_for_coverage_lcovr().) +# - Delete output dir, .info file on 'make clean' +# - Remove Python detection, since version mismatches will break gcovr +# - Minor cleanup (lowercase function names, update examples...) +# +# 2019-12-19, FeRD (Frank Dana) +# - Rename Lcov outputs, make filtered file canonical, fix cleanup for targets +# +# 2020-01-19, Bob Apthorpe +# - Added gfortran support +# +# 2020-02-17, FeRD (Frank Dana) +# - Make all add_custom_target()s VERBATIM to auto-escape wildcard characters +# in EXCLUDEs, and remove manual escaping from gcovr targets +# +# 2021-01-19, Robin Mueller +# - Add CODE_COVERAGE_VERBOSE option which will allow to print out commands which are run +# - Added the option for users to set the GCOVR_ADDITIONAL_ARGS variable to supply additional +# flags to the gcovr command +# +# 2020-05-04, Mihchael Davis +# - Add -fprofile-abs-path to make gcno files contain absolute paths +# - Fix BASE_DIRECTORY not working when defined +# - Change BYPRODUCT from folder to index.html to stop ninja from complaining about double defines +# +# 2021-05-10, Martin Stump +# - Check if the generator is multi-config before warning about non-Debug builds +# +# 2022-02-22, Marko Wehle +# - Change gcovr output from -o for --xml and --html output respectively. +# This will allow for Multiple Output Formats at the same time by making use of GCOVR_ADDITIONAL_ARGS, e.g. GCOVR_ADDITIONAL_ARGS "--txt". +# +# USAGE: +# +# 1. Copy this file into your cmake modules path. +# +# 2. Add the following line to your CMakeLists.txt (best inside an if-condition +# using a CMake option() to enable it just optionally): +# include(CodeCoverage) +# +# 3. Append necessary compiler flags for all supported source files: +# append_coverage_compiler_flags() +# Or for specific target: +# append_coverage_compiler_flags_to_target(YOUR_TARGET_NAME) +# +# 3.a (OPTIONAL) Set appropriate optimization flags, e.g. -O0, -O1 or -Og +# +# 4. If you need to exclude additional directories from the report, specify them +# using full paths in the COVERAGE_EXCLUDES variable before calling +# setup_target_for_coverage_*(). +# Example: +# set(COVERAGE_EXCLUDES +# '${PROJECT_SOURCE_DIR}/src/dir1/*' +# '/path/to/my/src/dir2/*') +# Or, use the EXCLUDE argument to setup_target_for_coverage_*(). +# Example: +# setup_target_for_coverage_lcov( +# NAME coverage +# EXECUTABLE testrunner +# EXCLUDE "${PROJECT_SOURCE_DIR}/src/dir1/*" "/path/to/my/src/dir2/*") +# +# 4.a NOTE: With CMake 3.4+, COVERAGE_EXCLUDES or EXCLUDE can also be set +# relative to the BASE_DIRECTORY (default: PROJECT_SOURCE_DIR) +# Example: +# set(COVERAGE_EXCLUDES "dir1/*") +# setup_target_for_coverage_gcovr_html( +# NAME coverage +# EXECUTABLE testrunner +# BASE_DIRECTORY "${PROJECT_SOURCE_DIR}/src" +# EXCLUDE "dir2/*") +# +# 5. Use the functions described below to create a custom make target which +# runs your test executable and produces a code coverage report. +# +# 6. Build a Debug build: +# cmake -DCMAKE_BUILD_TYPE=Debug .. +# make +# make my_coverage_target +# + +include(CMakeParseArguments) + +option(CODE_COVERAGE_VERBOSE "Verbose information" FALSE) + +# Check prereqs +find_program( GCOV_PATH gcov ) +find_program( LCOV_PATH NAMES lcov lcov.bat lcov.exe lcov.perl) +find_program( FASTCOV_PATH NAMES fastcov fastcov.py ) +find_program( GENHTML_PATH NAMES genhtml genhtml.perl genhtml.bat ) +find_program( GCOVR_PATH gcovr PATHS ${CMAKE_SOURCE_DIR}/scripts/test) +find_program( CPPFILT_PATH NAMES c++filt ) + +if(NOT GCOV_PATH) + message(FATAL_ERROR "gcov not found! Aborting...") +endif() # NOT GCOV_PATH + +get_property(LANGUAGES GLOBAL PROPERTY ENABLED_LANGUAGES) +list(GET LANGUAGES 0 LANG) + +if("${CMAKE_${LANG}_COMPILER_ID}" MATCHES "(Apple)?[Cc]lang") + if("${CMAKE_${LANG}_COMPILER_VERSION}" VERSION_LESS 3) + message(FATAL_ERROR "Clang version must be 3.0.0 or greater! Aborting...") + endif() +elseif(NOT CMAKE_COMPILER_IS_GNUCXX) + if("${CMAKE_Fortran_COMPILER_ID}" MATCHES "[Ff]lang") + # Do nothing; exit conditional without error if true + elseif("${CMAKE_Fortran_COMPILER_ID}" MATCHES "GNU") + # Do nothing; exit conditional without error if true + else() + message(FATAL_ERROR "Compiler is not GNU gcc! Aborting...") + endif() +endif() + +set(COVERAGE_COMPILER_FLAGS "-g -fprofile-arcs -ftest-coverage" + CACHE INTERNAL "") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + include(CheckCXXCompilerFlag) + check_cxx_compiler_flag(-fprofile-abs-path HAVE_fprofile_abs_path) + if(HAVE_fprofile_abs_path) + set(COVERAGE_COMPILER_FLAGS "${COVERAGE_COMPILER_FLAGS} -fprofile-abs-path") + endif() +endif() + +set(CMAKE_Fortran_FLAGS_COVERAGE + ${COVERAGE_COMPILER_FLAGS} + CACHE STRING "Flags used by the Fortran compiler during coverage builds." + FORCE ) +set(CMAKE_CXX_FLAGS_COVERAGE + ${COVERAGE_COMPILER_FLAGS} + CACHE STRING "Flags used by the C++ compiler during coverage builds." + FORCE ) +set(CMAKE_C_FLAGS_COVERAGE + ${COVERAGE_COMPILER_FLAGS} + CACHE STRING "Flags used by the C compiler during coverage builds." + FORCE ) +set(CMAKE_EXE_LINKER_FLAGS_COVERAGE + "" + CACHE STRING "Flags used for linking binaries during coverage builds." + FORCE ) +set(CMAKE_SHARED_LINKER_FLAGS_COVERAGE + "" + CACHE STRING "Flags used by the shared libraries linker during coverage builds." + FORCE ) +mark_as_advanced( + CMAKE_Fortran_FLAGS_COVERAGE + CMAKE_CXX_FLAGS_COVERAGE + CMAKE_C_FLAGS_COVERAGE + CMAKE_EXE_LINKER_FLAGS_COVERAGE + CMAKE_SHARED_LINKER_FLAGS_COVERAGE ) + +get_property(GENERATOR_IS_MULTI_CONFIG GLOBAL PROPERTY GENERATOR_IS_MULTI_CONFIG) +if(NOT (CMAKE_BUILD_TYPE STREQUAL "Debug" OR GENERATOR_IS_MULTI_CONFIG)) + message(WARNING "Code coverage results with an optimised (non-Debug) build may be misleading") +endif() # NOT (CMAKE_BUILD_TYPE STREQUAL "Debug" OR GENERATOR_IS_MULTI_CONFIG) + +if(CMAKE_C_COMPILER_ID STREQUAL "GNU" OR CMAKE_Fortran_COMPILER_ID STREQUAL "GNU") + link_libraries(gcov) +endif() + +# Defines a target for running and collection code coverage information +# Builds dependencies, runs the given executable and outputs reports. +# NOTE! The executable should always have a ZERO as exit code otherwise +# the coverage generation will not complete. +# +# setup_target_for_coverage_lcov( +# NAME testrunner_coverage # New target name +# EXECUTABLE testrunner -j ${PROCESSOR_COUNT} # Executable in PROJECT_BINARY_DIR +# DEPENDENCIES testrunner # Dependencies to build first +# BASE_DIRECTORY "../" # Base directory for report +# # (defaults to PROJECT_SOURCE_DIR) +# EXCLUDE "src/dir1/*" "src/dir2/*" # Patterns to exclude (can be relative +# # to BASE_DIRECTORY, with CMake 3.4+) +# NO_DEMANGLE # Don't demangle C++ symbols +# # even if c++filt is found +# ) +function(setup_target_for_coverage_lcov) + + set(options NO_DEMANGLE) + set(oneValueArgs BASE_DIRECTORY NAME) + set(multiValueArgs EXCLUDE EXECUTABLE EXECUTABLE_ARGS DEPENDENCIES LCOV_ARGS GENHTML_ARGS) + cmake_parse_arguments(Coverage "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(NOT LCOV_PATH) + message(FATAL_ERROR "lcov not found! Aborting...") + endif() # NOT LCOV_PATH + + if(NOT GENHTML_PATH) + message(FATAL_ERROR "genhtml not found! Aborting...") + endif() # NOT GENHTML_PATH + + # Set base directory (as absolute path), or default to PROJECT_SOURCE_DIR + if(DEFINED Coverage_BASE_DIRECTORY) + get_filename_component(BASEDIR ${Coverage_BASE_DIRECTORY} ABSOLUTE) + else() + set(BASEDIR ${PROJECT_SOURCE_DIR}) + endif() + + # Collect excludes (CMake 3.4+: Also compute absolute paths) + set(LCOV_EXCLUDES "") + foreach(EXCLUDE ${Coverage_EXCLUDE} ${COVERAGE_EXCLUDES} ${COVERAGE_LCOV_EXCLUDES}) + if(CMAKE_VERSION VERSION_GREATER 3.4) + get_filename_component(EXCLUDE ${EXCLUDE} ABSOLUTE BASE_DIR ${BASEDIR}) + endif() + list(APPEND LCOV_EXCLUDES "${EXCLUDE}") + endforeach() + list(REMOVE_DUPLICATES LCOV_EXCLUDES) + + # Conditional arguments + if(CPPFILT_PATH AND NOT ${Coverage_NO_DEMANGLE}) + set(GENHTML_EXTRA_ARGS "--demangle-cpp") + endif() + + # Setting up commands which will be run to generate coverage data. + # Cleanup lcov + set(LCOV_CLEAN_CMD + ${LCOV_PATH} ${Coverage_LCOV_ARGS} --gcov-tool ${GCOV_PATH} -directory . + -b ${BASEDIR} --zerocounters + ) + # Create baseline to make sure untouched files show up in the report + set(LCOV_BASELINE_CMD + ${LCOV_PATH} ${Coverage_LCOV_ARGS} --gcov-tool ${GCOV_PATH} -c -i -d . -b + ${BASEDIR} -o ${Coverage_NAME}.base + ) + # Run tests + set(LCOV_EXEC_TESTS_CMD + ${Coverage_EXECUTABLE} ${Coverage_EXECUTABLE_ARGS} + ) + # Capturing lcov counters and generating report + set(LCOV_CAPTURE_CMD + ${LCOV_PATH} ${Coverage_LCOV_ARGS} --gcov-tool ${GCOV_PATH} --directory . -b + ${BASEDIR} --capture --output-file ${Coverage_NAME}.capture + ) + # add baseline counters + set(LCOV_BASELINE_COUNT_CMD + ${LCOV_PATH} ${Coverage_LCOV_ARGS} --gcov-tool ${GCOV_PATH} -a ${Coverage_NAME}.base + -a ${Coverage_NAME}.capture --output-file ${Coverage_NAME}.total + ) + # filter collected data to final coverage report + set(LCOV_FILTER_CMD + ${LCOV_PATH} ${Coverage_LCOV_ARGS} --gcov-tool ${GCOV_PATH} --remove + ${Coverage_NAME}.total ${LCOV_EXCLUDES} --output-file ${Coverage_NAME}.info + ) + # Generate HTML output + set(LCOV_GEN_HTML_CMD + ${GENHTML_PATH} ${GENHTML_EXTRA_ARGS} ${Coverage_GENHTML_ARGS} -o + ${Coverage_NAME} ${Coverage_NAME}.info + ) + + + if(CODE_COVERAGE_VERBOSE) + message(STATUS "Executed command report") + message(STATUS "Command to clean up lcov: ") + string(REPLACE ";" " " LCOV_CLEAN_CMD_SPACED "${LCOV_CLEAN_CMD}") + message(STATUS "${LCOV_CLEAN_CMD_SPACED}") + + message(STATUS "Command to create baseline: ") + string(REPLACE ";" " " LCOV_BASELINE_CMD_SPACED "${LCOV_BASELINE_CMD}") + message(STATUS "${LCOV_BASELINE_CMD_SPACED}") + + message(STATUS "Command to run the tests: ") + string(REPLACE ";" " " LCOV_EXEC_TESTS_CMD_SPACED "${LCOV_EXEC_TESTS_CMD}") + message(STATUS "${LCOV_EXEC_TESTS_CMD_SPACED}") + + message(STATUS "Command to capture counters and generate report: ") + string(REPLACE ";" " " LCOV_CAPTURE_CMD_SPACED "${LCOV_CAPTURE_CMD}") + message(STATUS "${LCOV_CAPTURE_CMD_SPACED}") + + message(STATUS "Command to add baseline counters: ") + string(REPLACE ";" " " LCOV_BASELINE_COUNT_CMD_SPACED "${LCOV_BASELINE_COUNT_CMD}") + message(STATUS "${LCOV_BASELINE_COUNT_CMD_SPACED}") + + message(STATUS "Command to filter collected data: ") + string(REPLACE ";" " " LCOV_FILTER_CMD_SPACED "${LCOV_FILTER_CMD}") + message(STATUS "${LCOV_FILTER_CMD_SPACED}") + + message(STATUS "Command to generate lcov HTML output: ") + string(REPLACE ";" " " LCOV_GEN_HTML_CMD_SPACED "${LCOV_GEN_HTML_CMD}") + message(STATUS "${LCOV_GEN_HTML_CMD_SPACED}") + endif() + + # Setup target + add_custom_target(${Coverage_NAME} + COMMAND ${LCOV_CLEAN_CMD} + COMMAND ${LCOV_BASELINE_CMD} + COMMAND ${LCOV_EXEC_TESTS_CMD} + COMMAND ${LCOV_CAPTURE_CMD} + COMMAND ${LCOV_BASELINE_COUNT_CMD} + COMMAND ${LCOV_FILTER_CMD} + COMMAND ${LCOV_GEN_HTML_CMD} + + # Set output files as GENERATED (will be removed on 'make clean') + BYPRODUCTS + ${Coverage_NAME}.base + ${Coverage_NAME}.capture + ${Coverage_NAME}.total + ${Coverage_NAME}.info + ${Coverage_NAME}/index.html + WORKING_DIRECTORY ${PROJECT_BINARY_DIR} + DEPENDS ${Coverage_DEPENDENCIES} + VERBATIM # Protect arguments to commands + COMMENT "Resetting code coverage counters to zero.\nProcessing code coverage counters and generating report." + ) + + # Show where to find the lcov info report + add_custom_command(TARGET ${Coverage_NAME} POST_BUILD + COMMAND ; + COMMENT "Lcov code coverage info report saved in ${Coverage_NAME}.info." + ) + + # Show info where to find the report + add_custom_command(TARGET ${Coverage_NAME} POST_BUILD + COMMAND ; + COMMENT "Open ./${Coverage_NAME}/index.html in your browser to view the coverage report." + ) + +endfunction() # setup_target_for_coverage_lcov + +# Defines a target for running and collection code coverage information +# Builds dependencies, runs the given executable and outputs reports. +# NOTE! The executable should always have a ZERO as exit code otherwise +# the coverage generation will not complete. +# +# setup_target_for_coverage_gcovr_xml( +# NAME ctest_coverage # New target name +# EXECUTABLE ctest -j ${PROCESSOR_COUNT} # Executable in PROJECT_BINARY_DIR +# DEPENDENCIES executable_target # Dependencies to build first +# BASE_DIRECTORY "../" # Base directory for report +# # (defaults to PROJECT_SOURCE_DIR) +# EXCLUDE "src/dir1/*" "src/dir2/*" # Patterns to exclude (can be relative +# # to BASE_DIRECTORY, with CMake 3.4+) +# ) +# The user can set the variable GCOVR_ADDITIONAL_ARGS to supply additional flags to the +# GCVOR command. +function(setup_target_for_coverage_gcovr_xml) + + set(options NONE) + set(oneValueArgs BASE_DIRECTORY NAME) + set(multiValueArgs EXCLUDE EXECUTABLE EXECUTABLE_ARGS DEPENDENCIES) + cmake_parse_arguments(Coverage "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(NOT GCOVR_PATH) + message(FATAL_ERROR "gcovr not found! Aborting...") + endif() # NOT GCOVR_PATH + + # Set base directory (as absolute path), or default to PROJECT_SOURCE_DIR + if(DEFINED Coverage_BASE_DIRECTORY) + get_filename_component(BASEDIR ${Coverage_BASE_DIRECTORY} ABSOLUTE) + else() + set(BASEDIR ${PROJECT_SOURCE_DIR}) + endif() + + # Collect excludes (CMake 3.4+: Also compute absolute paths) + set(GCOVR_EXCLUDES "") + foreach(EXCLUDE ${Coverage_EXCLUDE} ${COVERAGE_EXCLUDES} ${COVERAGE_GCOVR_EXCLUDES}) + if(CMAKE_VERSION VERSION_GREATER 3.4) + get_filename_component(EXCLUDE ${EXCLUDE} ABSOLUTE BASE_DIR ${BASEDIR}) + endif() + list(APPEND GCOVR_EXCLUDES "${EXCLUDE}") + endforeach() + list(REMOVE_DUPLICATES GCOVR_EXCLUDES) + + # Combine excludes to several -e arguments + set(GCOVR_EXCLUDE_ARGS "") + foreach(EXCLUDE ${GCOVR_EXCLUDES}) + list(APPEND GCOVR_EXCLUDE_ARGS "-e") + list(APPEND GCOVR_EXCLUDE_ARGS "${EXCLUDE}") + endforeach() + + # Set up commands which will be run to generate coverage data + # Run tests + set(GCOVR_XML_EXEC_TESTS_CMD + ${Coverage_EXECUTABLE} ${Coverage_EXECUTABLE_ARGS} + ) + # Running gcovr + set(GCOVR_XML_CMD + ${GCOVR_PATH} --xml ${Coverage_NAME}.xml -r ${BASEDIR} ${GCOVR_ADDITIONAL_ARGS} + ${GCOVR_EXCLUDE_ARGS} --object-directory=${PROJECT_BINARY_DIR} + ) + + if(CODE_COVERAGE_VERBOSE) + message(STATUS "Executed command report") + + message(STATUS "Command to run tests: ") + string(REPLACE ";" " " GCOVR_XML_EXEC_TESTS_CMD_SPACED "${GCOVR_XML_EXEC_TESTS_CMD}") + message(STATUS "${GCOVR_XML_EXEC_TESTS_CMD_SPACED}") + + message(STATUS "Command to generate gcovr XML coverage data: ") + string(REPLACE ";" " " GCOVR_XML_CMD_SPACED "${GCOVR_XML_CMD}") + message(STATUS "${GCOVR_XML_CMD_SPACED}") + endif() + + add_custom_target(${Coverage_NAME} + COMMAND ${GCOVR_XML_EXEC_TESTS_CMD} + COMMAND ${GCOVR_XML_CMD} + + BYPRODUCTS ${Coverage_NAME}.xml + WORKING_DIRECTORY ${PROJECT_BINARY_DIR} + DEPENDS ${Coverage_DEPENDENCIES} + VERBATIM # Protect arguments to commands + COMMENT "Running gcovr to produce Cobertura code coverage report." + ) + + # Show info where to find the report + add_custom_command(TARGET ${Coverage_NAME} POST_BUILD + COMMAND ; + COMMENT "Cobertura code coverage report saved in ${Coverage_NAME}.xml." + ) +endfunction() # setup_target_for_coverage_gcovr_xml + +# Defines a target for running and collection code coverage information +# Builds dependencies, runs the given executable and outputs reports. +# NOTE! The executable should always have a ZERO as exit code otherwise +# the coverage generation will not complete. +# +# setup_target_for_coverage_gcovr_html( +# NAME ctest_coverage # New target name +# EXECUTABLE ctest -j ${PROCESSOR_COUNT} # Executable in PROJECT_BINARY_DIR +# DEPENDENCIES executable_target # Dependencies to build first +# BASE_DIRECTORY "../" # Base directory for report +# # (defaults to PROJECT_SOURCE_DIR) +# EXCLUDE "src/dir1/*" "src/dir2/*" # Patterns to exclude (can be relative +# # to BASE_DIRECTORY, with CMake 3.4+) +# ) +# The user can set the variable GCOVR_ADDITIONAL_ARGS to supply additional flags to the +# GCVOR command. +function(setup_target_for_coverage_gcovr_html) + + set(options NONE) + set(oneValueArgs BASE_DIRECTORY NAME) + set(multiValueArgs EXCLUDE EXECUTABLE EXECUTABLE_ARGS DEPENDENCIES) + cmake_parse_arguments(Coverage "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(NOT GCOVR_PATH) + message(FATAL_ERROR "gcovr not found! Aborting...") + endif() # NOT GCOVR_PATH + + # Set base directory (as absolute path), or default to PROJECT_SOURCE_DIR + if(DEFINED Coverage_BASE_DIRECTORY) + get_filename_component(BASEDIR ${Coverage_BASE_DIRECTORY} ABSOLUTE) + else() + set(BASEDIR ${PROJECT_SOURCE_DIR}) + endif() + + # Collect excludes (CMake 3.4+: Also compute absolute paths) + set(GCOVR_EXCLUDES "") + foreach(EXCLUDE ${Coverage_EXCLUDE} ${COVERAGE_EXCLUDES} ${COVERAGE_GCOVR_EXCLUDES}) + if(CMAKE_VERSION VERSION_GREATER 3.4) + get_filename_component(EXCLUDE ${EXCLUDE} ABSOLUTE BASE_DIR ${BASEDIR}) + endif() + list(APPEND GCOVR_EXCLUDES "${EXCLUDE}") + endforeach() + list(REMOVE_DUPLICATES GCOVR_EXCLUDES) + + # Combine excludes to several -e arguments + set(GCOVR_EXCLUDE_ARGS "") + foreach(EXCLUDE ${GCOVR_EXCLUDES}) + list(APPEND GCOVR_EXCLUDE_ARGS "-e") + list(APPEND GCOVR_EXCLUDE_ARGS "${EXCLUDE}") + endforeach() + + # Set up commands which will be run to generate coverage data + # Run tests + set(GCOVR_HTML_EXEC_TESTS_CMD + ${Coverage_EXECUTABLE} ${Coverage_EXECUTABLE_ARGS} + ) + # Create folder + set(GCOVR_HTML_FOLDER_CMD + ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/${Coverage_NAME} + ) + # Running gcovr + set(GCOVR_HTML_CMD + ${GCOVR_PATH} --html ${Coverage_NAME}/index.html --html-details -r ${BASEDIR} ${GCOVR_ADDITIONAL_ARGS} + ${GCOVR_EXCLUDE_ARGS} --object-directory=${PROJECT_BINARY_DIR} + ) + + if(CODE_COVERAGE_VERBOSE) + message(STATUS "Executed command report") + + message(STATUS "Command to run tests: ") + string(REPLACE ";" " " GCOVR_HTML_EXEC_TESTS_CMD_SPACED "${GCOVR_HTML_EXEC_TESTS_CMD}") + message(STATUS "${GCOVR_HTML_EXEC_TESTS_CMD_SPACED}") + + message(STATUS "Command to create a folder: ") + string(REPLACE ";" " " GCOVR_HTML_FOLDER_CMD_SPACED "${GCOVR_HTML_FOLDER_CMD}") + message(STATUS "${GCOVR_HTML_FOLDER_CMD_SPACED}") + + message(STATUS "Command to generate gcovr HTML coverage data: ") + string(REPLACE ";" " " GCOVR_HTML_CMD_SPACED "${GCOVR_HTML_CMD}") + message(STATUS "${GCOVR_HTML_CMD_SPACED}") + endif() + + add_custom_target(${Coverage_NAME} + COMMAND ${GCOVR_HTML_EXEC_TESTS_CMD} + COMMAND ${GCOVR_HTML_FOLDER_CMD} + COMMAND ${GCOVR_HTML_CMD} + + BYPRODUCTS ${PROJECT_BINARY_DIR}/${Coverage_NAME}/index.html # report directory + WORKING_DIRECTORY ${PROJECT_BINARY_DIR} + DEPENDS ${Coverage_DEPENDENCIES} + VERBATIM # Protect arguments to commands + COMMENT "Running gcovr to produce HTML code coverage report." + ) + + # Show info where to find the report + add_custom_command(TARGET ${Coverage_NAME} POST_BUILD + COMMAND ; + COMMENT "Open ./${Coverage_NAME}/index.html in your browser to view the coverage report." + ) + +endfunction() # setup_target_for_coverage_gcovr_html + +# Defines a target for running and collection code coverage information +# Builds dependencies, runs the given executable and outputs reports. +# NOTE! The executable should always have a ZERO as exit code otherwise +# the coverage generation will not complete. +# +# setup_target_for_coverage_fastcov( +# NAME testrunner_coverage # New target name +# EXECUTABLE testrunner -j ${PROCESSOR_COUNT} # Executable in PROJECT_BINARY_DIR +# DEPENDENCIES testrunner # Dependencies to build first +# BASE_DIRECTORY "../" # Base directory for report +# # (defaults to PROJECT_SOURCE_DIR) +# EXCLUDE "src/dir1/" "src/dir2/" # Patterns to exclude. +# NO_DEMANGLE # Don't demangle C++ symbols +# # even if c++filt is found +# SKIP_HTML # Don't create html report +# POST_CMD perl -i -pe s!${PROJECT_SOURCE_DIR}/!!g ctest_coverage.json # E.g. for stripping source dir from file paths +# ) +function(setup_target_for_coverage_fastcov) + + set(options NO_DEMANGLE SKIP_HTML) + set(oneValueArgs BASE_DIRECTORY NAME) + set(multiValueArgs EXCLUDE EXECUTABLE EXECUTABLE_ARGS DEPENDENCIES FASTCOV_ARGS GENHTML_ARGS POST_CMD) + cmake_parse_arguments(Coverage "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(NOT FASTCOV_PATH) + message(FATAL_ERROR "fastcov not found! Aborting...") + endif() + + if(NOT Coverage_SKIP_HTML AND NOT GENHTML_PATH) + message(FATAL_ERROR "genhtml not found! Aborting...") + endif() + + # Set base directory (as absolute path), or default to PROJECT_SOURCE_DIR + if(Coverage_BASE_DIRECTORY) + get_filename_component(BASEDIR ${Coverage_BASE_DIRECTORY} ABSOLUTE) + else() + set(BASEDIR ${PROJECT_SOURCE_DIR}) + endif() + + # Collect excludes (Patterns, not paths, for fastcov) + set(FASTCOV_EXCLUDES "") + foreach(EXCLUDE ${Coverage_EXCLUDE} ${COVERAGE_EXCLUDES} ${COVERAGE_FASTCOV_EXCLUDES}) + list(APPEND FASTCOV_EXCLUDES "${EXCLUDE}") + endforeach() + list(REMOVE_DUPLICATES FASTCOV_EXCLUDES) + + # Conditional arguments + if(CPPFILT_PATH AND NOT ${Coverage_NO_DEMANGLE}) + set(GENHTML_EXTRA_ARGS "--demangle-cpp") + endif() + + # Set up commands which will be run to generate coverage data + set(FASTCOV_EXEC_TESTS_CMD ${Coverage_EXECUTABLE} ${Coverage_EXECUTABLE_ARGS}) + + set(FASTCOV_CAPTURE_CMD ${FASTCOV_PATH} ${Coverage_FASTCOV_ARGS} --gcov ${GCOV_PATH} + --search-directory ${BASEDIR} + --process-gcno + --output ${Coverage_NAME}.json + --exclude ${FASTCOV_EXCLUDES} + --exclude ${FASTCOV_EXCLUDES} + ) + + set(FASTCOV_CONVERT_CMD ${FASTCOV_PATH} + -C ${Coverage_NAME}.json --lcov --output ${Coverage_NAME}.info + ) + + if(Coverage_SKIP_HTML) + set(FASTCOV_HTML_CMD ";") + else() + set(FASTCOV_HTML_CMD ${GENHTML_PATH} ${GENHTML_EXTRA_ARGS} ${Coverage_GENHTML_ARGS} + -o ${Coverage_NAME} ${Coverage_NAME}.info + ) + endif() + + set(FASTCOV_POST_CMD ";") + if(Coverage_POST_CMD) + set(FASTCOV_POST_CMD ${Coverage_POST_CMD}) + endif() + + if(CODE_COVERAGE_VERBOSE) + message(STATUS "Code coverage commands for target ${Coverage_NAME} (fastcov):") + + message(" Running tests:") + string(REPLACE ";" " " FASTCOV_EXEC_TESTS_CMD_SPACED "${FASTCOV_EXEC_TESTS_CMD}") + message(" ${FASTCOV_EXEC_TESTS_CMD_SPACED}") + + message(" Capturing fastcov counters and generating report:") + string(REPLACE ";" " " FASTCOV_CAPTURE_CMD_SPACED "${FASTCOV_CAPTURE_CMD}") + message(" ${FASTCOV_CAPTURE_CMD_SPACED}") + + message(" Converting fastcov .json to lcov .info:") + string(REPLACE ";" " " FASTCOV_CONVERT_CMD_SPACED "${FASTCOV_CONVERT_CMD}") + message(" ${FASTCOV_CONVERT_CMD_SPACED}") + + if(NOT Coverage_SKIP_HTML) + message(" Generating HTML report: ") + string(REPLACE ";" " " FASTCOV_HTML_CMD_SPACED "${FASTCOV_HTML_CMD}") + message(" ${FASTCOV_HTML_CMD_SPACED}") + endif() + if(Coverage_POST_CMD) + message(" Running post command: ") + string(REPLACE ";" " " FASTCOV_POST_CMD_SPACED "${FASTCOV_POST_CMD}") + message(" ${FASTCOV_POST_CMD_SPACED}") + endif() + endif() + + # Setup target + add_custom_target(${Coverage_NAME} + + # Cleanup fastcov + COMMAND ${FASTCOV_PATH} ${Coverage_FASTCOV_ARGS} --gcov ${GCOV_PATH} + --search-directory ${BASEDIR} + --zerocounters + + COMMAND ${FASTCOV_EXEC_TESTS_CMD} + COMMAND ${FASTCOV_CAPTURE_CMD} + COMMAND ${FASTCOV_CONVERT_CMD} + COMMAND ${FASTCOV_HTML_CMD} + COMMAND ${FASTCOV_POST_CMD} + + # Set output files as GENERATED (will be removed on 'make clean') + BYPRODUCTS + ${Coverage_NAME}.info + ${Coverage_NAME}.json + ${Coverage_NAME}/index.html # report directory + + WORKING_DIRECTORY ${PROJECT_BINARY_DIR} + DEPENDS ${Coverage_DEPENDENCIES} + VERBATIM # Protect arguments to commands + COMMENT "Resetting code coverage counters to zero. Processing code coverage counters and generating report." + ) + + set(INFO_MSG "fastcov code coverage info report saved in ${Coverage_NAME}.info and ${Coverage_NAME}.json.") + if(NOT Coverage_SKIP_HTML) + string(APPEND INFO_MSG " Open ${PROJECT_BINARY_DIR}/${Coverage_NAME}/index.html in your browser to view the coverage report.") + endif() + # Show where to find the fastcov info report + add_custom_command(TARGET ${Coverage_NAME} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E echo ${INFO_MSG} + ) + +endfunction() # setup_target_for_coverage_fastcov + +function(append_coverage_compiler_flags) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${COVERAGE_COMPILER_FLAGS}" PARENT_SCOPE) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${COVERAGE_COMPILER_FLAGS}" PARENT_SCOPE) + set(CMAKE_Fortran_FLAGS "${CMAKE_Fortran_FLAGS} ${COVERAGE_COMPILER_FLAGS}" PARENT_SCOPE) + message(STATUS "Appending code coverage compiler flags: ${COVERAGE_COMPILER_FLAGS}") +endfunction() # append_coverage_compiler_flags + +# Setup coverage for specific library +function(append_coverage_compiler_flags_to_target name) + target_compile_options(${name} + PRIVATE ${COVERAGE_COMPILER_FLAGS}) +endfunction() diff --git a/cmake/cmake-modules/bilke/LICENSE_1_0.txt b/cmake/cmake-modules/bilke/LICENSE_1_0.txt new file mode 100644 index 000000000..36b7cd93c --- /dev/null +++ b/cmake/cmake-modules/bilke/LICENSE_1_0.txt @@ -0,0 +1,23 @@ +Boost Software License - Version 1.0 - August 17th, 2003 + +Permission is hereby granted, free of charge, to any person or organization +obtaining a copy of the software and accompanying documentation covered by +this license (the "Software") to use, reproduce, display, distribute, +execute, and transmit the Software, and to prepare derivative works of the +Software, and to permit third-parties to whom the Software is furnished to +do so, all subject to the following: + +The copyright notices in the Software and this entire statement, including +the above license grant, this restriction and the following disclaimer, +must be included in all copies of the Software, in whole or in part, and +all derivative works of the Software, unless such copies or derivative +works are solely in the form of machine-executable object code generated by +a source language processor. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT +SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE +FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +DEALINGS IN THE SOFTWARE. diff --git a/cmake/cmake-modules/rpavlik/GetGitRevisionDescription.cmake b/cmake/cmake-modules/rpavlik/GetGitRevisionDescription.cmake new file mode 100644 index 000000000..69ef78b28 --- /dev/null +++ b/cmake/cmake-modules/rpavlik/GetGitRevisionDescription.cmake @@ -0,0 +1,284 @@ +# - Returns a version string from Git +# +# These functions force a re-configure on each git commit so that you can +# trust the values of the variables in your build system. +# +# get_git_head_revision( [ALLOW_LOOKING_ABOVE_CMAKE_SOURCE_DIR]) +# +# Returns the refspec and sha hash of the current head revision +# +# git_describe( [ ...]) +# +# Returns the results of git describe on the source tree, and adjusting +# the output so that it tests false if an error occurs. +# +# git_describe_working_tree( [ ...]) +# +# Returns the results of git describe on the working tree (--dirty option), +# and adjusting the output so that it tests false if an error occurs. +# +# git_get_exact_tag( [ ...]) +# +# Returns the results of git describe --exact-match on the source tree, +# and adjusting the output so that it tests false if there was no exact +# matching tag. +# +# git_local_changes() +# +# Returns either "CLEAN" or "DIRTY" with respect to uncommitted changes. +# Uses the return code of "git diff-index --quiet HEAD --". +# Does not regard untracked files. +# +# Requires CMake 2.6 or newer (uses the 'function' command) +# +# Original Author: +# 2009-2020 Ryan Pavlik +# http://academic.cleardefinition.com +# +# Copyright 2009-2013, Iowa State University. +# Copyright 2013-2020, Ryan Pavlik +# Copyright 2013-2020, Contributors +# SPDX-License-Identifier: BSL-1.0 +# Distributed under the Boost Software License, Version 1.0. +# (See accompanying file LICENSE_1_0.txt or copy at +# http://www.boost.org/LICENSE_1_0.txt) + +if(__get_git_revision_description) + return() +endif() +set(__get_git_revision_description YES) + +# We must run the following at "include" time, not at function call time, +# to find the path to this module rather than the path to a calling list file +get_filename_component(_gitdescmoddir ${CMAKE_CURRENT_LIST_FILE} PATH) + +# Function _git_find_closest_git_dir finds the next closest .git directory +# that is part of any directory in the path defined by _start_dir. +# The result is returned in the parent scope variable whose name is passed +# as variable _git_dir_var. If no .git directory can be found, the +# function returns an empty string via _git_dir_var. +# +# Example: Given a path C:/bla/foo/bar and assuming C:/bla/.git exists and +# neither foo nor bar contain a file/directory .git. This wil return +# C:/bla/.git +# +function(_git_find_closest_git_dir _start_dir _git_dir_var) + set(cur_dir "${_start_dir}") + set(git_dir "${_start_dir}/.git") + while(NOT EXISTS "${git_dir}") + # .git dir not found, search parent directories + set(git_previous_parent "${cur_dir}") + get_filename_component(cur_dir "${cur_dir}" DIRECTORY) + if(cur_dir STREQUAL git_previous_parent) + # We have reached the root directory, we are not in git + set(${_git_dir_var} + "" + PARENT_SCOPE) + return() + endif() + set(git_dir "${cur_dir}/.git") + endwhile() + set(${_git_dir_var} + "${git_dir}" + PARENT_SCOPE) +endfunction() + +function(get_git_head_revision _refspecvar _hashvar) + _git_find_closest_git_dir("${CMAKE_CURRENT_SOURCE_DIR}" GIT_DIR) + + if("${ARGN}" STREQUAL "ALLOW_LOOKING_ABOVE_CMAKE_SOURCE_DIR") + set(ALLOW_LOOKING_ABOVE_CMAKE_SOURCE_DIR TRUE) + else() + set(ALLOW_LOOKING_ABOVE_CMAKE_SOURCE_DIR FALSE) + endif() + if(NOT "${GIT_DIR}" STREQUAL "") + file(RELATIVE_PATH _relative_to_source_dir "${CMAKE_SOURCE_DIR}" + "${GIT_DIR}") + if("${_relative_to_source_dir}" MATCHES "[.][.]" AND NOT ALLOW_LOOKING_ABOVE_CMAKE_SOURCE_DIR) + # We've gone above the CMake root dir. + set(GIT_DIR "") + endif() + endif() + if("${GIT_DIR}" STREQUAL "") + set(${_refspecvar} + "GITDIR-NOTFOUND" + PARENT_SCOPE) + set(${_hashvar} + "GITDIR-NOTFOUND" + PARENT_SCOPE) + return() + endif() + + # Check if the current source dir is a git submodule or a worktree. + # In both cases .git is a file instead of a directory. + # + if(NOT IS_DIRECTORY ${GIT_DIR}) + # The following git command will return a non empty string that + # points to the super project working tree if the current + # source dir is inside a git submodule. + # Otherwise the command will return an empty string. + # + execute_process( + COMMAND "${GIT_EXECUTABLE}" rev-parse + --show-superproject-working-tree + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + OUTPUT_VARIABLE out + ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE) + if(NOT "${out}" STREQUAL "") + # If out is empty, GIT_DIR/CMAKE_CURRENT_SOURCE_DIR is in a submodule + file(READ ${GIT_DIR} submodule) + string(REGEX REPLACE "gitdir: (.*)$" "\\1" GIT_DIR_RELATIVE + ${submodule}) + string(STRIP ${GIT_DIR_RELATIVE} GIT_DIR_RELATIVE) + get_filename_component(SUBMODULE_DIR ${GIT_DIR} PATH) + get_filename_component(GIT_DIR ${SUBMODULE_DIR}/${GIT_DIR_RELATIVE} + ABSOLUTE) + set(HEAD_SOURCE_FILE "${GIT_DIR}/HEAD") + else() + # GIT_DIR/CMAKE_CURRENT_SOURCE_DIR is in a worktree + file(READ ${GIT_DIR} worktree_ref) + # The .git directory contains a path to the worktree information directory + # inside the parent git repo of the worktree. + # + string(REGEX REPLACE "gitdir: (.*)$" "\\1" git_worktree_dir + ${worktree_ref}) + string(STRIP ${git_worktree_dir} git_worktree_dir) + _git_find_closest_git_dir("${git_worktree_dir}" GIT_DIR) + set(HEAD_SOURCE_FILE "${git_worktree_dir}/HEAD") + endif() + else() + set(HEAD_SOURCE_FILE "${GIT_DIR}/HEAD") + endif() + set(GIT_DATA "${CMAKE_CURRENT_BINARY_DIR}/CMakeFiles/git-data") + if(NOT EXISTS "${GIT_DATA}") + file(MAKE_DIRECTORY "${GIT_DATA}") + endif() + + if(NOT EXISTS "${HEAD_SOURCE_FILE}") + return() + endif() + set(HEAD_FILE "${GIT_DATA}/HEAD") + configure_file("${HEAD_SOURCE_FILE}" "${HEAD_FILE}" COPYONLY) + + configure_file("${_gitdescmoddir}/GetGitRevisionDescription.cmake.in" + "${GIT_DATA}/grabRef.cmake" @ONLY) + include("${GIT_DATA}/grabRef.cmake") + + set(${_refspecvar} + "${HEAD_REF}" + PARENT_SCOPE) + set(${_hashvar} + "${HEAD_HASH}" + PARENT_SCOPE) +endfunction() + +function(git_describe _var) + if(NOT GIT_FOUND) + find_package(Git QUIET) + endif() + get_git_head_revision(refspec hash) + if(NOT GIT_FOUND) + set(${_var} + "GIT-NOTFOUND" + PARENT_SCOPE) + return() + endif() + if(NOT hash) + set(${_var} + "HEAD-HASH-NOTFOUND" + PARENT_SCOPE) + return() + endif() + + # TODO sanitize + #if((${ARGN}" MATCHES "&&") OR + # (ARGN MATCHES "||") OR + # (ARGN MATCHES "\\;")) + # message("Please report the following error to the project!") + # message(FATAL_ERROR "Looks like someone's doing something nefarious with git_describe! Passed arguments ${ARGN}") + #endif() + + #message(STATUS "Arguments to execute_process: ${ARGN}") + + execute_process( + COMMAND "${GIT_EXECUTABLE}" describe --tags --always ${hash} ${ARGN} + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + RESULT_VARIABLE res + OUTPUT_VARIABLE out + ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE) + if(NOT res EQUAL 0) + set(out "${out}-${res}-NOTFOUND") + endif() + + set(${_var} + "${out}" + PARENT_SCOPE) +endfunction() + +function(git_describe_working_tree _var) + if(NOT GIT_FOUND) + find_package(Git QUIET) + endif() + if(NOT GIT_FOUND) + set(${_var} + "GIT-NOTFOUND" + PARENT_SCOPE) + return() + endif() + + execute_process( + COMMAND "${GIT_EXECUTABLE}" describe --dirty ${ARGN} + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + RESULT_VARIABLE res + OUTPUT_VARIABLE out + ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE) + if(NOT res EQUAL 0) + set(out "${out}-${res}-NOTFOUND") + endif() + + set(${_var} + "${out}" + PARENT_SCOPE) +endfunction() + +function(git_get_exact_tag _var) + git_describe(out --exact-match ${ARGN}) + set(${_var} + "${out}" + PARENT_SCOPE) +endfunction() + +function(git_local_changes _var) + if(NOT GIT_FOUND) + find_package(Git QUIET) + endif() + get_git_head_revision(refspec hash) + if(NOT GIT_FOUND) + set(${_var} + "GIT-NOTFOUND" + PARENT_SCOPE) + return() + endif() + if(NOT hash) + set(${_var} + "HEAD-HASH-NOTFOUND" + PARENT_SCOPE) + return() + endif() + + execute_process( + COMMAND "${GIT_EXECUTABLE}" diff-index --quiet HEAD -- + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + RESULT_VARIABLE res + OUTPUT_VARIABLE out + ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE) + if(res EQUAL 0) + set(${_var} + "CLEAN" + PARENT_SCOPE) + else() + set(${_var} + "DIRTY" + PARENT_SCOPE) + endif() +endfunction() diff --git a/cmake/cmake-modules/rpavlik/GetGitRevisionDescription.cmake.in b/cmake/cmake-modules/rpavlik/GetGitRevisionDescription.cmake.in new file mode 100644 index 000000000..66eee6377 --- /dev/null +++ b/cmake/cmake-modules/rpavlik/GetGitRevisionDescription.cmake.in @@ -0,0 +1,43 @@ +# +# Internal file for GetGitRevisionDescription.cmake +# +# Requires CMake 2.6 or newer (uses the 'function' command) +# +# Original Author: +# 2009-2010 Ryan Pavlik +# http://academic.cleardefinition.com +# Iowa State University HCI Graduate Program/VRAC +# +# Copyright 2009-2012, Iowa State University +# Copyright 2011-2015, Contributors +# Distributed under the Boost Software License, Version 1.0. +# (See accompanying file LICENSE_1_0.txt or copy at +# http://www.boost.org/LICENSE_1_0.txt) +# SPDX-License-Identifier: BSL-1.0 + +set(HEAD_HASH) + +file(READ "@HEAD_FILE@" HEAD_CONTENTS LIMIT 1024) + +string(STRIP "${HEAD_CONTENTS}" HEAD_CONTENTS) +if(HEAD_CONTENTS MATCHES "ref") + # named branch + string(REPLACE "ref: " "" HEAD_REF "${HEAD_CONTENTS}") + if(EXISTS "@GIT_DIR@/${HEAD_REF}") + configure_file("@GIT_DIR@/${HEAD_REF}" "@GIT_DATA@/head-ref" COPYONLY) + else() + configure_file("@GIT_DIR@/packed-refs" "@GIT_DATA@/packed-refs" COPYONLY) + file(READ "@GIT_DATA@/packed-refs" PACKED_REFS) + if(${PACKED_REFS} MATCHES "([0-9a-z]*) ${HEAD_REF}") + set(HEAD_HASH "${CMAKE_MATCH_1}") + endif() + endif() +else() + # detached HEAD + configure_file("@GIT_DIR@/HEAD" "@GIT_DATA@/head-ref" COPYONLY) +endif() + +if(NOT HEAD_HASH) + file(READ "@GIT_DATA@/head-ref" HEAD_HASH LIMIT 1024) + string(STRIP "${HEAD_HASH}" HEAD_HASH) +endif() diff --git a/cmake/cmake-modules/rpavlik/LICENSES/BSD-3-Clause.txt b/cmake/cmake-modules/rpavlik/LICENSES/BSD-3-Clause.txt new file mode 100644 index 000000000..0741db789 --- /dev/null +++ b/cmake/cmake-modules/rpavlik/LICENSES/BSD-3-Clause.txt @@ -0,0 +1,26 @@ +Copyright (c) . All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the following disclaimer in the documentation +and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors +may be used to endorse or promote products derived from this software without +specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE +USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/cmake/cmake-modules/rpavlik/LICENSES/BSL-1.0.txt b/cmake/cmake-modules/rpavlik/LICENSES/BSL-1.0.txt new file mode 100644 index 000000000..cff35365a --- /dev/null +++ b/cmake/cmake-modules/rpavlik/LICENSES/BSL-1.0.txt @@ -0,0 +1,23 @@ +Boost Software License - Version 1.0 - August 17th, 2003 + +Permission is hereby granted, free of charge, to any person or organization +obtaining a copy of the software and accompanying documentation covered by +this license (the "Software") to use, reproduce, display, distribute, execute, +and transmit the Software, and to prepare derivative works of the Software, +and to permit third-parties to whom the Software is furnished to do so, all +subject to the following: + +The copyright notices in the Software and this entire statement, including +the above license grant, this restriction and the following disclaimer, must +be included in all copies of the Software, in whole or in part, and all derivative +works of the Software, unless such copies or derivative works are solely in +the form of machine-executable object code generated by a source language +processor. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS +FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE FOR ANY DAMAGES +OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/cmake/cmake-modules/rpavlik/LICENSE_1_0.txt b/cmake/cmake-modules/rpavlik/LICENSE_1_0.txt new file mode 100644 index 000000000..36b7cd93c --- /dev/null +++ b/cmake/cmake-modules/rpavlik/LICENSE_1_0.txt @@ -0,0 +1,23 @@ +Boost Software License - Version 1.0 - August 17th, 2003 + +Permission is hereby granted, free of charge, to any person or organization +obtaining a copy of the software and accompanying documentation covered by +this license (the "Software") to use, reproduce, display, distribute, +execute, and transmit the Software, and to prepare derivative works of the +Software, and to permit third-parties to whom the Software is furnished to +do so, all subject to the following: + +The copyright notices in the Software and this entire statement, including +the above license grant, this restriction and the following disclaimer, +must be included in all copies of the Software, in whole or in part, and +all derivative works of the Software, unless such copies or derivative +works are solely in the form of machine-executable object code generated by +a source language processor. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT +SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE +FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +DEALINGS IN THE SOFTWARE. diff --git a/docs/getting_started.rst b/docs/getting_started.rst index 34547211e..01724b3a4 100644 --- a/docs/getting_started.rst +++ b/docs/getting_started.rst @@ -19,6 +19,29 @@ A template configuration folder was provided and can be copied into the project a starting point. The [configuration section](docs/README-config.md#top) provides more specific information about the possible options. +Prerequisites +------------------- + +The Embedded Template Library (etl) is a dependency of the FSFW which is automatically +installed and provided by the build system unless the correction version was installed. +The current recommended version can be found inside the fsfw ``CMakeLists.txt`` file or by using +``ccmake`` and looking up the ``FSFW_ETL_LIB_MAJOR_VERSION`` variable. + +You can install the ETL library like this. On Linux, it might be necessary to add ``sudo`` before +the install call: + +.. code-block:: console + + git clone https://github.com/ETLCPP/etl + cd etl + git checkout + mkdir build && cd build + cmake .. + cmake --install . + +It is recommended to install ``20.27.2`` or newer for the package version handling of +ETL to work. + Adding the library ------------------- @@ -60,6 +83,20 @@ The FSFW also has unittests which use the `Catch2 library`_. These are built by setting the CMake option ``FSFW_BUILD_UNITTESTS`` to ``ON`` or `TRUE` from your project `CMakeLists.txt` file or from the command line. +You can install the Catch2 library, which prevents the build system to avoid re-downloading +the dependency if the unit tests are completely rebuilt. The current recommended version +can be found inside the fsfw ``CMakeLists.txt`` file or by using ``ccmake`` and looking up +the ``FSFW_CATCH2_LIB_VERSION`` variable. + +.. code-block:: console + + git clone https://github.com/catchorg/Catch2.git + cd Catch2 + git checkout + cmake -Bbuild -H. -DBUILD_TESTING=OFF + sudo cmake --build build/ --target install + + The fsfw-tests binary will be built as part of the static library and dropped alongside it. If the unittests are built, the library and the tests will be built with coverage information by default. This can be disabled by setting the `FSFW_TESTS_COV_GEN` option to `OFF` or `FALSE`. diff --git a/hal/CMakeLists.txt b/hal/CMakeLists.txt deleted file mode 100644 index 424a012d5..000000000 --- a/hal/CMakeLists.txt +++ /dev/null @@ -1,47 +0,0 @@ -cmake_minimum_required(VERSION 3.13) - -# Can also be changed by upper CMakeLists.txt file -find_library(LIB_FSFW_NAME fsfw REQUIRED) - -option(FSFW_HAL_ADD_LINUX "Add the Linux HAL to the sources. Requires gpiod library" OFF) -# On by default for now because I did not have an issue including and compiling those files -# and libraries on a Desktop Linux system and the primary target of the FSFW is still embedded -# Linux. The only exception from this is the gpiod library which requires a dedicated installation, -# but CMake is able to determine whether this library is installed with find_library. -option(FSFW_HAL_LINUX_ADD_PERIPHERAL_DRIVERS "Add peripheral drivers for embedded Linux" ON) - -option(FSFW_HAL_ADD_RASPBERRY_PI "Add Raspberry Pi specific code to the sources" OFF) -option(FSFW_HAL_ADD_STM32H7 "Add the STM32H7 HAL to the sources" OFF) -option(FSFW_HAL_WARNING_SHADOW_LOCAL_GCC "Enable -Wshadow=local warning in GCC" ON) - -set(LINUX_HAL_PATH_NAME linux) -set(STM32H7_PATH_NAME stm32h7) - -add_subdirectory(src) - -foreach(INCLUDE_PATH ${FSFW_HAL_ADDITIONAL_INC_PATHS}) - if(IS_ABSOLUTE ${INCLUDE_PATH}) - set(CURR_ABS_INC_PATH "${INCLUDE_PATH}") - else() - get_filename_component(CURR_ABS_INC_PATH - ${INCLUDE_PATH} REALPATH BASE_DIR ${CMAKE_SOURCE_DIR}) - endif() - - if(CMAKE_VERBOSE) - message(STATUS "FSFW include path: ${CURR_ABS_INC_PATH}") - endif() - - list(APPEND FSFW_HAL_ADD_INC_PATHS_ABS ${CURR_ABS_INC_PATH}) -endforeach() - -target_include_directories(${LIB_FSFW_NAME} PRIVATE - ${FSFW_HAL_ADD_INC_PATHS_ABS} -) - -target_compile_definitions(${LIB_FSFW_NAME} PRIVATE - ${FSFW_HAL_DEFINES} -) - -target_link_libraries(${LIB_FSFW_NAME} PRIVATE - ${FSFW_HAL_LINK_LIBS} -) diff --git a/hal/src/CMakeLists.txt b/hal/src/CMakeLists.txt deleted file mode 100644 index 76ee45c6d..000000000 --- a/hal/src/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -target_include_directories(${LIB_FSFW_NAME} PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR} -) - -target_include_directories(${LIB_FSFW_NAME} INTERFACE - ${CMAKE_CURRENT_SOURCE_DIR} -) - -add_subdirectory(fsfw_hal) diff --git a/hal/src/fsfw_hal/common/gpio/CMakeLists.txt b/hal/src/fsfw_hal/common/gpio/CMakeLists.txt deleted file mode 100644 index 098c05fa1..000000000 --- a/hal/src/fsfw_hal/common/gpio/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - GpioCookie.cpp -) \ No newline at end of file diff --git a/hal/src/fsfw_hal/devicehandlers/CMakeLists.txt b/hal/src/fsfw_hal/devicehandlers/CMakeLists.txt deleted file mode 100644 index 94e67c722..000000000 --- a/hal/src/fsfw_hal/devicehandlers/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - GyroL3GD20Handler.cpp - MgmRM3100Handler.cpp - MgmLIS3MDLHandler.cpp -) diff --git a/hal/src/fsfw_hal/linux/CMakeLists.txt b/hal/src/fsfw_hal/linux/CMakeLists.txt deleted file mode 100644 index 0fb2d385c..000000000 --- a/hal/src/fsfw_hal/linux/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -if(FSFW_HAL_ADD_RASPBERRY_PI) - add_subdirectory(rpi) -endif() - -target_sources(${LIB_FSFW_NAME} PRIVATE - UnixFileGuard.cpp - CommandExecutor.cpp - utility.cpp -) - -if(FSFW_HAL_LINUX_ADD_PERIPHERAL_DRIVERS) - add_subdirectory(gpio) - add_subdirectory(spi) - add_subdirectory(i2c) - add_subdirectory(uart) -endif() - -add_subdirectory(uio) diff --git a/hal/src/fsfw_hal/linux/gpio/CMakeLists.txt b/hal/src/fsfw_hal/linux/gpio/CMakeLists.txt deleted file mode 100644 index b16098504..000000000 --- a/hal/src/fsfw_hal/linux/gpio/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -# This abstraction layer requires the gpiod library. You can install this library -# with "sudo apt-get install -y libgpiod-dev". If you are cross-compiling, you need -# to install the package before syncing the sysroot to your host computer. -find_library(LIB_GPIO gpiod) - -if(${LIB_GPIO} MATCHES LIB_GPIO-NOTFOUND) - message(STATUS "gpiod library not found, not linking against it") -else() - target_sources(${LIB_FSFW_NAME} PRIVATE - LinuxLibgpioIF.cpp - ) - target_link_libraries(${LIB_FSFW_NAME} PRIVATE - ${LIB_GPIO} - ) -endif() - diff --git a/hal/src/fsfw_hal/linux/i2c/CMakeLists.txt b/hal/src/fsfw_hal/linux/i2c/CMakeLists.txt deleted file mode 100644 index 3eb0882cc..000000000 --- a/hal/src/fsfw_hal/linux/i2c/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PUBLIC - I2cComIF.cpp - I2cCookie.cpp -) - - - - diff --git a/hal/src/fsfw_hal/linux/rpi/CMakeLists.txt b/hal/src/fsfw_hal/linux/rpi/CMakeLists.txt deleted file mode 100644 index 47be218c0..000000000 --- a/hal/src/fsfw_hal/linux/rpi/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - GpioRPi.cpp -) \ No newline at end of file diff --git a/hal/src/fsfw_hal/linux/spi/CMakeLists.txt b/hal/src/fsfw_hal/linux/spi/CMakeLists.txt deleted file mode 100644 index 404e1f477..000000000 --- a/hal/src/fsfw_hal/linux/spi/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PUBLIC - SpiComIF.cpp - SpiCookie.cpp -) - - - - diff --git a/hal/src/fsfw_hal/linux/uart/CMakeLists.txt b/hal/src/fsfw_hal/linux/uart/CMakeLists.txt deleted file mode 100644 index 21ed0278f..000000000 --- a/hal/src/fsfw_hal/linux/uart/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PUBLIC - UartComIF.cpp - UartCookie.cpp -) diff --git a/hal/src/fsfw_hal/linux/uio/CMakeLists.txt b/hal/src/fsfw_hal/linux/uio/CMakeLists.txt deleted file mode 100644 index e98a08657..000000000 --- a/hal/src/fsfw_hal/linux/uio/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PUBLIC - UioMapper.cpp -) diff --git a/hal/src/fsfw_hal/stm32h7/devicetest/CMakeLists.txt b/hal/src/fsfw_hal/stm32h7/devicetest/CMakeLists.txt deleted file mode 100644 index 7bd4c3a96..000000000 --- a/hal/src/fsfw_hal/stm32h7/devicetest/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - GyroL3GD20H.cpp -) \ No newline at end of file diff --git a/hal/src/fsfw_hal/stm32h7/gpio/CMakeLists.txt b/hal/src/fsfw_hal/stm32h7/gpio/CMakeLists.txt deleted file mode 100644 index 35245b253..000000000 --- a/hal/src/fsfw_hal/stm32h7/gpio/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - gpio.cpp -) diff --git a/hal/src/fsfw_hal/stm32h7/i2c/CMakeLists.txt b/hal/src/fsfw_hal/stm32h7/i2c/CMakeLists.txt deleted file mode 100644 index 5ecb09909..000000000 --- a/hal/src/fsfw_hal/stm32h7/i2c/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE -) diff --git a/hal/src/fsfw_hal/stm32h7/spi/CMakeLists.txt b/hal/src/fsfw_hal/stm32h7/spi/CMakeLists.txt deleted file mode 100644 index aa5541bc1..000000000 --- a/hal/src/fsfw_hal/stm32h7/spi/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - spiCore.cpp - spiDefinitions.cpp - spiInterrupts.cpp - mspInit.cpp - SpiCookie.cpp - SpiComIF.cpp - stm32h743zi.cpp -) diff --git a/hal/src/fsfw_hal/stm32h7/uart/CMakeLists.txt b/hal/src/fsfw_hal/stm32h7/uart/CMakeLists.txt deleted file mode 100644 index 5ecb09909..000000000 --- a/hal/src/fsfw_hal/stm32h7/uart/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE -) diff --git a/scripts/apply-clang-format.sh b/scripts/apply-clang-format.sh deleted file mode 100755 index 272023240..000000000 --- a/scripts/apply-clang-format.sh +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/bash -if [[ ! -f README.md ]]; then - cd .. -fi - -find ./src -iname *.h -o -iname *.cpp -o -iname *.c | xargs clang-format --style=file -i -find ./hal -iname *.h -o -iname *.cpp -o -iname *.c | xargs clang-format --style=file -i -find ./tests -iname *.h -o -iname *.cpp -o -iname *.c | xargs clang-format --style=file -i diff --git a/scripts/auto-formatter.sh b/scripts/auto-formatter.sh new file mode 100755 index 000000000..c0ae7099c --- /dev/null +++ b/scripts/auto-formatter.sh @@ -0,0 +1,22 @@ +#!/bin/bash +if [[ ! -f README.md ]]; then + cd .. +fi + +cmake_fmt="cmake-format" +file_selectors="-iname CMakeLists.txt" +if command -v ${cmake_fmt} &> /dev/null; then + ${cmake_fmt} -i CMakeLists.txt + find ./src ${file_selectors} | xargs ${cmake_fmt} -i +else + echo "No ${cmake_fmt} tool found, not formatting CMake files" +fi + +cpp_format="clang-format" +file_selectors="-iname *.h -o -iname *.cpp -o -iname *.c -o -iname *.tpp" +if command -v ${cpp_format} &> /dev/null; then + find ./src ${file_selectors} | xargs ${cpp_format} --style=file -i + find ./unittests ${file_selectors} | xargs ${cpp_format} --style=file -i +else + echo "No ${cpp_format} tool found, not formatting C++/C files" +fi diff --git a/scripts/helper.py b/scripts/helper.py index 68693c40f..0ac616b61 100755 --- a/scripts/helper.py +++ b/scripts/helper.py @@ -48,6 +48,20 @@ def main(): action="store_true", help="Run valgrind on generated test binary", ) + parser.add_argument( + "-g", + "--generators", + default = "Ninja", + action="store", + help="CMake generators", + ) + parser.add_argument( + "-w", + "--windows", + default=False, + action="store_true", + help="Run on windows", + ) args = parser.parse_args() if args.all: @@ -97,11 +111,11 @@ def handle_docs_type(args, build_dir_list: list): build_directory = determine_build_dir(build_dir_list) os.chdir(build_directory) if args.build: - os.system("cmake --build . -j") + cmd_runner("cmake --build . -j") if args.open: if not os.path.isfile("docs/sphinx/index.html"): # try again.. - os.system("cmake --build . -j") + cmd_runner("cmake --build . -j") if not os.path.isfile("docs/sphinx/index.html"): print( "No Sphinx documentation file detected. " @@ -115,14 +129,14 @@ def handle_tests_type(args, build_dir_list: list): if args.create: if os.path.exists(UNITTEST_FOLDER_NAME): shutil.rmtree(UNITTEST_FOLDER_NAME) - create_tests_build_cfg() + create_tests_build_cfg(args) build_directory = UNITTEST_FOLDER_NAME elif len(build_dir_list) == 0: print( "No valid CMake tests build directory found. " "Trying to set up test build system" ) - create_tests_build_cfg() + create_tests_build_cfg(args) build_directory = UNITTEST_FOLDER_NAME elif len(build_dir_list) == 1: build_directory = build_dir_list[0] @@ -143,25 +157,26 @@ def handle_tests_type(args, build_dir_list: list): if which("valgrind") is None: print("Please install valgrind first") sys.exit(1) - if os.path.split(os.getcwd())[1] != UNITTEST_FOLDER_NAME: - # If we are in a different directory we try to switch into it but - # this might fail - os.chdir(UNITTEST_FOLDER_NAME) - os.system("valgrind --leak-check=full ./fsfw-tests") + cmd_runner("valgrind --leak-check=full ./fsfw-tests") os.chdir("..") -def create_tests_build_cfg(): +def create_tests_build_cfg(args): os.mkdir(UNITTEST_FOLDER_NAME) os.chdir(UNITTEST_FOLDER_NAME) - os.system("cmake -DFSFW_OSAL=host -DFSFW_BUILD_UNITTESTS=ON ..") + if args.windows: + cmake_cmd = 'cmake -G "' + args.generators + '" -DFSFW_OSAL=host -DFSFW_BUILD_TESTS=ON \ + -DGCOVR_PATH="py -m gcovr" ..' + else: + cmake_cmd = 'cmake -G "' + args.generators + '" -DFSFW_OSAL=host -DFSFW_BUILD_TESTS=ON ..' + cmd_runner(cmake_cmd) os.chdir("..") def create_docs_build_cfg(): os.mkdir(DOCS_FOLDER_NAME) os.chdir(DOCS_FOLDER_NAME) - os.system("cmake -DFSFW_OSAL=host -DFSFW_BUILD_DOCS=ON ..") + cmd_runner("cmake -DFSFW_OSAL=host -DFSFW_BUILD_DOCS=ON ..") os.chdir("..") @@ -184,7 +199,7 @@ def check_for_cmake_build_dir(build_dir_list: list) -> list: def perform_lcov_operation(directory: str, chdir: bool): if chdir: os.chdir(directory) - os.system("cmake --build . -- fsfw-tests_coverage -j") + cmd_runner("cmake --build . -- fsfw-tests_coverage -j") def determine_build_dir(build_dir_list: List[str]): @@ -206,5 +221,10 @@ def determine_build_dir(build_dir_list: List[str]): return build_directory +def cmd_runner(cmd: str): + print(f"Executing command: {cmd}") + os.system(cmd) + + if __name__ == "__main__": main() diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ed2f2522e..57b24bd55 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,9 +1,11 @@ -target_include_directories(${LIB_FSFW_NAME} PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR} -) +target_include_directories(${LIB_FSFW_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) -target_include_directories(${LIB_FSFW_NAME} INTERFACE - ${CMAKE_CURRENT_SOURCE_DIR} -) +target_include_directories(${LIB_FSFW_NAME} + INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) add_subdirectory(fsfw) +if(FSFW_ADD_HAL) + add_subdirectory(fsfw_hal) +endif() + +add_subdirectory(fsfw_tests) diff --git a/src/fsfw/CMakeLists.txt b/src/fsfw/CMakeLists.txt index 12c673ed6..1daad7142 100644 --- a/src/fsfw/CMakeLists.txt +++ b/src/fsfw/CMakeLists.txt @@ -1,3 +1,5 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE version.cpp) + # Core add_subdirectory(action) @@ -33,22 +35,22 @@ add_subdirectory(tmtcservices) # Optional if(FSFW_ADD_MONITORING) -add_subdirectory(monitoring) + add_subdirectory(monitoring) endif() if(FSFW_ADD_PUS) - add_subdirectory(pus) + add_subdirectory(pus) endif() if(FSFW_ADD_TMSTORAGE) - add_subdirectory(tmstorage) + add_subdirectory(tmstorage) endif() if(FSFW_ADD_COORDINATES) - add_subdirectory(coordinates) + add_subdirectory(coordinates) endif() if(FSFW_ADD_RMAP) - add_subdirectory(rmap) + add_subdirectory(rmap) endif() if(FSFW_ADD_DATALINKLAYER) - add_subdirectory(datalinklayer) + add_subdirectory(datalinklayer) endif() # OSAL diff --git a/src/fsfw/FSFW.h.in b/src/fsfw/FSFW.h.in index 88ad10cf3..0eeb00eb0 100644 --- a/src/fsfw/FSFW.h.in +++ b/src/fsfw/FSFW.h.in @@ -30,6 +30,10 @@ #define FSFW_VERBOSE_LEVEL 1 #endif /* FSFW_VERBOSE_LEVEL */ +#ifndef FSFW_DISABLE_PRINTOUT +#define FSFW_DISABLE_PRINTOUT 0 +#endif + #ifndef FSFW_USE_REALTIME_FOR_LINUX #define FSFW_USE_REALTIME_FOR_LINUX 0 #endif /* FSFW_USE_REALTIME_FOR_LINUX */ @@ -57,16 +61,9 @@ #define FSFW_HAL_SPI_WIRETAPPING 0 #endif -#ifndef FSFW_HAL_L3GD20_GYRO_DEBUG -#define FSFW_HAL_L3GD20_GYRO_DEBUG 0 -#endif /* FSFW_HAL_L3GD20_GYRO_DEBUG */ - -#ifndef FSFW_HAL_RM3100_MGM_DEBUG -#define FSFW_HAL_RM3100_MGM_DEBUG 0 -#endif /* FSFW_HAL_RM3100_MGM_DEBUG */ - -#ifndef FSFW_HAL_LIS3MDL_MGM_DEBUG -#define FSFW_HAL_LIS3MDL_MGM_DEBUG 0 -#endif /* FSFW_HAL_LIS3MDL_MGM_DEBUG */ +// Can be used for low-level debugging of the I2C bus +#ifndef FSFW_HAL_I2C_WIRETAPPING +#define FSFW_HAL_I2C_WIRETAPPING 0 +#endif #endif /* FSFW_FSFW_H_ */ diff --git a/src/fsfw/FSFWVersion.h.in b/src/fsfw/FSFWVersion.h.in index 7935b2f61..caff1efbd 100644 --- a/src/fsfw/FSFWVersion.h.in +++ b/src/fsfw/FSFWVersion.h.in @@ -1,9 +1,11 @@ #ifndef FSFW_VERSION_H_ #define FSFW_VERSION_H_ -// Versioning is kept in project CMakeLists.txt file -#define FSFW_VERSION @FSFW_VERSION@ -#define FSFW_SUBVERSION @FSFW_SUBVERSION@ -#define FSFW_REVISION @FSFW_REVISION@ +// Versioning is managed in project CMakeLists.txt file +static constexpr int FSFW_VERSION_MAJOR = @FSFW_VERSION@; +static constexpr int FSFW_VERSION_MINOR = @FSFW_SUBVERSION@; +static constexpr int FSFW_VERSION_REVISION = @FSFW_REVISION@; +// Also contains CST (Commits since tag) information +static const char FSFW_VCS_INFO[] = "@FSFW_VCS_INFO@"; #endif /* FSFW_VERSION_H_ */ diff --git a/src/fsfw/action/ActionHelper.cpp b/src/fsfw/action/ActionHelper.cpp index d41c78b4b..bee68b404 100644 --- a/src/fsfw/action/ActionHelper.cpp +++ b/src/fsfw/action/ActionHelper.cpp @@ -6,7 +6,7 @@ ActionHelper::ActionHelper(HasActionsIF* setOwner, MessageQueueIF* useThisQueue) : owner(setOwner), queueToUse(useThisQueue) {} -ActionHelper::~ActionHelper() {} +ActionHelper::~ActionHelper() = default; ReturnValue_t ActionHelper::handleActionMessage(CommandMessage* command) { if (command->getCommand() == ActionMessage::EXECUTE_ACTION) { @@ -59,7 +59,7 @@ void ActionHelper::setQueueToUse(MessageQueueIF* queue) { queueToUse = queue; } void ActionHelper::prepareExecution(MessageQueueId_t commandedBy, ActionId_t actionId, store_address_t dataAddress) { - const uint8_t* dataPtr = NULL; + const uint8_t* dataPtr = nullptr; size_t size = 0; ReturnValue_t result = ipcStore->getData(dataAddress, &dataPtr, &size); if (result != HasReturnvaluesIF::RETURN_OK) { diff --git a/src/fsfw/action/ActionHelper.h b/src/fsfw/action/ActionHelper.h index d86e87d23..a9910b05f 100644 --- a/src/fsfw/action/ActionHelper.h +++ b/src/fsfw/action/ActionHelper.h @@ -1,9 +1,9 @@ #ifndef FSFW_ACTION_ACTIONHELPER_H_ #define FSFW_ACTION_ACTIONHELPER_H_ -#include "../ipc/MessageQueueIF.h" -#include "../serialize/SerializeIF.h" #include "ActionMessage.h" +#include "fsfw/ipc/MessageQueueIF.h" +#include "fsfw/serialize/SerializeIF.h" /** * @brief Action Helper is a helper class which handles action messages * diff --git a/src/fsfw/action/ActionMessage.cpp b/src/fsfw/action/ActionMessage.cpp index 40a516b17..7fc685586 100644 --- a/src/fsfw/action/ActionMessage.cpp +++ b/src/fsfw/action/ActionMessage.cpp @@ -2,9 +2,9 @@ #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/storagemanager/StorageManagerIF.h" -ActionMessage::ActionMessage() {} +ActionMessage::ActionMessage() = default; -ActionMessage::~ActionMessage() {} +ActionMessage::~ActionMessage() = default; void ActionMessage::setCommand(CommandMessage* message, ActionId_t fid, store_address_t parameters) { @@ -64,9 +64,8 @@ void ActionMessage::clear(CommandMessage* message) { switch (message->getCommand()) { case EXECUTE_ACTION: case DATA_REPLY: { - StorageManagerIF* ipcStore = - ObjectManager::instance()->get(objects::IPC_STORE); - if (ipcStore != NULL) { + auto* ipcStore = ObjectManager::instance()->get(objects::IPC_STORE); + if (ipcStore != nullptr) { ipcStore->deleteData(getStoreId(message)); } break; diff --git a/src/fsfw/action/CMakeLists.txt b/src/fsfw/action/CMakeLists.txt index f9ac451df..7fb397af3 100644 --- a/src/fsfw/action/CMakeLists.txt +++ b/src/fsfw/action/CMakeLists.txt @@ -1,7 +1,3 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - ActionHelper.cpp - ActionMessage.cpp - CommandActionHelper.cpp - SimpleActionHelper.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} PRIVATE ActionHelper.cpp ActionMessage.cpp + CommandActionHelper.cpp SimpleActionHelper.cpp) diff --git a/src/fsfw/action/CommandActionHelper.cpp b/src/fsfw/action/CommandActionHelper.cpp index 19d8e9b89..a06bc44c9 100644 --- a/src/fsfw/action/CommandActionHelper.cpp +++ b/src/fsfw/action/CommandActionHelper.cpp @@ -2,14 +2,14 @@ #include "fsfw/objectmanager/ObjectManager.h" CommandActionHelper::CommandActionHelper(CommandsActionsIF *setOwner) - : owner(setOwner), queueToUse(NULL), ipcStore(NULL), commandCount(0), lastTarget(0) {} + : owner(setOwner), queueToUse(nullptr), ipcStore(nullptr), commandCount(0), lastTarget(0) {} -CommandActionHelper::~CommandActionHelper() {} +CommandActionHelper::~CommandActionHelper() = default; ReturnValue_t CommandActionHelper::commandAction(object_id_t commandTo, ActionId_t actionId, SerializeIF *data) { - HasActionsIF *receiver = ObjectManager::instance()->get(commandTo); - if (receiver == NULL) { + auto *receiver = ObjectManager::instance()->get(commandTo); + if (receiver == nullptr) { return CommandsActionsIF::OBJECT_HAS_NO_FUNCTIONS; } store_address_t storeId; @@ -29,11 +29,8 @@ ReturnValue_t CommandActionHelper::commandAction(object_id_t commandTo, ActionId ReturnValue_t CommandActionHelper::commandAction(object_id_t commandTo, ActionId_t actionId, const uint8_t *data, uint32_t size) { - // if (commandCount != 0) { - // return CommandsFunctionsIF::ALREADY_COMMANDING; - // } - HasActionsIF *receiver = ObjectManager::instance()->get(commandTo); - if (receiver == NULL) { + auto *receiver = ObjectManager::instance()->get(commandTo); + if (receiver == nullptr) { return CommandsActionsIF::OBJECT_HAS_NO_FUNCTIONS; } store_address_t storeId; @@ -59,12 +56,12 @@ ReturnValue_t CommandActionHelper::sendCommand(MessageQueueId_t queueId, ActionI ReturnValue_t CommandActionHelper::initialize() { ipcStore = ObjectManager::instance()->get(objects::IPC_STORE); - if (ipcStore == NULL) { + if (ipcStore == nullptr) { return HasReturnvaluesIF::RETURN_FAILED; } queueToUse = owner->getCommandQueuePtr(); - if (queueToUse == NULL) { + if (queueToUse == nullptr) { return HasReturnvaluesIF::RETURN_FAILED; } return HasReturnvaluesIF::RETURN_OK; @@ -104,7 +101,7 @@ ReturnValue_t CommandActionHelper::handleReply(CommandMessage *reply) { uint8_t CommandActionHelper::getCommandCount() const { return commandCount; } void CommandActionHelper::extractDataForOwner(ActionId_t actionId, store_address_t storeId) { - const uint8_t *data = NULL; + const uint8_t *data = nullptr; size_t size = 0; ReturnValue_t result = ipcStore->getData(storeId, &data, &size); if (result != HasReturnvaluesIF::RETURN_OK) { diff --git a/src/fsfw/action/CommandActionHelper.h b/src/fsfw/action/CommandActionHelper.h index dd8ad7f1c..a6ec0ca72 100644 --- a/src/fsfw/action/CommandActionHelper.h +++ b/src/fsfw/action/CommandActionHelper.h @@ -14,14 +14,14 @@ class CommandActionHelper { friend class CommandsActionsIF; public: - CommandActionHelper(CommandsActionsIF* owner); + explicit CommandActionHelper(CommandsActionsIF* owner); virtual ~CommandActionHelper(); ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId, const uint8_t* data, uint32_t size); ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId, SerializeIF* data); ReturnValue_t initialize(); ReturnValue_t handleReply(CommandMessage* reply); - uint8_t getCommandCount() const; + [[nodiscard]] uint8_t getCommandCount() const; private: CommandsActionsIF* owner; diff --git a/src/fsfw/action/CommandsActionsIF.h b/src/fsfw/action/CommandsActionsIF.h index 5870a9f2a..94d9a7c4e 100644 --- a/src/fsfw/action/CommandsActionsIF.h +++ b/src/fsfw/action/CommandsActionsIF.h @@ -1,9 +1,9 @@ #ifndef FSFW_ACTION_COMMANDSACTIONSIF_H_ #define FSFW_ACTION_COMMANDSACTIONSIF_H_ -#include "../ipc/MessageQueueIF.h" -#include "../returnvalues/HasReturnvaluesIF.h" #include "CommandActionHelper.h" +#include "fsfw/ipc/MessageQueueIF.h" +#include "fsfw/returnvalues/HasReturnvaluesIF.h" /** * Interface to separate commanding actions of other objects. @@ -21,7 +21,7 @@ class CommandsActionsIF { static const uint8_t INTERFACE_ID = CLASS_ID::COMMANDS_ACTIONS_IF; static const ReturnValue_t OBJECT_HAS_NO_FUNCTIONS = MAKE_RETURN_CODE(1); static const ReturnValue_t ALREADY_COMMANDING = MAKE_RETURN_CODE(2); - virtual ~CommandsActionsIF() {} + virtual ~CommandsActionsIF() = default; virtual MessageQueueIF* getCommandQueuePtr() = 0; protected: diff --git a/src/fsfw/action/HasActionsIF.h b/src/fsfw/action/HasActionsIF.h index acc502d71..89e58adde 100644 --- a/src/fsfw/action/HasActionsIF.h +++ b/src/fsfw/action/HasActionsIF.h @@ -1,11 +1,11 @@ #ifndef FSFW_ACTION_HASACTIONSIF_H_ #define FSFW_ACTION_HASACTIONSIF_H_ -#include "../ipc/MessageQueueIF.h" -#include "../returnvalues/HasReturnvaluesIF.h" #include "ActionHelper.h" #include "ActionMessage.h" #include "SimpleActionHelper.h" +#include "fsfw/ipc/MessageQueueIF.h" +#include "fsfw/returnvalues/HasReturnvaluesIF.h" /** * @brief @@ -40,12 +40,12 @@ class HasActionsIF { static const ReturnValue_t INVALID_PARAMETERS = MAKE_RETURN_CODE(2); static const ReturnValue_t EXECUTION_FINISHED = MAKE_RETURN_CODE(3); static const ReturnValue_t INVALID_ACTION_ID = MAKE_RETURN_CODE(4); - virtual ~HasActionsIF() {} + virtual ~HasActionsIF() = default; /** * Function to get the MessageQueueId_t of the implementing object * @return MessageQueueId_t of the object */ - virtual MessageQueueId_t getCommandQueue() const = 0; + [[nodiscard]] virtual MessageQueueId_t getCommandQueue() const = 0; /** * Execute or initialize the execution of a certain function. * The ActionHelpers will execute this function and behave differently diff --git a/src/fsfw/action/SimpleActionHelper.cpp b/src/fsfw/action/SimpleActionHelper.cpp index 894f0df6f..fc7e064e8 100644 --- a/src/fsfw/action/SimpleActionHelper.cpp +++ b/src/fsfw/action/SimpleActionHelper.cpp @@ -3,7 +3,7 @@ SimpleActionHelper::SimpleActionHelper(HasActionsIF* setOwner, MessageQueueIF* useThisQueue) : ActionHelper(setOwner, useThisQueue), isExecuting(false) {} -SimpleActionHelper::~SimpleActionHelper() {} +SimpleActionHelper::~SimpleActionHelper() = default; void SimpleActionHelper::step(ReturnValue_t result) { // STEP_OFFESET is subtracted to compensate for adding offset in base @@ -38,7 +38,7 @@ void SimpleActionHelper::prepareExecution(MessageQueueId_t commandedBy, ActionId ActionMessage::setStepReply(&reply, actionId, 0, HasActionsIF::IS_BUSY); queueToUse->sendMessage(commandedBy, &reply); } - const uint8_t* dataPtr = NULL; + const uint8_t* dataPtr = nullptr; size_t size = 0; ReturnValue_t result = ipcStore->getData(dataAddress, &dataPtr, &size); if (result != HasReturnvaluesIF::RETURN_OK) { diff --git a/src/fsfw/action/SimpleActionHelper.h b/src/fsfw/action/SimpleActionHelper.h index 5cb85fbd7..973c7cf2a 100644 --- a/src/fsfw/action/SimpleActionHelper.h +++ b/src/fsfw/action/SimpleActionHelper.h @@ -11,15 +11,15 @@ class SimpleActionHelper : public ActionHelper { public: SimpleActionHelper(HasActionsIF* setOwner, MessageQueueIF* useThisQueue); - virtual ~SimpleActionHelper(); + ~SimpleActionHelper() override; void step(ReturnValue_t result = HasReturnvaluesIF::RETURN_OK); void finish(ReturnValue_t result = HasReturnvaluesIF::RETURN_OK); ReturnValue_t reportData(SerializeIF* data); protected: void prepareExecution(MessageQueueId_t commandedBy, ActionId_t actionId, - store_address_t dataAddress); - virtual void resetHelper(); + store_address_t dataAddress) override; + void resetHelper() override; private: bool isExecuting; @@ -28,4 +28,4 @@ class SimpleActionHelper : public ActionHelper { uint8_t stepCount = 0; }; -#endif /* SIMPLEACTIONHELPER_H_ */ +#endif /* FSFW_ACTION_SIMPLEACTIONHELPER_H_ */ diff --git a/src/fsfw/cfdp/CMakeLists.txt b/src/fsfw/cfdp/CMakeLists.txt index 908dc32a8..0b926a9a2 100644 --- a/src/fsfw/cfdp/CMakeLists.txt +++ b/src/fsfw/cfdp/CMakeLists.txt @@ -1,7 +1,4 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - CFDPHandler.cpp - CFDPMessage.cpp -) +target_sources(${LIB_FSFW_NAME} PRIVATE CFDPHandler.cpp CFDPMessage.cpp) add_subdirectory(pdu) add_subdirectory(tlv) diff --git a/src/fsfw/cfdp/pdu/CMakeLists.txt b/src/fsfw/cfdp/pdu/CMakeLists.txt index 931db3066..4f345bdc0 100644 --- a/src/fsfw/cfdp/pdu/CMakeLists.txt +++ b/src/fsfw/cfdp/pdu/CMakeLists.txt @@ -1,32 +1,30 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - PduConfig.cpp - VarLenField.cpp - HeaderSerializer.cpp - HeaderDeserializer.cpp - FileDirectiveDeserializer.cpp - FileDirectiveSerializer.cpp - - AckInfo.cpp - AckPduSerializer.cpp - AckPduDeserializer.cpp - EofInfo.cpp - EofPduSerializer.cpp - EofPduDeserializer.cpp - NakInfo.cpp - NakPduSerializer.cpp - NakPduDeserializer.cpp - FinishedInfo.cpp - FinishedPduSerializer.cpp - FinishedPduDeserializer.cpp - MetadataInfo.cpp - MetadataPduSerializer.cpp - MetadataPduDeserializer.cpp - KeepAlivePduSerializer.cpp - KeepAlivePduDeserializer.cpp - PromptPduSerializer.cpp - PromptPduDeserializer.cpp - - FileDataSerializer.cpp - FileDataDeserializer.cpp - FileDataInfo.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} + PRIVATE PduConfig.cpp + VarLenField.cpp + HeaderSerializer.cpp + HeaderDeserializer.cpp + FileDirectiveDeserializer.cpp + FileDirectiveSerializer.cpp + AckInfo.cpp + AckPduSerializer.cpp + AckPduDeserializer.cpp + EofInfo.cpp + EofPduSerializer.cpp + EofPduDeserializer.cpp + NakInfo.cpp + NakPduSerializer.cpp + NakPduDeserializer.cpp + FinishedInfo.cpp + FinishedPduSerializer.cpp + FinishedPduDeserializer.cpp + MetadataInfo.cpp + MetadataPduSerializer.cpp + MetadataPduDeserializer.cpp + KeepAlivePduSerializer.cpp + KeepAlivePduDeserializer.cpp + PromptPduSerializer.cpp + PromptPduDeserializer.cpp + FileDataSerializer.cpp + FileDataDeserializer.cpp + FileDataInfo.cpp) diff --git a/src/fsfw/cfdp/pdu/HeaderSerializer.h b/src/fsfw/cfdp/pdu/HeaderSerializer.h index 8f2fc3fdd..1de97d63d 100644 --- a/src/fsfw/cfdp/pdu/HeaderSerializer.h +++ b/src/fsfw/cfdp/pdu/HeaderSerializer.h @@ -44,7 +44,7 @@ class HeaderSerializer : public SerializeIF, public PduHeaderIF { cfdp::WidthInBytes getLenEntityIds() const override; cfdp::WidthInBytes getLenSeqNum() const override; cfdp::SegmentMetadataFlag getSegmentMetadataFlag() const override; - bool hasSegmentMetadataFlag() const; + bool hasSegmentMetadataFlag() const override; void setSegmentationControl(cfdp::SegmentationControl); void getSourceId(cfdp::EntityId& sourceId) const override; diff --git a/src/fsfw/cfdp/tlv/CMakeLists.txt b/src/fsfw/cfdp/tlv/CMakeLists.txt index 24459cf83..cdf7b44a0 100644 --- a/src/fsfw/cfdp/tlv/CMakeLists.txt +++ b/src/fsfw/cfdp/tlv/CMakeLists.txt @@ -1,10 +1,10 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - EntityIdTlv.cpp - FilestoreRequestTlv.cpp - FilestoreResponseTlv.cpp - Lv.cpp - Tlv.cpp - FlowLabelTlv.cpp - MessageToUserTlv.cpp - FaultHandlerOverrideTlv.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} + PRIVATE EntityIdTlv.cpp + FilestoreRequestTlv.cpp + FilestoreResponseTlv.cpp + Lv.cpp + Tlv.cpp + FlowLabelTlv.cpp + MessageToUserTlv.cpp + FaultHandlerOverrideTlv.cpp) diff --git a/src/fsfw/cfdp/tlv/FilestoreTlvBase.h b/src/fsfw/cfdp/tlv/FilestoreTlvBase.h index bb9f10bba..04012cda2 100644 --- a/src/fsfw/cfdp/tlv/FilestoreTlvBase.h +++ b/src/fsfw/cfdp/tlv/FilestoreTlvBase.h @@ -6,10 +6,13 @@ #include #include #include +#include #include #include +#include "fsfw/FSFW.h" + namespace cfdp { enum FilestoreActionCode { diff --git a/src/fsfw/container/CMakeLists.txt b/src/fsfw/container/CMakeLists.txt index 13eced1d0..52087ff00 100644 --- a/src/fsfw/container/CMakeLists.txt +++ b/src/fsfw/container/CMakeLists.txt @@ -1,5 +1,2 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - SharedRingBuffer.cpp - SimpleRingBuffer.cpp -) \ No newline at end of file +target_sources(${LIB_FSFW_NAME} PRIVATE SharedRingBuffer.cpp + SimpleRingBuffer.cpp) diff --git a/src/fsfw/container/FIFOBase.tpp b/src/fsfw/container/FIFOBase.tpp index 2e6a3829c..91804b6c8 100644 --- a/src/fsfw/container/FIFOBase.tpp +++ b/src/fsfw/container/FIFOBase.tpp @@ -5,89 +5,88 @@ #error Include FIFOBase.h before FIFOBase.tpp! #endif -template -inline FIFOBase::FIFOBase(T* values, const size_t maxCapacity): - maxCapacity(maxCapacity), values(values){}; +template +inline FIFOBase::FIFOBase(T* values, const size_t maxCapacity) + : maxCapacity(maxCapacity), values(values){}; -template +template inline ReturnValue_t FIFOBase::insert(T value) { - if (full()) { - return FULL; - } else { - values[writeIndex] = value; - writeIndex = next(writeIndex); - ++currentSize; - return HasReturnvaluesIF::RETURN_OK; - } + if (full()) { + return FULL; + } else { + values[writeIndex] = value; + writeIndex = next(writeIndex); + ++currentSize; + return HasReturnvaluesIF::RETURN_OK; + } }; -template +template inline ReturnValue_t FIFOBase::retrieve(T* value) { - if (empty()) { - return EMPTY; - } else { - if (value == nullptr){ - return HasReturnvaluesIF::RETURN_FAILED; - } - *value = values[readIndex]; - readIndex = next(readIndex); - --currentSize; - return HasReturnvaluesIF::RETURN_OK; + if (empty()) { + return EMPTY; + } else { + if (value == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; } + *value = values[readIndex]; + readIndex = next(readIndex); + --currentSize; + return HasReturnvaluesIF::RETURN_OK; + } }; -template +template inline ReturnValue_t FIFOBase::peek(T* value) { - if(empty()) { - return EMPTY; - } else { - if (value == nullptr){ - return HasReturnvaluesIF::RETURN_FAILED; - } - *value = values[readIndex]; - return HasReturnvaluesIF::RETURN_OK; + if (empty()) { + return EMPTY; + } else { + if (value == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; } + *value = values[readIndex]; + return HasReturnvaluesIF::RETURN_OK; + } }; -template +template inline ReturnValue_t FIFOBase::pop() { - T value; - return this->retrieve(&value); + T value; + return this->retrieve(&value); }; -template +template inline bool FIFOBase::empty() { - return (currentSize == 0); + return (currentSize == 0); }; -template +template inline bool FIFOBase::full() { - return (currentSize == maxCapacity); + return (currentSize == maxCapacity); } -template +template inline size_t FIFOBase::size() { - return currentSize; + return currentSize; } -template +template inline size_t FIFOBase::next(size_t current) { - ++current; - if (current == maxCapacity) { - current = 0; - } - return current; + ++current; + if (current == maxCapacity) { + current = 0; + } + return current; } -template +template inline size_t FIFOBase::getMaxCapacity() const { - return maxCapacity; + return maxCapacity; } - -template -inline void FIFOBase::setContainer(T *data) { - this->values = data; +template +inline void FIFOBase::setContainer(T* data) { + this->values = data; } #endif diff --git a/src/fsfw/container/FixedArrayList.h b/src/fsfw/container/FixedArrayList.h index c09a421eb..fc8be393a 100644 --- a/src/fsfw/container/FixedArrayList.h +++ b/src/fsfw/container/FixedArrayList.h @@ -2,6 +2,7 @@ #define FIXEDARRAYLIST_H_ #include +#include #include "ArrayList.h" /** @@ -9,10 +10,9 @@ */ template class FixedArrayList : public ArrayList { -#if !defined(_MSC_VER) - static_assert(MAX_SIZE <= (std::pow(2, sizeof(count_t) * 8) - 1), + static_assert(MAX_SIZE <= std::numeric_limits::max(), "count_t is not large enough to hold MAX_SIZE"); -#endif + private: T data[MAX_SIZE]; diff --git a/src/fsfw/container/FixedOrderedMultimap.tpp b/src/fsfw/container/FixedOrderedMultimap.tpp index 294a161fe..fd58bc44a 100644 --- a/src/fsfw/container/FixedOrderedMultimap.tpp +++ b/src/fsfw/container/FixedOrderedMultimap.tpp @@ -1,109 +1,109 @@ #ifndef FRAMEWORK_CONTAINER_FIXEDORDEREDMULTIMAP_TPP_ #define FRAMEWORK_CONTAINER_FIXEDORDEREDMULTIMAP_TPP_ - -template -inline ReturnValue_t FixedOrderedMultimap::insert(key_t key, T value, Iterator *storedValue) { - if (_size == theMap.maxSize()) { - return MAP_FULL; - } - size_t position = findNicePlace(key); - memmove(static_cast(&theMap[position + 1]),static_cast(&theMap[position]), - (_size - position) * sizeof(std::pair)); - theMap[position].first = key; - theMap[position].second = value; - ++_size; - if (storedValue != nullptr) { - *storedValue = Iterator(&theMap[position]); - } - return HasReturnvaluesIF::RETURN_OK; +template +inline ReturnValue_t FixedOrderedMultimap::insert(key_t key, T value, + Iterator *storedValue) { + if (_size == theMap.maxSize()) { + return MAP_FULL; + } + size_t position = findNicePlace(key); + memmove(static_cast(&theMap[position + 1]), static_cast(&theMap[position]), + (_size - position) * sizeof(std::pair)); + theMap[position].first = key; + theMap[position].second = value; + ++_size; + if (storedValue != nullptr) { + *storedValue = Iterator(&theMap[position]); + } + return HasReturnvaluesIF::RETURN_OK; } -template +template inline ReturnValue_t FixedOrderedMultimap::insert(std::pair pair) { - return insert(pair.first, pair.second); + return insert(pair.first, pair.second); } -template +template inline ReturnValue_t FixedOrderedMultimap::exists(key_t key) const { - ReturnValue_t result = KEY_DOES_NOT_EXIST; - if (findFirstIndex(key) < _size) { - result = HasReturnvaluesIF::RETURN_OK; - } - return result; + ReturnValue_t result = KEY_DOES_NOT_EXIST; + if (findFirstIndex(key) < _size) { + result = HasReturnvaluesIF::RETURN_OK; + } + return result; } -template +template inline ReturnValue_t FixedOrderedMultimap::erase(Iterator *iter) { - size_t i; - if ((i = findFirstIndex((*iter).value->first)) >= _size) { - return KEY_DOES_NOT_EXIST; - } - removeFromPosition(i); - if (*iter != begin()) { - (*iter)--; - } else { - *iter = begin(); - } - return HasReturnvaluesIF::RETURN_OK; + size_t i; + if ((i = findFirstIndex((*iter).value->first)) >= _size) { + return KEY_DOES_NOT_EXIST; + } + removeFromPosition(i); + if (*iter != begin()) { + (*iter)--; + } else { + *iter = begin(); + } + return HasReturnvaluesIF::RETURN_OK; } -template +template inline ReturnValue_t FixedOrderedMultimap::erase(key_t key) { - size_t i; - if ((i = findFirstIndex(key)) >= _size) { - return KEY_DOES_NOT_EXIST; - } - do { - removeFromPosition(i); - i = findFirstIndex(key, i); - } while (i < _size); - return HasReturnvaluesIF::RETURN_OK; + size_t i; + if ((i = findFirstIndex(key)) >= _size) { + return KEY_DOES_NOT_EXIST; + } + do { + removeFromPosition(i); + i = findFirstIndex(key, i); + } while (i < _size); + return HasReturnvaluesIF::RETURN_OK; } -template +template inline ReturnValue_t FixedOrderedMultimap::find(key_t key, T **value) const { - ReturnValue_t result = exists(key); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - *value = &theMap[findFirstIndex(key)].second; - return HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = exists(key); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + *value = &theMap[findFirstIndex(key)].second; + return HasReturnvaluesIF::RETURN_OK; } -template -inline size_t FixedOrderedMultimap::findFirstIndex(key_t key, size_t startAt) const { - if (startAt >= _size) { - return startAt + 1; +template +inline size_t FixedOrderedMultimap::findFirstIndex(key_t key, + size_t startAt) const { + if (startAt >= _size) { + return startAt + 1; + } + size_t i = startAt; + for (i = startAt; i < _size; ++i) { + if (theMap[i].first == key) { + return i; } - size_t i = startAt; - for (i = startAt; i < _size; ++i) { - if (theMap[i].first == key) { - return i; - } - } - return i; + } + return i; } -template +template inline size_t FixedOrderedMultimap::findNicePlace(key_t key) const { - size_t i = 0; - for (i = 0; i < _size; ++i) { - if (myComp(key, theMap[i].first)) { - return i; - } + size_t i = 0; + for (i = 0; i < _size; ++i) { + if (myComp(key, theMap[i].first)) { + return i; } - return i; + } + return i; } -template +template inline void FixedOrderedMultimap::removeFromPosition(size_t position) { - if (_size <= position) { - return; - } - memmove(static_cast(&theMap[position]), static_cast(&theMap[position + 1]), - (_size - position - 1) * sizeof(std::pair)); - --_size; + if (_size <= position) { + return; + } + memmove(static_cast(&theMap[position]), static_cast(&theMap[position + 1]), + (_size - position - 1) * sizeof(std::pair)); + --_size; } - #endif /* FRAMEWORK_CONTAINER_FIXEDORDEREDMULTIMAP_TPP_ */ diff --git a/src/fsfw/container/HybridIterator.h b/src/fsfw/container/HybridIterator.h index e8b24a3d6..ad000ec25 100644 --- a/src/fsfw/container/HybridIterator.h +++ b/src/fsfw/container/HybridIterator.h @@ -10,16 +10,23 @@ class HybridIterator : public LinkedElement::Iterator, public ArrayList::Iterator *iter) - : LinkedElement::Iterator(*iter), value(iter->value), linked(true) {} + : LinkedElement::Iterator(*iter), linked(true) { + if (iter != nullptr) { + value = iter->value; + } + } - HybridIterator(LinkedElement *start) - : LinkedElement::Iterator(start), value(start->value), linked(true) {} + HybridIterator(LinkedElement *start) : LinkedElement::Iterator(start), linked(true) { + if (start != nullptr) { + value = start->value; + } + } HybridIterator(typename ArrayList::Iterator start, typename ArrayList::Iterator end) : ArrayList::Iterator(start), value(start.value), linked(false), end(end.value) { if (value == this->end) { - value = NULL; + value = nullptr; } } diff --git a/src/fsfw/container/SimpleRingBuffer.cpp b/src/fsfw/container/SimpleRingBuffer.cpp index bcf3cf20d..c104ea971 100644 --- a/src/fsfw/container/SimpleRingBuffer.cpp +++ b/src/fsfw/container/SimpleRingBuffer.cpp @@ -2,6 +2,9 @@ #include +#include "fsfw/FSFW.h" +#include "fsfw/serviceinterface.h" + SimpleRingBuffer::SimpleRingBuffer(const size_t size, bool overwriteOld, size_t maxExcessBytes) : RingBufferBase<>(0, size, overwriteOld), maxExcessBytes(maxExcessBytes) { if (maxExcessBytes > size) { @@ -48,6 +51,19 @@ void SimpleRingBuffer::confirmBytesWritten(size_t amount) { } ReturnValue_t SimpleRingBuffer::writeData(const uint8_t* data, size_t amount) { + if (data == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + if (amount > getMaxSize()) { +#if FSFW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "SimpleRingBuffer::writeData: Amount of data too large" << std::endl; +#else + sif::printError("SimpleRingBuffer::writeData: Amount of data too large\n"); +#endif +#endif + return HasReturnvaluesIF::RETURN_FAILED; + } if (availableWriteSpace() >= amount or overwriteOld) { size_t amountTillWrap = writeTillWrap(); if (amountTillWrap >= amount) { diff --git a/src/fsfw/controller/CMakeLists.txt b/src/fsfw/controller/CMakeLists.txt index 550acfcdd..c8c000d81 100644 --- a/src/fsfw/controller/CMakeLists.txt +++ b/src/fsfw/controller/CMakeLists.txt @@ -1,4 +1,2 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - ControllerBase.cpp - ExtendedControllerBase.cpp -) \ No newline at end of file +target_sources(${LIB_FSFW_NAME} PRIVATE ControllerBase.cpp + ExtendedControllerBase.cpp) diff --git a/src/fsfw/controller/ControllerBase.cpp b/src/fsfw/controller/ControllerBase.cpp index 953dacb46..7a8b6bc41 100644 --- a/src/fsfw/controller/ControllerBase.cpp +++ b/src/fsfw/controller/ControllerBase.cpp @@ -26,7 +26,7 @@ ReturnValue_t ControllerBase::initialize() { MessageQueueId_t parentQueue = 0; if (parentId != objects::NO_OBJECT) { - SubsystemBase* parent = ObjectManager::instance()->get(parentId); + auto* parent = ObjectManager::instance()->get(parentId); if (parent == nullptr) { return RETURN_FAILED; } @@ -52,7 +52,7 @@ MessageQueueId_t ControllerBase::getCommandQueue() const { return commandQueue-> void ControllerBase::handleQueue() { CommandMessage command; - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result; for (result = commandQueue->receiveMessage(&command); result == RETURN_OK; result = commandQueue->receiveMessage(&command)) { result = modeHelper.handleModeCommand(&command); @@ -73,20 +73,20 @@ void ControllerBase::handleQueue() { } } -void ControllerBase::startTransition(Mode_t mode, Submode_t submode) { +void ControllerBase::startTransition(Mode_t mode_, Submode_t submode_) { changeHK(this->mode, this->submode, false); triggerEvent(CHANGING_MODE, mode, submode); - this->mode = mode; - this->submode = submode; + mode = mode_; + submode = submode_; modeHelper.modeChanged(mode, submode); modeChanged(mode, submode); announceMode(false); changeHK(this->mode, this->submode, true); } -void ControllerBase::getMode(Mode_t* mode, Submode_t* submode) { - *mode = this->mode; - *submode = this->submode; +void ControllerBase::getMode(Mode_t* mode_, Submode_t* submode_) { + *mode_ = this->mode; + *submode_ = this->submode; } void ControllerBase::setToExternalControl() { healthHelper.setHealth(EXTERNAL_CONTROL); } @@ -99,7 +99,7 @@ ReturnValue_t ControllerBase::performOperation(uint8_t opCode) { return RETURN_OK; } -void ControllerBase::modeChanged(Mode_t mode, Submode_t submode) { return; } +void ControllerBase::modeChanged(Mode_t mode_, Submode_t submode_) {} ReturnValue_t ControllerBase::setHealth(HealthState health) { switch (health) { @@ -115,6 +115,6 @@ ReturnValue_t ControllerBase::setHealth(HealthState health) { HasHealthIF::HealthState ControllerBase::getHealth() { return healthHelper.getHealth(); } void ControllerBase::setTaskIF(PeriodicTaskIF* task_) { executingTask = task_; } -void ControllerBase::changeHK(Mode_t mode, Submode_t submode, bool enable) {} +void ControllerBase::changeHK(Mode_t mode_, Submode_t submode_, bool enable) {} ReturnValue_t ControllerBase::initializeAfterTaskCreation() { return HasReturnvaluesIF::RETURN_OK; } diff --git a/src/fsfw/controller/ControllerBase.h b/src/fsfw/controller/ControllerBase.h index db75982c8..550659b82 100644 --- a/src/fsfw/controller/ControllerBase.h +++ b/src/fsfw/controller/ControllerBase.h @@ -1,7 +1,6 @@ #ifndef FSFW_CONTROLLER_CONTROLLERBASE_H_ #define FSFW_CONTROLLER_CONTROLLERBASE_H_ -#include "fsfw/datapool/HkSwitchHelper.h" #include "fsfw/health/HasHealthIF.h" #include "fsfw/health/HealthHelper.h" #include "fsfw/modes/HasModesIF.h" @@ -25,21 +24,21 @@ class ControllerBase : public HasModesIF, static const Mode_t MODE_NORMAL = 2; ControllerBase(object_id_t setObjectId, object_id_t parentId, size_t commandQueueDepth = 3); - virtual ~ControllerBase(); + ~ControllerBase() override; /** SystemObject override */ - virtual ReturnValue_t initialize() override; + ReturnValue_t initialize() override; - virtual MessageQueueId_t getCommandQueue() const override; + [[nodiscard]] MessageQueueId_t getCommandQueue() const override; /** HasHealthIF overrides */ - virtual ReturnValue_t setHealth(HealthState health) override; - virtual HasHealthIF::HealthState getHealth() override; + ReturnValue_t setHealth(HealthState health) override; + HasHealthIF::HealthState getHealth() override; /** ExecutableObjectIF overrides */ - virtual ReturnValue_t performOperation(uint8_t opCode) override; - virtual void setTaskIF(PeriodicTaskIF *task) override; - virtual ReturnValue_t initializeAfterTaskCreation() override; + ReturnValue_t performOperation(uint8_t opCode) override; + void setTaskIF(PeriodicTaskIF *task) override; + ReturnValue_t initializeAfterTaskCreation() override; protected: /** @@ -55,8 +54,8 @@ class ControllerBase : public HasModesIF, */ virtual void performControlOperation() = 0; - virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, - uint32_t *msToReachTheMode) = 0; + ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode) override = 0; const object_id_t parentId; @@ -81,10 +80,10 @@ class ControllerBase : public HasModesIF, /** Mode helpers */ virtual void modeChanged(Mode_t mode, Submode_t submode); - virtual void startTransition(Mode_t mode, Submode_t submode); - virtual void getMode(Mode_t *mode, Submode_t *submode); - virtual void setToExternalControl(); - virtual void announceMode(bool recursive); + void startTransition(Mode_t mode, Submode_t submode) override; + void getMode(Mode_t *mode, Submode_t *submode) override; + void setToExternalControl() override; + void announceMode(bool recursive) override; /** HK helpers */ virtual void changeHK(Mode_t mode, Submode_t submode, bool enable); }; diff --git a/src/fsfw/controller/ExtendedControllerBase.cpp b/src/fsfw/controller/ExtendedControllerBase.cpp index 0dfd9dc7c..64b39a315 100644 --- a/src/fsfw/controller/ExtendedControllerBase.cpp +++ b/src/fsfw/controller/ExtendedControllerBase.cpp @@ -6,7 +6,7 @@ ExtendedControllerBase::ExtendedControllerBase(object_id_t objectId, object_id_t poolManager(this, commandQueue), actionHelper(this, commandQueue) {} -ExtendedControllerBase::~ExtendedControllerBase() {} +ExtendedControllerBase::~ExtendedControllerBase() = default; ReturnValue_t ExtendedControllerBase::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, @@ -31,7 +31,7 @@ ReturnValue_t ExtendedControllerBase::handleCommandMessage(CommandMessage *messa void ExtendedControllerBase::handleQueue() { CommandMessage command; - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result; for (result = commandQueue->receiveMessage(&command); result == RETURN_OK; result = commandQueue->receiveMessage(&command)) { result = actionHelper.handleActionMessage(&command); diff --git a/src/fsfw/controller/ExtendedControllerBase.h b/src/fsfw/controller/ExtendedControllerBase.h index 0c64f5b9a..b5583a880 100644 --- a/src/fsfw/controller/ExtendedControllerBase.h +++ b/src/fsfw/controller/ExtendedControllerBase.h @@ -18,16 +18,16 @@ class ExtendedControllerBase : public ControllerBase, public HasLocalDataPoolIF { public: ExtendedControllerBase(object_id_t objectId, object_id_t parentId, size_t commandQueueDepth = 3); - virtual ~ExtendedControllerBase(); + ~ExtendedControllerBase() override; /* SystemObjectIF overrides */ - virtual ReturnValue_t initialize() override; + ReturnValue_t initialize() override; - virtual MessageQueueId_t getCommandQueue() const override; + [[nodiscard]] MessageQueueId_t getCommandQueue() const override; /* ExecutableObjectIF overrides */ - virtual ReturnValue_t performOperation(uint8_t opCode) override; - virtual ReturnValue_t initializeAfterTaskCreation() override; + ReturnValue_t performOperation(uint8_t opCode) override; + ReturnValue_t initializeAfterTaskCreation() override; protected: LocalDataPoolManager poolManager; @@ -39,32 +39,32 @@ class ExtendedControllerBase : public ControllerBase, * @param message * @return */ - virtual ReturnValue_t handleCommandMessage(CommandMessage* message) = 0; + ReturnValue_t handleCommandMessage(CommandMessage* message) override = 0; /** * Periodic helper from ControllerBase, implemented by child class. */ - virtual void performControlOperation() = 0; + void performControlOperation() override = 0; /* Handle the four messages mentioned above */ void handleQueue() override; /* HasActionsIF overrides */ - virtual ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, - const uint8_t* data, size_t size) override; + ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, + const uint8_t* data, size_t size) override; /* HasLocalDatapoolIF overrides */ - virtual LocalDataPoolManager* getHkManagerHandle() override; - virtual object_id_t getObjectId() const override; - virtual uint32_t getPeriodicOperationFrequency() const override; + LocalDataPoolManager* getHkManagerHandle() override; + [[nodiscard]] object_id_t getObjectId() const override; + [[nodiscard]] uint32_t getPeriodicOperationFrequency() const override; - virtual ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, - LocalDataPoolManager& poolManager) override = 0; - virtual LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override = 0; + ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, + LocalDataPoolManager& poolManager) override = 0; + LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override = 0; // Mode abstract functions - virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, - uint32_t* msToReachTheMode) override = 0; + ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t* msToReachTheMode) override = 0; }; #endif /* FSFW_CONTROLLER_EXTENDEDCONTROLLERBASE_H_ */ diff --git a/src/fsfw/coordinates/CMakeLists.txt b/src/fsfw/coordinates/CMakeLists.txt index a1fa1e52b..15452b1ca 100644 --- a/src/fsfw/coordinates/CMakeLists.txt +++ b/src/fsfw/coordinates/CMakeLists.txt @@ -1,5 +1,2 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - CoordinateTransformations.cpp - Sgp4Propagator.cpp -) \ No newline at end of file +target_sources(${LIB_FSFW_NAME} PRIVATE CoordinateTransformations.cpp + Sgp4Propagator.cpp) diff --git a/src/fsfw/datalinklayer/CMakeLists.txt b/src/fsfw/datalinklayer/CMakeLists.txt index 148e7c5de..cc18088f8 100644 --- a/src/fsfw/datalinklayer/CMakeLists.txt +++ b/src/fsfw/datalinklayer/CMakeLists.txt @@ -1,12 +1,11 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - Clcw.cpp - DataLinkLayer.cpp - Farm1StateLockout.cpp - Farm1StateOpen.cpp - Farm1StateWait.cpp - MapPacketExtraction.cpp - TcTransferFrame.cpp - TcTransferFrameLocal.cpp - VirtualChannelReception.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} + PRIVATE Clcw.cpp + DataLinkLayer.cpp + Farm1StateLockout.cpp + Farm1StateOpen.cpp + Farm1StateWait.cpp + MapPacketExtraction.cpp + TcTransferFrame.cpp + TcTransferFrameLocal.cpp + VirtualChannelReception.cpp) diff --git a/src/fsfw/datapool/CMakeLists.txt b/src/fsfw/datapool/CMakeLists.txt index 0d53e1ba1..b2ac592c4 100644 --- a/src/fsfw/datapool/CMakeLists.txt +++ b/src/fsfw/datapool/CMakeLists.txt @@ -1,6 +1 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - HkSwitchHelper.cpp - PoolDataSetBase.cpp - PoolEntry.cpp -) \ No newline at end of file +target_sources(${LIB_FSFW_NAME} PRIVATE PoolDataSetBase.cpp PoolEntry.cpp) diff --git a/src/fsfw/datapool/HkSwitchHelper.cpp b/src/fsfw/datapool/HkSwitchHelper.cpp deleted file mode 100644 index 7f6ffd17f..000000000 --- a/src/fsfw/datapool/HkSwitchHelper.cpp +++ /dev/null @@ -1,67 +0,0 @@ -#include "fsfw/datapool/HkSwitchHelper.h" - -#include "fsfw/ipc/QueueFactory.h" - -HkSwitchHelper::HkSwitchHelper(EventReportingProxyIF* eventProxy) - : commandActionHelper(this), eventProxy(eventProxy) { - actionQueue = QueueFactory::instance()->createMessageQueue(); -} - -HkSwitchHelper::~HkSwitchHelper() { QueueFactory::instance()->deleteMessageQueue(actionQueue); } - -ReturnValue_t HkSwitchHelper::initialize() { - ReturnValue_t result = commandActionHelper.initialize(); - - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - return result; -} - -ReturnValue_t HkSwitchHelper::performOperation(uint8_t operationCode) { - CommandMessage command; - while (actionQueue->receiveMessage(&command) == HasReturnvaluesIF::RETURN_OK) { - ReturnValue_t result = commandActionHelper.handleReply(&command); - if (result == HasReturnvaluesIF::RETURN_OK) { - continue; - } - command.setToUnknownCommand(); - actionQueue->reply(&command); - } - - return HasReturnvaluesIF::RETURN_OK; -} - -void HkSwitchHelper::stepSuccessfulReceived(ActionId_t actionId, uint8_t step) {} - -void HkSwitchHelper::stepFailedReceived(ActionId_t actionId, uint8_t step, - ReturnValue_t returnCode) { - eventProxy->forwardEvent(SWITCHING_TM_FAILED, returnCode, actionId); -} - -void HkSwitchHelper::dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) {} - -void HkSwitchHelper::completionSuccessfulReceived(ActionId_t actionId) {} - -void HkSwitchHelper::completionFailedReceived(ActionId_t actionId, ReturnValue_t returnCode) { - eventProxy->forwardEvent(SWITCHING_TM_FAILED, returnCode, actionId); -} - -ReturnValue_t HkSwitchHelper::switchHK(SerializeIF* sids, bool enable) { - // ActionId_t action = HKService::DISABLE_HK; - // if (enable) { - // action = HKService::ENABLE_HK; - // } - // - // ReturnValue_t result = commandActionHelper.commandAction( - // objects::PUS_HK_SERVICE, action, sids); - // - // if (result != HasReturnvaluesIF::RETURN_OK) { - // eventProxy->forwardEvent(SWITCHING_TM_FAILED, result); - // } - // return result; - return HasReturnvaluesIF::RETURN_OK; -} - -MessageQueueIF* HkSwitchHelper::getCommandQueuePtr() { return actionQueue; } diff --git a/src/fsfw/datapool/HkSwitchHelper.h b/src/fsfw/datapool/HkSwitchHelper.h deleted file mode 100644 index a0becd81b..000000000 --- a/src/fsfw/datapool/HkSwitchHelper.h +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef FRAMEWORK_DATAPOOL_HKSWITCHHELPER_H_ -#define FRAMEWORK_DATAPOOL_HKSWITCHHELPER_H_ - -#include "fsfw/action/CommandsActionsIF.h" -#include "fsfw/events/EventReportingProxyIF.h" -#include "fsfw/tasks/ExecutableObjectIF.h" - -// TODO this class violations separation between mission and framework -// but it is only a transitional solution until the Datapool is -// implemented decentrally - -class HkSwitchHelper : public ExecutableObjectIF, public CommandsActionsIF { - public: - static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::HK; - static const Event SWITCHING_TM_FAILED = - MAKE_EVENT(1, severity::LOW); //!< Commanding the HK Service failed, p1: error code, p2 - //!< action: 0 disable / 1 enable - - HkSwitchHelper(EventReportingProxyIF* eventProxy); - virtual ~HkSwitchHelper(); - - ReturnValue_t initialize(); - - virtual ReturnValue_t performOperation(uint8_t operationCode = 0); - - ReturnValue_t switchHK(SerializeIF* sids, bool enable); - - virtual void setTaskIF(PeriodicTaskIF* task_){}; - - protected: - virtual void stepSuccessfulReceived(ActionId_t actionId, uint8_t step); - virtual void stepFailedReceived(ActionId_t actionId, uint8_t step, ReturnValue_t returnCode); - virtual void dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size); - virtual void completionSuccessfulReceived(ActionId_t actionId); - virtual void completionFailedReceived(ActionId_t actionId, ReturnValue_t returnCode); - virtual MessageQueueIF* getCommandQueuePtr(); - - private: - CommandActionHelper commandActionHelper; - MessageQueueIF* actionQueue; - EventReportingProxyIF* eventProxy; -}; - -#endif /* FRAMEWORK_DATAPOOL_HKSWITCHHELPER_H_ */ diff --git a/src/fsfw/datapool/PoolDataSetBase.h b/src/fsfw/datapool/PoolDataSetBase.h index 1b4bacdad..dc6ec135a 100644 --- a/src/fsfw/datapool/PoolDataSetBase.h +++ b/src/fsfw/datapool/PoolDataSetBase.h @@ -109,7 +109,7 @@ class PoolDataSetBase : public PoolDataSetIF, public SerializeIF, public HasRetu */ virtual ReturnValue_t unlockDataPool() override; - virtual uint16_t getFillCount() const; + virtual uint16_t getFillCount() const override; /* SerializeIF implementations */ virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, const size_t maxSize, diff --git a/src/fsfw/datapool/PoolEntry.cpp b/src/fsfw/datapool/PoolEntry.cpp index fd110e6c7..9138a7059 100644 --- a/src/fsfw/datapool/PoolEntry.cpp +++ b/src/fsfw/datapool/PoolEntry.cpp @@ -7,24 +7,26 @@ #include "fsfw/serviceinterface/ServiceInterface.h" template -PoolEntry::PoolEntry(std::initializer_list initValue, bool setValid) - : length(static_cast(initValue.size())), valid(setValid) { - this->address = new T[this->length]; - if (initValue.size() == 0) { - std::memset(this->address, 0, this->getByteSize()); - } else { - std::copy(initValue.begin(), initValue.end(), this->address); +PoolEntry::PoolEntry(uint8_t len, bool setValid) : length(len), valid(setValid) { + this->address = new T[this->length](); + std::memset(this->address, 0, this->getByteSize()); +} + +template +PoolEntry::PoolEntry(std::initializer_list initValues, bool setValid) + : length(static_cast(initValues.size())), valid(setValid) { + this->address = new T[this->length](); + if (initValues.size() > 0) { + std::copy(initValues.begin(), initValues.end(), this->address); } } template -PoolEntry::PoolEntry(T* initValue, uint8_t setLength, bool setValid) +PoolEntry::PoolEntry(const T* initValue, uint8_t setLength, bool setValid) : length(setLength), valid(setValid) { - this->address = new T[this->length]; + this->address = new T[this->length](); if (initValue != nullptr) { std::memcpy(this->address, initValue, this->getByteSize()); - } else { - std::memset(this->address, 0, this->getByteSize()); } } diff --git a/src/fsfw/datapool/PoolEntry.h b/src/fsfw/datapool/PoolEntry.h index d3d80f093..4010f78d2 100644 --- a/src/fsfw/datapool/PoolEntry.h +++ b/src/fsfw/datapool/PoolEntry.h @@ -33,6 +33,9 @@ class PoolEntry : public PoolEntryIF { "instead! The ECSS standard defines a boolean as a one bit " "field. Therefore it is preferred to store a boolean as an " "uint8_t"); + + PoolEntry(uint8_t len = 1, bool setValid = false); + /** * @brief In the classe's constructor, space is allocated on the heap and * potential initialization values are copied to that space. @@ -49,7 +52,7 @@ class PoolEntry : public PoolEntryIF { * @param setValid * Sets the initialization flag. It is invalid by default. */ - PoolEntry(std::initializer_list initValue = {0}, bool setValid = false); + PoolEntry(std::initializer_list initValue, bool setValid = false); /** * @brief In the classe's constructor, space is allocated on the heap and @@ -62,7 +65,7 @@ class PoolEntry : public PoolEntryIF { * @param setValid * Sets the initialization flag. It is invalid by default. */ - PoolEntry(T* initValue, uint8_t setLength = 1, bool setValid = false); + PoolEntry(const T* initValue, uint8_t setLength = 1, bool setValid = false); //! Explicitely deleted copy ctor, copying is not allowed. PoolEntry(const PoolEntry&) = delete; diff --git a/src/fsfw/datapoollocal/CMakeLists.txt b/src/fsfw/datapoollocal/CMakeLists.txt index e2db39eb0..749ef688b 100644 --- a/src/fsfw/datapoollocal/CMakeLists.txt +++ b/src/fsfw/datapoollocal/CMakeLists.txt @@ -1,10 +1,6 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - LocalDataPoolManager.cpp - LocalDataSet.cpp - LocalPoolDataSetBase.cpp - LocalPoolObjectBase.cpp - SharedLocalDataSet.cpp -) +target_sources( + ${LIB_FSFW_NAME} + PRIVATE LocalDataPoolManager.cpp LocalDataSet.cpp LocalPoolDataSetBase.cpp + LocalPoolObjectBase.cpp SharedLocalDataSet.cpp) -add_subdirectory(internal) \ No newline at end of file +add_subdirectory(internal) diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp index 9057de31b..9b7f800f1 100644 --- a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp +++ b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp @@ -84,8 +84,8 @@ ReturnValue_t LocalDataPoolManager::initializeHousekeepingPoolEntriesOnce() { return result; } - printWarningOrError(sif::OutputTypes::OUT_WARNING, "initialize", HasReturnvaluesIF::RETURN_FAILED, - "The map should only be initialized once"); + printWarningOrError(sif::OutputTypes::OUT_WARNING, "initializeHousekeepingPoolEntriesOnce", + HasReturnvaluesIF::RETURN_FAILED, "The map should only be initialized once"); return HasReturnvaluesIF::RETURN_OK; } @@ -696,9 +696,10 @@ void LocalDataPoolManager::performPeriodicHkGeneration(HkReceiver& receiver) { if (result != HasReturnvaluesIF::RETURN_OK) { /* Configuration error */ #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "LocalDataPoolManager::performHkOperation: HK generation failed." << std::endl; + sif::warning << "LocalDataPoolManager::performPeriodicHkOperation: HK generation failed." + << std::endl; #else - sif::printWarning("LocalDataPoolManager::performHkOperation: HK generation failed.\n"); + sif::printWarning("LocalDataPoolManager::performPeriodicHkOperation: HK generation failed.\n"); #endif } } @@ -787,6 +788,10 @@ ReturnValue_t LocalDataPoolManager::generateSetStructurePacket(sid_t sid, bool i // Serialize set packet into store. size_t size = 0; result = setPacket.serialize(&storePtr, &size, expectedSize, SerializeIF::Endianness::BIG); + if (result != HasReturnvaluesIF::RETURN_OK) { + ipcStore->deleteData(storeId); + return result; + } if (expectedSize != size) { printWarningOrError(sif::OutputTypes::OUT_WARNING, "generateSetStructurePacket", HasReturnvaluesIF::RETURN_FAILED, @@ -801,7 +806,10 @@ ReturnValue_t LocalDataPoolManager::generateSetStructurePacket(sid_t sid, bool i HousekeepingMessage::setHkStuctureReportReply(&reply, sid, storeId); } - hkQueue->reply(&reply); + result = hkQueue->reply(&reply); + if (result != HasReturnvaluesIF::RETURN_OK) { + ipcStore->deleteData(storeId); + } return result; } diff --git a/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp b/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp index 4a0762126..62fdb184f 100644 --- a/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp +++ b/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp @@ -94,13 +94,14 @@ ReturnValue_t LocalPoolDataSetBase::serializeWithValidityBuffer( ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; const uint8_t validityMaskSize = std::ceil(static_cast(fillCount) / 8.0); uint8_t *validityPtr = nullptr; -#ifdef _MSC_VER - /* Use a std::vector here because MSVC will (rightly) not create a fixed size array - with a non constant size specifier */ - std::vector validityMask(validityMaskSize); +#if defined(_MSC_VER) || defined(__clang__) + // Use a std::vector here because MSVC will (rightly) not create a fixed size array + // with a non constant size specifier. The Apple compiler (LLVM) will not accept + // the initialization of a variable sized array + std::vector validityMask(validityMaskSize, 0); validityPtr = validityMask.data(); #else - uint8_t validityMask[validityMaskSize] = {0}; + uint8_t validityMask[validityMaskSize] = {}; validityPtr = validityMask; #endif uint8_t validBufferIndex = 0; diff --git a/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp b/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp index c974601cc..82aefc18c 100644 --- a/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp +++ b/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp @@ -47,13 +47,14 @@ LocalPoolObjectBase::LocalPoolObjectBase(object_id_t poolOwner, lp_id_t poolId, HasLocalDataPoolIF* hkOwner = ObjectManager::instance()->get(poolOwner); if (hkOwner == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "LocalPoolVariable: The supplied pool owner did not implement the correct " - "interface HasLocalDataPoolIF!" - << std::endl; + sif::error << "LocalPoolVariable: The supplied pool owner 0x" << std::hex << poolOwner + << std::dec << " did not implement the correct interface " + << "HasLocalDataPoolIF" << std::endl; #else sif::printError( - "LocalPoolVariable: The supplied pool owner did not implement the correct " - "interface HasLocalDataPoolIF!\n"); + "LocalPoolVariable: The supplied pool owner 0x%08x did not implement the correct " + "interface HasLocalDataPoolIF\n", + poolOwner); #endif return; } diff --git a/src/fsfw/datapoollocal/LocalPoolObjectBase.h b/src/fsfw/datapoollocal/LocalPoolObjectBase.h index 56e190df1..b2ffa4c18 100644 --- a/src/fsfw/datapoollocal/LocalPoolObjectBase.h +++ b/src/fsfw/datapoollocal/LocalPoolObjectBase.h @@ -23,8 +23,8 @@ class LocalPoolObjectBase : public PoolVariableIF, public HasReturnvaluesIF, pub LocalPoolObjectBase(object_id_t poolOwner, lp_id_t poolId, DataSetIF* dataSet = nullptr, pool_rwm_t setReadWriteMode = pool_rwm_t::VAR_READ_WRITE); - void setReadWriteMode(pool_rwm_t newReadWriteMode); - pool_rwm_t getReadWriteMode() const; + void setReadWriteMode(pool_rwm_t newReadWriteMode) override; + pool_rwm_t getReadWriteMode() const override; bool isValid() const override; void setValid(bool valid) override; diff --git a/src/fsfw/datapoollocal/LocalPoolVariable.tpp b/src/fsfw/datapoollocal/LocalPoolVariable.tpp index 9bb30611b..f800dfd3e 100644 --- a/src/fsfw/datapoollocal/LocalPoolVariable.tpp +++ b/src/fsfw/datapoollocal/LocalPoolVariable.tpp @@ -5,205 +5,189 @@ #error Include LocalPoolVariable.h before LocalPoolVariable.tpp! #endif -template -inline LocalPoolVariable::LocalPoolVariable(HasLocalDataPoolIF* hkOwner, - lp_id_t poolId, DataSetIF* dataSet, pool_rwm_t setReadWriteMode): - LocalPoolObjectBase(poolId, hkOwner, dataSet, setReadWriteMode) {} +template +inline LocalPoolVariable::LocalPoolVariable(HasLocalDataPoolIF* hkOwner, lp_id_t poolId, + DataSetIF* dataSet, pool_rwm_t setReadWriteMode) + : LocalPoolObjectBase(poolId, hkOwner, dataSet, setReadWriteMode) {} -template -inline LocalPoolVariable::LocalPoolVariable(object_id_t poolOwner, - lp_id_t poolId, DataSetIF *dataSet, pool_rwm_t setReadWriteMode): - LocalPoolObjectBase(poolOwner, poolId, dataSet, setReadWriteMode) {} +template +inline LocalPoolVariable::LocalPoolVariable(object_id_t poolOwner, lp_id_t poolId, + DataSetIF* dataSet, pool_rwm_t setReadWriteMode) + : LocalPoolObjectBase(poolOwner, poolId, dataSet, setReadWriteMode) {} +template +inline LocalPoolVariable::LocalPoolVariable(gp_id_t globalPoolId, DataSetIF* dataSet, + pool_rwm_t setReadWriteMode) + : LocalPoolObjectBase(globalPoolId.objectId, globalPoolId.localPoolId, dataSet, + setReadWriteMode) {} -template -inline LocalPoolVariable::LocalPoolVariable(gp_id_t globalPoolId, - DataSetIF *dataSet, pool_rwm_t setReadWriteMode): - LocalPoolObjectBase(globalPoolId.objectId, globalPoolId.localPoolId, - dataSet, setReadWriteMode){} - - -template -inline ReturnValue_t LocalPoolVariable::read( - MutexIF::TimeoutType timeoutType, uint32_t timeoutMs) { - if(hkManager == nullptr) { - return readWithoutLock(); - } - MutexIF* mutex = LocalDpManagerAttorney::getMutexHandle(*hkManager); - ReturnValue_t result = mutex->lockMutex(timeoutType, timeoutMs); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - result = readWithoutLock(); - mutex->unlockMutex(); +template +inline ReturnValue_t LocalPoolVariable::read(MutexIF::TimeoutType timeoutType, + uint32_t timeoutMs) { + if (hkManager == nullptr) { + return readWithoutLock(); + } + MutexIF* mutex = LocalDpManagerAttorney::getMutexHandle(*hkManager); + ReturnValue_t result = mutex->lockMutex(timeoutType, timeoutMs); + if (result != HasReturnvaluesIF::RETURN_OK) { return result; + } + result = readWithoutLock(); + mutex->unlockMutex(); + return result; } -template +template inline ReturnValue_t LocalPoolVariable::readWithoutLock() { - if(readWriteMode == pool_rwm_t::VAR_WRITE) { - object_id_t targetObjectId = hkManager->getCreatorObjectId(); - reportReadCommitError("LocalPoolVector", - PoolVariableIF::INVALID_READ_WRITE_MODE, true, targetObjectId, - localPoolId); - return PoolVariableIF::INVALID_READ_WRITE_MODE; - } + if (readWriteMode == pool_rwm_t::VAR_WRITE) { + object_id_t targetObjectId = hkManager->getCreatorObjectId(); + reportReadCommitError("LocalPoolVector", PoolVariableIF::INVALID_READ_WRITE_MODE, true, + targetObjectId, localPoolId); + return PoolVariableIF::INVALID_READ_WRITE_MODE; + } - PoolEntry* poolEntry = nullptr; - ReturnValue_t result = LocalDpManagerAttorney::fetchPoolEntry(*hkManager, localPoolId, - &poolEntry); - if(result != RETURN_OK) { - object_id_t ownerObjectId = hkManager->getCreatorObjectId(); - reportReadCommitError("LocalPoolVariable", result, - false, ownerObjectId, localPoolId); - return result; - } - - this->value = *(poolEntry->getDataPtr()); - this->valid = poolEntry->getValid(); - return RETURN_OK; -} - -template -inline ReturnValue_t LocalPoolVariable::commit(bool setValid, - MutexIF::TimeoutType timeoutType, uint32_t timeoutMs) { - this->setValid(setValid); - return commit(timeoutType, timeoutMs); -} - -template -inline ReturnValue_t LocalPoolVariable::commit( - MutexIF::TimeoutType timeoutType, uint32_t timeoutMs) { - if(hkManager == nullptr) { - return commitWithoutLock(); - } - MutexIF* mutex = LocalDpManagerAttorney::getMutexHandle(*hkManager); - ReturnValue_t result = mutex->lockMutex(timeoutType, timeoutMs); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - result = commitWithoutLock(); - mutex->unlockMutex(); + PoolEntry* poolEntry = nullptr; + ReturnValue_t result = + LocalDpManagerAttorney::fetchPoolEntry(*hkManager, localPoolId, &poolEntry); + if (result != RETURN_OK) { + object_id_t ownerObjectId = hkManager->getCreatorObjectId(); + reportReadCommitError("LocalPoolVariable", result, false, ownerObjectId, localPoolId); return result; + } + + this->value = *(poolEntry->getDataPtr()); + this->valid = poolEntry->getValid(); + return RETURN_OK; } -template +template +inline ReturnValue_t LocalPoolVariable::commit(bool setValid, MutexIF::TimeoutType timeoutType, + uint32_t timeoutMs) { + this->setValid(setValid); + return commit(timeoutType, timeoutMs); +} + +template +inline ReturnValue_t LocalPoolVariable::commit(MutexIF::TimeoutType timeoutType, + uint32_t timeoutMs) { + if (hkManager == nullptr) { + return commitWithoutLock(); + } + MutexIF* mutex = LocalDpManagerAttorney::getMutexHandle(*hkManager); + ReturnValue_t result = mutex->lockMutex(timeoutType, timeoutMs); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = commitWithoutLock(); + mutex->unlockMutex(); + return result; +} + +template inline ReturnValue_t LocalPoolVariable::commitWithoutLock() { - if(readWriteMode == pool_rwm_t::VAR_READ) { - object_id_t targetObjectId = hkManager->getCreatorObjectId(); - reportReadCommitError("LocalPoolVector", - PoolVariableIF::INVALID_READ_WRITE_MODE, false, targetObjectId, - localPoolId); - return PoolVariableIF::INVALID_READ_WRITE_MODE; - } + if (readWriteMode == pool_rwm_t::VAR_READ) { + object_id_t targetObjectId = hkManager->getCreatorObjectId(); + reportReadCommitError("LocalPoolVector", PoolVariableIF::INVALID_READ_WRITE_MODE, false, + targetObjectId, localPoolId); + return PoolVariableIF::INVALID_READ_WRITE_MODE; + } - PoolEntry* poolEntry = nullptr; - ReturnValue_t result = LocalDpManagerAttorney::fetchPoolEntry(*hkManager, localPoolId, - &poolEntry); - if(result != RETURN_OK) { - object_id_t ownerObjectId = hkManager->getCreatorObjectId(); - reportReadCommitError("LocalPoolVariable", result, - false, ownerObjectId, localPoolId); - return result; - } + PoolEntry* poolEntry = nullptr; + ReturnValue_t result = + LocalDpManagerAttorney::fetchPoolEntry(*hkManager, localPoolId, &poolEntry); + if (result != RETURN_OK) { + object_id_t ownerObjectId = hkManager->getCreatorObjectId(); + reportReadCommitError("LocalPoolVariable", result, false, ownerObjectId, localPoolId); + return result; + } - *(poolEntry->getDataPtr()) = this->value; - poolEntry->setValid(this->valid); - return RETURN_OK; + *(poolEntry->getDataPtr()) = this->value; + poolEntry->setValid(this->valid); + return RETURN_OK; } -template -inline ReturnValue_t LocalPoolVariable::serialize(uint8_t** buffer, - size_t* size, const size_t max_size, - SerializeIF::Endianness streamEndianness) const { - return SerializeAdapter::serialize(&value, - buffer, size ,max_size, streamEndianness); +template +inline ReturnValue_t LocalPoolVariable::serialize( + uint8_t** buffer, size_t* size, const size_t max_size, + SerializeIF::Endianness streamEndianness) const { + return SerializeAdapter::serialize(&value, buffer, size, max_size, streamEndianness); } -template +template inline size_t LocalPoolVariable::getSerializedSize() const { - return SerializeAdapter::getSerializedSize(&value); + return SerializeAdapter::getSerializedSize(&value); } -template -inline ReturnValue_t LocalPoolVariable::deSerialize(const uint8_t** buffer, - size_t* size, SerializeIF::Endianness streamEndianness) { - return SerializeAdapter::deSerialize(&value, buffer, size, streamEndianness); +template +inline ReturnValue_t LocalPoolVariable::deSerialize(const uint8_t** buffer, size_t* size, + SerializeIF::Endianness streamEndianness) { + return SerializeAdapter::deSerialize(&value, buffer, size, streamEndianness); } #if FSFW_CPP_OSTREAM_ENABLED == 1 -template -inline std::ostream& operator<< (std::ostream &out, - const LocalPoolVariable &var) { - out << var.value; - return out; +template +inline std::ostream& operator<<(std::ostream& out, const LocalPoolVariable& var) { + out << var.value; + return out; } #endif -template +template inline LocalPoolVariable::operator T() const { - return value; + return value; } -template -inline LocalPoolVariable & LocalPoolVariable::operator=( - const T& newValue) { - value = newValue; - return *this; +template +inline LocalPoolVariable& LocalPoolVariable::operator=(const T& newValue) { + value = newValue; + return *this; } -template -inline LocalPoolVariable& LocalPoolVariable::operator =( - const LocalPoolVariable& newPoolVariable) { - value = newPoolVariable.value; - return *this; +template +inline LocalPoolVariable& LocalPoolVariable::operator=( + const LocalPoolVariable& newPoolVariable) { + value = newPoolVariable.value; + return *this; } -template -inline bool LocalPoolVariable::operator ==( - const LocalPoolVariable &other) const { - return this->value == other.value; +template +inline bool LocalPoolVariable::operator==(const LocalPoolVariable& other) const { + return this->value == other.value; } -template -inline bool LocalPoolVariable::operator ==(const T &other) const { - return this->value == other; +template +inline bool LocalPoolVariable::operator==(const T& other) const { + return this->value == other; } - -template -inline bool LocalPoolVariable::operator !=( - const LocalPoolVariable &other) const { - return not (*this == other); +template +inline bool LocalPoolVariable::operator!=(const LocalPoolVariable& other) const { + return not(*this == other); } -template -inline bool LocalPoolVariable::operator !=(const T &other) const { - return not (*this == other); +template +inline bool LocalPoolVariable::operator!=(const T& other) const { + return not(*this == other); } - -template -inline bool LocalPoolVariable::operator <( - const LocalPoolVariable &other) const { - return this->value < other.value; +template +inline bool LocalPoolVariable::operator<(const LocalPoolVariable& other) const { + return this->value < other.value; } -template -inline bool LocalPoolVariable::operator <(const T &other) const { - return this->value < other; +template +inline bool LocalPoolVariable::operator<(const T& other) const { + return this->value < other; } - -template -inline bool LocalPoolVariable::operator >( - const LocalPoolVariable &other) const { - return not (*this < other); +template +inline bool LocalPoolVariable::operator>(const LocalPoolVariable& other) const { + return not(*this < other); } -template -inline bool LocalPoolVariable::operator >(const T &other) const { - return not (*this < other); +template +inline bool LocalPoolVariable::operator>(const T& other) const { + return not(*this < other); } #endif /* FSFW_DATAPOOLLOCAL_LOCALPOOLVARIABLE_TPP_ */ diff --git a/src/fsfw/datapoollocal/LocalPoolVector.tpp b/src/fsfw/datapoollocal/LocalPoolVector.tpp index 044b8fa72..a2c2b752c 100644 --- a/src/fsfw/datapoollocal/LocalPoolVector.tpp +++ b/src/fsfw/datapoollocal/LocalPoolVector.tpp @@ -5,174 +5,172 @@ #error Include LocalPoolVector.h before LocalPoolVector.tpp! #endif -template -inline LocalPoolVector::LocalPoolVector( - HasLocalDataPoolIF* hkOwner, lp_id_t poolId, DataSetIF* dataSet, - pool_rwm_t setReadWriteMode): - LocalPoolObjectBase(poolId, hkOwner, dataSet, setReadWriteMode) {} +template +inline LocalPoolVector::LocalPoolVector(HasLocalDataPoolIF* hkOwner, lp_id_t poolId, + DataSetIF* dataSet, + pool_rwm_t setReadWriteMode) + : LocalPoolObjectBase(poolId, hkOwner, dataSet, setReadWriteMode) {} -template -inline LocalPoolVector::LocalPoolVector(object_id_t poolOwner, - lp_id_t poolId, DataSetIF *dataSet, pool_rwm_t setReadWriteMode): - LocalPoolObjectBase(poolOwner, poolId, dataSet, setReadWriteMode) {} +template +inline LocalPoolVector::LocalPoolVector(object_id_t poolOwner, lp_id_t poolId, + DataSetIF* dataSet, + pool_rwm_t setReadWriteMode) + : LocalPoolObjectBase(poolOwner, poolId, dataSet, setReadWriteMode) {} -template -inline LocalPoolVector::LocalPoolVector(gp_id_t globalPoolId, - DataSetIF *dataSet, pool_rwm_t setReadWriteMode): - LocalPoolObjectBase(globalPoolId.objectId, globalPoolId.localPoolId, - dataSet, setReadWriteMode) {} +template +inline LocalPoolVector::LocalPoolVector(gp_id_t globalPoolId, DataSetIF* dataSet, + pool_rwm_t setReadWriteMode) + : LocalPoolObjectBase(globalPoolId.objectId, globalPoolId.localPoolId, dataSet, + setReadWriteMode) {} -template -inline ReturnValue_t LocalPoolVector::read( - MutexIF::TimeoutType timeoutType, uint32_t timeoutMs) { - MutexGuard(LocalDpManagerAttorney::getMutexHandle(*hkManager), timeoutType, timeoutMs); - return readWithoutLock(); +template +inline ReturnValue_t LocalPoolVector::read(MutexIF::TimeoutType timeoutType, + uint32_t timeoutMs) { + MutexGuard(LocalDpManagerAttorney::getMutexHandle(*hkManager), timeoutType, timeoutMs); + return readWithoutLock(); } -template +template inline ReturnValue_t LocalPoolVector::readWithoutLock() { - if(readWriteMode == pool_rwm_t::VAR_WRITE) { - object_id_t targetObjectId = hkManager->getCreatorObjectId(); - reportReadCommitError("LocalPoolVector", - PoolVariableIF::INVALID_READ_WRITE_MODE, true, targetObjectId, - localPoolId); - return PoolVariableIF::INVALID_READ_WRITE_MODE; - } + if (readWriteMode == pool_rwm_t::VAR_WRITE) { + object_id_t targetObjectId = hkManager->getCreatorObjectId(); + reportReadCommitError("LocalPoolVector", PoolVariableIF::INVALID_READ_WRITE_MODE, true, + targetObjectId, localPoolId); + return PoolVariableIF::INVALID_READ_WRITE_MODE; + } - PoolEntry* poolEntry = nullptr; - ReturnValue_t result = LocalDpManagerAttorney::fetchPoolEntry(*hkManager, localPoolId, - &poolEntry); - memset(this->value, 0, vectorSize * sizeof(T)); + PoolEntry* poolEntry = nullptr; + ReturnValue_t result = + LocalDpManagerAttorney::fetchPoolEntry(*hkManager, localPoolId, &poolEntry); + memset(this->value, 0, vectorSize * sizeof(T)); - if(result != RETURN_OK) { - object_id_t targetObjectId = hkManager->getCreatorObjectId(); - reportReadCommitError("LocalPoolVector", result, true, targetObjectId, - localPoolId); - return result; - } - std::memcpy(this->value, poolEntry->getDataPtr(), poolEntry->getByteSize()); - this->valid = poolEntry->getValid(); - return RETURN_OK; + if (result != RETURN_OK) { + object_id_t targetObjectId = hkManager->getCreatorObjectId(); + reportReadCommitError("LocalPoolVector", result, true, targetObjectId, localPoolId); + return result; + } + std::memcpy(this->value, poolEntry->getDataPtr(), poolEntry->getByteSize()); + this->valid = poolEntry->getValid(); + return RETURN_OK; } -template +template inline ReturnValue_t LocalPoolVector::commit(bool valid, - MutexIF::TimeoutType timeoutType, uint32_t timeoutMs) { - this->setValid(valid); - return commit(timeoutType, timeoutMs); + MutexIF::TimeoutType timeoutType, + uint32_t timeoutMs) { + this->setValid(valid); + return commit(timeoutType, timeoutMs); } -template -inline ReturnValue_t LocalPoolVector::commit( - MutexIF::TimeoutType timeoutType, uint32_t timeoutMs) { - MutexGuard(LocalDpManagerAttorney::getMutexHandle(*hkManager), timeoutType, timeoutMs); - return commitWithoutLock(); +template +inline ReturnValue_t LocalPoolVector::commit(MutexIF::TimeoutType timeoutType, + uint32_t timeoutMs) { + MutexGuard(LocalDpManagerAttorney::getMutexHandle(*hkManager), timeoutType, timeoutMs); + return commitWithoutLock(); } -template +template inline ReturnValue_t LocalPoolVector::commitWithoutLock() { - if(readWriteMode == pool_rwm_t::VAR_READ) { - object_id_t targetObjectId = hkManager->getCreatorObjectId(); - reportReadCommitError("LocalPoolVector", - PoolVariableIF::INVALID_READ_WRITE_MODE, false, targetObjectId, - localPoolId); - return PoolVariableIF::INVALID_READ_WRITE_MODE; - } - PoolEntry* poolEntry = nullptr; - ReturnValue_t result = LocalDpManagerAttorney::fetchPoolEntry(*hkManager, localPoolId, - &poolEntry); - if(result != RETURN_OK) { - object_id_t targetObjectId = hkManager->getCreatorObjectId(); - reportReadCommitError("LocalPoolVector", result, false, targetObjectId, - localPoolId); - return result; - } - std::memcpy(poolEntry->getDataPtr(), this->value, poolEntry->getByteSize()); - poolEntry->setValid(this->valid); - return RETURN_OK; -} - -template -inline T& LocalPoolVector::operator [](size_t i) { - if(i < vectorSize) { - return value[i]; - } - // If this happens, I have to set some value. I consider this - // a configuration error, but I wont exit here. -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "LocalPoolVector: Invalid index. Setting or returning" - " last value!" << std::endl; -#else - sif::printWarning("LocalPoolVector: Invalid index. Setting or returning" - " last value!\n"); -#endif - return value[vectorSize - 1]; -} - -template -inline const T& LocalPoolVector::operator [](size_t i) const { - if(i < vectorSize) { - return value[i]; - } - // If this happens, I have to set some value. I consider this - // a configuration error, but I wont exit here. -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "LocalPoolVector: Invalid index. Setting or returning" - " last value!" << std::endl; -#else - sif::printWarning("LocalPoolVector: Invalid index. Setting or returning" - " last value!\n"); -#endif - return value[vectorSize - 1]; -} - -template -inline ReturnValue_t LocalPoolVector::serialize(uint8_t** buffer, - size_t* size, size_t maxSize, - SerializeIF::Endianness streamEndianness) const { - ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED; - for (uint16_t i = 0; i < vectorSize; i++) { - result = SerializeAdapter::serialize(&(value[i]), buffer, size, - maxSize, streamEndianness); - if (result != HasReturnvaluesIF::RETURN_OK) { - break; - } - } + if (readWriteMode == pool_rwm_t::VAR_READ) { + object_id_t targetObjectId = hkManager->getCreatorObjectId(); + reportReadCommitError("LocalPoolVector", PoolVariableIF::INVALID_READ_WRITE_MODE, false, + targetObjectId, localPoolId); + return PoolVariableIF::INVALID_READ_WRITE_MODE; + } + PoolEntry* poolEntry = nullptr; + ReturnValue_t result = + LocalDpManagerAttorney::fetchPoolEntry(*hkManager, localPoolId, &poolEntry); + if (result != RETURN_OK) { + object_id_t targetObjectId = hkManager->getCreatorObjectId(); + reportReadCommitError("LocalPoolVector", result, false, targetObjectId, localPoolId); return result; + } + std::memcpy(poolEntry->getDataPtr(), this->value, poolEntry->getByteSize()); + poolEntry->setValid(this->valid); + return RETURN_OK; } -template +template +inline T& LocalPoolVector::operator[](size_t i) { + if (i < vectorSize) { + return value[i]; + } + // If this happens, I have to set some value. I consider this + // a configuration error, but I wont exit here. +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "LocalPoolVector: Invalid index. Setting or returning" + " last value!" + << std::endl; +#else + sif::printWarning( + "LocalPoolVector: Invalid index. Setting or returning" + " last value!\n"); +#endif + return value[vectorSize - 1]; +} + +template +inline const T& LocalPoolVector::operator[](size_t i) const { + if (i < vectorSize) { + return value[i]; + } + // If this happens, I have to set some value. I consider this + // a configuration error, but I wont exit here. +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "LocalPoolVector: Invalid index. Setting or returning" + " last value!" + << std::endl; +#else + sif::printWarning( + "LocalPoolVector: Invalid index. Setting or returning" + " last value!\n"); +#endif + return value[vectorSize - 1]; +} + +template +inline ReturnValue_t LocalPoolVector::serialize( + uint8_t** buffer, size_t* size, size_t maxSize, + SerializeIF::Endianness streamEndianness) const { + ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED; + for (uint16_t i = 0; i < vectorSize; i++) { + result = SerializeAdapter::serialize(&(value[i]), buffer, size, maxSize, streamEndianness); + if (result != HasReturnvaluesIF::RETURN_OK) { + break; + } + } + return result; +} + +template inline size_t LocalPoolVector::getSerializedSize() const { - return vectorSize * SerializeAdapter::getSerializedSize(value); + return vectorSize * SerializeAdapter::getSerializedSize(value); } -template +template inline ReturnValue_t LocalPoolVector::deSerialize( - const uint8_t** buffer, size_t* size, - SerializeIF::Endianness streamEndianness) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED; - for (uint16_t i = 0; i < vectorSize; i++) { - result = SerializeAdapter::deSerialize(&(value[i]), buffer, size, - streamEndianness); - if (result != HasReturnvaluesIF::RETURN_OK) { - break; - } + const uint8_t** buffer, size_t* size, SerializeIF::Endianness streamEndianness) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED; + for (uint16_t i = 0; i < vectorSize; i++) { + result = SerializeAdapter::deSerialize(&(value[i]), buffer, size, streamEndianness); + if (result != HasReturnvaluesIF::RETURN_OK) { + break; } - return result; + } + return result; } #if FSFW_CPP_OSTREAM_ENABLED == 1 -template -inline std::ostream& operator<< (std::ostream &out, - const LocalPoolVector &var) { - out << "Vector: ["; - for(int i = 0;i < vectorSize; i++) { - out << var.value[i]; - if(i < vectorSize - 1) { - out << ", "; - } +template +inline std::ostream& operator<<(std::ostream& out, const LocalPoolVector& var) { + out << "Vector: ["; + for (int i = 0; i < vectorSize; i++) { + out << var.value[i]; + if (i < vectorSize - 1) { + out << ", "; } - out << "]"; - return out; + } + out << "]"; + return out; } #endif diff --git a/src/fsfw/datapoollocal/StaticLocalDataSet.h b/src/fsfw/datapoollocal/StaticLocalDataSet.h index dd264e582..59047d67b 100644 --- a/src/fsfw/datapoollocal/StaticLocalDataSet.h +++ b/src/fsfw/datapoollocal/StaticLocalDataSet.h @@ -46,7 +46,7 @@ class StaticLocalDataSet : public LocalPoolDataSetBase { } private: - std::array poolVarList; + std::array poolVarList = {}; }; #endif /* FSFW_DATAPOOLLOCAL_STATICLOCALDATASET_H_ */ diff --git a/src/fsfw/datapoollocal/internal/CMakeLists.txt b/src/fsfw/datapoollocal/internal/CMakeLists.txt index 554f3b884..6585d06e3 100644 --- a/src/fsfw/datapoollocal/internal/CMakeLists.txt +++ b/src/fsfw/datapoollocal/internal/CMakeLists.txt @@ -1,5 +1,2 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - HasLocalDpIFUserAttorney.cpp - HasLocalDpIFManagerAttorney.cpp -) +target_sources(${LIB_FSFW_NAME} PRIVATE HasLocalDpIFUserAttorney.cpp + HasLocalDpIFManagerAttorney.cpp) diff --git a/src/fsfw/devicehandlers/CMakeLists.txt b/src/fsfw/devicehandlers/CMakeLists.txt index 50c1008fb..180a89da5 100644 --- a/src/fsfw/devicehandlers/CMakeLists.txt +++ b/src/fsfw/devicehandlers/CMakeLists.txt @@ -1,11 +1,10 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - AssemblyBase.cpp - ChildHandlerBase.cpp - ChildHandlerFDIR.cpp - DeviceHandlerBase.cpp - DeviceHandlerFailureIsolation.cpp - DeviceHandlerMessage.cpp - DeviceTmReportingWrapper.cpp - HealthDevice.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} + PRIVATE AssemblyBase.cpp + ChildHandlerBase.cpp + ChildHandlerFDIR.cpp + DeviceHandlerBase.cpp + DeviceHandlerFailureIsolation.cpp + DeviceHandlerMessage.cpp + DeviceTmReportingWrapper.cpp + HealthDevice.cpp) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index ea1fcdf1d..cf457a038 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -65,7 +65,9 @@ void DeviceHandlerBase::setThermalStateRequestPoolIds(lp_id_t thermalStatePoolId } DeviceHandlerBase::~DeviceHandlerBase() { - delete comCookie; + if (comCookie != nullptr) { + delete comCookie; + } if (defaultFDIRUsed) { delete fdirInstance; } @@ -233,17 +235,26 @@ ReturnValue_t DeviceHandlerBase::initialize() { } void DeviceHandlerBase::decrementDeviceReplyMap() { + bool timedOut = false; for (std::pair& replyPair : deviceReplyMap) { - if (replyPair.second.delayCycles != 0) { + if (replyPair.second.countdown != nullptr && replyPair.second.active) { + if (replyPair.second.countdown->hasTimedOut()) { + resetTimeoutControlledReply(&replyPair.second); + timedOut = true; + } + } + if (replyPair.second.delayCycles != 0 && replyPair.second.countdown == nullptr) { replyPair.second.delayCycles--; if (replyPair.second.delayCycles == 0) { - if (replyPair.second.periodic) { - replyPair.second.delayCycles = replyPair.second.maxDelayCycles; - } - replyToReply(replyPair.first, replyPair.second, TIMEOUT); - missedReply(replyPair.first); + resetDelayCyclesControlledReply(&replyPair.second); + timedOut = true; } } + if (timedOut) { + replyToReply(replyPair.first, replyPair.second, TIMEOUT); + missedReply(replyPair.first); + timedOut = false; + } } } @@ -359,7 +370,6 @@ void DeviceHandlerBase::doStateMachine() { setMode(MODE_OFF); break; } - if (currentUptime - timeoutStart >= powerSwitcher->getSwitchDelayMs()) { triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT, 0); setMode(MODE_ERROR_ON); @@ -408,20 +418,22 @@ ReturnValue_t DeviceHandlerBase::isModeCombinationValid(Mode_t mode, Submode_t s ReturnValue_t DeviceHandlerBase::insertInCommandAndReplyMap( DeviceCommandId_t deviceCommand, uint16_t maxDelayCycles, LocalPoolDataSetBase* replyDataSet, - size_t replyLen, bool periodic, bool hasDifferentReplyId, DeviceCommandId_t replyId) { + size_t replyLen, bool periodic, bool hasDifferentReplyId, DeviceCommandId_t replyId, + Countdown* countdown) { // No need to check, as we may try to insert multiple times. - insertInCommandMap(deviceCommand); + insertInCommandMap(deviceCommand, hasDifferentReplyId, replyId); if (hasDifferentReplyId) { - return insertInReplyMap(replyId, maxDelayCycles, replyDataSet, replyLen, periodic); + return insertInReplyMap(replyId, maxDelayCycles, replyDataSet, replyLen, periodic, countdown); } else { - return insertInReplyMap(deviceCommand, maxDelayCycles, replyDataSet, replyLen, periodic); + return insertInReplyMap(deviceCommand, maxDelayCycles, replyDataSet, replyLen, periodic, + countdown); } } ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId, uint16_t maxDelayCycles, LocalPoolDataSetBase* dataSet, size_t replyLen, - bool periodic) { + bool periodic, Countdown* countdown) { DeviceReplyInfo info; info.maxDelayCycles = maxDelayCycles; info.periodic = periodic; @@ -429,6 +441,7 @@ ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId, info.replyLen = replyLen; info.dataSet = dataSet; info.command = deviceCommandMap.end(); + info.countdown = countdown; auto resultPair = deviceReplyMap.emplace(replyId, info); if (resultPair.second) { return RETURN_OK; @@ -437,11 +450,15 @@ ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId, } } -ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceCommand) { +ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceCommand, + bool useAlternativeReply, + DeviceCommandId_t alternativeReplyId) { DeviceCommandInfo info; info.expectedReplies = 0; info.isExecuting = false; info.sendReplyTo = NO_COMMANDER; + info.useAlternativeReplyId = alternativeReplyId; + info.alternativeReplyId = alternativeReplyId; auto resultPair = deviceCommandMap.emplace(deviceCommand, info); if (resultPair.second) { return RETURN_OK; @@ -451,12 +468,21 @@ ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceComm } size_t DeviceHandlerBase::getNextReplyLength(DeviceCommandId_t commandId) { - DeviceReplyIter iter = deviceReplyMap.find(commandId); - if (iter != deviceReplyMap.end()) { - return iter->second.replyLen; + DeviceCommandId_t replyId = NO_COMMAND_ID; + DeviceCommandMap::iterator command = cookieInfo.pendingCommand; + if (command->second.useAlternativeReplyId) { + replyId = command->second.alternativeReplyId; } else { - return 0; + replyId = commandId; } + DeviceReplyIter iter = deviceReplyMap.find(replyId); + if (iter != deviceReplyMap.end()) { + if ((iter->second.delayCycles != 0 && iter->second.countdown == nullptr) || + (iter->second.active && iter->second.countdown != nullptr)) { + return iter->second.replyLen; + } + } + return 0; } ReturnValue_t DeviceHandlerBase::updateReplyMapEntry(DeviceCommandId_t deviceReply, @@ -488,9 +514,19 @@ ReturnValue_t DeviceHandlerBase::updatePeriodicReply(bool enable, DeviceCommandI return COMMAND_NOT_SUPPORTED; } if (enable) { - info->delayCycles = info->maxDelayCycles; + info->active = true; + if (info->countdown != nullptr) { + info->delayCycles = info->maxDelayCycles; + } else { + info->countdown->resetTimer(); + } } else { - info->delayCycles = 0; + info->active = false; + if (info->countdown != nullptr) { + info->delayCycles = 0; + } else { + info->countdown->timeOut(); + } } } return HasReturnvaluesIF::RETURN_OK; @@ -651,7 +687,9 @@ void DeviceHandlerBase::doGetWrite() { // We need to distinguish here, because a raw command never expects a reply. //(Could be done in eRIRM, but then child implementations need to be careful. - result = enableReplyInReplyMap(cookieInfo.pendingCommand); + DeviceCommandMap::iterator command = cookieInfo.pendingCommand; + result = enableReplyInReplyMap(command, 1, command->second.useAlternativeReplyId, + command->second.alternativeReplyId); } else { // always generate a failure event, so that FDIR knows what's up triggerEvent(DEVICE_SENDING_COMMAND_FAILED, result, cookieInfo.pendingCommand->first); @@ -794,17 +832,18 @@ void DeviceHandlerBase::handleReply(const uint8_t* receivedData, DeviceCommandId DeviceReplyInfo* info = &(iter->second); - if (info->delayCycles != 0) { + if ((info->delayCycles != 0 && info->countdown == nullptr) || + (info->active && info->countdown != nullptr)) { result = interpretDeviceReply(foundId, receivedData); if (result == IGNORE_REPLY_DATA) { return; } - if (info->periodic) { - info->delayCycles = info->maxDelayCycles; - } else { - info->delayCycles = 0; + if (info->active && info->countdown != nullptr) { + resetTimeoutControlledReply(info); + } else if (info->delayCycles != 0) { + resetDelayCyclesControlledReply(info); } if (result != RETURN_OK) { @@ -823,8 +862,26 @@ void DeviceHandlerBase::handleReply(const uint8_t* receivedData, DeviceCommandId } } +void DeviceHandlerBase::resetTimeoutControlledReply(DeviceReplyInfo* info) { + if (info->periodic) { + info->countdown->resetTimer(); + } else { + info->active = false; + info->countdown->timeOut(); + } +} + +void DeviceHandlerBase::resetDelayCyclesControlledReply(DeviceReplyInfo* info) { + if (info->periodic) { + info->delayCycles = info->maxDelayCycles; + } else { + info->delayCycles = 0; + info->active = false; + } +} + ReturnValue_t DeviceHandlerBase::getStorageData(store_address_t storageAddress, uint8_t** data, - uint32_t* len) { + size_t* len) { size_t lenTmp; if (IPCStore == nullptr) { @@ -945,9 +1002,15 @@ ReturnValue_t DeviceHandlerBase::enableReplyInReplyMap(DeviceCommandMap::iterato } if (iter != deviceReplyMap.end()) { DeviceReplyInfo* info = &(iter->second); + // If a countdown has been set, the delay cycles will be ignored and the reply times out + // as soon as the countdown has expired info->delayCycles = info->maxDelayCycles; info->command = command; command->second.expectedReplies = expectedReplies; + if (info->countdown != nullptr) { + info->countdown->resetTimer(); + } + info->active = true; return RETURN_OK; } else { return NO_REPLY_EXPECTED; @@ -1182,7 +1245,8 @@ void DeviceHandlerBase::setParentQueue(MessageQueueId_t parentQueueId) { bool DeviceHandlerBase::isAwaitingReply() { std::map::iterator iter; for (iter = deviceReplyMap.begin(); iter != deviceReplyMap.end(); ++iter) { - if (iter->second.delayCycles != 0) { + if ((iter->second.delayCycles != 0 && iter->second.countdown == nullptr) || + (iter->second.active && iter->second.countdown != nullptr)) { return true; } } @@ -1337,6 +1401,13 @@ uint8_t DeviceHandlerBase::getReplyDelayCycles(DeviceCommandId_t deviceCommand) DeviceReplyMap::iterator iter = deviceReplyMap.find(deviceCommand); if (iter == deviceReplyMap.end()) { return 0; + } else if (iter->second.countdown != nullptr) { + // fake a useful return value for legacy code + if (iter->second.active) { + return 1; + } else { + return 0; + } } return iter->second.delayCycles; } diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 037f4bd75..e29e65967 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -163,7 +163,7 @@ class DeviceHandlerBase : public DeviceHandlerIF, * @param counter Specifies which Action to perform * @return RETURN_OK for successful execution */ - virtual ReturnValue_t performOperation(uint8_t counter); + virtual ReturnValue_t performOperation(uint8_t counter) override; /** * @brief Initializes the device handler @@ -173,7 +173,7 @@ class DeviceHandlerBase : public DeviceHandlerIF, * Calls fillCommandAndReplyMap(). * @return */ - virtual ReturnValue_t initialize(); + virtual ReturnValue_t initialize() override; /** * @brief Intialization steps performed after all tasks have been created. @@ -448,6 +448,9 @@ class DeviceHandlerBase : public DeviceHandlerIF, * by the device repeatedly without request) or not. Default is aperiodic (0). * Please note that periodic replies are disabled by default. You can enable them with * #updatePeriodicReply + * @param countdown Instead of using maxDelayCycles to timeout a device reply it is also possible + * to provide a pointer to a Countdown object which will signal the timeout + * when expired * @return - @c RETURN_OK when the command was successfully inserted, * - @c RETURN_FAILED else. */ @@ -455,7 +458,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, LocalPoolDataSetBase *replyDataSet = nullptr, size_t replyLen = 0, bool periodic = false, bool hasDifferentReplyId = false, - DeviceCommandId_t replyId = 0); + DeviceCommandId_t replyId = 0, + Countdown *countdown = nullptr); /** * @brief This is a helper method to insert replies in the reply map. * @param deviceCommand Identifier of the reply to add. @@ -465,12 +469,15 @@ class DeviceHandlerBase : public DeviceHandlerIF, * by the device repeatedly without request) or not. Default is aperiodic (0). * Please note that periodic replies are disabled by default. You can enable them with * #updatePeriodicReply + * @param countdown Instead of using maxDelayCycles to timeout a device reply it is also possible + * to provide a pointer to a Countdown object which will signal the timeout + * when expired * @return - @c RETURN_OK when the command was successfully inserted, * - @c RETURN_FAILED else. */ ReturnValue_t insertInReplyMap(DeviceCommandId_t deviceCommand, uint16_t maxDelayCycles, LocalPoolDataSetBase *dataSet = nullptr, size_t replyLen = 0, - bool periodic = false); + bool periodic = false, Countdown *countdown = nullptr); /** * @brief A simple command to add a command to the commandList. @@ -478,7 +485,9 @@ class DeviceHandlerBase : public DeviceHandlerIF, * @return - @c RETURN_OK when the command was successfully inserted, * - @c RETURN_FAILED else. */ - ReturnValue_t insertInCommandMap(DeviceCommandId_t deviceCommand); + ReturnValue_t insertInCommandMap(DeviceCommandId_t deviceCommand, + bool useAlternativeReply = false, + DeviceCommandId_t alternativeReplyId = 0); /** * Enables a periodic reply for a given command. It sets to delay cycles to the specified @@ -673,7 +682,7 @@ class DeviceHandlerBase : public DeviceHandlerIF, //! Pointer to the raw packet that will be sent. uint8_t *rawPacket = nullptr; //! Size of the #rawPacket. - uint32_t rawPacketLen = 0; + size_t rawPacketLen = 0; /** * The mode the device handler is currently in. @@ -751,6 +760,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, //! if this is != NO_COMMANDER, DHB was commanded externally and shall //! report everything to commander. MessageQueueId_t sendReplyTo; + bool useAlternativeReplyId; + DeviceCommandId_t alternativeReplyId; }; using DeviceCommandMap = std::map; /** @@ -779,6 +790,11 @@ class DeviceHandlerBase : public DeviceHandlerIF, LocalPoolDataSetBase *dataSet = nullptr; //! The command that expects this reply. DeviceCommandMap::iterator command; + //! Instead of using delayCycles to specify the maximum time to wait for the device reply, it + //! is also possible specify a countdown + Countdown *countdown = nullptr; + //! will be set to true when reply is enabled + bool active = false; }; using DeviceReplyMap = std::map; @@ -1054,11 +1070,12 @@ class DeviceHandlerBase : public DeviceHandlerIF, * @param parameter1 Optional parameter 1 * @param parameter2 Optional parameter 2 */ - void triggerEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0); + void triggerEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0) override; /** * Same as triggerEvent, but for forwarding if object is used as proxy. */ - virtual void forwardEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0) const; + virtual void forwardEvent(Event event, uint32_t parameter1 = 0, + uint32_t parameter2 = 0) const override; /** * Checks if current mode is transitional mode. @@ -1239,6 +1256,17 @@ class DeviceHandlerBase : public DeviceHandlerIF, */ void doGetRead(void); + /** + * @brief Resets replies which use a timeout to detect missed replies. + */ + void resetTimeoutControlledReply(DeviceReplyInfo *info); + + /** + * @brief Resets replies which use a number of maximum delay cycles to detect + * missed replies. + */ + void resetDelayCyclesControlledReply(DeviceReplyInfo *info); + /** * Retrive data from the #IPCStore. * @@ -1250,7 +1278,7 @@ class DeviceHandlerBase : public DeviceHandlerIF, * - @c RETURN_FAILED IPCStore is nullptr * - the return value from the IPCStore if it was not @c RETURN_OK */ - ReturnValue_t getStorageData(store_address_t storageAddress, uint8_t **data, uint32_t *len); + ReturnValue_t getStorageData(store_address_t storageAddress, uint8_t **data, size_t *len); /** * @param modeTo either @c MODE_ON, MODE_NORMAL or MODE_RAW, nothing else! diff --git a/src/fsfw/events/CMakeLists.txt b/src/fsfw/events/CMakeLists.txt index 28eec7723..704cca85b 100644 --- a/src/fsfw/events/CMakeLists.txt +++ b/src/fsfw/events/CMakeLists.txt @@ -1,6 +1,3 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - EventManager.cpp - EventMessage.cpp -) +target_sources(${LIB_FSFW_NAME} PRIVATE EventManager.cpp EventMessage.cpp) add_subdirectory(eventmatching) diff --git a/src/fsfw/events/Event.h b/src/fsfw/events/Event.h index 20b66679e..ecab04932 100644 --- a/src/fsfw/events/Event.h +++ b/src/fsfw/events/Event.h @@ -7,8 +7,14 @@ // could be moved to more suitable location #include -typedef uint16_t EventId_t; -typedef uint8_t EventSeverity_t; +using EventId_t = uint16_t; +using EventSeverity_t = uint8_t; +using UniqueEventId_t = uint8_t; + +namespace severity { +enum Severity : EventSeverity_t { INFO = 1, LOW = 2, MEDIUM = 3, HIGH = 4 }; + +} // namespace severity #define MAKE_EVENT(id, severity) (((severity) << 16) + (SUBSYSTEM_ID * 100) + (id)) @@ -20,18 +26,11 @@ constexpr EventId_t getEventId(Event event) { return (event & 0xFFFF); } constexpr EventSeverity_t getSeverity(Event event) { return ((event >> 16) & 0xFF); } -constexpr Event makeEvent(uint8_t subsystemId, uint8_t uniqueEventId, +constexpr Event makeEvent(uint8_t subsystemId, UniqueEventId_t uniqueEventId, EventSeverity_t eventSeverity) { return (eventSeverity << 16) + (subsystemId * 100) + uniqueEventId; } } // namespace event -namespace severity { -static constexpr EventSeverity_t INFO = 1; -static constexpr EventSeverity_t LOW = 2; -static constexpr EventSeverity_t MEDIUM = 3; -static constexpr EventSeverity_t HIGH = 4; -} // namespace severity - #endif /* EVENTOBJECT_EVENT_H_ */ diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp index aaa7d6c52..47270d2ab 100644 --- a/src/fsfw/events/EventManager.cpp +++ b/src/fsfw/events/EventManager.cpp @@ -88,6 +88,11 @@ ReturnValue_t EventManager::subscribeToEventRange(MessageQueueId_t listener, Eve return result; } +ReturnValue_t EventManager::unsubscribeFromAllEvents(MessageQueueId_t listener, + object_id_t object) { + return unsubscribeFromEventRange(listener, 0, 0, true, object); +} + ReturnValue_t EventManager::unsubscribeFromEventRange(MessageQueueId_t listener, EventId_t idFrom, EventId_t idTo, bool idInverted, object_id_t reporterFrom, diff --git a/src/fsfw/events/EventManager.h b/src/fsfw/events/EventManager.h index f2d642ffb..70245f5d1 100644 --- a/src/fsfw/events/EventManager.h +++ b/src/fsfw/events/EventManager.h @@ -37,6 +37,7 @@ class EventManager : public EventManagerIF, public ExecutableObjectIF, public Sy EventId_t idTo = 0, bool idInverted = false, object_id_t reporterFrom = 0, object_id_t reporterTo = 0, bool reporterInverted = false); + ReturnValue_t unsubscribeFromAllEvents(MessageQueueId_t listener, object_id_t object); ReturnValue_t unsubscribeFromEventRange(MessageQueueId_t listener, EventId_t idFrom = 0, EventId_t idTo = 0, bool idInverted = false, object_id_t reporterFrom = 0, object_id_t reporterTo = 0, diff --git a/src/fsfw/events/EventManagerIF.h b/src/fsfw/events/EventManagerIF.h index 98051ba05..adb61f2b1 100644 --- a/src/fsfw/events/EventManagerIF.h +++ b/src/fsfw/events/EventManagerIF.h @@ -20,6 +20,7 @@ class EventManagerIF { bool forwardAllButSelected = false) = 0; virtual ReturnValue_t subscribeToEvent(MessageQueueId_t listener, EventId_t event) = 0; virtual ReturnValue_t subscribeToAllEventsFrom(MessageQueueId_t listener, object_id_t object) = 0; + virtual ReturnValue_t unsubscribeFromAllEvents(MessageQueueId_t listener, object_id_t object) = 0; virtual ReturnValue_t subscribeToEventRange(MessageQueueId_t listener, EventId_t idFrom = 0, EventId_t idTo = 0, bool idInverted = false, object_id_t reporterFrom = 0, diff --git a/src/fsfw/events/eventmatching/CMakeLists.txt b/src/fsfw/events/eventmatching/CMakeLists.txt index 81ff9ed84..a9f9c7b30 100644 --- a/src/fsfw/events/eventmatching/CMakeLists.txt +++ b/src/fsfw/events/eventmatching/CMakeLists.txt @@ -1,7 +1,3 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - EventIdRangeMatcher.cpp - EventMatchTree.cpp - ReporterRangeMatcher.cpp - SeverityRangeMatcher.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} PRIVATE EventIdRangeMatcher.cpp EventMatchTree.cpp + ReporterRangeMatcher.cpp SeverityRangeMatcher.cpp) diff --git a/src/fsfw/events/fwSubsystemIdRanges.h b/src/fsfw/events/fwSubsystemIdRanges.h index 211236000..fa4351e99 100644 --- a/src/fsfw/events/fwSubsystemIdRanges.h +++ b/src/fsfw/events/fwSubsystemIdRanges.h @@ -27,6 +27,7 @@ enum : uint8_t { PUS_SERVICE_6 = 86, PUS_SERVICE_8 = 88, PUS_SERVICE_9 = 89, + PUS_SERVICE_11 = 91, PUS_SERVICE_17 = 97, PUS_SERVICE_23 = 103, MGM_LIS3MDL = 106, diff --git a/src/fsfw/fdir/CMakeLists.txt b/src/fsfw/fdir/CMakeLists.txt index f5ffbba8d..d41ee2ebc 100644 --- a/src/fsfw/fdir/CMakeLists.txt +++ b/src/fsfw/fdir/CMakeLists.txt @@ -1,6 +1,3 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - EventCorrelation.cpp - FailureIsolationBase.cpp - FaultCounter.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} PRIVATE EventCorrelation.cpp FailureIsolationBase.cpp + FaultCounter.cpp) diff --git a/src/fsfw/fdir/FailureIsolationBase.cpp b/src/fsfw/fdir/FailureIsolationBase.cpp index fedef8696..3e6d5dcfc 100644 --- a/src/fsfw/fdir/FailureIsolationBase.cpp +++ b/src/fsfw/fdir/FailureIsolationBase.cpp @@ -14,6 +14,16 @@ FailureIsolationBase::FailureIsolationBase(object_id_t owner, object_id_t parent } FailureIsolationBase::~FailureIsolationBase() { + EventManagerIF* manager = ObjectManager::instance()->get(objects::EVENT_MANAGER); + if (manager == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "FailureIsolationBase::~FailureIsolationBase: Event Manager has not" + " been initialized!" + << std::endl; +#endif + return; + } + manager->unsubscribeFromAllEvents(eventQueue->getId(), ownerId); QueueFactory::instance()->deleteMessageQueue(eventQueue); } diff --git a/src/fsfw/fdir/FaultCounter.cpp b/src/fsfw/fdir/FaultCounter.cpp index e87cf6134..4515e5b11 100644 --- a/src/fsfw/fdir/FaultCounter.cpp +++ b/src/fsfw/fdir/FaultCounter.cpp @@ -60,14 +60,14 @@ ReturnValue_t FaultCounter::getParameter(uint8_t domainId, uint8_t uniqueId, return INVALID_DOMAIN_ID; } - switch (uniqueId) { - case 0: + switch (static_cast(uniqueId)) { + case ParameterIds::FAILURE_THRESHOLD: parameterWrapper->set(failureThreshold); break; - case 1: + case ParameterIds::FAULT_COUNT: parameterWrapper->set(faultCount); break; - case 2: + case ParameterIds::TIMEOUT: parameterWrapper->set(timer.timeout); break; default: diff --git a/src/fsfw/fdir/FaultCounter.h b/src/fsfw/fdir/FaultCounter.h index 3b59885c2..cca780d6c 100644 --- a/src/fsfw/fdir/FaultCounter.h +++ b/src/fsfw/fdir/FaultCounter.h @@ -6,6 +6,8 @@ class FaultCounter : public HasParametersIF { public: + enum class ParameterIds { FAILURE_THRESHOLD, FAULT_COUNT, TIMEOUT }; + FaultCounter(); FaultCounter(uint32_t failureThreshold, uint32_t decrementAfterMs, uint8_t setParameterDomain = 0); @@ -25,7 +27,8 @@ class FaultCounter : public HasParametersIF { virtual ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, ParameterWrapper *parameterWrapper, - const ParameterWrapper *newValues, uint16_t startAtIndex); + const ParameterWrapper *newValues = nullptr, + uint16_t startAtIndex = 0); void setParameterDomain(uint8_t domain); diff --git a/src/fsfw/globalfunctions/CMakeLists.txt b/src/fsfw/globalfunctions/CMakeLists.txt index 5ccd3c4cc..cfa02696e 100644 --- a/src/fsfw/globalfunctions/CMakeLists.txt +++ b/src/fsfw/globalfunctions/CMakeLists.txt @@ -1,13 +1,12 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - arrayprinter.cpp - AsciiConverter.cpp - CRC.cpp - DleEncoder.cpp - PeriodicOperationDivider.cpp - timevalOperations.cpp - Type.cpp - bitutility.cpp -) +target_sources( + ${LIB_FSFW_NAME} + PRIVATE arrayprinter.cpp + AsciiConverter.cpp + CRC.cpp + DleEncoder.cpp + PeriodicOperationDivider.cpp + timevalOperations.cpp + Type.cpp + bitutility.cpp) add_subdirectory(math) diff --git a/src/fsfw/globalfunctions/CRC.cpp b/src/fsfw/globalfunctions/CRC.cpp index 6b8140c52..03945612a 100644 --- a/src/fsfw/globalfunctions/CRC.cpp +++ b/src/fsfw/globalfunctions/CRC.cpp @@ -28,7 +28,7 @@ const uint16_t CRC::crc16ccitt_table[256] = { // CRC implementation uint16_t CRC::crc16ccitt(uint8_t const input[], uint32_t length, uint16_t startingCrc) { - uint8_t *data = (uint8_t *)input; + const uint8_t *data = static_cast(input); unsigned int tbl_idx; while (length--) { @@ -39,88 +39,4 @@ uint16_t CRC::crc16ccitt(uint8_t const input[], uint32_t length, uint16_t starti } return startingCrc & 0xffff; - // The part below is not used! - // bool temr[16]; - // bool xor_out[16]; - // bool r[16]; - // bool d[8]; - // uint16_t crc_value = 0; - // - // - // for (int i=0; i<16 ;i++) { - // temr[i] = false; - // xor_out[i] = false; - // } - // - // - // for (int i=0; i<16 ;i++) - // r[i] = true; // initialize with 0xFFFF - // - // - // - // for (int j=0; j +#include #include "fsfw/serviceinterface/ServiceInterface.h" @@ -68,7 +69,7 @@ void arrayprinter::printHex(const uint8_t *data, size_t size, size_t maxCharPerL currentPos += snprintf(printBuffer + currentPos, 6, "%02x", data[i]); if (i < size - 1) { currentPos += sprintf(printBuffer + currentPos, ","); - if (i > 0 and (i + 1) % maxCharPerLine == 0) { + if ((i + 1) % maxCharPerLine == 0) { currentPos += sprintf(printBuffer + currentPos, "\n"); } } @@ -97,20 +98,21 @@ void arrayprinter::printDec(const uint8_t *data, size_t size, size_t maxCharPerL } std::cout << "]" << std::endl; #else - // General format: 32, 243, -12 so it is number of chars times 5 + // General format: 32,243,-12 so it is number of chars times 4 // plus line break plus small safety margin. - char printBuffer[(size + 1) * 5 + 1] = {}; + uint16_t expectedLines = ceil((double)size / maxCharPerLine); + char printBuffer[size * 4 + 1 + expectedLines] = {}; size_t currentPos = 0; for (size_t i = 0; i < size; i++) { // To avoid buffer overflows. - if (sizeof(printBuffer) - currentPos <= 5) { + if (sizeof(printBuffer) - currentPos <= 4) { break; } - currentPos += snprintf(printBuffer + currentPos, 3, "%d", data[i]); + currentPos += snprintf(printBuffer + currentPos, 4, "%d", data[i]); if (i < size - 1) { currentPos += sprintf(printBuffer + currentPos, ","); - if (i > 0 and (i + 1) % maxCharPerLine == 0) { + if ((i + 1) % maxCharPerLine == 0) { currentPos += sprintf(printBuffer + currentPos, "\n"); } } diff --git a/src/fsfw/globalfunctions/matching/MatchTree.h b/src/fsfw/globalfunctions/matching/MatchTree.h index f7775d45b..ec98e22a7 100644 --- a/src/fsfw/globalfunctions/matching/MatchTree.h +++ b/src/fsfw/globalfunctions/matching/MatchTree.h @@ -25,7 +25,7 @@ class MatchTree : public SerializeableMatcherIF, public BinaryTree>(root.element), maxDepth(maxDepth) {} MatchTree() : BinaryTree>(), maxDepth(-1) {} virtual ~MatchTree() {} - virtual bool match(T number) { return matchesTree(number); } + virtual bool match(T number) override { return matchesTree(number); } bool matchesTree(T number) { iterator iter = this->begin(); if (iter == this->end()) { @@ -179,6 +179,9 @@ class MatchTree : public SerializeableMatcherIF, public BinaryTreematch(number); if (isMatch) { if (iter.left() == this->end()) { diff --git a/src/fsfw/globalfunctions/matching/RangeMatcher.h b/src/fsfw/globalfunctions/matching/RangeMatcher.h index 4e1f39222..c5962ab3e 100644 --- a/src/fsfw/globalfunctions/matching/RangeMatcher.h +++ b/src/fsfw/globalfunctions/matching/RangeMatcher.h @@ -15,7 +15,7 @@ class RangeMatcher : public SerializeableMatcherIF { RangeMatcher(T lowerBound, T upperBound, bool inverted = false) : inverted(inverted), lowerBound(lowerBound), upperBound(upperBound) {} - bool match(T input) { + bool match(T input) override { if (inverted) { return !doMatch(input); } else { diff --git a/src/fsfw/globalfunctions/math/CMakeLists.txt b/src/fsfw/globalfunctions/math/CMakeLists.txt index a9c4ded78..1eeb69b50 100644 --- a/src/fsfw/globalfunctions/math/CMakeLists.txt +++ b/src/fsfw/globalfunctions/math/CMakeLists.txt @@ -1,4 +1 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - QuaternionOperations.cpp -) +target_sources(${LIB_FSFW_NAME} PRIVATE QuaternionOperations.cpp) diff --git a/src/fsfw/health/CMakeLists.txt b/src/fsfw/health/CMakeLists.txt index d5f3ccd30..37e4ce48f 100644 --- a/src/fsfw/health/CMakeLists.txt +++ b/src/fsfw/health/CMakeLists.txt @@ -1,6 +1,2 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - HealthHelper.cpp - HealthMessage.cpp - HealthTable.cpp -) \ No newline at end of file +target_sources(${LIB_FSFW_NAME} PRIVATE HealthHelper.cpp HealthMessage.cpp + HealthTable.cpp) diff --git a/src/fsfw/health/HealthHelper.cpp b/src/fsfw/health/HealthHelper.cpp index 4bef128c1..0f51ddebe 100644 --- a/src/fsfw/health/HealthHelper.cpp +++ b/src/fsfw/health/HealthHelper.cpp @@ -5,7 +5,7 @@ HealthHelper::HealthHelper(HasHealthIF* owner, object_id_t objectId) : objectId(objectId), owner(owner) {} -HealthHelper::~HealthHelper() {} +HealthHelper::~HealthHelper() { healthTable->removeObject(objectId); } ReturnValue_t HealthHelper::handleHealthCommand(CommandMessage* message) { switch (message->getCommand()) { diff --git a/src/fsfw/health/HealthTable.cpp b/src/fsfw/health/HealthTable.cpp index ab4881f41..f4bda1c37 100644 --- a/src/fsfw/health/HealthTable.cpp +++ b/src/fsfw/health/HealthTable.cpp @@ -27,6 +27,15 @@ ReturnValue_t HealthTable::registerObject(object_id_t object, return HasReturnvaluesIF::RETURN_OK; } +ReturnValue_t HealthTable::removeObject(object_id_t object) { + mapIterator = healthMap.find(object); + if (mapIterator == healthMap.end()) { + return HasReturnvaluesIF::RETURN_FAILED; + } + healthMap.erase(mapIterator); + return HasReturnvaluesIF::RETURN_OK; +} + void HealthTable::setHealth(object_id_t object, HasHealthIF::HealthState newState) { MutexGuard(mutex, timeoutType, mutexTimeoutMs); HealthMap::iterator iter = healthMap.find(object); diff --git a/src/fsfw/health/HealthTable.h b/src/fsfw/health/HealthTable.h index 4718abc35..60e00f92c 100644 --- a/src/fsfw/health/HealthTable.h +++ b/src/fsfw/health/HealthTable.h @@ -17,6 +17,7 @@ class HealthTable : public HealthTableIF, public SystemObject { /** HealthTableIF overrides */ virtual ReturnValue_t registerObject( object_id_t object, HasHealthIF::HealthState initilialState = HasHealthIF::HEALTHY) override; + ReturnValue_t removeObject(object_id_t object) override; virtual size_t getPrintSize() override; virtual void printAll(uint8_t* pointer, size_t maxSize) override; diff --git a/src/fsfw/health/HealthTableIF.h b/src/fsfw/health/HealthTableIF.h index 50266ac23..aab3f0eb1 100644 --- a/src/fsfw/health/HealthTableIF.h +++ b/src/fsfw/health/HealthTableIF.h @@ -14,6 +14,8 @@ class HealthTableIF : public ManagesHealthIF { virtual ReturnValue_t registerObject( object_id_t object, HasHealthIF::HealthState initilialState = HasHealthIF::HEALTHY) = 0; + virtual ReturnValue_t removeObject(object_id_t objectId) = 0; + virtual size_t getPrintSize() = 0; virtual void printAll(uint8_t *pointer, size_t maxSize) = 0; diff --git a/src/fsfw/housekeeping/CMakeLists.txt b/src/fsfw/housekeeping/CMakeLists.txt index fecad2e3d..236d3204d 100644 --- a/src/fsfw/housekeeping/CMakeLists.txt +++ b/src/fsfw/housekeeping/CMakeLists.txt @@ -1,5 +1,2 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - HousekeepingMessage.cpp - PeriodicHousekeepingHelper.cpp -) \ No newline at end of file +target_sources(${LIB_FSFW_NAME} PRIVATE HousekeepingMessage.cpp + PeriodicHousekeepingHelper.cpp) diff --git a/src/fsfw/internalerror/CMakeLists.txt b/src/fsfw/internalerror/CMakeLists.txt index 2b383914c..87d3c3f7c 100644 --- a/src/fsfw/internalerror/CMakeLists.txt +++ b/src/fsfw/internalerror/CMakeLists.txt @@ -1,4 +1 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - InternalErrorReporter.cpp -) \ No newline at end of file +target_sources(${LIB_FSFW_NAME} PRIVATE InternalErrorReporter.cpp) diff --git a/src/fsfw/internalerror/InternalErrorReporter.cpp b/src/fsfw/internalerror/InternalErrorReporter.cpp index e7088e2cf..fa16ec3f6 100644 --- a/src/fsfw/internalerror/InternalErrorReporter.cpp +++ b/src/fsfw/internalerror/InternalErrorReporter.cpp @@ -57,6 +57,9 @@ ReturnValue_t InternalErrorReporter::performOperation(uint8_t opCode) { internalErrorDataset.storeHits.value += newStoreHits; internalErrorDataset.tmHits.value += newTmHits; internalErrorDataset.setValidity(true, true); + if ((newQueueHits != 0) or (newStoreHits != 0) or (newTmHits != 0)) { + internalErrorDataset.setChanged(true); + } } } @@ -77,14 +80,6 @@ uint32_t InternalErrorReporter::getAndResetQueueHits() { return value; } -uint32_t InternalErrorReporter::getQueueHits() { - uint32_t value; - mutex->lockMutex(timeoutType, timeoutMs); - value = queueHits; - mutex->unlockMutex(); - return value; -} - void InternalErrorReporter::incrementQueueHits() { mutex->lockMutex(timeoutType, timeoutMs); queueHits++; @@ -100,14 +95,6 @@ uint32_t InternalErrorReporter::getAndResetTmHits() { return value; } -uint32_t InternalErrorReporter::getTmHits() { - uint32_t value; - mutex->lockMutex(timeoutType, timeoutMs); - value = tmHits; - mutex->unlockMutex(); - return value; -} - void InternalErrorReporter::incrementTmHits() { mutex->lockMutex(timeoutType, timeoutMs); tmHits++; @@ -125,14 +112,6 @@ uint32_t InternalErrorReporter::getAndResetStoreHits() { return value; } -uint32_t InternalErrorReporter::getStoreHits() { - uint32_t value; - mutex->lockMutex(timeoutType, timeoutMs); - value = storeHits; - mutex->unlockMutex(); - return value; -} - void InternalErrorReporter::incrementStoreHits() { mutex->lockMutex(timeoutType, timeoutMs); storeHits++; diff --git a/src/fsfw/internalerror/InternalErrorReporter.h b/src/fsfw/internalerror/InternalErrorReporter.h index 987111396..549be4bdc 100644 --- a/src/fsfw/internalerror/InternalErrorReporter.h +++ b/src/fsfw/internalerror/InternalErrorReporter.h @@ -46,11 +46,11 @@ class InternalErrorReporter : public SystemObject, virtual ReturnValue_t initializeAfterTaskCreation() override; virtual ReturnValue_t performOperation(uint8_t opCode) override; - virtual void queueMessageNotSent(); + virtual void queueMessageNotSent() override; - virtual void lostTm(); + virtual void lostTm() override; - virtual void storeFull(); + virtual void storeFull() override; virtual void setTaskIF(PeriodicTaskIF* task) override; @@ -74,15 +74,12 @@ class InternalErrorReporter : public SystemObject, uint32_t storeHits = 0; uint32_t getAndResetQueueHits(); - uint32_t getQueueHits(); void incrementQueueHits(); uint32_t getAndResetTmHits(); - uint32_t getTmHits(); void incrementTmHits(); uint32_t getAndResetStoreHits(); - uint32_t getStoreHits(); void incrementStoreHits(); }; diff --git a/src/fsfw/internalerror/InternalErrorReporterIF.h b/src/fsfw/internalerror/InternalErrorReporterIF.h index 9eb8e7d7e..61bb52e7b 100644 --- a/src/fsfw/internalerror/InternalErrorReporterIF.h +++ b/src/fsfw/internalerror/InternalErrorReporterIF.h @@ -1,22 +1,34 @@ #ifndef INTERNALERRORREPORTERIF_H_ #define INTERNALERRORREPORTERIF_H_ +/** + * @brief Interface which is used to report internal errors like full message queues or stores. + * @details + * This interface smust be used for the InteralErrorReporter object. + * It should be used to indicate that there was a Problem with Queues or Stores. + * + * It can be used to report missing Telemetry which could not be sent due to a internal problem. + * + */ class InternalErrorReporterIF { public: virtual ~InternalErrorReporterIF() {} - /** - * Thread safe + * @brief Function to be called if a message queue could not be sent. + * @details OSAL Implementations should call this function to indicate that + * a message was lost. + * + * Implementations are required to be Thread safe */ virtual void queueMessageNotSent() = 0; - /** - * Thread safe + * @brief Function to be called if Telemetry could not be sent + * @details Implementations must be Thread safe */ virtual void lostTm() = 0; - /** - * Thread safe + * @brief Function to be called if a onboard storage is full + * @details Implementations must be Thread safe */ virtual void storeFull() = 0; }; diff --git a/src/fsfw/ipc/CMakeLists.txt b/src/fsfw/ipc/CMakeLists.txt index 6a3afe33e..92b91f357 100644 --- a/src/fsfw/ipc/CMakeLists.txt +++ b/src/fsfw/ipc/CMakeLists.txt @@ -1,6 +1,3 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - CommandMessage.cpp - CommandMessageCleaner.cpp - MessageQueueMessage.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} PRIVATE CommandMessage.cpp CommandMessageCleaner.cpp + MessageQueueMessage.cpp MessageQueueBase.cpp) diff --git a/src/fsfw/ipc/MessageQueueBase.cpp b/src/fsfw/ipc/MessageQueueBase.cpp new file mode 100644 index 000000000..c43670ed5 --- /dev/null +++ b/src/fsfw/ipc/MessageQueueBase.cpp @@ -0,0 +1,54 @@ +#include "MessageQueueBase.h" + +MessageQueueBase::MessageQueueBase(MessageQueueId_t id, MessageQueueId_t defaultDest, MqArgs* args) + : id(id) { + this->defaultDest = defaultDest; + if (args != nullptr) { + this->args = *args; + } +} + +MessageQueueBase::~MessageQueueBase() {} + +ReturnValue_t MessageQueueBase::sendToDefault(MessageQueueMessageIF* message) { + return sendToDefaultFrom(message, this->getId(), false); +} + +ReturnValue_t MessageQueueBase::reply(MessageQueueMessageIF* message) { + if (this->last != MessageQueueIF::NO_QUEUE) { + return sendMessageFrom(this->last, message, this->getId()); + } else { + return NO_REPLY_PARTNER; + } +} + +ReturnValue_t MessageQueueBase::receiveMessage(MessageQueueMessageIF* message, + MessageQueueId_t* receivedFrom) { + ReturnValue_t status = this->receiveMessage(message); + *receivedFrom = this->last; + return status; +} + +MessageQueueId_t MessageQueueBase::getLastPartner() const { return last; } + +MessageQueueId_t MessageQueueBase::getId() const { return id; } + +MqArgs& MessageQueueBase::getMqArgs() { return args; } + +void MessageQueueBase::setDefaultDestination(MessageQueueId_t defaultDestination) { + this->defaultDest = defaultDestination; +} + +MessageQueueId_t MessageQueueBase::getDefaultDestination() const { return defaultDest; } + +bool MessageQueueBase::isDefaultDestinationSet() const { return (defaultDest != NO_QUEUE); } + +ReturnValue_t MessageQueueBase::sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message, + bool ignoreFault) { + return sendMessageFrom(sendTo, message, this->getId(), false); +} + +ReturnValue_t MessageQueueBase::sendToDefaultFrom(MessageQueueMessageIF* message, + MessageQueueId_t sentFrom, bool ignoreFault) { + return sendMessageFrom(defaultDest, message, sentFrom, ignoreFault); +} diff --git a/src/fsfw/ipc/MessageQueueBase.h b/src/fsfw/ipc/MessageQueueBase.h new file mode 100644 index 000000000..942b61217 --- /dev/null +++ b/src/fsfw/ipc/MessageQueueBase.h @@ -0,0 +1,40 @@ +#ifndef FSFW_SRC_FSFW_IPC_MESSAGEQUEUEBASE_H_ +#define FSFW_SRC_FSFW_IPC_MESSAGEQUEUEBASE_H_ + +#include +#include + +class MessageQueueBase : public MessageQueueIF { + public: + MessageQueueBase(MessageQueueId_t id, MessageQueueId_t defaultDest, MqArgs* mqArgs); + virtual ~MessageQueueBase(); + + // Default implementations for MessageQueueIF where possible + virtual MessageQueueId_t getLastPartner() const override; + virtual MessageQueueId_t getId() const override; + virtual MqArgs& getMqArgs() override; + virtual void setDefaultDestination(MessageQueueId_t defaultDestination) override; + virtual MessageQueueId_t getDefaultDestination() const override; + virtual bool isDefaultDestinationSet() const override; + virtual ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message, + bool ignoreFault) override; + virtual ReturnValue_t sendToDefault(MessageQueueMessageIF* message) override; + virtual ReturnValue_t reply(MessageQueueMessageIF* message) override; + virtual ReturnValue_t receiveMessage(MessageQueueMessageIF* message, + MessageQueueId_t* receivedFrom) override; + virtual ReturnValue_t sendToDefaultFrom(MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault = false) override; + + // OSAL specific, forward the abstract function + virtual ReturnValue_t receiveMessage(MessageQueueMessageIF* message) = 0; + virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message, + MessageQueueId_t sentFrom, bool ignoreFault = false) = 0; + + protected: + MessageQueueId_t id = MessageQueueIF::NO_QUEUE; + MessageQueueId_t last = MessageQueueIF::NO_QUEUE; + MessageQueueId_t defaultDest = MessageQueueIF::NO_QUEUE; + MqArgs args = {}; +}; + +#endif /* FSFW_SRC_FSFW_IPC_MESSAGEQUEUEBASE_H_ */ diff --git a/src/fsfw/ipc/MessageQueueIF.h b/src/fsfw/ipc/MessageQueueIF.h index 9aef58b78..9532b2d61 100644 --- a/src/fsfw/ipc/MessageQueueIF.h +++ b/src/fsfw/ipc/MessageQueueIF.h @@ -1,6 +1,8 @@ #ifndef FSFW_IPC_MESSAGEQUEUEIF_H_ #define FSFW_IPC_MESSAGEQUEUEIF_H_ +#include + #include #include "../returnvalues/HasReturnvaluesIF.h" @@ -44,8 +46,8 @@ class MessageQueueIF { virtual ReturnValue_t reply(MessageQueueMessageIF* message) = 0; /** - * @brief This function reads available messages from the message queue - * and returns the sender. + * @brief This function reads available messages from the message queue and returns the + * sender. * @details * It works identically to the other receiveMessage call, but in addition * returns the sender's queue id. @@ -78,19 +80,16 @@ class MessageQueueIF { */ virtual ReturnValue_t flush(uint32_t* count) = 0; /** - * @brief This method returns the message queue - * id of the last communication partner. + * @brief This method returns the message queue ID of the last communication partner. */ virtual MessageQueueId_t getLastPartner() const = 0; /** - * @brief This method returns the message queue - * id of this class's message queue. + * @brief This method returns the message queue ID of this class's message queue. */ virtual MessageQueueId_t getId() const = 0; /** - * @brief With the sendMessage call, a queue message - * is sent to a receiving queue. + * @brief With the sendMessage call, a queue message is sent to a receiving queue. * @details * This method takes the message provided, adds the sentFrom information * and passes it on to the destination provided with an operating system @@ -129,8 +128,7 @@ class MessageQueueIF { bool ignoreFault = false) = 0; /** - * @brief The sendToDefaultFrom method sends a queue message - * to the default destination. + * @brief The sendToDefaultFrom method sends a queue message to the default destination. * @details * In all other aspects, it works identical to the sendMessage method. * @param message @@ -164,6 +162,8 @@ class MessageQueueIF { virtual MessageQueueId_t getDefaultDestination() const = 0; virtual bool isDefaultDestinationSet() const = 0; + + virtual MqArgs& getMqArgs() = 0; }; #endif /* FSFW_IPC_MESSAGEQUEUEIF_H_ */ diff --git a/src/fsfw/ipc/QueueFactory.h b/src/fsfw/ipc/QueueFactory.h index 864c456d4..8069d8369 100644 --- a/src/fsfw/ipc/QueueFactory.h +++ b/src/fsfw/ipc/QueueFactory.h @@ -5,6 +5,7 @@ #include "MessageQueueIF.h" #include "MessageQueueMessage.h" +#include "definitions.h" /** * Creates message queues. @@ -22,7 +23,8 @@ class QueueFactory { static QueueFactory* instance(); MessageQueueIF* createMessageQueue(uint32_t messageDepth = 3, - size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE); + size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE, + MqArgs* args = nullptr); void deleteMessageQueue(MessageQueueIF* queue); diff --git a/src/fsfw/ipc/definitions.h b/src/fsfw/ipc/definitions.h new file mode 100644 index 000000000..150502bbe --- /dev/null +++ b/src/fsfw/ipc/definitions.h @@ -0,0 +1,14 @@ +#ifndef FSFW_SRC_FSFW_IPC_DEFINITIONS_H_ +#define FSFW_SRC_FSFW_IPC_DEFINITIONS_H_ + +#include +#include + +struct MqArgs { + MqArgs(){}; + MqArgs(object_id_t objectId, void* args = nullptr) : objectId(objectId), args(args) {} + object_id_t objectId = objects::NO_OBJECT; + void* args = nullptr; +}; + +#endif /* FSFW_SRC_FSFW_IPC_DEFINITIONS_H_ */ diff --git a/src/fsfw/memory/CMakeLists.txt b/src/fsfw/memory/CMakeLists.txt index c713cd42d..9e591bae4 100644 --- a/src/fsfw/memory/CMakeLists.txt +++ b/src/fsfw/memory/CMakeLists.txt @@ -1,5 +1,2 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - MemoryHelper.cpp - MemoryMessage.cpp - GenericFileSystemMessage.cpp -) \ No newline at end of file +target_sources(${LIB_FSFW_NAME} PRIVATE MemoryHelper.cpp MemoryMessage.cpp + GenericFileSystemMessage.cpp) diff --git a/src/fsfw/modes/CMakeLists.txt b/src/fsfw/modes/CMakeLists.txt index 8e5c719bc..4eef58e0f 100644 --- a/src/fsfw/modes/CMakeLists.txt +++ b/src/fsfw/modes/CMakeLists.txt @@ -1,5 +1 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - ModeHelper.cpp - ModeMessage.cpp -) \ No newline at end of file +target_sources(${LIB_FSFW_NAME} PRIVATE ModeHelper.cpp ModeMessage.cpp) diff --git a/src/fsfw/monitoring/CMakeLists.txt b/src/fsfw/monitoring/CMakeLists.txt index d26e807c3..48f945b5f 100644 --- a/src/fsfw/monitoring/CMakeLists.txt +++ b/src/fsfw/monitoring/CMakeLists.txt @@ -1,5 +1,2 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - LimitViolationReporter.cpp - MonitoringMessage.cpp -) \ No newline at end of file +target_sources(${LIB_FSFW_NAME} PRIVATE LimitViolationReporter.cpp + MonitoringMessage.cpp) diff --git a/src/fsfw/monitoring/MonitoringMessageContent.h b/src/fsfw/monitoring/MonitoringMessageContent.h index 0ac455ebb..fb3ace3da 100644 --- a/src/fsfw/monitoring/MonitoringMessageContent.h +++ b/src/fsfw/monitoring/MonitoringMessageContent.h @@ -34,7 +34,7 @@ class MonitoringReportContent : public SerialLinkedListAdapter { SerializeElement limitValue; SerializeElement oldState; SerializeElement newState; - uint8_t rawTimestamp[TimeStamperIF::MISSION_TIMESTAMP_SIZE]; + uint8_t rawTimestamp[TimeStamperIF::MISSION_TIMESTAMP_SIZE] = {}; SerializeElement> timestampSerializer; TimeStamperIF* timeStamper; MonitoringReportContent() @@ -46,7 +46,6 @@ class MonitoringReportContent : public SerialLinkedListAdapter { limitValue(0), oldState(0), newState(0), - rawTimestamp({0}), timestampSerializer(rawTimestamp, sizeof(rawTimestamp)), timeStamper(NULL) { setAllNext(); diff --git a/src/fsfw/objectmanager/CMakeLists.txt b/src/fsfw/objectmanager/CMakeLists.txt index 72aaec896..c71f43aa5 100644 --- a/src/fsfw/objectmanager/CMakeLists.txt +++ b/src/fsfw/objectmanager/CMakeLists.txt @@ -1,5 +1 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - ObjectManager.cpp - SystemObject.cpp -) \ No newline at end of file +target_sources(${LIB_FSFW_NAME} PRIVATE ObjectManager.cpp SystemObject.cpp) diff --git a/src/fsfw/objectmanager/SystemObject.h b/src/fsfw/objectmanager/SystemObject.h index 6d5b83035..c541ac5ec 100644 --- a/src/fsfw/objectmanager/SystemObject.h +++ b/src/fsfw/objectmanager/SystemObject.h @@ -48,9 +48,10 @@ class SystemObject : public SystemObjectIF { virtual ~SystemObject(); object_id_t getObjectId() const override; virtual ReturnValue_t initialize() override; - virtual ReturnValue_t checkObjectConnections(); + virtual ReturnValue_t checkObjectConnections() override; - virtual void forwardEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0) const; + virtual void forwardEvent(Event event, uint32_t parameter1 = 0, + uint32_t parameter2 = 0) const override; }; #endif /* FSFW_OBJECTMANAGER_SYSTEMOBJECT_H_ */ diff --git a/src/fsfw/objectmanager/frameworkObjects.h b/src/fsfw/objectmanager/frameworkObjects.h index cc233c0f1..cddc6ba24 100644 --- a/src/fsfw/objectmanager/frameworkObjects.h +++ b/src/fsfw/objectmanager/frameworkObjects.h @@ -14,6 +14,7 @@ enum framework_objects : object_id_t { PUS_SERVICE_5_EVENT_REPORTING = 0x53000005, PUS_SERVICE_8_FUNCTION_MGMT = 0x53000008, PUS_SERVICE_9_TIME_MGMT = 0x53000009, + PUS_SERVICE_11_TC_SCHEDULER = 0x53000011, PUS_SERVICE_17_TEST = 0x53000017, PUS_SERVICE_20_PARAMETERS = 0x53000020, PUS_SERVICE_200_MODE_MGMT = 0x53000200, diff --git a/src/fsfw/osal/CMakeLists.txt b/src/fsfw/osal/CMakeLists.txt index f3c5cfadc..50fd61025 100644 --- a/src/fsfw/osal/CMakeLists.txt +++ b/src/fsfw/osal/CMakeLists.txt @@ -1,35 +1,33 @@ # Check the OS_FSFW variable if(FSFW_OSAL MATCHES "freertos") - add_subdirectory(freertos) + add_subdirectory(freertos) elseif(FSFW_OSAL MATCHES "rtems") - add_subdirectory(rtems) + add_subdirectory(rtems) elseif(FSFW_OSAL MATCHES "linux") - add_subdirectory(linux) + add_subdirectory(linux) elseif(FSFW_OSAL MATCHES "host") - add_subdirectory(host) - if (WIN32) - add_subdirectory(windows) - elseif(UNIX) - # We still need to pull in some Linux specific sources - target_sources(${LIB_FSFW_NAME} PUBLIC - linux/tcpipHelpers.cpp - ) - endif () + add_subdirectory(host) + if(WIN32) + add_subdirectory(windows) + elseif(UNIX) + # We still need to pull in some Linux specific sources + target_sources(${LIB_FSFW_NAME} PUBLIC linux/tcpipHelpers.cpp) + endif() else() - message(WARNING "The OS_FSFW variable was not set. Assuming host OS..") - # Not set. Assumuing this is a host build, try to determine host OS - if (WIN32) - add_subdirectory(host) - add_subdirectory(windows) - elseif (UNIX) - add_subdirectory(linux) - else () - # MacOS or other OSes have not been tested yet / are not supported. - message(FATAL_ERROR "The host OS could not be determined! Aborting.") - endif() + message(WARNING "The OS_FSFW variable was not set. Assuming host OS..") + # Not set. Assumuing this is a host build, try to determine host OS + if(WIN32) + add_subdirectory(host) + add_subdirectory(windows) + elseif(UNIX) + add_subdirectory(linux) + else() + # MacOS or other OSes have not been tested yet / are not supported. + message(FATAL_ERROR "The host OS could not be determined! Aborting.") + endif() endif() -add_subdirectory(common) \ No newline at end of file +add_subdirectory(common) diff --git a/src/fsfw/osal/common/CMakeLists.txt b/src/fsfw/osal/common/CMakeLists.txt index b7c8c033a..c08141721 100644 --- a/src/fsfw/osal/common/CMakeLists.txt +++ b/src/fsfw/osal/common/CMakeLists.txt @@ -1,17 +1,10 @@ if(DEFINED WIN32 OR DEFINED UNIX) - target_sources(${LIB_FSFW_NAME} PRIVATE - tcpipCommon.cpp - TcpIpBase.cpp - UdpTcPollingTask.cpp - UdpTmTcBridge.cpp - TcpTmTcServer.cpp - TcpTmTcBridge.cpp - ) + target_sources( + ${LIB_FSFW_NAME} + PRIVATE tcpipCommon.cpp TcpIpBase.cpp UdpTcPollingTask.cpp + UdpTmTcBridge.cpp TcpTmTcServer.cpp TcpTmTcBridge.cpp) endif() if(WIN32) - target_link_libraries(${LIB_FSFW_NAME} PRIVATE - wsock32 - ws2_32 - ) -endif() \ No newline at end of file + target_link_libraries(${LIB_FSFW_NAME} PRIVATE wsock32 ws2_32) +endif() diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 2e6910c5a..a8890006f 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -19,6 +19,8 @@ #include #elif defined(PLATFORM_UNIX) #include + +#include #endif const std::string TcpTmTcServer::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_PORT; @@ -29,7 +31,7 @@ TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge, : SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), receptionMode(receptionMode), - tcpConfig(customTcpServerPort), + tcpConfig(std::move(customTcpServerPort)), receptionBuffer(receptionBufferSize), ringBuffer(ringBufferSize, true) {} @@ -103,12 +105,12 @@ ReturnValue_t TcpTmTcServer::initialize() { TcpTmTcServer::~TcpTmTcServer() { closeSocket(listenerTcpSocket); } -ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) { +[[noreturn]] ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) { using namespace tcpip; // If a connection is accepted, the corresponding socket will be assigned to the new socket socket_t connSocket = 0; - // sockaddr clientSockAddr = {}; - // socklen_t connectorSockAddrLen = 0; + sockaddr clientSockAddr = {}; + socklen_t connectorSockAddrLen = 0; int retval = 0; // Listen for connection requests permanently for lifetime of program @@ -119,8 +121,7 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) { continue; } - // connSocket = accept(listenerTcpSocket, &clientSockAddr, &connectorSockAddrLen); - connSocket = accept(listenerTcpSocket, nullptr, nullptr); + connSocket = accept(listenerTcpSocket, &clientSockAddr, &connectorSockAddrLen); if (connSocket == INVALID_SOCKET) { handleError(Protocol::TCP, ErrorSources::ACCEPT_CALL, 500); @@ -135,10 +136,10 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) { if (retval != 0) { handleError(Protocol::TCP, ErrorSources::SHUTDOWN_CALL); } + closeSocket(connSocket); connSocket = 0; } - return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t TcpTmTcServer::initializeAfterTaskCreation() { @@ -159,8 +160,8 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { #endif while (true) { - int retval = recv(connSocket, reinterpret_cast(receptionBuffer.data()), - receptionBuffer.capacity(), tcpConfig.tcpFlags); + ssize_t retval = recv(connSocket, reinterpret_cast(receptionBuffer.data()), + receptionBuffer.capacity(), tcpConfig.tcpFlags); if (retval == 0) { size_t availableReadData = ringBuffer.getAvailableReadData(); if (availableReadData > lastRingBufferSize) { @@ -252,17 +253,17 @@ ReturnValue_t TcpTmTcServer::handleTcReception(uint8_t* spacePacket, size_t pack return result; } -std::string TcpTmTcServer::getTcpPort() const { return tcpConfig.tcpPort; } +const std::string& TcpTmTcServer::getTcpPort() const { return tcpConfig.tcpPort; } -void TcpTmTcServer::setSpacePacketParsingOptions(std::vector validPacketIds) { - this->validPacketIds = validPacketIds; +void TcpTmTcServer::setSpacePacketParsingOptions(std::vector validPacketIds_) { + this->validPacketIds = std::move(validPacketIds_); } TcpTmTcServer::TcpConfig& TcpTmTcServer::getTcpConfigStruct() { return tcpConfig; } ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent) { // Access to the FIFO is mutex protected because it is filled by the bridge - MutexGuard(tmtcBridge->mutex, tmtcBridge->timeoutType, tmtcBridge->mutexTimeoutMs); + MutexGuard mg(tmtcBridge->mutex, tmtcBridge->timeoutType, tmtcBridge->mutexTimeoutMs); store_address_t storeId; while ((not tmtcBridge->tmFifo->empty()) and (tmtcBridge->packetSentCounter < tmtcBridge->sentPacketsPerCycle)) { @@ -283,8 +284,8 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent) #endif arrayprinter::print(storeAccessor.data(), storeAccessor.size()); } - int retval = send(connSocket, reinterpret_cast(storeAccessor.data()), - storeAccessor.size(), tcpConfig.tcpTmFlags); + ssize_t retval = send(connSocket, reinterpret_cast(storeAccessor.data()), + storeAccessor.size(), tcpConfig.tcpTmFlags); if (retval == static_cast(storeAccessor.size())) { // Packet sent, clear FIFO entry tmtcBridge->tmFifo->pop(); @@ -339,6 +340,9 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) { size_t foundSize = 0; size_t readLen = 0; while (readLen < readAmount) { + if (spacePacketParser == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } result = spacePacketParser->parseSpacePackets(bufPtrPtr, readAmount, startIdx, foundSize, readLen); switch (result) { diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h index d062a0ce0..faf6e0a17 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.h +++ b/src/fsfw/osal/common/TcpTmTcServer.h @@ -17,6 +17,7 @@ #endif #include +#include #include class TcpTmTcBridge; @@ -44,7 +45,7 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb struct TcpConfig { public: - TcpConfig(std::string tcpPort) : tcpPort(tcpPort) {} + explicit TcpConfig(std::string tcpPort) : tcpPort(std::move(tcpPort)) {} /** * Passed to the recv call @@ -84,7 +85,7 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb size_t ringBufferSize = RING_BUFFER_SIZE, std::string customTcpServerPort = DEFAULT_SERVER_PORT, ReceptionModes receptionMode = ReceptionModes::SPACE_PACKETS); - virtual ~TcpTmTcServer(); + ~TcpTmTcServer() override; void enableWiretapping(bool enable); @@ -97,10 +98,10 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb void setSpacePacketParsingOptions(std::vector validPacketIds); ReturnValue_t initialize() override; - ReturnValue_t performOperation(uint8_t opCode) override; + [[noreturn]] ReturnValue_t performOperation(uint8_t opCode) override; ReturnValue_t initializeAfterTaskCreation() override; - std::string getTcpPort() const; + [[nodiscard]] const std::string& getTcpPort() const; protected: StorageManagerIF* tcStore = nullptr; @@ -115,7 +116,7 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb ReceptionModes receptionMode; TcpConfig tcpConfig; - struct sockaddr tcpAddress; + struct sockaddr tcpAddress = {}; socket_t listenerTcpSocket = 0; MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE; diff --git a/src/fsfw/osal/common/UdpTcPollingTask.cpp b/src/fsfw/osal/common/UdpTcPollingTask.cpp index ab982a3c8..bcc8e9e33 100644 --- a/src/fsfw/osal/common/UdpTcPollingTask.cpp +++ b/src/fsfw/osal/common/UdpTcPollingTask.cpp @@ -16,11 +16,13 @@ //! Debugging preprocessor define. #define FSFW_UDP_RECV_WIRETAPPING_ENABLED 0 +const timeval UdpTcPollingTask::DEFAULT_TIMEOUT = {0, 500000}; + UdpTcPollingTask::UdpTcPollingTask(object_id_t objectId, object_id_t tmtcUdpBridge, size_t maxRecvSize, double timeoutSeconds) : SystemObject(objectId), tmtcBridgeId(tmtcUdpBridge) { - if (frameSize > 0) { - this->frameSize = frameSize; + if (maxRecvSize > 0) { + this->frameSize = maxRecvSize; } else { this->frameSize = DEFAULT_MAX_RECV_SIZE; } @@ -31,22 +33,20 @@ UdpTcPollingTask::UdpTcPollingTask(object_id_t objectId, object_id_t tmtcUdpBrid receptionBuffer.resize(this->frameSize); if (timeoutSeconds == -1) { - receptionTimeout = DEFAULT_TIMEOUT; + receptionTimeout = UdpTcPollingTask::DEFAULT_TIMEOUT; } else { receptionTimeout = timevalOperations::toTimeval(timeoutSeconds); } } -UdpTcPollingTask::~UdpTcPollingTask() {} - -ReturnValue_t UdpTcPollingTask::performOperation(uint8_t opCode) { +[[noreturn]] ReturnValue_t UdpTcPollingTask::performOperation(uint8_t opCode) { /* Sender Address is cached here. */ - struct sockaddr senderAddress; + struct sockaddr senderAddress {}; socklen_t senderAddressSize = sizeof(senderAddress); /* Poll for new UDP datagrams in permanent loop. */ while (true) { - int bytesReceived = + ssize_t bytesReceived = recvfrom(this->serverSocket, reinterpret_cast(receptionBuffer.data()), frameSize, receptionFlags, &senderAddress, &senderAddressSize); if (bytesReceived == SOCKET_ERROR) { @@ -70,7 +70,6 @@ ReturnValue_t UdpTcPollingTask::performOperation(uint8_t opCode) { } tmtcBridge->checkAndSetClientAddress(senderAddress); } - return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t UdpTcPollingTask::handleSuccessfullTcRead(size_t bytesRead) { @@ -155,7 +154,7 @@ void UdpTcPollingTask::setTimeout(double timeoutSeconds) { #endif } #elif defined(PLATFORM_UNIX) - timeval tval; + timeval tval{}; tval = timevalOperations::toTimeval(timeoutSeconds); int result = setsockopt(serverSocket, SOL_SOCKET, SO_RCVTIMEO, &tval, sizeof(receptionTimeout)); if (result == -1) { diff --git a/src/fsfw/osal/common/UdpTcPollingTask.h b/src/fsfw/osal/common/UdpTcPollingTask.h index afcd32a1b..894716d70 100644 --- a/src/fsfw/osal/common/UdpTcPollingTask.h +++ b/src/fsfw/osal/common/UdpTcPollingTask.h @@ -21,11 +21,11 @@ class UdpTcPollingTask : public TcpIpBase, public SystemObject, public Executabl public: static constexpr size_t DEFAULT_MAX_RECV_SIZE = 1500; //! 0.5 default milliseconds timeout for now. - static constexpr timeval DEFAULT_TIMEOUT = {0, 500}; + static const timeval DEFAULT_TIMEOUT; UdpTcPollingTask(object_id_t objectId, object_id_t tmtcUdpBridge, size_t maxRecvSize = 0, double timeoutSeconds = -1); - virtual ~UdpTcPollingTask(); + ~UdpTcPollingTask() override = default; /** * Turn on optional timeout for UDP polling. In the default mode, @@ -34,9 +34,9 @@ class UdpTcPollingTask : public TcpIpBase, public SystemObject, public Executabl */ void setTimeout(double timeoutSeconds); - virtual ReturnValue_t performOperation(uint8_t opCode) override; - virtual ReturnValue_t initialize() override; - virtual ReturnValue_t initializeAfterTaskCreation() override; + [[noreturn]] ReturnValue_t performOperation(uint8_t opCode) override; + ReturnValue_t initialize() override; + ReturnValue_t initializeAfterTaskCreation() override; protected: StorageManagerIF* tcStore = nullptr; @@ -51,7 +51,7 @@ class UdpTcPollingTask : public TcpIpBase, public SystemObject, public Executabl std::vector receptionBuffer; size_t frameSize = 0; - timeval receptionTimeout; + timeval receptionTimeout{}; ReturnValue_t handleSuccessfullTcRead(size_t bytesRead); }; diff --git a/src/fsfw/osal/common/UdpTmTcBridge.cpp b/src/fsfw/osal/common/UdpTmTcBridge.cpp index 0efe1c744..e3cad58ff 100644 --- a/src/fsfw/osal/common/UdpTmTcBridge.cpp +++ b/src/fsfw/osal/common/UdpTmTcBridge.cpp @@ -20,13 +20,13 @@ const std::string UdpTmTcBridge::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_PORT; UdpTmTcBridge::UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination, - std::string udpServerPort, object_id_t tmStoreId, + const std::string &udpServerPort_, object_id_t tmStoreId, object_id_t tcStoreId) : TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId) { - if (udpServerPort == "") { - this->udpServerPort = DEFAULT_SERVER_PORT; + if (udpServerPort_.empty()) { + udpServerPort = DEFAULT_SERVER_PORT; } else { - this->udpServerPort = udpServerPort; + udpServerPort = udpServerPort_; } mutex = MutexFactory::instance()->createMutex(); @@ -117,8 +117,8 @@ ReturnValue_t UdpTmTcBridge::sendTm(const uint8_t *data, size_t dataLen) { tcpip::printAddress(&clientAddress); #endif - int bytesSent = sendto(serverSocket, reinterpret_cast(data), dataLen, flags, - &clientAddress, clientAddressLen); + ssize_t bytesSent = sendto(serverSocket, reinterpret_cast(data), dataLen, flags, + &clientAddress, clientAddressLen); if (bytesSent == SOCKET_ERROR) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "TmTcUdpBridge::sendTm: Send operation failed." << std::endl; @@ -150,7 +150,7 @@ void UdpTmTcBridge::checkAndSetClientAddress(sockaddr &newAddress) { clientAddressLen = sizeof(clientAddress); } -void UdpTmTcBridge::setMutexProperties(MutexIF::TimeoutType timeoutType, dur_millis_t timeoutMs) { - this->timeoutType = timeoutType; - this->mutexTimeoutMs = timeoutMs; +void UdpTmTcBridge::setMutexProperties(MutexIF::TimeoutType timeoutType_, dur_millis_t timeoutMs) { + timeoutType = timeoutType_; + mutexTimeoutMs = timeoutMs; } diff --git a/src/fsfw/osal/common/UdpTmTcBridge.h b/src/fsfw/osal/common/UdpTmTcBridge.h index 78843ccd3..92829c468 100644 --- a/src/fsfw/osal/common/UdpTmTcBridge.h +++ b/src/fsfw/osal/common/UdpTmTcBridge.h @@ -29,10 +29,10 @@ class UdpTmTcBridge : public TmTcBridge, public TcpIpBase { /* The ports chosen here should not be used by any other process. */ static const std::string DEFAULT_SERVER_PORT; - UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination, std::string udpServerPort = "", - object_id_t tmStoreId = objects::TM_STORE, + UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination, + const std::string& udpServerPort = "", object_id_t tmStoreId = objects::TM_STORE, object_id_t tcStoreId = objects::TC_STORE); - virtual ~UdpTmTcBridge(); + ~UdpTmTcBridge() override; /** * Set properties of internal mutex. @@ -46,12 +46,12 @@ class UdpTmTcBridge : public TmTcBridge, public TcpIpBase { std::string getUdpPort() const; protected: - virtual ReturnValue_t sendTm(const uint8_t* data, size_t dataLen) override; + ReturnValue_t sendTm(const uint8_t* data, size_t dataLen) override; private: std::string udpServerPort; - struct sockaddr clientAddress; + struct sockaddr clientAddress = {}; socklen_t clientAddressLen = 0; //! Access to the client address is mutex protected as it is set by another task. diff --git a/src/fsfw/osal/freertos/CMakeLists.txt b/src/fsfw/osal/freertos/CMakeLists.txt index 40bdcd0f5..cb6ac55aa 100644 --- a/src/fsfw/osal/freertos/CMakeLists.txt +++ b/src/fsfw/osal/freertos/CMakeLists.txt @@ -1,32 +1,30 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - Clock.cpp - FixedTimeslotTask.cpp - BinarySemaphore.cpp - BinSemaphUsingTask.cpp - CountingSemaphore.cpp - CountingSemaphUsingTask.cpp - MessageQueue.cpp - Mutex.cpp - MutexFactory.cpp - PeriodicTask.cpp - QueueFactory.cpp - SemaphoreFactory.cpp - TaskFactory.cpp - Timekeeper.cpp - TaskManagement.cpp - QueueMapManager.cpp -) +target_sources( + ${LIB_FSFW_NAME} + PRIVATE Clock.cpp + FixedTimeslotTask.cpp + BinarySemaphore.cpp + BinSemaphUsingTask.cpp + CountingSemaphore.cpp + CountingSemaphUsingTask.cpp + MessageQueue.cpp + Mutex.cpp + MutexFactory.cpp + PeriodicTask.cpp + QueueFactory.cpp + SemaphoreFactory.cpp + TaskFactory.cpp + Timekeeper.cpp + TaskManagement.cpp + QueueMapManager.cpp) -# FreeRTOS is required to link the FSFW now. It is recommended to compile -# FreeRTOS as a static library and set LIB_OS_NAME to the target name of the +# FreeRTOS is required to link the FSFW now. It is recommended to compile +# FreeRTOS as a static library and set LIB_OS_NAME to the target name of the # library. if(NOT LIB_OS_NAME) - message(STATUS - "LIB_OS_NAME is empty. Make sure to include the FreeRTOS header path properly." - ) + message( + STATUS + "LIB_OS_NAME is empty. Make sure to include the FreeRTOS header path properly." + ) else() - target_link_libraries(${LIB_FSFW_NAME} PRIVATE - ${LIB_OS_NAME} - ) + target_link_libraries(${LIB_FSFW_NAME} PRIVATE ${LIB_OS_NAME}) endif() diff --git a/src/fsfw/osal/freertos/Clock.cpp b/src/fsfw/osal/freertos/Clock.cpp index 050839684..cabf7d812 100644 --- a/src/fsfw/osal/freertos/Clock.cpp +++ b/src/fsfw/osal/freertos/Clock.cpp @@ -11,9 +11,6 @@ // TODO sanitize input? // TODO much of this code can be reused for tick-only systems -uint16_t Clock::leapSeconds = 0; -MutexIF* Clock::timeMutex = nullptr; - uint32_t Clock::getTicksPerSecond(void) { return 1000; } ReturnValue_t Clock::setClock(const TimeOfDay_t* time) { diff --git a/src/fsfw/osal/freertos/FixedTimeslotTask.cpp b/src/fsfw/osal/freertos/FixedTimeslotTask.cpp index e0e3779e1..e86636abc 100644 --- a/src/fsfw/osal/freertos/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/freertos/FixedTimeslotTask.cpp @@ -1,27 +1,23 @@ #include "fsfw/osal/freertos/FixedTimeslotTask.h" -#include "fsfw/objectmanager/ObjectManager.h" -#include "fsfw/serviceinterface/ServiceInterface.h" +#include "fsfw/serviceinterface.h" -uint32_t FixedTimeslotTask::deadlineMissedCount = 0; const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = configMINIMAL_STACK_SIZE; FixedTimeslotTask::FixedTimeslotTask(TaskName name, TaskPriority setPriority, - TaskStackSize setStack, TaskPeriod overallPeriod, - void (*setDeadlineMissedFunc)()) - : started(false), handle(nullptr), pst(overallPeriod * 1000) { + TaskStackSize setStack, TaskPeriod period, + TaskDeadlineMissedFunction dlmFunc_) + : FixedTimeslotTaskBase(period, dlmFunc_), started(false), handle(nullptr) { configSTACK_DEPTH_TYPE stackSize = setStack / sizeof(configSTACK_DEPTH_TYPE); xTaskCreate(taskEntryPoint, name, stackSize, this, setPriority, &handle); - // All additional attributes are applied to the object. - this->deadlineMissedFunc = setDeadlineMissedFunc; } -FixedTimeslotTask::~FixedTimeslotTask() {} +FixedTimeslotTask::~FixedTimeslotTask() = default; void FixedTimeslotTask::taskEntryPoint(void* argument) { // The argument is re-interpreted as FixedTimeslotTask. The Task object is // global, so it is found from any place. - FixedTimeslotTask* originalTask(reinterpret_cast(argument)); + auto* originalTask(reinterpret_cast(argument)); /* Task should not start until explicitly requested, * but in FreeRTOS, tasks start as soon as they are created if the scheduler * is running but not if the scheduler is not running. @@ -32,26 +28,18 @@ void FixedTimeslotTask::taskEntryPoint(void* argument) { * can continue */ if (not originalTask->started) { - vTaskSuspend(NULL); + vTaskSuspend(nullptr); } originalTask->taskFunctionality(); #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "Polling task " << originalTask->handle << " returned from taskFunctionality." << std::endl; +#else + sif::printDebug("Polling task returned from taskFunctionality\n"); #endif } -void FixedTimeslotTask::missedDeadlineCounter() { - FixedTimeslotTask::deadlineMissedCount++; - if (FixedTimeslotTask::deadlineMissedCount % 10 == 0) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines." - << std::endl; -#endif - } -} - ReturnValue_t FixedTimeslotTask::startTask() { started = true; @@ -63,31 +51,12 @@ ReturnValue_t FixedTimeslotTask::startTask() { return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, uint32_t slotTimeMs, - int8_t executionStep) { - ExecutableObjectIF* handler = ObjectManager::instance()->get(componentId); - if (handler != nullptr) { - pst.addSlot(componentId, slotTimeMs, executionStep, handler, this); - return HasReturnvaluesIF::RETURN_OK; - } - -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Component " << std::hex << componentId << " not found, not adding it to pst" - << std::endl; -#endif - return HasReturnvaluesIF::RETURN_FAILED; -} - -uint32_t FixedTimeslotTask::getPeriodMs() const { return pst.getLengthMs(); } - -ReturnValue_t FixedTimeslotTask::checkSequence() const { return pst.checkSequence(); } - -void FixedTimeslotTask::taskFunctionality() { +[[noreturn]] void FixedTimeslotTask::taskFunctionality() { // A local iterator for the Polling Sequence Table is created to find the // start time for the first entry. - auto slotListIter = pst.current; + auto slotListIter = pollingSeqTable.current; - pst.intializeSequenceAfterTaskCreation(); + pollingSeqTable.intializeSequenceAfterTaskCreation(); // The start time for the first entry is read. uint32_t intervalMs = slotListIter->pollingTimeMs; @@ -108,10 +77,10 @@ void FixedTimeslotTask::taskFunctionality() { /* Enter the loop that defines the task behavior. */ for (;;) { // The component for this slot is executed and the next one is chosen. - this->pst.executeAndAdvance(); - if (not pst.slotFollowsImmediately()) { + this->pollingSeqTable.executeAndAdvance(); + if (not pollingSeqTable.slotFollowsImmediately()) { // Get the interval till execution of the next slot. - intervalMs = this->pst.getIntervalToPreviousSlotMs(); + intervalMs = this->pollingSeqTable.getIntervalToPreviousSlotMs(); interval = pdMS_TO_TICKS(intervalMs); #if (tskKERNEL_VERSION_MAJOR == 10 && tskKERNEL_VERSION_MINOR >= 4) || tskKERNEL_VERSION_MAJOR > 10 @@ -132,8 +101,8 @@ void FixedTimeslotTask::taskFunctionality() { } void FixedTimeslotTask::handleMissedDeadline() { - if (deadlineMissedFunc != nullptr) { - this->deadlineMissedFunc(); + if (dlmFunc != nullptr) { + dlmFunc(); } } diff --git a/src/fsfw/osal/freertos/FixedTimeslotTask.h b/src/fsfw/osal/freertos/FixedTimeslotTask.h index 77999d717..53c9a11c1 100644 --- a/src/fsfw/osal/freertos/FixedTimeslotTask.h +++ b/src/fsfw/osal/freertos/FixedTimeslotTask.h @@ -4,11 +4,11 @@ #include "FreeRTOS.h" #include "FreeRTOSTaskIF.h" #include "fsfw/tasks/FixedSlotSequence.h" -#include "fsfw/tasks/FixedTimeslotTaskIF.h" -#include "fsfw/tasks/Typedef.h" +#include "fsfw/tasks/FixedTimeslotTaskBase.h" +#include "fsfw/tasks/definitions.h" #include "task.h" -class FixedTimeslotTask : public FixedTimeslotTaskIF, public FreeRTOSTaskIF { +class FixedTimeslotTask : public FixedTimeslotTaskBase, public FreeRTOSTaskIF { public: /** * Keep in mind that you need to call before vTaskStartScheduler()! @@ -23,7 +23,7 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF, public FreeRTOSTaskIF { * @return Pointer to the newly created task. */ FixedTimeslotTask(TaskName name, TaskPriority setPriority, TaskStackSize setStack, - TaskPeriod overallPeriod, void (*setDeadlineMissedFunc)()); + TaskPeriod overallPeriod, TaskDeadlineMissedFunction dlmFunc); /** * @brief The destructor of the class. @@ -32,26 +32,9 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF, public FreeRTOSTaskIF { * initialization for the PST and the device handlers. This is done by * calling the PST's destructor. */ - virtual ~FixedTimeslotTask(void); + ~FixedTimeslotTask() override; - ReturnValue_t startTask(void); - /** - * This static function can be used as #deadlineMissedFunc. - * It counts missedDeadlines and prints the number of missed deadlines - * every 10th time. - */ - static void missedDeadlineCounter(); - /** - * A helper variable to count missed deadlines. - */ - static uint32_t deadlineMissedCount; - - ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, - int8_t executionStep) override; - - uint32_t getPeriodMs() const override; - - ReturnValue_t checkSequence() const override; + ReturnValue_t startTask() override; ReturnValue_t sleepFor(uint32_t ms) override; @@ -61,17 +44,6 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF, public FreeRTOSTaskIF { bool started; TaskHandle_t handle; - FixedSlotSequence pst; - - /** - * @brief This attribute holds a function pointer that is executed when - * a deadline was missed. - * @details - * Another function may be announced to determine the actions to perform - * when a deadline was missed. Currently, only one function for missing - * any deadline is allowed. If not used, it shall be declared NULL. - */ - void (*deadlineMissedFunc)(void); /** * @brief This is the entry point for a new task. * @details @@ -88,7 +60,7 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF, public FreeRTOSTaskIF { * It links the functionalities provided by FixedSlotSequence with the * OS's System Calls to keep the timing of the periods. */ - void taskFunctionality(void); + [[noreturn]] void taskFunctionality(); void handleMissedDeadline(); }; diff --git a/src/fsfw/osal/freertos/FreeRTOSTaskIF.h b/src/fsfw/osal/freertos/FreeRTOSTaskIF.h index ad5c9f7f3..dbefc6c16 100644 --- a/src/fsfw/osal/freertos/FreeRTOSTaskIF.h +++ b/src/fsfw/osal/freertos/FreeRTOSTaskIF.h @@ -6,11 +6,11 @@ class FreeRTOSTaskIF { public: - virtual ~FreeRTOSTaskIF() {} + virtual ~FreeRTOSTaskIF() = default; virtual TaskHandle_t getTaskHandle() = 0; protected: - bool checkMissedDeadline(const TickType_t xLastWakeTime, const TickType_t interval) { + static bool checkMissedDeadline(const TickType_t xLastWakeTime, const TickType_t interval) { /* Check whether deadline was missed while also taking overflows * into account. Drawing this on paper with a timeline helps to understand * it. */ diff --git a/src/fsfw/osal/freertos/MessageQueue.cpp b/src/fsfw/osal/freertos/MessageQueue.cpp index a8333fe55..d1a7f691c 100644 --- a/src/fsfw/osal/freertos/MessageQueue.cpp +++ b/src/fsfw/osal/freertos/MessageQueue.cpp @@ -4,8 +4,9 @@ #include "fsfw/osal/freertos/QueueMapManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" -MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize) - : maxMessageSize(maxMessageSize) { +MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize, MqArgs* args) + : MessageQueueBase(MessageQueueIF::NO_QUEUE, MessageQueueIF::NO_QUEUE, args), + maxMessageSize(maxMessageSize) { handle = xQueueCreate(messageDepth, maxMessageSize); if (handle == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -15,10 +16,10 @@ MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize) #else sif::printError("MessageQueue::MessageQueue: Creation failed\n"); sif::printError("Specified Message Depth: %d\n", messageDepth); - sif::printError("Specified MAximum Message Size: %d\n", maxMessageSize); + sif::printError("Specified Maximum Message Size: %d\n", maxMessageSize); #endif } - QueueMapManager::instance()->addMessageQueue(handle, &queueId); + QueueMapManager::instance()->addMessageQueue(handle, &id); } MessageQueue::~MessageQueue() { @@ -29,28 +30,6 @@ MessageQueue::~MessageQueue() { void MessageQueue::switchSystemContext(CallContext callContext) { this->callContext = callContext; } -ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message, - bool ignoreFault) { - return sendMessageFrom(sendTo, message, this->getId(), ignoreFault); -} - -ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) { - return sendToDefaultFrom(message, this->getId()); -} - -ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message, - MessageQueueId_t sentFrom, bool ignoreFault) { - return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault); -} - -ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) { - if (this->lastPartner != MessageQueueIF::NO_QUEUE) { - return sendMessageFrom(this->lastPartner, message, this->getId()); - } else { - return NO_REPLY_PARTNER; - } -} - ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message, MessageQueueId_t sentFrom, bool ignoreFault) { return sendMessageFromMessageQueue(sendTo, message, sentFrom, ignoreFault, callContext); @@ -72,27 +51,16 @@ ReturnValue_t MessageQueue::handleSendResult(BaseType_t result, bool ignoreFault return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message, - MessageQueueId_t* receivedFrom) { - ReturnValue_t status = this->receiveMessage(message); - if (status == HasReturnvaluesIF::RETURN_OK) { - *receivedFrom = this->lastPartner; - } - return status; -} - ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { BaseType_t result = xQueueReceive(handle, reinterpret_cast(message->getBuffer()), 0); if (result == pdPASS) { - this->lastPartner = message->getSender(); + this->last = message->getSender(); return HasReturnvaluesIF::RETURN_OK; } else { return MessageQueueIF::EMPTY; } } -MessageQueueId_t MessageQueue::getLastPartner() const { return lastPartner; } - ReturnValue_t MessageQueue::flush(uint32_t* count) { // TODO FreeRTOS does not support flushing partially // Is always successful @@ -100,17 +68,6 @@ ReturnValue_t MessageQueue::flush(uint32_t* count) { return HasReturnvaluesIF::RETURN_OK; } -MessageQueueId_t MessageQueue::getId() const { return queueId; } - -void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) { - defaultDestinationSet = true; - this->defaultDestination = defaultDestination; -} - -MessageQueueId_t MessageQueue::getDefaultDestination() const { return defaultDestination; } - -bool MessageQueue::isDefaultDestinationSet() const { return defaultDestinationSet; } - // static core function to send messages. ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, MessageQueueMessageIF* message, diff --git a/src/fsfw/osal/freertos/MessageQueue.h b/src/fsfw/osal/freertos/MessageQueue.h index 1cb343d13..ee3479aaa 100644 --- a/src/fsfw/osal/freertos/MessageQueue.h +++ b/src/fsfw/osal/freertos/MessageQueue.h @@ -1,12 +1,15 @@ #ifndef FSFW_OSAL_FREERTOS_MESSAGEQUEUE_H_ #define FSFW_OSAL_FREERTOS_MESSAGEQUEUE_H_ +#include + #include "FreeRTOS.h" #include "TaskManagement.h" #include "fsfw/internalerror/InternalErrorReporterIF.h" #include "fsfw/ipc/MessageQueueIF.h" #include "fsfw/ipc/MessageQueueMessage.h" #include "fsfw/ipc/MessageQueueMessageIF.h" +#include "fsfw/ipc/definitions.h" #include "queue.h" /** @@ -32,7 +35,7 @@ * @ingroup osal * @ingroup message_queue */ -class MessageQueue : public MessageQueueIF { +class MessageQueue : public MessageQueueBase { friend class MessageQueueSenderIF; public: @@ -53,7 +56,8 @@ class MessageQueue : public MessageQueueIF { * This should be left default. */ MessageQueue(size_t messageDepth = 3, - size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE); + size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE, + MqArgs* args = nullptr); /** Copying message queues forbidden */ MessageQueue(const MessageQueue&) = delete; @@ -73,40 +77,15 @@ class MessageQueue : public MessageQueueIF { */ void switchSystemContext(CallContext callContext); - /** MessageQueueIF implementation */ - ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message, - bool ignoreFault = false) override; + QueueHandle_t getNativeQueueHandle(); - ReturnValue_t sendToDefault(MessageQueueMessageIF* message) override; - - ReturnValue_t reply(MessageQueueMessageIF* message) override; + // Implement non-generic MessageQueueIF functions not handled by MessageQueueBase virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false) override; - - virtual ReturnValue_t sendToDefaultFrom(MessageQueueMessageIF* message, - MessageQueueId_t sentFrom = NO_QUEUE, - bool ignoreFault = false) override; - - ReturnValue_t receiveMessage(MessageQueueMessageIF* message, - MessageQueueId_t* receivedFrom) override; - ReturnValue_t receiveMessage(MessageQueueMessageIF* message) override; - ReturnValue_t flush(uint32_t* count) override; - MessageQueueId_t getLastPartner() const override; - - MessageQueueId_t getId() const override; - - void setDefaultDestination(MessageQueueId_t defaultDestination) override; - - MessageQueueId_t getDefaultDestination() const override; - - bool isDefaultDestinationSet() const override; - - QueueHandle_t getNativeQueueHandle(); - protected: /** * @brief Implementation to be called from any send Call within @@ -136,12 +115,8 @@ class MessageQueue : public MessageQueueIF { static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault); private: - bool defaultDestinationSet = false; QueueHandle_t handle; - MessageQueueId_t queueId = MessageQueueIF::NO_QUEUE; - MessageQueueId_t defaultDestination = MessageQueueIF::NO_QUEUE; - MessageQueueId_t lastPartner = MessageQueueIF::NO_QUEUE; const size_t maxMessageSize; //! Stores the current system context CallContext callContext = CallContext::TASK; diff --git a/src/fsfw/osal/freertos/PeriodicTask.cpp b/src/fsfw/osal/freertos/PeriodicTask.cpp index 8dfa02908..665be06c7 100644 --- a/src/fsfw/osal/freertos/PeriodicTask.cpp +++ b/src/fsfw/osal/freertos/PeriodicTask.cpp @@ -5,27 +5,28 @@ #include "fsfw/tasks/ExecutableObjectIF.h" PeriodicTask::PeriodicTask(const char* name, TaskPriority setPriority, TaskStackSize setStack, - TaskPeriod setPeriod, TaskDeadlineMissedFunction deadlineMissedFunc) - : started(false), handle(NULL), period(setPeriod), deadlineMissedFunc(deadlineMissedFunc) { + TaskPeriod setPeriod, TaskDeadlineMissedFunction dlmFunc_) + : PeriodicTaskBase(setPeriod, dlmFunc_), started(false), handle(nullptr) { configSTACK_DEPTH_TYPE stackSize = setStack / sizeof(configSTACK_DEPTH_TYPE); BaseType_t status = xTaskCreate(taskEntryPoint, name, stackSize, this, setPriority, &handle); if (status != pdPASS) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::debug << "PeriodicTask Insufficient heap memory remaining. " - "Status: " + sif::debug << "PeriodicTask::PeriodicTask Insufficient heap memory remaining. Status: " << status << std::endl; +#else + sif::printDebug("PeriodicTask::PeriodicTask: Insufficient heap memory remaining. Status: %d\n", + status); #endif } } -PeriodicTask::~PeriodicTask(void) { - // Do not delete objects, we were responsible for ptrs only. -} +// Do not delete objects, we were responsible for ptrs only. +PeriodicTask::~PeriodicTask() = default; void PeriodicTask::taskEntryPoint(void* argument) { // The argument is re-interpreted as PeriodicTask. The Task object is // global, so it is found from any place. - PeriodicTask* originalTask(reinterpret_cast(argument)); + auto* originalTask(reinterpret_cast(argument)); /* Task should not start until explicitly requested, * but in FreeRTOS, tasks start as soon as they are created if the scheduler * is running but not if the scheduler is not running. @@ -36,7 +37,7 @@ void PeriodicTask::taskEntryPoint(void* argument) { * can continue */ if (not originalTask->started) { - vTaskSuspend(NULL); + vTaskSuspend(nullptr); } originalTask->taskFunctionality(); @@ -62,13 +63,11 @@ ReturnValue_t PeriodicTask::sleepFor(uint32_t ms) { return HasReturnvaluesIF::RETURN_OK; } -void PeriodicTask::taskFunctionality() { +[[noreturn]] void PeriodicTask::taskFunctionality() { TickType_t xLastWakeTime; const TickType_t xPeriod = pdMS_TO_TICKS(this->period * 1000.); - for (auto const& object : objectList) { - object->initializeAfterTaskCreation(); - } + initObjsAfterTaskCreation(); /* The xLastWakeTime variable needs to be initialized with the current tick count. Note that this is the only time the variable is written to @@ -77,8 +76,8 @@ void PeriodicTask::taskFunctionality() { xLastWakeTime = xTaskGetTickCount(); /* Enter the loop that defines the task behavior. */ for (;;) { - for (auto const& object : objectList) { - object->performOperation(); + for (auto const& objectPair : objectList) { + objectPair.first->performOperation(objectPair.second); } #if (tskKERNEL_VERSION_MAJOR == 10 && tskKERNEL_VERSION_MINOR >= 4) || tskKERNEL_VERSION_MAJOR > 10 @@ -95,28 +94,10 @@ void PeriodicTask::taskFunctionality() { } } -ReturnValue_t PeriodicTask::addComponent(object_id_t object) { - ExecutableObjectIF* newObject = ObjectManager::instance()->get(object); - if (newObject == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" - "it implement ExecutableObjectIF" - << std::endl; -#endif - return HasReturnvaluesIF::RETURN_FAILED; - } - objectList.push_back(newObject); - newObject->setTaskIF(this); - - return HasReturnvaluesIF::RETURN_OK; -} - -uint32_t PeriodicTask::getPeriodMs() const { return period * 1000; } - TaskHandle_t PeriodicTask::getTaskHandle() { return handle; } void PeriodicTask::handleMissedDeadline() { - if (deadlineMissedFunc != nullptr) { - this->deadlineMissedFunc(); + if (dlmFunc != nullptr) { + dlmFunc(); } } diff --git a/src/fsfw/osal/freertos/PeriodicTask.h b/src/fsfw/osal/freertos/PeriodicTask.h index cc3951768..c3fb9d70d 100644 --- a/src/fsfw/osal/freertos/PeriodicTask.h +++ b/src/fsfw/osal/freertos/PeriodicTask.h @@ -6,8 +6,8 @@ #include "FreeRTOS.h" #include "FreeRTOSTaskIF.h" #include "fsfw/objectmanager/ObjectManagerIF.h" -#include "fsfw/tasks/PeriodicTaskIF.h" -#include "fsfw/tasks/Typedef.h" +#include "fsfw/tasks/PeriodicTaskBase.h" +#include "fsfw/tasks/definitions.h" #include "task.h" class ExecutableObjectIF; @@ -17,7 +17,7 @@ class ExecutableObjectIF; * periodic activities of multiple objects. * @ingroup task_handling */ -class PeriodicTask : public PeriodicTaskIF, public FreeRTOSTaskIF { +class PeriodicTask : public PeriodicTaskBase, public FreeRTOSTaskIF { public: /** * Keep in Mind that you need to call before this vTaskStartScheduler()! @@ -43,7 +43,7 @@ class PeriodicTask : public PeriodicTaskIF, public FreeRTOSTaskIF { * @brief Currently, the executed object's lifetime is not coupled with * the task object's lifetime, so the destructor is empty. */ - virtual ~PeriodicTask(void); + ~PeriodicTask() override; /** * @brief The method to start the task. @@ -53,17 +53,6 @@ class PeriodicTask : public PeriodicTaskIF, public FreeRTOSTaskIF { * to the system call. */ ReturnValue_t startTask() override; - /** - * Adds an object to the list of objects to be executed. - * The objects are executed in the order added. - * @param object Id of the object to add. - * @return - * -@c RETURN_OK on success - * -@c RETURN_FAILED if the object could not be added. - */ - ReturnValue_t addComponent(object_id_t object) override; - - uint32_t getPeriodMs() const override; ReturnValue_t sleepFor(uint32_t ms) override; @@ -73,28 +62,6 @@ class PeriodicTask : public PeriodicTaskIF, public FreeRTOSTaskIF { bool started; TaskHandle_t handle; - //! Typedef for the List of objects. - typedef std::vector ObjectList; - /** - * @brief This attribute holds a list of objects to be executed. - */ - ObjectList objectList; - /** - * @brief The period of the task. - * @details - * The period determines the frequency of the task's execution. - * It is expressed in clock ticks. - */ - TaskPeriod period; - /** - * @brief The pointer to the deadline-missed function. - * @details - * This pointer stores the function that is executed if the task's deadline - * is missed so each may react individually on a timing failure. - * The pointer may be NULL, then nothing happens on missing the deadline. - * The deadline is equal to the next execution of the periodic task. - */ - void (*deadlineMissedFunc)(void); /** * @brief This is the function executed in the new task's context. * @details @@ -115,7 +82,7 @@ class PeriodicTask : public PeriodicTaskIF, public FreeRTOSTaskIF { * the next period. * On missing the deadline, the deadlineMissedFunction is executed. */ - void taskFunctionality(void); + [[noreturn]] void taskFunctionality(); void handleMissedDeadline(); }; diff --git a/src/fsfw/osal/freertos/QueueFactory.cpp b/src/fsfw/osal/freertos/QueueFactory.cpp index f4941481d..8424123cc 100644 --- a/src/fsfw/osal/freertos/QueueFactory.cpp +++ b/src/fsfw/osal/freertos/QueueFactory.cpp @@ -22,8 +22,9 @@ QueueFactory::QueueFactory() {} QueueFactory::~QueueFactory() {} -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize) { - return new MessageQueue(messageDepth, maxMessageSize); +MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, + MqArgs* args) { + return new MessageQueue(messageDepth, maxMessageSize, args); } void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; } diff --git a/src/fsfw/osal/host/CMakeLists.txt b/src/fsfw/osal/host/CMakeLists.txt index 2d29ce5df..95ab25c9d 100644 --- a/src/fsfw/osal/host/CMakeLists.txt +++ b/src/fsfw/osal/host/CMakeLists.txt @@ -1,24 +1,23 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - Clock.cpp - FixedTimeslotTask.cpp - MessageQueue.cpp - Mutex.cpp - MutexFactory.cpp - PeriodicTask.cpp - QueueFactory.cpp - QueueMapManager.cpp - SemaphoreFactory.cpp - TaskFactory.cpp - taskHelpers.cpp -) +target_sources( + ${LIB_FSFW_NAME} + PRIVATE Clock.cpp + FixedTimeslotTask.cpp + MessageQueue.cpp + Mutex.cpp + MutexFactory.cpp + PeriodicTask.cpp + QueueFactory.cpp + QueueMapManager.cpp + SemaphoreFactory.cpp + TaskFactory.cpp + taskHelpers.cpp) if(UNIX) - find_package(Threads REQUIRED) - - target_link_libraries(${LIB_FSFW_NAME} - PRIVATE - rt - ${CMAKE_THREAD_LIBS_INIT} - ) -endif() \ No newline at end of file + find_package(Threads REQUIRED) + + target_link_libraries(${LIB_FSFW_NAME} PRIVATE ${CMAKE_THREAD_LIBS_INIT}) + if(NOT APPLE) + target_link_libraries(${LIB_FSFW_NAME} PRIVATE rt) + endif() + +endif() diff --git a/src/fsfw/osal/host/Clock.cpp b/src/fsfw/osal/host/Clock.cpp index 62f9d9d5f..19e120b39 100644 --- a/src/fsfw/osal/host/Clock.cpp +++ b/src/fsfw/osal/host/Clock.cpp @@ -2,6 +2,7 @@ #include +#include "fsfw/ipc/MutexGuard.h" #include "fsfw/platform.h" #include "fsfw/serviceinterface/ServiceInterface.h" @@ -11,9 +12,6 @@ #include #endif -uint16_t Clock::leapSeconds = 0; -MutexIF* Clock::timeMutex = NULL; - using SystemClock = std::chrono::system_clock; uint32_t Clock::getTicksPerSecond(void) { @@ -127,6 +125,13 @@ ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) { auto seconds = std::chrono::time_point_cast(now); auto fraction = now - seconds; time_t tt = SystemClock::to_time_t(now); + ReturnValue_t result = checkOrCreateClockMutex(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + MutexGuard helper(timeMutex); + // gmtime writes its output in a global buffer which is not Thread Safe + // Therefore we have to use a Mutex here struct tm* timeInfo; timeInfo = gmtime(&tt); time->year = timeInfo->tm_year + 1900; @@ -150,17 +155,14 @@ ReturnValue_t Clock::convertTimeOfDayToTimeval(const TimeOfDay_t* from, timeval* time_tm.tm_hour = from->hour; time_tm.tm_min = from->minute; time_tm.tm_sec = from->second; + time_tm.tm_isdst = 0; - time_t seconds = mktime(&time_tm); + time_t seconds = timegm(&time_tm); to->tv_sec = seconds; to->tv_usec = from->usecond; // Fails in 2038.. return HasReturnvaluesIF::RETURN_OK; -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "Clock::convertTimeBla: not implemented yet" << std::endl; -#endif - return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) { diff --git a/src/fsfw/osal/host/FixedTimeslotTask.cpp b/src/fsfw/osal/host/FixedTimeslotTask.cpp index 078539381..1d10b8d8e 100644 --- a/src/fsfw/osal/host/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/host/FixedTimeslotTask.cpp @@ -3,9 +3,7 @@ #include #include -#include "fsfw/ipc/MutexFactory.h" #include "fsfw/objectmanager/ObjectManager.h" -#include "fsfw/osal/host/FixedTimeslotTask.h" #include "fsfw/osal/host/Mutex.h" #include "fsfw/osal/host/taskHelpers.h" #include "fsfw/platform.h" @@ -22,12 +20,8 @@ FixedTimeslotTask::FixedTimeslotTask(const char* name, TaskPriority setPriority, TaskStackSize setStack, TaskPeriod setPeriod, - void (*setDeadlineMissedFunc)()) - : started(false), - pollingSeqTable(setPeriod * 1000), - taskName(name), - period(setPeriod), - deadlineMissedFunc(setDeadlineMissedFunc) { + TaskDeadlineMissedFunction dlmFunc_) + : FixedTimeslotTaskBase(setPeriod, dlmFunc_), started(false), taskName(name) { // It is propably possible to set task priorities by using the native // task handles for Windows / Linux mainThread = std::thread(&FixedTimeslotTask::taskEntryPoint, this, this); @@ -39,7 +33,7 @@ FixedTimeslotTask::FixedTimeslotTask(const char* name, TaskPriority setPriority, tasks::insertTaskName(mainThread.get_id(), taskName); } -FixedTimeslotTask::~FixedTimeslotTask(void) { +FixedTimeslotTask::~FixedTimeslotTask() { // Do not delete objects, we were responsible for ptrs only. terminateThread = true; if (mainThread.joinable()) { @@ -48,7 +42,7 @@ FixedTimeslotTask::~FixedTimeslotTask(void) { } void FixedTimeslotTask::taskEntryPoint(void* argument) { - FixedTimeslotTask* originalTask(reinterpret_cast(argument)); + auto* originalTask(reinterpret_cast(argument)); if (not originalTask->started) { // we have to suspend/block here until the task is started. @@ -81,7 +75,9 @@ ReturnValue_t FixedTimeslotTask::sleepFor(uint32_t ms) { } void FixedTimeslotTask::taskFunctionality() { - pollingSeqTable.intializeSequenceAfterTaskCreation(); + ReturnValue_t result = pollingSeqTable.intializeSequenceAfterTaskCreation(); + // Ignore returnvalue for now + static_cast(result); // A local iterator for the Polling Sequence Table is created to // find the start time for the first entry. @@ -106,37 +102,15 @@ void FixedTimeslotTask::taskFunctionality() { // we need to wait before executing the current slot // this gives us the time to wait: interval = chron_ms(this->pollingSeqTable.getIntervalToPreviousSlotMs()); - delayForInterval(¤tStartTime, interval); - // TODO deadline missed check + if (not delayForInterval(¤tStartTime, interval)) { + if (dlmFunc != nullptr) { + dlmFunc(); + } + } } } } -ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, uint32_t slotTimeMs, - int8_t executionStep) { - ExecutableObjectIF* executableObject = - ObjectManager::instance()->get(componentId); - if (executableObject != nullptr) { - pollingSeqTable.addSlot(componentId, slotTimeMs, executionStep, executableObject, this); - return HasReturnvaluesIF::RETURN_OK; - } - -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Component " << std::hex << "0x" << componentId - << "not found, " - "not adding it to PST.." - << std::dec << std::endl; -#else - sif::printError("Component 0x%08x not found, not adding it to PST..\n", - static_cast(componentId)); -#endif - return HasReturnvaluesIF::RETURN_FAILED; -} - -ReturnValue_t FixedTimeslotTask::checkSequence() const { return pollingSeqTable.checkSequence(); } - -uint32_t FixedTimeslotTask::getPeriodMs() const { return period * 1000; } - bool FixedTimeslotTask::delayForInterval(chron_ms* previousWakeTimeMs, const chron_ms interval) { bool shouldDelay = false; // Get current wakeup time diff --git a/src/fsfw/osal/host/FixedTimeslotTask.h b/src/fsfw/osal/host/FixedTimeslotTask.h index cdbc6f238..4e77f8fd4 100644 --- a/src/fsfw/osal/host/FixedTimeslotTask.h +++ b/src/fsfw/osal/host/FixedTimeslotTask.h @@ -6,10 +6,10 @@ #include #include -#include "../../objectmanager/ObjectManagerIF.h" -#include "../../tasks/FixedSlotSequence.h" -#include "../../tasks/FixedTimeslotTaskIF.h" -#include "../../tasks/Typedef.h" +#include "fsfw/objectmanager/ObjectManagerIF.h" +#include "fsfw/tasks/FixedSlotSequence.h" +#include "fsfw/tasks/FixedTimeslotTaskBase.h" +#include "fsfw/tasks/definitions.h" class ExecutableObjectIF; @@ -19,7 +19,7 @@ class ExecutableObjectIF; * @details * @ingroup task_handling */ -class FixedTimeslotTask : public FixedTimeslotTaskIF { +class FixedTimeslotTask : public FixedTimeslotTaskBase { public: /** * @brief Standard constructor of the class. @@ -39,7 +39,7 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { * @brief Currently, the executed object's lifetime is not coupled with * the task object's lifetime, so the destructor is empty. */ - virtual ~FixedTimeslotTask(void); + ~FixedTimeslotTask() override; /** * @brief The method to start the task. @@ -48,56 +48,22 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { * The address of the task object is passed as an argument * to the system call. */ - ReturnValue_t startTask(void); + ReturnValue_t startTask() override; - /** - * Add timeslot to the polling sequence table. - * @param componentId - * @param slotTimeMs - * @param executionStep - * @return - */ - ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, int8_t executionStep); - - ReturnValue_t checkSequence() const override; - - uint32_t getPeriodMs() const; - - ReturnValue_t sleepFor(uint32_t ms); + ReturnValue_t sleepFor(uint32_t ms) override; protected: using chron_ms = std::chrono::milliseconds; bool started; - //!< Typedef for the List of objects. - typedef std::vector ObjectList; + std::thread mainThread; std::atomic terminateThread{false}; - //! Polling sequence table which contains the object to execute - //! and information like the timeslots and the passed execution step. - FixedSlotSequence pollingSeqTable; - std::condition_variable initCondition; std::mutex initMutex; std::string taskName; - /** - * @brief The period of the task. - * @details - * The period determines the frequency of the task's execution. - * It is expressed in clock ticks. - */ - TaskPeriod period; - /** - * @brief The pointer to the deadline-missed function. - * @details - * This pointer stores the function that is executed if the task's deadline - * is missed. So, each may react individually on a timing failure. - * The pointer may be NULL, then nothing happens on missing the deadline. - * The deadline is equal to the next execution of the periodic task. - */ - void (*deadlineMissedFunc)(void); /** * @brief This is the function executed in the new task's context. * @details @@ -117,9 +83,9 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { * the checkAndRestartPeriod system call blocks the task until the next * period. On missing the deadline, the deadlineMissedFunction is executed. */ - void taskFunctionality(void); + void taskFunctionality(); - bool delayForInterval(chron_ms* previousWakeTimeMs, const chron_ms interval); + static bool delayForInterval(chron_ms* previousWakeTimeMs, chron_ms interval); }; #endif /* FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ */ diff --git a/src/fsfw/osal/host/MessageQueue.cpp b/src/fsfw/osal/host/MessageQueue.cpp index db66b6715..d0a128505 100644 --- a/src/fsfw/osal/host/MessageQueue.cpp +++ b/src/fsfw/osal/host/MessageQueue.cpp @@ -8,10 +8,12 @@ #include "fsfw/osal/host/QueueMapManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" -MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize) - : messageSize(maxMessageSize), messageDepth(messageDepth) { +MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize, MqArgs* args) + : MessageQueueBase(MessageQueueIF::NO_QUEUE, MessageQueueIF::NO_QUEUE, args), + messageSize(maxMessageSize), + messageDepth(messageDepth) { queueLock = MutexFactory::instance()->createMutex(); - auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId); + auto result = QueueMapManager::instance()->addMessageQueue(this, &id); if (result != HasReturnvaluesIF::RETURN_OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "MessageQueue::MessageQueue: Could not be created" << std::endl; @@ -23,42 +25,11 @@ MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize) MessageQueue::~MessageQueue() { MutexFactory::instance()->deleteMutex(queueLock); } -ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message, - bool ignoreFault) { - return sendMessageFrom(sendTo, message, this->getId(), ignoreFault); -} - -ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) { - return sendToDefaultFrom(message, this->getId()); -} - -ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message, - MessageQueueId_t sentFrom, bool ignoreFault) { - return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault); -} - -ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) { - if (this->lastPartner != MessageQueueIF::NO_QUEUE) { - return sendMessageFrom(this->lastPartner, message, this->getId()); - } else { - return MessageQueueIF::NO_REPLY_PARTNER; - } -} - ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message, MessageQueueId_t sentFrom, bool ignoreFault) { return sendMessageFromMessageQueue(sendTo, message, sentFrom, ignoreFault); } -ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message, - MessageQueueId_t* receivedFrom) { - ReturnValue_t status = this->receiveMessage(message); - if (status == HasReturnvaluesIF::RETURN_OK) { - *receivedFrom = this->lastPartner; - } - return status; -} - ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { if (messageQueue.empty()) { return MessageQueueIF::EMPTY; @@ -68,12 +39,10 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { message->getBuffer()); messageQueue.pop(); // The last partner is the first uint32_t field in the message - this->lastPartner = message->getSender(); + this->last = message->getSender(); return HasReturnvaluesIF::RETURN_OK; } -MessageQueueId_t MessageQueue::getLastPartner() const { return lastPartner; } - ReturnValue_t MessageQueue::flush(uint32_t* count) { *count = messageQueue.size(); // Clears the queue. @@ -81,17 +50,6 @@ ReturnValue_t MessageQueue::flush(uint32_t* count) { return HasReturnvaluesIF::RETURN_OK; } -MessageQueueId_t MessageQueue::getId() const { return mqId; } - -void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) { - defaultDestinationSet = true; - this->defaultDestination = defaultDestination; -} - -MessageQueueId_t MessageQueue::getDefaultDestination() const { return defaultDestination; } - -bool MessageQueue::isDefaultDestinationSet() const { return defaultDestinationSet; } - // static core function to send messages. ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, MessageQueueMessageIF* message, @@ -125,6 +83,13 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, memcpy(targetQueue->messageQueue.back().data(), message->getBuffer(), message->getMaximumMessageSize()); } else { + if (not ignoreFault) { + InternalErrorReporterIF* internalErrorReporter = + ObjectManager::instance()->get(objects::INTERNAL_ERROR_REPORTER); + if (internalErrorReporter != nullptr) { + internalErrorReporter->queueMessageNotSent(); + } + } return MessageQueueIF::FULL; } return HasReturnvaluesIF::RETURN_OK; diff --git a/src/fsfw/osal/host/MessageQueue.h b/src/fsfw/osal/host/MessageQueue.h index 49375bb56..4020c6dcc 100644 --- a/src/fsfw/osal/host/MessageQueue.h +++ b/src/fsfw/osal/host/MessageQueue.h @@ -5,9 +5,11 @@ #include #include "fsfw/internalerror/InternalErrorReporterIF.h" +#include "fsfw/ipc/MessageQueueBase.h" #include "fsfw/ipc/MessageQueueIF.h" #include "fsfw/ipc/MessageQueueMessage.h" #include "fsfw/ipc/MutexIF.h" +#include "fsfw/ipc/definitions.h" #include "fsfw/timemanager/Clock.h" /** @@ -33,7 +35,7 @@ * @ingroup osal * @ingroup message_queue */ -class MessageQueue : public MessageQueueIF { +class MessageQueue : public MessageQueueBase { friend class MessageQueueSenderIF; public: @@ -54,7 +56,8 @@ class MessageQueue : public MessageQueueIF { * This should be left default. */ MessageQueue(size_t messageDepth = 3, - size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE); + size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE, + MqArgs* args = nullptr); /** Copying message queues forbidden */ MessageQueue(const MessageQueue&) = delete; @@ -67,121 +70,12 @@ class MessageQueue : public MessageQueueIF { */ virtual ~MessageQueue(); - /** - * @brief This operation sends a message to the given destination. - * @details It directly uses the sendMessage call of the MessageQueueSender - * parent, but passes its queue id as "sentFrom" parameter. - * @param sendTo This parameter specifies the message queue id of the - * destination message queue. - * @param message A pointer to a previously created message, which is sent. - * @param ignoreFault If set to true, the internal software fault counter - * is not incremented if queue is full. - */ - ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message, - bool ignoreFault = false) override; - /** - * @brief This operation sends a message to the default destination. - * @details As in the sendMessage method, this function uses the - * sendToDefault call of the MessageQueueSender parent class and adds its - * queue id as "sentFrom" information. - * @param message A pointer to a previously created message, which is sent. - */ - ReturnValue_t sendToDefault(MessageQueueMessageIF* message) override; - /** - * @brief This operation sends a message to the last communication partner. - * @details This operation simplifies answering an incoming message by using - * the stored lastPartner information as destination. If there was no - * message received yet (i.e. lastPartner is zero), an error code is returned. - * @param message A pointer to a previously created message, which is sent. - */ - ReturnValue_t reply(MessageQueueMessageIF* message) override; - - /** - * @brief With the sendMessage call, a queue message is sent to a - * receiving queue. - * @details - * This method takes the message provided, adds the sentFrom information and - * passes it on to the destination provided with an operating system call. - * The OS's return value is returned. - * @param sendTo This parameter specifies the message queue id to send - * the message to. - * @param message This is a pointer to a previously created message, - * which is sent. - * @param sentFrom The sentFrom information can be set to inject the - * sender's queue id into the message. This variable is set to zero by - * default. - * @param ignoreFault If set to true, the internal software fault counter - * is not incremented if queue is full. - */ + // Implement non-generic MessageQueueIF functions not handled by MessageQueueBase virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false) override; - - /** - * @brief The sendToDefault method sends a queue message to the default - * destination. - * @details - * In all other aspects, it works identical to the sendMessage method. - * @param message This is a pointer to a previously created message, - * which is sent. - * @param sentFrom The sentFrom information can be set to inject the - * sender's queue id into the message. This variable is set to zero by - * default. - */ - virtual ReturnValue_t sendToDefaultFrom(MessageQueueMessageIF* message, - MessageQueueId_t sentFrom = NO_QUEUE, - bool ignoreFault = false) override; - - /** - * @brief This function reads available messages from the message queue - * and returns the sender. - * @details - * It works identically to the other receiveMessage call, but in addition - * returns the sender's queue id. - * @param message A pointer to a message in which the received data is stored. - * @param receivedFrom A pointer to a queue id in which the sender's id is stored. - */ - ReturnValue_t receiveMessage(MessageQueueMessageIF* message, - MessageQueueId_t* receivedFrom) override; - - /** - * @brief This function reads available messages from the message queue. - * @details - * If data is available it is stored in the passed message pointer. - * The message's original content is overwritten and the sendFrom - * information is stored in the lastPartner attribute. Else, the lastPartner - * information remains untouched, the message's content is cleared and the - * function returns immediately. - * @param message A pointer to a message in which the received data is stored. - */ ReturnValue_t receiveMessage(MessageQueueMessageIF* message) override; - /** - * Deletes all pending messages in the queue. - * @param count The number of flushed messages. - * @return RETURN_OK on success. - */ ReturnValue_t flush(uint32_t* count) override; - /** - * @brief This method returns the message queue id of the last - * communication partner. - */ - MessageQueueId_t getLastPartner() const override; - /** - * @brief This method returns the message queue id of this class's - * message queue. - */ - MessageQueueId_t getId() const override; - - /** - * @brief This method is a simple setter for the default destination. - */ - void setDefaultDestination(MessageQueueId_t defaultDestination) override; - /** - * @brief This method is a simple getter for the default destination. - */ - MessageQueueId_t getDefaultDestination() const override; - - bool isDefaultDestinationSet() const override; ReturnValue_t lockQueue(MutexIF::TimeoutType timeoutType, dur_millis_t lockTimeout); ReturnValue_t unlockQueue(); @@ -211,23 +105,14 @@ class MessageQueue : public MessageQueueIF { MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false); - // static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault); - private: std::queue> messageQueue; - /** - * @brief The class stores the queue id it got assigned. - * If initialization fails, the queue id is set to zero. - */ - MessageQueueId_t mqId = MessageQueueIF::NO_QUEUE; size_t messageSize = 0; size_t messageDepth = 0; MutexIF* queueLock; - bool defaultDestinationSet = false; MessageQueueId_t defaultDestination = MessageQueueIF::NO_QUEUE; - MessageQueueId_t lastPartner = MessageQueueIF::NO_QUEUE; }; #endif /* FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ */ diff --git a/src/fsfw/osal/host/PeriodicTask.cpp b/src/fsfw/osal/host/PeriodicTask.cpp index 1de4aedfc..1f18d3359 100644 --- a/src/fsfw/osal/host/PeriodicTask.cpp +++ b/src/fsfw/osal/host/PeriodicTask.cpp @@ -3,13 +3,10 @@ #include #include -#include "fsfw/ipc/MutexFactory.h" -#include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/osal/host/Mutex.h" #include "fsfw/osal/host/taskHelpers.h" #include "fsfw/platform.h" #include "fsfw/serviceinterface/ServiceInterface.h" -#include "fsfw/tasks/ExecutableObjectIF.h" #if defined(PLATFORM_WIN) #include @@ -20,8 +17,8 @@ #endif PeriodicTask::PeriodicTask(const char* name, TaskPriority setPriority, TaskStackSize setStack, - TaskPeriod setPeriod, void (*setDeadlineMissedFunc)()) - : started(false), taskName(name), period(setPeriod), deadlineMissedFunc(setDeadlineMissedFunc) { + TaskPeriod setPeriod, TaskDeadlineMissedFunction dlmFunc_) + : PeriodicTaskBase(setPeriod, dlmFunc_), started(false), taskName(name) { // It is probably possible to set task priorities by using the native // task handles for Windows / Linux mainThread = std::thread(&PeriodicTask::taskEntryPoint, this, this); @@ -33,7 +30,7 @@ PeriodicTask::PeriodicTask(const char* name, TaskPriority setPriority, TaskStack tasks::insertTaskName(mainThread.get_id(), taskName); } -PeriodicTask::~PeriodicTask(void) { +PeriodicTask::~PeriodicTask() { // Do not delete objects, we were responsible for ptrs only. terminateThread = true; if (mainThread.joinable()) { @@ -42,7 +39,7 @@ PeriodicTask::~PeriodicTask(void) { } void PeriodicTask::taskEntryPoint(void* argument) { - PeriodicTask* originalTask(reinterpret_cast(argument)); + auto* originalTask(reinterpret_cast(argument)); if (not originalTask->started) { // we have to suspend/block here until the task is started. @@ -75,43 +72,27 @@ ReturnValue_t PeriodicTask::sleepFor(uint32_t ms) { } void PeriodicTask::taskFunctionality() { - for (const auto& object : objectList) { - object->initializeAfterTaskCreation(); - } + initObjsAfterTaskCreation(); std::chrono::milliseconds periodChrono(static_cast(period * 1000)); auto currentStartTime{std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch())}; - auto nextStartTime{currentStartTime}; - /* Enter the loop that defines the task behavior. */ for (;;) { if (terminateThread.load()) { break; } - for (const auto& object : objectList) { - object->performOperation(); + for (const auto& objectPair : objectList) { + objectPair.first->performOperation(objectPair.second); } if (not delayForInterval(¤tStartTime, periodChrono)) { - if (deadlineMissedFunc != nullptr) { - this->deadlineMissedFunc(); + if (dlmFunc != nullptr) { + this->dlmFunc(); } } } } -ReturnValue_t PeriodicTask::addComponent(object_id_t object) { - ExecutableObjectIF* newObject = ObjectManager::instance()->get(object); - if (newObject == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - newObject->setTaskIF(this); - objectList.push_back(newObject); - return HasReturnvaluesIF::RETURN_OK; -} - -uint32_t PeriodicTask::getPeriodMs() const { return period * 1000; } - bool PeriodicTask::delayForInterval(chron_ms* previousWakeTimeMs, const chron_ms interval) { bool shouldDelay = false; // Get current wakeup time diff --git a/src/fsfw/osal/host/PeriodicTask.h b/src/fsfw/osal/host/PeriodicTask.h index 0c41c0f70..6fdaae4ee 100644 --- a/src/fsfw/osal/host/PeriodicTask.h +++ b/src/fsfw/osal/host/PeriodicTask.h @@ -6,9 +6,9 @@ #include #include -#include "../../objectmanager/ObjectManagerIF.h" -#include "../../tasks/PeriodicTaskIF.h" -#include "../../tasks/Typedef.h" +#include "fsfw/objectmanager/ObjectManagerIF.h" +#include "fsfw/tasks/PeriodicTaskBase.h" +#include "fsfw/tasks/definitions.h" class ExecutableObjectIF; @@ -19,7 +19,7 @@ class ExecutableObjectIF; * * @ingroup task_handling */ -class PeriodicTask : public PeriodicTaskIF { +class PeriodicTask : public PeriodicTaskBase { public: /** * @brief Standard constructor of the class. @@ -34,12 +34,12 @@ class PeriodicTask : public PeriodicTaskIF { * assigned. */ PeriodicTask(const char* name, TaskPriority setPriority, TaskStackSize setStack, - TaskPeriod setPeriod, void (*setDeadlineMissedFunc)()); + TaskPeriod setPeriod, TaskDeadlineMissedFunction dlmFunc); /** * @brief Currently, the executed object's lifetime is not coupled with * the task object's lifetime, so the destructor is empty. */ - virtual ~PeriodicTask(void); + ~PeriodicTask() override; /** * @brief The method to start the task. @@ -48,53 +48,20 @@ class PeriodicTask : public PeriodicTaskIF { * The address of the task object is passed as an argument * to the system call. */ - ReturnValue_t startTask(void); - /** - * Adds an object to the list of objects to be executed. - * The objects are executed in the order added. - * @param object Id of the object to add. - * @return - * -@c RETURN_OK on success - * -@c RETURN_FAILED if the object could not be added. - */ - ReturnValue_t addComponent(object_id_t object); + ReturnValue_t startTask() override; - uint32_t getPeriodMs() const; - - ReturnValue_t sleepFor(uint32_t ms); + ReturnValue_t sleepFor(uint32_t ms) override; protected: using chron_ms = std::chrono::milliseconds; bool started; - //!< Typedef for the List of objects. - typedef std::vector ObjectList; std::thread mainThread; std::atomic terminateThread{false}; - /** - * @brief This attribute holds a list of objects to be executed. - */ - ObjectList objectList; - std::condition_variable initCondition; std::mutex initMutex; std::string taskName; - /** - * @brief The period of the task. - * @details - * The period determines the frequency of the task's execution. - * It is expressed in clock ticks. - */ - TaskPeriod period; - /** - * @brief The pointer to the deadline-missed function. - * @details - * This pointer stores the function that is executed if the task's deadline - * is missed. So, each may react individually on a timing failure. - * The pointer may be NULL, then nothing happens on missing the deadline. - * The deadline is equal to the next execution of the periodic task. - */ - void (*deadlineMissedFunc)(void); + /** * @brief This is the function executed in the new task's context. * @details @@ -114,9 +81,9 @@ class PeriodicTask : public PeriodicTaskIF { * the checkAndRestartPeriod system call blocks the task until the next * period. On missing the deadline, the deadlineMissedFunction is executed. */ - void taskFunctionality(void); + void taskFunctionality(); - bool delayForInterval(chron_ms* previousWakeTimeMs, const chron_ms interval); + static bool delayForInterval(chron_ms* previousWakeTimeMs, chron_ms interval); }; -#endif /* PERIODICTASK_H_ */ +#endif /* FRAMEWORK_OSAL_HOST_PERIODICTASK_H_ */ diff --git a/src/fsfw/osal/host/QueueFactory.cpp b/src/fsfw/osal/host/QueueFactory.cpp index 3c63e6c9a..732892caf 100644 --- a/src/fsfw/osal/host/QueueFactory.cpp +++ b/src/fsfw/osal/host/QueueFactory.cpp @@ -27,12 +27,13 @@ QueueFactory::QueueFactory() {} QueueFactory::~QueueFactory() {} -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize) { +MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, + MqArgs* args) { // A thread-safe queue can be implemented by using a combination // of std::queue and std::mutex. This uses dynamic memory allocation // which could be alleviated by using a custom allocator, external library // (etl::queue) or simply using std::queue, we're on a host machine anyway. - return new MessageQueue(messageDepth, maxMessageSize); + return new MessageQueue(messageDepth, maxMessageSize, args); } void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; } diff --git a/src/fsfw/osal/host/TaskFactory.cpp b/src/fsfw/osal/host/TaskFactory.cpp index 6e74fd57f..ec4c1554c 100644 --- a/src/fsfw/osal/host/TaskFactory.cpp +++ b/src/fsfw/osal/host/TaskFactory.cpp @@ -14,9 +14,9 @@ TaskFactory* TaskFactory::factoryInstance = new TaskFactory(); // Not used for the host implementation for now because C++ thread abstraction is used const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = 0; -TaskFactory::TaskFactory() {} +TaskFactory::TaskFactory() = default; -TaskFactory::~TaskFactory() {} +TaskFactory::~TaskFactory() = default; TaskFactory* TaskFactory::instance() { return TaskFactory::factoryInstance; } diff --git a/src/fsfw/osal/host/taskHelpers.cpp b/src/fsfw/osal/host/taskHelpers.cpp index aba2948a3..432cf30cc 100644 --- a/src/fsfw/osal/host/taskHelpers.cpp +++ b/src/fsfw/osal/host/taskHelpers.cpp @@ -6,7 +6,7 @@ std::mutex nameMapLock; std::map taskNameMap; -ReturnValue_t tasks::insertTaskName(std::thread::id threadId, std::string taskName) { +ReturnValue_t tasks::insertTaskName(std::thread::id threadId, const std::string& taskName) { std::lock_guard lg(nameMapLock); auto returnPair = taskNameMap.emplace(threadId, taskName); if (not returnPair.second) { diff --git a/src/fsfw/osal/host/taskHelpers.h b/src/fsfw/osal/host/taskHelpers.h index cf553011e..13a71d166 100644 --- a/src/fsfw/osal/host/taskHelpers.h +++ b/src/fsfw/osal/host/taskHelpers.h @@ -7,7 +7,7 @@ namespace tasks { -ReturnValue_t insertTaskName(std::thread::id threadId, std::string taskName); +ReturnValue_t insertTaskName(std::thread::id threadId, const std::string& taskName); std::string getTaskName(std::thread::id threadId); } // namespace tasks diff --git a/src/fsfw/osal/linux/BinarySemaphore.cpp b/src/fsfw/osal/linux/BinarySemaphore.cpp index ffe0e3a88..4fb67f15d 100644 --- a/src/fsfw/osal/linux/BinarySemaphore.cpp +++ b/src/fsfw/osal/linux/BinarySemaphore.cpp @@ -1,6 +1,7 @@ #include "fsfw/osal/linux/BinarySemaphore.h" #include +#include #include #include diff --git a/src/fsfw/osal/linux/CMakeLists.txt b/src/fsfw/osal/linux/CMakeLists.txt index dcdade67f..72a62b86f 100644 --- a/src/fsfw/osal/linux/CMakeLists.txt +++ b/src/fsfw/osal/linux/CMakeLists.txt @@ -1,29 +1,25 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - Clock.cpp - BinarySemaphore.cpp - CountingSemaphore.cpp - FixedTimeslotTask.cpp - InternalErrorCodes.cpp - MessageQueue.cpp - Mutex.cpp - MutexFactory.cpp - PeriodicPosixTask.cpp - PosixThread.cpp - QueueFactory.cpp - SemaphoreFactory.cpp - TaskFactory.cpp - tcpipHelpers.cpp - unixUtility.cpp -) +target_sources( + ${LIB_FSFW_NAME} + PRIVATE Clock.cpp + BinarySemaphore.cpp + CountingSemaphore.cpp + FixedTimeslotTask.cpp + InternalErrorCodes.cpp + MessageQueue.cpp + Mutex.cpp + MutexFactory.cpp + PeriodicPosixTask.cpp + PosixThread.cpp + QueueFactory.cpp + SemaphoreFactory.cpp + TaskFactory.cpp + tcpipHelpers.cpp + unixUtility.cpp) find_package(Threads REQUIRED) -target_link_libraries(${LIB_FSFW_NAME} PRIVATE - ${CMAKE_THREAD_LIBS_INIT} - rt -) - -target_link_libraries(${LIB_FSFW_NAME} INTERFACE - ${CMAKE_THREAD_LIBS_INIT} -) +target_link_libraries(${LIB_FSFW_NAME} PUBLIC ${CMAKE_THREAD_LIBS_INIT}) +if(NOT APPLE) + target_link_libraries(${LIB_FSFW_NAME} PUBLIC rt) +endif() diff --git a/src/fsfw/osal/linux/Clock.cpp b/src/fsfw/osal/linux/Clock.cpp index 7092e6b2b..534e7e22c 100644 --- a/src/fsfw/osal/linux/Clock.cpp +++ b/src/fsfw/osal/linux/Clock.cpp @@ -8,11 +8,9 @@ #include +#include "fsfw/ipc/MutexGuard.h" #include "fsfw/serviceinterface/ServiceInterface.h" -uint16_t Clock::leapSeconds = 0; -MutexIF* Clock::timeMutex = NULL; - uint32_t Clock::getTicksPerSecond(void) { uint32_t ticks = sysconf(_SC_CLK_TCK); return ticks; @@ -117,7 +115,13 @@ ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) { // TODO errno return HasReturnvaluesIF::RETURN_FAILED; } - + ReturnValue_t result = checkOrCreateClockMutex(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + MutexGuard helper(timeMutex); + // gmtime writes its output in a global buffer which is not Thread Safe + // Therefore we have to use a Mutex here struct tm* timeInfo; timeInfo = gmtime(&timeUnix.tv_sec); time->year = timeInfo->tm_year + 1900; @@ -140,8 +144,9 @@ ReturnValue_t Clock::convertTimeOfDayToTimeval(const TimeOfDay_t* from, timeval* fromTm.tm_hour = from->hour; fromTm.tm_min = from->minute; fromTm.tm_sec = from->second; + fromTm.tm_isdst = 0; - to->tv_sec = mktime(&fromTm); + to->tv_sec = timegm(&fromTm); to->tv_usec = from->usecond; return HasReturnvaluesIF::RETURN_OK; } diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.cpp b/src/fsfw/osal/linux/FixedTimeslotTask.cpp index 1f4d6e23c..156413eac 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/linux/FixedTimeslotTask.cpp @@ -1,22 +1,20 @@ #include "fsfw/osal/linux/FixedTimeslotTask.h" -#include +#include -#include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" -uint32_t FixedTimeslotTask::deadlineMissedCount = 0; const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = PTHREAD_STACK_MIN; -FixedTimeslotTask::FixedTimeslotTask(const char* name_, int priority_, size_t stackSize_, - uint32_t periodMs_) - : PosixThread(name_, priority_, stackSize_), pst(periodMs_), started(false) {} - -FixedTimeslotTask::~FixedTimeslotTask() {} +FixedTimeslotTask::FixedTimeslotTask(const char* name_, TaskPriority priority_, size_t stackSize_, + TaskPeriod periodSeconds_, TaskDeadlineMissedFunction dlmFunc_) + : FixedTimeslotTaskBase(periodSeconds_, dlmFunc_), + posixThread(name_, priority_, stackSize_), + started(false) {} void* FixedTimeslotTask::taskEntryPoint(void* arg) { // The argument is re-interpreted as PollingTask. - FixedTimeslotTask* originalTask(reinterpret_cast(arg)); + auto* originalTask(reinterpret_cast(arg)); // The task's functionality is called. originalTask->taskFunctionality(); return nullptr; @@ -24,7 +22,7 @@ void* FixedTimeslotTask::taskEntryPoint(void* arg) { ReturnValue_t FixedTimeslotTask::startTask() { started = true; - createTask(&taskEntryPoint, this); + posixThread.createTask(&taskEntryPoint, this); return HasReturnvaluesIF::RETURN_OK; } @@ -32,63 +30,36 @@ ReturnValue_t FixedTimeslotTask::sleepFor(uint32_t ms) { return PosixThread::sleep((uint64_t)ms * 1000000); } -uint32_t FixedTimeslotTask::getPeriodMs() const { return pst.getLengthMs(); } - -ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, uint32_t slotTimeMs, - int8_t executionStep) { - ExecutableObjectIF* executableObject = - ObjectManager::instance()->get(componentId); - if (executableObject != nullptr) { - pst.addSlot(componentId, slotTimeMs, executionStep, executableObject, this); - return HasReturnvaluesIF::RETURN_OK; - } - -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Component " << std::hex << componentId << " not found, not adding it to pst" - << std::dec << std::endl; -#endif - return HasReturnvaluesIF::RETURN_FAILED; -} - -ReturnValue_t FixedTimeslotTask::checkSequence() const { return pst.checkSequence(); } - -void FixedTimeslotTask::taskFunctionality() { +[[noreturn]] void FixedTimeslotTask::taskFunctionality() { // Like FreeRTOS pthreads are running as soon as they are created if (!started) { - suspend(); + posixThread.suspend(); } - pst.intializeSequenceAfterTaskCreation(); + // Returnvalue ignored for now + static_cast(pollingSeqTable.intializeSequenceAfterTaskCreation()); // The start time for the first entry is read. - uint64_t lastWakeTime = getCurrentMonotonicTimeMs(); - uint64_t interval = pst.getIntervalToNextSlotMs(); + uint64_t lastWakeTime = PosixThread::getCurrentMonotonicTimeMs(); + uint32_t interval = 0; // The task's "infinite" inner loop is entered. - while (1) { - if (pst.slotFollowsImmediately()) { + while (true) { + if (pollingSeqTable.slotFollowsImmediately()) { // Do nothing } else { // The interval for the next polling slot is selected. - interval = this->pst.getIntervalToPreviousSlotMs(); + interval = pollingSeqTable.getIntervalToPreviousSlotMs(); // The period is checked and restarted with the new interval. // If the deadline was missed, the deadlineMissedFunc is called. if (!PosixThread::delayUntil(&lastWakeTime, interval)) { // No time left on timer -> we missed the deadline - missedDeadlineCounter(); + if (dlmFunc != nullptr) { + dlmFunc(); + } } } // The device handler for this slot is executed and the next one is chosen. - this->pst.executeAndAdvance(); - } -} - -void FixedTimeslotTask::missedDeadlineCounter() { - FixedTimeslotTask::deadlineMissedCount++; - if (FixedTimeslotTask::deadlineMissedCount % 10 == 0) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines." - << std::endl; -#endif + pollingSeqTable.executeAndAdvance(); } } diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.h b/src/fsfw/osal/linux/FixedTimeslotTask.h index 76b92db39..1f5766a2e 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.h +++ b/src/fsfw/osal/linux/FixedTimeslotTask.h @@ -3,11 +3,12 @@ #include -#include "../../tasks/FixedSlotSequence.h" -#include "../../tasks/FixedTimeslotTaskIF.h" #include "PosixThread.h" +#include "fsfw/tasks/FixedSlotSequence.h" +#include "fsfw/tasks/FixedTimeslotTaskBase.h" +#include "fsfw/tasks/definitions.h" -class FixedTimeslotTask : public FixedTimeslotTaskIF, public PosixThread { +class FixedTimeslotTask : public FixedTimeslotTaskBase { public: /** * Create a generic periodic task. @@ -21,29 +22,13 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF, public PosixThread { * @param period_ * @param deadlineMissedFunc_ */ - FixedTimeslotTask(const char* name_, int priority_, size_t stackSize_, uint32_t periodMs_); - virtual ~FixedTimeslotTask(); + FixedTimeslotTask(const char* name_, TaskPriority priority_, size_t stackSize_, + TaskPeriod periodSeconds_, TaskDeadlineMissedFunction dlmFunc_); + ~FixedTimeslotTask() override = default; - virtual ReturnValue_t startTask(); + ReturnValue_t startTask() override; - virtual ReturnValue_t sleepFor(uint32_t ms); - - virtual uint32_t getPeriodMs() const; - - virtual ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, int8_t executionStep); - - virtual ReturnValue_t checkSequence() const; - - /** - * This static function can be used as #deadlineMissedFunc. - * It counts missedDeadlines and prints the number of missed deadlines every 10th time. - */ - static void missedDeadlineCounter(); - - /** - * A helper variable to count missed deadlines. - */ - static uint32_t deadlineMissedCount; + ReturnValue_t sleepFor(uint32_t ms) override; protected: /** @@ -53,9 +38,12 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF, public PosixThread { * It links the functionalities provided by FixedSlotSequence with the * OS's System Calls to keep the timing of the periods. */ - virtual void taskFunctionality(); + [[noreturn]] virtual void taskFunctionality(); private: + PosixThread posixThread; + bool started; + /** * @brief This is the entry point in a new thread. * @@ -68,9 +56,6 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF, public PosixThread { * arbitrary data. */ static void* taskEntryPoint(void* arg); - FixedSlotSequence pst; - - bool started; }; #endif /* FSFW_OSAL_LINUX_FIXEDTIMESLOTTASK_H_ */ diff --git a/src/fsfw/osal/linux/MessageQueue.cpp b/src/fsfw/osal/linux/MessageQueue.cpp index f876ec6e9..ec2121651 100644 --- a/src/fsfw/osal/linux/MessageQueue.cpp +++ b/src/fsfw/osal/linux/MessageQueue.cpp @@ -11,13 +11,10 @@ #include "fsfw/osal/linux/unixUtility.h" #include "fsfw/serviceinterface/ServiceInterface.h" -MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize) - : id(MessageQueueIF::NO_QUEUE), - lastPartner(MessageQueueIF::NO_QUEUE), - defaultDestination(MessageQueueIF::NO_QUEUE), +MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize, MqArgs* args) + : MessageQueueBase(MessageQueueIF::NO_QUEUE, MessageQueueIF::NO_QUEUE, args), maxMessageSize(maxMessageSize) { mq_attr attributes; - this->id = 0; // Set attributes attributes.mq_curmsgs = 0; attributes.mq_maxmsg = messageDepth; @@ -50,30 +47,6 @@ MessageQueue::~MessageQueue() { } } -ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message, - bool ignoreFault) { - return sendMessageFrom(sendTo, message, this->getId(), false); -} - -ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) { - return sendToDefaultFrom(message, this->getId()); -} - -ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) { - if (this->lastPartner != 0) { - return sendMessageFrom(this->lastPartner, message, this->getId()); - } else { - return NO_REPLY_PARTNER; - } -} - -ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message, - MessageQueueId_t* receivedFrom) { - ReturnValue_t status = this->receiveMessage(message); - *receivedFrom = this->lastPartner; - return status; -} - ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { if (message == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -96,7 +69,7 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { int status = mq_receive(id, reinterpret_cast(message->getBuffer()), message->getMaximumMessageSize(), &messagePriority); if (status > 0) { - this->lastPartner = message->getSender(); + this->last = message->getSender(); // Check size of incoming message. if (message->getMessageSize() < message->getMinimumMessageSize()) { return HasReturnvaluesIF::RETURN_FAILED; @@ -164,8 +137,6 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { } } -MessageQueueId_t MessageQueue::getLastPartner() const { return this->lastPartner; } - ReturnValue_t MessageQueue::flush(uint32_t* count) { mq_attr attrib; int status = mq_getattr(id, &attrib); @@ -212,26 +183,11 @@ ReturnValue_t MessageQueue::flush(uint32_t* count) { return HasReturnvaluesIF::RETURN_OK; } -MessageQueueId_t MessageQueue::getId() const { return this->id; } - -void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) { - this->defaultDestination = defaultDestination; -} - -ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message, - MessageQueueId_t sentFrom, bool ignoreFault) { - return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault); -} - ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message, MessageQueueId_t sentFrom, bool ignoreFault) { return sendMessageFromMessageQueue(sendTo, message, sentFrom, ignoreFault); } -MessageQueueId_t MessageQueue::getDefaultDestination() const { return this->defaultDestination; } - -bool MessageQueue::isDefaultDestinationSet() const { return (defaultDestination != NO_QUEUE); } - uint16_t MessageQueue::queueCounter = 0; ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, @@ -240,9 +196,9 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, bool ignoreFault) { if (message == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is nullptr!" << std::endl; + sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is nullptr" << std::endl; #else - sif::printError("MessageQueue::sendMessageFromMessageQueue: Message is nullptr!\n"); + sif::printError("MessageQueue::sendMessageFromMessageQueue: Message is nullptr\n"); #endif return HasReturnvaluesIF::RETURN_FAILED; } @@ -256,7 +212,7 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, if (!ignoreFault) { InternalErrorReporterIF* internalErrorReporter = ObjectManager::instance()->get(objects::INTERNAL_ERROR_REPORTER); - if (internalErrorReporter != NULL) { + if (internalErrorReporter != nullptr) { internalErrorReporter->queueMessageNotSent(); } } @@ -334,9 +290,9 @@ ReturnValue_t MessageQueue::handleOpenError(mq_attr* attributes, uint32_t messag */ #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "MessageQueue::MessageQueue: Default MQ size " << defaultMqMaxMsg - << " is too small for requested size " << messageDepth << std::endl; + << " is too small for requested message depth " << messageDepth << std::endl; sif::error << "This error can be fixed by setting the maximum " - "allowed message size higher!" + "allowed message depth higher" << std::endl; #else sif::printError( diff --git a/src/fsfw/osal/linux/MessageQueue.h b/src/fsfw/osal/linux/MessageQueue.h index dbf6555e8..108ec797d 100644 --- a/src/fsfw/osal/linux/MessageQueue.h +++ b/src/fsfw/osal/linux/MessageQueue.h @@ -1,11 +1,13 @@ #ifndef FSFW_OSAL_LINUX_MESSAGEQUEUE_H_ #define FSFW_OSAL_LINUX_MESSAGEQUEUE_H_ +#include #include #include "fsfw/internalerror/InternalErrorReporterIF.h" #include "fsfw/ipc/MessageQueueIF.h" #include "fsfw/ipc/MessageQueueMessage.h" +#include "fsfw/ipc/definitions.h" /** * @brief This class manages sending and receiving of message queue messages. * @@ -25,7 +27,7 @@ * makes use of the operating system calls provided. * @ingroup message_queue */ -class MessageQueue : public MessageQueueIF { +class MessageQueue : public MessageQueueBase { friend class MessageQueueSenderIF; public: @@ -42,104 +44,24 @@ class MessageQueue : public MessageQueueIF { * This should be left default. */ MessageQueue(uint32_t messageDepth = 3, - size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE); + size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE, + MqArgs* args = nullptr); + + /** Copying message queues forbidden */ + MessageQueue(const MessageQueue&) = delete; + MessageQueue& operator=(const MessageQueue&) = delete; + /** * @brief The destructor deletes the formerly created message queue. * @details This is accomplished by using the delete call provided by the operating system. */ virtual ~MessageQueue(); - /** - * @brief This operation sends a message to the given destination. - * @details It directly uses the sendMessage call of the MessageQueueSender parent, but passes - * its queue id as "sentFrom" parameter. - * @param sendTo This parameter specifies the message queue id of the destination message - * queue. - * @param message A pointer to a previously created message, which is sent. - * @param ignoreFault If set to true, the internal software fault counter is not incremented if - * queue is full. - */ - virtual ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message, - bool ignoreFault = false); - /** - * @brief This operation sends a message to the default destination. - * @details As in the sendMessage method, this function uses the sendToDefault call of the - * MessageQueueSender parent class and adds its queue id as "sentFrom" - * information. - * @param message A pointer to a previously created message, which is sent. - */ - virtual ReturnValue_t sendToDefault(MessageQueueMessageIF* message); - /** - * @brief This operation sends a message to the last communication partner. - * @details This operation simplifies answering an incoming message by using the stored - * lastParnter information as destination. If there was no message received yet - * (i.e. lastPartner is zero), an error code is returned. - * @param message A pointer to a previously created message, which is sent. - */ - ReturnValue_t reply(MessageQueueMessageIF* message); - /** - * @brief This function reads available messages from the message queue and returns the - * sender. - * @details It works identically to the other receiveMessage call, but in addition returns the - * sender's queue id. - * @param message A pointer to a message in which the received data is stored. - * @param receivedFrom A pointer to a queue id in which the sender's id is stored. - */ - ReturnValue_t receiveMessage(MessageQueueMessageIF* message, MessageQueueId_t* receivedFrom); - - /** - * @brief This function reads available messages from the message queue. - * @details If data is available it is stored in the passed message pointer. The message's - * original content is overwritten and the sendFrom information is stored in - * the lastPartner attribute. Else, the lastPartner information remains untouched, the message's - * content is cleared and the function returns immediately. - * @param message A pointer to a message in which the received data is stored. - */ - ReturnValue_t receiveMessage(MessageQueueMessageIF* message); - /** - * Deletes all pending messages in the queue. - * @param count The number of flushed messages. - * @return RETURN_OK on success. - */ - ReturnValue_t flush(uint32_t* count); - /** - * @brief This method returns the message queue id of the last communication partner. - */ - MessageQueueId_t getLastPartner() const; - /** - * @brief This method returns the message queue id of this class's message queue. - */ - MessageQueueId_t getId() const; - /** - * \brief With the sendMessage call, a queue message is sent to a receiving queue. - * \param sendTo This parameter specifies the message queue id to send the message to. - * \param message This is a pointer to a previously created message, which is sent. - * \param sentFrom The sentFrom information can be set to inject the sender's queue id into the - * message. This variable is set to zero by default. \param ignoreFault If set to true, the - * internal software fault counter is not incremented if queue is full. - */ - virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message, - MessageQueueId_t sentFrom, bool ignoreFault = false); - /** - * \brief The sendToDefault method sends a queue message to the default destination. - * \details In all other aspects, it works identical to the sendMessage method. - * \param message This is a pointer to a previously created message, which is sent. - * \param sentFrom The sentFrom information can be set to inject the sender's queue id into the - * message. This variable is set to zero by default. - */ - virtual ReturnValue_t sendToDefaultFrom(MessageQueueMessageIF* message, - MessageQueueId_t sentFrom = NO_QUEUE, - bool ignoreFault = false); - /** - * \brief This method is a simple setter for the default destination. - */ - void setDefaultDestination(MessageQueueId_t defaultDestination); - /** - * \brief This method is a simple getter for the default destination. - */ - MessageQueueId_t getDefaultDestination() const; - - bool isDefaultDestinationSet() const; + // Implement non-generic MessageQueueIF functions not handled by MessageQueueBase + ReturnValue_t receiveMessage(MessageQueueMessageIF* message) override; + ReturnValue_t flush(uint32_t* count) override; + ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message, + MessageQueueId_t sentFrom, bool ignoreFault = false) override; protected: /** @@ -158,31 +80,10 @@ class MessageQueue : public MessageQueueIF { bool ignoreFault = false); private: - /** - * @brief The class stores the queue id it got assigned from the operating system in this - * attribute. If initialization fails, the queue id is set to zero. - */ - MessageQueueId_t id; - /** - * @brief In this attribute, the queue id of the last communication partner is stored - * to allow for replying. - */ - MessageQueueId_t lastPartner; - /** - * @brief The message queue's name -a user specific information for the operating system- is - * generated automatically with the help of this static counter. - */ - /** - * \brief This attribute stores a default destination to send messages to. - * \details It is stored to simplify sending to always-the-same receiver. The attribute may - * be set in the constructor or by a setter call to setDefaultDestination. - */ - MessageQueueId_t defaultDestination; - /** * The name of the message queue, stored for unlinking */ - char name[16]; + char name[16] = {}; static uint16_t queueCounter; const size_t maxMessageSize; diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.cpp b/src/fsfw/osal/linux/PeriodicPosixTask.cpp index ca346670d..09b106eda 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.cpp +++ b/src/fsfw/osal/linux/PeriodicPosixTask.cpp @@ -1,82 +1,54 @@ -#include "fsfw/osal/linux/PeriodicPosixTask.h" +#include "PeriodicPosixTask.h" -#include - -#include "fsfw/objectmanager/ObjectManager.h" -#include "fsfw/serviceinterface/ServiceInterface.h" +#include "fsfw/serviceinterface.h" #include "fsfw/tasks/ExecutableObjectIF.h" PeriodicPosixTask::PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, - uint32_t period_, void(deadlineMissedFunc_)()) - : PosixThread(name_, priority_, stackSize_), - objectList(), - started(false), - periodMs(period_), - deadlineMissedFunc(deadlineMissedFunc_) {} - -PeriodicPosixTask::~PeriodicPosixTask() { - // Not Implemented -} + TaskPeriod period_, TaskDeadlineMissedFunction dlmFunc_) + : PeriodicTaskBase(period_, dlmFunc_), + posixThread(name_, priority_, stackSize_), + started(false) {} void* PeriodicPosixTask::taskEntryPoint(void* arg) { // The argument is re-interpreted as PollingTask. - PeriodicPosixTask* originalTask(reinterpret_cast(arg)); + auto* originalTask(reinterpret_cast(arg)); // The task's functionality is called. originalTask->taskFunctionality(); - return NULL; -} - -ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object) { - ExecutableObjectIF* newObject = ObjectManager::instance()->get(object); - if (newObject == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" - << " it implements ExecutableObjectIF!" << std::endl; -#else - sif::printError( - "PeriodicTask::addComponent: Invalid object. Make sure it " - "implements ExecutableObjectIF!\n"); -#endif - return HasReturnvaluesIF::RETURN_FAILED; - } - objectList.push_back(newObject); - newObject->setTaskIF(this); - - return HasReturnvaluesIF::RETURN_OK; + return nullptr; } ReturnValue_t PeriodicPosixTask::sleepFor(uint32_t ms) { - return PosixThread::sleep((uint64_t)ms * 1000000); + return PosixThread::sleep(static_cast(ms) * 1000000); } -ReturnValue_t PeriodicPosixTask::startTask(void) { +ReturnValue_t PeriodicPosixTask::startTask() { + if (isEmpty()) { + return HasReturnvaluesIF::RETURN_FAILED; + } started = true; - PosixThread::createTask(&taskEntryPoint, this); + posixThread.createTask(&taskEntryPoint, this); return HasReturnvaluesIF::RETURN_OK; } -void PeriodicPosixTask::taskFunctionality(void) { +[[noreturn]] void PeriodicPosixTask::taskFunctionality() { if (not started) { - suspend(); + posixThread.suspend(); } - for (auto const& object : objectList) { - object->initializeAfterTaskCreation(); - } + initObjsAfterTaskCreation(); - uint64_t lastWakeTime = getCurrentMonotonicTimeMs(); + uint64_t lastWakeTime = PosixThread::getCurrentMonotonicTimeMs(); + uint64_t periodMs = getPeriodMs(); // The task's "infinite" inner loop is entered. - while (1) { - for (auto const& object : objectList) { - object->performOperation(); + while (true) { + for (auto const& objOpCodePair : objectList) { + objOpCodePair.first->performOperation(objOpCodePair.second); } if (not PosixThread::delayUntil(&lastWakeTime, periodMs)) { - if (this->deadlineMissedFunc != nullptr) { - this->deadlineMissedFunc(); + if (dlmFunc != nullptr) { + dlmFunc(); } } } } - -uint32_t PeriodicPosixTask::getPeriodMs() const { return periodMs; } diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.h b/src/fsfw/osal/linux/PeriodicPosixTask.h index 3c9a3a0d7..085c10b97 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.h +++ b/src/fsfw/osal/linux/PeriodicPosixTask.h @@ -3,12 +3,13 @@ #include -#include "../../objectmanager/ObjectManagerIF.h" -#include "../../tasks/ExecutableObjectIF.h" -#include "../../tasks/PeriodicTaskIF.h" #include "PosixThread.h" +#include "fsfw/objectmanager/ObjectManagerIF.h" +#include "fsfw/tasks/ExecutableObjectIF.h" +#include "fsfw/tasks/PeriodicTaskBase.h" +#include "fsfw/tasks/PeriodicTaskIF.h" -class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { +class PeriodicPosixTask : public PeriodicTaskBase { public: /** * Create a generic periodic task. @@ -22,9 +23,9 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { * @param period_ * @param deadlineMissedFunc_ */ - PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, uint32_t period_, - void (*deadlineMissedFunc_)()); - virtual ~PeriodicPosixTask(); + PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, TaskPeriod period_, + TaskDeadlineMissedFunction dlmFunc_); + ~PeriodicPosixTask() override = default; /** * @brief The method to start the task. @@ -34,42 +35,26 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { * to the system call. */ ReturnValue_t startTask() override; - /** - * Adds an object to the list of objects to be executed. - * The objects are executed in the order added. - * @param object Id of the object to add. - * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. - */ - ReturnValue_t addComponent(object_id_t object) override; - - uint32_t getPeriodMs() const override; ReturnValue_t sleepFor(uint32_t ms) override; private: - typedef std::vector ObjectList; //!< Typedef for the List of objects. - /** - * @brief This attribute holds a list of objects to be executed. - */ - ObjectList objectList; + PosixThread posixThread; /** * @brief Flag to indicate that the task was started and is allowed to run */ bool started; - /** - * @brief Period of the task in milliseconds - */ - uint32_t periodMs; /** * @brief The function containing the actual functionality of the task. * @details The method sets and starts - * the task's period, then enters a loop that is repeated indefinitely. Within the - * loop, all performOperation methods of the added objects are called. Afterwards the task will be - * blocked until the next period. On missing the deadline, the deadlineMissedFunction is executed. + * the task's period, then enters a loop that is repeated indefinitely. Within + * the loop, all performOperation methods of the added objects are called. Afterwards the task + * will be blocked until the next period. On missing the deadline, the deadlineMissedFunction is + * executed. */ - virtual void taskFunctionality(void); + [[noreturn]] virtual void taskFunctionality(); /** * @brief This is the entry point in a new thread. * @@ -77,14 +62,6 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { * of the child class. Needs a valid pointer to the derived class. */ static void* taskEntryPoint(void* arg); - /** - * @brief The pointer to the deadline-missed function. - * @details This pointer stores the function that is executed if the task's deadline is missed. - * So, each may react individually on a timing failure. The pointer may be - * NULL, then nothing happens on missing the deadline. The deadline is equal to the next execution - * of the periodic task. - */ - void (*deadlineMissedFunc)(); }; #endif /* FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ */ diff --git a/src/fsfw/osal/linux/PosixThread.h b/src/fsfw/osal/linux/PosixThread.h index 69c6c5b7e..78fdfa2b5 100644 --- a/src/fsfw/osal/linux/PosixThread.h +++ b/src/fsfw/osal/linux/PosixThread.h @@ -35,6 +35,21 @@ class PosixThread { */ void resume(); + /** + * @brief Function that has to be called by derived class because the + * derived class pointer has to be valid as argument. + * @details + * This function creates a pthread with the given parameters. As the + * function requires a pointer to the derived object it has to be called + * after the this pointer of the derived object is valid. + * Sets the taskEntryPoint as function to be called by new a thread. + * @param fnc_ Function which will be executed by the thread. + * @param arg_ + * argument of the taskEntryPoint function, needs to be this pointer + * of derived class + */ + void createTask(void* (*fnc_)(void*), void* arg_); + /** * Delay function similar to FreeRtos delayUntil function * @@ -55,21 +70,6 @@ class PosixThread { protected: pthread_t thread; - /** - * @brief Function that has to be called by derived class because the - * derived class pointer has to be valid as argument. - * @details - * This function creates a pthread with the given parameters. As the - * function requires a pointer to the derived object it has to be called - * after the this pointer of the derived object is valid. - * Sets the taskEntryPoint as function to be called by new a thread. - * @param fnc_ Function which will be executed by the thread. - * @param arg_ - * argument of the taskEntryPoint function, needs to be this pointer - * of derived class - */ - void createTask(void* (*fnc_)(void*), void* arg_); - private: char name[PTHREAD_MAX_NAMELEN]; int priority; diff --git a/src/fsfw/osal/linux/QueueFactory.cpp b/src/fsfw/osal/linux/QueueFactory.cpp index d1b1cfdb7..24ace1ae3 100644 --- a/src/fsfw/osal/linux/QueueFactory.cpp +++ b/src/fsfw/osal/linux/QueueFactory.cpp @@ -28,8 +28,9 @@ QueueFactory::QueueFactory() {} QueueFactory::~QueueFactory() {} -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize) { - return new MessageQueue(messageDepth, maxMessageSize); +MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, + MqArgs* args) { + return new MessageQueue(messageDepth, maxMessageSize, args); } void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; } diff --git a/src/fsfw/osal/linux/TaskFactory.cpp b/src/fsfw/osal/linux/TaskFactory.cpp index 8503039f8..a28e685da 100644 --- a/src/fsfw/osal/linux/TaskFactory.cpp +++ b/src/fsfw/osal/linux/TaskFactory.cpp @@ -8,21 +8,22 @@ // TODO: Different variant than the lazy loading in QueueFactory. What's better and why? TaskFactory* TaskFactory::factoryInstance = new TaskFactory(); -TaskFactory::~TaskFactory() {} +TaskFactory::~TaskFactory() = default; TaskFactory* TaskFactory::instance() { return TaskFactory::factoryInstance; } PeriodicTaskIF* TaskFactory::createPeriodicTask( TaskName name_, TaskPriority taskPriority_, TaskStackSize stackSize_, TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_) { - return new PeriodicPosixTask(name_, taskPriority_, stackSize_, periodInSeconds_ * 1000, + return new PeriodicPosixTask(name_, taskPriority_, stackSize_, periodInSeconds_, deadLineMissedFunction_); } FixedTimeslotTaskIF* TaskFactory::createFixedTimeslotTask( TaskName name_, TaskPriority taskPriority_, TaskStackSize stackSize_, TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_) { - return new FixedTimeslotTask(name_, taskPriority_, stackSize_, periodInSeconds_ * 1000); + return new FixedTimeslotTask(name_, taskPriority_, stackSize_, periodInSeconds_, + deadLineMissedFunction_); } ReturnValue_t TaskFactory::deleteTask(PeriodicTaskIF* task) { diff --git a/src/fsfw/osal/rtems/CMakeLists.txt b/src/fsfw/osal/rtems/CMakeLists.txt index 734566a37..1b47e1b9d 100644 --- a/src/fsfw/osal/rtems/CMakeLists.txt +++ b/src/fsfw/osal/rtems/CMakeLists.txt @@ -1,20 +1,17 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - Clock.cpp - CpuUsage.cpp - InitTask.cpp - InternalErrorCodes.cpp - MessageQueue.cpp - PeriodicTask.cpp - Mutex.cpp - MutexFactory.cpp - FixedTimeslotTask.cpp - QueueFactory.cpp - RtemsBasic.cpp - RTEMSTaskBase.cpp - TaskFactory.cpp - BinarySemaphore.cpp - SemaphoreFactory.cpp -) - - +target_sources( + ${LIB_FSFW_NAME} + PRIVATE Clock.cpp + CpuUsage.cpp + InitTask.cpp + InternalErrorCodes.cpp + MessageQueue.cpp + PeriodicTask.cpp + Mutex.cpp + MutexFactory.cpp + FixedTimeslotTask.cpp + QueueFactory.cpp + RtemsBasic.cpp + RTEMSTaskBase.cpp + TaskFactory.cpp + BinarySemaphore.cpp + SemaphoreFactory.cpp) diff --git a/src/fsfw/osal/rtems/Clock.cpp b/src/fsfw/osal/rtems/Clock.cpp index 06b0c1d8d..831c67d43 100644 --- a/src/fsfw/osal/rtems/Clock.cpp +++ b/src/fsfw/osal/rtems/Clock.cpp @@ -6,9 +6,6 @@ #include "fsfw/ipc/MutexGuard.h" #include "fsfw/osal/rtems/RtemsBasic.h" -uint16_t Clock::leapSeconds = 0; -MutexIF* Clock::timeMutex = nullptr; - uint32_t Clock::getTicksPerSecond(void) { rtems_interval ticks_per_second = rtems_clock_get_ticks_per_second(); return static_cast(ticks_per_second); diff --git a/src/fsfw/osal/rtems/FixedTimeslotTask.cpp b/src/fsfw/osal/rtems/FixedTimeslotTask.cpp index 3347739af..f400c2135 100644 --- a/src/fsfw/osal/rtems/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/rtems/FixedTimeslotTask.cpp @@ -1,42 +1,32 @@ #include "fsfw/osal/rtems/FixedTimeslotTask.h" -#include #include -#include #include #include #include -#include -#include "fsfw/objectmanager/ObjectManager.h" -#include "fsfw/objectmanager/SystemObjectIF.h" #include "fsfw/osal/rtems/RtemsBasic.h" #include "fsfw/returnvalues/HasReturnvaluesIF.h" #include "fsfw/serviceinterface/ServiceInterface.h" -#include "fsfw/tasks/FixedSequenceSlot.h" #if FSFW_CPP_OSTREAM_ENABLED == 1 #include #endif #include -#include - -uint32_t FixedTimeslotTask::deadlineMissedCount = 0; FixedTimeslotTask::FixedTimeslotTask(const char *name, rtems_task_priority setPriority, - size_t setStack, uint32_t setOverallPeriod, - void (*setDeadlineMissedFunc)(void)) - : RTEMSTaskBase(setPriority, setStack, name), periodId(0), pst(setOverallPeriod) { - // All additional attributes are applied to the object. - this->deadlineMissedFunc = setDeadlineMissedFunc; -} + size_t setStack, TaskPeriod setOverallPeriod, + TaskDeadlineMissedFunction dlmFunc_) + : FixedTimeslotTaskBase(setOverallPeriod, dlmFunc_), + RTEMSTaskBase(setPriority, setStack, name), + periodId(0) {} -FixedTimeslotTask::~FixedTimeslotTask() {} +FixedTimeslotTask::~FixedTimeslotTask() = default; rtems_task FixedTimeslotTask::taskEntryPoint(rtems_task_argument argument) { /* The argument is re-interpreted as a FixedTimeslotTask */ - FixedTimeslotTask *originalTask(reinterpret_cast(argument)); + auto *originalTask(reinterpret_cast(argument)); /* The task's functionality is called. */ return originalTask->taskFunctionality(); /* Should never be reached */ @@ -46,16 +36,6 @@ rtems_task FixedTimeslotTask::taskEntryPoint(rtems_task_argument argument) { #endif } -void FixedTimeslotTask::missedDeadlineCounter() { - FixedTimeslotTask::deadlineMissedCount++; - if (FixedTimeslotTask::deadlineMissedCount % 10 == 0) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines." - << std::endl; -#endif - } -} - ReturnValue_t FixedTimeslotTask::startTask() { rtems_status_code status = rtems_task_start(id, FixedTimeslotTask::taskEntryPoint, rtems_task_argument((void *)this)); @@ -79,54 +59,35 @@ ReturnValue_t FixedTimeslotTask::startTask() { } } -ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, uint32_t slotTimeMs, - int8_t executionStep) { - ExecutableObjectIF *object = ObjectManager::instance()->get(componentId); - if (object != nullptr) { - pst.addSlot(componentId, slotTimeMs, executionStep, object, this); - return HasReturnvaluesIF::RETURN_OK; - } - -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Component " << std::hex << componentId << " not found, not adding it to pst" - << std::endl; -#endif - return HasReturnvaluesIF::RETURN_FAILED; -} - -uint32_t FixedTimeslotTask::getPeriodMs() const { return pst.getLengthMs(); } - -ReturnValue_t FixedTimeslotTask::checkSequence() const { return pst.checkSequence(); } - -void FixedTimeslotTask::taskFunctionality() { +[[noreturn]] void FixedTimeslotTask::taskFunctionality() { /* A local iterator for the Polling Sequence Table is created to find the start time for the first entry. */ - FixedSlotSequence::SlotListIter it = pst.current; + auto it = pollingSeqTable.current; /* Initialize the PST with the correct calling task */ - pst.intializeSequenceAfterTaskCreation(); + pollingSeqTable.intializeSequenceAfterTaskCreation(); /* The start time for the first entry is read. */ rtems_interval interval = RtemsBasic::convertMsToTicks(it->pollingTimeMs); RTEMSTaskBase::setAndStartPeriod(interval, &periodId); // The task's "infinite" inner loop is entered. - while (1) { - if (pst.slotFollowsImmediately()) { + while (true) { + if (pollingSeqTable.slotFollowsImmediately()) { /* Do nothing */ } else { /* The interval for the next polling slot is selected. */ - interval = RtemsBasic::convertMsToTicks(this->pst.getIntervalToNextSlotMs()); + interval = RtemsBasic::convertMsToTicks(pollingSeqTable.getIntervalToNextSlotMs()); /* The period is checked and restarted with the new interval. If the deadline was missed, the deadlineMissedFunc is called. */ rtems_status_code status = RTEMSTaskBase::restartPeriod(interval, periodId); if (status == RTEMS_TIMEOUT) { - if (this->deadlineMissedFunc != nullptr) { - this->deadlineMissedFunc(); + if (dlmFunc != nullptr) { + dlmFunc(); } } } /* The device handler for this slot is executed and the next one is chosen. */ - this->pst.executeAndAdvance(); + this->pollingSeqTable.executeAndAdvance(); } } diff --git a/src/fsfw/osal/rtems/FixedTimeslotTask.h b/src/fsfw/osal/rtems/FixedTimeslotTask.h index 6dedfa441..781c93dbe 100644 --- a/src/fsfw/osal/rtems/FixedTimeslotTask.h +++ b/src/fsfw/osal/rtems/FixedTimeslotTask.h @@ -1,11 +1,11 @@ #ifndef FSFW_OSAL_RTEMS_FIXEDTIMESLOTTASK_H_ #define FSFW_OSAL_RTEMS_FIXEDTIMESLOTTASK_H_ -#include "../../tasks/FixedSlotSequence.h" -#include "../../tasks/FixedTimeslotTaskIF.h" #include "RTEMSTaskBase.h" +#include "fsfw/tasks/FixedSlotSequence.h" +#include "fsfw/tasks/FixedTimeslotTaskBase.h" -class FixedTimeslotTask : public RTEMSTaskBase, public FixedTimeslotTaskIF { +class FixedTimeslotTask : public FixedTimeslotTaskBase, public RTEMSTaskBase { public: /** * @brief The standard constructor of the class. @@ -17,7 +17,7 @@ class FixedTimeslotTask : public RTEMSTaskBase, public FixedTimeslotTaskIF { * @param getPst The object id of the completely initialized polling sequence. */ FixedTimeslotTask(const char *name, rtems_task_priority setPriority, size_t setStackSize, - uint32_t overallPeriod, void (*setDeadlineMissedFunc)()); + TaskPeriod overallPeriod, TaskDeadlineMissedFunction dlmFunc); /** * @brief The destructor of the class. @@ -25,44 +25,17 @@ class FixedTimeslotTask : public RTEMSTaskBase, public FixedTimeslotTaskIF { * The destructor frees all heap memory that was allocated on thread initialization * for the PST andthe device handlers. This is done by calling the PST's destructor. */ - virtual ~FixedTimeslotTask(void); + ~FixedTimeslotTask() override; ReturnValue_t startTask(void); - /** - * This static function can be used as #deadlineMissedFunc. - * It counts missedDeadlines and prints the number of missed deadlines every 10th time. - */ - static void missedDeadlineCounter(); - /** - * A helper variable to count missed deadlines. - */ - static uint32_t deadlineMissedCount; - ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, int8_t executionStep); - - uint32_t getPeriodMs() const; - - ReturnValue_t checkSequence() const; - - ReturnValue_t sleepFor(uint32_t ms); + ReturnValue_t sleepFor(uint32_t ms) override; protected: /** * @brief id of the associated OS period */ rtems_id periodId; - - FixedSlotSequence pst; - - /** - * @brief This attribute holds a function pointer that is executed when a deadline was missed. - * - * @details - * Another function may be announced to determine the actions to perform when a deadline - * was missed. Currently, only one function for missing any deadline is allowed. - * If not used, it shall be declared NULL. - */ - void (*deadlineMissedFunc)(void) = nullptr; /** * @brief This is the entry point in a new polling thread. * @details This method is the entry point in the new thread @@ -76,7 +49,7 @@ class FixedTimeslotTask : public RTEMSTaskBase, public FixedTimeslotTaskIF { * It links the functionalities provided by FixedSlotSequence with the OS's system calls to * keep the timing of the periods. */ - void taskFunctionality(void); + [[noreturn]] void taskFunctionality(); }; #endif /* FSFW_OSAL_RTEMS_FIXEDTIMESLOTTASK_H_ */ diff --git a/src/fsfw/osal/rtems/MessageQueue.cpp b/src/fsfw/osal/rtems/MessageQueue.cpp index e45679d52..534015dc9 100644 --- a/src/fsfw/osal/rtems/MessageQueue.cpp +++ b/src/fsfw/osal/rtems/MessageQueue.cpp @@ -6,8 +6,9 @@ #include "fsfw/osal/rtems/RtemsBasic.h" #include "fsfw/serviceinterface/ServiceInterface.h" -MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size) - : id(0), lastPartner(0), defaultDestination(NO_QUEUE), internalErrorReporter(nullptr) { +MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size, MqArgs* args) + : MessageQueueBase(MessageQueueIF::NO_QUEUE, MessageQueueIF::NO_QUEUE, args), + internalErrorReporter(nullptr) { rtems_name name = ('Q' << 24) + (queueCounter++ << 8); rtems_status_code status = rtems_message_queue_create(name, message_depth, max_message_size, 0, &(this->id)); @@ -16,43 +17,19 @@ MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size) sif::error << "MessageQueue::MessageQueue: Creating Queue " << std::hex << name << std::dec << " failed with status:" << (uint32_t)status << std::endl; #endif - this->id = 0; + this->id = MessageQueueIF::NO_QUEUE; } } MessageQueue::~MessageQueue() { rtems_message_queue_delete(id); } -ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message, - bool ignoreFault) { - return sendMessageFrom(sendTo, message, this->getId(), ignoreFault); -} - -ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) { - return sendToDefaultFrom(message, this->getId()); -} - -ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) { - if (this->lastPartner != 0) { - return sendMessage(this->lastPartner, message, this->getId()); - } else { - return NO_REPLY_PARTNER; - } -} - -ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message, - MessageQueueId_t* receivedFrom) { - ReturnValue_t status = this->receiveMessage(message); - *receivedFrom = this->lastPartner; - return status; -} - ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { size_t size = 0; rtems_status_code status = rtems_message_queue_receive(id, message->getBuffer(), &size, RTEMS_NO_WAIT, 1); if (status == RTEMS_SUCCESSFUL) { message->setMessageSize(size); - this->lastPartner = message->getSender(); + this->last = message->getSender(); // Check size of incoming message. if (message->getMessageSize() < message->getMinimumMessageSize()) { return HasReturnvaluesIF::RETURN_FAILED; @@ -65,19 +42,11 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { return convertReturnCode(status); } -MessageQueueId_t MessageQueue::getLastPartner() const { return this->lastPartner; } - ReturnValue_t MessageQueue::flush(uint32_t* count) { rtems_status_code status = rtems_message_queue_flush(id, count); return convertReturnCode(status); } -MessageQueueId_t MessageQueue::getId() const { return this->id; } - -void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) { - this->defaultDestination = defaultDestination; -} - ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message, MessageQueueId_t sentFrom, bool ignoreFault) { message->setSender(sentFrom); @@ -96,22 +65,13 @@ ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, MessageQueu } ReturnValue_t returnCode = convertReturnCode(result); - if (result == MessageQueueIF::EMPTY) { + if (returnCode == MessageQueueIF::EMPTY) { return HasReturnvaluesIF::RETURN_FAILED; } return returnCode; } -ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message, - MessageQueueId_t sentFrom, bool ignoreFault) { - return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault); -} - -MessageQueueId_t MessageQueue::getDefaultDestination() const { return this->defaultDestination; } - -bool MessageQueue::isDefaultDestinationSet() const { return (defaultDestination != NO_QUEUE); } - ReturnValue_t MessageQueue::convertReturnCode(rtems_status_code inValue) { switch (inValue) { case RTEMS_SUCCESSFUL: diff --git a/src/fsfw/osal/rtems/MessageQueue.h b/src/fsfw/osal/rtems/MessageQueue.h index 89aae2adb..6f1ce5682 100644 --- a/src/fsfw/osal/rtems/MessageQueue.h +++ b/src/fsfw/osal/rtems/MessageQueue.h @@ -1,10 +1,13 @@ #ifndef FSFW_OSAL_RTEMS_MESSAGEQUEUE_H_ #define FSFW_OSAL_RTEMS_MESSAGEQUEUE_H_ +#include + #include "RtemsBasic.h" #include "fsfw/internalerror/InternalErrorReporterIF.h" #include "fsfw/ipc/MessageQueueIF.h" #include "fsfw/ipc/MessageQueueMessage.h" +#include "fsfw/ipc/definitions.h" /** * @brief This class manages sending and receiving of message queue messages. @@ -19,7 +22,7 @@ *as well as sending and receiving messages, the class makes use of the operating system calls *provided. \ingroup message_queue */ -class MessageQueue : public MessageQueueIF { +class MessageQueue : public MessageQueueBase { public: /** * @brief The constructor initializes and configures the message queue. @@ -33,135 +36,31 @@ class MessageQueue : public MessageQueueIF { * @param max_message_size With this parameter, the maximum message size can be adjusted. * This should be left default. */ - MessageQueue(size_t message_depth = 3, - size_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE); + explicit MessageQueue(size_t message_depth = 3, + size_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE, + MqArgs* args = nullptr); + + /** Copying message queues forbidden */ + MessageQueue(const MessageQueue&) = delete; + MessageQueue& operator=(const MessageQueue&) = delete; + /** * @brief The destructor deletes the formerly created message queue. * @details This is accomplished by using the delete call provided by the operating system. */ - virtual ~MessageQueue(); - /** - * @brief This operation sends a message to the given destination. - * @details It directly uses the sendMessage call of the MessageQueueSender parent, but passes - * its queue id as "sentFrom" parameter. - * @param sendTo This parameter specifies the message queue id of the destination message - * queue. - * @param message A pointer to a previously created message, which is sent. - * @param ignoreFault If set to true, the internal software fault counter is not incremented if - * queue is full. - */ - ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message, - bool ignoreFault = false); - /** - * @brief This operation sends a message to the default destination. - * @details As in the sendMessage method, this function uses the sendToDefault call of the - * MessageQueueSender parent class and adds its queue id as "sentFrom" - * information. - * @param message A pointer to a previously created message, which is sent. - */ - ReturnValue_t sendToDefault(MessageQueueMessageIF* message); - /** - * @brief This operation sends a message to the last communication partner. - * @details This operation simplifies answering an incoming message by using the stored - * lastParnter information as destination. If there was no message received yet - * (i.e. lastPartner is zero), an error code is returned. - * @param message A pointer to a previously created message, which is sent. - */ - ReturnValue_t reply(MessageQueueMessageIF* message); + ~MessageQueue() override; - /** - * @brief This function reads available messages from the message queue and returns the - * sender. - * @details It works identically to the other receiveMessage call, but in addition returns the - * sender's queue id. - * @param message A pointer to a message in which the received data is stored. - * @param receivedFrom A pointer to a queue id in which the sender's id is stored. - */ - ReturnValue_t receiveMessage(MessageQueueMessageIF* message, MessageQueueId_t* receivedFrom); + // Implement non-generic MessageQueueIF functions not handled by MessageQueueBase + ReturnValue_t flush(uint32_t* count) override; - /** - * @brief This function reads available messages from the message queue. - * @details If data is available it is stored in the passed message pointer. The message's - * original content is overwritten and the sendFrom information is stored in - * the lastPartner attribute. Else, the lastPartner information remains untouched, the message's - * content is cleared and the function returns immediately. - * @param message A pointer to a message in which the received data is stored. - */ - ReturnValue_t receiveMessage(MessageQueueMessageIF* message); - /** - * Deletes all pending messages in the queue. - * @param count The number of flushed messages. - * @return RETURN_OK on success. - */ - ReturnValue_t flush(uint32_t* count); - /** - * @brief This method returns the message queue id of the last communication partner. - */ - MessageQueueId_t getLastPartner() const; - /** - * @brief This method returns the message queue id of this class's message queue. - */ - MessageQueueId_t getId() const; - /** - * \brief With the sendMessage call, a queue message is sent to a receiving queue. - * \details This method takes the message provided, adds the sentFrom information and passes - * it on to the destination provided with an operating system call. The OS's - * return value is returned. - * \param sendTo This parameter specifies the message queue id to send the message to. - * \param message This is a pointer to a previously created message, which is sent. - * \param sentFrom The sentFrom information can be set to inject the sender's queue id into the - * message. This variable is set to zero by default. \param ignoreFault If set to true, the - * internal software fault counter is not incremented if queue is full. - */ - virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message, - MessageQueueId_t sentFrom = NO_QUEUE, - bool ignoreFault = false); - /** - * \brief The sendToDefault method sends a queue message to the default destination. - * \details In all other aspects, it works identical to the sendMessage method. - * \param message This is a pointer to a previously created message, which is sent. - * \param sentFrom The sentFrom information can be set to inject the sender's queue id into the - * message. This variable is set to zero by default. - */ - virtual ReturnValue_t sendToDefaultFrom(MessageQueueMessageIF* message, - MessageQueueId_t sentFrom = NO_QUEUE, - bool ignoreFault = false); - /** - * \brief This method is a simple setter for the default destination. - */ - void setDefaultDestination(MessageQueueId_t defaultDestination); - /** - * \brief This method is a simple getter for the default destination. - */ - MessageQueueId_t getDefaultDestination() const; - - bool isDefaultDestinationSet() const; + ReturnValue_t receiveMessage(MessageQueueMessageIF* message) override; + ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message, + MessageQueueId_t sentFrom, bool ignoreFault) override; private: /** - * @brief The class stores the queue id it got assigned from the operating system in this - * attribute. If initialization fails, the queue id is set to zero. - */ - MessageQueueId_t id; - /** - * @brief In this attribute, the queue id of the last communication partner is stored - * to allow for replying. - */ - MessageQueueId_t lastPartner; - /** - * @brief The message queue's name -a user specific information for the operating system- is - * generated automatically with the help of this static counter. - */ - /** - * \brief This attribute stores a default destination to send messages to. - * \details It is stored to simplify sending to always-the-same receiver. The attribute may - * be set in the constructor or by a setter call to setDefaultDestination. - */ - MessageQueueId_t defaultDestination; - - /** - * \brief This attribute stores a reference to the internal error reporter for reporting full - * queues. \details In the event of a full destination queue, the reporter will be notified. The + * @brief This attribute stores a reference to the internal error reporter for reporting full + * queues. @details In the event of a full destination queue, the reporter will be notified. The * reference is set by lazy loading */ InternalErrorReporterIF* internalErrorReporter; diff --git a/src/fsfw/osal/rtems/PeriodicTask.cpp b/src/fsfw/osal/rtems/PeriodicTask.cpp index 1785c8cfb..cccf937f5 100644 --- a/src/fsfw/osal/rtems/PeriodicTask.cpp +++ b/src/fsfw/osal/rtems/PeriodicTask.cpp @@ -5,12 +5,12 @@ #include "fsfw/tasks/ExecutableObjectIF.h" PeriodicTask::PeriodicTask(const char* name, rtems_task_priority setPriority, size_t setStack, - rtems_interval setPeriod, void (*setDeadlineMissedFunc)()) - : RTEMSTaskBase(setPriority, setStack, name), - periodTicks(RtemsBasic::convertMsToTicks(setPeriod)), - deadlineMissedFunc(setDeadlineMissedFunc) {} + TaskPeriod setPeriod, TaskDeadlineMissedFunction dlmFunc_) + : PeriodicTaskBase(setPeriod, dlmFunc_), + RTEMSTaskBase(setPriority, setStack, name), + periodTicks(RtemsBasic::convertMsToTicks(static_cast(setPeriod * 1000.0))) {} -PeriodicTask::~PeriodicTask(void) { +PeriodicTask::~PeriodicTask() { /* Do not delete objects, we were responsible for pointers only. */ rtems_rate_monotonic_delete(periodId); } @@ -18,7 +18,7 @@ PeriodicTask::~PeriodicTask(void) { rtems_task PeriodicTask::taskEntryPoint(rtems_task_argument argument) { /* The argument is re-interpreted as MultiObjectTask. The Task object is global, so it is found from any place. */ - PeriodicTask* originalTask(reinterpret_cast(argument)); + auto* originalTask(reinterpret_cast(argument)); return originalTask->taskFunctionality(); ; } @@ -28,8 +28,10 @@ ReturnValue_t PeriodicTask::startTask() { rtems_task_start(id, PeriodicTask::taskEntryPoint, rtems_task_argument((void*)this)); if (status != RTEMS_SUCCESSFUL) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "ObjectTask::startTask for " << std::hex << this->getId() << std::dec - << " failed." << std::endl; + sif::error << "PeriodicTask::startTask for " << std::hex << this->getId() << std::dec + << " failed" << std::endl; +#else + sif::printError("PeriodicTask::startTask for 0x%08x failed\n", getId()); #endif } switch (status) { @@ -47,34 +49,20 @@ ReturnValue_t PeriodicTask::startTask() { ReturnValue_t PeriodicTask::sleepFor(uint32_t ms) { return RTEMSTaskBase::sleepFor(ms); } -void PeriodicTask::taskFunctionality() { +[[noreturn]] void PeriodicTask::taskFunctionality() { RTEMSTaskBase::setAndStartPeriod(periodTicks, &periodId); - for (const auto& object : objectList) { - object->initializeAfterTaskCreation(); - } + initObjsAfterTaskCreation(); + /* The task's "infinite" inner loop is entered. */ - while (1) { - for (const auto& object : objectList) { - object->performOperation(); + while (true) { + for (const auto& objectPair : objectList) { + objectPair.first->performOperation(objectPair.second); } rtems_status_code status = RTEMSTaskBase::restartPeriod(periodTicks, periodId); if (status == RTEMS_TIMEOUT) { - if (this->deadlineMissedFunc != nullptr) { - this->deadlineMissedFunc(); + if (dlmFunc != nullptr) { + dlmFunc(); } } } } - -ReturnValue_t PeriodicTask::addComponent(object_id_t object) { - ExecutableObjectIF* newObject = ObjectManager::instance()->get(object); - if (newObject == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - objectList.push_back(newObject); - newObject->setTaskIF(this); - - return HasReturnvaluesIF::RETURN_OK; -} - -uint32_t PeriodicTask::getPeriodMs() const { return RtemsBasic::convertTicksToMs(periodTicks); } diff --git a/src/fsfw/osal/rtems/PeriodicTask.h b/src/fsfw/osal/rtems/PeriodicTask.h index ff8617fca..d987a82e0 100644 --- a/src/fsfw/osal/rtems/PeriodicTask.h +++ b/src/fsfw/osal/rtems/PeriodicTask.h @@ -3,9 +3,10 @@ #include -#include "../../objectmanager/ObjectManagerIF.h" -#include "../../tasks/PeriodicTaskIF.h" #include "RTEMSTaskBase.h" +#include "fsfw/objectmanager/ObjectManagerIF.h" +#include "fsfw/tasks/PeriodicTaskBase.h" +#include "fsfw/tasks/PeriodicTaskIF.h" class ExecutableObjectIF; @@ -13,12 +14,12 @@ class ExecutableObjectIF; * @brief This class represents a specialized task for periodic activities of multiple objects. * * @details MultiObjectTask is an extension to ObjectTask in the way that it is able to execute - * multiple objects that implement the ExecutableObjectIF interface. The objects - * must be added prior to starting the task. + * multiple objects that implement the ExecutableObjectIF interface. The + * objects must be added prior to starting the task. * @author baetz * @ingroup task_handling */ -class PeriodicTask : public RTEMSTaskBase, public PeriodicTaskIF { +class PeriodicTask : public PeriodicTaskBase, public RTEMSTaskBase { public: /** * @brief Standard constructor of the class. @@ -36,12 +37,12 @@ class PeriodicTask : public RTEMSTaskBase, public PeriodicTaskIF { * that shall be assigned. */ PeriodicTask(const char *name, rtems_task_priority setPriority, size_t setStack, - rtems_interval setPeriod, void (*setDeadlineMissedFunc)()); + TaskPeriod setPeriod, TaskDeadlineMissedFunction dlmFunc); /** * @brief Currently, the executed object's lifetime is not coupled with the task object's * lifetime, so the destructor is empty. */ - virtual ~PeriodicTask(void); + ~PeriodicTask() override; /** * @brief The method to start the task. @@ -50,25 +51,11 @@ class PeriodicTask : public RTEMSTaskBase, public PeriodicTaskIF { * The address of the task object is passed as an argument * to the system call. */ - ReturnValue_t startTask(void); - /** - * Adds an object to the list of objects to be executed. - * The objects are executed in the order added. - * @param object Id of the object to add. - * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. - */ - ReturnValue_t addComponent(object_id_t object) override; - - uint32_t getPeriodMs() const override; + ReturnValue_t startTask() override; ReturnValue_t sleepFor(uint32_t ms) override; protected: - typedef std::vector ObjectList; //!< Typedef for the List of objects. - /** - * @brief This attribute holds a list of objects to be executed. - */ - ObjectList objectList; /** * @brief The period of the task. * @details The period determines the frequency of the task's execution. It is expressed in @@ -79,14 +66,7 @@ class PeriodicTask : public RTEMSTaskBase, public PeriodicTaskIF { * @brief id of the associated OS period */ rtems_id periodId = 0; - /** - * @brief The pointer to the deadline-missed function. - * @details This pointer stores the function that is executed if the task's deadline is missed. - * So, each may react individually on a timing failure. The pointer may be - * nullptr, then nothing happens on missing the deadline. The deadline is equal to the next - * execution of the periodic task. - */ - void (*deadlineMissedFunc)(void); + /** * @brief This is the function executed in the new task's context. * @details It converts the argument back to the thread object type and copies the class @@ -102,7 +82,7 @@ class PeriodicTask : public RTEMSTaskBase, public PeriodicTaskIF { * are called. Afterwards the checkAndRestartPeriod system call blocks the task until the next * period. On missing the deadline, the deadlineMissedFunction is executed. */ - void taskFunctionality(void); + [[noreturn]] void taskFunctionality(); }; #endif /* FSFW_OSAL_RTEMS_PERIODICTASK_H_ */ diff --git a/src/fsfw/osal/rtems/QueueFactory.cpp b/src/fsfw/osal/rtems/QueueFactory.cpp index 3d517f609..074ce273e 100644 --- a/src/fsfw/osal/rtems/QueueFactory.cpp +++ b/src/fsfw/osal/rtems/QueueFactory.cpp @@ -45,12 +45,13 @@ QueueFactory* QueueFactory::instance() { return factoryInstance; } -QueueFactory::QueueFactory() {} +QueueFactory::QueueFactory() = default; -QueueFactory::~QueueFactory() {} +QueueFactory::~QueueFactory() = default; -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize) { - return new MessageQueue(messageDepth, maxMessageSize); +MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, + MqArgs* args) { + return new MessageQueue(messageDepth, maxMessageSize, args); } void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; } diff --git a/src/fsfw/osal/rtems/RTEMSTaskBase.cpp b/src/fsfw/osal/rtems/RTEMSTaskBase.cpp index a01b78029..a306b9e21 100644 --- a/src/fsfw/osal/rtems/RTEMSTaskBase.cpp +++ b/src/fsfw/osal/rtems/RTEMSTaskBase.cpp @@ -32,7 +32,7 @@ RTEMSTaskBase::RTEMSTaskBase(rtems_task_priority set_priority, size_t stack_size RTEMSTaskBase::~RTEMSTaskBase() { rtems_task_delete(id); } -rtems_id RTEMSTaskBase::getId() { return this->id; } +rtems_id RTEMSTaskBase::getId() const { return this->id; } ReturnValue_t RTEMSTaskBase::sleepFor(uint32_t ms) { rtems_status_code status = rtems_task_wake_after(RtemsBasic::convertMsToTicks(ms)); diff --git a/src/fsfw/osal/rtems/RTEMSTaskBase.h b/src/fsfw/osal/rtems/RTEMSTaskBase.h index 9ae9e7552..ed9972d3b 100644 --- a/src/fsfw/osal/rtems/RTEMSTaskBase.h +++ b/src/fsfw/osal/rtems/RTEMSTaskBase.h @@ -36,9 +36,9 @@ class RTEMSTaskBase { /** * @brief This method returns the task id of this class. */ - rtems_id getId(); + rtems_id getId() const; - ReturnValue_t sleepFor(uint32_t ms); + static ReturnValue_t sleepFor(uint32_t ms); static ReturnValue_t setAndStartPeriod(rtems_interval period, rtems_id *periodId); static rtems_status_code restartPeriod(rtems_interval period, rtems_id periodId); diff --git a/src/fsfw/osal/rtems/TaskFactory.cpp b/src/fsfw/osal/rtems/TaskFactory.cpp index 8bfd53edc..fb52eb0ee 100644 --- a/src/fsfw/osal/rtems/TaskFactory.cpp +++ b/src/fsfw/osal/rtems/TaskFactory.cpp @@ -1,7 +1,6 @@ #include "fsfw/tasks/TaskFactory.h" #include "fsfw/osal/rtems/FixedTimeslotTask.h" -#include "fsfw/osal/rtems/InitTask.h" #include "fsfw/osal/rtems/PeriodicTask.h" #include "fsfw/osal/rtems/RtemsBasic.h" #include "fsfw/returnvalues/HasReturnvaluesIF.h" @@ -9,29 +8,29 @@ // TODO: Different variant than the lazy loading in QueueFactory. What's better and why? TaskFactory* TaskFactory::factoryInstance = new TaskFactory(); -TaskFactory::~TaskFactory() {} +TaskFactory::TaskFactory() = default; + +TaskFactory::~TaskFactory() = default; TaskFactory* TaskFactory::instance() { return TaskFactory::factoryInstance; } PeriodicTaskIF* TaskFactory::createPeriodicTask( TaskName name_, TaskPriority taskPriority_, TaskStackSize stackSize_, TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_) { - rtems_interval taskPeriod = periodInSeconds_ * Clock::getTicksPerSecond(); - - return static_cast( - new PeriodicTask(name_, taskPriority_, stackSize_, taskPeriod, deadLineMissedFunction_)); + return static_cast(new PeriodicTask(name_, taskPriority_, stackSize_, + periodInSeconds_, deadLineMissedFunction_)); } FixedTimeslotTaskIF* TaskFactory::createFixedTimeslotTask( TaskName name_, TaskPriority taskPriority_, TaskStackSize stackSize_, TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_) { - rtems_interval taskPeriod = periodInSeconds_ * Clock::getTicksPerSecond(); - return static_cast( - new FixedTimeslotTask(name_, taskPriority_, stackSize_, taskPeriod, deadLineMissedFunction_)); + return static_cast(new FixedTimeslotTask( + name_, taskPriority_, stackSize_, periodInSeconds_, deadLineMissedFunction_)); } ReturnValue_t TaskFactory::deleteTask(PeriodicTaskIF* task) { - // TODO not implemented + // This should call the OS specific destructor + delete (dynamic_cast(task)); return HasReturnvaluesIF::RETURN_FAILED; } @@ -45,5 +44,3 @@ void TaskFactory::printMissedDeadline() { /* TODO: Implement */ return; } - -TaskFactory::TaskFactory() {} diff --git a/src/fsfw/osal/windows/CMakeLists.txt b/src/fsfw/osal/windows/CMakeLists.txt index 36a547658..e961b25b6 100644 --- a/src/fsfw/osal/windows/CMakeLists.txt +++ b/src/fsfw/osal/windows/CMakeLists.txt @@ -1,4 +1 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - tcpipHelpers.cpp - winTaskHelpers.cpp -) +target_sources(${LIB_FSFW_NAME} PRIVATE tcpipHelpers.cpp winTaskHelpers.cpp) diff --git a/src/fsfw/parameters/CMakeLists.txt b/src/fsfw/parameters/CMakeLists.txt index fb5e45900..98a8085cf 100644 --- a/src/fsfw/parameters/CMakeLists.txt +++ b/src/fsfw/parameters/CMakeLists.txt @@ -1,6 +1,3 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - ParameterHelper.cpp - ParameterMessage.cpp - ParameterWrapper.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} PRIVATE ParameterHelper.cpp ParameterMessage.cpp + ParameterWrapper.cpp) diff --git a/src/fsfw/power/CMakeLists.txt b/src/fsfw/power/CMakeLists.txt index 1c625db11..b4ab0006d 100644 --- a/src/fsfw/power/CMakeLists.txt +++ b/src/fsfw/power/CMakeLists.txt @@ -1,7 +1,4 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - Fuse.cpp - PowerComponent.cpp - PowerSensor.cpp - PowerSwitcher.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} + PRIVATE Fuse.cpp PowerComponent.cpp PowerSensor.cpp PowerSwitcher.cpp + DummyPowerSwitcher.cpp PowerSwitcherComponent.cpp) diff --git a/src/fsfw/power/DummyPowerSwitcher.cpp b/src/fsfw/power/DummyPowerSwitcher.cpp new file mode 100644 index 000000000..48ab22c6c --- /dev/null +++ b/src/fsfw/power/DummyPowerSwitcher.cpp @@ -0,0 +1,47 @@ +#include "DummyPowerSwitcher.h" + +DummyPowerSwitcher::DummyPowerSwitcher(object_id_t objectId, size_t numberOfSwitches, + size_t numberOfFuses, bool registerGlobally, + uint32_t switchDelayMs) + : SystemObject(objectId, registerGlobally), + switcherList(numberOfSwitches), + fuseList(numberOfFuses), + switchDelayMs(switchDelayMs) {} + +void DummyPowerSwitcher::setInitialSwitcherList(std::vector switcherList) { + this->switcherList = switcherList; +} + +void DummyPowerSwitcher::setInitialFusesList(std::vector fuseList) { + this->fuseList = fuseList; +} + +ReturnValue_t DummyPowerSwitcher::sendSwitchCommand(power::Switch_t switchNr, ReturnValue_t onOff) { + if (switchNr < switcherList.size()) { + switcherList[switchNr] = onOff; + } + return RETURN_FAILED; +} + +ReturnValue_t DummyPowerSwitcher::sendFuseOnCommand(uint8_t fuseNr) { + if (fuseNr < fuseList.size()) { + fuseList[fuseNr] = FUSE_ON; + } + return RETURN_FAILED; +} + +ReturnValue_t DummyPowerSwitcher::getSwitchState(power::Switch_t switchNr) const { + if (switchNr < switcherList.size()) { + return switcherList[switchNr]; + } + return HasReturnvaluesIF::RETURN_FAILED; +} + +ReturnValue_t DummyPowerSwitcher::getFuseState(uint8_t fuseNr) const { + if (fuseNr < fuseList.size()) { + return fuseList[fuseNr]; + } + return HasReturnvaluesIF::RETURN_FAILED; +} + +uint32_t DummyPowerSwitcher::getSwitchDelayMs(void) const { return switchDelayMs; } diff --git a/src/fsfw/power/DummyPowerSwitcher.h b/src/fsfw/power/DummyPowerSwitcher.h new file mode 100644 index 000000000..6ccd8dd7a --- /dev/null +++ b/src/fsfw/power/DummyPowerSwitcher.h @@ -0,0 +1,38 @@ +#ifndef FSFW_SRC_FSFW_POWER_DUMMYPOWERSWITCHER_H_ +#define FSFW_SRC_FSFW_POWER_DUMMYPOWERSWITCHER_H_ + +#include +#include + +#include "PowerSwitchIF.h" +#include "definitions.h" +#include "fsfw/objectmanager/SystemObject.h" + +/** + * @brief This component can be used to simulate a power switcher like a + * Power Control Distribution Unit (PCDU) + * @details + * The dummy switcher will simply cache the commanded fuse and switch states and return them + * in the according switch getter functions. In that sense, it simulates an ideal PCDU. + */ +class DummyPowerSwitcher : public SystemObject, public PowerSwitchIF { + public: + DummyPowerSwitcher(object_id_t objectId, size_t numberOfSwitches, size_t numberOfFuses, + bool registerGlobally = true, uint32_t switchDelayMs = 5000); + + void setInitialSwitcherList(std::vector switcherList); + void setInitialFusesList(std::vector switcherList); + + virtual ReturnValue_t sendSwitchCommand(power::Switch_t switchNr, ReturnValue_t onOff) override; + virtual ReturnValue_t sendFuseOnCommand(uint8_t fuseNr) override; + virtual ReturnValue_t getSwitchState(power::Switch_t switchNr) const override; + virtual ReturnValue_t getFuseState(uint8_t fuseNr) const override; + virtual uint32_t getSwitchDelayMs(void) const override; + + private: + std::vector switcherList; + std::vector fuseList; + uint32_t switchDelayMs = 5000; +}; + +#endif /* FSFW_SRC_FSFW_POWER_DUMMYPOWERSWITCHER_H_ */ diff --git a/src/fsfw/power/Fuse.h b/src/fsfw/power/Fuse.h index e6f9c1492..43896f75a 100644 --- a/src/fsfw/power/Fuse.h +++ b/src/fsfw/power/Fuse.h @@ -34,14 +34,14 @@ class Fuse : public SystemObject, }; static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PCDU_1; - static const Event FUSE_CURRENT_HIGH = MAKE_EVENT( - 1, severity::LOW); //!< PSS detected that current on a fuse is totally out of bounds. - static const Event FUSE_WENT_OFF = - MAKE_EVENT(2, severity::LOW); //!< PSS detected a fuse that went off. - static const Event POWER_ABOVE_HIGH_LIMIT = - MAKE_EVENT(4, severity::LOW); //!< PSS detected a fuse that violates its limits. - static const Event POWER_BELOW_LOW_LIMIT = - MAKE_EVENT(5, severity::LOW); //!< PSS detected a fuse that violates its limits. + //! PSS detected that current on a fuse is totally out of bounds. + static const Event FUSE_CURRENT_HIGH = MAKE_EVENT(1, severity::LOW); + //! PSS detected a fuse that went off. + static const Event FUSE_WENT_OFF = MAKE_EVENT(2, severity::LOW); + //! PSS detected a fuse that violates its limits. + static const Event POWER_ABOVE_HIGH_LIMIT = MAKE_EVENT(4, severity::LOW); + //! PSS detected a fuse that violates its limits. + static const Event POWER_BELOW_LOW_LIMIT = MAKE_EVENT(5, severity::LOW); typedef std::list DeviceList; Fuse(object_id_t fuseObjectId, uint8_t fuseId, sid_t variableSet, VariableIds ids, @@ -54,7 +54,7 @@ class Fuse : public SystemObject, ReturnValue_t check(); uint8_t getFuseId() const; - ReturnValue_t initialize(); + ReturnValue_t initialize() override; DeviceList devices; ReturnValue_t serialize(uint8_t **buffer, size_t *size, size_t maxSize, SerializeIF::Endianness streamEndianness) const override; diff --git a/src/fsfw/power/PowerSwitchIF.h b/src/fsfw/power/PowerSwitchIF.h index a31d89711..bc883fbc6 100644 --- a/src/fsfw/power/PowerSwitchIF.h +++ b/src/fsfw/power/PowerSwitchIF.h @@ -3,6 +3,7 @@ #include "../events/Event.h" #include "../returnvalues/HasReturnvaluesIF.h" +#include "definitions.h" /** * * @brief This interface defines a connection to a device that is capable of @@ -37,11 +38,11 @@ class PowerSwitchIF : public HasReturnvaluesIF { * @param switchNr * @param onOff on == @c SWITCH_ON; off != @c SWITCH_ON */ - virtual void sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) const = 0; + virtual ReturnValue_t sendSwitchCommand(power::Switch_t switchNr, ReturnValue_t onOff) = 0; /** * Sends a command to the Power Unit to enable a certain fuse. */ - virtual void sendFuseOnCommand(uint8_t fuseNr) const = 0; + virtual ReturnValue_t sendFuseOnCommand(uint8_t fuseNr) = 0; /** * get the state of the Switches. @@ -51,7 +52,7 @@ class PowerSwitchIF : public HasReturnvaluesIF { * - @c SWITCH_OFF if the specified switch is off. * - @c RETURN_FAILED if an error occured */ - virtual ReturnValue_t getSwitchState(uint8_t switchNr) const = 0; + virtual ReturnValue_t getSwitchState(power::Switch_t switchNr) const = 0; /** * get state of a fuse. * @param fuseNr diff --git a/src/fsfw/power/PowerSwitcher.cpp b/src/fsfw/power/PowerSwitcher.cpp index 0a3f8521d..7608c6e75 100644 --- a/src/fsfw/power/PowerSwitcher.cpp +++ b/src/fsfw/power/PowerSwitcher.cpp @@ -1,19 +1,12 @@ #include "fsfw/power/PowerSwitcher.h" +#include "definitions.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" -PowerSwitcher::PowerSwitcher(uint8_t setSwitch1, uint8_t setSwitch2, - PowerSwitcher::State_t setStartState) - : state(setStartState), firstSwitch(setSwitch1), secondSwitch(setSwitch2) {} - -ReturnValue_t PowerSwitcher::initialize(object_id_t powerSwitchId) { - power = ObjectManager::instance()->get(powerSwitchId); - if (power == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - return HasReturnvaluesIF::RETURN_OK; -} +PowerSwitcher::PowerSwitcher(PowerSwitchIF* switcher, power::Switch_t setSwitch1, + power::Switch_t setSwitch2, PowerSwitcher::State_t setStartState) + : power(switcher), state(setStartState), firstSwitch(setSwitch1), secondSwitch(setSwitch2) {} ReturnValue_t PowerSwitcher::getStateOfSwitches() { SwitchReturn_t result = howManySwitches(); @@ -52,18 +45,37 @@ void PowerSwitcher::commandSwitches(ReturnValue_t onOff) { return; } -void PowerSwitcher::turnOn() { +void PowerSwitcher::turnOn(bool checkCurrentState) { + if (checkCurrentState) { + if (getStateOfSwitches() == PowerSwitchIF::SWITCH_ON) { + state = SWITCH_IS_ON; + return; + } + } commandSwitches(PowerSwitchIF::SWITCH_ON); state = WAIT_ON; } -void PowerSwitcher::turnOff() { +void PowerSwitcher::turnOff(bool checkCurrentState) { + if (checkCurrentState) { + if (getStateOfSwitches() == PowerSwitchIF::SWITCH_OFF) { + state = SWITCH_IS_OFF; + return; + } + } commandSwitches(PowerSwitchIF::SWITCH_OFF); state = WAIT_OFF; } +bool PowerSwitcher::active() { + if (state == WAIT_OFF or state == WAIT_ON) { + return true; + } + return false; +} + PowerSwitcher::SwitchReturn_t PowerSwitcher::howManySwitches() { - if (secondSwitch == NO_SWITCH) { + if (secondSwitch == power::NO_SWITCH) { return ONE_SWITCH; } else { return TWO_SWITCHES; diff --git a/src/fsfw/power/PowerSwitcher.h b/src/fsfw/power/PowerSwitcher.h index c72c241d1..279ffacfe 100644 --- a/src/fsfw/power/PowerSwitcher.h +++ b/src/fsfw/power/PowerSwitcher.h @@ -14,28 +14,29 @@ class PowerSwitcher : public HasReturnvaluesIF { SWITCH_IS_OFF, SWITCH_IS_ON, }; - State_t state; + static const uint8_t INTERFACE_ID = CLASS_ID::POWER_SWITCHER; static const ReturnValue_t IN_POWER_TRANSITION = MAKE_RETURN_CODE(1); static const ReturnValue_t SWITCH_STATE_MISMATCH = MAKE_RETURN_CODE(2); - PowerSwitcher(uint8_t setSwitch1, uint8_t setSwitch2 = NO_SWITCH, + PowerSwitcher(PowerSwitchIF* switcher, power::Switch_t setSwitch1, + power::Switch_t setSwitch2 = power::NO_SWITCH, State_t setStartState = SWITCH_IS_OFF); - ReturnValue_t initialize(object_id_t powerSwitchId); - void turnOn(); - void turnOff(); + void turnOn(bool checkCurrentState = true); + void turnOff(bool checkCurrentState = true); + bool active(); void doStateMachine(); State_t getState(); ReturnValue_t checkSwitchState(); uint32_t getSwitchDelay(); - uint8_t getFirstSwitch() const; - uint8_t getSecondSwitch() const; + power::Switch_t getFirstSwitch() const; + power::Switch_t getSecondSwitch() const; private: - uint8_t firstSwitch; - uint8_t secondSwitch; PowerSwitchIF* power = nullptr; + State_t state; + power::Switch_t firstSwitch = power::NO_SWITCH; + power::Switch_t secondSwitch = power::NO_SWITCH; - static const uint8_t NO_SWITCH = 0xFF; enum SwitchReturn_t { ONE_SWITCH = 1, TWO_SWITCHES = 2 }; ReturnValue_t getStateOfSwitches(); void commandSwitches(ReturnValue_t onOff); diff --git a/src/fsfw/power/PowerSwitcherComponent.cpp b/src/fsfw/power/PowerSwitcherComponent.cpp new file mode 100644 index 000000000..9c1ed4cfb --- /dev/null +++ b/src/fsfw/power/PowerSwitcherComponent.cpp @@ -0,0 +1,107 @@ +#include "PowerSwitcherComponent.h" + +#include +#include + +PowerSwitcherComponent::PowerSwitcherComponent(object_id_t objectId, PowerSwitchIF *pwrSwitcher, + power::Switch_t pwrSwitch) + : SystemObject(objectId), + switcher(pwrSwitcher, pwrSwitch), + modeHelper(this), + healthHelper(this, objectId) { + queue = QueueFactory::instance()->createMessageQueue(); +} + +ReturnValue_t PowerSwitcherComponent::performOperation(uint8_t opCode) { + ReturnValue_t result; + CommandMessage command; + + for (result = queue->receiveMessage(&command); result == RETURN_OK; + result = queue->receiveMessage(&command)) { + result = healthHelper.handleHealthCommand(&command); + if (result == RETURN_OK) { + continue; + } + + result = modeHelper.handleModeCommand(&command); + if (result == RETURN_OK) { + continue; + } + } + if (switcher.active()) { + switcher.doStateMachine(); + auto currState = switcher.getState(); + if (currState == PowerSwitcher::SWITCH_IS_OFF) { + setMode(MODE_OFF, 0); + } else if (currState == PowerSwitcher::SWITCH_IS_ON) { + setMode(MODE_ON, 0); + } + } + return RETURN_OK; +} + +ReturnValue_t PowerSwitcherComponent::initialize() { + ReturnValue_t result = modeHelper.initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = healthHelper.initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return SystemObject::initialize(); +} + +MessageQueueId_t PowerSwitcherComponent::getCommandQueue() const { return queue->getId(); } + +void PowerSwitcherComponent::getMode(Mode_t *mode, Submode_t *submode) { + *mode = this->mode; + *submode = this->submode; +} + +ReturnValue_t PowerSwitcherComponent::setHealth(HealthState health) { + healthHelper.setHealth(health); + return RETURN_OK; +} + +ReturnValue_t PowerSwitcherComponent::checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode) { + *msToReachTheMode = 5000; + if (mode != MODE_ON and mode != MODE_OFF) { + return TRANS_NOT_ALLOWED; + } + return RETURN_OK; +} + +void PowerSwitcherComponent::startTransition(Mode_t mode, Submode_t submode) { + if (mode == MODE_OFF) { + switcher.turnOff(true); + switcher.doStateMachine(); + if (switcher.getState() == PowerSwitcher::SWITCH_IS_OFF) { + setMode(MODE_OFF, 0); + } + } else if (mode == MODE_ON) { + switcher.turnOn(true); + switcher.doStateMachine(); + if (switcher.getState() == PowerSwitcher::SWITCH_IS_ON) { + setMode(MODE_ON, 0); + } + } +} + +void PowerSwitcherComponent::setToExternalControl() { + healthHelper.setHealth(HasHealthIF::EXTERNAL_CONTROL); +} + +void PowerSwitcherComponent::announceMode(bool recursive) { + triggerEvent(MODE_INFO, mode, submode); +} + +void PowerSwitcherComponent::setMode(Mode_t newMode, Submode_t newSubmode) { + this->mode = newMode; + this->submode = newSubmode; + modeHelper.modeChanged(mode, submode); + announceMode(false); +} + +HasHealthIF::HealthState PowerSwitcherComponent::getHealth() { return healthHelper.getHealth(); } diff --git a/src/fsfw/power/PowerSwitcherComponent.h b/src/fsfw/power/PowerSwitcherComponent.h new file mode 100644 index 000000000..a3ed640e0 --- /dev/null +++ b/src/fsfw/power/PowerSwitcherComponent.h @@ -0,0 +1,62 @@ +#ifndef _FSFW_POWER_POWERSWITCHERCOMPONENT_H_ +#define _FSFW_POWER_POWERSWITCHERCOMPONENT_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +class PowerSwitchIF; + +/** + * @brief Allows to create an power switch object with its own mode and health + * @details + * This basic component allows to create an object which is solely responsible for managing a + * switch. It also has a mode and a health by implementing the respective interface components + * which allows integrating this component into a system mode tree. + * + * Commanding this component to MODE_OFF will cause the switcher to turn the switch off while + * commanding in to MODE_ON will cause the switcher to turn the switch on. + */ +class PowerSwitcherComponent : public SystemObject, + public HasReturnvaluesIF, + public ExecutableObjectIF, + public HasModesIF, + public HasHealthIF { + public: + PowerSwitcherComponent(object_id_t objectId, PowerSwitchIF *pwrSwitcher, + power::Switch_t pwrSwitch); + + private: + MessageQueueIF *queue = nullptr; + PowerSwitcher switcher; + + Mode_t mode = MODE_OFF; + Submode_t submode = 0; + + ModeHelper modeHelper; + HealthHelper healthHelper; + + void setMode(Mode_t newMode, Submode_t newSubmode); + + virtual ReturnValue_t performOperation(uint8_t opCode) override; + + ReturnValue_t initialize() override; + + MessageQueueId_t getCommandQueue() const override; + void getMode(Mode_t *mode, Submode_t *submode) override; + ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode) override; + void startTransition(Mode_t mode, Submode_t submode) override; + void setToExternalControl() override; + void announceMode(bool recursive) override; + + ReturnValue_t setHealth(HealthState health) override; + HasHealthIF::HealthState getHealth() override; +}; + +#endif /* _FSFW_POWER_POWERSWITCHERCOMPONENT_H_ */ diff --git a/src/fsfw/power/definitions.h b/src/fsfw/power/definitions.h new file mode 100644 index 000000000..ed2a0037d --- /dev/null +++ b/src/fsfw/power/definitions.h @@ -0,0 +1,13 @@ +#ifndef FSFW_SRC_FSFW_POWER_DEFINITIONS_H_ +#define FSFW_SRC_FSFW_POWER_DEFINITIONS_H_ + +#include + +namespace power { + +using Switch_t = uint8_t; +static constexpr Switch_t NO_SWITCH = 0xFF; + +} // namespace power + +#endif /* FSFW_SRC_FSFW_POWER_DEFINITIONS_H_ */ diff --git a/src/fsfw/pus/CMakeLists.txt b/src/fsfw/pus/CMakeLists.txt index 8b55adf03..35b35beac 100644 --- a/src/fsfw/pus/CMakeLists.txt +++ b/src/fsfw/pus/CMakeLists.txt @@ -1,12 +1,12 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - Service1TelecommandVerification.cpp - Service2DeviceAccess.cpp - Service3Housekeeping.cpp - Service5EventReporting.cpp - Service8FunctionManagement.cpp - Service9TimeManagement.cpp - Service17Test.cpp - Service20ParameterManagement.cpp - CService200ModeCommanding.cpp - CService201HealthCommanding.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} + PRIVATE Service1TelecommandVerification.cpp + Service2DeviceAccess.cpp + Service3Housekeeping.cpp + Service5EventReporting.cpp + Service8FunctionManagement.cpp + Service9TimeManagement.cpp + Service17Test.cpp + Service20ParameterManagement.cpp + CService200ModeCommanding.cpp + CService201HealthCommanding.cpp) diff --git a/src/fsfw/pus/CService201HealthCommanding.cpp b/src/fsfw/pus/CService201HealthCommanding.cpp index 4d9806f9d..644e0d7ce 100644 --- a/src/fsfw/pus/CService201HealthCommanding.cpp +++ b/src/fsfw/pus/CService201HealthCommanding.cpp @@ -13,8 +13,6 @@ CService201HealthCommanding::CService201HealthCommanding(object_id_t objectId, u : CommandingServiceBase(objectId, apid, serviceId, numParallelCommands, commandTimeoutSeconds) { } -CService201HealthCommanding::~CService201HealthCommanding() {} - ReturnValue_t CService201HealthCommanding::isValidSubservice(uint8_t subservice) { switch (subservice) { case (Subservice::COMMAND_SET_HEALTH): @@ -43,8 +41,8 @@ ReturnValue_t CService201HealthCommanding::getMessageQueueAndObject(uint8_t subs } ReturnValue_t CService201HealthCommanding::checkInterfaceAndAcquireMessageQueue( - MessageQueueId_t *messageQueueToSet, object_id_t *objectId) { - HasHealthIF *destination = ObjectManager::instance()->get(*objectId); + MessageQueueId_t *messageQueueToSet, const object_id_t *objectId) { + auto *destination = ObjectManager::instance()->get(*objectId); if (destination == nullptr) { return CommandingServiceBase::INVALID_OBJECT; } @@ -77,6 +75,10 @@ ReturnValue_t CService201HealthCommanding::prepareCommand(CommandMessage *messag HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_ANNOUNCE_ALL); break; } + default: { + // Should never happen, subservice was already checked + result = RETURN_FAILED; + } } return result; } @@ -95,10 +97,10 @@ ReturnValue_t CService201HealthCommanding::handleReply(const CommandMessage *rep } // Not used for now, health state already reported by event -ReturnValue_t CService201HealthCommanding::prepareHealthSetReply(const CommandMessage *reply) { - prepareHealthSetReply(reply); - uint8_t health = static_cast(HealthMessage::getHealth(reply)); - uint8_t oldHealth = static_cast(HealthMessage::getOldHealth(reply)); +[[maybe_unused]] ReturnValue_t CService201HealthCommanding::prepareHealthSetReply( + const CommandMessage *reply) { + auto health = static_cast(HealthMessage::getHealth(reply)); + auto oldHealth = static_cast(HealthMessage::getOldHealth(reply)); HealthSetReply healthSetReply(health, oldHealth); return sendTmPacket(Subservice::REPLY_HEALTH_SET, &healthSetReply); } diff --git a/src/fsfw/pus/CService201HealthCommanding.h b/src/fsfw/pus/CService201HealthCommanding.h index 5e52ab39e..71b7caa05 100644 --- a/src/fsfw/pus/CService201HealthCommanding.h +++ b/src/fsfw/pus/CService201HealthCommanding.h @@ -1,7 +1,7 @@ #ifndef FSFW_PUS_CSERVICE201HEALTHCOMMANDING_H_ #define FSFW_PUS_CSERVICE201HEALTHCOMMANDING_H_ -#include "../tmtcservices/CommandingServiceBase.h" +#include "fsfw/tmtcservices/CommandingServiceBase.h" /** * @brief Custom PUS service to set health of all objects @@ -21,7 +21,7 @@ class CService201HealthCommanding : public CommandingServiceBase { public: CService201HealthCommanding(object_id_t objectId, uint16_t apid, uint8_t serviceId, uint8_t numParallelCommands = 4, uint16_t commandTimeoutSeconds = 60); - virtual ~CService201HealthCommanding(); + ~CService201HealthCommanding() override = default; protected: /* CSB abstract function implementations */ @@ -38,12 +38,10 @@ class CService201HealthCommanding : public CommandingServiceBase { bool *isStep) override; private: - ReturnValue_t checkAndAcquireTargetID(object_id_t *objectIdToSet, const uint8_t *tcData, - size_t tcDataLen); - ReturnValue_t checkInterfaceAndAcquireMessageQueue(MessageQueueId_t *MessageQueueToSet, - object_id_t *objectId); + static ReturnValue_t checkInterfaceAndAcquireMessageQueue(MessageQueueId_t *MessageQueueToSet, + const object_id_t *objectId); - ReturnValue_t prepareHealthSetReply(const CommandMessage *reply); + [[maybe_unused]] ReturnValue_t prepareHealthSetReply(const CommandMessage *reply); enum Subservice { //! [EXPORT] : [TC] Set health of target object diff --git a/src/fsfw/pus/Service11TelecommandScheduling.h b/src/fsfw/pus/Service11TelecommandScheduling.h new file mode 100644 index 000000000..0fed8bcaf --- /dev/null +++ b/src/fsfw/pus/Service11TelecommandScheduling.h @@ -0,0 +1,203 @@ +#ifndef MISSION_PUS_SERVICE11TELECOMMANDSCHEDULING_H_ +#define MISSION_PUS_SERVICE11TELECOMMANDSCHEDULING_H_ + +#include +#include +#include + +#include "fsfw/FSFW.h" +#include "fsfw/returnvalues/FwClassIds.h" + +/** + * @brief: PUS-Service 11 - Telecommand scheduling. + * @details: + * PUS-Service 11 - Telecommand scheduling. + * Full documentation: ECSS-E-ST-70-41C, p. 168: + * ST[11] time-based scheduling + * + * This service provides the capability to command pre-loaded + * application processes (telecommands) by releasing them at their + * due-time. + * References to telecommands are stored together with their due-timepoints + * and are released at their corresponding due-time. + * + * Necessary subservice functionalities are implemented. + * Those are: + * TC[11,4] activity insertion + * TC[11,5] activity deletion + * TC[11,6] filter-based activity deletion + * TC[11,7] activity time-shift + * TC[11,8] filter-based activity time-shift + * + * Groups are not supported. + * This service remains always enabled. Sending a disable-request has no effect. + */ +template +class Service11TelecommandScheduling final : public PusServiceBase { + public: + static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_11; + + static constexpr ReturnValue_t INVALID_TYPE_TIME_WINDOW = + HasReturnvaluesIF::makeReturnCode(CLASS_ID, 1); + static constexpr ReturnValue_t TIMESHIFTING_NOT_POSSIBLE = + HasReturnvaluesIF::makeReturnCode(CLASS_ID, 2); + static constexpr ReturnValue_t INVALID_RELATIVE_TIME = + HasReturnvaluesIF::makeReturnCode(CLASS_ID, 3); + + static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_11; + + //! [EXPORT] : [COMMENT] Deletion of a TC from the map failed. + //! P1: First 32 bit of request ID, P2. Last 32 bit of Request ID + static constexpr Event TC_DELETION_FAILED = event::makeEvent(SUBSYSTEM_ID, 0, severity::MEDIUM); + + // The types of PUS-11 subservices + enum Subservice : uint8_t { + ENABLE_SCHEDULING = 1, + DISABLE_SCHEDULING = 2, + RESET_SCHEDULING = 3, + INSERT_ACTIVITY = 4, + DELETE_ACTIVITY = 5, + FILTER_DELETE_ACTIVITY = 6, + TIMESHIFT_ACTIVITY = 7, + FILTER_TIMESHIFT_ACTIVITY = 8, + DETAIL_REPORT = 9, + TIMEBASE_SCHEDULE_DETAIL_REPORT = 10, + TIMESHIFT_ALL_SCHEDULE_ACTIVITIES = 15 + }; + + // The types of time windows for TC[11,6] and TC[11,8], as defined in ECSS-E-ST-70-41C, + // requirement 8.11.3c (p. 507) + enum TypeOfTimeWindow : uint32_t { + SELECT_ALL = 0, + FROM_TIMETAG_TO_TIMETAG = 1, + FROM_TIMETAG = 2, + TO_TIMETAG = 3 + }; + + Service11TelecommandScheduling(object_id_t objectId, uint16_t apid, uint8_t serviceId, + AcceptsTelecommandsIF* tcRecipient, + uint16_t releaseTimeMarginSeconds = DEFAULT_RELEASE_TIME_MARGIN, + bool debugMode = false); + + ~Service11TelecommandScheduling() override; + + void enableExpiredTcDeletion(); + void disableExpiredTcDeletion(); + + /** PusServiceBase overrides */ + ReturnValue_t handleRequest(uint8_t subservice) override; + ReturnValue_t performService() override; + ReturnValue_t initialize() override; + + private: + struct TelecommandStruct { + uint64_t requestId{}; + uint32_t seconds{}; + store_address_t storeAddr; // uint16 + }; + + static constexpr uint16_t DEFAULT_RELEASE_TIME_MARGIN = 5; + + // minimum release time offset to insert into schedule + const uint16_t RELEASE_TIME_MARGIN_SECONDS = 5; + + /** + * By default, the scheduling will be disabled. This is a standard requirement + */ + bool schedulingEnabled = false; + bool deleteExpiredTcWhenDisabled = true; + bool debugMode = false; + StorageManagerIF* tcStore = nullptr; + AcceptsTelecommandsIF* tcRecipient = nullptr; + MessageQueueId_t recipientMsgQueueId = 0; + + /** + * The telecommand map uses the exectution time as a Unix time stamp as + * the key. This is mapped to a generic telecommand struct. + */ + using TelecommandMap = etl::multimap; + using TcMapIter = typename TelecommandMap::iterator; + + TelecommandMap telecommandMap; + + ReturnValue_t handleResetCommand(); + /** + * @brief Logic to be performed on an incoming TC[11,4]. + * @return RETURN_OK if successful + */ + ReturnValue_t doInsertActivity(const uint8_t* data, size_t size); + + /** + * @brief Logic to be performed on an incoming TC[11,5]. + * @return RETURN_OK if successful + */ + ReturnValue_t doDeleteActivity(const uint8_t* data, size_t size); + + /** + * @brief Logic to be performed on an incoming TC[11,6]. + * @return RETURN_OK if successful + */ + ReturnValue_t doFilterDeleteActivity(const uint8_t* data, size_t size); + + /** + * @brief Logic to be performed on an incoming TC[11,7]. + * @return RETURN_OK if successful + */ + ReturnValue_t doTimeshiftActivity(const uint8_t* data, size_t size); + + /** + * @brief Logic to be performed on an incoming TC[11,8]. + * @return RETURN_OK if successful + */ + ReturnValue_t doFilterTimeshiftActivity(const uint8_t* data, size_t size); + + /** + * @brief Extracts the Request ID from the Application Data of a TC by utilizing a ctor of the + * class TcPacketPus. + * NOTE: This only works if the payload data is a TC (e.g. not for TC[11,5] which does not + * send a TC as payload)! + * @param data The Application data of the TC (get via getApplicationData()). + * @return requestId + */ + uint64_t getRequestIdFromDataTC(const uint8_t* data) const; + + /** + * @brief Extracts the Request ID from the Application Data directly, assuming it is packed + * as follows (acc. to ECSS): | source ID (uint32) | apid (uint32) | ssc (uint32) |. + * @param data Pointer to first byte described data + * @param dataSize Remaining size of data NOTE: non-const, this is modified by the function + * @param [out] requestId Request ID + * @return RETURN_OK if successful + */ + ReturnValue_t getRequestIdFromData(const uint8_t*& data, size_t& dataSize, uint64_t& requestId); + + /** + * @brief Builds the Request ID from its three elements. + * @param sourceId Source ID + * @param apid Application Process ID (APID) + * @param ssc Source Sequence Count + * @return Request ID + */ + [[nodiscard]] uint64_t buildRequestId(uint32_t sourceId, uint16_t apid, uint16_t ssc) const; + + /** + * @brief Gets the filter range for filter TCs from a data packet + * @param data TC data + * @param dataSize TC data size + * @param [out] itBegin Begin of filter range + * @param [out] itEnd End of filter range + * @return RETURN_OK if successful + */ + ReturnValue_t getMapFilterFromData(const uint8_t*& data, size_t& size, TcMapIter& itBegin, + TcMapIter& itEnd); + + ReturnValue_t handleInvalidData(const char* ctx); + /** + * @brief Prints content of multimap. Use for simple debugging only. + */ + void debugPrintMultimapContent() const; +}; + +#include "Service11TelecommandScheduling.tpp" + +#endif /* MISSION_PUS_SERVICE11TELECOMMANDSCHEDULING_H_ */ diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp new file mode 100644 index 000000000..968a59ffa --- /dev/null +++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp @@ -0,0 +1,645 @@ +#pragma once + +#include + +#include "fsfw/objectmanager/ObjectManager.h" +#include "fsfw/serialize/SerializeAdapter.h" +#include "fsfw/serviceinterface.h" +#include "fsfw/tmtcservices/AcceptsTelecommandsIF.h" + +static constexpr auto DEF_END = SerializeIF::Endianness::BIG; + +template +inline Service11TelecommandScheduling::Service11TelecommandScheduling( + object_id_t objectId, uint16_t apid, uint8_t serviceId, AcceptsTelecommandsIF *tcRecipient, + uint16_t releaseTimeMarginSeconds, bool debugMode) + : PusServiceBase(objectId, apid, serviceId), + RELEASE_TIME_MARGIN_SECONDS(releaseTimeMarginSeconds), + debugMode(debugMode), + tcRecipient(tcRecipient) {} + +template +inline Service11TelecommandScheduling::~Service11TelecommandScheduling() = default; + +template +inline ReturnValue_t Service11TelecommandScheduling::handleRequest( + uint8_t subservice) { + if (debugMode) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "PUS11::handleRequest: Handling request " << static_cast(subservice); +#else + sif::printInfo("PUS11::handleRequest: Handling request %d\n", subservice); +#endif + } + // Get de-serialized Timestamp + const uint8_t *data = currentPacket.getApplicationData(); + size_t size = currentPacket.getApplicationDataSize(); + if (data == nullptr) { + return handleInvalidData("handleRequest"); + } + switch (subservice) { + case Subservice::ENABLE_SCHEDULING: { + schedulingEnabled = true; + break; + } + case Subservice::DISABLE_SCHEDULING: { + schedulingEnabled = false; + break; + } + case Subservice::RESET_SCHEDULING: { + return handleResetCommand(); + } + case Subservice::INSERT_ACTIVITY: + return doInsertActivity(data, size); + case Subservice::DELETE_ACTIVITY: + return doDeleteActivity(data, size); + case Subservice::FILTER_DELETE_ACTIVITY: + return doFilterDeleteActivity(data, size); + case Subservice::TIMESHIFT_ACTIVITY: + return doTimeshiftActivity(data, size); + case Subservice::FILTER_TIMESHIFT_ACTIVITY: + return doFilterTimeshiftActivity(data, size); + default: + return AcceptsTelecommandsIF::INVALID_SUBSERVICE; + } + return RETURN_OK; +} + +template +inline ReturnValue_t Service11TelecommandScheduling::performService() { + if (not schedulingEnabled) { + return RETURN_OK; + } + // get current time as UNIX timestamp + timeval tNow = {}; + Clock::getClock_timeval(&tNow); + + // TODO: Optionally limit the max number of released TCs per cycle? + // NOTE: The iterator is increased in the loop here. Increasing the iterator as for-loop arg + // does not work in this case as we are deleting the current element here. + for (auto it = telecommandMap.begin(); it != telecommandMap.end();) { + if (it->first <= tNow.tv_sec) { + if (schedulingEnabled) { + // release tc + TmTcMessage releaseMsg(it->second.storeAddr); + auto sendRet = this->requestQueue->sendMessage(recipientMsgQueueId, &releaseMsg, false); + + if (sendRet != HasReturnvaluesIF::RETURN_OK) { + return sendRet; + } + if (debugMode) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "Released TC & erased it from TC map" << std::endl; +#else + sif::printInfo("Released TC & erased it from TC map\n"); +#endif + } + telecommandMap.erase(it++); + } else if (deleteExpiredTcWhenDisabled) { + telecommandMap.erase(it++); + } + continue; + } + it++; + } + + return HasReturnvaluesIF::RETURN_OK; +} + +template +inline ReturnValue_t Service11TelecommandScheduling::initialize() { + ReturnValue_t res = PusServiceBase::initialize(); + if (res != HasReturnvaluesIF::RETURN_OK) { + return res; + } + + tcStore = ObjectManager::instance()->get(objects::TC_STORE); + if (!tcStore) { + return ObjectManagerIF::CHILD_INIT_FAILED; + } + + if (tcRecipient == nullptr) { + return ObjectManagerIF::CHILD_INIT_FAILED; + } + recipientMsgQueueId = tcRecipient->getRequestQueue(); + + return res; +} + +template +inline ReturnValue_t Service11TelecommandScheduling::handleResetCommand() { + for (auto it = telecommandMap.begin(); it != telecommandMap.end(); it++) { + ReturnValue_t result = tcStore->deleteData(it->second.storeAddr); + if (result != RETURN_OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + // This should not happen + sif::warning << "Service11TelecommandScheduling::handleRequestDeleting: Deletion failed" + << std::endl; +#else + sif::printWarning("Service11TelecommandScheduling::handleRequestDeleting: Deletion failed\n"); +#endif + triggerEvent(TC_DELETION_FAILED, (it->second.requestId >> 32) & 0xffffffff, + it->second.requestId & 0xffffffff); + } + } + telecommandMap.clear(); + return RETURN_OK; +} + +template +inline ReturnValue_t Service11TelecommandScheduling::doInsertActivity( + const uint8_t *data, size_t size) { + uint32_t timestamp = 0; + ReturnValue_t result = SerializeAdapter::deSerialize(×tamp, &data, &size, DEF_END); + if (result != RETURN_OK) { + return result; + } + + // Insert possible if sched. time is above margin + // (See requirement for Time margin) + timeval tNow = {}; + Clock::getClock_timeval(&tNow); + if (timestamp - tNow.tv_sec <= RELEASE_TIME_MARGIN_SECONDS) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "Service11TelecommandScheduling::doInsertActivity: Release time too close to " + "current time" + << std::endl; +#else + sif::printWarning( + "Service11TelecommandScheduling::doInsertActivity: Release time too close to current " + "time\n"); +#endif + return RETURN_FAILED; + } + + // store currentPacket and receive the store address + store_address_t addr{}; + if (tcStore->addData(&addr, data, size) != RETURN_OK || + addr.raw == storeId::INVALID_STORE_ADDRESS) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "Service11TelecommandScheduling::doInsertActivity: Adding data to TC Store failed" + << std::endl; +#else + sif::printError( + "Service11TelecommandScheduling::doInsertActivity: Adding data to TC Store failed\n"); +#endif + return RETURN_FAILED; + } + + // insert into multimap with new store address + TelecommandStruct tc; + tc.seconds = timestamp; + tc.storeAddr = addr; + tc.requestId = + getRequestIdFromDataTC(data); // TODO: Missing sanity check of the returned request id + + auto it = telecommandMap.insert(std::pair(timestamp, tc)); + if (it == telecommandMap.end()) { + return RETURN_FAILED; + } + + if (debugMode) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "PUS11::doInsertActivity: Inserted into Multimap:" << std::endl; +#else + sif::printInfo("PUS11::doInsertActivity: Inserted into Multimap:\n"); +#endif + debugPrintMultimapContent(); + } + return HasReturnvaluesIF::RETURN_OK; +} + +template +inline ReturnValue_t Service11TelecommandScheduling::doDeleteActivity( + const uint8_t *data, size_t size) { + // Get request ID + uint64_t requestId; + ReturnValue_t result = getRequestIdFromData(data, size, requestId); + if (result != RETURN_OK) { + return result; + } + + // DEBUG + if (debugMode) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "PUS11::doDeleteActivity: requestId: " << requestId << std::endl; +#else + sif::printInfo("PUS11::doDeleteActivity: requestId: %d\n", requestId); +#endif + } + + TcMapIter tcToDelete; // handle to the TC to be deleted, can be used if counter is valid + int tcToDeleteCount = 0; // counter of all found TCs. Should be 1. + + for (auto it = telecommandMap.begin(); it != telecommandMap.end(); it++) { + if (it->second.requestId == requestId) { + tcToDelete = it; + tcToDeleteCount++; + } + } + + // check if 1 explicit TC is found via request ID + if (tcToDeleteCount == 0 || tcToDeleteCount > 1) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "Service11TelecommandScheduling::doDeleteActivity: No or more than 1 TC found. " + "Cannot explicitly delete TC" + << std::endl; +#else + sif::printWarning( + "Service11TelecommandScheduling::doDeleteActivity: No or more than 1 TC found. " + "Cannot explicitly delete TC"); +#endif + return RETURN_FAILED; + } + + // delete packet from store + if (tcStore->deleteData(tcToDelete->second.storeAddr) != RETURN_OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "Service11TelecommandScheduling::doDeleteActivity: Could not delete TC from Store" + << std::endl; +#else + sif::printError( + "Service11TelecommandScheduling::doDeleteActivity: Could not delete TC from Store\n"); +#endif + return RETURN_FAILED; + } + + telecommandMap.erase(tcToDelete); + if (debugMode) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "PUS11::doDeleteActivity: Deleted TC from map" << std::endl; +#else + sif::printInfo("PUS11::doDeleteActivity: Deleted TC from map\n"); +#endif + } + + return RETURN_OK; +} + +template +inline ReturnValue_t Service11TelecommandScheduling::doFilterDeleteActivity( + const uint8_t *data, size_t size) { + TcMapIter itBegin; + TcMapIter itEnd; + + ReturnValue_t result = getMapFilterFromData(data, size, itBegin, itEnd); + // get the filter window as map range via dedicated method + if (result != RETURN_OK) { + return result; + } + + int deletedTCs = 0; + for (TcMapIter it = itBegin; it != itEnd; it++) { + // delete packet from store + if (tcStore->deleteData(it->second.storeAddr) != RETURN_OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "Service11TelecommandScheduling::doFilterDeleteActivity: Could not delete TC " + "from Store" + << std::endl; +#else + sif::printError( + "Service11TelecommandScheduling::doFilterDeleteActivity: Could not delete TC from " + "Store\n"); +#endif + continue; + } + deletedTCs++; + } + + // NOTE: Spec says this function erases all elements including itBegin but not itEnd, + // see here: https://www.cplusplus.com/reference/map/multimap/erase/ + // Therefore we need to increase itEnd by 1. (Note that end() returns the "past-the-end" iterator) + if (itEnd != telecommandMap.end()) { + itEnd++; + } + + telecommandMap.erase(itBegin, itEnd); + + if (debugMode) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "PUS11::doFilterDeleteActivity: Deleted " << deletedTCs << " TCs" << std::endl; +#else + sif::printInfo("PUS11::doFilterDeleteActivity: Deleted %d TCs\n", deletedTCs); +#endif + } + return RETURN_OK; +} + +template +inline ReturnValue_t Service11TelecommandScheduling::doTimeshiftActivity( + const uint8_t *data, size_t size) { + // Get relative time + uint32_t relativeTime = 0; + ReturnValue_t result = SerializeAdapter::deSerialize(&relativeTime, &data, &size, DEF_END); + if (result != RETURN_OK) { + return result; + } + if (relativeTime == 0) { + return INVALID_RELATIVE_TIME; + } + // TODO further check sanity of the relative time? + + // Get request ID + uint64_t requestId; + result = getRequestIdFromData(data, size, requestId); + if (result != RETURN_OK) { + return result; + } + + if (debugMode) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "PUS11::doTimeshiftActivity: requestId: " << requestId << std::endl; +#else + sif::printInfo("PUS11::doTimeshiftActivity: requestId: %d\n", requestId); +#endif + } + + // NOTE: Despite having C++17 ETL multimap has no member function extract :( + + TcMapIter tcToTimeshiftIt; + int tcToTimeshiftCount = 0; + + for (auto it = telecommandMap.begin(); it != telecommandMap.end(); it++) { + if (it->second.requestId == requestId) { + tcToTimeshiftIt = it; + tcToTimeshiftCount++; + } + } + + if (tcToTimeshiftCount == 0 || tcToTimeshiftCount > 1) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "Service11TelecommandScheduling::doTimeshiftActivity: Either 0 or more than 1 " + "TCs found. No explicit timeshifting " + "possible" + << std::endl; + +#else + sif::printWarning( + "Service11TelecommandScheduling::doTimeshiftActivity: Either 0 or more than 1 TCs found. " + "No explicit timeshifting possible\n"); +#endif + return TIMESHIFTING_NOT_POSSIBLE; + } + + // temporarily hold the item + TelecommandStruct tempTc(tcToTimeshiftIt->second); + uint32_t tempKey = tcToTimeshiftIt->first + relativeTime; + + // delete old entry from the mm + telecommandMap.erase(tcToTimeshiftIt); + + // and then insert it again as new entry + telecommandMap.insert(std::make_pair(tempKey, tempTc)); + + if (debugMode) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "PUS11::doTimeshiftActivity: Shifted TC" << std::endl; +#else + sif::printDebug("PUS11::doTimeshiftActivity: Shifted TC\n"); +#endif + debugPrintMultimapContent(); + } + + return RETURN_OK; +} + +template +inline ReturnValue_t Service11TelecommandScheduling::doFilterTimeshiftActivity( + const uint8_t *data, size_t size) { + // Get relative time + uint32_t relativeTime = 0; + ReturnValue_t result = SerializeAdapter::deSerialize(&relativeTime, &data, &size, DEF_END); + if (result != RETURN_OK) { + return result; + } + if (relativeTime == 0) { + return INVALID_RELATIVE_TIME; + } + + // Do time window + TcMapIter itBegin; + TcMapIter itEnd; + result = getMapFilterFromData(data, size, itBegin, itEnd); + if (result != RETURN_OK) { + return result; + } + + int shiftedItemsCount = 0; + for (auto it = itBegin; it != itEnd;) { + // temporarily hold the item + TelecommandStruct tempTc(it->second); + uint32_t tempKey = it->first + relativeTime; + + // delete the old entry from the mm + telecommandMap.erase(it++); + + // and then insert it again as new entry + telecommandMap.insert(std::make_pair(tempKey, tempTc)); + shiftedItemsCount++; + } + + if (debugMode) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "PUS11::doFilterTimeshiftActivity: shiftedItemsCount: " << shiftedItemsCount + << std::endl; +#else + sif::printInfo("PUS11::doFilterTimeshiftActivity: shiftedItemsCount: %d\n", shiftedItemsCount); +#endif + debugPrintMultimapContent(); + } + + if (shiftedItemsCount > 0) { + return RETURN_OK; + } + return RETURN_FAILED; +} + +template +inline uint64_t Service11TelecommandScheduling::getRequestIdFromDataTC( + const uint8_t *data) const { + TcPacketPus mask(data); + + uint32_t sourceId = mask.getSourceId(); + uint16_t apid = mask.getAPID(); + uint16_t sequenceCount = mask.getPacketSequenceCount(); + + return buildRequestId(sourceId, apid, sequenceCount); +} + +template +inline ReturnValue_t Service11TelecommandScheduling::getRequestIdFromData( + const uint8_t *&data, size_t &dataSize, uint64_t &requestId) { + uint32_t srcId = 0; + uint16_t apid = 0; + uint16_t ssc = 0; + + ReturnValue_t result = SerializeAdapter::deSerialize(&srcId, &data, &dataSize, DEF_END); + if (result != RETURN_OK) { + return result; + } + result = SerializeAdapter::deSerialize(&apid, &data, &dataSize, DEF_END); + if (result != RETURN_OK) { + return result; + } + result = SerializeAdapter::deSerialize(&ssc, &data, &dataSize, DEF_END); + if (result != RETURN_OK) { + return result; + } + requestId = buildRequestId(srcId, apid, ssc); + + return RETURN_OK; +} + +template +inline uint64_t Service11TelecommandScheduling::buildRequestId(uint32_t sourceId, + uint16_t apid, + uint16_t ssc) const { + auto sourceId64 = static_cast(sourceId); + auto apid64 = static_cast(apid); + auto ssc64 = static_cast(ssc); + + return (sourceId64 << 32) | (apid64 << 16) | ssc64; +} + +template +inline ReturnValue_t Service11TelecommandScheduling::getMapFilterFromData( + const uint8_t *&data, size_t &dataSize, TcMapIter &itBegin, TcMapIter &itEnd) { + // get filter type first + uint32_t typeRaw = 0; + ReturnValue_t result = SerializeAdapter::deSerialize(&typeRaw, &data, &dataSize, DEF_END); + if (result != RETURN_OK) { + return result; + } + + if (typeRaw > 3) { + return INVALID_TYPE_TIME_WINDOW; + } + auto type = static_cast(typeRaw); + + // we now have the type of delete activity - so now we set the range to delete, + // according to the type of time window. + // NOTE: Blocks are used in this switch-case statement so that timestamps can be + // cleanly redefined for each case. + switch (type) { + case TypeOfTimeWindow::SELECT_ALL: { + itBegin = telecommandMap.begin(); + itEnd = telecommandMap.end(); + break; + } + + case TypeOfTimeWindow::FROM_TIMETAG: { + uint32_t fromTimestamp = 0; + result = SerializeAdapter::deSerialize(&fromTimestamp, &data, &dataSize, DEF_END); + if (result != RETURN_OK) { + return result; + } + + itBegin = telecommandMap.begin(); + while (itBegin->first < fromTimestamp && itBegin != telecommandMap.end()) { + itBegin++; + } + + itEnd = telecommandMap.end(); + break; + } + + case TypeOfTimeWindow::TO_TIMETAG: { + uint32_t toTimestamp; + result = SerializeAdapter::deSerialize(&toTimestamp, &data, &dataSize, DEF_END); + if (result != RETURN_OK) { + return result; + } + itBegin = telecommandMap.begin(); + itEnd = telecommandMap.begin(); + while (itEnd->first <= toTimestamp && itEnd != telecommandMap.end()) { + itEnd++; + } + break; + } + + case TypeOfTimeWindow::FROM_TIMETAG_TO_TIMETAG: { + uint32_t fromTimestamp; + uint32_t toTimestamp; + + result = SerializeAdapter::deSerialize(&fromTimestamp, &data, &dataSize, + SerializeIF::Endianness::BIG); + if (result != RETURN_OK) { + return result; + } + result = SerializeAdapter::deSerialize(&toTimestamp, &data, &dataSize, + SerializeIF::Endianness::BIG); + if (result != RETURN_OK) { + return result; + } + itBegin = telecommandMap.begin(); + itEnd = telecommandMap.begin(); + + while (itBegin->first < fromTimestamp && itBegin != telecommandMap.end()) { + itBegin++; + } + while (itEnd->first <= toTimestamp && itEnd != telecommandMap.end()) { + itEnd++; + } + break; + } + + default: + return RETURN_FAILED; + } + + // additional security check, this should never be true + if (itBegin->first > itEnd->first) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 +#else + sif::printError("11::GetMapFilterFromData: itBegin > itEnd\n"); +#endif + return RETURN_FAILED; + } + + // the map range should now be set according to the sent filter. + return RETURN_OK; +} + +template +inline ReturnValue_t Service11TelecommandScheduling::handleInvalidData( + const char *ctx) { +#if FSFW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "Service11TelecommandScheduling:: " << ctx << ": Invalid buffer" << std::endl; +#else + sif::printWarning("Service11TelecommandScheduling::%s: Invalid buffer\n", ctx); +#endif +#endif + return RETURN_FAILED; +} + +template +inline void Service11TelecommandScheduling::debugPrintMultimapContent() const { +#if FSFW_DISABLE_PRINTOUT == 0 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::debug << "Service11TelecommandScheduling::debugPrintMultimapContent: Multimap Content" + << std::endl; +#else + sif::printDebug("Service11TelecommandScheduling::debugPrintMultimapContent: Multimap Content\n"); +#endif + for (const auto &dit : telecommandMap) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::debug << "[" << dit.first << "]: Request ID: " << dit.second.requestId << " | " + << "Store Address: " << dit.second.storeAddr.raw << std::endl; +#else + sif::printDebug("[%d]: Request ID: %d | Store Address: %d\n", dit.first, dit.second.requestId, + dit.second.storeAddr); +#endif + } +#endif +} + +template +inline void Service11TelecommandScheduling::enableExpiredTcDeletion() { + deleteExpiredTcWhenDisabled = true; +} + +template +inline void Service11TelecommandScheduling::disableExpiredTcDeletion() { + deleteExpiredTcWhenDisabled = false; +} diff --git a/src/fsfw/pus/Service3Housekeeping.h b/src/fsfw/pus/Service3Housekeeping.h index c1928891e..70f157625 100644 --- a/src/fsfw/pus/Service3Housekeeping.h +++ b/src/fsfw/pus/Service3Housekeeping.h @@ -43,7 +43,7 @@ class Service3Housekeeping : public CommandingServiceBase, public AcceptsHkPacke CommandMessage* optionalNextCommand, object_id_t objectId, bool* isStep) override; - virtual MessageQueueId_t getHkQueue() const; + virtual MessageQueueId_t getHkQueue() const override; private: enum class Subservice { diff --git a/src/fsfw/pus/Service5EventReporting.cpp b/src/fsfw/pus/Service5EventReporting.cpp index 4517bc262..987217dc7 100644 --- a/src/fsfw/pus/Service5EventReporting.cpp +++ b/src/fsfw/pus/Service5EventReporting.cpp @@ -86,8 +86,8 @@ ReturnValue_t Service5EventReporting::handleRequest(uint8_t subservice) { // In addition to the default PUSServiceBase initialization, this service needs // to be registered to the event manager to listen for events. ReturnValue_t Service5EventReporting::initialize() { - EventManagerIF* manager = ObjectManager::instance()->get(objects::EVENT_MANAGER); - if (manager == NULL) { + auto* manager = ObjectManager::instance()->get(objects::EVENT_MANAGER); + if (manager == nullptr) { return RETURN_FAILED; } // register Service 5 as listener for events diff --git a/src/fsfw/returnvalues/FwClassIds.h b/src/fsfw/returnvalues/FwClassIds.h index d07cedc99..f5f57276e 100644 --- a/src/fsfw/returnvalues/FwClassIds.h +++ b/src/fsfw/returnvalues/FwClassIds.h @@ -72,6 +72,7 @@ enum : uint8_t { DLE_ENCODER, // DLEE PUS_SERVICE_3, // PUS3 PUS_SERVICE_9, // PUS9 + PUS_SERVICE_11, // PUS11 FILE_SYSTEM, // FILS LINUX_OSAL, // UXOS HAL_SPI, // HSPI diff --git a/src/fsfw/rmap/CMakeLists.txt b/src/fsfw/rmap/CMakeLists.txt index 78c99e420..441848608 100644 --- a/src/fsfw/rmap/CMakeLists.txt +++ b/src/fsfw/rmap/CMakeLists.txt @@ -1,7 +1,2 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - RMAP.cpp - RMAPCookie.cpp - RmapDeviceCommunicationIF.cpp -) - +target_sources(${LIB_FSFW_NAME} PRIVATE RMAP.cpp RMAPCookie.cpp + RmapDeviceCommunicationIF.cpp) diff --git a/src/fsfw/rmap/RMAP.h b/src/fsfw/rmap/RMAP.h index 42ee1ac5d..d274fb159 100644 --- a/src/fsfw/rmap/RMAP.h +++ b/src/fsfw/rmap/RMAP.h @@ -169,8 +169,8 @@ class RMAP : public HasReturnvaluesIF { * @param buffer the data to write * @param length length of data * @return - * - @c COMMAND_NULLPOINTER datalen was != 0 but data was == NULL in - * write command + * - @c COMMAND_NULLPOINTER datalen was != 0 but data was == + * NULL in write command * - return codes of RMAPChannelIF::sendCommand() */ static ReturnValue_t sendWriteCommand(RMAPCookie *cookie, const uint8_t *buffer, size_t length); @@ -205,8 +205,8 @@ class RMAP : public HasReturnvaluesIF { * @param cookie to cookie to read from * @param expLength the expected maximum length of the reply * @return - * - @c COMMAND_NULLPOINTER datalen was != 0 but data was == NULL in - * write command, or nullpointer in read command + * - @c COMMAND_NULLPOINTER datalen was != 0 but data was == + * NULL in write command, or nullpointer in read command * - return codes of RMAPChannelIF::sendCommand() */ static ReturnValue_t sendReadCommand(RMAPCookie *cookie, uint32_t expLength); diff --git a/src/fsfw/rmap/RMAPChannelIF.h b/src/fsfw/rmap/RMAPChannelIF.h index 20dfd5f80..9e666dfb6 100644 --- a/src/fsfw/rmap/RMAPChannelIF.h +++ b/src/fsfw/rmap/RMAPChannelIF.h @@ -75,11 +75,11 @@ class RMAPChannelIF { * - @c RETURN_OK * - @c COMMAND_NO_DESCRIPTORS_AVAILABLE no descriptors available for sending * command; command was not sent - * - @c COMMAND_BUFFER_FULL no receiver buffer available for expected len; - * command was not sent - * - @c COMMAND_TOO_BIG the data that was to be sent was too long for the hw - * to handle (write command) or the expected len was bigger than maximal expected len (read - * command) command was not sent + * - @c COMMAND_BUFFER_FULL no receiver buffer available for + * expected len; command was not sent + * - @c COMMAND_TOO_BIG the data that was to be sent was too + * long for the hw to handle (write command) or the expected len was bigger than maximal expected + * len (read command) command was not sent * - @c COMMAND_CHANNEL_DEACTIVATED the channel has no port set * - @c NOT_SUPPORTED if you dont feel like * implementing something... @@ -97,8 +97,8 @@ class RMAPChannelIF { * - @c REPLY_NO_REPLY no reply was received * - @c REPLY_NOT_SENT command was not sent, implies no reply * - @c REPLY_NOT_YET_SENT command is still waiting to be sent - * - @c WRITE_REPLY_INTERFACE_BUSY Interface is busy (transmission buffer still - * being processed) + * - @c WRITE_REPLY_INTERFACE_BUSY Interface is busy (transmission + * buffer still being processed) * - @c WRITE_REPLY_TRANSMISSION_ERROR Interface encountered errors during last * operation, data could not be processed. (transmission error) * - @c WRITE_REPLY_INVALID_DATA Invalid data (amount / value) diff --git a/src/fsfw/serialize/CMakeLists.txt b/src/fsfw/serialize/CMakeLists.txt index fc2387e80..5ac92d7f1 100644 --- a/src/fsfw/serialize/CMakeLists.txt +++ b/src/fsfw/serialize/CMakeLists.txt @@ -1,4 +1 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - SerialBufferAdapter.cpp -) \ No newline at end of file +target_sources(${LIB_FSFW_NAME} PRIVATE SerialBufferAdapter.cpp) diff --git a/src/fsfw/serviceinterface/CMakeLists.txt b/src/fsfw/serviceinterface/CMakeLists.txt index 84c791770..df3f074ec 100644 --- a/src/fsfw/serviceinterface/CMakeLists.txt +++ b/src/fsfw/serviceinterface/CMakeLists.txt @@ -1,5 +1,4 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - ServiceInterfaceStream.cpp - ServiceInterfaceBuffer.cpp - ServiceInterfacePrinter.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} + PRIVATE ServiceInterfaceStream.cpp ServiceInterfaceBuffer.cpp + ServiceInterfacePrinter.cpp) diff --git a/src/fsfw/serviceinterface/ServiceInterfacePrinter.h b/src/fsfw/serviceinterface/ServiceInterfacePrinter.h index 89d793c35..65f7250ab 100644 --- a/src/fsfw/serviceinterface/ServiceInterfacePrinter.h +++ b/src/fsfw/serviceinterface/ServiceInterfacePrinter.h @@ -1,6 +1,8 @@ #ifndef FSFW_SERVICEINTERFACE_SERVICEINTERFACEPRINTER #define FSFW_SERVICEINTERFACE_SERVICEINTERFACEPRINTER +#include "fsfw/FSFW.h" + #if FSFW_DISABLE_PRINTOUT == 0 #include #endif diff --git a/src/fsfw/storagemanager/CMakeLists.txt b/src/fsfw/storagemanager/CMakeLists.txt index b8138cae7..50ce50ed1 100644 --- a/src/fsfw/storagemanager/CMakeLists.txt +++ b/src/fsfw/storagemanager/CMakeLists.txt @@ -1,7 +1,3 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - ConstStorageAccessor.cpp - StorageAccessor.cpp - LocalPool.cpp - PoolManager.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} PRIVATE ConstStorageAccessor.cpp StorageAccessor.cpp + LocalPool.cpp PoolManager.cpp) diff --git a/src/fsfw/storagemanager/LocalPool.cpp b/src/fsfw/storagemanager/LocalPool.cpp index 075de4b82..cee1d4070 100644 --- a/src/fsfw/storagemanager/LocalPool.cpp +++ b/src/fsfw/storagemanager/LocalPool.cpp @@ -110,7 +110,7 @@ ReturnValue_t LocalPool::modifyData(store_address_t storeId, uint8_t** packetPtr } ReturnValue_t LocalPool::deleteData(store_address_t storeId) { -#if FSFW_VERBOSE_PRINTOUT == 2 +#if FSFW_VERBOSE_LEVEL >= 2 #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "Delete: Pool: " << std::dec << storeId.poolIndex << " Index: " << storeId.packetIndex << std::endl; @@ -148,7 +148,7 @@ ReturnValue_t LocalPool::deleteData(uint8_t* ptr, size_t size, store_address_t* // element of an object. localId.packetIndex = deltaAddress / elementSizes[n]; result = deleteData(localId); -#if FSFW_VERBOSE_PRINTOUT == 2 +#if FSFW_VERBOSE_LEVEL >= 2 if (deltaAddress % elementSizes[n] != 0) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "LocalPool::deleteData: Address not aligned!" << std::endl; @@ -219,7 +219,7 @@ ReturnValue_t LocalPool::reserveSpace(const size_t size, store_address_t* storeI status = findEmpty(storeId->poolIndex, &storeId->packetIndex); } if (status == RETURN_OK) { -#if FSFW_VERBOSE_PRINTOUT == 2 +#if FSFW_VERBOSE_LEVEL >= 2 #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "Reserve: Pool: " << std::dec << storeId->poolIndex << " Index: " << storeId->packetIndex << std::endl; @@ -257,7 +257,7 @@ void LocalPool::setToSpillToHigherPools(bool enable) { this->spillsToHigherPools ReturnValue_t LocalPool::getSubPoolIndex(size_t packetSize, uint16_t* subpoolIndex, uint16_t startAtIndex) { for (uint16_t n = startAtIndex; n < NUMBER_OF_SUBPOOLS; n++) { -#if FSFW_VERBOSE_PRINTOUT == 2 +#if FSFW_VERBOSE_LEVEL >= 2 #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "LocalPool " << getObjectId() << "::getPoolIndex: Pool: " << n << ", Element Size: " << elementSizes[n] << std::endl; diff --git a/src/fsfw/storagemanager/PoolManager.cpp b/src/fsfw/storagemanager/PoolManager.cpp index 6ff003609..92de1dfe0 100644 --- a/src/fsfw/storagemanager/PoolManager.cpp +++ b/src/fsfw/storagemanager/PoolManager.cpp @@ -17,7 +17,7 @@ ReturnValue_t PoolManager::reserveSpace(const size_t size, store_address_t* addr } ReturnValue_t PoolManager::deleteData(store_address_t storeId) { -#if FSFW_VERBOSE_PRINTOUT == 2 +#if FSFW_VERBOSE_LEVEL >= 2 #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "PoolManager( " << translateObject(getObjectId()) << " )::deleteData from store " << storeId.poolIndex << ". id is " << storeId.packetIndex << std::endl; diff --git a/src/fsfw/storagemanager/StorageAccessor.cpp b/src/fsfw/storagemanager/StorageAccessor.cpp index e2b083b21..b8096c1e9 100644 --- a/src/fsfw/storagemanager/StorageAccessor.cpp +++ b/src/fsfw/storagemanager/StorageAccessor.cpp @@ -13,7 +13,7 @@ StorageAccessor::StorageAccessor(store_address_t storeId, StorageManagerIF* stor StorageAccessor& StorageAccessor::operator=(StorageAccessor&& other) { // Call the parent move assignment and also assign own member. dataPointer = other.dataPointer; - StorageAccessor::operator=(std::move(other)); + ConstStorageAccessor::operator=(std::move(other)); return *this; } diff --git a/src/fsfw/subsystem/CMakeLists.txt b/src/fsfw/subsystem/CMakeLists.txt index 5c98ee707..164c90f7e 100644 --- a/src/fsfw/subsystem/CMakeLists.txt +++ b/src/fsfw/subsystem/CMakeLists.txt @@ -1,7 +1,3 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - Subsystem.cpp - SubsystemBase.cpp -) +target_sources(${LIB_FSFW_NAME} PRIVATE Subsystem.cpp SubsystemBase.cpp) -add_subdirectory(modes) \ No newline at end of file +add_subdirectory(modes) diff --git a/src/fsfw/subsystem/Subsystem.cpp b/src/fsfw/subsystem/Subsystem.cpp index a837bf83e..27e6ae8e0 100644 --- a/src/fsfw/subsystem/Subsystem.cpp +++ b/src/fsfw/subsystem/Subsystem.cpp @@ -30,11 +30,11 @@ ReturnValue_t Subsystem::checkSequence(HybridIterator iter, return FALLBACK_SEQUENCE_DOES_NOT_EXIST; } - if (iter.value == NULL) { + if (iter.value == nullptr) { return NO_TARGET_TABLE; } - for (; iter.value != NULL; ++iter) { + for (; iter.value != nullptr; ++iter) { if (!existsModeTable(iter->getTableId())) { return TABLE_DOES_NOT_EXIST; } else { @@ -66,13 +66,18 @@ HybridIterator Subsystem::getCurrentTable() { void Subsystem::performChildOperation() { if (isInTransition) { if (commandsOutstanding <= 0) { // all children of the current table were commanded and replied - if (currentSequenceIterator.value == NULL) { // we're through with this sequence + if (currentSequenceIterator.value == nullptr) { // we're through with this sequence if (checkStateAgainstTable(currentTargetTable, targetSubmode) == RETURN_OK) { setMode(targetMode, targetSubmode); isInTransition = false; return; } else { - transitionFailed(TARGET_TABLE_NOT_REACHED, getSequence(targetMode)->getTableId()); + Mode_t tableId = 0; + auto seq = getSequence(targetMode); + if (seq.value != nullptr) { + tableId = seq->getTableId(); + } + transitionFailed(TARGET_TABLE_NOT_REACHED, tableId); return; } } @@ -248,10 +253,13 @@ ReturnValue_t Subsystem::handleCommandMessage(CommandMessage *message) { case ModeSequenceMessage::READ_TABLE: { ReturnValue_t result; Mode_t table = ModeSequenceMessage::getSequenceId(message); - EntryPointer *entry = NULL; + EntryPointer *entry = nullptr; result = modeTables.find(table, &entry); - if (result != RETURN_OK) { + if (result != RETURN_OK or entry == nullptr) { replyToCommand(result, 0); + if (entry == nullptr) { + return result; + } } SerializeIF *elements[2]; @@ -299,6 +307,11 @@ void Subsystem::replyToCommand(ReturnValue_t status, uint32_t parameter) { } } +ReturnValue_t Subsystem::addSequence(SequenceEntry sequence) { + return addSequence(sequence.table, sequence.mode, sequence.fallbackMode, sequence.inStore, + sequence.preInit); +} + ReturnValue_t Subsystem::addSequence(ArrayList *sequence, Mode_t id, Mode_t fallbackSequence, bool inStore, bool preInit) { ReturnValue_t result; @@ -342,6 +355,10 @@ ReturnValue_t Subsystem::addSequence(ArrayList *sequence, Mode_t return result; } +ReturnValue_t Subsystem::addTable(TableEntry table) { + return addTable(table.table, table.mode, table.inStore, table.preInit); +} + ReturnValue_t Subsystem::addTable(ArrayList *table, Mode_t id, bool inStore, bool preInit) { ReturnValue_t result; @@ -450,6 +467,7 @@ ReturnValue_t Subsystem::initialize() { } mode = initialMode; + submode = initSubmode; return RETURN_OK; } @@ -587,7 +605,10 @@ ReturnValue_t Subsystem::checkObjectConnections() { return RETURN_OK; } -void Subsystem::setInitialMode(Mode_t mode) { initialMode = mode; } +void Subsystem::setInitialMode(Mode_t mode, Submode_t submode) { + this->initialMode = mode; + this->initSubmode = submode; +} void Subsystem::cantKeepMode() { ReturnValue_t result; diff --git a/src/fsfw/subsystem/Subsystem.h b/src/fsfw/subsystem/Subsystem.h index 2c78c8cd3..855c8f7ab 100644 --- a/src/fsfw/subsystem/Subsystem.h +++ b/src/fsfw/subsystem/Subsystem.h @@ -1,16 +1,37 @@ #ifndef FSFW_SUBSYSTEM_SUBSYSTEM_H_ #define FSFW_SUBSYSTEM_SUBSYSTEM_H_ -#include - #include "../container/FixedArrayList.h" #include "../container/FixedMap.h" #include "../container/HybridIterator.h" #include "../container/SinglyLinkedList.h" #include "../serialize/SerialArrayListAdapter.h" #include "SubsystemBase.h" +#include "fsfw/FSFW.h" #include "modes/ModeDefinitions.h" +struct TableSequenceBase { + public: + TableSequenceBase(Mode_t mode, ArrayList *table) : mode(mode), table(table){}; + Mode_t mode; + ArrayList *table; + bool inStore = false; + bool preInit = true; +}; + +struct TableEntry : public TableSequenceBase { + public: + TableEntry(Mode_t mode, ArrayList *table) : TableSequenceBase(mode, table){}; +}; + +struct SequenceEntry : public TableSequenceBase { + public: + SequenceEntry(Mode_t mode, ArrayList *table, Mode_t fallbackMode) + : TableSequenceBase(mode, table), fallbackMode(fallbackMode) {} + + Mode_t fallbackMode; +}; + /** * @brief TODO: documentation missing * @details @@ -45,13 +66,15 @@ class Subsystem : public SubsystemBase, public HasModeSequenceIF { uint32_t maxNumberOfTables); virtual ~Subsystem(); + ReturnValue_t addSequence(SequenceEntry sequence); ReturnValue_t addSequence(ArrayList *sequence, Mode_t id, Mode_t fallbackSequence, bool inStore = true, bool preInit = true); + ReturnValue_t addTable(TableEntry table); ReturnValue_t addTable(ArrayList *table, Mode_t id, bool inStore = true, bool preInit = true); - void setInitialMode(Mode_t mode); + void setInitialMode(Mode_t mode, Submode_t submode = SUBMODE_NONE); virtual ReturnValue_t initialize() override; @@ -90,6 +113,7 @@ class Subsystem : public SubsystemBase, public HasModeSequenceIF { Submode_t targetSubmode; Mode_t initialMode = 0; + Submode_t initSubmode = SUBMODE_NONE; HybridIterator currentSequenceIterator; @@ -127,18 +151,18 @@ class Subsystem : public SubsystemBase, public HasModeSequenceIF { ReturnValue_t deleteTable(Mode_t id); - virtual void performChildOperation(); + virtual void performChildOperation() override; - virtual ReturnValue_t handleCommandMessage(CommandMessage *message); + virtual ReturnValue_t handleCommandMessage(CommandMessage *message) override; bool isFallbackSequence(Mode_t SequenceId); bool isTableUsed(Mode_t tableId); virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, - uint32_t *msToReachTheMode); + uint32_t *msToReachTheMode) override; - virtual void startTransition(Mode_t mode, Submode_t submode); + virtual void startTransition(Mode_t mode, Submode_t submode) override; void sendSerializablesAsCommandMessage(Command_t command, SerializeIF **elements, uint8_t count); diff --git a/src/fsfw/subsystem/SubsystemBase.h b/src/fsfw/subsystem/SubsystemBase.h index 8cfd5be07..52f9891e0 100644 --- a/src/fsfw/subsystem/SubsystemBase.h +++ b/src/fsfw/subsystem/SubsystemBase.h @@ -123,15 +123,15 @@ class SubsystemBase : public SystemObject, virtual void performChildOperation() = 0; virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, - uint32_t *msToReachTheMode) = 0; + uint32_t *msToReachTheMode) override = 0; - virtual void startTransition(Mode_t mode, Submode_t submode) = 0; + virtual void startTransition(Mode_t mode, Submode_t submode) override = 0; - virtual void getMode(Mode_t *mode, Submode_t *submode); + virtual void getMode(Mode_t *mode, Submode_t *submode) override; - virtual void setToExternalControl(); + virtual void setToExternalControl() override; - virtual void announceMode(bool recursive); + virtual void announceMode(bool recursive) override; virtual void modeChanged(); }; diff --git a/src/fsfw/subsystem/modes/CMakeLists.txt b/src/fsfw/subsystem/modes/CMakeLists.txt index 6ac6a293a..ba57de2c3 100644 --- a/src/fsfw/subsystem/modes/CMakeLists.txt +++ b/src/fsfw/subsystem/modes/CMakeLists.txt @@ -1,5 +1 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - ModeSequenceMessage.cpp - ModeStore.cpp -) +target_sources(${LIB_FSFW_NAME} PRIVATE ModeSequenceMessage.cpp ModeStore.cpp) diff --git a/src/fsfw/tasks/CMakeLists.txt b/src/fsfw/tasks/CMakeLists.txt index 1964bb4e6..1d4ab4e13 100644 --- a/src/fsfw/tasks/CMakeLists.txt +++ b/src/fsfw/tasks/CMakeLists.txt @@ -1,5 +1,3 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - FixedSequenceSlot.cpp - FixedSlotSequence.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} PRIVATE FixedSequenceSlot.cpp FixedSlotSequence.cpp + PeriodicTaskBase.cpp FixedTimeslotTaskBase.cpp) diff --git a/src/fsfw/tasks/FixedSlotSequence.cpp b/src/fsfw/tasks/FixedSlotSequence.cpp index d4c67b4da..9305b4416 100644 --- a/src/fsfw/tasks/FixedSlotSequence.cpp +++ b/src/fsfw/tasks/FixedSlotSequence.cpp @@ -29,7 +29,7 @@ void FixedSlotSequence::executeAndAdvance() { uint32_t FixedSlotSequence::getIntervalToNextSlotMs() { uint32_t oldTime; - SlotListIter slotListIter = current; + auto slotListIter = current; // Get the pollingTimeMs of the current slot object. oldTime = slotListIter->pollingTimeMs; // Advance to the next object. @@ -51,7 +51,7 @@ uint32_t FixedSlotSequence::getIntervalToNextSlotMs() { uint32_t FixedSlotSequence::getIntervalToPreviousSlotMs() { uint32_t currentTime; - SlotListIter slotListIter = current; + auto slotListIter = current; // Get the pollingTimeMs of the current slot object. currentTime = slotListIter->pollingTimeMs; @@ -67,7 +67,7 @@ uint32_t FixedSlotSequence::getIntervalToPreviousSlotMs() { bool FixedSlotSequence::slotFollowsImmediately() { uint32_t currentTime = current->pollingTimeMs; - SlotListIter fixedSequenceIter = this->current; + auto fixedSequenceIter = this->current; // Get the pollingTimeMs of the current slot object. if (fixedSequenceIter == slotList.begin()) return false; fixedSequenceIter--; @@ -96,8 +96,8 @@ ReturnValue_t FixedSlotSequence::checkSequence() const { return FixedTimeslotTaskIF::SLOT_LIST_EMPTY; } - if (customCheckFunction != nullptr) { - ReturnValue_t result = customCheckFunction(slotList); + if (customChecker != nullptr) { + ReturnValue_t result = customChecker(slotList, customCheckArgs); if (result != HasReturnvaluesIF::RETURN_OK) { // Continue for now but print error output. #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -161,6 +161,9 @@ ReturnValue_t FixedSlotSequence::intializeSequenceAfterTaskCreation() const { return HasReturnvaluesIF::RETURN_OK; } -void FixedSlotSequence::addCustomCheck(ReturnValue_t (*customCheckFunction)(const SlotList&)) { - this->customCheckFunction = customCheckFunction; +void FixedSlotSequence::addCustomCheck(CustomCheckFunc customChecker_, void* checkerArgs_) { + customChecker = customChecker_; + customCheckArgs = checkerArgs_; } + +bool FixedSlotSequence::isEmpty() const { return slotList.empty(); } diff --git a/src/fsfw/tasks/FixedSlotSequence.h b/src/fsfw/tasks/FixedSlotSequence.h index a287c5b22..2e38ee4b4 100644 --- a/src/fsfw/tasks/FixedSlotSequence.h +++ b/src/fsfw/tasks/FixedSlotSequence.h @@ -30,12 +30,12 @@ class FixedSlotSequence { public: using SlotList = std::multiset; using SlotListIter = std::multiset::iterator; - + using CustomCheckFunc = ReturnValue_t (*)(const SlotList&, void* args); /** * @brief The constructor of the FixedSlotSequence object. * @param setLength The period length, expressed in ms. */ - FixedSlotSequence(uint32_t setLengthMs); + explicit FixedSlotSequence(uint32_t setLengthMs); /** * @brief The destructor of the FixedSlotSequence object. @@ -106,7 +106,7 @@ class FixedSlotSequence { /** * @brief This method returns the length of this FixedSlotSequence instance. */ - uint32_t getLengthMs() const; + [[nodiscard]] uint32_t getLengthMs() const; /** * @brief The method to execute the device handler entered in the current @@ -137,7 +137,7 @@ class FixedSlotSequence { * @return * - SLOT_LIST_EMPTY if the slot list is empty */ - ReturnValue_t checkSequence() const; + [[nodiscard]] ReturnValue_t checkSequence() const; /** * @brief A custom check can be injected for the respective slot list. @@ -149,7 +149,7 @@ class FixedSlotSequence { * @param customCheckFunction * */ - void addCustomCheck(ReturnValue_t (*customCheckFunction)(const SlotList&)); + void addCustomCheck(CustomCheckFunc func, void* userArgs); /** * @brief Perform any initialization steps required after the executing @@ -157,7 +157,9 @@ class FixedSlotSequence { * executing task! * @return */ - ReturnValue_t intializeSequenceAfterTaskCreation() const; + [[nodiscard]] ReturnValue_t intializeSequenceAfterTaskCreation() const; + + [[nodiscard]] bool isEmpty() const; protected: /** @@ -173,7 +175,8 @@ class FixedSlotSequence { */ SlotList slotList; - ReturnValue_t (*customCheckFunction)(const SlotList&) = nullptr; + CustomCheckFunc customChecker = nullptr; + void* customCheckArgs = nullptr; uint32_t lengthMs; }; diff --git a/src/fsfw/tasks/FixedTimeslotTaskBase.cpp b/src/fsfw/tasks/FixedTimeslotTaskBase.cpp new file mode 100644 index 000000000..5d12d5657 --- /dev/null +++ b/src/fsfw/tasks/FixedTimeslotTaskBase.cpp @@ -0,0 +1,27 @@ +#include "FixedTimeslotTaskBase.h" + +#include "fsfw/objectmanager/ObjectManager.h" + +FixedTimeslotTaskBase::FixedTimeslotTaskBase(TaskPeriod period_, + TaskDeadlineMissedFunction dlmFunc_) + : period(period_), pollingSeqTable(getPeriodMs()), dlmFunc(dlmFunc_) {} +uint32_t FixedTimeslotTaskBase::getPeriodMs() const { return static_cast(period * 1000); } + +bool FixedTimeslotTaskBase::isEmpty() const { return pollingSeqTable.isEmpty(); } + +ReturnValue_t FixedTimeslotTaskBase::checkSequence() { return pollingSeqTable.checkSequence(); } + +ReturnValue_t FixedTimeslotTaskBase::addSlot(object_id_t execId, ExecutableObjectIF* execObj, + uint32_t slotTimeMs, int8_t executionStep) { + if (execObj == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "Component 0x" << std::hex << std::setw(8) << std::setfill('0') << execObj + << std::setfill(' ') << " not found, not adding it to PST" << std::dec << std::endl; +#else + sif::printError("Component 0x%08x not found, not adding it to PST\n"); +#endif + return HasReturnvaluesIF::RETURN_FAILED; + } + pollingSeqTable.addSlot(execId, slotTimeMs, executionStep, execObj, this); + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/src/fsfw/tasks/FixedTimeslotTaskBase.h b/src/fsfw/tasks/FixedTimeslotTaskBase.h new file mode 100644 index 000000000..b88b393c7 --- /dev/null +++ b/src/fsfw/tasks/FixedTimeslotTaskBase.h @@ -0,0 +1,44 @@ +#ifndef FSFW_EXAMPLE_HOSTED_FIXEDTIMESLOTTASKBASE_H +#define FSFW_EXAMPLE_HOSTED_FIXEDTIMESLOTTASKBASE_H + +#include "FixedSlotSequence.h" +#include "FixedTimeslotTaskIF.h" +#include "definitions.h" + +class FixedTimeslotTaskBase : public FixedTimeslotTaskIF { + public: + explicit FixedTimeslotTaskBase(TaskPeriod period, TaskDeadlineMissedFunction dlmFunc = nullptr); + ~FixedTimeslotTaskBase() override = default; + ; + + protected: + /** + * @brief Period of task in floating point seconds + */ + TaskPeriod period; + + //! Polling sequence table which contains the object to execute + //! and information like the timeslots and the passed execution step. + FixedSlotSequence pollingSeqTable; + + /** + * @brief The pointer to the deadline-missed function. + * @details + * This pointer stores the function that is executed if the task's deadline + * is missed. So, each may react individually on a timing failure. + * The pointer may be NULL, then nothing happens on missing the deadline. + * The deadline is equal to the next execution of the periodic task. + */ + TaskDeadlineMissedFunction dlmFunc = nullptr; + + ReturnValue_t checkSequence() override; + + [[nodiscard]] uint32_t getPeriodMs() const override; + + [[nodiscard]] bool isEmpty() const override; + + ReturnValue_t addSlot(object_id_t execId, ExecutableObjectIF* componentId, uint32_t slotTimeMs, + int8_t executionStep) override; +}; + +#endif // FSFW_EXAMPLE_HOSTED_FIXEDTIMESLOTTASKBASE_H diff --git a/src/fsfw/tasks/FixedTimeslotTaskIF.h b/src/fsfw/tasks/FixedTimeslotTaskIF.h index 497db2452..3f5e30d81 100644 --- a/src/fsfw/tasks/FixedTimeslotTaskIF.h +++ b/src/fsfw/tasks/FixedTimeslotTaskIF.h @@ -2,6 +2,7 @@ #define FRAMEWORK_TASKS_FIXEDTIMESLOTTASKIF_H_ #include "PeriodicTaskIF.h" +#include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/objectmanager/ObjectManagerIF.h" #include "fsfw/returnvalues/FwClassIds.h" @@ -11,10 +12,23 @@ */ class FixedTimeslotTaskIF : public PeriodicTaskIF { public: - virtual ~FixedTimeslotTaskIF() {} + ~FixedTimeslotTaskIF() override = default; static constexpr ReturnValue_t SLOT_LIST_EMPTY = HasReturnvaluesIF::makeReturnCode(CLASS_ID::FIXED_SLOT_TASK_IF, 0); + + /** + * Add an object with a slot time and the execution step to the task. + * The execution step will be passed to the object (e.g. as an operation + * code in #performOperation) + * @param componentId + * @param slotTimeMs + * @param executionStep + * @return + */ + virtual ReturnValue_t addSlot(object_id_t execId, ExecutableObjectIF* obj, uint32_t slotTimeMs, + int8_t executionStep) = 0; + /** * Add an object with a slot time and the execution step to the task. * The execution step will be passed to the object (e.g. as an operation @@ -25,12 +39,24 @@ class FixedTimeslotTaskIF : public PeriodicTaskIF { * @return */ virtual ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, - int8_t executionStep) = 0; + int8_t executionStep) { + auto* execObj = ObjectManager::instance()->get(componentId); + return addSlot(componentId, execObj, slotTimeMs, executionStep); + } + /** * Check whether the sequence is valid and perform all other required * initialization steps which are needed after task creation */ - virtual ReturnValue_t checkSequence() const = 0; + virtual ReturnValue_t checkSequence() = 0; + + ReturnValue_t addComponent(object_id_t object, uint8_t opCode) override { + return HasReturnvaluesIF::RETURN_FAILED; + } + + ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode) override { + return HasReturnvaluesIF::RETURN_FAILED; + } }; #endif /* FRAMEWORK_TASKS_FIXEDTIMESLOTTASKIF_H_ */ diff --git a/src/fsfw/tasks/PeriodicTaskBase.cpp b/src/fsfw/tasks/PeriodicTaskBase.cpp new file mode 100644 index 000000000..ce925a458 --- /dev/null +++ b/src/fsfw/tasks/PeriodicTaskBase.cpp @@ -0,0 +1,71 @@ +#include "PeriodicTaskBase.h" + +#include + +#include "fsfw/objectmanager/ObjectManager.h" +#include "fsfw/serviceinterface.h" + +PeriodicTaskBase::PeriodicTaskBase(TaskPeriod period_, TaskDeadlineMissedFunction dlmFunc_) + : period(period_), dlmFunc(dlmFunc_) { + // Hints at configuration error + if (PeriodicTaskBase::getPeriodMs() <= 1) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "Passed task period 0 or smaller than 1 ms" << std::endl; +#else + sif::printWarning("Passed task period 0 or smaller than 1ms\n"); +#endif + } +} + +uint32_t PeriodicTaskBase::getPeriodMs() const { return static_cast(period * 1000); } + +bool PeriodicTaskBase::isEmpty() const { return objectList.empty(); } + +ReturnValue_t PeriodicTaskBase::addComponent(object_id_t object) { return addComponent(object, 0); } + +ReturnValue_t PeriodicTaskBase::addComponent(ExecutableObjectIF* object) { + return addComponent(object, 0); +} + +ReturnValue_t PeriodicTaskBase::initObjsAfterTaskCreation() { + std::set uniqueObjects; + ReturnValue_t status = HasReturnvaluesIF::RETURN_OK; + uint32_t count = 0; + for (const auto& obj : objectList) { + // Ensure that each unique object is initialized once. + if (uniqueObjects.find(obj.first) == uniqueObjects.end()) { + ReturnValue_t result = obj.first->initializeAfterTaskCreation(); + if (result != HasReturnvaluesIF::RETURN_OK) { + count++; + status = result; + } + uniqueObjects.emplace(obj.first); + } + } + if (count > 0) { + } + return status; +} + +ReturnValue_t PeriodicTaskBase::addComponent(object_id_t object, uint8_t opCode) { + auto* newObject = ObjectManager::instance()->get(object); + return addComponent(newObject, opCode); +} + +ReturnValue_t PeriodicTaskBase::addComponent(ExecutableObjectIF* object, uint8_t opCode) { + if (object == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" + << " it implements ExecutableObjectIF!" << std::endl; +#else + sif::printError( + "PeriodicTask::addComponent: Invalid object. Make sure it " + "implements ExecutableObjectIF!\n"); +#endif + return HasReturnvaluesIF::RETURN_FAILED; + } + objectList.push_back({object, opCode}); + object->setTaskIF(this); + + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/src/fsfw/tasks/PeriodicTaskBase.h b/src/fsfw/tasks/PeriodicTaskBase.h new file mode 100644 index 000000000..9023f104d --- /dev/null +++ b/src/fsfw/tasks/PeriodicTaskBase.h @@ -0,0 +1,54 @@ +#ifndef FSFW_SRC_FSFW_TASKS_PERIODICTASKBASE_H_ +#define FSFW_SRC_FSFW_TASKS_PERIODICTASKBASE_H_ + +#include +#include + +#include "fsfw/tasks/PeriodicTaskIF.h" +#include "fsfw/tasks/definitions.h" + +class ExecutableObjectIF; + +class PeriodicTaskBase : public PeriodicTaskIF { + public: + explicit PeriodicTaskBase(TaskPeriod period, + TaskDeadlineMissedFunction deadlineMissedFunc = nullptr); + + ReturnValue_t addComponent(object_id_t object, uint8_t opCode) override; + ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode) override; + + ReturnValue_t addComponent(object_id_t object) override; + ReturnValue_t addComponent(ExecutableObjectIF* object) override; + + [[nodiscard]] uint32_t getPeriodMs() const override; + + [[nodiscard]] bool isEmpty() const override; + + ReturnValue_t initObjsAfterTaskCreation(); + + protected: + //! Typedef for the List of objects. Will contain the objects to execute and their respective + //! operation codes + using ObjectList = std::vector>; + /** + * @brief This attribute holds a list of objects to be executed. + */ + ObjectList objectList; + + /** + * @brief Period of task in floating point seconds + */ + TaskPeriod period; + + /** + * @brief The pointer to the deadline-missed function. + * @details + * This pointer stores the function that is executed if the task's deadline + * is missed. So, each may react individually on a timing failure. + * The pointer may be NULL, then nothing happens on missing the deadline. + * The deadline is equal to the next execution of the periodic task. + */ + TaskDeadlineMissedFunction dlmFunc = nullptr; +}; + +#endif /* FSFW_SRC_FSFW_TASKS_PERIODICTASKBASE_H_ */ diff --git a/src/fsfw/tasks/PeriodicTaskIF.h b/src/fsfw/tasks/PeriodicTaskIF.h index a8a512296..03b709ab9 100644 --- a/src/fsfw/tasks/PeriodicTaskIF.h +++ b/src/fsfw/tasks/PeriodicTaskIF.h @@ -3,9 +3,8 @@ #include -#include "../objectmanager/SystemObjectIF.h" -#include "../timemanager/Clock.h" -class ExecutableObjectIF; +#include "fsfw/objectmanager/SystemObjectIF.h" +#include "fsfw/tasks/ExecutableObjectIF.h" /** * New version of TaskIF @@ -18,7 +17,7 @@ class PeriodicTaskIF { /** * @brief A virtual destructor as it is mandatory for interfaces. */ - virtual ~PeriodicTaskIF() {} + virtual ~PeriodicTaskIF() = default; /** * @brief With the startTask method, a created task can be started * for the first time. @@ -26,24 +25,29 @@ class PeriodicTaskIF { virtual ReturnValue_t startTask() = 0; /** - * Add a component (object) to a periodic task. The pointer to the - * task can be set optionally - * @param object - * Add an object to the task. The most important case is to add an - * executable object with a function which will be called regularly - * (see ExecutableObjectIF) - * @param setTaskIF - * Can be used to specify whether the task object pointer is passed - * to the component. - * @return + * Adds an object to the list of objects to be executed. + * The objects are executed in the order added. The object needs to implement + * ExecutableObjectIF + * @param object Id of the object to add. + * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. */ - virtual ReturnValue_t addComponent(object_id_t object) { - return HasReturnvaluesIF::RETURN_FAILED; - }; + virtual ReturnValue_t addComponent(object_id_t object, uint8_t opCode) = 0; + virtual ReturnValue_t addComponent(object_id_t object) { return addComponent(object, 0); }; + + /** + * Adds an object to the list of objects to be executed. + * The objects are executed in the order added. + * @param object pointer to the object to add. + * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. + */ + virtual ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode) = 0; + virtual ReturnValue_t addComponent(ExecutableObjectIF* object) { return addComponent(object, 0); } virtual ReturnValue_t sleepFor(uint32_t ms) = 0; - virtual uint32_t getPeriodMs() const = 0; + [[nodiscard]] virtual uint32_t getPeriodMs() const = 0; + + [[nodiscard]] virtual bool isEmpty() const = 0; }; -#endif /* PERIODICTASKIF_H_ */ +#endif /* FRAMEWORK_TASK_PERIODICTASKIF_H_ */ diff --git a/src/fsfw/tasks/TaskFactory.h b/src/fsfw/tasks/TaskFactory.h index fcd62678c..828c533e3 100644 --- a/src/fsfw/tasks/TaskFactory.h +++ b/src/fsfw/tasks/TaskFactory.h @@ -4,7 +4,7 @@ #include #include "FixedTimeslotTaskIF.h" -#include "Typedef.h" +#include "definitions.h" /** * Singleton Class that produces Tasks. diff --git a/src/fsfw/tasks/Typedef.h b/src/fsfw/tasks/Typedef.h deleted file mode 100644 index 1bb751312..000000000 --- a/src/fsfw/tasks/Typedef.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef FSFW_TASKS_TYPEDEF_H_ -#define FSFW_TASKS_TYPEDEF_H_ - -#include -#include - -typedef const char* TaskName; -typedef uint32_t TaskPriority; -typedef size_t TaskStackSize; -typedef double TaskPeriod; -typedef void (*TaskDeadlineMissedFunction)(); - -#endif /* FSFW_TASKS_TYPEDEF_H_ */ diff --git a/src/fsfw/tasks/definitions.h b/src/fsfw/tasks/definitions.h new file mode 100644 index 000000000..586884b6e --- /dev/null +++ b/src/fsfw/tasks/definitions.h @@ -0,0 +1,13 @@ +#ifndef FSFW_TASKS_TYPEDEF_H_ +#define FSFW_TASKS_TYPEDEF_H_ + +#include +#include + +using TaskName = const char*; +using TaskPriority = int; +using TaskStackSize = size_t; +using TaskPeriod = double; +using TaskDeadlineMissedFunction = void (*)(); + +#endif /* FSFW_TASKS_TYPEDEF_H_ */ diff --git a/src/fsfw/tcdistribution/CCSDSDistributor.cpp b/src/fsfw/tcdistribution/CCSDSDistributor.cpp index 3f4bbee34..628dd8d06 100644 --- a/src/fsfw/tcdistribution/CCSDSDistributor.cpp +++ b/src/fsfw/tcdistribution/CCSDSDistributor.cpp @@ -9,7 +9,7 @@ CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid, object_id_t setObjectId) : TcDistributor(setObjectId), defaultApid(setDefaultApid) {} -CCSDSDistributor::~CCSDSDistributor() {} +CCSDSDistributor::~CCSDSDistributor() = default; TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() { #if CCSDS_DISTRIBUTOR_DEBUGGING == 1 @@ -38,6 +38,7 @@ TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() { " store failed!\n"); #endif #endif + return queueMap.end(); } SpacePacketBase currentPacket(packet); @@ -45,7 +46,7 @@ TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() { sif::info << "CCSDSDistributor::selectDestination has packet with APID " << std::hex << currentPacket.getAPID() << std::dec << std::endl; #endif - TcMqMapIter position = this->queueMap.find(currentPacket.getAPID()); + auto position = this->queueMap.find(currentPacket.getAPID()); if (position != this->queueMap.end()) { return position; } else { diff --git a/src/fsfw/tcdistribution/CMakeLists.txt b/src/fsfw/tcdistribution/CMakeLists.txt index 7118c38cf..ab32c509d 100644 --- a/src/fsfw/tcdistribution/CMakeLists.txt +++ b/src/fsfw/tcdistribution/CMakeLists.txt @@ -1,9 +1,4 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - CCSDSDistributor.cpp - PUSDistributor.cpp - TcDistributor.cpp - TcPacketCheckPUS.cpp - TcPacketCheckCFDP.cpp - CFDPDistributor.cpp -) - +target_sources( + ${LIB_FSFW_NAME} + PRIVATE CCSDSDistributor.cpp PUSDistributor.cpp TcDistributor.cpp + TcPacketCheckPUS.cpp TcPacketCheckCFDP.cpp CFDPDistributor.cpp) diff --git a/src/fsfw/tcdistribution/PUSDistributor.cpp b/src/fsfw/tcdistribution/PUSDistributor.cpp index aadecd695..dad002a1a 100644 --- a/src/fsfw/tcdistribution/PUSDistributor.cpp +++ b/src/fsfw/tcdistribution/PUSDistributor.cpp @@ -15,7 +15,7 @@ PUSDistributor::PUSDistributor(uint16_t setApid, object_id_t setObjectId, tcStatus(RETURN_FAILED), packetSource(setPacketSource) {} -PUSDistributor::~PUSDistributor() {} +PUSDistributor::~PUSDistributor() = default; PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() { #if FSFW_CPP_OSTREAM_ENABLED == 1 && PUS_DISTRIBUTOR_DEBUGGING == 1 @@ -23,7 +23,7 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() { sif::debug << "PUSDistributor::handlePacket received: " << storeId.poolIndex << ", " << storeId.packetIndex << std::endl; #endif - TcMqMapIter queueMapIt = this->queueMap.end(); + auto queueMapIt = this->queueMap.end(); if (this->currentPacket == nullptr) { return queueMapIt; } @@ -48,10 +48,8 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() { sif::warning << "PUSDistributor::handlePacket: Packet format invalid, " << keyword << " error" << std::endl; #else - sif::printWarning( - "PUSDistributor::handlePacket: Packet format invalid, " - "%s error\n", - keyword); + sif::printWarning("PUSDistributor::handlePacket: Packet format invalid, %s error\n", + keyword); #endif #endif } @@ -133,8 +131,7 @@ ReturnValue_t PUSDistributor::initialize() { return ObjectManagerIF::CHILD_INIT_FAILED; } - CCSDSDistributorIF* ccsdsDistributor = - ObjectManager::instance()->get(packetSource); + auto* ccsdsDistributor = ObjectManager::instance()->get(packetSource); if (ccsdsDistributor == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "PUSDistributor::initialize: Packet source invalid" << std::endl; diff --git a/src/fsfw/thermal/CMakeLists.txt b/src/fsfw/thermal/CMakeLists.txt index ad5327214..995ebc4d3 100644 --- a/src/fsfw/thermal/CMakeLists.txt +++ b/src/fsfw/thermal/CMakeLists.txt @@ -1,10 +1,9 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - AbstractTemperatureSensor.cpp - Heater.cpp - RedundantHeater.cpp - ThermalComponentCore.cpp - ThermalComponent.cpp - ThermalModule.cpp - ThermalMonitorReporter.cpp -) +target_sources( + ${LIB_FSFW_NAME} + PRIVATE AbstractTemperatureSensor.cpp + Heater.cpp + RedundantHeater.cpp + ThermalComponentCore.cpp + ThermalComponent.cpp + ThermalModule.cpp + ThermalMonitorReporter.cpp) diff --git a/src/fsfw/timemanager/CCSDSTime.cpp b/src/fsfw/timemanager/CCSDSTime.cpp index 4445fd244..1f84dd03d 100644 --- a/src/fsfw/timemanager/CCSDSTime.cpp +++ b/src/fsfw/timemanager/CCSDSTime.cpp @@ -6,10 +6,6 @@ #include "fsfw/FSFW.h" -CCSDSTime::CCSDSTime() {} - -CCSDSTime::~CCSDSTime() {} - ReturnValue_t CCSDSTime::convertToCcsds(Ccs_seconds* to, const Clock::TimeOfDay_t* from) { ReturnValue_t result = checkTimeOfDay(from); if (result != RETURN_OK) { @@ -91,7 +87,7 @@ ReturnValue_t CCSDSTime::convertFromCDS(Clock::TimeOfDay_t* to, const uint8_t* f if (result != HasReturnvaluesIF::RETURN_OK) { return result; } - return convertTimevalToTimeOfDay(to, &time); + return Clock::convertTimevalToTimeOfDay(&time, to); } ReturnValue_t CCSDSTime::convertFromCCS(Clock::TimeOfDay_t* to, const uint8_t* from, @@ -109,8 +105,8 @@ ReturnValue_t CCSDSTime::convertFromCCS(Clock::TimeOfDay_t* to, const uint8_t* f if (result != RETURN_OK) { return result; } - - Ccs_mseconds* temp = (Ccs_mseconds*)from; + // At this point we made sure that this is a valid ccs time + const Ccs_mseconds* temp = reinterpret_cast(from); to->year = (temp->yearMSB << 8) + temp->yearLSB; to->hour = temp->hour; @@ -118,16 +114,19 @@ ReturnValue_t CCSDSTime::convertFromCCS(Clock::TimeOfDay_t* to, const uint8_t* f to->second = temp->second; if (temp->pField & (1 << 3)) { // day of year variation - uint16_t tempDay = (temp->month << 8) + temp->day; - result = convertDaysOfYear(tempDay, to->year, &(temp->month), &(temp->day)); + uint16_t tempDayOfYear = (temp->month << 8) + temp->day; + uint8_t tempDay = 0; + uint8_t tempMonth = 0; + result = convertDaysOfYear(tempDayOfYear, to->year, &tempMonth, &tempDay); if (result != RETURN_OK) { return result; } + to->month = tempMonth; + to->day = tempDay; + } else { + to->month = temp->month; + to->day = temp->day; } - - to->month = temp->month; - to->day = temp->day; - to->usecond = 0; if (subsecondsLength > 0) { *foundLength += 1; @@ -162,7 +161,7 @@ ReturnValue_t CCSDSTime::convertFromASCII(Clock::TimeOfDay_t* to, const uint8_t* uint16_t hour; uint16_t minute; float second; - int count = sscanf((char*)from, + int count = sscanf((const char*)from, "%4" SCNu16 "-%2" SCNu16 "-%2" SCNu16 "T%" "2" SCNu16 ":%2" SCNu16 ":%fZ", @@ -179,7 +178,7 @@ ReturnValue_t CCSDSTime::convertFromASCII(Clock::TimeOfDay_t* to, const uint8_t* } // try Code B (yyyy-ddd) - count = sscanf((char*)from, + count = sscanf((const char*)from, "%4" SCNu16 "-%3" SCNu16 "T%2" SCNu16 ":%" "2" SCNu16 ":%fZ", @@ -211,7 +210,7 @@ ReturnValue_t CCSDSTime::convertFromASCII(Clock::TimeOfDay_t* to, const uint8_t* float second; // try Code A (yyyy-mm-dd) int count = - sscanf((char*)from, "%4" SCNu16 "-%2" SCNu8 "-%2" SCNu16 "T%2" SCNu8 ":%2" SCNu8 ":%fZ", + sscanf((const char*)from, "%4" SCNu16 "-%2" SCNu8 "-%2" SCNu16 "T%2" SCNu8 ":%2" SCNu8 ":%fZ", &year, &month, &day, &hour, &minute, &second); if (count == 6) { to->year = year; @@ -225,8 +224,8 @@ ReturnValue_t CCSDSTime::convertFromASCII(Clock::TimeOfDay_t* to, const uint8_t* } // try Code B (yyyy-ddd) - count = sscanf((char*)from, "%4" SCNu16 "-%3" SCNu16 "T%2" SCNu8 ":%2" SCNu8 ":%fZ", &year, &day, - &hour, &minute, &second); + count = sscanf((const char*)from, "%4" SCNu16 "-%3" SCNu16 "T%2" SCNu8 ":%2" SCNu8 ":%fZ", &year, + &day, &hour, &minute, &second); if (count == 5) { uint8_t tempDay; ReturnValue_t result = CCSDSTime::convertDaysOfYear(day, year, &month, &tempDay); @@ -248,7 +247,7 @@ ReturnValue_t CCSDSTime::convertFromASCII(Clock::TimeOfDay_t* to, const uint8_t* } ReturnValue_t CCSDSTime::checkCcs(const uint8_t* time, uint8_t length) { - Ccs_mseconds* time_struct = (Ccs_mseconds*)time; + const Ccs_mseconds* time_struct = reinterpret_cast(time); uint8_t additionalBytes = time_struct->pField & 0b111; if ((additionalBytes == 0b111) || (length < (additionalBytes + 8))) { @@ -278,7 +277,7 @@ ReturnValue_t CCSDSTime::checkCcs(const uint8_t* time, uint8_t length) { return INVALID_TIME_FORMAT; } - uint8_t* additionalByte = &time_struct->secondEminus2; + const uint8_t* additionalByte = &time_struct->secondEminus2; for (; additionalBytes != 0; additionalBytes--) { if (*additionalByte++ > 99) { @@ -425,7 +424,7 @@ ReturnValue_t CCSDSTime::convertFromCUC(timeval* to, const uint8_t* from, size_t from++; ReturnValue_t result = convertFromCUC(to, pField, from, foundLength, maxLength - 1); if (result == HasReturnvaluesIF::RETURN_OK) { - if (foundLength != NULL) { + if (foundLength != nullptr) { *foundLength += 1; } } @@ -486,11 +485,6 @@ ReturnValue_t CCSDSTime::checkTimeOfDay(const Clock::TimeOfDay_t* time) { return RETURN_OK; } -ReturnValue_t CCSDSTime::convertTimevalToTimeOfDay(Clock::TimeOfDay_t* to, timeval* from) { - // This is rather tricky. Implement only if needed. Also, if so, move to OSAL. - return UNSUPPORTED_TIME_FORMAT; -} - ReturnValue_t CCSDSTime::convertFromCDS(timeval* to, const uint8_t* from, size_t* foundLength, size_t maxLength) { uint8_t pField = *from; @@ -554,6 +548,35 @@ ReturnValue_t CCSDSTime::convertFromCDS(timeval* to, const uint8_t* from, size_t return RETURN_OK; } +ReturnValue_t CCSDSTime::convertFromCDS(timeval* to, const CCSDSTime::CDS_short* from) { + if (to == nullptr or from == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + uint16_t days = (from->dayMSB << 8) + from->dayLSB; + if (days <= DAYS_CCSDS_TO_UNIX_EPOCH) { + return INVALID_TIME_FORMAT; + } + days -= DAYS_CCSDS_TO_UNIX_EPOCH; + to->tv_sec = days * SECONDS_PER_DAY; + uint32_t msDay = + (from->msDay_hh << 24) + (from->msDay_h << 16) + (from->msDay_l << 8) + from->msDay_ll; + to->tv_sec += (msDay / 1000); + to->tv_usec = (msDay % 1000) * 1000; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t CCSDSTime::convertFromCDS(Clock::TimeOfDay_t* to, const CCSDSTime::CDS_short* from) { + if (to == nullptr or from == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + timeval tempTimeval; + ReturnValue_t result = convertFromCDS(&tempTimeval, from); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return Clock::convertTimevalToTimeOfDay(&tempTimeval, to); +} + ReturnValue_t CCSDSTime::convertFromCUC(timeval* to, uint8_t pField, const uint8_t* from, size_t* foundLength, size_t maxLength) { uint32_t secs = 0; @@ -561,18 +584,18 @@ ReturnValue_t CCSDSTime::convertFromCUC(timeval* to, uint8_t pField, const uint8 uint8_t nCoarse = ((pField & 0b1100) >> 2) + 1; uint8_t nFine = (pField & 0b11); size_t totalLength = nCoarse + nFine; - if (foundLength != NULL) { + if (foundLength != nullptr) { *foundLength = totalLength; } if (totalLength > maxLength) { return LENGTH_MISMATCH; } - for (int count = 0; count < nCoarse; count++) { - secs += *from << ((nCoarse * 8 - 8) * (1 + count)); + for (int count = nCoarse; count > 0; count--) { + secs += *from << (count * 8 - 8); from++; } - for (int count = 0; count < nFine; count++) { - subSeconds += *from << ((nFine * 8 - 8) * (1 + count)); + for (int count = nFine; count > 0; count--) { + subSeconds += *from << (count * 8 - 8); from++; } // Move to POSIX epoch. diff --git a/src/fsfw/timemanager/CCSDSTime.h b/src/fsfw/timemanager/CCSDSTime.h index 4a0e1bfc4..2a2316af8 100644 --- a/src/fsfw/timemanager/CCSDSTime.h +++ b/src/fsfw/timemanager/CCSDSTime.h @@ -161,18 +161,37 @@ class CCSDSTime : public HasReturnvaluesIF { */ static ReturnValue_t convertFromCcsds(timeval *to, uint8_t const *from, size_t *foundLength, size_t maxLength); - + /** + * @brief Currently unsupported conversion due to leapseconds + * + * @param to Time Of Day (UTC) + * @param from Buffer to take the CUC from + * @param length Length of buffer + * @return ReturnValue_t UNSUPPORTED_TIME_FORMAT in any case ATM + */ static ReturnValue_t convertFromCUC(Clock::TimeOfDay_t *to, uint8_t const *from, uint8_t length); - + /** + * @brief Converts from CCSDS CUC to timeval + * + * If input is CCSDS Epoch this is TAI! -> No leapsecond support. + * + * Currently, it only supports seconds + 2 Byte Subseconds (1/65536 seconds) + * + * + * @param to Timeval to write the result to + * @param from Buffer to read from + * @param foundLength Length found by this function (can be nullptr if unused) + * @param maxLength Max length of the buffer to be read + * @return ReturnValue_t - RETURN_OK if successful + * - LENGTH_MISMATCH if expected length is larger than maxLength + */ static ReturnValue_t convertFromCUC(timeval *to, uint8_t const *from, size_t *foundLength, size_t maxLength); - static ReturnValue_t convertFromCUC(timeval *to, uint8_t pField, uint8_t const *from, size_t *foundLength, size_t maxLength); static ReturnValue_t convertFromCCS(timeval *to, uint8_t const *from, size_t *foundLength, size_t maxLength); - static ReturnValue_t convertFromCCS(timeval *to, uint8_t pField, uint8_t const *from, size_t *foundLength, size_t maxLength); @@ -180,6 +199,8 @@ class CCSDSTime : public HasReturnvaluesIF { static ReturnValue_t convertFromCDS(timeval *to, uint8_t const *from, size_t *foundLength, size_t maxLength); + static ReturnValue_t convertFromCDS(timeval *to, const CCSDSTime::CDS_short *from); + static ReturnValue_t convertFromCDS(Clock::TimeOfDay_t *to, const CCSDSTime::CDS_short *from); static ReturnValue_t convertFromCCS(Clock::TimeOfDay_t *to, uint8_t const *from, size_t *foundLength, size_t maxLength); @@ -190,8 +211,8 @@ class CCSDSTime : public HasReturnvaluesIF { static uint32_t subsecondsToMicroseconds(uint16_t subseconds); private: - CCSDSTime(); - virtual ~CCSDSTime(); + CCSDSTime(){}; + virtual ~CCSDSTime(){}; /** * checks a ccs time stream for validity * @@ -221,7 +242,6 @@ class CCSDSTime : public HasReturnvaluesIF { uint8_t *day); static bool isLeapYear(uint32_t year); - static ReturnValue_t convertTimevalToTimeOfDay(Clock::TimeOfDay_t *to, timeval *from); }; #endif /* FSFW_TIMEMANAGER_CCSDSTIME_H_ */ diff --git a/src/fsfw/timemanager/CMakeLists.txt b/src/fsfw/timemanager/CMakeLists.txt index 004677729..c4f773959 100644 --- a/src/fsfw/timemanager/CMakeLists.txt +++ b/src/fsfw/timemanager/CMakeLists.txt @@ -1,8 +1,3 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - CCSDSTime.cpp - Countdown.cpp - Stopwatch.cpp - TimeMessage.cpp - TimeStamper.cpp - ClockCommon.cpp -) +target_sources( + ${LIB_FSFW_NAME} PRIVATE CCSDSTime.cpp Countdown.cpp Stopwatch.cpp + TimeMessage.cpp TimeStamper.cpp ClockCommon.cpp) diff --git a/src/fsfw/timemanager/Clock.h b/src/fsfw/timemanager/Clock.h index 99e8a56ad..75c898e52 100644 --- a/src/fsfw/timemanager/Clock.h +++ b/src/fsfw/timemanager/Clock.h @@ -99,6 +99,13 @@ class Clock { */ static ReturnValue_t getDateAndTime(TimeOfDay_t *time); + /** + * Convert to time of day struct given the POSIX timeval struct + * @param from + * @param to + * @return + */ + static ReturnValue_t convertTimevalToTimeOfDay(const timeval *from, TimeOfDay_t *to); /** * Converts a time of day struct to POSIX seconds. * @param time The time of day as input @@ -166,6 +173,7 @@ class Clock { static MutexIF *timeMutex; static uint16_t leapSeconds; + static bool leapSecondsSet; }; #endif /* FSFW_TIMEMANAGER_CLOCK_H_ */ diff --git a/src/fsfw/timemanager/ClockCommon.cpp b/src/fsfw/timemanager/ClockCommon.cpp index e5749b19f..ca8b12a44 100644 --- a/src/fsfw/timemanager/ClockCommon.cpp +++ b/src/fsfw/timemanager/ClockCommon.cpp @@ -1,7 +1,13 @@ +#include + #include "fsfw/ipc/MutexGuard.h" #include "fsfw/timemanager/Clock.h" -ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt) { +uint16_t Clock::leapSeconds = 0; +MutexIF* Clock::timeMutex = nullptr; +bool Clock::leapSecondsSet = false; + +ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) { uint16_t leapSeconds; ReturnValue_t result = getLeapSeconds(&leapSeconds); if (result != HasReturnvaluesIF::RETURN_OK) { @@ -27,12 +33,16 @@ ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) { MutexGuard helper(timeMutex); leapSeconds = leapSeconds_; + leapSecondsSet = true; return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) { - if (timeMutex == nullptr) { +ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) { + if (not leapSecondsSet) { + return HasReturnvaluesIF::RETURN_FAILED; + } + if (checkOrCreateClockMutex() != HasReturnvaluesIF::RETURN_OK) { return HasReturnvaluesIF::RETURN_FAILED; } MutexGuard helper(timeMutex); @@ -42,9 +52,32 @@ ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) { return HasReturnvaluesIF::RETURN_OK; } +ReturnValue_t Clock::convertTimevalToTimeOfDay(const timeval* from, TimeOfDay_t* to) { + struct tm* timeInfo; + // According to https://en.cppreference.com/w/c/chrono/gmtime, the implementation of gmtime_s + // in the Windows CRT is incompatible with the C standard but this should not be an issue for + // this implementation + ReturnValue_t result = checkOrCreateClockMutex(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + MutexGuard helper(timeMutex); + // gmtime writes its output in a global buffer which is not Thread Safe + // Therefore we have to use a Mutex here + timeInfo = gmtime(&from->tv_sec); + to->year = timeInfo->tm_year + 1900; + to->month = timeInfo->tm_mon + 1; + to->day = timeInfo->tm_mday; + to->hour = timeInfo->tm_hour; + to->minute = timeInfo->tm_min; + to->second = timeInfo->tm_sec; + to->usecond = from->tv_usec; + return HasReturnvaluesIF::RETURN_OK; +} + ReturnValue_t Clock::checkOrCreateClockMutex() { if (timeMutex == nullptr) { - MutexFactory *mutexFactory = MutexFactory::instance(); + MutexFactory* mutexFactory = MutexFactory::instance(); if (mutexFactory == nullptr) { return HasReturnvaluesIF::RETURN_FAILED; } diff --git a/src/fsfw/timemanager/Countdown.cpp b/src/fsfw/timemanager/Countdown.cpp index 8ff46f8df..a8ba78cbb 100644 --- a/src/fsfw/timemanager/Countdown.cpp +++ b/src/fsfw/timemanager/Countdown.cpp @@ -1,6 +1,8 @@ #include "fsfw/timemanager/Countdown.h" -Countdown::Countdown(uint32_t initialTimeout) : timeout(initialTimeout) {} +Countdown::Countdown(uint32_t initialTimeout) : timeout(initialTimeout) { + setTimeout(initialTimeout); +} Countdown::~Countdown() {} diff --git a/src/fsfw/tmstorage/CMakeLists.txt b/src/fsfw/tmstorage/CMakeLists.txt index 7990d85a1..80da7faf8 100644 --- a/src/fsfw/tmstorage/CMakeLists.txt +++ b/src/fsfw/tmstorage/CMakeLists.txt @@ -1,4 +1 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - TmStoreMessage.cpp -) +target_sources(${LIB_FSFW_NAME} PRIVATE TmStoreMessage.cpp) diff --git a/src/fsfw/tmtcpacket/CMakeLists.txt b/src/fsfw/tmtcpacket/CMakeLists.txt index e1deaba90..196ba7528 100644 --- a/src/fsfw/tmtcpacket/CMakeLists.txt +++ b/src/fsfw/tmtcpacket/CMakeLists.txt @@ -1,8 +1,5 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - SpacePacket.cpp - SpacePacketBase.cpp -) +target_sources(${LIB_FSFW_NAME} PRIVATE SpacePacket.cpp SpacePacketBase.cpp) add_subdirectory(cfdp) add_subdirectory(packetmatcher) -add_subdirectory(pus) \ No newline at end of file +add_subdirectory(pus) diff --git a/src/fsfw/tmtcpacket/SpacePacket.cpp b/src/fsfw/tmtcpacket/SpacePacket.cpp index c8ebbd978..16d968fb9 100644 --- a/src/fsfw/tmtcpacket/SpacePacket.cpp +++ b/src/fsfw/tmtcpacket/SpacePacket.cpp @@ -19,8 +19,8 @@ SpacePacket::SpacePacket(uint16_t packetDataLength, bool isTelecommand, uint16_t SpacePacket::~SpacePacket(void) {} bool SpacePacket::addWholeData(const uint8_t* p_Data, uint32_t packet_size) { - if (packet_size <= sizeof(this->data)) { - memcpy(&this->localData.byteStream, p_Data, packet_size); + if (packet_size <= sizeof(this->localData)) { + memcpy(this->localData.byteStream, p_Data, packet_size); return true; } else { return false; diff --git a/src/fsfw/tmtcpacket/cfdp/CMakeLists.txt b/src/fsfw/tmtcpacket/cfdp/CMakeLists.txt index 0b7ab18a9..7d20aab88 100644 --- a/src/fsfw/tmtcpacket/cfdp/CMakeLists.txt +++ b/src/fsfw/tmtcpacket/cfdp/CMakeLists.txt @@ -1,4 +1 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - CFDPPacket.cpp - CFDPPacketStored.cpp -) +target_sources(${LIB_FSFW_NAME} PRIVATE CFDPPacket.cpp CFDPPacketStored.cpp) diff --git a/src/fsfw/tmtcpacket/packetmatcher/CMakeLists.txt b/src/fsfw/tmtcpacket/packetmatcher/CMakeLists.txt index e9a8d03bb..6ea947991 100644 --- a/src/fsfw/tmtcpacket/packetmatcher/CMakeLists.txt +++ b/src/fsfw/tmtcpacket/packetmatcher/CMakeLists.txt @@ -1,4 +1 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - PacketMatchTree.cpp -) +target_sources(${LIB_FSFW_NAME} PRIVATE PacketMatchTree.cpp) diff --git a/src/fsfw/tmtcpacket/pus/tc/CMakeLists.txt b/src/fsfw/tmtcpacket/pus/tc/CMakeLists.txt index dc6112639..09c63bfd7 100644 --- a/src/fsfw/tmtcpacket/pus/tc/CMakeLists.txt +++ b/src/fsfw/tmtcpacket/pus/tc/CMakeLists.txt @@ -1,6 +1,3 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - TcPacketPusBase.cpp - TcPacketPus.cpp - TcPacketStoredBase.cpp - TcPacketStoredPus.cpp -) +target_sources( + ${LIB_FSFW_NAME} PRIVATE TcPacketPusBase.cpp TcPacketPus.cpp + TcPacketStoredBase.cpp TcPacketStoredPus.cpp) diff --git a/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredBase.cpp b/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredBase.cpp index 8cc38c5f9..229185262 100644 --- a/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredBase.cpp +++ b/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredBase.cpp @@ -6,20 +6,20 @@ #include "fsfw/objectmanager/frameworkObjects.h" #include "fsfw/serviceinterface/ServiceInterface.h" -StorageManagerIF* TcPacketStoredBase::store = nullptr; +StorageManagerIF* TcPacketStoredBase::STORE = nullptr; TcPacketStoredBase::TcPacketStoredBase() { this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; - this->checkAndSetStore(); + TcPacketStoredBase::checkAndSetStore(); } -TcPacketStoredBase::~TcPacketStoredBase() {} +TcPacketStoredBase::~TcPacketStoredBase() = default; ReturnValue_t TcPacketStoredBase::getData(const uint8_t** dataPtr, size_t* dataSize) { - auto result = this->store->getData(storeAddress, dataPtr, dataSize); + auto result = TcPacketStoredBase::STORE->getData(storeAddress, dataPtr, dataSize); if (result != HasReturnvaluesIF::RETURN_OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "TcPacketStoredBase: Could not get data!" << std::endl; + sif::warning << "TcPacketStoredBase: Could not get data" << std::endl; #else sif::printWarning("TcPacketStoredBase: Could not get data!\n"); #endif @@ -28,11 +28,13 @@ ReturnValue_t TcPacketStoredBase::getData(const uint8_t** dataPtr, size_t* dataS } bool TcPacketStoredBase::checkAndSetStore() { - if (this->store == nullptr) { - this->store = ObjectManager::instance()->get(objects::TC_STORE); - if (this->store == nullptr) { + if (TcPacketStoredBase::STORE == nullptr) { + TcPacketStoredBase::STORE = ObjectManager::instance()->get(objects::TC_STORE); + if (TcPacketStoredBase::STORE == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "TcPacketStoredBase::TcPacketStoredBase: TC Store not found!" << std::endl; + sif::error << "TcPacketStoredBase::TcPacketStoredBase: TC Store not found" << std::endl; +#else + sif::printError("TcPacketStoredBase::TcPacketStoredBase: TC Store not found\n"); #endif return false; } @@ -47,7 +49,7 @@ void TcPacketStoredBase::setStoreAddress(store_address_t setAddress, size_t tempSize; ReturnValue_t status = StorageManagerIF::RETURN_FAILED; if (this->checkAndSetStore()) { - status = this->store->getData(this->storeAddress, &tempData, &tempSize); + status = TcPacketStoredBase::STORE->getData(this->storeAddress, &tempData, &tempSize); } if (status == StorageManagerIF::RETURN_OK) { diff --git a/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredBase.h b/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredBase.h index 86f0d94ec..ece0e4827 100644 --- a/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredBase.h +++ b/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredBase.h @@ -65,7 +65,7 @@ class TcPacketStoredBase : public TcPacketStoredIF { * call tries to set it and throws an error message in case of failures. * The default store is objects::TC_STORE. */ - static StorageManagerIF* store; + static StorageManagerIF* STORE; /** * The address where the packet data of the object instance is stored. */ @@ -77,7 +77,7 @@ class TcPacketStoredBase : public TcPacketStoredIF { * @return @li @c true if the store is linked or could be created. * @li @c false otherwise. */ - bool checkAndSetStore(); + static bool checkAndSetStore(); }; -#endif /* TMTCPACKET_PUS_TcPacketStoredBase_H_ */ +#endif /* TMTCPACKET_PUS_TCPACKETSTORED_H_ */ diff --git a/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredIF.h b/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredIF.h index ac4019cd4..7ac8c3319 100644 --- a/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredIF.h +++ b/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredIF.h @@ -9,7 +9,8 @@ class TcPacketStoredIF { public: - virtual ~TcPacketStoredIF(){}; + virtual ~TcPacketStoredIF() = default; + ; /** * With this call, the stored packet can be set to another packet in a store. This is useful diff --git a/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredPus.cpp b/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredPus.cpp index 153ad8636..643c2ecca 100644 --- a/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredPus.cpp +++ b/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredPus.cpp @@ -14,8 +14,8 @@ TcPacketStoredPus::TcPacketStoredPus(uint16_t apid, uint8_t service, uint8_t sub } uint8_t* pData = nullptr; ReturnValue_t returnValue = - this->store->getFreeElement(&this->storeAddress, (TC_PACKET_MIN_SIZE + size), &pData); - if (returnValue != this->store->RETURN_OK) { + this->STORE->getFreeElement(&this->storeAddress, (TC_PACKET_MIN_SIZE + size), &pData); + if (returnValue != this->STORE->RETURN_OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "TcPacketStoredBase: Could not get free element from store!" << std::endl; #endif @@ -44,19 +44,19 @@ TcPacketStoredPus::TcPacketStoredPus(const uint8_t* data, size_t size) : TcPacke return; } if (this->checkAndSetStore()) { - ReturnValue_t status = store->addData(&storeAddress, data, size); + ReturnValue_t status = STORE->addData(&storeAddress, data, size); if (status != HasReturnvaluesIF::RETURN_OK) { this->setData(nullptr, size); } const uint8_t* storePtr = nullptr; // Repoint base data pointer to the data in the store. - store->getData(storeAddress, &storePtr, &size); + STORE->getData(storeAddress, &storePtr, &size); this->setData(const_cast(storePtr), size); } } ReturnValue_t TcPacketStoredPus::deletePacket() { - ReturnValue_t result = this->store->deleteData(this->storeAddress); + ReturnValue_t result = this->STORE->deleteData(this->storeAddress); this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; // To circumvent size checks this->setData(nullptr, -1); @@ -68,7 +68,7 @@ TcPacketPusBase* TcPacketStoredPus::getPacketBase() { return this; } bool TcPacketStoredPus::isSizeCorrect() { const uint8_t* temp_data = nullptr; size_t temp_size; - ReturnValue_t status = this->store->getData(this->storeAddress, &temp_data, &temp_size); + ReturnValue_t status = this->STORE->getData(this->storeAddress, &temp_data, &temp_size); if (status == StorageManagerIF::RETURN_OK) { if (this->getFullSize() == temp_size) { return true; diff --git a/src/fsfw/tmtcpacket/pus/tm/CMakeLists.txt b/src/fsfw/tmtcpacket/pus/tm/CMakeLists.txt index ace87820d..ded74ce2c 100644 --- a/src/fsfw/tmtcpacket/pus/tm/CMakeLists.txt +++ b/src/fsfw/tmtcpacket/pus/tm/CMakeLists.txt @@ -1,9 +1,9 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - TmPacketStoredPusA.cpp - TmPacketStoredPusC.cpp - TmPacketPusA.cpp - TmPacketPusC.cpp - TmPacketStoredBase.cpp - TmPacketBase.cpp - TmPacketMinimal.cpp -) +target_sources( + ${LIB_FSFW_NAME} + PRIVATE TmPacketStoredPusA.cpp + TmPacketStoredPusC.cpp + TmPacketPusA.cpp + TmPacketPusC.cpp + TmPacketStoredBase.cpp + TmPacketBase.cpp + TmPacketMinimal.cpp) diff --git a/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h b/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h index 4186f4dfe..e18a4f3a6 100644 --- a/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +++ b/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h @@ -1,7 +1,7 @@ #ifndef FRAMEWORK_TMTCSERVICES_ACCEPTSTELECOMMANDSIF_H_ #define FRAMEWORK_TMTCSERVICES_ACCEPTSTELECOMMANDSIF_H_ -#include "../ipc/MessageQueueSenderIF.h" +#include "fsfw/ipc/MessageQueueSenderIF.h" /** * @brief This interface is implemented by classes that are sinks for @@ -20,7 +20,7 @@ class AcceptsTelecommandsIF { /** * @brief The virtual destructor as it is mandatory for C++ interfaces. */ - virtual ~AcceptsTelecommandsIF() {} + virtual ~AcceptsTelecommandsIF() = default; /** * @brief Getter for the service id. * @details Any receiving service (at least any PUS service) shall have a diff --git a/src/fsfw/tmtcservices/CMakeLists.txt b/src/fsfw/tmtcservices/CMakeLists.txt index 96cf99b57..d2a3f4ed1 100644 --- a/src/fsfw/tmtcservices/CMakeLists.txt +++ b/src/fsfw/tmtcservices/CMakeLists.txt @@ -1,10 +1,9 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - CommandingServiceBase.cpp - PusServiceBase.cpp - PusVerificationReport.cpp - TmTcBridge.cpp - TmTcMessage.cpp - VerificationReporter.cpp - SpacePacketParser.cpp -) \ No newline at end of file +target_sources( + ${LIB_FSFW_NAME} + PRIVATE CommandingServiceBase.cpp + PusServiceBase.cpp + PusVerificationReport.cpp + TmTcBridge.cpp + TmTcMessage.cpp + VerificationReporter.cpp + SpacePacketParser.cpp) diff --git a/src/fsfw/tmtcservices/CommandingServiceBase.h b/src/fsfw/tmtcservices/CommandingServiceBase.h index 0ae195051..4dcad024e 100644 --- a/src/fsfw/tmtcservices/CommandingServiceBase.h +++ b/src/fsfw/tmtcservices/CommandingServiceBase.h @@ -95,7 +95,7 @@ class CommandingServiceBase : public SystemObject, */ virtual ReturnValue_t performOperation(uint8_t opCode) override; - virtual uint16_t getIdentifier(); + virtual uint16_t getIdentifier() override; /** * Returns the requestQueue MessageQueueId_t @@ -104,7 +104,7 @@ class CommandingServiceBase : public SystemObject, * * @return requestQueue messageQueueId_t */ - virtual MessageQueueId_t getRequestQueue(); + virtual MessageQueueId_t getRequestQueue() override; /** * Returns the commandQueue MessageQueueId_t @@ -166,7 +166,7 @@ class CommandingServiceBase : public SystemObject, * @param objectId Target object ID * @return * - @c RETURN_OK to generate a verification start message - * - @c EXECUTION_COMPELTE Fire-and-forget command. Generate a completion + * - @c EXECUTION_COMPLETE Fire-and-forget command. Generate a completion * verification message. * - @c Anything else rejects the packets and generates a start failure * verification. diff --git a/src/fsfw/version.cpp b/src/fsfw/version.cpp new file mode 100644 index 000000000..050187a9a --- /dev/null +++ b/src/fsfw/version.cpp @@ -0,0 +1,40 @@ +#include "version.h" + +#include + +#include "fsfw/FSFWVersion.h" + +#ifdef major +#undef major +#endif + +#ifdef minor +#undef minor +#endif + +const fsfw::Version fsfw::FSFW_VERSION = {FSFW_VERSION_MAJOR, FSFW_VERSION_MINOR, + FSFW_VERSION_REVISION, FSFW_VCS_INFO}; + +fsfw::Version::Version(int major, int minor, int revision, const char* addInfo) + : major(major), minor(minor), revision(revision), addInfo(addInfo) {} + +void fsfw::Version::getVersion(char* str, size_t maxLen) const { + size_t len = snprintf(str, maxLen, "%d.%d.%d", major, minor, revision); + if (addInfo != nullptr) { + snprintf(str + len, maxLen - len, "-%s", addInfo); + } +} + +namespace fsfw { + +#if FSFW_CPP_OSTREAM_ENABLED == 1 +std::ostream& operator<<(std::ostream& os, const Version& v) { + os << v.major << "." << v.minor << "." << v.revision; + if (v.addInfo != nullptr) { + os << "-" << v.addInfo; + } + return os; +} +#endif + +} // namespace fsfw diff --git a/src/fsfw/version.h b/src/fsfw/version.h new file mode 100644 index 000000000..a538c39a2 --- /dev/null +++ b/src/fsfw/version.h @@ -0,0 +1,64 @@ +#ifndef FSFW_SRC_FSFW_VERSION_H_ +#define FSFW_SRC_FSFW_VERSION_H_ + +#include "fsfw/FSFW.h" + +#if FSFW_CPP_OSTREAM_ENABLED == 1 +#include +#endif +#include + +namespace fsfw { + +class Version { + public: + Version(int major, int minor, int revision, const char* addInfo = nullptr); + int major = -1; + int minor = -1; + int revision = -1; + + // Additional information, e.g. a git SHA hash + const char* addInfo = nullptr; + + friend bool operator==(const Version& v1, const Version& v2) { + return (v1.major == v2.major and v1.minor == v2.minor and v1.revision == v2.revision); + } + + friend bool operator!=(const Version& v1, const Version& v2) { return not(v1 == v2); } + + friend bool operator<(const Version& v1, const Version& v2) { + return ((v1.major < v2.major) or (v1.major == v2.major and v1.minor < v2.minor) or + (v1.major == v2.major and v1.minor == v2.minor and v1.revision < v2.revision)); + } + + friend bool operator>(const Version& v1, const Version& v2) { + return not(v1 < v2) and not(v1 == v2); + } + + friend bool operator<=(const Version& v1, const Version& v2) { return ((v1 == v2) or (v1 < v2)); } + + friend bool operator>=(const Version& v1, const Version& v2) { return ((v1 == v2) or (v1 > v2)); } + +#if FSFW_CPP_OSTREAM_ENABLED == 1 + /** + * Print format to given ostream using format "major.minor.revision" + * @param os + * @param v + * @return + */ + friend std::ostream& operator<<(std::ostream& os, const Version& v); +#endif + + /** + * Get version as format "major.minor.revision" + * @param str + * @param maxLen + */ + void getVersion(char* str, size_t maxLen) const; +}; + +extern const Version FSFW_VERSION; + +} // namespace fsfw + +#endif /* FSFW_SRC_FSFW_VERSION_H_ */ diff --git a/hal/src/fsfw_hal/CMakeLists.txt b/src/fsfw_hal/CMakeLists.txt similarity index 65% rename from hal/src/fsfw_hal/CMakeLists.txt rename to src/fsfw_hal/CMakeLists.txt index b7559d4b8..057ab3a69 100644 --- a/hal/src/fsfw_hal/CMakeLists.txt +++ b/src/fsfw_hal/CMakeLists.txt @@ -2,9 +2,9 @@ add_subdirectory(devicehandlers) add_subdirectory(common) if(UNIX) - add_subdirectory(linux) + add_subdirectory(linux) endif() if(FSFW_HAL_ADD_STM32H7) - add_subdirectory(stm32h7) + add_subdirectory(stm32h7) endif() diff --git a/hal/src/fsfw_hal/common/CMakeLists.txt b/src/fsfw_hal/common/CMakeLists.txt similarity index 100% rename from hal/src/fsfw_hal/common/CMakeLists.txt rename to src/fsfw_hal/common/CMakeLists.txt diff --git a/src/fsfw_hal/common/gpio/CMakeLists.txt b/src/fsfw_hal/common/gpio/CMakeLists.txt new file mode 100644 index 000000000..5c81d9ccc --- /dev/null +++ b/src/fsfw_hal/common/gpio/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE GpioCookie.cpp) diff --git a/hal/src/fsfw_hal/common/gpio/GpioCookie.cpp b/src/fsfw_hal/common/gpio/GpioCookie.cpp similarity index 100% rename from hal/src/fsfw_hal/common/gpio/GpioCookie.cpp rename to src/fsfw_hal/common/gpio/GpioCookie.cpp diff --git a/hal/src/fsfw_hal/common/gpio/GpioCookie.h b/src/fsfw_hal/common/gpio/GpioCookie.h similarity index 100% rename from hal/src/fsfw_hal/common/gpio/GpioCookie.h rename to src/fsfw_hal/common/gpio/GpioCookie.h diff --git a/hal/src/fsfw_hal/common/gpio/GpioIF.h b/src/fsfw_hal/common/gpio/GpioIF.h similarity index 100% rename from hal/src/fsfw_hal/common/gpio/GpioIF.h rename to src/fsfw_hal/common/gpio/GpioIF.h diff --git a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h b/src/fsfw_hal/common/gpio/gpioDefinitions.h similarity index 93% rename from hal/src/fsfw_hal/common/gpio/gpioDefinitions.h rename to src/fsfw_hal/common/gpio/gpioDefinitions.h index b429449bb..eb90629eb 100644 --- a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h +++ b/src/fsfw_hal/common/gpio/gpioDefinitions.h @@ -9,11 +9,11 @@ using gpioId_t = uint16_t; namespace gpio { -enum Levels : uint8_t { LOW = 0, HIGH = 1, NONE = 99 }; +enum class Levels : int { LOW = 0, HIGH = 1, NONE = 99 }; -enum Direction : uint8_t { IN = 0, OUT = 1 }; +enum class Direction : int { IN = 0, OUT = 1 }; -enum GpioOperation { READ, WRITE }; +enum class GpioOperation { READ, WRITE }; enum class GpioTypes { NONE, @@ -80,7 +80,7 @@ class GpiodRegularByChip : public GpiodRegularBase { public: GpiodRegularByChip() : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, std::string(), gpio::Direction::IN, - gpio::LOW, 0) {} + gpio::Levels::LOW, 0) {} GpiodRegularByChip(std::string chipname_, int lineNum_, std::string consumer_, gpio::Direction direction_, gpio::Levels initValue_) @@ -90,7 +90,7 @@ class GpiodRegularByChip : public GpiodRegularBase { GpiodRegularByChip(std::string chipname_, int lineNum_, std::string consumer_) : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, consumer_, gpio::Direction::IN, - gpio::LOW, lineNum_), + gpio::Levels::LOW, lineNum_), chipname(chipname_) {} std::string chipname; @@ -106,7 +106,7 @@ class GpiodRegularByLabel : public GpiodRegularBase { GpiodRegularByLabel(std::string label_, int lineNum_, std::string consumer_) : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL, consumer_, gpio::Direction::IN, - gpio::LOW, lineNum_), + gpio::Levels::LOW, lineNum_), label(label_) {} std::string label; @@ -127,7 +127,7 @@ class GpiodRegularByLineName : public GpiodRegularBase { GpiodRegularByLineName(std::string lineName_, std::string consumer_) : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_, gpio::Direction::IN, - gpio::LOW), + gpio::Levels::LOW), lineName(lineName_) {} std::string lineName; diff --git a/hal/src/fsfw_hal/common/spi/spiCommon.h b/src/fsfw_hal/common/spi/spiCommon.h similarity index 100% rename from hal/src/fsfw_hal/common/spi/spiCommon.h rename to src/fsfw_hal/common/spi/spiCommon.h diff --git a/src/fsfw_hal/devicehandlers/CMakeLists.txt b/src/fsfw_hal/devicehandlers/CMakeLists.txt new file mode 100644 index 000000000..17139416c --- /dev/null +++ b/src/fsfw_hal/devicehandlers/CMakeLists.txt @@ -0,0 +1,3 @@ +target_sources( + ${LIB_FSFW_NAME} PRIVATE GyroL3GD20Handler.cpp MgmRM3100Handler.cpp + MgmLIS3MDLHandler.cpp) diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp similarity index 91% rename from hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp rename to src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp index be4c9aa97..94e1331c6 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp +++ b/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp @@ -8,11 +8,7 @@ GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceC CookieIF *comCookie, uint32_t transitionDelayMs) : DeviceHandlerBase(objectId, deviceCommunication, comCookie), transitionDelayMs(transitionDelayMs), - dataset(this) { -#if FSFW_HAL_L3GD20_GYRO_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(3); -#endif -} + dataset(this) {} GyroHandlerL3GD20H::~GyroHandlerL3GD20H() {} @@ -193,22 +189,22 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX]; float temperature = 25.0 + temperaturOffset; -#if FSFW_HAL_L3GD20_GYRO_DEBUG == 1 - if (debugDivider->checkAndIncrement()) { - /* Set terminal to utf-8 if there is an issue with micro printout. */ + if (periodicPrintout) { + if (debugDivider.checkAndIncrement()) { + /* Set terminal to utf-8 if there is an issue with micro printout. */ #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::info << "GyroHandlerL3GD20H: Angular velocities (deg/s):" << std::endl; - sif::info << "X: " << angVelocX << std::endl; - sif::info << "Y: " << angVelocY << std::endl; - sif::info << "Z: " << angVelocZ << std::endl; + sif::info << "GyroHandlerL3GD20H: Angular velocities (deg/s):" << std::endl; + sif::info << "X: " << angVelocX << std::endl; + sif::info << "Y: " << angVelocY << std::endl; + sif::info << "Z: " << angVelocZ << std::endl; #else - sif::printInfo("GyroHandlerL3GD20H: Angular velocities (deg/s):\n"); - sif::printInfo("X: %f\n", angVelocX); - sif::printInfo("Y: %f\n", angVelocY); - sif::printInfo("Z: %f\n", angVelocZ); + sif::printInfo("GyroHandlerL3GD20H: Angular velocities (deg/s):\n"); + sif::printInfo("X: %f\n", angVelocX); + sif::printInfo("Y: %f\n", angVelocY); + sif::printInfo("Z: %f\n", angVelocZ); #endif + } } -#endif PoolReadGuard readSet(&dataset); if (readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) { @@ -272,3 +268,8 @@ void GyroHandlerL3GD20H::setAbsoluteLimits(float limitX, float limitY, float lim this->absLimitY = limitY; this->absLimitZ = limitZ; } + +void GyroHandlerL3GD20H::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h b/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h similarity index 94% rename from hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h rename to src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h index 784dcf4cd..7c1ebdac1 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h +++ b/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h @@ -5,7 +5,6 @@ #include #include "devicedefinitions/GyroL3GD20Definitions.h" -#include "fsfw/FSFW.h" /** * @brief Device Handler for the L3GD20H gyroscope sensor @@ -22,6 +21,8 @@ class GyroHandlerL3GD20H : public DeviceHandlerBase { uint32_t transitionDelayMs); virtual ~GyroHandlerL3GD20H(); + void enablePeriodicPrintouts(bool enable, uint8_t divider); + /** * Set the absolute limit for the values on the axis in degrees per second. * The dataset values will be marked as invalid if that limit is exceeded @@ -80,9 +81,8 @@ class GyroHandlerL3GD20H : public DeviceHandlerBase { // Set default value float sensitivity = L3GD20H::SENSITIVITY_00; -#if FSFW_HAL_L3GD20_GYRO_DEBUG == 1 - PeriodicOperationDivider *debugDivider = nullptr; -#endif + bool periodicPrintout = false; + PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); }; #endif /* MISSION_DEVICES_GYROL3GD20HANDLER_H_ */ diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp similarity index 93% rename from hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp rename to src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp index f3ea99428..644b488dd 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp +++ b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp @@ -1,20 +1,14 @@ #include "MgmLIS3MDLHandler.h" -#include "fsfw/datapool/PoolReadGuard.h" -#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1 -#include "fsfw/globalfunctions/PeriodicOperationDivider.h" -#endif - #include +#include "fsfw/datapool/PoolReadGuard.h" + MgmLIS3MDLHandler::MgmLIS3MDLHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, uint32_t transitionDelay) : DeviceHandlerBase(objectId, deviceCommunication, comCookie), dataset(this), transitionDelay(transitionDelay) { -#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(3); -#endif // Set to default values right away registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT; registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT; @@ -264,7 +258,7 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id, cons int16_t mgmMeasurementRawZ = packet[MGMLIS3MDL::Z_HIGHBYTE_IDX] << 8 | packet[MGMLIS3MDL::Z_LOWBYTE_IDX]; - /* Target value in microtesla */ + // Target value in microtesla float mgmX = static_cast(mgmMeasurementRawX) * sensitivityFactor * MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR; float mgmY = static_cast(mgmMeasurementRawY) * sensitivityFactor * @@ -272,23 +266,24 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id, cons float mgmZ = static_cast(mgmMeasurementRawZ) * sensitivityFactor * MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR; -#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1 - if (debugDivider->checkAndIncrement()) { + if (periodicPrintout) { + if (debugDivider.checkAndIncrement()) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::info << "MGMHandlerLIS3: Magnetic field strength in" - " microtesla:" - << std::endl; - sif::info << "X: " << mgmX << " uT" << std::endl; - sif::info << "Y: " << mgmY << " uT" << std::endl; - sif::info << "Z: " << mgmZ << " uT" << std::endl; + sif::info << "MGMHandlerLIS3: Magnetic field strength in" + " microtesla:" + << std::endl; + sif::info << "X: " << mgmX << " uT" << std::endl; + sif::info << "Y: " << mgmY << " uT" << std::endl; + sif::info << "Z: " << mgmZ << " uT" << std::endl; #else - sif::printInfo("MGMHandlerLIS3: Magnetic field strength in microtesla:\n"); - sif::printInfo("X: %f uT\n", mgmX); - sif::printInfo("Y: %f uT\n", mgmY); - sif::printInfo("Z: %f uT\n", mgmZ); + sif::printInfo("MGMHandlerLIS3: Magnetic field strength in microtesla:\n"); + sif::printInfo("X: %f uT\n", mgmX); + sif::printInfo("Y: %f uT\n", mgmY); + sif::printInfo("Z: %f uT\n", mgmZ); #endif /* FSFW_CPP_OSTREAM_ENABLED == 0 */ + } } -#endif /* OBSW_VERBOSE_LEVEL >= 1 */ + PoolReadGuard readHelper(&dataset); if (readHelper.getReadResult() == HasReturnvaluesIF::RETURN_OK) { if (std::abs(mgmX) < absLimitX) { @@ -318,15 +313,16 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id, cons case MGMLIS3MDL::READ_TEMPERATURE: { int16_t tempValueRaw = packet[2] << 8 | packet[1]; float tempValue = 25.0 + ((static_cast(tempValueRaw)) / 8.0); -#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1 - if (debugDivider->check()) { + if (periodicPrintout) { + if (debugDivider.check()) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::info << "MGMHandlerLIS3: Temperature: " << tempValue << " C" << std::endl; + sif::info << "MGMHandlerLIS3: Temperature: " << tempValue << " C" << std::endl; #else - sif::printInfo("MGMHandlerLIS3: Temperature: %f C\n"); + sif::printInfo("MGMHandlerLIS3: Temperature: %f C\n"); #endif + } } -#endif + ReturnValue_t result = dataset.read(); if (result == HasReturnvaluesIF::RETURN_OK) { dataset.temperature = tempValue; @@ -462,7 +458,9 @@ ReturnValue_t MgmLIS3MDLHandler::prepareCtrlRegisterWrite() { return RETURN_OK; } -void MgmLIS3MDLHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {} +void MgmLIS3MDLHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { + DeviceHandlerBase::doTransition(modeFrom, subModeFrom); +} uint32_t MgmLIS3MDLHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { return transitionDelay; } @@ -482,3 +480,8 @@ void MgmLIS3MDLHandler::setAbsoluteLimits(float xLimit, float yLimit, float zLim this->absLimitY = yLimit; this->absLimitZ = zLimit; } + +void MgmLIS3MDLHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h similarity index 96% rename from hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h rename to src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h index b7cfc3783..42bd5d4ce 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h +++ b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h @@ -3,8 +3,8 @@ #include "devicedefinitions/MgmLIS3HandlerDefs.h" #include "events/subsystemIdRanges.h" -#include "fsfw/FSFW.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" +#include "fsfw/globalfunctions/PeriodicOperationDivider.h" class PeriodicOperationDivider; @@ -30,6 +30,7 @@ class MgmLIS3MDLHandler : public DeviceHandlerBase { uint32_t transitionDelay); virtual ~MgmLIS3MDLHandler(); + void enablePeriodicPrintouts(bool enable, uint8_t divider); /** * Set the absolute limit for the values on the axis in microtesla. The dataset values will * be marked as invalid if that limit is exceeded @@ -167,9 +168,8 @@ class MgmLIS3MDLHandler : public DeviceHandlerBase { */ ReturnValue_t prepareCtrlRegisterWrite(); -#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1 - PeriodicOperationDivider *debugDivider; -#endif + bool periodicPrintout = false; + PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); }; #endif /* MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ */ diff --git a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp similarity index 93% rename from hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp rename to src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp index 4c6e09b12..f9929d638 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp +++ b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp @@ -10,11 +10,7 @@ MgmRM3100Handler::MgmRM3100Handler(object_id_t objectId, object_id_t deviceCommu CookieIF *comCookie, uint32_t transitionDelay) : DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this), - transitionDelay(transitionDelay) { -#if FSFW_HAL_RM3100_MGM_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(3); -#endif -} + transitionDelay(transitionDelay) {} MgmRM3100Handler::~MgmRM3100Handler() {} @@ -337,23 +333,23 @@ ReturnValue_t MgmRM3100Handler::handleDataReadout(const uint8_t *packet) { float fieldStrengthY = fieldStrengthRawY * scaleFactorX; float fieldStrengthZ = fieldStrengthRawZ * scaleFactorX; -#if FSFW_HAL_RM3100_MGM_DEBUG == 1 - if (debugDivider->checkAndIncrement()) { + if (periodicPrintout) { + if (debugDivider.checkAndIncrement()) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::info << "MgmRM3100Handler: Magnetic field strength in" - " microtesla:" - << std::endl; - sif::info << "X: " << fieldStrengthX << " uT" << std::endl; - sif::info << "Y: " << fieldStrengthY << " uT" << std::endl; - sif::info << "Z: " << fieldStrengthZ << " uT" << std::endl; + sif::info << "MgmRM3100Handler: Magnetic field strength in" + " microtesla:" + << std::endl; + sif::info << "X: " << fieldStrengthX << " uT" << std::endl; + sif::info << "Y: " << fieldStrengthY << " uT" << std::endl; + sif::info << "Z: " << fieldStrengthZ << " uT" << std::endl; #else - sif::printInfo("MgmRM3100Handler: Magnetic field strength in microtesla:\n"); - sif::printInfo("X: %f uT\n", fieldStrengthX); - sif::printInfo("Y: %f uT\n", fieldStrengthY); - sif::printInfo("Z: %f uT\n", fieldStrengthZ); + sif::printInfo("MgmRM3100Handler: Magnetic field strength in microtesla:\n"); + sif::printInfo("X: %f uT\n", fieldStrengthX); + sif::printInfo("Y: %f uT\n", fieldStrengthY); + sif::printInfo("Z: %f uT\n", fieldStrengthZ); #endif + } } -#endif // TODO: Sanity check on values? PoolReadGuard readGuard(&primaryDataset); @@ -365,3 +361,8 @@ ReturnValue_t MgmRM3100Handler::handleDataReadout(const uint8_t *packet) { } return RETURN_OK; } + +void MgmRM3100Handler::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} diff --git a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h similarity index 96% rename from hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h rename to src/fsfw_hal/devicehandlers/MgmRM3100Handler.h index 67362f591..d1048cb6e 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h +++ b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h @@ -2,12 +2,8 @@ #define MISSION_DEVICES_MGMRM3100HANDLER_H_ #include "devicedefinitions/MgmRM3100HandlerDefs.h" -#include "fsfw/FSFW.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" - -#if FSFW_HAL_RM3100_MGM_DEBUG == 1 #include "fsfw/globalfunctions/PeriodicOperationDivider.h" -#endif /** * @brief Device Handler for the RM3100 geomagnetic magnetometer sensor @@ -33,6 +29,7 @@ class MgmRM3100Handler : public DeviceHandlerBase { uint32_t transitionDelay); virtual ~MgmRM3100Handler(); + void enablePeriodicPrintouts(bool enable, uint8_t divider); /** * Configure device handler to go to normal mode after startup immediately * @param enable @@ -98,9 +95,9 @@ class MgmRM3100Handler : public DeviceHandlerBase { size_t commandDataLen); ReturnValue_t handleDataReadout(const uint8_t *packet); -#if FSFW_HAL_RM3100_MGM_DEBUG == 1 - PeriodicOperationDivider *debugDivider; -#endif + + bool periodicPrintout = false; + PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); }; #endif /* MISSION_DEVICEHANDLING_MGMRM3100HANDLER_H_ */ diff --git a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h b/src/fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h similarity index 100% rename from hal/src/fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h rename to src/fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h diff --git a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h b/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h similarity index 100% rename from hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h rename to src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h diff --git a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h b/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h similarity index 100% rename from hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h rename to src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h diff --git a/hal/src/fsfw_hal/host/CMakeLists.txt b/src/fsfw_hal/host/CMakeLists.txt similarity index 100% rename from hal/src/fsfw_hal/host/CMakeLists.txt rename to src/fsfw_hal/host/CMakeLists.txt diff --git a/src/fsfw_hal/linux/CMakeLists.txt b/src/fsfw_hal/linux/CMakeLists.txt new file mode 100644 index 000000000..ffa5f5ee5 --- /dev/null +++ b/src/fsfw_hal/linux/CMakeLists.txt @@ -0,0 +1,21 @@ +if(FSFW_HAL_ADD_RASPBERRY_PI) + add_subdirectory(rpi) +endif() + +target_sources(${LIB_FSFW_NAME} PRIVATE UnixFileGuard.cpp CommandExecutor.cpp + utility.cpp) + +if(FSFW_HAL_LINUX_ADD_PERIPHERAL_DRIVERS) + if(FSFW_HAL_LINUX_ADD_LIBGPIOD) + add_subdirectory(gpio) + endif() + add_subdirectory(uart) + # Adding those does not really make sense on Apple systems which are generally + # host systems. It won't even compile as the headers are missing + if(NOT APPLE) + add_subdirectory(i2c) + add_subdirectory(spi) + endif() +endif() + +add_subdirectory(uio) diff --git a/hal/src/fsfw_hal/linux/CommandExecutor.cpp b/src/fsfw_hal/linux/CommandExecutor.cpp similarity index 100% rename from hal/src/fsfw_hal/linux/CommandExecutor.cpp rename to src/fsfw_hal/linux/CommandExecutor.cpp diff --git a/hal/src/fsfw_hal/linux/CommandExecutor.h b/src/fsfw_hal/linux/CommandExecutor.h similarity index 100% rename from hal/src/fsfw_hal/linux/CommandExecutor.h rename to src/fsfw_hal/linux/CommandExecutor.h diff --git a/hal/src/fsfw_hal/linux/UnixFileGuard.cpp b/src/fsfw_hal/linux/UnixFileGuard.cpp similarity index 100% rename from hal/src/fsfw_hal/linux/UnixFileGuard.cpp rename to src/fsfw_hal/linux/UnixFileGuard.cpp diff --git a/hal/src/fsfw_hal/linux/UnixFileGuard.h b/src/fsfw_hal/linux/UnixFileGuard.h similarity index 100% rename from hal/src/fsfw_hal/linux/UnixFileGuard.h rename to src/fsfw_hal/linux/UnixFileGuard.h diff --git a/src/fsfw_hal/linux/gpio/CMakeLists.txt b/src/fsfw_hal/linux/gpio/CMakeLists.txt new file mode 100644 index 000000000..f6e7f2b0e --- /dev/null +++ b/src/fsfw_hal/linux/gpio/CMakeLists.txt @@ -0,0 +1,12 @@ +# This abstraction layer requires the gpiod library. You can install this +# library with "sudo apt-get install -y libgpiod-dev". If you are +# cross-compiling, you need to install the package before syncing the sysroot to +# your host computer. +find_library(LIB_GPIO gpiod) + +if(${LIB_GPIO} MATCHES LIB_GPIO-NOTFOUND) + message(STATUS "gpiod library not found, not linking against it") +else() + target_sources(${LIB_FSFW_NAME} PRIVATE LinuxLibgpioIF.cpp) + target_link_libraries(${LIB_FSFW_NAME} PRIVATE ${LIB_GPIO}) +endif() diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp b/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp similarity index 93% rename from hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp rename to src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp index a33305b6c..15061d144 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp +++ b/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp @@ -20,7 +20,9 @@ LinuxLibgpioIF::~LinuxLibgpioIF() { ReturnValue_t LinuxLibgpioIF::addGpios(GpioCookie* gpioCookie) { ReturnValue_t result; if (gpioCookie == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "LinuxLibgpioIF::addGpios: Invalid cookie" << std::endl; +#endif return RETURN_FAILED; } @@ -44,6 +46,7 @@ ReturnValue_t LinuxLibgpioIF::addGpios(GpioCookie* gpioCookie) { } ReturnValue_t LinuxLibgpioIF::configureGpios(GpioMap& mapToAdd) { + ReturnValue_t result = RETURN_OK; for (auto& gpioConfig : mapToAdd) { auto& gpioType = gpioConfig.second->gpioType; switch (gpioType) { @@ -55,7 +58,7 @@ ReturnValue_t LinuxLibgpioIF::configureGpios(GpioMap& mapToAdd) { if (regularGpio == nullptr) { return GPIO_INVALID_INSTANCE; } - configureGpioByChip(gpioConfig.first, *regularGpio); + result = configureGpioByChip(gpioConfig.first, *regularGpio); break; } case (gpio::GpioTypes::GPIO_REGULAR_BY_LABEL): { @@ -63,7 +66,7 @@ ReturnValue_t LinuxLibgpioIF::configureGpios(GpioMap& mapToAdd) { if (regularGpio == nullptr) { return GPIO_INVALID_INSTANCE; } - configureGpioByLabel(gpioConfig.first, *regularGpio); + result = configureGpioByLabel(gpioConfig.first, *regularGpio); break; } case (gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME): { @@ -71,7 +74,7 @@ ReturnValue_t LinuxLibgpioIF::configureGpios(GpioMap& mapToAdd) { if (regularGpio == nullptr) { return GPIO_INVALID_INSTANCE; } - configureGpioByLineName(gpioConfig.first, *regularGpio); + result = configureGpioByLineName(gpioConfig.first, *regularGpio); break; } case (gpio::GpioTypes::CALLBACK): { @@ -83,8 +86,11 @@ ReturnValue_t LinuxLibgpioIF::configureGpios(GpioMap& mapToAdd) { gpioCallback->initValue, gpioCallback->callbackArgs); } } + if (result != RETURN_OK) { + return GPIO_INIT_FAILED; + } } - return RETURN_OK; + return result; } ReturnValue_t LinuxLibgpioIF::configureGpioByLabel(gpioId_t gpioId, @@ -92,8 +98,10 @@ ReturnValue_t LinuxLibgpioIF::configureGpioByLabel(gpioId_t gpioId, std::string& label = gpioByLabel.label; struct gpiod_chip* chip = gpiod_chip_open_by_label(label.c_str()); if (chip == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "LinuxLibgpioIF::configureGpioByLabel: Failed to open gpio from gpio " << "group with label " << label << ". Gpio ID: " << gpioId << std::endl; +#endif return RETURN_FAILED; } std::string failOutput = "label: " + label; @@ -104,8 +112,10 @@ ReturnValue_t LinuxLibgpioIF::configureGpioByChip(gpioId_t gpioId, GpiodRegularB std::string& chipname = gpioByChip.chipname; struct gpiod_chip* chip = gpiod_chip_open_by_name(chipname.c_str()); if (chip == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "LinuxLibgpioIF::configureGpioByChip: Failed to open chip " << chipname << ". Gpio ID: " << gpioId << std::endl; +#endif return RETURN_FAILED; } std::string failOutput = "chipname: " + chipname; @@ -129,8 +139,10 @@ ReturnValue_t LinuxLibgpioIF::configureGpioByLineName(gpioId_t gpioId, struct gpiod_chip* chip = gpiod_chip_open_by_name(chipname); if (chip == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "LinuxLibgpioIF::configureGpioByLineName: Failed to open chip " << chipname << ". (regularGpio.initValue)); break; } - case (gpio::IN): { + case (gpio::Direction::IN): { result = gpiod_line_request_input(lineHandle, consumer.c_str()); break; } default: { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "LinuxLibgpioIF::configureGpios: Invalid direction specified" << std::endl; +#endif return GPIO_INVALID_INSTANCE; } @@ -199,7 +216,9 @@ ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, struct gpiod ReturnValue_t LinuxLibgpioIF::pullHigh(gpioId_t gpioId) { gpioMapIter = gpioMap.find(gpioId); if (gpioMapIter == gpioMap.end()) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "LinuxLibgpioIF::pullHigh: Unknown GPIO ID " << gpioId << std::endl; +#endif return UNKNOWN_GPIO_ID; } @@ -211,7 +230,7 @@ ReturnValue_t LinuxLibgpioIF::pullHigh(gpioId_t gpioId) { if (regularGpio == nullptr) { return GPIO_TYPE_FAILURE; } - return driveGpio(gpioId, *regularGpio, gpio::HIGH); + return driveGpio(gpioId, *regularGpio, gpio::Levels::HIGH); } else { auto gpioCallback = dynamic_cast(gpioMapIter->second); if (gpioCallback->callback == nullptr) { @@ -243,7 +262,7 @@ ReturnValue_t LinuxLibgpioIF::pullLow(gpioId_t gpioId) { if (regularGpio == nullptr) { return GPIO_TYPE_FAILURE; } - return driveGpio(gpioId, *regularGpio, gpio::LOW); + return driveGpio(gpioId, *regularGpio, gpio::Levels::LOW); } else { auto gpioCallback = dynamic_cast(gpioMapIter->second); if (gpioCallback->callback == nullptr) { @@ -258,11 +277,11 @@ ReturnValue_t LinuxLibgpioIF::pullLow(gpioId_t gpioId) { ReturnValue_t LinuxLibgpioIF::driveGpio(gpioId_t gpioId, GpiodRegularBase& regularGpio, gpio::Levels logicLevel) { - int result = gpiod_line_set_value(regularGpio.lineHandle, logicLevel); + int result = gpiod_line_set_value(regularGpio.lineHandle, static_cast(logicLevel)); if (result < 0) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "LinuxLibgpioIF::driveGpio: Failed to pull GPIO with ID " << gpioId - << " to logic level " << logicLevel << std::endl; + << " to logic level " << static_cast(logicLevel) << std::endl; #else sif::printWarning( "LinuxLibgpioIF::driveGpio: Failed to pull GPIO with ID %d to " diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h b/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h similarity index 96% rename from hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h rename to src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h index 7d49e6e20..fcc9c7753 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +++ b/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h @@ -29,6 +29,8 @@ class LinuxLibgpioIF : public GpioIF, public SystemObject { HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 4); static constexpr ReturnValue_t GPIO_DUPLICATE_DETECTED = HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 5); + static constexpr ReturnValue_t GPIO_INIT_FAILED = + HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 6); LinuxLibgpioIF(object_id_t objectId); virtual ~LinuxLibgpioIF(); diff --git a/src/fsfw_hal/linux/i2c/CMakeLists.txt b/src/fsfw_hal/linux/i2c/CMakeLists.txt new file mode 100644 index 000000000..b94bdc991 --- /dev/null +++ b/src/fsfw_hal/linux/i2c/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PUBLIC I2cComIF.cpp I2cCookie.cpp) diff --git a/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp similarity index 94% rename from hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp rename to src/fsfw_hal/linux/i2c/I2cComIF.cpp index dc23542d8..4f53dc1f7 100644 --- a/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -1,4 +1,13 @@ -#include "fsfw_hal/linux/i2c/I2cComIF.h" +#include "I2cComIF.h" + +#include "fsfw/FSFW.h" +#include "fsfw/serviceinterface.h" +#include "fsfw_hal/linux/UnixFileGuard.h" +#include "fsfw_hal/linux/utility.h" + +#if FSFW_HAL_I2C_WIRETAPPING == 1 +#include "fsfw/globalfunctions/arrayprinter.h" +#endif #include #include @@ -8,11 +17,6 @@ #include -#include "fsfw/FSFW.h" -#include "fsfw/serviceinterface.h" -#include "fsfw_hal/linux/UnixFileGuard.h" -#include "fsfw_hal/linux/utility.h" - I2cComIF::I2cComIF(object_id_t objectId) : SystemObject(objectId) {} I2cComIF::~I2cComIF() {} @@ -112,6 +116,11 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s #endif return HasReturnvaluesIF::RETURN_FAILED; } + +#if FSFW_HAL_I2C_WIRETAPPING == 1 + sif::info << "Sent I2C data to bus " << deviceFile << ":" << std::endl; + arrayprinter::print(sendData, sendLen); +#endif return HasReturnvaluesIF::RETURN_OK; } @@ -176,6 +185,11 @@ ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe return HasReturnvaluesIF::RETURN_FAILED; } +#if FSFW_HAL_I2C_WIRETAPPING == 1 + sif::info << "I2C read bytes from bus " << deviceFile << ":" << std::endl; + arrayprinter::print(replyBuffer, requestLen); +#endif + i2cDeviceMapIter->second.replyLen = requestLen; return HasReturnvaluesIF::RETURN_OK; } diff --git a/hal/src/fsfw_hal/linux/i2c/I2cComIF.h b/src/fsfw_hal/linux/i2c/I2cComIF.h similarity index 100% rename from hal/src/fsfw_hal/linux/i2c/I2cComIF.h rename to src/fsfw_hal/linux/i2c/I2cComIF.h diff --git a/hal/src/fsfw_hal/linux/i2c/I2cCookie.cpp b/src/fsfw_hal/linux/i2c/I2cCookie.cpp similarity index 100% rename from hal/src/fsfw_hal/linux/i2c/I2cCookie.cpp rename to src/fsfw_hal/linux/i2c/I2cCookie.cpp diff --git a/hal/src/fsfw_hal/linux/i2c/I2cCookie.h b/src/fsfw_hal/linux/i2c/I2cCookie.h similarity index 100% rename from hal/src/fsfw_hal/linux/i2c/I2cCookie.h rename to src/fsfw_hal/linux/i2c/I2cCookie.h diff --git a/src/fsfw_hal/linux/rpi/CMakeLists.txt b/src/fsfw_hal/linux/rpi/CMakeLists.txt new file mode 100644 index 000000000..3a865037c --- /dev/null +++ b/src/fsfw_hal/linux/rpi/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE GpioRPi.cpp) diff --git a/hal/src/fsfw_hal/linux/rpi/GpioRPi.cpp b/src/fsfw_hal/linux/rpi/GpioRPi.cpp similarity index 95% rename from hal/src/fsfw_hal/linux/rpi/GpioRPi.cpp rename to src/fsfw_hal/linux/rpi/GpioRPi.cpp index c005e24f8..d3c0a5776 100644 --- a/hal/src/fsfw_hal/linux/rpi/GpioRPi.cpp +++ b/src/fsfw_hal/linux/rpi/GpioRPi.cpp @@ -7,7 +7,7 @@ ReturnValue_t gpio::createRpiGpioConfig(GpioCookie* cookie, gpioId_t gpioId, int bcmPin, std::string consumer, gpio::Direction direction, - int initValue) { + gpio::Levels initValue) { if (cookie == nullptr) { return HasReturnvaluesIF::RETURN_FAILED; } diff --git a/hal/src/fsfw_hal/linux/rpi/GpioRPi.h b/src/fsfw_hal/linux/rpi/GpioRPi.h similarity index 91% rename from hal/src/fsfw_hal/linux/rpi/GpioRPi.h rename to src/fsfw_hal/linux/rpi/GpioRPi.h index 499f984bd..8ca7065a9 100644 --- a/hal/src/fsfw_hal/linux/rpi/GpioRPi.h +++ b/src/fsfw_hal/linux/rpi/GpioRPi.h @@ -21,7 +21,8 @@ namespace gpio { * @return */ ReturnValue_t createRpiGpioConfig(GpioCookie* cookie, gpioId_t gpioId, int bcmPin, - std::string consumer, gpio::Direction direction, int initValue); + std::string consumer, gpio::Direction direction, + gpio::Levels initValue); } // namespace gpio #endif /* BSP_RPI_GPIO_GPIORPI_H_ */ diff --git a/src/fsfw_hal/linux/spi/CMakeLists.txt b/src/fsfw_hal/linux/spi/CMakeLists.txt new file mode 100644 index 000000000..f242bb3bc --- /dev/null +++ b/src/fsfw_hal/linux/spi/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PUBLIC SpiComIF.cpp SpiCookie.cpp) diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/src/fsfw_hal/linux/spi/SpiComIF.cpp similarity index 96% rename from hal/src/fsfw_hal/linux/spi/SpiComIF.cpp rename to src/fsfw_hal/linux/spi/SpiComIF.cpp index d95232c1e..b06def693 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -210,7 +210,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const #endif return result; } - ReturnValue_t result = gpioComIF->pullLow(gpioId); + result = gpioComIF->pullLow(gpioId); if (result != HasReturnvaluesIF::RETURN_OK) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -401,4 +401,12 @@ void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) if (retval != 0) { utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Setting SPI speed failed"); } + // This updates the SPI clock default polarity. Only setting the mode does not update + // the line state, which can be an issue on mode switches because the clock line will + // switch the state after the chip select is pulled low + clockUpdateTransfer.len = 0; + retval = ioctl(spiFd, SPI_IOC_MESSAGE(1), &clockUpdateTransfer); + if (retval != 0) { + utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Updating SPI default clock failed"); + } } diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.h b/src/fsfw_hal/linux/spi/SpiComIF.h similarity index 98% rename from hal/src/fsfw_hal/linux/spi/SpiComIF.h rename to src/fsfw_hal/linux/spi/SpiComIF.h index 1f825d526..357afa2f7 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.h +++ b/src/fsfw_hal/linux/spi/SpiComIF.h @@ -74,6 +74,7 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { MutexIF* spiMutex = nullptr; MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; uint32_t timeoutMs = 20; + spi_ioc_transfer clockUpdateTransfer = {}; using SpiDeviceMap = std::unordered_map; using SpiDeviceMapIter = SpiDeviceMap::iterator; diff --git a/hal/src/fsfw_hal/linux/spi/SpiCookie.cpp b/src/fsfw_hal/linux/spi/SpiCookie.cpp similarity index 99% rename from hal/src/fsfw_hal/linux/spi/SpiCookie.cpp rename to src/fsfw_hal/linux/spi/SpiCookie.cpp index e6271c5ee..c94fcdf1b 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiCookie.cpp +++ b/src/fsfw_hal/linux/spi/SpiCookie.cpp @@ -1,4 +1,4 @@ -#include "fsfw_hal/linux/spi/SpiCookie.h" +#include "SpiCookie.h" SpiCookie::SpiCookie(address_t spiAddress, gpioId_t chipSelect, std::string spiDev, const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed) diff --git a/hal/src/fsfw_hal/linux/spi/SpiCookie.h b/src/fsfw_hal/linux/spi/SpiCookie.h similarity index 100% rename from hal/src/fsfw_hal/linux/spi/SpiCookie.h rename to src/fsfw_hal/linux/spi/SpiCookie.h diff --git a/hal/src/fsfw_hal/linux/spi/spiDefinitions.h b/src/fsfw_hal/linux/spi/spiDefinitions.h similarity index 100% rename from hal/src/fsfw_hal/linux/spi/spiDefinitions.h rename to src/fsfw_hal/linux/spi/spiDefinitions.h diff --git a/src/fsfw_hal/linux/uart/CMakeLists.txt b/src/fsfw_hal/linux/uart/CMakeLists.txt new file mode 100644 index 000000000..9cad62a40 --- /dev/null +++ b/src/fsfw_hal/linux/uart/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PUBLIC UartComIF.cpp UartCookie.cpp) diff --git a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp b/src/fsfw_hal/linux/uart/UartComIF.cpp similarity index 88% rename from hal/src/fsfw_hal/linux/uart/UartComIF.cpp rename to src/fsfw_hal/linux/uart/UartComIF.cpp index a648df3ad..f77bdeae6 100644 --- a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/src/fsfw_hal/linux/uart/UartComIF.cpp @@ -148,16 +148,16 @@ void UartComIF::setDatasizeOptions(struct termios* options, UartCookie* uartCook /* Clear size bits */ options->c_cflag &= ~CSIZE; switch (uartCookie->getBitsPerWord()) { - case 5: + case BitsPerWord::BITS_5: options->c_cflag |= CS5; break; - case 6: + case BitsPerWord::BITS_6: options->c_cflag |= CS6; break; - case 7: + case BitsPerWord::BITS_7: options->c_cflag |= CS7; break; - case 8: + case BitsPerWord::BITS_8: options->c_cflag |= CS8; break; default: @@ -193,82 +193,128 @@ void UartComIF::setFixedOptions(struct termios* options) { void UartComIF::configureBaudrate(struct termios* options, UartCookie* uartCookie) { switch (uartCookie->getBaudrate()) { - case 50: + case UartBaudRate::RATE_50: cfsetispeed(options, B50); cfsetospeed(options, B50); break; - case 75: + case UartBaudRate::RATE_75: cfsetispeed(options, B75); cfsetospeed(options, B75); break; - case 110: + case UartBaudRate::RATE_110: cfsetispeed(options, B110); cfsetospeed(options, B110); break; - case 134: + case UartBaudRate::RATE_134: cfsetispeed(options, B134); cfsetospeed(options, B134); break; - case 150: + case UartBaudRate::RATE_150: cfsetispeed(options, B150); cfsetospeed(options, B150); break; - case 200: + case UartBaudRate::RATE_200: cfsetispeed(options, B200); cfsetospeed(options, B200); break; - case 300: + case UartBaudRate::RATE_300: cfsetispeed(options, B300); cfsetospeed(options, B300); break; - case 600: + case UartBaudRate::RATE_600: cfsetispeed(options, B600); cfsetospeed(options, B600); break; - case 1200: + case UartBaudRate::RATE_1200: cfsetispeed(options, B1200); cfsetospeed(options, B1200); break; - case 1800: + case UartBaudRate::RATE_1800: cfsetispeed(options, B1800); cfsetospeed(options, B1800); break; - case 2400: + case UartBaudRate::RATE_2400: cfsetispeed(options, B2400); cfsetospeed(options, B2400); break; - case 4800: + case UartBaudRate::RATE_4800: cfsetispeed(options, B4800); cfsetospeed(options, B4800); break; - case 9600: + case UartBaudRate::RATE_9600: cfsetispeed(options, B9600); cfsetospeed(options, B9600); break; - case 19200: + case UartBaudRate::RATE_19200: cfsetispeed(options, B19200); cfsetospeed(options, B19200); break; - case 38400: + case UartBaudRate::RATE_38400: cfsetispeed(options, B38400); cfsetospeed(options, B38400); break; - case 57600: + case UartBaudRate::RATE_57600: cfsetispeed(options, B57600); cfsetospeed(options, B57600); break; - case 115200: + case UartBaudRate::RATE_115200: cfsetispeed(options, B115200); cfsetospeed(options, B115200); break; - case 230400: + case UartBaudRate::RATE_230400: cfsetispeed(options, B230400); cfsetospeed(options, B230400); break; - case 460800: +#ifndef __APPLE__ + case UartBaudRate::RATE_460800: cfsetispeed(options, B460800); cfsetospeed(options, B460800); break; + case UartBaudRate::RATE_500000: + cfsetispeed(options, B500000); + cfsetospeed(options, B500000); + break; + case UartBaudRate::RATE_576000: + cfsetispeed(options, B576000); + cfsetospeed(options, B576000); + break; + case UartBaudRate::RATE_921600: + cfsetispeed(options, B921600); + cfsetospeed(options, B921600); + break; + case UartBaudRate::RATE_1000000: + cfsetispeed(options, B1000000); + cfsetospeed(options, B1000000); + break; + case UartBaudRate::RATE_1152000: + cfsetispeed(options, B1152000); + cfsetospeed(options, B1152000); + break; + case UartBaudRate::RATE_1500000: + cfsetispeed(options, B1500000); + cfsetospeed(options, B1500000); + break; + case UartBaudRate::RATE_2000000: + cfsetispeed(options, B2000000); + cfsetospeed(options, B2000000); + break; + case UartBaudRate::RATE_2500000: + cfsetispeed(options, B2500000); + cfsetospeed(options, B2500000); + break; + case UartBaudRate::RATE_3000000: + cfsetispeed(options, B3000000); + cfsetospeed(options, B3000000); + break; + case UartBaudRate::RATE_3500000: + cfsetispeed(options, B3500000); + cfsetospeed(options, B3500000); + break; + case UartBaudRate::RATE_4000000: + cfsetispeed(options, B4000000); + cfsetospeed(options, B4000000); + break; +#endif // ! __APPLE__ default: #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::configureBaudrate: Baudrate not supported" << std::endl; @@ -427,7 +473,7 @@ ReturnValue_t UartComIF::handleNoncanonicalRead(UartCookie& uartCookie, UartDevi auto bufferPtr = iter->second.replyBuffer.data(); // Size check to prevent buffer overflow if (requestLen > uartCookie.getMaxReplyLen()) { -#if OBSW_VERBOSE_LEVEL >= 1 +#if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::requestReceiveMessage: Next read would cause overflow!" << std::endl; diff --git a/hal/src/fsfw_hal/linux/uart/UartComIF.h b/src/fsfw_hal/linux/uart/UartComIF.h similarity index 100% rename from hal/src/fsfw_hal/linux/uart/UartComIF.h rename to src/fsfw_hal/linux/uart/UartComIF.h diff --git a/hal/src/fsfw_hal/linux/uart/UartCookie.cpp b/src/fsfw_hal/linux/uart/UartCookie.cpp similarity index 70% rename from hal/src/fsfw_hal/linux/uart/UartCookie.cpp rename to src/fsfw_hal/linux/uart/UartCookie.cpp index 99e80a8e7..3fedc9d44 100644 --- a/hal/src/fsfw_hal/linux/uart/UartCookie.cpp +++ b/src/fsfw_hal/linux/uart/UartCookie.cpp @@ -1,9 +1,9 @@ -#include "fsfw_hal/linux/uart/UartCookie.h" +#include "UartCookie.h" #include UartCookie::UartCookie(object_id_t handlerId, std::string deviceFile, UartModes uartMode, - uint32_t baudrate, size_t maxReplyLen) + UartBaudRate baudrate, size_t maxReplyLen) : handlerId(handlerId), deviceFile(deviceFile), uartMode(uartMode), @@ -12,7 +12,7 @@ UartCookie::UartCookie(object_id_t handlerId, std::string deviceFile, UartModes UartCookie::~UartCookie() {} -uint32_t UartCookie::getBaudrate() const { return baudrate; } +UartBaudRate UartCookie::getBaudrate() const { return baudrate; } size_t UartCookie::getMaxReplyLen() const { return maxReplyLen; } @@ -24,23 +24,9 @@ void UartCookie::setParityEven() { parity = Parity::EVEN; } Parity UartCookie::getParity() const { return parity; } -void UartCookie::setBitsPerWord(uint8_t bitsPerWord_) { - switch (bitsPerWord_) { - case 5: - case 6: - case 7: - case 8: - break; - default: -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::debug << "UartCookie::setBitsPerWord: Invalid bits per word specified" << std::endl; -#endif - return; - } - bitsPerWord = bitsPerWord_; -} +void UartCookie::setBitsPerWord(BitsPerWord bitsPerWord_) { bitsPerWord = bitsPerWord_; } -uint8_t UartCookie::getBitsPerWord() const { return bitsPerWord; } +BitsPerWord UartCookie::getBitsPerWord() const { return bitsPerWord; } StopBits UartCookie::getStopBits() const { return stopBits; } diff --git a/hal/src/fsfw_hal/linux/uart/UartCookie.h b/src/fsfw_hal/linux/uart/UartCookie.h similarity index 80% rename from hal/src/fsfw_hal/linux/uart/UartCookie.h rename to src/fsfw_hal/linux/uart/UartCookie.h index 884665e5f..6840b3521 100644 --- a/hal/src/fsfw_hal/linux/uart/UartCookie.h +++ b/src/fsfw_hal/linux/uart/UartCookie.h @@ -12,6 +12,41 @@ enum class StopBits { ONE_STOP_BIT, TWO_STOP_BITS }; enum class UartModes { CANONICAL, NON_CANONICAL }; +enum class BitsPerWord { BITS_5, BITS_6, BITS_7, BITS_8 }; + +enum class UartBaudRate { + RATE_50, + RATE_75, + RATE_110, + RATE_134, + RATE_150, + RATE_200, + RATE_300, + RATE_600, + RATE_1200, + RATE_1800, + RATE_2400, + RATE_4800, + RATE_9600, + RATE_19200, + RATE_38400, + RATE_57600, + RATE_115200, + RATE_230400, + RATE_460800, + RATE_500000, + RATE_576000, + RATE_921600, + RATE_1000000, + RATE_1152000, + RATE_1500000, + RATE_2000000, + RATE_2500000, + RATE_3000000, + RATE_3500000, + RATE_4000000 +}; + /** * @brief Cookie for the UartComIF. There are many options available to configure the UART driver. * The constructor only requests for common options like the baudrate. Other options can @@ -27,25 +62,23 @@ class UartCookie : public CookieIF { * @param uartMode Specify the UART mode. The canonical mode should be used if the * messages are separated by a delimited character like '\n'. See the * termios documentation for more information - * @param baudrate The baudrate to use for input and output. Possible Baudrates are: 50, - * 75, 110, 134, 150, 200, 300, 600, 1200, 1800, 2400, 4800, 9600, B19200, - * 38400, 57600, 115200, 230400, 460800 + * @param baudrate The baudrate to use for input and output. * @param maxReplyLen The maximum size an object using this cookie expects * @details * Default configuration: No parity * 8 databits (number of bits transfered with one uart frame) * One stop bit */ - UartCookie(object_id_t handlerId, std::string deviceFile, UartModes uartMode, uint32_t baudrate, - size_t maxReplyLen); + UartCookie(object_id_t handlerId, std::string deviceFile, UartModes uartMode, + UartBaudRate baudrate, size_t maxReplyLen); virtual ~UartCookie(); - uint32_t getBaudrate() const; + UartBaudRate getBaudrate() const; size_t getMaxReplyLen() const; std::string getDeviceFile() const; Parity getParity() const; - uint8_t getBitsPerWord() const; + BitsPerWord getBitsPerWord() const; StopBits getStopBits() const; UartModes getUartMode() const; object_id_t getHandlerId() const; @@ -76,7 +109,7 @@ class UartCookie : public CookieIF { /** * Function two set number of bits per UART frame. */ - void setBitsPerWord(uint8_t bitsPerWord_); + void setBitsPerWord(BitsPerWord bitsPerWord_); /** * Function to specify the number of stopbits. @@ -97,10 +130,10 @@ class UartCookie : public CookieIF { std::string deviceFile; const UartModes uartMode; bool flushInput = false; - uint32_t baudrate; + UartBaudRate baudrate; size_t maxReplyLen = 0; Parity parity = Parity::NONE; - uint8_t bitsPerWord = 8; + BitsPerWord bitsPerWord = BitsPerWord::BITS_8; uint8_t readCycles = 1; StopBits stopBits = StopBits::ONE_STOP_BIT; bool replySizeFixed = true; diff --git a/src/fsfw_hal/linux/uio/CMakeLists.txt b/src/fsfw_hal/linux/uio/CMakeLists.txt new file mode 100644 index 000000000..e3498246a --- /dev/null +++ b/src/fsfw_hal/linux/uio/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PUBLIC UioMapper.cpp) diff --git a/hal/src/fsfw_hal/linux/uio/UioMapper.cpp b/src/fsfw_hal/linux/uio/UioMapper.cpp similarity index 100% rename from hal/src/fsfw_hal/linux/uio/UioMapper.cpp rename to src/fsfw_hal/linux/uio/UioMapper.cpp diff --git a/hal/src/fsfw_hal/linux/uio/UioMapper.h b/src/fsfw_hal/linux/uio/UioMapper.h similarity index 100% rename from hal/src/fsfw_hal/linux/uio/UioMapper.h rename to src/fsfw_hal/linux/uio/UioMapper.h diff --git a/hal/src/fsfw_hal/linux/utility.cpp b/src/fsfw_hal/linux/utility.cpp similarity index 100% rename from hal/src/fsfw_hal/linux/utility.cpp rename to src/fsfw_hal/linux/utility.cpp diff --git a/hal/src/fsfw_hal/linux/utility.h b/src/fsfw_hal/linux/utility.h similarity index 100% rename from hal/src/fsfw_hal/linux/utility.h rename to src/fsfw_hal/linux/utility.h diff --git a/hal/src/fsfw_hal/stm32h7/CMakeLists.txt b/src/fsfw_hal/stm32h7/CMakeLists.txt similarity index 58% rename from hal/src/fsfw_hal/stm32h7/CMakeLists.txt rename to src/fsfw_hal/stm32h7/CMakeLists.txt index bae3b1ac8..e8843ed3b 100644 --- a/hal/src/fsfw_hal/stm32h7/CMakeLists.txt +++ b/src/fsfw_hal/stm32h7/CMakeLists.txt @@ -2,6 +2,4 @@ add_subdirectory(spi) add_subdirectory(gpio) add_subdirectory(devicetest) -target_sources(${LIB_FSFW_NAME} PRIVATE - dma.cpp -) +target_sources(${LIB_FSFW_NAME} PRIVATE dma.cpp) diff --git a/hal/src/fsfw_hal/stm32h7/definitions.h b/src/fsfw_hal/stm32h7/definitions.h similarity index 100% rename from hal/src/fsfw_hal/stm32h7/definitions.h rename to src/fsfw_hal/stm32h7/definitions.h diff --git a/src/fsfw_hal/stm32h7/devicetest/CMakeLists.txt b/src/fsfw_hal/stm32h7/devicetest/CMakeLists.txt new file mode 100644 index 000000000..8e789ddb2 --- /dev/null +++ b/src/fsfw_hal/stm32h7/devicetest/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE GyroL3GD20H.cpp) diff --git a/hal/src/fsfw_hal/stm32h7/devicetest/GyroL3GD20H.cpp b/src/fsfw_hal/stm32h7/devicetest/GyroL3GD20H.cpp similarity index 100% rename from hal/src/fsfw_hal/stm32h7/devicetest/GyroL3GD20H.cpp rename to src/fsfw_hal/stm32h7/devicetest/GyroL3GD20H.cpp diff --git a/hal/src/fsfw_hal/stm32h7/devicetest/GyroL3GD20H.h b/src/fsfw_hal/stm32h7/devicetest/GyroL3GD20H.h similarity index 94% rename from hal/src/fsfw_hal/stm32h7/devicetest/GyroL3GD20H.h rename to src/fsfw_hal/stm32h7/devicetest/GyroL3GD20H.h index a6c3376ad..a0c3748a8 100644 --- a/hal/src/fsfw_hal/stm32h7/devicetest/GyroL3GD20H.h +++ b/src/fsfw_hal/stm32h7/devicetest/GyroL3GD20H.h @@ -10,6 +10,10 @@ #include "stm32h7xx_hal.h" #include "stm32h7xx_hal_spi.h" +#ifndef STM_USE_PERIPHERAL_TX_BUFFER_MPU_PROTECTION +#define STM_USE_PERIPHERAL_TX_BUFFER_MPU_PROTECTION 1 +#endif + enum class TransferStates { IDLE, WAIT, SUCCESS, FAILURE }; class GyroL3GD20H { diff --git a/hal/src/fsfw_hal/stm32h7/dma.cpp b/src/fsfw_hal/stm32h7/dma.cpp similarity index 100% rename from hal/src/fsfw_hal/stm32h7/dma.cpp rename to src/fsfw_hal/stm32h7/dma.cpp diff --git a/hal/src/fsfw_hal/stm32h7/dma.h b/src/fsfw_hal/stm32h7/dma.h similarity index 100% rename from hal/src/fsfw_hal/stm32h7/dma.h rename to src/fsfw_hal/stm32h7/dma.h diff --git a/src/fsfw_hal/stm32h7/gpio/CMakeLists.txt b/src/fsfw_hal/stm32h7/gpio/CMakeLists.txt new file mode 100644 index 000000000..54d76b2de --- /dev/null +++ b/src/fsfw_hal/stm32h7/gpio/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE gpio.cpp) diff --git a/hal/src/fsfw_hal/stm32h7/gpio/gpio.cpp b/src/fsfw_hal/stm32h7/gpio/gpio.cpp similarity index 100% rename from hal/src/fsfw_hal/stm32h7/gpio/gpio.cpp rename to src/fsfw_hal/stm32h7/gpio/gpio.cpp diff --git a/hal/src/fsfw_hal/stm32h7/gpio/gpio.h b/src/fsfw_hal/stm32h7/gpio/gpio.h similarity index 100% rename from hal/src/fsfw_hal/stm32h7/gpio/gpio.h rename to src/fsfw_hal/stm32h7/gpio/gpio.h diff --git a/src/fsfw_hal/stm32h7/i2c/CMakeLists.txt b/src/fsfw_hal/stm32h7/i2c/CMakeLists.txt new file mode 100644 index 000000000..a0d48465e --- /dev/null +++ b/src/fsfw_hal/stm32h7/i2c/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE) diff --git a/hal/src/fsfw_hal/stm32h7/interrupts.h b/src/fsfw_hal/stm32h7/interrupts.h similarity index 100% rename from hal/src/fsfw_hal/stm32h7/interrupts.h rename to src/fsfw_hal/stm32h7/interrupts.h diff --git a/src/fsfw_hal/stm32h7/spi/CMakeLists.txt b/src/fsfw_hal/stm32h7/spi/CMakeLists.txt new file mode 100644 index 000000000..9a98f5021 --- /dev/null +++ b/src/fsfw_hal/stm32h7/spi/CMakeLists.txt @@ -0,0 +1,9 @@ +target_sources( + ${LIB_FSFW_NAME} + PRIVATE spiCore.cpp + spiDefinitions.cpp + spiInterrupts.cpp + mspInit.cpp + SpiCookie.cpp + SpiComIF.cpp + stm32h743zi.cpp) diff --git a/hal/src/fsfw_hal/stm32h7/spi/SpiComIF.cpp b/src/fsfw_hal/stm32h7/spi/SpiComIF.cpp similarity index 100% rename from hal/src/fsfw_hal/stm32h7/spi/SpiComIF.cpp rename to src/fsfw_hal/stm32h7/spi/SpiComIF.cpp diff --git a/hal/src/fsfw_hal/stm32h7/spi/SpiComIF.h b/src/fsfw_hal/stm32h7/spi/SpiComIF.h similarity index 100% rename from hal/src/fsfw_hal/stm32h7/spi/SpiComIF.h rename to src/fsfw_hal/stm32h7/spi/SpiComIF.h diff --git a/hal/src/fsfw_hal/stm32h7/spi/SpiCookie.cpp b/src/fsfw_hal/stm32h7/spi/SpiCookie.cpp similarity index 100% rename from hal/src/fsfw_hal/stm32h7/spi/SpiCookie.cpp rename to src/fsfw_hal/stm32h7/spi/SpiCookie.cpp diff --git a/hal/src/fsfw_hal/stm32h7/spi/SpiCookie.h b/src/fsfw_hal/stm32h7/spi/SpiCookie.h similarity index 100% rename from hal/src/fsfw_hal/stm32h7/spi/SpiCookie.h rename to src/fsfw_hal/stm32h7/spi/SpiCookie.h diff --git a/hal/src/fsfw_hal/stm32h7/spi/mspInit.cpp b/src/fsfw_hal/stm32h7/spi/mspInit.cpp similarity index 100% rename from hal/src/fsfw_hal/stm32h7/spi/mspInit.cpp rename to src/fsfw_hal/stm32h7/spi/mspInit.cpp diff --git a/hal/src/fsfw_hal/stm32h7/spi/mspInit.h b/src/fsfw_hal/stm32h7/spi/mspInit.h similarity index 99% rename from hal/src/fsfw_hal/stm32h7/spi/mspInit.h rename to src/fsfw_hal/stm32h7/spi/mspInit.h index 00c68017e..f0658fb91 100644 --- a/hal/src/fsfw_hal/stm32h7/spi/mspInit.h +++ b/src/fsfw_hal/stm32h7/spi/mspInit.h @@ -21,7 +21,7 @@ using mspCb = void (*)(void); namespace spi { struct MspCfgBase { - MspCfgBase(); + MspCfgBase() {} MspCfgBase(stm32h7::GpioCfg sck, stm32h7::GpioCfg mosi, stm32h7::GpioCfg miso, mspCb cleanupCb = nullptr, mspCb setupCb = nullptr) : sck(sck), mosi(mosi), miso(miso), cleanupCb(cleanupCb), setupCb(setupCb) {} diff --git a/hal/src/fsfw_hal/stm32h7/spi/spiCore.cpp b/src/fsfw_hal/stm32h7/spi/spiCore.cpp similarity index 100% rename from hal/src/fsfw_hal/stm32h7/spi/spiCore.cpp rename to src/fsfw_hal/stm32h7/spi/spiCore.cpp diff --git a/hal/src/fsfw_hal/stm32h7/spi/spiCore.h b/src/fsfw_hal/stm32h7/spi/spiCore.h similarity index 100% rename from hal/src/fsfw_hal/stm32h7/spi/spiCore.h rename to src/fsfw_hal/stm32h7/spi/spiCore.h diff --git a/hal/src/fsfw_hal/stm32h7/spi/spiDefinitions.cpp b/src/fsfw_hal/stm32h7/spi/spiDefinitions.cpp similarity index 100% rename from hal/src/fsfw_hal/stm32h7/spi/spiDefinitions.cpp rename to src/fsfw_hal/stm32h7/spi/spiDefinitions.cpp diff --git a/hal/src/fsfw_hal/stm32h7/spi/spiDefinitions.h b/src/fsfw_hal/stm32h7/spi/spiDefinitions.h similarity index 100% rename from hal/src/fsfw_hal/stm32h7/spi/spiDefinitions.h rename to src/fsfw_hal/stm32h7/spi/spiDefinitions.h diff --git a/hal/src/fsfw_hal/stm32h7/spi/spiInterrupts.cpp b/src/fsfw_hal/stm32h7/spi/spiInterrupts.cpp similarity index 100% rename from hal/src/fsfw_hal/stm32h7/spi/spiInterrupts.cpp rename to src/fsfw_hal/stm32h7/spi/spiInterrupts.cpp diff --git a/hal/src/fsfw_hal/stm32h7/spi/spiInterrupts.h b/src/fsfw_hal/stm32h7/spi/spiInterrupts.h similarity index 100% rename from hal/src/fsfw_hal/stm32h7/spi/spiInterrupts.h rename to src/fsfw_hal/stm32h7/spi/spiInterrupts.h diff --git a/hal/src/fsfw_hal/stm32h7/spi/stm32h743zi.cpp b/src/fsfw_hal/stm32h7/spi/stm32h743zi.cpp similarity index 100% rename from hal/src/fsfw_hal/stm32h7/spi/stm32h743zi.cpp rename to src/fsfw_hal/stm32h7/spi/stm32h743zi.cpp diff --git a/hal/src/fsfw_hal/stm32h7/spi/stm32h743zi.h b/src/fsfw_hal/stm32h7/spi/stm32h743zi.h similarity index 100% rename from hal/src/fsfw_hal/stm32h7/spi/stm32h743zi.h rename to src/fsfw_hal/stm32h7/spi/stm32h743zi.h diff --git a/src/fsfw_hal/stm32h7/uart/CMakeLists.txt b/src/fsfw_hal/stm32h7/uart/CMakeLists.txt new file mode 100644 index 000000000..a0d48465e --- /dev/null +++ b/src/fsfw_hal/stm32h7/uart/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE) diff --git a/src/fsfw_tests/CMakeLists.txt b/src/fsfw_tests/CMakeLists.txt new file mode 100644 index 000000000..d161699a5 --- /dev/null +++ b/src/fsfw_tests/CMakeLists.txt @@ -0,0 +1,7 @@ +if(FSFW_ADD_INTERNAL_TESTS) + add_subdirectory(internal) +endif() + +if(NOT FSFW_BUILD_TESTS) + add_subdirectory(integration) +endif() diff --git a/tests/src/fsfw_tests/integration/CMakeLists.txt b/src/fsfw_tests/integration/CMakeLists.txt similarity index 100% rename from tests/src/fsfw_tests/integration/CMakeLists.txt rename to src/fsfw_tests/integration/CMakeLists.txt diff --git a/src/fsfw_tests/integration/assemblies/CMakeLists.txt b/src/fsfw_tests/integration/assemblies/CMakeLists.txt new file mode 100644 index 000000000..63a6447aa --- /dev/null +++ b/src/fsfw_tests/integration/assemblies/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE TestAssembly.cpp) diff --git a/tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp b/src/fsfw_tests/integration/assemblies/TestAssembly.cpp similarity index 100% rename from tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp rename to src/fsfw_tests/integration/assemblies/TestAssembly.cpp diff --git a/tests/src/fsfw_tests/integration/assemblies/TestAssembly.h b/src/fsfw_tests/integration/assemblies/TestAssembly.h similarity index 100% rename from tests/src/fsfw_tests/integration/assemblies/TestAssembly.h rename to src/fsfw_tests/integration/assemblies/TestAssembly.h diff --git a/src/fsfw_tests/integration/controller/CMakeLists.txt b/src/fsfw_tests/integration/controller/CMakeLists.txt new file mode 100644 index 000000000..5eeb5c68c --- /dev/null +++ b/src/fsfw_tests/integration/controller/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE TestController.cpp) diff --git a/tests/src/fsfw_tests/integration/controller/TestController.cpp b/src/fsfw_tests/integration/controller/TestController.cpp similarity index 100% rename from tests/src/fsfw_tests/integration/controller/TestController.cpp rename to src/fsfw_tests/integration/controller/TestController.cpp diff --git a/tests/src/fsfw_tests/integration/controller/TestController.h b/src/fsfw_tests/integration/controller/TestController.h similarity index 100% rename from tests/src/fsfw_tests/integration/controller/TestController.h rename to src/fsfw_tests/integration/controller/TestController.h diff --git a/tests/src/fsfw_tests/integration/controller/ctrldefinitions/testCtrlDefinitions.h b/src/fsfw_tests/integration/controller/ctrldefinitions/testCtrlDefinitions.h similarity index 100% rename from tests/src/fsfw_tests/integration/controller/ctrldefinitions/testCtrlDefinitions.h rename to src/fsfw_tests/integration/controller/ctrldefinitions/testCtrlDefinitions.h diff --git a/src/fsfw_tests/integration/devices/CMakeLists.txt b/src/fsfw_tests/integration/devices/CMakeLists.txt new file mode 100644 index 000000000..d57991974 --- /dev/null +++ b/src/fsfw_tests/integration/devices/CMakeLists.txt @@ -0,0 +1,2 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE TestCookie.cpp TestDeviceHandler.cpp + TestEchoComIF.cpp) diff --git a/tests/src/fsfw_tests/integration/devices/TestCookie.cpp b/src/fsfw_tests/integration/devices/TestCookie.cpp similarity index 100% rename from tests/src/fsfw_tests/integration/devices/TestCookie.cpp rename to src/fsfw_tests/integration/devices/TestCookie.cpp diff --git a/tests/src/fsfw_tests/integration/devices/TestCookie.h b/src/fsfw_tests/integration/devices/TestCookie.h similarity index 100% rename from tests/src/fsfw_tests/integration/devices/TestCookie.h rename to src/fsfw_tests/integration/devices/TestCookie.h diff --git a/tests/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp b/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp similarity index 98% rename from tests/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp rename to src/fsfw_tests/integration/devices/TestDeviceHandler.cpp index 41098723d..36f1ea8c8 100644 --- a/tests/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp +++ b/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp @@ -13,14 +13,14 @@ TestDevice::TestDevice(object_id_t objectId, object_id_t comIF, CookieIF* cookie dataset(this), fullInfoPrintout(fullInfoPrintout) {} -TestDevice::~TestDevice() {} +TestDevice::~TestDevice() = default; void TestDevice::performOperationHook() { if (periodicPrintout) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::info << "TestDevice" << deviceIdx << "::performOperationHook: Alive!" << std::endl; #else - sif::printInfo("TestDevice%d::performOperationHook: Alive!", deviceIdx); + sif::printInfo("TestDevice%d::performOperationHook: Alive!\n", deviceIdx); #endif } @@ -208,7 +208,7 @@ ReturnValue_t TestDevice::buildNormalModeCommand(DeviceCommandId_t deviceCommand const uint8_t* commandData, size_t commandDataLen) { if (fullInfoPrintout) { -#if OBSW_VERBOSE_LEVEL >= 3 +#if FSFW_VERBOSE_LEVEL >= 3 #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::info << "TestDevice::buildTestCommand1: Building normal command" << std::endl; #else @@ -351,7 +351,7 @@ ReturnValue_t TestDevice::scanForReply(const uint8_t* start, size_t len, DeviceC switch (pendingCmd) { case (TEST_NORMAL_MODE_CMD): { if (fullInfoPrintout) { -#if OBSW_VERBOSE_LEVEL >= 3 +#if FSFW_VERBOSE_LEVEL >= 3 #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::info << "TestDevice::scanForReply: Reply for normal commnand (ID " << TEST_NORMAL_MODE_CMD << ") received!" << std::endl; @@ -678,7 +678,6 @@ ReturnValue_t TestDevice::getParameter(uint8_t domainId, uint8_t uniqueId, int32_t newValue = 0; ReturnValue_t result = newValues->getElement(&newValue, 0, 0); if (result == HasReturnvaluesIF::RETURN_OK) { -#if OBSW_DEVICE_HANDLER_PRINTOUT == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::info << "TestDevice" << deviceIdx << "::getParameter: Setting parameter 1 to " @@ -688,7 +687,6 @@ ReturnValue_t TestDevice::getParameter(uint8_t domainId, uint8_t uniqueId, sif::printInfo("TestDevice%d::getParameter: Setting parameter 1 to new value %lu\n", deviceIdx, static_cast(newValue)); #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ -#endif /* OBSW_DEVICE_HANDLER_PRINTOUT == 1 */ } } parameterWrapper->set(testParameter1); @@ -702,7 +700,6 @@ ReturnValue_t TestDevice::getParameter(uint8_t domainId, uint8_t uniqueId, newValues->getElement(newVector + 2, 0, 2) != RETURN_OK) { return HasReturnvaluesIF::RETURN_FAILED; } -#if OBSW_DEVICE_HANDLER_PRINTOUT == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::info << "TestDevice" << deviceIdx << "::getParameter: Setting parameter 3 to " @@ -715,7 +712,6 @@ ReturnValue_t TestDevice::getParameter(uint8_t domainId, uint8_t uniqueId, "[%f, %f, %f]\n", deviceIdx, newVector[0], newVector[1], newVector[2]); #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ -#endif /* OBSW_DEVICE_HANDLER_PRINTOUT == 1 */ } parameterWrapper->setVector(vectorFloatParams2); break; diff --git a/tests/src/fsfw_tests/integration/devices/TestDeviceHandler.h b/src/fsfw_tests/integration/devices/TestDeviceHandler.h similarity index 100% rename from tests/src/fsfw_tests/integration/devices/TestDeviceHandler.h rename to src/fsfw_tests/integration/devices/TestDeviceHandler.h diff --git a/tests/src/fsfw_tests/integration/devices/TestEchoComIF.cpp b/src/fsfw_tests/integration/devices/TestEchoComIF.cpp similarity index 100% rename from tests/src/fsfw_tests/integration/devices/TestEchoComIF.cpp rename to src/fsfw_tests/integration/devices/TestEchoComIF.cpp diff --git a/tests/src/fsfw_tests/integration/devices/TestEchoComIF.h b/src/fsfw_tests/integration/devices/TestEchoComIF.h similarity index 100% rename from tests/src/fsfw_tests/integration/devices/TestEchoComIF.h rename to src/fsfw_tests/integration/devices/TestEchoComIF.h diff --git a/tests/src/fsfw_tests/integration/devices/devicedefinitions/testDeviceDefinitions.h b/src/fsfw_tests/integration/devices/devicedefinitions/testDeviceDefinitions.h similarity index 100% rename from tests/src/fsfw_tests/integration/devices/devicedefinitions/testDeviceDefinitions.h rename to src/fsfw_tests/integration/devices/devicedefinitions/testDeviceDefinitions.h diff --git a/src/fsfw_tests/integration/task/CMakeLists.txt b/src/fsfw_tests/integration/task/CMakeLists.txt new file mode 100644 index 000000000..62d5e8efa --- /dev/null +++ b/src/fsfw_tests/integration/task/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE TestTask.cpp) diff --git a/tests/src/fsfw_tests/integration/task/TestTask.cpp b/src/fsfw_tests/integration/task/TestTask.cpp similarity index 96% rename from tests/src/fsfw_tests/integration/task/TestTask.cpp rename to src/fsfw_tests/integration/task/TestTask.cpp index 65e444e38..a6a4a30b0 100644 --- a/tests/src/fsfw_tests/integration/task/TestTask.cpp +++ b/src/fsfw_tests/integration/task/TestTask.cpp @@ -3,7 +3,6 @@ #include #include -bool TestTask::oneShotAction = true; MutexIF* TestTask::testLock = nullptr; TestTask::TestTask(object_id_t objectId) : SystemObject(objectId), testMode(testModes::A) { @@ -13,7 +12,7 @@ TestTask::TestTask(object_id_t objectId) : SystemObject(objectId), testMode(test IPCStore = ObjectManager::instance()->get(objects::IPC_STORE); } -TestTask::~TestTask() {} +TestTask::~TestTask() = default; ReturnValue_t TestTask::performOperation(uint8_t operationCode) { ReturnValue_t result = RETURN_OK; diff --git a/tests/src/fsfw_tests/integration/task/TestTask.h b/src/fsfw_tests/integration/task/TestTask.h similarity index 85% rename from tests/src/fsfw_tests/integration/task/TestTask.h rename to src/fsfw_tests/integration/task/TestTask.h index 557b50b21..038355c32 100644 --- a/tests/src/fsfw_tests/integration/task/TestTask.h +++ b/src/fsfw_tests/integration/task/TestTask.h @@ -13,9 +13,9 @@ */ class TestTask : public SystemObject, public ExecutableObjectIF, public HasReturnvaluesIF { public: - TestTask(object_id_t objectId); - virtual ~TestTask(); - virtual ReturnValue_t performOperation(uint8_t operationCode = 0) override; + explicit TestTask(object_id_t objectId); + ~TestTask() override; + ReturnValue_t performOperation(uint8_t operationCode) override; protected: virtual ReturnValue_t performOneShotAction(); @@ -29,7 +29,7 @@ class TestTask : public SystemObject, public ExecutableObjectIF, public HasRetur bool testFlag = false; private: - static bool oneShotAction; + bool oneShotAction = true; static MutexIF* testLock; StorageManagerIF* IPCStore; }; diff --git a/src/fsfw_tests/internal/CMakeLists.txt b/src/fsfw_tests/internal/CMakeLists.txt new file mode 100644 index 000000000..c1af5467b --- /dev/null +++ b/src/fsfw_tests/internal/CMakeLists.txt @@ -0,0 +1,6 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE InternalUnitTester.cpp + UnittDefinitions.cpp) + +add_subdirectory(osal) +add_subdirectory(serialize) +add_subdirectory(globalfunctions) diff --git a/tests/src/fsfw_tests/internal/InternalUnitTester.cpp b/src/fsfw_tests/internal/InternalUnitTester.cpp similarity index 100% rename from tests/src/fsfw_tests/internal/InternalUnitTester.cpp rename to src/fsfw_tests/internal/InternalUnitTester.cpp diff --git a/tests/src/fsfw_tests/internal/InternalUnitTester.h b/src/fsfw_tests/internal/InternalUnitTester.h similarity index 100% rename from tests/src/fsfw_tests/internal/InternalUnitTester.h rename to src/fsfw_tests/internal/InternalUnitTester.h diff --git a/tests/src/fsfw_tests/internal/UnittDefinitions.cpp b/src/fsfw_tests/internal/UnittDefinitions.cpp similarity index 100% rename from tests/src/fsfw_tests/internal/UnittDefinitions.cpp rename to src/fsfw_tests/internal/UnittDefinitions.cpp diff --git a/tests/src/fsfw_tests/internal/UnittDefinitions.h b/src/fsfw_tests/internal/UnittDefinitions.h similarity index 100% rename from tests/src/fsfw_tests/internal/UnittDefinitions.h rename to src/fsfw_tests/internal/UnittDefinitions.h diff --git a/src/fsfw_tests/internal/globalfunctions/CMakeLists.txt b/src/fsfw_tests/internal/globalfunctions/CMakeLists.txt new file mode 100644 index 000000000..6e7e58ade --- /dev/null +++ b/src/fsfw_tests/internal/globalfunctions/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE TestArrayPrinter.cpp) diff --git a/tests/src/fsfw_tests/internal/globalfunctions/TestArrayPrinter.cpp b/src/fsfw_tests/internal/globalfunctions/TestArrayPrinter.cpp similarity index 100% rename from tests/src/fsfw_tests/internal/globalfunctions/TestArrayPrinter.cpp rename to src/fsfw_tests/internal/globalfunctions/TestArrayPrinter.cpp diff --git a/tests/src/fsfw_tests/internal/globalfunctions/TestArrayPrinter.h b/src/fsfw_tests/internal/globalfunctions/TestArrayPrinter.h similarity index 100% rename from tests/src/fsfw_tests/internal/globalfunctions/TestArrayPrinter.h rename to src/fsfw_tests/internal/globalfunctions/TestArrayPrinter.h diff --git a/src/fsfw_tests/internal/osal/CMakeLists.txt b/src/fsfw_tests/internal/osal/CMakeLists.txt new file mode 100644 index 000000000..db1031a05 --- /dev/null +++ b/src/fsfw_tests/internal/osal/CMakeLists.txt @@ -0,0 +1,2 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE testMq.cpp testMutex.cpp + testSemaphore.cpp) diff --git a/tests/src/fsfw_tests/internal/osal/testMq.cpp b/src/fsfw_tests/internal/osal/testMq.cpp similarity index 100% rename from tests/src/fsfw_tests/internal/osal/testMq.cpp rename to src/fsfw_tests/internal/osal/testMq.cpp diff --git a/tests/src/fsfw_tests/internal/osal/testMq.h b/src/fsfw_tests/internal/osal/testMq.h similarity index 100% rename from tests/src/fsfw_tests/internal/osal/testMq.h rename to src/fsfw_tests/internal/osal/testMq.h diff --git a/tests/src/fsfw_tests/internal/osal/testMutex.cpp b/src/fsfw_tests/internal/osal/testMutex.cpp similarity index 100% rename from tests/src/fsfw_tests/internal/osal/testMutex.cpp rename to src/fsfw_tests/internal/osal/testMutex.cpp diff --git a/tests/src/fsfw_tests/internal/osal/testMutex.h b/src/fsfw_tests/internal/osal/testMutex.h similarity index 100% rename from tests/src/fsfw_tests/internal/osal/testMutex.h rename to src/fsfw_tests/internal/osal/testMutex.h diff --git a/tests/src/fsfw_tests/internal/osal/testSemaphore.cpp b/src/fsfw_tests/internal/osal/testSemaphore.cpp similarity index 100% rename from tests/src/fsfw_tests/internal/osal/testSemaphore.cpp rename to src/fsfw_tests/internal/osal/testSemaphore.cpp diff --git a/tests/src/fsfw_tests/internal/osal/testSemaphore.h b/src/fsfw_tests/internal/osal/testSemaphore.h similarity index 100% rename from tests/src/fsfw_tests/internal/osal/testSemaphore.h rename to src/fsfw_tests/internal/osal/testSemaphore.h diff --git a/src/fsfw_tests/internal/serialize/CMakeLists.txt b/src/fsfw_tests/internal/serialize/CMakeLists.txt new file mode 100644 index 000000000..ee264d9d3 --- /dev/null +++ b/src/fsfw_tests/internal/serialize/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE IntTestSerialization.cpp) diff --git a/tests/src/fsfw_tests/internal/serialize/IntTestSerialization.cpp b/src/fsfw_tests/internal/serialize/IntTestSerialization.cpp similarity index 100% rename from tests/src/fsfw_tests/internal/serialize/IntTestSerialization.cpp rename to src/fsfw_tests/internal/serialize/IntTestSerialization.cpp diff --git a/tests/src/fsfw_tests/internal/serialize/IntTestSerialization.h b/src/fsfw_tests/internal/serialize/IntTestSerialization.h similarity index 100% rename from tests/src/fsfw_tests/internal/serialize/IntTestSerialization.h rename to src/fsfw_tests/internal/serialize/IntTestSerialization.h diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt deleted file mode 100644 index febd4f0ab..000000000 --- a/tests/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -add_subdirectory(src) diff --git a/tests/src/CMakeLists.txt b/tests/src/CMakeLists.txt deleted file mode 100644 index 6673f1e41..000000000 --- a/tests/src/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -target_include_directories(${LIB_FSFW_NAME} PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR} -) - -target_include_directories(${LIB_FSFW_NAME} INTERFACE - ${CMAKE_CURRENT_SOURCE_DIR} -) - -add_subdirectory(fsfw_tests) diff --git a/tests/src/fsfw_tests/CMakeLists.txt b/tests/src/fsfw_tests/CMakeLists.txt deleted file mode 100644 index f6e1b8abd..000000000 --- a/tests/src/fsfw_tests/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -if(FSFW_ADD_INTERNAL_TESTS) - add_subdirectory(internal) -endif() - -if(FSFW_BUILD_UNITTESTS) - add_subdirectory(unit) -else() - add_subdirectory(integration) -endif() diff --git a/tests/src/fsfw_tests/integration/assemblies/CMakeLists.txt b/tests/src/fsfw_tests/integration/assemblies/CMakeLists.txt deleted file mode 100644 index 22c06600f..000000000 --- a/tests/src/fsfw_tests/integration/assemblies/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - TestAssembly.cpp -) \ No newline at end of file diff --git a/tests/src/fsfw_tests/integration/controller/CMakeLists.txt b/tests/src/fsfw_tests/integration/controller/CMakeLists.txt deleted file mode 100644 index f5655b712..000000000 --- a/tests/src/fsfw_tests/integration/controller/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - TestController.cpp -) \ No newline at end of file diff --git a/tests/src/fsfw_tests/integration/devices/CMakeLists.txt b/tests/src/fsfw_tests/integration/devices/CMakeLists.txt deleted file mode 100644 index cfd238d2c..000000000 --- a/tests/src/fsfw_tests/integration/devices/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - TestCookie.cpp - TestDeviceHandler.cpp - TestEchoComIF.cpp -) diff --git a/tests/src/fsfw_tests/integration/task/CMakeLists.txt b/tests/src/fsfw_tests/integration/task/CMakeLists.txt deleted file mode 100644 index 4cd481bf6..000000000 --- a/tests/src/fsfw_tests/integration/task/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - TestTask.cpp -) \ No newline at end of file diff --git a/tests/src/fsfw_tests/internal/CMakeLists.txt b/tests/src/fsfw_tests/internal/CMakeLists.txt deleted file mode 100644 index 2a144a9be..000000000 --- a/tests/src/fsfw_tests/internal/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - InternalUnitTester.cpp - UnittDefinitions.cpp -) - -add_subdirectory(osal) -add_subdirectory(serialize) -add_subdirectory(globalfunctions) \ No newline at end of file diff --git a/tests/src/fsfw_tests/internal/globalfunctions/CMakeLists.txt b/tests/src/fsfw_tests/internal/globalfunctions/CMakeLists.txt deleted file mode 100644 index cde97734f..000000000 --- a/tests/src/fsfw_tests/internal/globalfunctions/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - TestArrayPrinter.cpp -) diff --git a/tests/src/fsfw_tests/internal/osal/CMakeLists.txt b/tests/src/fsfw_tests/internal/osal/CMakeLists.txt deleted file mode 100644 index 8d79d7596..000000000 --- a/tests/src/fsfw_tests/internal/osal/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - testMq.cpp - testMutex.cpp - testSemaphore.cpp -) diff --git a/tests/src/fsfw_tests/internal/serialize/CMakeLists.txt b/tests/src/fsfw_tests/internal/serialize/CMakeLists.txt deleted file mode 100644 index 47e8b538e..000000000 --- a/tests/src/fsfw_tests/internal/serialize/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE - IntTestSerialization.cpp -) diff --git a/tests/src/fsfw_tests/unit/CMakeLists.txt b/unittests/CMakeLists.txt similarity index 59% rename from tests/src/fsfw_tests/unit/CMakeLists.txt rename to unittests/CMakeLists.txt index fc73f7b50..f32c6e08f 100644 --- a/tests/src/fsfw_tests/unit/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -2,17 +2,19 @@ target_sources(${FSFW_TEST_TGT} PRIVATE CatchDefinitions.cpp CatchFactory.cpp printChar.cpp + testVersion.cpp ) -# if(FSFW_CUSTOM_UNITTEST_RUNNER) - target_sources(${FSFW_TEST_TGT} PRIVATE - CatchRunner.cpp - CatchSetup.cpp - ) -# endif() +target_sources(${FSFW_TEST_TGT} PRIVATE + CatchRunner.cpp + CatchSetup.cpp +) add_subdirectory(testcfg) +add_subdirectory(mocks) + add_subdirectory(action) +add_subdirectory(power) add_subdirectory(container) add_subdirectory(osal) add_subdirectory(serialize) @@ -23,3 +25,7 @@ add_subdirectory(timemanager) add_subdirectory(tmtcpacket) add_subdirectory(cfdp) add_subdirectory(hal) +add_subdirectory(internalerror) +add_subdirectory(devicehandler) + +target_include_directories(${FSFW_TEST_TGT} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/tests/src/fsfw_tests/unit/CatchDefinitions.cpp b/unittests/CatchDefinitions.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/CatchDefinitions.cpp rename to unittests/CatchDefinitions.cpp diff --git a/tests/src/fsfw_tests/unit/CatchDefinitions.h b/unittests/CatchDefinitions.h similarity index 100% rename from tests/src/fsfw_tests/unit/CatchDefinitions.h rename to unittests/CatchDefinitions.h diff --git a/tests/src/fsfw_tests/unit/CatchFactory.cpp b/unittests/CatchFactory.cpp similarity index 97% rename from tests/src/fsfw_tests/unit/CatchFactory.cpp rename to unittests/CatchFactory.cpp index 01018dfac..860a9bed2 100644 --- a/tests/src/fsfw_tests/unit/CatchFactory.cpp +++ b/unittests/CatchFactory.cpp @@ -65,7 +65,7 @@ void Factory::setStaticFrameworkObjectIds() { VerificationReporter::messageReceiver = objects::PUS_SERVICE_1_VERIFICATION; DeviceHandlerBase::powerSwitcherId = objects::NO_OBJECT; - DeviceHandlerBase::rawDataReceiverId = objects::PUS_SERVICE_2_DEVICE_ACCESS; + DeviceHandlerBase::rawDataReceiverId = objects::NO_OBJECT; LocalDataPoolManager::defaultHkDestination = objects::HK_RECEIVER_MOCK; diff --git a/tests/src/fsfw_tests/unit/CatchFactory.h b/unittests/CatchFactory.h similarity index 100% rename from tests/src/fsfw_tests/unit/CatchFactory.h rename to unittests/CatchFactory.h diff --git a/tests/src/fsfw_tests/unit/CatchRunner.cpp b/unittests/CatchRunner.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/CatchRunner.cpp rename to unittests/CatchRunner.cpp diff --git a/tests/src/fsfw_tests/unit/CatchRunner.h b/unittests/CatchRunner.h similarity index 100% rename from tests/src/fsfw_tests/unit/CatchRunner.h rename to unittests/CatchRunner.h diff --git a/tests/src/fsfw_tests/unit/CatchSetup.cpp b/unittests/CatchSetup.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/CatchSetup.cpp rename to unittests/CatchSetup.cpp diff --git a/tests/src/fsfw_tests/unit/action/CMakeLists.txt b/unittests/action/CMakeLists.txt similarity index 100% rename from tests/src/fsfw_tests/unit/action/CMakeLists.txt rename to unittests/action/CMakeLists.txt diff --git a/tests/src/fsfw_tests/unit/action/TestActionHelper.cpp b/unittests/action/TestActionHelper.cpp similarity index 98% rename from tests/src/fsfw_tests/unit/action/TestActionHelper.cpp rename to unittests/action/TestActionHelper.cpp index 923b74361..6461a195c 100644 --- a/tests/src/fsfw_tests/unit/action/TestActionHelper.cpp +++ b/unittests/action/TestActionHelper.cpp @@ -6,7 +6,7 @@ #include #include -#include "fsfw_tests/unit/mocks/MessageQueueMockBase.h" +#include "mocks/MessageQueueMockBase.h" TEST_CASE("Action Helper", "[ActionHelper]") { ActionHelperOwnerMockBase testDhMock; diff --git a/tests/src/fsfw_tests/unit/action/TestActionHelper.h b/unittests/action/TestActionHelper.h similarity index 96% rename from tests/src/fsfw_tests/unit/action/TestActionHelper.h rename to unittests/action/TestActionHelper.h index 243f030a4..75a9f3fef 100644 --- a/tests/src/fsfw_tests/unit/action/TestActionHelper.h +++ b/unittests/action/TestActionHelper.h @@ -6,7 +6,7 @@ #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" class ActionHelperOwnerMockBase : public HasActionsIF { public: diff --git a/tests/src/fsfw_tests/unit/cfdp/CMakeLists.txt b/unittests/cfdp/CMakeLists.txt similarity index 100% rename from tests/src/fsfw_tests/unit/cfdp/CMakeLists.txt rename to unittests/cfdp/CMakeLists.txt diff --git a/tests/src/fsfw_tests/unit/cfdp/testAckPdu.cpp b/unittests/cfdp/testAckPdu.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/cfdp/testAckPdu.cpp rename to unittests/cfdp/testAckPdu.cpp diff --git a/tests/src/fsfw_tests/unit/cfdp/testCfdp.cpp b/unittests/cfdp/testCfdp.cpp similarity index 99% rename from tests/src/fsfw_tests/unit/cfdp/testCfdp.cpp rename to unittests/cfdp/testCfdp.cpp index 19b1ec7fe..78ef618b3 100644 --- a/tests/src/fsfw_tests/unit/cfdp/testCfdp.cpp +++ b/unittests/cfdp/testCfdp.cpp @@ -2,6 +2,7 @@ #include #include +#include "CatchDefinitions.h" #include "fsfw/cfdp/FileSize.h" #include "fsfw/cfdp/pdu/FileDirectiveDeserializer.h" #include "fsfw/cfdp/pdu/FileDirectiveSerializer.h" @@ -9,7 +10,6 @@ #include "fsfw/cfdp/pdu/HeaderSerializer.h" #include "fsfw/globalfunctions/arrayprinter.h" #include "fsfw/serialize/SerializeAdapter.h" -#include "fsfw_tests/unit/CatchDefinitions.h" TEST_CASE("CFDP Base", "[CfdpBase]") { using namespace cfdp; diff --git a/tests/src/fsfw_tests/unit/cfdp/testEofPdu.cpp b/unittests/cfdp/testEofPdu.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/cfdp/testEofPdu.cpp rename to unittests/cfdp/testEofPdu.cpp diff --git a/tests/src/fsfw_tests/unit/cfdp/testFileData.cpp b/unittests/cfdp/testFileData.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/cfdp/testFileData.cpp rename to unittests/cfdp/testFileData.cpp diff --git a/tests/src/fsfw_tests/unit/cfdp/testFinishedPdu.cpp b/unittests/cfdp/testFinishedPdu.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/cfdp/testFinishedPdu.cpp rename to unittests/cfdp/testFinishedPdu.cpp diff --git a/tests/src/fsfw_tests/unit/cfdp/testKeepAlivePdu.cpp b/unittests/cfdp/testKeepAlivePdu.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/cfdp/testKeepAlivePdu.cpp rename to unittests/cfdp/testKeepAlivePdu.cpp diff --git a/tests/src/fsfw_tests/unit/cfdp/testMetadataPdu.cpp b/unittests/cfdp/testMetadataPdu.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/cfdp/testMetadataPdu.cpp rename to unittests/cfdp/testMetadataPdu.cpp diff --git a/tests/src/fsfw_tests/unit/cfdp/testNakPdu.cpp b/unittests/cfdp/testNakPdu.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/cfdp/testNakPdu.cpp rename to unittests/cfdp/testNakPdu.cpp diff --git a/tests/src/fsfw_tests/unit/cfdp/testPromptPdu.cpp b/unittests/cfdp/testPromptPdu.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/cfdp/testPromptPdu.cpp rename to unittests/cfdp/testPromptPdu.cpp diff --git a/tests/src/fsfw_tests/unit/cfdp/testTlvsLvs.cpp b/unittests/cfdp/testTlvsLvs.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/cfdp/testTlvsLvs.cpp rename to unittests/cfdp/testTlvsLvs.cpp diff --git a/tests/src/fsfw_tests/unit/container/CMakeLists.txt b/unittests/container/CMakeLists.txt similarity index 100% rename from tests/src/fsfw_tests/unit/container/CMakeLists.txt rename to unittests/container/CMakeLists.txt diff --git a/tests/src/fsfw_tests/unit/container/RingBufferTest.cpp b/unittests/container/RingBufferTest.cpp similarity index 94% rename from tests/src/fsfw_tests/unit/container/RingBufferTest.cpp rename to unittests/container/RingBufferTest.cpp index a83fa2aca..f4f111a61 100644 --- a/tests/src/fsfw_tests/unit/container/RingBufferTest.cpp +++ b/unittests/container/RingBufferTest.cpp @@ -3,7 +3,7 @@ #include #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" TEST_CASE("Ring Buffer Test", "[RingBufferTest]") { uint8_t testData[13] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}; @@ -31,6 +31,8 @@ TEST_CASE("Ring Buffer Test", "[RingBufferTest]") { for (uint8_t i = 0; i < 9; i++) { CHECK(readBuffer[i] == i); } + REQUIRE(ringBuffer.writeData(testData, 1024) == retval::CATCH_FAILED); + REQUIRE(ringBuffer.writeData(nullptr, 5) == retval::CATCH_FAILED); } SECTION("Get Free Element Test") { @@ -144,12 +146,13 @@ TEST_CASE("Ring Buffer Test2", "[RingBufferTest2]") { SECTION("Overflow") { REQUIRE(ringBuffer.availableWriteSpace() == 9); - // Writing more than the buffer is large, technically thats allowed - // But it is senseless and has undesired impact on read call - REQUIRE(ringBuffer.writeData(testData, 13) == retval::CATCH_OK); - REQUIRE(ringBuffer.getAvailableReadData() == 3); + // We don't allow writing of Data that is larger than the ring buffer in total + REQUIRE(ringBuffer.getMaxSize() == 9); + REQUIRE(ringBuffer.writeData(testData, 13) == retval::CATCH_FAILED); + REQUIRE(ringBuffer.getAvailableReadData() == 0); ringBuffer.clear(); uint8_t *ptr = nullptr; + // With excess Bytes 13 Bytes can be written to this Buffer REQUIRE(ringBuffer.getFreeElement(&ptr, 13) == retval::CATCH_OK); REQUIRE(ptr != nullptr); memcpy(ptr, testData, 13); @@ -234,11 +237,13 @@ TEST_CASE("Ring Buffer Test3", "[RingBufferTest3]") { SECTION("Overflow") { REQUIRE(ringBuffer.availableWriteSpace() == 9); - // Writing more than the buffer is large, technically thats allowed - // But it is senseless and has undesired impact on read call - REQUIRE(ringBuffer.writeData(testData, 13) == retval::CATCH_OK); - REQUIRE(ringBuffer.getAvailableReadData() == 3); + // Writing more than the buffer is large. + // This write will be rejected and is seen as a configuration mistake + REQUIRE(ringBuffer.writeData(testData, 13) == retval::CATCH_FAILED); + REQUIRE(ringBuffer.getAvailableReadData() == 0); ringBuffer.clear(); + // Using FreeElement allows the usage of excessBytes but + // should be used with caution uint8_t *ptr = nullptr; REQUIRE(ringBuffer.getFreeElement(&ptr, 13) == retval::CATCH_OK); REQUIRE(ptr != nullptr); diff --git a/tests/src/fsfw_tests/unit/container/TestArrayList.cpp b/unittests/container/TestArrayList.cpp similarity index 98% rename from tests/src/fsfw_tests/unit/container/TestArrayList.cpp rename to unittests/container/TestArrayList.cpp index 2c5a37d97..4daf59d4f 100644 --- a/tests/src/fsfw_tests/unit/container/TestArrayList.cpp +++ b/unittests/container/TestArrayList.cpp @@ -3,7 +3,7 @@ #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" /** * @brief Array List test diff --git a/tests/src/fsfw_tests/unit/container/TestDynamicFifo.cpp b/unittests/container/TestDynamicFifo.cpp similarity index 99% rename from tests/src/fsfw_tests/unit/container/TestDynamicFifo.cpp rename to unittests/container/TestDynamicFifo.cpp index 540ea31e7..6769c2471 100644 --- a/tests/src/fsfw_tests/unit/container/TestDynamicFifo.cpp +++ b/unittests/container/TestDynamicFifo.cpp @@ -4,7 +4,7 @@ #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" TEST_CASE("Dynamic Fifo Tests", "[TestDynamicFifo]") { INFO("Dynamic Fifo Tests"); diff --git a/tests/src/fsfw_tests/unit/container/TestFifo.cpp b/unittests/container/TestFifo.cpp similarity index 99% rename from tests/src/fsfw_tests/unit/container/TestFifo.cpp rename to unittests/container/TestFifo.cpp index a9eb7956b..0b4b41af5 100644 --- a/tests/src/fsfw_tests/unit/container/TestFifo.cpp +++ b/unittests/container/TestFifo.cpp @@ -4,7 +4,7 @@ #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" TEST_CASE("Static Fifo Tests", "[TestFifo]") { INFO("Fifo Tests"); diff --git a/tests/src/fsfw_tests/unit/container/TestFixedArrayList.cpp b/unittests/container/TestFixedArrayList.cpp similarity index 96% rename from tests/src/fsfw_tests/unit/container/TestFixedArrayList.cpp rename to unittests/container/TestFixedArrayList.cpp index 42ae01d5c..6beb8d5d0 100644 --- a/tests/src/fsfw_tests/unit/container/TestFixedArrayList.cpp +++ b/unittests/container/TestFixedArrayList.cpp @@ -3,7 +3,7 @@ #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" TEST_CASE("FixedArrayList Tests", "[TestFixedArrayList]") { INFO("FixedArrayList Tests"); diff --git a/tests/src/fsfw_tests/unit/container/TestFixedMap.cpp b/unittests/container/TestFixedMap.cpp similarity index 99% rename from tests/src/fsfw_tests/unit/container/TestFixedMap.cpp rename to unittests/container/TestFixedMap.cpp index 4c3cad1e4..d3c657608 100644 --- a/tests/src/fsfw_tests/unit/container/TestFixedMap.cpp +++ b/unittests/container/TestFixedMap.cpp @@ -3,7 +3,7 @@ #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" template class FixedMap; diff --git a/tests/src/fsfw_tests/unit/container/TestFixedOrderedMultimap.cpp b/unittests/container/TestFixedOrderedMultimap.cpp similarity index 99% rename from tests/src/fsfw_tests/unit/container/TestFixedOrderedMultimap.cpp rename to unittests/container/TestFixedOrderedMultimap.cpp index 7dd63b34a..88b326940 100644 --- a/tests/src/fsfw_tests/unit/container/TestFixedOrderedMultimap.cpp +++ b/unittests/container/TestFixedOrderedMultimap.cpp @@ -3,7 +3,7 @@ #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" TEST_CASE("FixedOrderedMultimap Tests", "[TestFixedOrderedMultimap]") { INFO("FixedOrderedMultimap Tests"); diff --git a/tests/src/fsfw_tests/unit/container/TestPlacementFactory.cpp b/unittests/container/TestPlacementFactory.cpp similarity index 97% rename from tests/src/fsfw_tests/unit/container/TestPlacementFactory.cpp rename to unittests/container/TestPlacementFactory.cpp index 0140ce05f..1333567ec 100644 --- a/tests/src/fsfw_tests/unit/container/TestPlacementFactory.cpp +++ b/unittests/container/TestPlacementFactory.cpp @@ -5,7 +5,7 @@ #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" TEST_CASE("PlacementFactory Tests", "[TestPlacementFactory]") { INFO("PlacementFactory Tests"); diff --git a/tests/src/fsfw_tests/unit/datapoollocal/CMakeLists.txt b/unittests/datapoollocal/CMakeLists.txt similarity index 100% rename from tests/src/fsfw_tests/unit/datapoollocal/CMakeLists.txt rename to unittests/datapoollocal/CMakeLists.txt diff --git a/tests/src/fsfw_tests/unit/datapoollocal/DataSetTest.cpp b/unittests/datapoollocal/DataSetTest.cpp similarity index 99% rename from tests/src/fsfw_tests/unit/datapoollocal/DataSetTest.cpp rename to unittests/datapoollocal/DataSetTest.cpp index 902d59ef1..c9f023ef4 100644 --- a/tests/src/fsfw_tests/unit/datapoollocal/DataSetTest.cpp +++ b/unittests/datapoollocal/DataSetTest.cpp @@ -8,8 +8,8 @@ #include #include +#include "CatchDefinitions.h" #include "LocalPoolOwnerBase.h" -#include "fsfw_tests/unit/CatchDefinitions.h" #include "tests/TestsConfig.h" TEST_CASE("DataSetTest", "[DataSetTest]") { diff --git a/tests/src/fsfw_tests/unit/datapoollocal/LocalPoolManagerTest.cpp b/unittests/datapoollocal/LocalPoolManagerTest.cpp similarity index 78% rename from tests/src/fsfw_tests/unit/datapoollocal/LocalPoolManagerTest.cpp rename to unittests/datapoollocal/LocalPoolManagerTest.cpp index fb22972e3..58a6065ee 100644 --- a/tests/src/fsfw_tests/unit/datapoollocal/LocalPoolManagerTest.cpp +++ b/unittests/datapoollocal/LocalPoolManagerTest.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include #include @@ -10,8 +11,8 @@ #include #include +#include "CatchDefinitions.h" #include "LocalPoolOwnerBase.h" -#include "fsfw_tests/unit/CatchDefinitions.h" TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { LocalPoolOwnerBase* poolOwner = @@ -20,8 +21,10 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { REQUIRE(poolOwner->initializeHkManager() == retval::CATCH_OK); REQUIRE(poolOwner->initializeHkManagerAfterTaskCreation() == retval::CATCH_OK); - MessageQueueMockBase* mqMock = poolOwner->getMockQueueHandle(); - REQUIRE(mqMock != nullptr); + MessageQueueMockBase* poolOwnerMock = poolOwner->getMockQueueHandle(); + REQUIRE(poolOwnerMock != nullptr); + + // MessageQueueIF* hkCommander = QueueFactory::instance()->createMessageQueue(); CommandMessage messageSent; uint8_t messagesSent = 0; @@ -40,9 +43,9 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { poolOwner->dataset.setChanged(true); /* Now the update message should be generated. */ REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK); - REQUIRE(mqMock->wasMessageSent() == true); + REQUIRE(poolOwnerMock->wasMessageSent() == true); - REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK); + REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK); CHECK(messageSent.getCommand() == static_cast(HousekeepingMessage::UPDATE_NOTIFICATION_SET)); @@ -52,9 +55,9 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { poolOwner->dataset.setChanged(true); REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK); + REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK); CHECK(messageSent.getCommand() == static_cast(HousekeepingMessage::UPDATE_NOTIFICATION_SET)); @@ -62,15 +65,15 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { REQUIRE(poolOwner->subscribeWrapperSetUpdateHk() == retval::CATCH_OK); poolOwner->dataset.setChanged(true); REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 2); /* first message sent should be the update notification, considering the internal list is a vector checked in insertion order. */ - REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK); + REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK); CHECK(messageSent.getCommand() == static_cast(HousekeepingMessage::UPDATE_NOTIFICATION_SET)); - REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK); + REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK); CHECK(messageSent.getCommand() == static_cast(HousekeepingMessage::HK_REPORT)); /* Clear message to avoid memory leak, our mock won't do it for us (yet) */ CommandMessageCleaner::clearCommandMessage(&messageSent); @@ -93,16 +96,14 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { poolOwner->dataset.setChanged(true); /* Store current time, we are going to check the (approximate) time equality later */ - CCSDSTime::CDS_short timeCdsNow; timeval now; Clock::getClock_timeval(&now); - CCSDSTime::convertToCcsds(&timeCdsNow, &now); /* Trigger generation of snapshot */ REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK); + REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK); /* Check that snapshot was generated */ CHECK(messageSent.getCommand() == static_cast(HousekeepingMessage::UPDATE_SNAPSHOT_SET)); /* Now we deserialize the snapshot into a new dataset instance */ @@ -131,13 +132,11 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { CHECK(newSet.localPoolUint16Vec.value[2] == 42932); /* Now we check that both times are equal */ - CHECK(cdsShort.pField == timeCdsNow.pField); - CHECK(cdsShort.dayLSB == Catch::Approx(timeCdsNow.dayLSB).margin(1)); - CHECK(cdsShort.dayMSB == Catch::Approx(timeCdsNow.dayMSB).margin(1)); - CHECK(cdsShort.msDay_h == Catch::Approx(timeCdsNow.msDay_h).margin(1)); - CHECK(cdsShort.msDay_hh == Catch::Approx(timeCdsNow.msDay_hh).margin(1)); - CHECK(cdsShort.msDay_l == Catch::Approx(timeCdsNow.msDay_l).margin(1)); - CHECK(cdsShort.msDay_ll == Catch::Approx(timeCdsNow.msDay_ll).margin(5)); + timeval timeFromHK; + auto result = CCSDSTime::convertFromCDS(&timeFromHK, &cdsShort); + CHECK(result == HasReturnvaluesIF::RETURN_OK); + timeval difference = timeFromHK - now; + CHECK(timevalOperations::toDouble(difference) < 1.0); } SECTION("VariableSnapshotTest") { @@ -158,22 +157,19 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { } poolVar->setChanged(true); - /* Store current time, we are going to check the (approximate) time equality later */ CCSDSTime::CDS_short timeCdsNow; timeval now; Clock::getClock_timeval(&now); - CCSDSTime::convertToCcsds(&timeCdsNow, &now); - REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK); /* Check update snapshot was sent. */ - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); /* Should have been reset. */ CHECK(poolVar->hasChanged() == false); - REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK); + REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK); CHECK(messageSent.getCommand() == static_cast(HousekeepingMessage::UPDATE_SNAPSHOT_VARIABLE)); /* Now we deserialize the snapshot into a new dataset instance */ @@ -193,13 +189,11 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { CHECK(varCopy.value == 25); /* Now we check that both times are equal */ - CHECK(cdsShort.pField == timeCdsNow.pField); - CHECK(cdsShort.dayLSB == Catch::Approx(timeCdsNow.dayLSB).margin(1)); - CHECK(cdsShort.dayMSB == Catch::Approx(timeCdsNow.dayMSB).margin(1)); - CHECK(cdsShort.msDay_h == Catch::Approx(timeCdsNow.msDay_h).margin(1)); - CHECK(cdsShort.msDay_hh == Catch::Approx(timeCdsNow.msDay_hh).margin(1)); - CHECK(cdsShort.msDay_l == Catch::Approx(timeCdsNow.msDay_l).margin(1)); - CHECK(cdsShort.msDay_ll == Catch::Approx(timeCdsNow.msDay_ll).margin(5)); + timeval timeFromHK; + auto result = CCSDSTime::convertFromCDS(&timeFromHK, &cdsShort); + CHECK(result == HasReturnvaluesIF::RETURN_OK); + timeval difference = timeFromHK - now; + CHECK(timevalOperations::toDouble(difference) < 1.0); } SECTION("VariableNotificationTest") { @@ -217,11 +211,11 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK); /* Check update notification was sent. */ - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); /* Should have been reset. */ CHECK(poolVar->hasChanged() == false); - REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK); + REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK); CHECK(messageSent.getCommand() == static_cast(HousekeepingMessage::UPDATE_NOTIFICATION_VARIABLE)); /* Now subscribe for the dataset update (HK and update) again with subscription interface */ @@ -233,26 +227,26 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { poolOwner->dataset.setChanged(true); REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK); /* Now two messages should be sent. */ - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 2); - mqMock->clearMessages(true); + poolOwnerMock->clearMessages(true); poolOwner->dataset.setChanged(true); poolVar->setChanged(true); REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK); /* Now three messages should be sent. */ - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 3); - REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK); + REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK); CHECK(messageSent.getCommand() == static_cast(HousekeepingMessage::UPDATE_NOTIFICATION_VARIABLE)); - REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK); + REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK); CHECK(messageSent.getCommand() == static_cast(HousekeepingMessage::UPDATE_NOTIFICATION_SET)); - REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK); + REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK); CHECK(messageSent.getCommand() == static_cast(HousekeepingMessage::HK_REPORT)); CommandMessageCleaner::clearCommandMessage(&messageSent); - REQUIRE(mqMock->receiveMessage(&messageSent) == static_cast(MessageQueueIF::EMPTY)); + REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == static_cast(MessageQueueIF::EMPTY)); } SECTION("PeriodicHKAndMessaging") { @@ -263,38 +257,38 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { REQUIRE(poolOwner->subscribePeriodicHk(true) == retval::CATCH_OK); REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK); /* Now HK packet should be sent as message immediately. */ - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); LocalPoolDataSetBase* setHandle = poolOwner->getDataSetHandle(lpool::testSid); REQUIRE(setHandle != nullptr); CHECK(poolOwner->poolManager.generateHousekeepingPacket(lpool::testSid, setHandle, false) == retval::CATCH_OK); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); CHECK(setHandle->getReportingEnabled() == true); CommandMessage hkCmd; HousekeepingMessage::setToggleReportingCommand(&hkCmd, lpool::testSid, false, false); CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK); CHECK(setHandle->getReportingEnabled() == false); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setToggleReportingCommand(&hkCmd, lpool::testSid, true, false); CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK); CHECK(setHandle->getReportingEnabled() == true); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setToggleReportingCommand(&hkCmd, lpool::testSid, false, false); CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK); CHECK(setHandle->getReportingEnabled() == false); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setCollectionIntervalModificationCommand(&hkCmd, lpool::testSid, 0.4, false); @@ -302,23 +296,23 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { /* For non-diagnostics and a specified minimum frequency of 0.2 seconds, the resulting collection interval should be 1.0 second */ CHECK(poolOwner->dataset.getCollectionInterval() == 1.0); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setStructureReportingCommand(&hkCmd, lpool::testSid, false); REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK); CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK); /* Now HK packet should be sent as message. */ - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setOneShotReportCommand(&hkCmd, lpool::testSid, false); CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setUpdateNotificationSetCommand(&hkCmd, lpool::testSid); sid_t sidToCheck; @@ -334,62 +328,62 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == static_cast(LocalDataPoolManager::WRONG_HK_PACKET_TYPE)); /* We still expect a failure message being sent */ - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setCollectionIntervalModificationCommand(&hkCmd, lpool::testSid, 0.4, false); CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == static_cast(LocalDataPoolManager::WRONG_HK_PACKET_TYPE)); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setStructureReportingCommand(&hkCmd, lpool::testSid, false); CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == static_cast(LocalDataPoolManager::WRONG_HK_PACKET_TYPE)); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setStructureReportingCommand(&hkCmd, lpool::testSid, true); CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setCollectionIntervalModificationCommand(&hkCmd, lpool::testSid, 0.4, true); CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setToggleReportingCommand(&hkCmd, lpool::testSid, true, true); CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setToggleReportingCommand(&hkCmd, lpool::testSid, false, true); CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setOneShotReportCommand(&hkCmd, lpool::testSid, false); CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == static_cast(LocalDataPoolManager::WRONG_HK_PACKET_TYPE)); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setOneShotReportCommand(&hkCmd, lpool::testSid, true); CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK); - REQUIRE(mqMock->wasMessageSent(&messagesSent) == true); + REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true); CHECK(messagesSent == 1); - CHECK(mqMock->popMessage() == retval::CATCH_OK); + CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK); HousekeepingMessage::setUpdateNotificationVariableCommand(&hkCmd, lpool::uint8VarGpid); gp_id_t gpidToCheck; @@ -415,5 +409,5 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") { /* we need to reset the subscription list because the pool owner is a global object. */ CHECK(poolOwner->reset() == retval::CATCH_OK); - mqMock->clearMessages(true); + poolOwnerMock->clearMessages(true); } diff --git a/tests/src/fsfw_tests/unit/datapoollocal/LocalPoolOwnerBase.cpp b/unittests/datapoollocal/LocalPoolOwnerBase.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/datapoollocal/LocalPoolOwnerBase.cpp rename to unittests/datapoollocal/LocalPoolOwnerBase.cpp diff --git a/tests/src/fsfw_tests/unit/datapoollocal/LocalPoolOwnerBase.h b/unittests/datapoollocal/LocalPoolOwnerBase.h similarity index 100% rename from tests/src/fsfw_tests/unit/datapoollocal/LocalPoolOwnerBase.h rename to unittests/datapoollocal/LocalPoolOwnerBase.h diff --git a/tests/src/fsfw_tests/unit/datapoollocal/LocalPoolVariableTest.cpp b/unittests/datapoollocal/LocalPoolVariableTest.cpp similarity index 98% rename from tests/src/fsfw_tests/unit/datapoollocal/LocalPoolVariableTest.cpp rename to unittests/datapoollocal/LocalPoolVariableTest.cpp index 73d51d92a..3479cdbeb 100644 --- a/tests/src/fsfw_tests/unit/datapoollocal/LocalPoolVariableTest.cpp +++ b/unittests/datapoollocal/LocalPoolVariableTest.cpp @@ -3,12 +3,12 @@ #include +#include "CatchDefinitions.h" #include "LocalPoolOwnerBase.h" -#include "fsfw_tests/unit/CatchDefinitions.h" #include "tests/TestsConfig.h" TEST_CASE("LocalPoolVariable", "[LocPoolVarTest]") { - LocalPoolOwnerBase* poolOwner = + auto* poolOwner = ObjectManager::instance()->get(objects::TEST_LOCAL_POOL_OWNER_BASE); REQUIRE(poolOwner != nullptr); REQUIRE(poolOwner->initializeHkManager() == retval::CATCH_OK); diff --git a/tests/src/fsfw_tests/unit/datapoollocal/LocalPoolVectorTest.cpp b/unittests/datapoollocal/LocalPoolVectorTest.cpp similarity index 98% rename from tests/src/fsfw_tests/unit/datapoollocal/LocalPoolVectorTest.cpp rename to unittests/datapoollocal/LocalPoolVectorTest.cpp index 5932db44a..2235ad39b 100644 --- a/tests/src/fsfw_tests/unit/datapoollocal/LocalPoolVectorTest.cpp +++ b/unittests/datapoollocal/LocalPoolVectorTest.cpp @@ -3,8 +3,8 @@ #include +#include "CatchDefinitions.h" #include "LocalPoolOwnerBase.h" -#include "fsfw_tests/unit/CatchDefinitions.h" #include "tests/TestsConfig.h" TEST_CASE("LocalPoolVector", "[LocPoolVecTest]") { diff --git a/unittests/devicehandler/CMakeLists.txt b/unittests/devicehandler/CMakeLists.txt new file mode 100644 index 000000000..7ad5d3169 --- /dev/null +++ b/unittests/devicehandler/CMakeLists.txt @@ -0,0 +1,8 @@ +target_sources(${FSFW_TEST_TGT} PRIVATE + CookieIFMock.cpp + ComIFMock.cpp + DeviceHandlerCommander.cpp + DeviceHandlerMock.cpp + DeviceFdirMock.cpp + TestDeviceHandlerBase.cpp +) diff --git a/unittests/devicehandler/ComIFMock.cpp b/unittests/devicehandler/ComIFMock.cpp new file mode 100644 index 000000000..650b74bd8 --- /dev/null +++ b/unittests/devicehandler/ComIFMock.cpp @@ -0,0 +1,46 @@ +#include "ComIFMock.h" + +#include "DeviceHandlerMock.h" + +ComIFMock::ComIFMock(object_id_t objectId) : SystemObject(objectId) {} + +ComIFMock::~ComIFMock() {} + +ReturnValue_t ComIFMock::initializeInterface(CookieIF *cookie) { return RETURN_OK; } + +ReturnValue_t ComIFMock::sendMessage(CookieIF *cookie, const uint8_t *sendData, size_t sendLen) { + data = *sendData; + return RETURN_OK; +} + +ReturnValue_t ComIFMock::getSendSuccess(CookieIF *cookie) { return RETURN_OK; } + +ReturnValue_t ComIFMock::requestReceiveMessage(CookieIF *cookie, size_t requestLen) { + return RETURN_OK; +} + +ReturnValue_t ComIFMock::readReceivedMessage(CookieIF *cookie, uint8_t **buffer, size_t *size) { + switch (testCase) { + case TestCase::MISSED_REPLY: { + *size = 0; + return RETURN_OK; + } + case TestCase::SIMPLE_COMMAND_NOMINAL: { + *size = 1; + data = DeviceHandlerMock::SIMPLE_COMMAND_DATA; + *buffer = &data; + break; + } + case TestCase::PERIODIC_REPLY_NOMINAL: { + *size = 1; + data = DeviceHandlerMock::PERIODIC_REPLY_DATA; + *buffer = &data; + break; + } + default: + break; + } + return RETURN_OK; +} + +void ComIFMock::setTestCase(TestCase testCase_) { testCase = testCase_; } diff --git a/unittests/devicehandler/ComIFMock.h b/unittests/devicehandler/ComIFMock.h new file mode 100644 index 000000000..d16cc0a6b --- /dev/null +++ b/unittests/devicehandler/ComIFMock.h @@ -0,0 +1,37 @@ +#ifndef TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_COMIFMOCK_H_ +#define TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_COMIFMOCK_H_ + +#include +#include + +/** + * @brief The ComIFMock supports the simulation of various device communication error cases + * like incomplete or wrong replies and can be used to test the + * DeviceHandlerBase. + */ +class ComIFMock : public DeviceCommunicationIF, public SystemObject { + public: + enum class TestCase { SIMPLE_COMMAND_NOMINAL, PERIODIC_REPLY_NOMINAL, MISSED_REPLY }; + + ComIFMock(object_id_t objectId); + virtual ~ComIFMock(); + + virtual ReturnValue_t initializeInterface(CookieIF *cookie) override; + virtual ReturnValue_t sendMessage(CookieIF *cookie, const uint8_t *sendData, + size_t sendLen) override; + virtual ReturnValue_t getSendSuccess(CookieIF *cookie) override; + virtual ReturnValue_t requestReceiveMessage(CookieIF *cookie, size_t requestLen) override; + virtual ReturnValue_t readReceivedMessage(CookieIF *cookie, uint8_t **buffer, + size_t *size) override; + void setTestCase(TestCase testCase_); + + private: + TestCase testCase = TestCase::SIMPLE_COMMAND_NOMINAL; + + static const uint8_t SIMPLE_COMMAND_DATA = 1; + static const uint8_t PERIODIC_REPLY_DATA = 2; + + uint8_t data = 0; +}; + +#endif /* TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_COMIFMOCK_H_ */ diff --git a/unittests/devicehandler/CookieIFMock.cpp b/unittests/devicehandler/CookieIFMock.cpp new file mode 100644 index 000000000..1ae2eb6e9 --- /dev/null +++ b/unittests/devicehandler/CookieIFMock.cpp @@ -0,0 +1,5 @@ +#include "CookieIFMock.h" + +CookieIFMock::CookieIFMock() {} + +CookieIFMock::~CookieIFMock() {} diff --git a/unittests/devicehandler/CookieIFMock.h b/unittests/devicehandler/CookieIFMock.h new file mode 100644 index 000000000..1243b0a71 --- /dev/null +++ b/unittests/devicehandler/CookieIFMock.h @@ -0,0 +1,12 @@ +#ifndef TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_COOKIEIFMOCK_H_ +#define TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_COOKIEIFMOCK_H_ + +#include "fsfw/devicehandlers/CookieIF.h" + +class CookieIFMock : public CookieIF { + public: + CookieIFMock(); + virtual ~CookieIFMock(); +}; + +#endif /* TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_COOKIEIFMOCK_H_ */ diff --git a/unittests/devicehandler/DeviceFdirMock.cpp b/unittests/devicehandler/DeviceFdirMock.cpp new file mode 100644 index 000000000..e3ac39ac6 --- /dev/null +++ b/unittests/devicehandler/DeviceFdirMock.cpp @@ -0,0 +1,18 @@ +#include "DeviceFdirMock.h" + +#include "devicehandler/DeviceFdirMock.h" + +DeviceFdirMock::DeviceFdirMock(object_id_t owner, object_id_t parent) + : DeviceHandlerFailureIsolation(owner, parent) {} + +DeviceFdirMock::~DeviceFdirMock() {} + +uint32_t DeviceFdirMock::getMissedReplyCount() { + ParameterWrapper parameterWrapper; + this->getParameter(MISSED_REPLY_DOMAIN_ID, + static_cast(FaultCounter::ParameterIds::FAULT_COUNT), + ¶meterWrapper, nullptr, 0); + uint32_t missedReplyCount = 0; + parameterWrapper.getElement(&missedReplyCount); + return missedReplyCount; +} diff --git a/unittests/devicehandler/DeviceFdirMock.h b/unittests/devicehandler/DeviceFdirMock.h new file mode 100644 index 000000000..b314fc989 --- /dev/null +++ b/unittests/devicehandler/DeviceFdirMock.h @@ -0,0 +1,18 @@ +#ifndef TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_DEVICEFDIRMOCK_H_ +#define TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_DEVICEFDIRMOCK_H_ + +#include "fsfw/devicehandlers/DeviceHandlerFailureIsolation.h" + +class DeviceFdirMock : public DeviceHandlerFailureIsolation { + public: + DeviceFdirMock(object_id_t owner, object_id_t parent); + virtual ~DeviceFdirMock(); + + uint32_t getMissedReplyCount(); + + private: + static const uint8_t STRANGE_REPLY_DOMAIN_ID = 0xF0; + static const uint8_t MISSED_REPLY_DOMAIN_ID = 0xF1; +}; + +#endif /* TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_DEVICEFDIRMOCK_H_ */ diff --git a/unittests/devicehandler/DeviceHandlerCommander.cpp b/unittests/devicehandler/DeviceHandlerCommander.cpp new file mode 100644 index 000000000..d38166adb --- /dev/null +++ b/unittests/devicehandler/DeviceHandlerCommander.cpp @@ -0,0 +1,64 @@ +#include "DeviceHandlerCommander.h" + +#include + +DeviceHandlerCommander::DeviceHandlerCommander(object_id_t objectId) + : SystemObject(objectId), commandActionHelper(this) { + auto mqArgs = MqArgs(this->getObjectId()); + commandQueue = QueueFactory::instance()->createMessageQueue( + QUEUE_SIZE, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); +} + +DeviceHandlerCommander::~DeviceHandlerCommander() {} + +ReturnValue_t DeviceHandlerCommander::performOperation(uint8_t operationCode) { + readCommandQueue(); + return RETURN_OK; +} + +ReturnValue_t DeviceHandlerCommander::initialize() { + ReturnValue_t result = commandActionHelper.initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return HasReturnvaluesIF::RETURN_OK; +} + +MessageQueueIF* DeviceHandlerCommander::getCommandQueuePtr() { return commandQueue; } + +void DeviceHandlerCommander::stepSuccessfulReceived(ActionId_t actionId, uint8_t step) {} + +void DeviceHandlerCommander::stepFailedReceived(ActionId_t actionId, uint8_t step, + ReturnValue_t returnCode) {} + +void DeviceHandlerCommander::dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) { +} + +void DeviceHandlerCommander::completionSuccessfulReceived(ActionId_t actionId) { + lastReplyReturnCode = RETURN_OK; +} + +void DeviceHandlerCommander::completionFailedReceived(ActionId_t actionId, + ReturnValue_t returnCode) { + lastReplyReturnCode = returnCode; +} + +void DeviceHandlerCommander::readCommandQueue() { + CommandMessage message; + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + for (result = commandQueue->receiveMessage(&message); result == HasReturnvaluesIF::RETURN_OK; + result = commandQueue->receiveMessage(&message)) { + result = commandActionHelper.handleReply(&message); + if (result == HasReturnvaluesIF::RETURN_OK) { + continue; + } + } +} + +ReturnValue_t DeviceHandlerCommander::sendCommand(object_id_t target, ActionId_t actionId) { + return commandActionHelper.commandAction(target, actionId, nullptr, 0); +} + +ReturnValue_t DeviceHandlerCommander::getReplyReturnCode() { return lastReplyReturnCode; } + +void DeviceHandlerCommander::resetReplyReturnCode() { lastReplyReturnCode = RETURN_FAILED; } diff --git a/unittests/devicehandler/DeviceHandlerCommander.h b/unittests/devicehandler/DeviceHandlerCommander.h new file mode 100644 index 000000000..435d00177 --- /dev/null +++ b/unittests/devicehandler/DeviceHandlerCommander.h @@ -0,0 +1,50 @@ +#ifndef TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_DEVICEHANDLERCOMMANDER_H_ +#define TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_DEVICEHANDLERCOMMANDER_H_ + +#include "fsfw/action/CommandActionHelper.h" +#include "fsfw/action/CommandsActionsIF.h" +#include "fsfw/objectmanager/SystemObject.h" +#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/tasks/ExecutableObjectIF.h" + +class DeviceHandlerCommander : public ExecutableObjectIF, + public SystemObject, + public CommandsActionsIF, + public HasReturnvaluesIF { + public: + DeviceHandlerCommander(object_id_t objectId); + virtual ~DeviceHandlerCommander(); + + ReturnValue_t performOperation(uint8_t operationCode = 0); + ReturnValue_t initialize() override; + MessageQueueIF* getCommandQueuePtr() override; + void stepSuccessfulReceived(ActionId_t actionId, uint8_t step) override; + void stepFailedReceived(ActionId_t actionId, uint8_t step, ReturnValue_t returnCode) override; + void dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) override; + void completionSuccessfulReceived(ActionId_t actionId) override; + void completionFailedReceived(ActionId_t actionId, ReturnValue_t returnCode) override; + + /** + * @brief Calling this function will send the command to the device handler object. + * + * @param target Object ID of the device handler + * @param actionId Action ID of the command to send + */ + ReturnValue_t sendCommand(object_id_t target, ActionId_t actionId); + + ReturnValue_t getReplyReturnCode(); + void resetReplyReturnCode(); + + private: + static const uint32_t QUEUE_SIZE = 20; + + MessageQueueIF* commandQueue = nullptr; + + CommandActionHelper commandActionHelper; + + ReturnValue_t lastReplyReturnCode = RETURN_FAILED; + + void readCommandQueue(); +}; + +#endif /* TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_DEVICEHANDLERCOMMANDER_H_ */ diff --git a/unittests/devicehandler/DeviceHandlerMock.cpp b/unittests/devicehandler/DeviceHandlerMock.cpp new file mode 100644 index 000000000..ea30ff6a1 --- /dev/null +++ b/unittests/devicehandler/DeviceHandlerMock.cpp @@ -0,0 +1,103 @@ +#include "DeviceHandlerMock.h" + +#include + +DeviceHandlerMock::DeviceHandlerMock(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie, FailureIsolationBase *fdirInstance) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie, fdirInstance) { + mode = MODE_ON; +} + +DeviceHandlerMock::~DeviceHandlerMock() {} + +void DeviceHandlerMock::doStartUp() { setMode(_MODE_TO_ON); } + +void DeviceHandlerMock::doShutDown() { setMode(_MODE_POWER_DOWN); } + +ReturnValue_t DeviceHandlerMock::buildNormalDeviceCommand(DeviceCommandId_t *id) { + return NOTHING_TO_SEND; +} + +ReturnValue_t DeviceHandlerMock::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + return NOTHING_TO_SEND; +} + +ReturnValue_t DeviceHandlerMock::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, + size_t commandDataLen) { + switch (deviceCommand) { + case SIMPLE_COMMAND: { + commandBuffer[0] = SIMPLE_COMMAND_DATA; + rawPacket = commandBuffer; + rawPacketLen = sizeof(SIMPLE_COMMAND_DATA); + break; + } + default: + WARN("DeviceHandlerMock::buildCommandFromCommand: Invalid device command"); + break; + } + return RETURN_OK; +} + +ReturnValue_t DeviceHandlerMock::scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) { + switch (*start) { + case SIMPLE_COMMAND_DATA: { + *foundId = SIMPLE_COMMAND; + *foundLen = sizeof(SIMPLE_COMMAND_DATA); + return RETURN_OK; + break; + } + case PERIODIC_REPLY_DATA: { + *foundId = PERIODIC_REPLY; + *foundLen = sizeof(PERIODIC_REPLY_DATA); + return RETURN_OK; + break; + } + default: + break; + } + return RETURN_FAILED; +} + +ReturnValue_t DeviceHandlerMock::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { + switch (id) { + case SIMPLE_COMMAND: + case PERIODIC_REPLY: { + periodicReplyReceived = true; + break; + } + default: + break; + } + return RETURN_OK; +} + +void DeviceHandlerMock::fillCommandAndReplyMap() { + insertInCommandAndReplyMap(SIMPLE_COMMAND, 0, nullptr, 0, false, false, 0, + &simpleCommandReplyTimeout); + insertInCommandAndReplyMap(PERIODIC_REPLY, 0, nullptr, 0, true, false, 0, + &periodicReplyCountdown); +} + +uint32_t DeviceHandlerMock::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; } + +void DeviceHandlerMock::changePeriodicReplyCountdown(uint32_t timeout) { + periodicReplyCountdown.setTimeout(timeout); +} + +void DeviceHandlerMock::changeSimpleCommandReplyCountdown(uint32_t timeout) { + simpleCommandReplyTimeout.setTimeout(timeout); +} + +void DeviceHandlerMock::resetPeriodicReplyState() { periodicReplyReceived = false; } + +bool DeviceHandlerMock::getPeriodicReplyReceived() { return periodicReplyReceived; } + +ReturnValue_t DeviceHandlerMock::enablePeriodicReply(DeviceCommandId_t replyId) { + return updatePeriodicReply(true, replyId); +} + +ReturnValue_t DeviceHandlerMock::disablePeriodicReply(DeviceCommandId_t replyId) { + return updatePeriodicReply(false, replyId); +} diff --git a/unittests/devicehandler/DeviceHandlerMock.h b/unittests/devicehandler/DeviceHandlerMock.h new file mode 100644 index 000000000..ef1649c34 --- /dev/null +++ b/unittests/devicehandler/DeviceHandlerMock.h @@ -0,0 +1,46 @@ +#ifndef TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_DEVICEHANDLERMOCK_H_ +#define TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_DEVICEHANDLERMOCK_H_ + +#include + +class DeviceHandlerMock : public DeviceHandlerBase { + public: + static const DeviceCommandId_t SIMPLE_COMMAND = 1; + static const DeviceCommandId_t PERIODIC_REPLY = 2; + + static const uint8_t SIMPLE_COMMAND_DATA = 1; + static const uint8_t PERIODIC_REPLY_DATA = 2; + + DeviceHandlerMock(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, + FailureIsolationBase *fdirInstance); + virtual ~DeviceHandlerMock(); + void changePeriodicReplyCountdown(uint32_t timeout); + void changeSimpleCommandReplyCountdown(uint32_t timeout); + void resetPeriodicReplyState(); + bool getPeriodicReplyReceived(); + ReturnValue_t enablePeriodicReply(DeviceCommandId_t replyId); + ReturnValue_t disablePeriodicReply(DeviceCommandId_t replyId); + + protected: + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + void fillCommandAndReplyMap() override; + uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; + + private: + Countdown simpleCommandReplyTimeout = Countdown(1000); + Countdown periodicReplyCountdown = Countdown(1000); + + uint8_t commandBuffer[1]; + + bool periodicReplyReceived = false; +}; + +#endif /* TESTS_SRC_FSFW_TESTS_UNIT_DEVICEHANDLER_DEVICEHANDLERMOCK_H_ */ diff --git a/unittests/devicehandler/TestDeviceHandlerBase.cpp b/unittests/devicehandler/TestDeviceHandlerBase.cpp new file mode 100644 index 000000000..e8fdd17b9 --- /dev/null +++ b/unittests/devicehandler/TestDeviceHandlerBase.cpp @@ -0,0 +1,95 @@ +#include + +#include "ComIFMock.h" +#include "DeviceFdirMock.h" +#include "DeviceHandlerCommander.h" +#include "DeviceHandlerMock.h" +#include "devicehandler/CookieIFMock.h" +#include "objects/systemObjectList.h" + +TEST_CASE("Device Handler Base", "[DeviceHandlerBase]") { + // Will be deleted with DHB destructor + auto* cookieIFMock = new CookieIFMock; + ComIFMock comIF(objects::COM_IF_MOCK); + DeviceFdirMock deviceFdirMock(objects::DEVICE_HANDLER_MOCK, objects::NO_OBJECT); + DeviceHandlerMock deviceHandlerMock(objects::DEVICE_HANDLER_MOCK, objects::COM_IF_MOCK, + cookieIFMock, &deviceFdirMock); + ReturnValue_t result = deviceHandlerMock.initialize(); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + DeviceHandlerCommander deviceHandlerCommander(objects::DEVICE_HANDLER_COMMANDER); + result = deviceHandlerCommander.initialize(); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + + SECTION("Commanding nominal") { + comIF.setTestCase(ComIFMock::TestCase::SIMPLE_COMMAND_NOMINAL); + result = deviceHandlerCommander.sendCommand(objects::DEVICE_HANDLER_MOCK, + DeviceHandlerMock::SIMPLE_COMMAND); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + deviceHandlerMock.performOperation(DeviceHandlerIF::PERFORM_OPERATION); + deviceHandlerMock.performOperation(DeviceHandlerIF::SEND_WRITE); + deviceHandlerMock.performOperation(DeviceHandlerIF::GET_WRITE); + deviceHandlerMock.performOperation(DeviceHandlerIF::SEND_READ); + deviceHandlerMock.performOperation(DeviceHandlerIF::GET_READ); + deviceHandlerCommander.performOperation(); + result = deviceHandlerCommander.getReplyReturnCode(); + uint32_t missedReplies = deviceFdirMock.getMissedReplyCount(); + REQUIRE(missedReplies == 0); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + } + + SECTION("Commanding missed reply") { + comIF.setTestCase(ComIFMock::TestCase::MISSED_REPLY); + deviceHandlerCommander.resetReplyReturnCode(); + // Set the timeout to 0 to immediately timeout the reply + deviceHandlerMock.changeSimpleCommandReplyCountdown(0); + result = deviceHandlerCommander.sendCommand(objects::DEVICE_HANDLER_MOCK, + DeviceHandlerMock::SIMPLE_COMMAND); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + deviceHandlerMock.performOperation(DeviceHandlerIF::PERFORM_OPERATION); + deviceHandlerMock.performOperation(DeviceHandlerIF::SEND_WRITE); + deviceHandlerMock.performOperation(DeviceHandlerIF::GET_WRITE); + deviceHandlerMock.performOperation(DeviceHandlerIF::SEND_READ); + deviceHandlerMock.performOperation(DeviceHandlerIF::GET_READ); + deviceHandlerMock.performOperation(DeviceHandlerIF::PERFORM_OPERATION); + deviceHandlerCommander.performOperation(); + result = deviceHandlerCommander.getReplyReturnCode(); + REQUIRE(result == DeviceHandlerIF::TIMEOUT); + uint32_t missedReplies = deviceFdirMock.getMissedReplyCount(); + REQUIRE(missedReplies == 1); + } + + SECTION("Periodic reply nominal") { + comIF.setTestCase(ComIFMock::TestCase::PERIODIC_REPLY_NOMINAL); + deviceHandlerMock.enablePeriodicReply(DeviceHandlerMock::PERIODIC_REPLY); + deviceHandlerMock.performOperation(DeviceHandlerIF::PERFORM_OPERATION); + deviceHandlerMock.performOperation(DeviceHandlerIF::SEND_WRITE); + deviceHandlerMock.performOperation(DeviceHandlerIF::GET_WRITE); + deviceHandlerMock.performOperation(DeviceHandlerIF::SEND_READ); + deviceHandlerMock.performOperation(DeviceHandlerIF::GET_READ); + REQUIRE(deviceHandlerMock.getPeriodicReplyReceived() == true); + } + + SECTION("Missed periodic reply") { + comIF.setTestCase(ComIFMock::TestCase::MISSED_REPLY); + // Set the timeout to 0 to immediately timeout the reply + deviceHandlerMock.changePeriodicReplyCountdown(0); + deviceHandlerMock.enablePeriodicReply(DeviceHandlerMock::PERIODIC_REPLY); + deviceHandlerMock.performOperation(DeviceHandlerIF::PERFORM_OPERATION); + deviceHandlerMock.performOperation(DeviceHandlerIF::SEND_WRITE); + deviceHandlerMock.performOperation(DeviceHandlerIF::GET_WRITE); + deviceHandlerMock.performOperation(DeviceHandlerIF::SEND_READ); + deviceHandlerMock.performOperation(DeviceHandlerIF::GET_READ); + uint32_t missedReplies = deviceFdirMock.getMissedReplyCount(); + REQUIRE(missedReplies == 1); + // Test if disabling of periodic reply + deviceHandlerMock.disablePeriodicReply(DeviceHandlerMock::PERIODIC_REPLY); + deviceHandlerMock.performOperation(DeviceHandlerIF::PERFORM_OPERATION); + deviceHandlerMock.performOperation(DeviceHandlerIF::SEND_WRITE); + deviceHandlerMock.performOperation(DeviceHandlerIF::GET_WRITE); + deviceHandlerMock.performOperation(DeviceHandlerIF::SEND_READ); + deviceHandlerMock.performOperation(DeviceHandlerIF::GET_READ); + missedReplies = deviceFdirMock.getMissedReplyCount(); + // Should still be 1 because periodic reply is now disabled + REQUIRE(missedReplies == 1); + } +} diff --git a/tests/src/fsfw_tests/unit/globalfunctions/CMakeLists.txt b/unittests/globalfunctions/CMakeLists.txt similarity index 69% rename from tests/src/fsfw_tests/unit/globalfunctions/CMakeLists.txt rename to unittests/globalfunctions/CMakeLists.txt index 3b29f23fc..348b99fc7 100644 --- a/tests/src/fsfw_tests/unit/globalfunctions/CMakeLists.txt +++ b/unittests/globalfunctions/CMakeLists.txt @@ -2,4 +2,6 @@ target_sources(${FSFW_TEST_TGT} PRIVATE testDleEncoder.cpp testOpDivider.cpp testBitutil.cpp + testCRC.cpp + testTimevalOperations.cpp ) diff --git a/tests/src/fsfw_tests/unit/globalfunctions/testBitutil.cpp b/unittests/globalfunctions/testBitutil.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/globalfunctions/testBitutil.cpp rename to unittests/globalfunctions/testBitutil.cpp diff --git a/unittests/globalfunctions/testCRC.cpp b/unittests/globalfunctions/testCRC.cpp new file mode 100644 index 000000000..2b6de4db2 --- /dev/null +++ b/unittests/globalfunctions/testCRC.cpp @@ -0,0 +1,14 @@ +#include + +#include "CatchDefinitions.h" +#include "catch2/catch_test_macros.hpp" +#include "fsfw/globalfunctions/CRC.h" + +TEST_CASE("CRC", "[CRC]") { + std::array testData = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + uint16_t crc = CRC::crc16ccitt(testData.data(), 10); + REQUIRE(crc == 49729); + for (uint8_t index = 0; index < testData.size(); index++) { + REQUIRE(testData[index] == index); + } +} \ No newline at end of file diff --git a/tests/src/fsfw_tests/unit/globalfunctions/testDleEncoder.cpp b/unittests/globalfunctions/testDleEncoder.cpp similarity index 99% rename from tests/src/fsfw_tests/unit/globalfunctions/testDleEncoder.cpp rename to unittests/globalfunctions/testDleEncoder.cpp index 034cb3a05..0c707f4c7 100644 --- a/tests/src/fsfw_tests/unit/globalfunctions/testDleEncoder.cpp +++ b/unittests/globalfunctions/testDleEncoder.cpp @@ -1,8 +1,8 @@ #include +#include "CatchDefinitions.h" #include "catch2/catch_test_macros.hpp" #include "fsfw/globalfunctions/DleEncoder.h" -#include "fsfw_tests/unit/CatchDefinitions.h" const std::vector TEST_ARRAY_0 = {0, 0, 0, 0, 0}; const std::vector TEST_ARRAY_1 = {0, DleEncoder::DLE_CHAR, 5}; diff --git a/tests/src/fsfw_tests/unit/globalfunctions/testOpDivider.cpp b/unittests/globalfunctions/testOpDivider.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/globalfunctions/testOpDivider.cpp rename to unittests/globalfunctions/testOpDivider.cpp diff --git a/unittests/globalfunctions/testTimevalOperations.cpp b/unittests/globalfunctions/testTimevalOperations.cpp new file mode 100644 index 000000000..155e6b153 --- /dev/null +++ b/unittests/globalfunctions/testTimevalOperations.cpp @@ -0,0 +1,124 @@ +#include + +#include +#include + +#include "CatchDefinitions.h" + +TEST_CASE("TimevalTest", "[timevalOperations]") { + SECTION("Comparison") { + timeval t1; + t1.tv_sec = 1648227422; + t1.tv_usec = 123456; + timeval t2; + t2.tv_sec = 1648227422; + t2.tv_usec = 123456; + REQUIRE(t1 == t2); + REQUIRE(t2 == t1); + REQUIRE_FALSE(t1 != t2); + REQUIRE_FALSE(t2 != t1); + REQUIRE(t1 <= t2); + REQUIRE(t2 <= t1); + REQUIRE(t1 >= t2); + REQUIRE(t2 >= t1); + REQUIRE_FALSE(t1 < t2); + REQUIRE_FALSE(t2 < t1); + REQUIRE_FALSE(t1 > t2); + REQUIRE_FALSE(t2 > t1); + + timeval t3; + t3.tv_sec = 1648227422; + t3.tv_usec = 123457; + REQUIRE_FALSE(t1 == t3); + REQUIRE(t1 != t3); + REQUIRE(t1 <= t3); + REQUIRE_FALSE(t3 <= t1); + REQUIRE_FALSE(t1 >= t3); + REQUIRE(t3 >= t1); + REQUIRE(t1 < t3); + REQUIRE_FALSE(t3 < t1); + REQUIRE_FALSE(t1 > t3); + REQUIRE(t3 > t1); + + timeval t4; + t4.tv_sec = 1648227423; + t4.tv_usec = 123456; + REQUIRE_FALSE(t1 == t4); + REQUIRE(t1 != t4); + REQUIRE(t1 <= t4); + REQUIRE_FALSE(t4 <= t1); + REQUIRE_FALSE(t1 >= t4); + REQUIRE(t4 >= t1); + REQUIRE(t1 < t4); + REQUIRE_FALSE(t4 < t1); + REQUIRE_FALSE(t1 > t4); + REQUIRE(t4 > t1); + } + SECTION("Operators") { + timeval t1; + t1.tv_sec = 1648227422; + t1.tv_usec = 123456; + timeval t2; + t2.tv_sec = 1648227422; + t2.tv_usec = 123456; + timeval t3 = t1 - t2; + REQUIRE(t3.tv_sec == 0); + REQUIRE(t3.tv_usec == 0); + timeval t4 = t1 - t3; + REQUIRE(t4.tv_sec == 1648227422); + REQUIRE(t4.tv_usec == 123456); + timeval t5 = t3 - t1; + REQUIRE(t5.tv_sec == -1648227422); + REQUIRE(t5.tv_usec == -123456); + + timeval t6; + t6.tv_sec = 1648227400; + t6.tv_usec = 999999; + + timeval t7 = t6 + t1; + REQUIRE(t7.tv_sec == (1648227422ull + 1648227400ull + 1ull)); + REQUIRE(t7.tv_usec == 123455); + + timeval t8 = t1 - t6; + REQUIRE(t8.tv_sec == 1648227422 - 1648227400 - 1); + REQUIRE(t8.tv_usec == 123457); + + double scalar = 2; + timeval t9 = t1 * scalar; + REQUIRE(t9.tv_sec == 3296454844); + REQUIRE(t9.tv_usec == 246912); + timeval t10 = scalar * t1; + REQUIRE(t10.tv_sec == 3296454844); + REQUIRE(t10.tv_usec == 246912); + timeval t11 = t6 * scalar; + REQUIRE(t11.tv_sec == (3296454800 + 1)); + REQUIRE(t11.tv_usec == 999998); + + timeval t12 = t1 / scalar; + REQUIRE(t12.tv_sec == 824113711); + REQUIRE(t12.tv_usec == 61728); + + timeval t13 = t6 / scalar; + REQUIRE(t13.tv_sec == 824113700); + // Rounding issue + REQUIRE(t13.tv_usec == 499999); + + double scalar2 = t9 / t1; + REQUIRE(scalar2 == Catch::Approx(2.0)); + double scalar3 = t1 / t6; + REQUIRE(scalar3 == Catch::Approx(1.000000013)); + double scalar4 = t3 / t1; + REQUIRE(scalar4 == Catch::Approx(0)); + double scalar5 = t12 / t1; + REQUIRE(scalar5 == Catch::Approx(0.5)); + } + + SECTION("timevalOperations::toTimeval") { + double seconds = 1648227422.123456; + timeval t1 = timevalOperations::toTimeval(seconds); + REQUIRE(t1.tv_sec == 1648227422); + // Allow 1 usec rounding tolerance + REQUIRE(t1.tv_usec >= 123455); + REQUIRE(t1.tv_usec <= 123457); + } +} \ No newline at end of file diff --git a/tests/src/fsfw_tests/unit/hal/CMakeLists.txt b/unittests/hal/CMakeLists.txt similarity index 100% rename from tests/src/fsfw_tests/unit/hal/CMakeLists.txt rename to unittests/hal/CMakeLists.txt diff --git a/tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp b/unittests/hal/testCommandExecutor.cpp similarity index 87% rename from tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp rename to unittests/hal/testCommandExecutor.cpp index 3ad26876c..d34f67aae 100644 --- a/tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp +++ b/unittests/hal/testCommandExecutor.cpp @@ -2,12 +2,14 @@ #include #include +#include #include "fsfw/container/DynamicFIFO.h" #include "fsfw/container/SimpleRingBuffer.h" #include "fsfw/platform.h" #include "fsfw/serviceinterface.h" #include "fsfw_hal/linux/CommandExecutor.h" +#include "tests/TestsConfig.h" #ifdef PLATFORM_UNIX @@ -61,6 +63,9 @@ TEST_CASE("Command Executor", "[cmd-exec]") { std::string cmpString = "Hello World\n"; CHECK(readString == cmpString); outputBuffer.deleteData(12, true); + + // Issues with CI/CD +#if FSFW_CICD_BUILD == 0 // Test more complex command result = cmdExecutor.load("ping -c 1 localhost", false, false); REQUIRE(cmdExecutor.getCurrentState() == CommandExecutor::States::COMMAND_LOADED); @@ -81,16 +86,27 @@ TEST_CASE("Command Executor", "[cmd-exec]") { REQUIRE(cmdExecutor.getCurrentState() == CommandExecutor::States::IDLE); readBytes = 0; sizesFifo.retrieve(&readBytes); - // That's about the size of the reply - bool beTrue = (readBytes > 200) and (readBytes < 300); - REQUIRE(beTrue); uint8_t largerReadBuffer[1024] = {}; + // That's about the size of the reply + bool beTrue = (readBytes > 100) and (readBytes < 400); + if (not beTrue) { + size_t readLen = outputBuffer.getAvailableReadData(); + if (readLen > sizeof(largerReadBuffer) - 1) { + readLen = sizeof(largerReadBuffer) - 1; + } + outputBuffer.readData(largerReadBuffer, readLen); + std::string readString(reinterpret_cast(largerReadBuffer)); + std::cerr << "Catch2 tag cmd-exec: Read " << readBytes << ": " << std::endl; + std::cerr << readString << std::endl; + } + REQUIRE(beTrue); outputBuffer.readData(largerReadBuffer, readBytes); // You can also check this output in the debugger std::string allTheReply(reinterpret_cast(largerReadBuffer)); // I am just going to assume that this string is the same across ping implementations // of different Linux systems REQUIRE(allTheReply.find("PING localhost") != std::string::npos); +#endif // Now check failing command result = cmdExecutor.load("false", false, false); diff --git a/unittests/internalerror/CMakeLists.txt b/unittests/internalerror/CMakeLists.txt new file mode 100644 index 000000000..d49ce0067 --- /dev/null +++ b/unittests/internalerror/CMakeLists.txt @@ -0,0 +1,3 @@ +target_sources(${FSFW_TEST_TGT} PRIVATE + TestInternalErrorReporter.cpp +) diff --git a/unittests/internalerror/TestInternalErrorReporter.cpp b/unittests/internalerror/TestInternalErrorReporter.cpp new file mode 100644 index 000000000..2b999faea --- /dev/null +++ b/unittests/internalerror/TestInternalErrorReporter.cpp @@ -0,0 +1,120 @@ +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "CatchDefinitions.h" +#include "fsfw/action/ActionMessage.h" +#include "fsfw/ipc/CommandMessage.h" +#include "fsfw/ipc/MessageQueueMessage.h" +#include "fsfw/objectmanager/frameworkObjects.h" +#include "mocks/PeriodicTaskIFMock.h" + +TEST_CASE("Internal Error Reporter", "[TestInternalError]") { + PeriodicTaskMock task(10, nullptr); + ObjectManagerIF* manager = ObjectManager::instance(); + if (manager == nullptr) { + FAIL(); + } + auto* internalErrorReporter = dynamic_cast( + ObjectManager::instance()->get(objects::INTERNAL_ERROR_REPORTER)); + if (internalErrorReporter == nullptr) { + FAIL(); + } + task.addComponent(objects::INTERNAL_ERROR_REPORTER); + // This calls the initializeAfterTaskCreation function + task.startTask(); + MessageQueueIF* testQueue = QueueFactory::instance()->createMessageQueue(1); + MessageQueueIF* hkQueue = QueueFactory::instance()->createMessageQueue(1); + internalErrorReporter->getSubscriptionInterface()->subscribeForSetUpdateMessage( + InternalErrorDataset::ERROR_SET_ID, objects::NO_OBJECT, hkQueue->getId(), true); + StorageManagerIF* ipcStore = ObjectManager::instance()->get(objects::IPC_STORE); + SECTION("MessageQueueFull") { + CommandMessage message; + ActionMessage::setCompletionReply(&message, 10, true); + auto result = hkQueue->sendMessage(testQueue->getId(), &message); + REQUIRE(result == retval::CATCH_OK); + uint32_t queueHits = 0; + uint32_t lostTm = 0; + uint32_t storeHits = 0; + /* We don't know if another test caused a queue Hit so we will enforce one, + then remeber the queueHit count and force another hit */ + internalErrorReporter->queueMessageNotSent(); + internalErrorReporter->performOperation(0); + { + CommandMessage hkMessage; + result = hkQueue->receiveMessage(&hkMessage); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + REQUIRE(hkMessage.getCommand() == HousekeepingMessage::UPDATE_SNAPSHOT_SET); + store_address_t storeAddress; + gp_id_t gpid = + HousekeepingMessage::getUpdateSnapshotVariableCommand(&hkMessage, &storeAddress); + REQUIRE(gpid.objectId == objects::INTERNAL_ERROR_REPORTER); + // We need the object ID of the reporter here (NO_OBJECT) + InternalErrorDataset dataset(objects::INTERNAL_ERROR_REPORTER); + CCSDSTime::CDS_short time; + ConstAccessorPair data = ipcStore->getData(storeAddress); + REQUIRE(data.first == HasReturnvaluesIF::RETURN_OK); + HousekeepingSnapshot hkSnapshot(&time, &dataset); + const uint8_t* buffer = data.second.data(); + size_t size = data.second.size(); + result = hkSnapshot.deSerialize(&buffer, &size, SerializeIF::Endianness::MACHINE); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + // Remember the amount of queueHits before to see the increase + queueHits = dataset.queueHits.value; + lostTm = dataset.tmHits.value; + storeHits = dataset.storeHits.value; + } + result = hkQueue->sendMessage(testQueue->getId(), &message); + REQUIRE(result == MessageQueueIF::FULL); + internalErrorReporter->lostTm(); + internalErrorReporter->storeFull(); + { + internalErrorReporter->performOperation(0); + CommandMessage hkMessage; + result = hkQueue->receiveMessage(&hkMessage); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + REQUIRE(hkMessage.getCommand() == HousekeepingMessage::UPDATE_SNAPSHOT_SET); + store_address_t storeAddress; + gp_id_t gpid = + HousekeepingMessage::getUpdateSnapshotVariableCommand(&hkMessage, &storeAddress); + REQUIRE(gpid.objectId == objects::INTERNAL_ERROR_REPORTER); + + ConstAccessorPair data = ipcStore->getData(storeAddress); + REQUIRE(data.first == HasReturnvaluesIF::RETURN_OK); + CCSDSTime::CDS_short time; + // We need the object ID of the reporter here (NO_OBJECT) + InternalErrorDataset dataset(objects::INTERNAL_ERROR_REPORTER); + HousekeepingSnapshot hkSnapshot(&time, &dataset); + const uint8_t* buffer = data.second.data(); + size_t size = data.second.size(); + result = hkSnapshot.deSerialize(&buffer, &size, SerializeIF::Endianness::MACHINE); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + // Test that we had one more queueHit + REQUIRE(dataset.queueHits.value == (queueHits + 1)); + REQUIRE(dataset.tmHits.value == (lostTm + 1)); + REQUIRE(dataset.storeHits.value == (storeHits + 1)); + } + // Complete Coverage + internalErrorReporter->setDiagnosticPrintout(true); + internalErrorReporter->setMutexTimeout(MutexIF::TimeoutType::BLOCKING, 0); + { + // Message Queue Id + MessageQueueId_t id = internalErrorReporter->getCommandQueue(); + REQUIRE(id != MessageQueueIF::NO_QUEUE); + CommandMessage message; + sid_t sid(objects::INTERNAL_ERROR_REPORTER, InternalErrorDataset::ERROR_SET_ID); + HousekeepingMessage::setToggleReportingCommand(&message, sid, true, false); + result = hkQueue->sendMessage(id, &message); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + internalErrorReporter->performOperation(0); + } + } + QueueFactory::instance()->deleteMessageQueue(testQueue); + QueueFactory::instance()->deleteMessageQueue(hkQueue); +} diff --git a/unittests/mocks/CMakeLists.txt b/unittests/mocks/CMakeLists.txt new file mode 100644 index 000000000..1b86547ce --- /dev/null +++ b/unittests/mocks/CMakeLists.txt @@ -0,0 +1,3 @@ +target_sources(${FSFW_TEST_TGT} PRIVATE + PowerSwitcherMock.cpp +) diff --git a/tests/src/fsfw_tests/unit/mocks/HkReceiverMock.h b/unittests/mocks/HkReceiverMock.h similarity index 100% rename from tests/src/fsfw_tests/unit/mocks/HkReceiverMock.h rename to unittests/mocks/HkReceiverMock.h diff --git a/tests/src/fsfw_tests/unit/mocks/MessageQueueMockBase.h b/unittests/mocks/MessageQueueMockBase.h similarity index 59% rename from tests/src/fsfw_tests/unit/mocks/MessageQueueMockBase.h rename to unittests/mocks/MessageQueueMockBase.h index f60c74028..d33231864 100644 --- a/tests/src/fsfw_tests/unit/mocks/MessageQueueMockBase.h +++ b/unittests/mocks/MessageQueueMockBase.h @@ -4,16 +4,18 @@ #include #include +#include "CatchDefinitions.h" #include "fsfw/ipc/CommandMessage.h" +#include "fsfw/ipc/MessageQueueBase.h" #include "fsfw/ipc/MessageQueueIF.h" #include "fsfw/ipc/MessageQueueMessage.h" -#include "fsfw_tests/unit/CatchDefinitions.h" -class MessageQueueMockBase : public MessageQueueIF { +class MessageQueueMockBase : public MessageQueueBase { public: - MessageQueueId_t myQueueId = tconst::testQueueId; + MessageQueueMockBase() + : MessageQueueBase(MessageQueueIF::NO_QUEUE, MessageQueueIF::NO_QUEUE, nullptr) {} + uint8_t messageSentCounter = 0; - bool defaultDestSet = false; bool messageSent = false; bool wasMessageSent(uint8_t* messageSentCounter = nullptr, bool resetCounter = true) { @@ -38,53 +40,30 @@ class MessageQueueMockBase : public MessageQueueIF { return receiveMessage(&message); } - virtual ReturnValue_t reply(MessageQueueMessageIF* message) { - return sendMessage(myQueueId, message); - }; - virtual ReturnValue_t receiveMessage(MessageQueueMessageIF* message, - MessageQueueId_t* receivedFrom) { - return receiveMessage(message); - } - - virtual ReturnValue_t receiveMessage(MessageQueueMessageIF* message) { + virtual ReturnValue_t receiveMessage(MessageQueueMessageIF* message) override { if (messagesSentQueue.empty()) { return MessageQueueIF::EMPTY; } - + this->last = message->getSender(); std::memcpy(message->getBuffer(), messagesSentQueue.front().getBuffer(), message->getMessageSize()); messagesSentQueue.pop(); return HasReturnvaluesIF::RETURN_OK; } virtual ReturnValue_t flush(uint32_t* count) { return HasReturnvaluesIF::RETURN_OK; } - virtual MessageQueueId_t getLastPartner() const { return myQueueId; } - virtual MessageQueueId_t getId() const { return myQueueId; } virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message, - MessageQueueId_t sentFrom, bool ignoreFault = false) { - return sendMessage(sendTo, message); - } - virtual ReturnValue_t sendToDefaultFrom(MessageQueueMessageIF* message, MessageQueueId_t sentFrom, - bool ignoreFault = false) { - return sendMessage(myQueueId, message); - } - virtual ReturnValue_t sendToDefault(MessageQueueMessageIF* message) { - return sendMessage(myQueueId, message); - } - virtual ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message, - bool ignoreFault = false) override { + MessageQueueId_t sentFrom, + bool ignoreFault = false) override { messageSent = true; messageSentCounter++; MessageQueueMessage& messageRef = *(dynamic_cast(message)); messagesSentQueue.push(messageRef); return HasReturnvaluesIF::RETURN_OK; } - virtual void setDefaultDestination(MessageQueueId_t defaultDestination) { - myQueueId = defaultDestination; - defaultDestSet = true; - } - virtual MessageQueueId_t getDefaultDestination() const { return myQueueId; } - virtual bool isDefaultDestinationSet() const { return defaultDestSet; } + virtual ReturnValue_t reply(MessageQueueMessageIF* message) override { + return sendMessageFrom(MessageQueueIF::NO_QUEUE, message, this->getId(), false); + } void clearMessages(bool clearCommandMessages = true) { while (not messagesSentQueue.empty()) { diff --git a/unittests/mocks/PeriodicTaskIFMock.h b/unittests/mocks/PeriodicTaskIFMock.h new file mode 100644 index 000000000..dc0ccefc4 --- /dev/null +++ b/unittests/mocks/PeriodicTaskIFMock.h @@ -0,0 +1,25 @@ +#ifndef FSFW_UNITTEST_TESTS_MOCKS_PERIODICTASKMOCK_H_ +#define FSFW_UNITTEST_TESTS_MOCKS_PERIODICTASKMOCK_H_ + +#include +#include + +class PeriodicTaskMock : public PeriodicTaskBase { + public: + PeriodicTaskMock(TaskPeriod period, TaskDeadlineMissedFunction dlmFunc) + : PeriodicTaskBase(period, dlmFunc) {} + + virtual ~PeriodicTaskMock() {} + /** + * @brief With the startTask method, a created task can be started + * for the first time. + */ + virtual ReturnValue_t startTask() override { + initObjsAfterTaskCreation(); + return HasReturnvaluesIF::RETURN_OK; + }; + + virtual ReturnValue_t sleepFor(uint32_t ms) override { return HasReturnvaluesIF::RETURN_OK; }; +}; + +#endif // FSFW_UNITTEST_TESTS_MOCKS_PERIODICTASKMOCK_H_ diff --git a/unittests/mocks/PowerSwitcherMock.cpp b/unittests/mocks/PowerSwitcherMock.cpp new file mode 100644 index 000000000..5c6935e44 --- /dev/null +++ b/unittests/mocks/PowerSwitcherMock.cpp @@ -0,0 +1,77 @@ +#include "PowerSwitcherMock.h" + +static uint32_t SWITCH_REQUEST_UPDATE_VALUE = 0; + +PowerSwitcherMock::PowerSwitcherMock() {} + +ReturnValue_t PowerSwitcherMock::sendSwitchCommand(power::Switch_t switchNr, ReturnValue_t onOff) { + if (switchMap.count(switchNr) == 0) { + switchMap.emplace(switchNr, SwitchInfo(switchNr, onOff)); + } else { + SwitchInfo& info = switchMap.at(switchNr); + info.currentState = onOff; + if (onOff == PowerSwitchIF::SWITCH_ON) { + info.timesCalledOn++; + } else { + info.timesCalledOff++; + } + } + return RETURN_OK; +} + +ReturnValue_t PowerSwitcherMock::sendFuseOnCommand(uint8_t fuseNr) { + if (fuseMap.count(fuseNr) == 0) { + fuseMap.emplace(fuseNr, FuseInfo(fuseNr)); + } else { + FuseInfo& info = fuseMap.at(fuseNr); + info.timesCalled++; + } + return RETURN_OK; +} + +ReturnValue_t PowerSwitcherMock::getSwitchState(power::Switch_t switchNr) const { + if (switchMap.count(switchNr) == 1) { + auto& info = switchMap.at(switchNr); + SWITCH_REQUEST_UPDATE_VALUE++; + return info.currentState; + } + return RETURN_FAILED; +} + +ReturnValue_t PowerSwitcherMock::getFuseState(uint8_t fuseNr) const { + if (fuseMap.count(fuseNr) == 1) { + return FUSE_ON; + } else { + return FUSE_OFF; + } + return RETURN_FAILED; +} + +uint32_t PowerSwitcherMock::getSwitchDelayMs(void) const { return 5000; } + +SwitchInfo::SwitchInfo() : switcher(0) {} + +SwitchInfo::SwitchInfo(power::Switch_t switcher, ReturnValue_t initState) + : switcher(switcher), currentState(initState) {} + +FuseInfo::FuseInfo(uint8_t fuse) : fuse(fuse) {} + +void PowerSwitcherMock::getSwitchInfo(power::Switch_t switcher, SwitchInfo& info) { + if (switchMap.count(switcher) == 1) { + info = switchMap.at(switcher); + } +} + +void PowerSwitcherMock::getFuseInfo(uint8_t fuse, FuseInfo& info) { + if (fuseMap.count(fuse) == 1) { + info = fuseMap.at(fuse); + } +} + +uint32_t PowerSwitcherMock::getAmountSwitchStatWasRequested() { + return SWITCH_REQUEST_UPDATE_VALUE; +} + +void PowerSwitcherMock::initSwitch(power::Switch_t switchNr) { + switchMap.emplace(switchNr, SwitchInfo(switchNr, PowerSwitchIF::SWITCH_OFF)); +} diff --git a/unittests/mocks/PowerSwitcherMock.h b/unittests/mocks/PowerSwitcherMock.h new file mode 100644 index 000000000..3a740e66a --- /dev/null +++ b/unittests/mocks/PowerSwitcherMock.h @@ -0,0 +1,52 @@ +#ifndef FSFW_TESTS_SRC_FSFW_TESTS_UNIT_MOCKS_POWERSWITCHERMOCK_H_ +#define FSFW_TESTS_SRC_FSFW_TESTS_UNIT_MOCKS_POWERSWITCHERMOCK_H_ + +#include + +#include +#include + +struct SwitchInfo { + public: + SwitchInfo(); + SwitchInfo(power::Switch_t switcher, ReturnValue_t initState); + + power::Switch_t switcher; + ReturnValue_t currentState = PowerSwitchIF::SWITCH_OFF; + uint32_t timesCalledOn = 0; + uint32_t timesCalledOff = 0; + uint32_t timesStatusRequested = 0; +}; + +struct FuseInfo { + public: + FuseInfo(uint8_t fuse); + uint8_t fuse; + uint32_t timesCalled = 0; +}; + +class PowerSwitcherMock : public PowerSwitchIF { + public: + PowerSwitcherMock(); + + ReturnValue_t sendSwitchCommand(power::Switch_t switchNr, ReturnValue_t onOff) override; + ReturnValue_t sendFuseOnCommand(uint8_t fuseNr) override; + ReturnValue_t getSwitchState(power::Switch_t switchNr) const override; + ReturnValue_t getFuseState(uint8_t fuseNr) const override; + uint32_t getSwitchDelayMs(void) const override; + + void getSwitchInfo(power::Switch_t switcher, SwitchInfo& info); + void getFuseInfo(uint8_t fuse, FuseInfo& info); + + uint32_t getAmountSwitchStatWasRequested(); + + void initSwitch(power::Switch_t switchNr); + + private: + using SwitchOnOffPair = std::pair; + using FuseOnOffPair = std::pair; + std::map switchMap; + std::map fuseMap; +}; + +#endif /* FSFW_TESTS_SRC_FSFW_TESTS_UNIT_MOCKS_POWERSWITCHERMOCK_H_ */ diff --git a/tests/src/fsfw_tests/unit/osal/CMakeLists.txt b/unittests/osal/CMakeLists.txt similarity index 84% rename from tests/src/fsfw_tests/unit/osal/CMakeLists.txt rename to unittests/osal/CMakeLists.txt index 293be2e85..030d363b8 100644 --- a/tests/src/fsfw_tests/unit/osal/CMakeLists.txt +++ b/unittests/osal/CMakeLists.txt @@ -1,4 +1,5 @@ target_sources(${FSFW_TEST_TGT} PRIVATE TestMessageQueue.cpp TestSemaphore.cpp + TestClock.cpp ) diff --git a/unittests/osal/TestClock.cpp b/unittests/osal/TestClock.cpp new file mode 100644 index 000000000..9979b28c3 --- /dev/null +++ b/unittests/osal/TestClock.cpp @@ -0,0 +1,86 @@ +#include +#include + +#include +#include +#include + +#include "CatchDefinitions.h" + +TEST_CASE("OSAL::Clock Test", "[OSAL::Clock Test]") { + SECTION("Test getClock") { + timeval time; + ReturnValue_t result = Clock::getClock_timeval(&time); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + Clock::TimeOfDay_t timeOfDay; + result = Clock::getDateAndTime(&timeOfDay); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + timeval timeOfDayAsTimeval; + result = Clock::convertTimeOfDayToTimeval(&timeOfDay, &timeOfDayAsTimeval); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + // We require timeOfDayAsTimeval to be larger than time as it + // was request a few ns later + double difference = timevalOperations::toDouble(timeOfDayAsTimeval - time); + CHECK(difference >= 0.0); + CHECK(difference <= 0.005); + + // Conversion in the other direction + Clock::TimeOfDay_t timevalAsTimeOfDay; + result = Clock::convertTimevalToTimeOfDay(&time, &timevalAsTimeOfDay); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + CHECK(timevalAsTimeOfDay.year <= timeOfDay.year); + // TODO We should write TimeOfDay operators! + } + SECTION("Leap seconds") { + uint16_t leapSeconds = 0; + ReturnValue_t result = Clock::getLeapSeconds(&leapSeconds); + REQUIRE(result == HasReturnvaluesIF::RETURN_FAILED); + REQUIRE(leapSeconds == 0); + result = Clock::setLeapSeconds(18); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + result = Clock::getLeapSeconds(&leapSeconds); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + REQUIRE(leapSeconds == 18); + } + SECTION("usec Test") { + timeval timeAsTimeval; + ReturnValue_t result = Clock::getClock_timeval(&timeAsTimeval); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + uint64_t timeAsUsec = 0; + result = Clock::getClock_usecs(&timeAsUsec); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + double timeAsUsecDouble = static_cast(timeAsUsec) / 1000000.0; + timeval timeAsUsecTimeval = timevalOperations::toTimeval(timeAsUsecDouble); + double difference = timevalOperations::toDouble(timeAsUsecTimeval - timeAsTimeval); + // We accept 5 ms difference + CHECK(difference >= 0.0); + CHECK(difference <= 0.005); + uint64_t timevalAsUint64 = static_cast(timeAsTimeval.tv_sec) * 1000000ull + + static_cast(timeAsTimeval.tv_usec); + CHECK((timeAsUsec - timevalAsUint64) >= 0); + CHECK((timeAsUsec - timevalAsUint64) <= (5 * 1000)); + } + SECTION("Test j2000") { + double j2000; + timeval time; + time.tv_sec = 1648208539; + time.tv_usec = 0; + ReturnValue_t result = Clock::convertTimevalToJD2000(time, &j2000); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + double correctJ2000 = 2459663.98772 - 2451545.0; + CHECK(j2000 == Catch::Approx(correctJ2000).margin(1.2 * 1e-8)); + } + SECTION("Convert to TT") { + timeval utcTime; + utcTime.tv_sec = 1648208539; + utcTime.tv_usec = 999000; + timeval tt; + ReturnValue_t result = Clock::setLeapSeconds(27); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + result = Clock::convertUTCToTT(utcTime, &tt); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + CHECK(tt.tv_usec == 183000); + // The plus 1 is a own forced overflow of usecs + CHECK(tt.tv_sec == (1648208539 + 27 + 10 + 32 + 1)); + } +} \ No newline at end of file diff --git a/tests/src/fsfw_tests/unit/osal/TestMessageQueue.cpp b/unittests/osal/TestMessageQueue.cpp similarity index 57% rename from tests/src/fsfw_tests/unit/osal/TestMessageQueue.cpp rename to unittests/osal/TestMessageQueue.cpp index 232ffb94b..df15b33d5 100644 --- a/tests/src/fsfw_tests/unit/osal/TestMessageQueue.cpp +++ b/unittests/osal/TestMessageQueue.cpp @@ -4,7 +4,7 @@ #include #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" TEST_CASE("MessageQueue Basic Test", "[TestMq]") { MessageQueueIF* testSenderMq = QueueFactory::instance()->createMessageQueue(1); @@ -35,4 +35,25 @@ TEST_CASE("MessageQueue Basic Test", "[TestMq]") { senderId = testReceiverMq->getLastPartner(); CHECK(senderId == testSenderMqId); } + SECTION("Test Full") { + auto result = testSenderMq->sendMessage(testReceiverMqId, &testMessage); + REQUIRE(result == retval::CATCH_OK); + result = testSenderMq->sendMessage(testReceiverMqId, &testMessage); + REQUIRE(result == MessageQueueIF::FULL); + // We try another message + result = testSenderMq->sendMessage(testReceiverMqId, &testMessage); + REQUIRE(result == MessageQueueIF::FULL); + MessageQueueMessage recvMessage; + result = testReceiverMq->receiveMessage(&recvMessage); + REQUIRE(result == retval::CATCH_OK); + CHECK(recvMessage.getData()[0] == 42); + result = testSenderMq->sendMessage(testReceiverMqId, &testMessage); + REQUIRE(result == retval::CATCH_OK); + result = testReceiverMq->receiveMessage(&recvMessage); + REQUIRE(result == retval::CATCH_OK); + CHECK(recvMessage.getData()[0] == 42); + } + // We have to clear MQs ourself ATM + QueueFactory::instance()->deleteMessageQueue(testSenderMq); + QueueFactory::instance()->deleteMessageQueue(testReceiverMq); } diff --git a/tests/src/fsfw_tests/unit/osal/TestSemaphore.cpp b/unittests/osal/TestSemaphore.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/osal/TestSemaphore.cpp rename to unittests/osal/TestSemaphore.cpp diff --git a/unittests/power/CMakeLists.txt b/unittests/power/CMakeLists.txt new file mode 100644 index 000000000..667e6f51d --- /dev/null +++ b/unittests/power/CMakeLists.txt @@ -0,0 +1,3 @@ +target_sources(${FSFW_TEST_TGT} PRIVATE + testPowerSwitcher.cpp +) diff --git a/unittests/power/testPowerSwitcher.cpp b/unittests/power/testPowerSwitcher.cpp new file mode 100644 index 000000000..f6a24583c --- /dev/null +++ b/unittests/power/testPowerSwitcher.cpp @@ -0,0 +1,71 @@ +#include +#include + +#include + +#include "mocks/PowerSwitcherMock.h" +#include "objects/systemObjectList.h" + +TEST_CASE("Power Switcher", "[power-switcher]") { + PowerSwitcherMock mock; + PowerSwitcher switcher(&mock, 1); + DummyPowerSwitcher dummySwitcher(objects::DUMMY_POWER_SWITCHER, 5, 5, false); + PowerSwitcher switcherUsingDummy(&dummySwitcher, 1); + SwitchInfo switchInfo; + mock.initSwitch(1); + + SECTION("Basic Tests") { + REQUIRE(switcher.getFirstSwitch() == 1); + REQUIRE(switcher.getSecondSwitch() == power::NO_SWITCH); + // Default start state + REQUIRE(switcher.getState() == PowerSwitcher::SWITCH_IS_OFF); + switcher.turnOn(true); + REQUIRE(mock.getAmountSwitchStatWasRequested() == 1); + REQUIRE(switcher.getState() == PowerSwitcher::WAIT_ON); + REQUIRE(switcher.checkSwitchState() == PowerSwitcher::IN_POWER_TRANSITION); + REQUIRE(switcher.active()); + switcher.doStateMachine(); + REQUIRE(switcher.getState() == PowerSwitcher::SWITCH_IS_ON); + mock.getSwitchInfo(1, switchInfo); + REQUIRE(switchInfo.timesCalledOn == 1); + REQUIRE(not switcher.active()); + REQUIRE(mock.getAmountSwitchStatWasRequested() == 2); + REQUIRE(switcher.checkSwitchState() == HasReturnvaluesIF::RETURN_OK); + REQUIRE(mock.getAmountSwitchStatWasRequested() == 3); + switcher.turnOff(false); + REQUIRE(mock.getAmountSwitchStatWasRequested() == 3); + REQUIRE(switcher.getState() == PowerSwitcher::WAIT_OFF); + REQUIRE(switcher.active()); + REQUIRE(switcher.getState() == PowerSwitcher::WAIT_OFF); + switcher.doStateMachine(); + mock.getSwitchInfo(1, switchInfo); + REQUIRE(switcher.getState() == PowerSwitcher::SWITCH_IS_OFF); + REQUIRE(switchInfo.timesCalledOn == 1); + REQUIRE(switchInfo.timesCalledOff == 1); + REQUIRE(not switcher.active()); + REQUIRE(mock.getAmountSwitchStatWasRequested() == 4); + } + + SECTION("Dummy Test") { + // Same tests, but we can't really check the dummy + REQUIRE(switcherUsingDummy.getFirstSwitch() == 1); + REQUIRE(switcherUsingDummy.getSecondSwitch() == power::NO_SWITCH); + REQUIRE(switcherUsingDummy.getState() == PowerSwitcher::SWITCH_IS_OFF); + switcherUsingDummy.turnOn(true); + REQUIRE(switcherUsingDummy.getState() == PowerSwitcher::WAIT_ON); + REQUIRE(switcherUsingDummy.active()); + switcherUsingDummy.doStateMachine(); + REQUIRE(switcherUsingDummy.getState() == PowerSwitcher::SWITCH_IS_ON); + REQUIRE(not switcherUsingDummy.active()); + + switcherUsingDummy.turnOff(false); + REQUIRE(switcherUsingDummy.getState() == PowerSwitcher::WAIT_OFF); + REQUIRE(switcherUsingDummy.active()); + REQUIRE(switcherUsingDummy.getState() == PowerSwitcher::WAIT_OFF); + switcherUsingDummy.doStateMachine(); + REQUIRE(switcherUsingDummy.getState() == PowerSwitcher::SWITCH_IS_OFF); + REQUIRE(not switcherUsingDummy.active()); + } + + SECTION("More Dummy Tests") {} +} diff --git a/tests/src/fsfw_tests/unit/printChar.cpp b/unittests/printChar.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/printChar.cpp rename to unittests/printChar.cpp diff --git a/tests/src/fsfw_tests/unit/printChar.h b/unittests/printChar.h similarity index 100% rename from tests/src/fsfw_tests/unit/printChar.h rename to unittests/printChar.h diff --git a/tests/src/fsfw_tests/unit/serialize/CMakeLists.txt b/unittests/serialize/CMakeLists.txt similarity index 100% rename from tests/src/fsfw_tests/unit/serialize/CMakeLists.txt rename to unittests/serialize/CMakeLists.txt diff --git a/tests/src/fsfw_tests/unit/serialize/TestSerialBufferAdapter.cpp b/unittests/serialize/TestSerialBufferAdapter.cpp similarity index 99% rename from tests/src/fsfw_tests/unit/serialize/TestSerialBufferAdapter.cpp rename to unittests/serialize/TestSerialBufferAdapter.cpp index 9b30427f9..2aa76ec8b 100644 --- a/tests/src/fsfw_tests/unit/serialize/TestSerialBufferAdapter.cpp +++ b/unittests/serialize/TestSerialBufferAdapter.cpp @@ -3,7 +3,7 @@ #include #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" static bool test_value_bool = true; static uint16_t tv_uint16{283}; diff --git a/tests/src/fsfw_tests/unit/serialize/TestSerialLinkedPacket.cpp b/unittests/serialize/TestSerialLinkedPacket.cpp similarity index 98% rename from tests/src/fsfw_tests/unit/serialize/TestSerialLinkedPacket.cpp rename to unittests/serialize/TestSerialLinkedPacket.cpp index e3bdf882a..2d6e476fa 100644 --- a/tests/src/fsfw_tests/unit/serialize/TestSerialLinkedPacket.cpp +++ b/unittests/serialize/TestSerialLinkedPacket.cpp @@ -5,7 +5,7 @@ #include #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" TEST_CASE("Serial Linked Packet", "[SerLinkPacket]") { // perform set-up here diff --git a/tests/src/fsfw_tests/unit/serialize/TestSerialLinkedPacket.h b/unittests/serialize/TestSerialLinkedPacket.h similarity index 100% rename from tests/src/fsfw_tests/unit/serialize/TestSerialLinkedPacket.h rename to unittests/serialize/TestSerialLinkedPacket.h diff --git a/tests/src/fsfw_tests/unit/serialize/TestSerialization.cpp b/unittests/serialize/TestSerialization.cpp similarity index 99% rename from tests/src/fsfw_tests/unit/serialize/TestSerialization.cpp rename to unittests/serialize/TestSerialization.cpp index 52cb70f6d..a3340a7d9 100644 --- a/tests/src/fsfw_tests/unit/serialize/TestSerialization.cpp +++ b/unittests/serialize/TestSerialization.cpp @@ -5,7 +5,7 @@ #include #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" static bool testBool = true; static uint8_t tvUint8{5}; diff --git a/tests/src/fsfw_tests/unit/storagemanager/CMakeLists.txt b/unittests/storagemanager/CMakeLists.txt similarity index 100% rename from tests/src/fsfw_tests/unit/storagemanager/CMakeLists.txt rename to unittests/storagemanager/CMakeLists.txt diff --git a/tests/src/fsfw_tests/unit/storagemanager/TestNewAccessor.cpp b/unittests/storagemanager/TestNewAccessor.cpp similarity index 84% rename from tests/src/fsfw_tests/unit/storagemanager/TestNewAccessor.cpp rename to unittests/storagemanager/TestNewAccessor.cpp index e1db9ce04..2cc2e4695 100644 --- a/tests/src/fsfw_tests/unit/storagemanager/TestNewAccessor.cpp +++ b/unittests/storagemanager/TestNewAccessor.cpp @@ -4,7 +4,7 @@ #include #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" TEST_CASE("New Accessor", "[NewAccessor]") { LocalPool::LocalPoolConfig poolCfg = {{1, 10}}; @@ -156,4 +156,31 @@ TEST_CASE("New Accessor", "[NewAccessor]") { CHECK(receptionArray[i] == 42); } } + + SECTION("Operators") { + result = SimplePool.addData(&testStoreId, testDataArray.data(), size); + REQUIRE(result == retval::CATCH_OK); + { + StorageAccessor accessor(testStoreId); + StorageAccessor accessor2(0); + accessor2 = std::move(accessor); + REQUIRE(accessor.data() == nullptr); + std::array data; + size_t size = 6; + result = accessor.write(data.data(), data.size()); + REQUIRE(result == HasReturnvaluesIF::RETURN_FAILED); + result = SimplePool.modifyData(testStoreId, accessor2); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + CHECK(accessor2.getId() == testStoreId); + CHECK(accessor2.size() == 10); + + std::array newData; + // Expect data to be invalid so this must return RETURN_FAILED + result = accessor.getDataCopy(newData.data(), newData.size()); + REQUIRE(result == HasReturnvaluesIF::RETURN_FAILED); + // Expect data to be too small + result = accessor2.getDataCopy(data.data(), data.size()); + REQUIRE(result == HasReturnvaluesIF::RETURN_FAILED); + } + } } diff --git a/tests/src/fsfw_tests/unit/storagemanager/TestPool.cpp b/unittests/storagemanager/TestPool.cpp similarity index 99% rename from tests/src/fsfw_tests/unit/storagemanager/TestPool.cpp rename to unittests/storagemanager/TestPool.cpp index 2cbf951d4..e37c69343 100644 --- a/tests/src/fsfw_tests/unit/storagemanager/TestPool.cpp +++ b/unittests/storagemanager/TestPool.cpp @@ -1,10 +1,11 @@ #include #include +#include #include #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" TEST_CASE("Local Pool Simple Tests [1 Pool]", "[TestPool]") { LocalPool::LocalPoolConfig config = {{1, 10}}; diff --git a/unittests/testVersion.cpp b/unittests/testVersion.cpp new file mode 100644 index 000000000..2f4870314 --- /dev/null +++ b/unittests/testVersion.cpp @@ -0,0 +1,91 @@ + +#include + +#include "CatchDefinitions.h" +#include "fsfw/serviceinterface.h" +#include "fsfw/version.h" + +TEST_CASE("Version API Tests", "[TestVersionAPI]") { + // Check that major version is non-zero + REQUIRE(fsfw::FSFW_VERSION.major > 0); + uint32_t fsfwMajor = fsfw::FSFW_VERSION.major; + REQUIRE(fsfw::Version(255, 0, 0) > fsfw::FSFW_VERSION); + REQUIRE(fsfw::Version(255, 0, 0) >= fsfw::FSFW_VERSION); + REQUIRE(fsfw::Version(0, 0, 0) < fsfw::FSFW_VERSION); + REQUIRE(fsfw::Version(0, 0, 0) <= fsfw::FSFW_VERSION); + auto v1 = fsfw::Version(1, 1, 1); + auto v2 = fsfw::Version(1, 1, 1); + REQUIRE(v1 == v2); + REQUIRE(not(v1 < v2)); + REQUIRE(not(v1 > v2)); + REQUIRE(v1 <= v2); + REQUIRE(v1 >= v2); + v1.revision -= 1; + REQUIRE(v1 != v2); + REQUIRE(not(v1 == v2)); + REQUIRE(not(v1 > v2)); + REQUIRE(not(v1 >= v2)); + REQUIRE(v1 < v2); + REQUIRE(v1 <= v2); + v1.revision += 1; + v1.minor -= 1; + REQUIRE(v1 != v2); + REQUIRE(v1 < v2); + REQUIRE(v1 <= v2); + REQUIRE(not(v1 == v2)); + REQUIRE(not(v1 > v2)); + REQUIRE(not(v1 >= v2)); + v1.minor += 1; + v1.major -= 1; + REQUIRE(v1 != v2); + REQUIRE(v1 < v2); + REQUIRE(v1 <= v2); + REQUIRE(not(v1 == v2)); + REQUIRE(not(v1 > v2)); + REQUIRE(not(v1 >= v2)); + v1.major += 1; + REQUIRE(v1 == v2); + REQUIRE(v1 <= v2); + REQUIRE(v1 >= v2); + REQUIRE(not(v1 != v2)); + REQUIRE(not(v1 > v2)); + REQUIRE(not(v1 < v2)); + v1.major += 1; + v1.minor -= 1; + REQUIRE(v1 != v2); + REQUIRE(v1 > v2); + REQUIRE(v1 >= v2); + REQUIRE(not(v1 == v2)); + REQUIRE(not(v1 < v2)); + REQUIRE(not(v1 <= v2)); + v1.major -= 1; + v1.minor += 2; + v1.revision -= 1; + REQUIRE(v1 != v2); + REQUIRE(v1 > v2); + REQUIRE(v1 >= v2); + REQUIRE(not(v1 == v2)); + REQUIRE(not(v1 < v2)); + REQUIRE(not(v1 <= v2)); + v1.minor -= 1; + v1.revision += 2; + REQUIRE(v1 != v2); + REQUIRE(v1 > v2); + REQUIRE(v1 >= v2); + REQUIRE(not(v1 == v2)); + REQUIRE(not(v1 < v2)); + REQUIRE(not(v1 <= v2)); + v1.revision -= 1; + REQUIRE(v1 == v2); + REQUIRE(v1 <= v2); + REQUIRE(v1 >= v2); + REQUIRE(not(v1 != v2)); +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "v" << fsfw::FSFW_VERSION << std::endl; +#endif + char verString[10] = {}; + fsfw::FSFW_VERSION.getVersion(verString, sizeof(verString)); +#if FSFW_DISABLE_PRINTOUT == 0 + printf("v%s\n", verString); +#endif +} diff --git a/tests/src/fsfw_tests/unit/testcfg/CMakeLists.txt b/unittests/testcfg/CMakeLists.txt similarity index 100% rename from tests/src/fsfw_tests/unit/testcfg/CMakeLists.txt rename to unittests/testcfg/CMakeLists.txt diff --git a/tests/src/fsfw_tests/unit/testcfg/FSFWConfig.h.in b/unittests/testcfg/FSFWConfig.h.in similarity index 97% rename from tests/src/fsfw_tests/unit/testcfg/FSFWConfig.h.in rename to unittests/testcfg/FSFWConfig.h.in index f05ef40bd..ef58f4cd0 100644 --- a/tests/src/fsfw_tests/unit/testcfg/FSFWConfig.h.in +++ b/unittests/testcfg/FSFWConfig.h.in @@ -11,10 +11,12 @@ //! More FSFW related printouts depending on level. Useful for development. #define FSFW_VERBOSE_LEVEL 0 - + //! Can be used to completely disable printouts, even the C stdio ones. #if FSFW_CPP_OSTREAM_ENABLED == 0 && FSFW_VERBOSE_LEVEL == 0 #define FSFW_DISABLE_PRINTOUT 1 +#else + #define FSFW_DISABLE_PRINTOUT 0 #endif #define FSFW_USE_PUS_C_TELEMETRY 1 diff --git a/tests/src/fsfw_tests/unit/testcfg/OBSWConfig.h.in b/unittests/testcfg/OBSWConfig.h.in similarity index 100% rename from tests/src/fsfw_tests/unit/testcfg/OBSWConfig.h.in rename to unittests/testcfg/OBSWConfig.h.in diff --git a/tests/src/fsfw_tests/unit/testcfg/TestsConfig.h.in b/unittests/testcfg/TestsConfig.h.in similarity index 92% rename from tests/src/fsfw_tests/unit/testcfg/TestsConfig.h.in rename to unittests/testcfg/TestsConfig.h.in index 7d9500702..1865c41ad 100644 --- a/tests/src/fsfw_tests/unit/testcfg/TestsConfig.h.in +++ b/unittests/testcfg/TestsConfig.h.in @@ -1,6 +1,8 @@ #ifndef FSFW_UNITTEST_CONFIG_TESTSCONFIG_H_ #define FSFW_UNITTEST_CONFIG_TESTSCONFIG_H_ +#cmakedefine01 FSFW_CICD_BUILD + #define FSFW_ADD_DEFAULT_FACTORY_FUNCTIONS 1 #ifdef __cplusplus diff --git a/tests/src/fsfw_tests/unit/testcfg/devices/logicalAddresses.cpp b/unittests/testcfg/devices/logicalAddresses.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/testcfg/devices/logicalAddresses.cpp rename to unittests/testcfg/devices/logicalAddresses.cpp diff --git a/tests/src/fsfw_tests/unit/testcfg/devices/logicalAddresses.h b/unittests/testcfg/devices/logicalAddresses.h similarity index 100% rename from tests/src/fsfw_tests/unit/testcfg/devices/logicalAddresses.h rename to unittests/testcfg/devices/logicalAddresses.h diff --git a/tests/src/fsfw_tests/unit/testcfg/devices/powerSwitcherList.cpp b/unittests/testcfg/devices/powerSwitcherList.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/testcfg/devices/powerSwitcherList.cpp rename to unittests/testcfg/devices/powerSwitcherList.cpp diff --git a/tests/src/fsfw_tests/unit/testcfg/devices/powerSwitcherList.h b/unittests/testcfg/devices/powerSwitcherList.h similarity index 100% rename from tests/src/fsfw_tests/unit/testcfg/devices/powerSwitcherList.h rename to unittests/testcfg/devices/powerSwitcherList.h diff --git a/tests/src/fsfw_tests/unit/testcfg/events/subsystemIdRanges.h b/unittests/testcfg/events/subsystemIdRanges.h similarity index 100% rename from tests/src/fsfw_tests/unit/testcfg/events/subsystemIdRanges.h rename to unittests/testcfg/events/subsystemIdRanges.h diff --git a/tests/src/fsfw_tests/unit/testcfg/ipc/MissionMessageTypes.cpp b/unittests/testcfg/ipc/MissionMessageTypes.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/testcfg/ipc/MissionMessageTypes.cpp rename to unittests/testcfg/ipc/MissionMessageTypes.cpp diff --git a/tests/src/fsfw_tests/unit/testcfg/ipc/MissionMessageTypes.h b/unittests/testcfg/ipc/MissionMessageTypes.h similarity index 100% rename from tests/src/fsfw_tests/unit/testcfg/ipc/MissionMessageTypes.h rename to unittests/testcfg/ipc/MissionMessageTypes.h diff --git a/tests/src/fsfw_tests/unit/testcfg/objects/systemObjectList.h b/unittests/testcfg/objects/systemObjectList.h similarity index 83% rename from tests/src/fsfw_tests/unit/testcfg/objects/systemObjectList.h rename to unittests/testcfg/objects/systemObjectList.h index 3eba14840..7d12e5c6e 100644 --- a/tests/src/fsfw_tests/unit/testcfg/objects/systemObjectList.h +++ b/unittests/testcfg/objects/systemObjectList.h @@ -21,8 +21,13 @@ enum sourceObjects : uint32_t { HK_RECEIVER_MOCK = 22, TEST_LOCAL_POOL_OWNER_BASE = 25, - SHARED_SET_ID = 26 + SHARED_SET_ID = 26, + DUMMY_POWER_SWITCHER = 28, + + DEVICE_HANDLER_MOCK = 29, + COM_IF_MOCK = 30, + DEVICE_HANDLER_COMMANDER = 40, }; } diff --git a/tests/src/fsfw_tests/unit/testcfg/pollingsequence/PollingSequenceFactory.cpp b/unittests/testcfg/pollingsequence/PollingSequenceFactory.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/testcfg/pollingsequence/PollingSequenceFactory.cpp rename to unittests/testcfg/pollingsequence/PollingSequenceFactory.cpp diff --git a/tests/src/fsfw_tests/unit/testcfg/pollingsequence/PollingSequenceFactory.h b/unittests/testcfg/pollingsequence/PollingSequenceFactory.h similarity index 100% rename from tests/src/fsfw_tests/unit/testcfg/pollingsequence/PollingSequenceFactory.h rename to unittests/testcfg/pollingsequence/PollingSequenceFactory.h diff --git a/tests/src/fsfw_tests/unit/testcfg/returnvalues/classIds.h b/unittests/testcfg/returnvalues/classIds.h similarity index 100% rename from tests/src/fsfw_tests/unit/testcfg/returnvalues/classIds.h rename to unittests/testcfg/returnvalues/classIds.h diff --git a/tests/src/fsfw_tests/unit/testcfg/tmtc/apid.h b/unittests/testcfg/tmtc/apid.h similarity index 100% rename from tests/src/fsfw_tests/unit/testcfg/tmtc/apid.h rename to unittests/testcfg/tmtc/apid.h diff --git a/tests/src/fsfw_tests/unit/testcfg/tmtc/pusIds.h b/unittests/testcfg/tmtc/pusIds.h similarity index 100% rename from tests/src/fsfw_tests/unit/testcfg/tmtc/pusIds.h rename to unittests/testcfg/tmtc/pusIds.h diff --git a/tests/src/fsfw_tests/unit/testtemplate/TestTemplate.cpp b/unittests/testtemplate/TestTemplate.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/testtemplate/TestTemplate.cpp rename to unittests/testtemplate/TestTemplate.cpp diff --git a/tests/src/fsfw_tests/unit/timemanager/CMakeLists.txt b/unittests/timemanager/CMakeLists.txt similarity index 76% rename from tests/src/fsfw_tests/unit/timemanager/CMakeLists.txt rename to unittests/timemanager/CMakeLists.txt index 6ce1c6c6c..4b1693a93 100644 --- a/tests/src/fsfw_tests/unit/timemanager/CMakeLists.txt +++ b/unittests/timemanager/CMakeLists.txt @@ -1,3 +1,4 @@ target_sources(${FSFW_TEST_TGT} PRIVATE TestCountdown.cpp + TestCCSDSTime.cpp ) diff --git a/unittests/timemanager/TestCCSDSTime.cpp b/unittests/timemanager/TestCCSDSTime.cpp new file mode 100644 index 000000000..9c457ddac --- /dev/null +++ b/unittests/timemanager/TestCCSDSTime.cpp @@ -0,0 +1,219 @@ +#include +#include + +#include +#include +#include + +#include "CatchDefinitions.h" + +TEST_CASE("CCSDSTime Tests", "[TestCCSDSTime]") { + INFO("CCSDSTime Tests"); + CCSDSTime::Ccs_mseconds cssMilliSecconds{}; + Clock::TimeOfDay_t time; + time.year = 2020; + time.month = 2; + time.day = 29; + time.hour = 13; + time.minute = 24; + time.second = 45; + time.usecond = 123456; + SECTION("Test CCS Time") { + auto result = CCSDSTime::convertToCcsds(&cssMilliSecconds, &time); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + REQUIRE(cssMilliSecconds.pField == 0x52); // 0b01010010 + REQUIRE(cssMilliSecconds.yearMSB == 0x07); + REQUIRE(cssMilliSecconds.yearLSB == 0xe4); + REQUIRE(cssMilliSecconds.month == 2); + REQUIRE(cssMilliSecconds.day == 29); + REQUIRE(cssMilliSecconds.hour == 13); + REQUIRE(cssMilliSecconds.minute == 24); + REQUIRE(cssMilliSecconds.second == 45); + uint16_t secondsMinus4 = (static_cast(cssMilliSecconds.secondEminus2) * 100) + + cssMilliSecconds.secondEminus4; + REQUIRE(secondsMinus4 == 1234); + Clock::TimeOfDay_t timeTo; + const uint8_t* dataPtr = reinterpret_cast(&cssMilliSecconds); + size_t length = sizeof(CCSDSTime::Ccs_mseconds); + result = CCSDSTime::convertFromCCS(&timeTo, dataPtr, &length, sizeof(CCSDSTime::Ccs_mseconds)); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + REQUIRE(cssMilliSecconds.pField == 0x52); // 0b01010010 + REQUIRE(cssMilliSecconds.yearMSB == 0x07); + REQUIRE(cssMilliSecconds.yearLSB == 0xe4); + REQUIRE(cssMilliSecconds.month == 2); + REQUIRE(cssMilliSecconds.day == 29); + REQUIRE(cssMilliSecconds.hour == 13); + REQUIRE(cssMilliSecconds.minute == 24); + REQUIRE(cssMilliSecconds.second == 45); + REQUIRE(timeTo.year == 2020); + REQUIRE(timeTo.month == 2); + REQUIRE(timeTo.day == 29); + REQUIRE(timeTo.hour == 13); + REQUIRE(timeTo.minute == 24); + REQUIRE(timeTo.second == 45); + REQUIRE(timeTo.usecond == 123400); + } + SECTION("CCS_Day of Year") { + Clock::TimeOfDay_t timeTo; + std::array ccsDayOfYear = {0b01011000, 0x07, 0xe4, 0, 60, 13, 24, 45}; + size_t length = ccsDayOfYear.size(); + auto result = + CCSDSTime::convertFromCCS(&timeTo, ccsDayOfYear.data(), &length, ccsDayOfYear.size()); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + // Check constness + REQUIRE(ccsDayOfYear[0] == 0b01011000); + REQUIRE(ccsDayOfYear[1] == 0x07); + REQUIRE(ccsDayOfYear[2] == 0xe4); + REQUIRE(ccsDayOfYear[3] == 0x0); + REQUIRE(ccsDayOfYear[4] == 60); + REQUIRE(ccsDayOfYear[5] == 13); + REQUIRE(ccsDayOfYear[6] == 24); + REQUIRE(ccsDayOfYear[7] == 45); + REQUIRE(timeTo.year == 2020); + REQUIRE(timeTo.month == 2); + REQUIRE(timeTo.day == 29); + REQUIRE(timeTo.hour == 13); + REQUIRE(timeTo.minute == 24); + REQUIRE(timeTo.second == 45); + REQUIRE(timeTo.usecond == 0); + } + SECTION("Test convertFromASCII") { + std::string timeAscii = "2022-12-31T23:59:59.123Z"; + Clock::TimeOfDay_t timeTo; + const uint8_t* timeChar = reinterpret_cast(timeAscii.c_str()); + auto result = CCSDSTime::convertFromASCII(&timeTo, timeChar, timeAscii.length()); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + REQUIRE(timeTo.year == 2022); + REQUIRE(timeTo.month == 12); + REQUIRE(timeTo.day == 31); + REQUIRE(timeTo.hour == 23); + REQUIRE(timeTo.minute == 59); + REQUIRE(timeTo.second == 59); + REQUIRE(timeTo.usecond == Catch::Approx(123000)); + + std::string timeAscii2 = "2022-365T23:59:59.123Z"; + const uint8_t* timeChar2 = reinterpret_cast(timeAscii2.c_str()); + Clock::TimeOfDay_t timeTo2; + result = CCSDSTime::convertFromCcsds(&timeTo2, timeChar2, timeAscii2.length()); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + REQUIRE(timeTo2.year == 2022); + REQUIRE(timeTo2.month == 12); + REQUIRE(timeTo2.day == 31); + REQUIRE(timeTo2.hour == 23); + REQUIRE(timeTo2.minute == 59); + REQUIRE(timeTo2.second == 59); + REQUIRE(timeTo2.usecond == Catch::Approx(123000)); + } + + SECTION("CDS Conversions") { + // Preperation + Clock::TimeOfDay_t time; + time.year = 2020; + time.month = 2; + time.day = 29; + time.hour = 13; + time.minute = 24; + time.second = 45; + time.usecond = 123456; + timeval timeAsTimeval; + auto result = Clock::convertTimeOfDayToTimeval(&time, &timeAsTimeval); + CHECK(result == HasReturnvaluesIF::RETURN_OK); + CHECK(timeAsTimeval.tv_sec == 1582982685); + CHECK(timeAsTimeval.tv_usec == 123456); + + // Conversion to CDS Short + CCSDSTime::CDS_short cdsTime; + result = CCSDSTime::convertToCcsds(&cdsTime, &timeAsTimeval); + CHECK(result == HasReturnvaluesIF::RETURN_OK); + // Days in CCSDS Epoch 22704 (0x58B0) + CHECK(cdsTime.dayMSB == 0x58); + CHECK(cdsTime.dayLSB == 0xB0); + // MS of day 48285123.456 (floored here) + CHECK(cdsTime.msDay_hh == 0x2); + CHECK(cdsTime.msDay_h == 0xE0); + CHECK(cdsTime.msDay_l == 0xC5); + CHECK(cdsTime.msDay_ll == 0xC3); + CHECK(cdsTime.pField == CCSDSTime::P_FIELD_CDS_SHORT); + + // Conversion back to timeval + timeval timeReturnAsTimeval; + result = CCSDSTime::convertFromCDS(&timeReturnAsTimeval, &cdsTime); + CHECK(result == HasReturnvaluesIF::RETURN_OK); + // micro seconds precision is lost + timeval difference = timeAsTimeval - timeReturnAsTimeval; + CHECK(difference.tv_usec == 456); + CHECK(difference.tv_sec == 0); + + Clock::TimeOfDay_t timeReturnAsTimeOfDay; + result = CCSDSTime::convertFromCDS(&timeReturnAsTimeOfDay, &cdsTime); + CHECK(result == HasReturnvaluesIF::RETURN_OK); + CHECK(timeReturnAsTimeOfDay.year == 2020); + CHECK(timeReturnAsTimeOfDay.month == 2); + CHECK(timeReturnAsTimeOfDay.day == 29); + CHECK(timeReturnAsTimeOfDay.hour == 13); + CHECK(timeReturnAsTimeOfDay.minute == 24); + CHECK(timeReturnAsTimeOfDay.second == 45); + // micro seconds precision is lost + CHECK(timeReturnAsTimeOfDay.usecond == 123000); + + Clock::TimeOfDay_t timeReturnAsTodFromBuffer; + const uint8_t* buffer = reinterpret_cast(&cdsTime); + result = CCSDSTime::convertFromCDS(&timeReturnAsTodFromBuffer, buffer, sizeof(cdsTime)); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + CHECK(timeReturnAsTodFromBuffer.year == time.year); + CHECK(timeReturnAsTodFromBuffer.month == time.month); + CHECK(timeReturnAsTodFromBuffer.day == time.day); + CHECK(timeReturnAsTodFromBuffer.hour == time.hour); + CHECK(timeReturnAsTodFromBuffer.minute == time.minute); + CHECK(timeReturnAsTodFromBuffer.second == time.second); + CHECK(timeReturnAsTodFromBuffer.usecond == 123000); + + Clock::TimeOfDay_t todFromCCSDS; + result = CCSDSTime::convertFromCcsds(&todFromCCSDS, buffer, sizeof(cdsTime)); + CHECK(result == HasReturnvaluesIF::RETURN_OK); + CHECK(todFromCCSDS.year == time.year); + CHECK(todFromCCSDS.month == time.month); + CHECK(todFromCCSDS.day == time.day); + CHECK(todFromCCSDS.hour == time.hour); + CHECK(todFromCCSDS.minute == time.minute); + CHECK(todFromCCSDS.second == time.second); + CHECK(todFromCCSDS.usecond == 123000); + } + SECTION("CUC") { + timeval to; + // seconds = 0x771E960F, microseconds = 0x237 + // microseconds = 567000 + // This gives 37158.912 1/65536 seconds -> rounded to 37159 -> 0x9127 + // This results in -> 567001 us + std::array cucBuffer = { + CCSDSTime::P_FIELD_CUC_6B_CCSDS, 0x77, 0x1E, 0x96, 0x0F, 0x91, 0x27}; + size_t foundLength = 0; + auto result = CCSDSTime::convertFromCUC(&to, cucBuffer.data(), &foundLength, cucBuffer.size()); + REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + REQUIRE(foundLength == 7); + REQUIRE(to.tv_sec == 1619801999); // TAI (no leap seconds) + REQUIRE(to.tv_usec == 567001); + + Clock::TimeOfDay_t tod; + result = CCSDSTime::convertFromCUC(&tod, cucBuffer.data(), cucBuffer.size()); + // This test must be changed if this is ever going to be implemented + REQUIRE(result == CCSDSTime::UNSUPPORTED_TIME_FORMAT); + } + + SECTION("CCSDS Failures") { + Clock::TimeOfDay_t time; + time.year = 2020; + time.month = 12; + time.day = 32; + time.hour = 13; + time.minute = 24; + time.second = 45; + time.usecond = 123456; + CCSDSTime::Ccs_mseconds to; + auto result = CCSDSTime::convertToCcsds(&to, &time); + REQUIRE(result == CCSDSTime::INVALID_TIME_FORMAT); + CCSDSTime::Ccs_seconds to2; + result = CCSDSTime::convertToCcsds(&to2, &time); + REQUIRE(result == CCSDSTime::INVALID_TIME_FORMAT); + } +} \ No newline at end of file diff --git a/tests/src/fsfw_tests/unit/timemanager/TestCountdown.cpp b/unittests/timemanager/TestCountdown.cpp similarity index 94% rename from tests/src/fsfw_tests/unit/timemanager/TestCountdown.cpp rename to unittests/timemanager/TestCountdown.cpp index bc39b02e8..d0af659da 100644 --- a/tests/src/fsfw_tests/unit/timemanager/TestCountdown.cpp +++ b/unittests/timemanager/TestCountdown.cpp @@ -2,7 +2,7 @@ #include -#include "fsfw_tests/unit/CatchDefinitions.h" +#include "CatchDefinitions.h" TEST_CASE("Countdown Tests", "[TestCountdown]") { INFO("Countdown Tests"); diff --git a/tests/src/fsfw_tests/unit/tmtcpacket/CMakeLists.txt b/unittests/tmtcpacket/CMakeLists.txt similarity index 100% rename from tests/src/fsfw_tests/unit/tmtcpacket/CMakeLists.txt rename to unittests/tmtcpacket/CMakeLists.txt diff --git a/tests/src/fsfw_tests/unit/tmtcpacket/PusTmTest.cpp b/unittests/tmtcpacket/PusTmTest.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/tmtcpacket/PusTmTest.cpp rename to unittests/tmtcpacket/PusTmTest.cpp diff --git a/tests/src/fsfw_tests/unit/tmtcpacket/testCcsds.cpp b/unittests/tmtcpacket/testCcsds.cpp similarity index 100% rename from tests/src/fsfw_tests/unit/tmtcpacket/testCcsds.cpp rename to unittests/tmtcpacket/testCcsds.cpp